move conversion functions to utils.h

mavlink · Apr 18, 2017 · 88fe78e · 88fe78e
1 parent 79239ee
commit 88fe78e
Show file tree

Hide file tree

Showing 5 changed files with 61 additions and 79 deletions.
diff --git a/mavros/CMakeLists.txt b/mavros/CMakeLists.txt
@@ -40,29 +40,8 @@ include(MavrosMavlink)
 
 ## Find GeographicLib
 list(INSERT CMAKE_MODULE_PATH 0 "${CMAKE_CURRENT_SOURCE_DIR}/cmake/Modules")
-find_package(GeographicLib 1.40 COMPONENTS SHARED)
-if(NOT GeographicLib_FOUND)
-  include(ExternalProject)
-
-  # The configure step fails at catkin_package() if this directory doesn't exist yet
-  file(MAKE_DIRECTORY ${CATKIN_DEVEL_PREFIX}/include)
-
-  ExternalProject_Add(
-      GeographicLib
-      GIT_REPOSITORY git://git.code.sourceforge.net/p/geographiclib/code
-      PATCH_COMMAND mkdir -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/data
-      CONFIGURE_COMMAND cd ${CMAKE_BINARY_DIR}/GeographicLib-prefix/src/GeographicLib && cmake -D CMAKE_INSTALL_PREFIX=${CATKIN_DEVEL_PREFIX} -D GEOGRAPHICLIB_LIB_TYPE=SHARED -D GEOGRAPHICLIB_DATA=${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/share/data .
-      BUILD_COMMAND cd ${CMAKE_BINARY_DIR}/GeographicLib-prefix/src/GeographicLib && make -j1
-      INSTALL_COMMAND cd ${CMAKE_BINARY_DIR}/GeographicLib-prefix/src/GeographicLib && make install &&
-          ./tools/geographiclib-get-geoids -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/share/data egm96-5 &&
-          ./tools/geographiclib-get-gravity -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/share/data egm96 &&
-          ./tools/geographiclib-get-magnetic -p ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/share/data emm2015
-  )
-  set(GeographicLib_INCLUDE_DIRS ${CATKIN_DEVEL_PREFIX}/include)
-  set(GeographicLib_LIBRARIES ${CATKIN_DEVEL_PREFIX}/lib/libGeographic${CMAKE_SHARED_LIBRARY_SUFFIX})
-
-else()
-
+find_package(GeographicLib)
+if(GeographicLib_FOUND)
   IF(NOT EXISTS /usr/local/share/GeographicLib/geoids)
      execute_process(COMMAND sudo geographiclib-get-geoids egm96-5)
   endif()
@@ -72,9 +51,6 @@ else()
   IF(NOT EXISTS /usr/local/share/GeographicLib/magnetic)
      execute_process(COMMAND sudo geographiclib-get-magnetic emm2015)
   endif()
-  set(GeographicLib_INCLUDE_DIRS ${GeographicLib_INCLUDE_DIR})
-  set(GeographicLib_LIBRARIES ${GeographicLib_LIBRARIES})
-
 endif()
 
 # detect if sensor_msgs has BatteryState.msg
@@ -105,7 +81,7 @@ catkin_package(
   INCLUDE_DIRS include
   LIBRARIES mavros
   CATKIN_DEPENDS diagnostic_msgs diagnostic_updater pluginlib roscpp sensor_msgs nav_msgs std_msgs tf2_ros geometry_msgs libmavconn message_runtime eigen_conversions mavros_msgs
-  DEPENDS Boost EIGEN3
+  DEPENDS Boost EIGEN3 GeographicLib
 )
 
 ###########
@@ -169,13 +145,11 @@ add_library(mavros_plugins
 )
 add_dependencies(mavros_plugins
   mavros
-  GeographicLib
 )
 target_link_libraries(mavros_plugins
   mavros
   ${catkin_LIBRARIES}
   ${Boost_LIBRARIES}
-  ${GeographicLib_LIBRARIES}
 )
 
 ## Declare a cpp executable
@@ -184,13 +158,11 @@ add_executable(mavros_node
 )
 add_dependencies(mavros_node
   mavros
-  GeographicLib
 )
 target_link_libraries(mavros_node
   mavros
   ${catkin_LIBRARIES}
   ${Boost_LIBRARIES}
-  ${GeographicLib_LIBRARIES}
 )
 
 add_executable(gcs_bridge

diff --git a/mavros/cmake/Modules/FindGeographicLib.cmake b/mavros/cmake/Modules/FindGeographicLib.cmake
@@ -1,22 +1,31 @@
-# Used to find GeographicLib
-# Thanks to: Kitware, Inc. repo
+# Look for GeographicLib
+#
+# Set
+#  GEOGRAPHICLIB_FOUND = TRUE
+#  GeographicLib_INCLUDE_DIRS = /usr/local/include
+#  GeographicLib_LIBRARIES = /usr/local/lib/libGeographic.so
+#  GeographicLib_LIBRARY_DIRS = /usr/local/lib
 
-if( Geographiclib_DIR )
-  find_package( GeographicLib NO_MODULE )
-elseif( NOT GeographicLib_FOUND )
-  include(CommonFindMacros)
+find_library (GeographicLib_LIBRARIES Geographic
+  PATHS "${CMAKE_INSTALL_PREFIX}/../GeographicLib/lib")
 
-  setup_find_root_context(GeographicLib)
-  find_path( GeographicLib_INCLUDE_DIR GeographicLib/GeoCoords.hpp
-    ${GeographicLib_FIND_OPTS})
-  find_library( GeographicLib_LIBRARY
-    NAMES Geographic GeographicLib Geographic_d GeographicLib_d
-    ${GeographicLib_FIND_OPTS})
-  restore_find_root_context(GeographicLib)
+if (GeographicLib_LIBRARIES)
+  get_filename_component (GeographicLib_LIBRARY_DIRS
+    "${GeographicLib_LIBRARIES}" PATH)
+  get_filename_component (_ROOT_DIR "${GeographicLib_LIBRARY_DIRS}" PATH)
+  set (GeographicLib_INCLUDE_DIRS "${_ROOT_DIR}/include")
+  set (GeographicLib_BINARY_DIRS "${_ROOT_DIR}/bin")
+  unset (_ROOT_DIR)
+  if (NOT EXISTS "${GeographicLib_INCLUDE_DIRS}/GeographicLib/Config.h")
+    unset (GeographicLib_INCLUDE_DIRS)
+    unset (GeographicLib_LIBRARIES)
+    unset (GeographicLib_LIBRARY_DIRS)
+    unset (GeographicLib_BINARY_DIRS)
+  endif ()
+endif ()
 
-  include( FindPackageHandleStandardArgs )
-  FIND_PACKAGE_HANDLE_STANDARD_ARGS( GeographicLib GeographicLib_INCLUDE_DIR GeographicLib_LIBRARY )
-  if( GEOGRAPHICLIB_FOUND )
-    set( GeographicLib_FOUND TRUE )
-  endif()
-endif()
+include (FindPackageHandleStandardArgs)
+find_package_handle_standard_args (GeographicLib DEFAULT_MSG
+  GeographicLib_LIBRARY_DIRS GeographicLib_LIBRARIES GeographicLib_INCLUDE_DIRS)
+mark_as_advanced (GeographicLib_LIBRARY_DIRS GeographicLib_LIBRARIES
+  GeographicLib_INCLUDE_DIRS)
diff --git a/mavros/include/mavros/utils.h b/mavros/include/mavros/utils.h
@@ -21,18 +21,18 @@
 #include <mavconn/thread_utils.h>
 #include <mavros_msgs/mavlink_convert.h>
 #include <mavconn/mavlink_dialect.h>
+#include <GeographicLib/Geoid.hpp>
 
 // OS X compat: missing error codes
 #ifdef __APPLE__
-#define EBADE 50   /* Invalid exchange */
-#define EBADFD 81  /* File descriptor in bad state */
-#define EBADRQC 54 /* Invalid request code */
-#define EBADSLT 55 /* Invalid slot */
+#define EBADE 50	/* Invalid exchange */
+#define EBADFD 81	/* File descriptor in bad state */
+#define EBADRQC 54	/* Invalid request code */
+#define EBADSLT 55	/* Invalid slot */
 #endif
 
 namespace mavros {
 namespace utils {
-
 using mavconn::utils::format;
 
 /**
@@ -91,5 +91,26 @@ int sensor_orientation_from_str(const std::string &sensor_orientation);
  */
 timesync_mode timesync_mode_from_str(const std::string &mode);
 
+/**
+ * @brief Conversion from height above geoid (AMSL)
+ * to height above ellipsoid (WGS-84)
+ */
+template <class T>
+inline double geoid_to_ellipsoid_height(T lla){
+	GeographicLib::Geoid egm96("egm96-5");
+	return GeographicLib::Geoid::GEOIDTOELLIPSOID
+	       * egm96(lla->latitude, lla->longitude);
+}
+
+/**
+ * @brief Conversion from height above ellipsoid (WGS-84)
+ * to height above geoid (AMSL)
+ */
+template <class T>
+inline double ellipsoid_to_geoid_height(T lla) {
+	GeographicLib::Geoid egm96("egm96-5");
+	return GeographicLib::Geoid::ELLIPSOIDTOGEOID
+	       * egm96(lla->latitude, lla->longitude);
+}
 }	// namespace utils
 }	// namespace mavros
diff --git a/mavros/package.xml b/mavros/package.xml
@@ -25,6 +25,8 @@
   <build_export_depend>eigen</build_export_depend>
   <build_depend>mavlink</build_depend>
   <build_export_depend>mavlink</build_export_depend>
+  <build_depend>geographiclib</build_depend>
+  <build_export_depend>geographiclib</build_export_depend>
   <!-- pymavlink dependency
   <exec_depend>mavlink</exec_depend>
   -->

diff --git a/mavros/src/plugins/global_position.cpp b/mavros/src/plugins/global_position.cpp
@@ -19,7 +19,6 @@
 #include <angles/angles.h>
 #include <mavros/mavros_plugin.h>
 #include <eigen_conversions/eigen_msg.h>
-#include <GeographicLib/Geoid.hpp>
 #include <GeographicLib/GeoCoords.hpp>
 
 #include <std_msgs/Float64.h>
@@ -43,8 +42,7 @@ class GlobalPositionPlugin : public plugin::PluginBase {
 	GlobalPositionPlugin() : PluginBase(),
 		gp_nh("~global_position"),
 		tf_send(false),
-		rot_cov(99999.0),
-		egm96("egm96-5")
+		rot_cov(99999.0)
 	{ }
 
 	void initialize(UAS &uas_)
@@ -97,31 +95,11 @@ class GlobalPositionPlugin : public plugin::PluginBase {
 	bool tf_send;
 	double rot_cov;
 
-	GeographicLib::Geoid egm96;
-
-	/*
-	 * @brief Conversion from height above geoid (AMSL)
-	 * to height above ellipsoid (WGS-84)
-	 */
-	inline double geoid_to_ellipsoid_height(sensor_msgs::NavSatFix::Ptr fix) {
-		return GeographicLib::Geoid::GEOIDTOELLIPSOID
-					* egm96(fix->latitude, fix->longitude);
-	}
-
-	/*
-	 * @brief Conversion from height above ellipsoid (WGS-84)
-	 * to height above geoid (AMSL)
-	 */
-	inline double ellipsoid_to_geoid_height(sensor_msgs::NavSatFix::Ptr fix) {
-		return GeographicLib::Geoid::ELLIPSOIDTOGEOID
-					* egm96(fix->latitude, fix->longitude);
-	}
-
 	template<typename MsgT>
 	inline void fill_lla(MsgT &msg, sensor_msgs::NavSatFix::Ptr fix) {
 		fix->latitude = msg.lat / 1E7;		// deg
 		fix->longitude = msg.lon / 1E7;		// deg
-		fix->altitude = msg.alt / 1E3 + geoid_to_ellipsoid_height(fix); // in meters
+		fix->altitude = msg.alt / 1E3 + utils::geoid_to_ellipsoid_height(fix);	// in meters
 	}
 
 	inline void fill_unknown_cov(sensor_msgs::NavSatFix::Ptr fix) {