Merge pull request #28 from SteveMacenski/master

ROS2 port of geographic_msgs and geographic_info for robot_localization

Author: SteveMacenski

geodesy/CHANGELOG.rst

@@ -1,6 +1,9 @@
 Change history
 ==============
 
+0.5.3 (2018-03-27)
+------------------
+
 0.5.2 (2017-04-15)
 ------------------
 

geodesy/CMakeLists.txt

@@ -1,56 +1,41 @@
-# http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(geodesy)
-find_package(catkin REQUIRED
-             COMPONENTS
-             angles
-             geographic_msgs
-             geometry_msgs
-             sensor_msgs
-             tf
-             unique_id
-             uuid_msgs)
+find_package(ament_cmake REQUIRED)
+find_package(angles REQUIRED)
+find_package(geographic_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(unique_identifier_msgs REQUIRED)
 
-include_directories(include ${catkin_INCLUDE_DIRS})
-catkin_python_setup()
+include_directories(
+  include
+)
 
-# what other packages will need to use this one
-catkin_package(CATKIN_DEPENDS
-               geographic_msgs
-               geometry_msgs
-               sensor_msgs
-               tf
-               unique_id
-               uuid_msgs
-               INCLUDE_DIRS include
-               LIBRARIES geoconv)
+set(dependencies
+  angles
+  geographic_msgs
+  geometry_msgs
+  sensor_msgs
+  unique_identifier_msgs
+)
+
+ament_python_install_package(${PROJECT_NAME} PACKAGE_DIR src/${PROJECT_NAME})
 
 # build C++ coordinate conversion library
-add_library(geoconv src/conv/utm_conversions.cpp)
-target_link_libraries(geoconv ${catkin_LIBRARIES})
-add_dependencies(geoconv ${catkin_EXPORTED_TARGETS})
+add_library(geoconv STATIC src/conv/utm_conversions.cpp)
+ament_target_dependencies(geoconv 
+  ${dependencies}
+)
 
 install(DIRECTORY include/${PROJECT_NAME}/
-        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
+        DESTINATION include/${PROJECT_NAME}/)
 install(TARGETS geoconv
-        RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
-        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
-
-# unit tests
-if (CATKIN_ENABLE_TESTING)
-
-  catkin_add_gtest(test_wgs84 tests/test_wgs84.cpp)
-  target_link_libraries(test_wgs84 ${catkin_LIBRARIES})
-  add_dependencies(test_wgs84 ${catkin_EXPORTED_TARGETS})
-  
-  catkin_add_gtest(test_utm tests/test_utm.cpp)
-  target_link_libraries(test_utm geoconv ${catkin_LIBRARIES})
-  add_dependencies(test_utm ${catkin_EXPORTED_TARGETS})
-  
-  catkin_add_nosetests(tests/test_bounding_box.py)
-  catkin_add_nosetests(tests/test_props.py)
-  catkin_add_nosetests(tests/test_utm.py)
-  catkin_add_nosetests(tests/test_wu_point.py)
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+        RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
 
-endif (CATKIN_ENABLE_TESTING)
+ament_export_dependencies(${dependencies})
+ament_export_include_directories(include)
+ament_export_libraries(geoconv)
+ament_package()

geodesy/include/geodesy/utm.h

@@ -43,11 +43,11 @@
 #include <iostream>
 #include <iomanip>
 
-#include <tf/tf.h>
-
 #include <geodesy/wgs84.h>
-#include <geometry_msgs/Point.h>
-#include <geometry_msgs/Pose.h>
+#include "geographic_msgs/msg/geo_point.hpp"
+#include "geometry_msgs/msg/point.hpp"
+#include "geographic_msgs/msg/geo_pose.hpp"
+#include "geometry_msgs/msg/pose.hpp"
 
 /** @file
 
@@ -101,7 +101,7 @@ class UTMPoint
     band(that.band)
   {}
 
-  UTMPoint(const geographic_msgs::GeoPoint &pt);
+  UTMPoint(const geographic_msgs::msg::GeoPoint &pt);
 
   /** Create a flattened 2-D grid point. */
   UTMPoint(double _easting, double _northing, uint8_t _zone, char _band):
@@ -149,36 +149,36 @@ class UTMPose
   {}
 
   /** Create from a WGS 84 geodetic pose. */
-  UTMPose(const geographic_msgs::GeoPose &pose):
+  UTMPose(const geographic_msgs::msg::GeoPose &pose):
     position(pose.position),
     orientation(pose.orientation)
   {}
 
   /** Create from a UTMPoint and a quaternion. */
   UTMPose(UTMPoint pt,
-          const geometry_msgs::Quaternion &q):
+          const geometry_msgs::msg::Quaternion &q):
     position(pt),
     orientation(q)
   {}
 
   /** Create from a WGS 84 geodetic point and a quaternion. */
-  UTMPose(const geographic_msgs::GeoPoint &pt,
-          const geometry_msgs::Quaternion &q):
+  UTMPose(const geographic_msgs::msg::GeoPoint &pt,
+          const geometry_msgs::msg::Quaternion &q):
     position(pt),
     orientation(q)
   {}
 
   // data members
   UTMPoint position;
-  geometry_msgs::Quaternion orientation;
+  geometry_msgs::msg::Quaternion orientation;
 
 }; // class UTMPose
 
 // conversion function prototypes
-void fromMsg(const geographic_msgs::GeoPoint &from, UTMPoint &to);
-void fromMsg(const geographic_msgs::GeoPose &from, UTMPose &to);
-geographic_msgs::GeoPoint toMsg(const UTMPoint &from);
-geographic_msgs::GeoPose toMsg(const UTMPose &from);
+void fromMsg(const geographic_msgs::msg::GeoPoint &from, UTMPoint &to);
+void fromMsg(const geographic_msgs::msg::GeoPose &from, UTMPose &to);
+geographic_msgs::msg::GeoPoint toMsg(const UTMPoint &from);
+geographic_msgs::msg::GeoPose toMsg(const UTMPose &from);
 
 /** @return true if no altitude specified. */
 static inline bool is2D(const UTMPoint &pt)
@@ -203,7 +203,7 @@ bool isValid(const UTMPose &pose);
  */
 static inline void normalize(UTMPoint &pt)
 {
-  geographic_msgs::GeoPoint ll(toMsg(pt));
+  geographic_msgs::msg::GeoPoint ll(toMsg(pt));
   normalize(ll);
   fromMsg(ll, pt);
 }
@@ -241,19 +241,19 @@ static inline bool sameGridZone(const UTMPose &pose1, const UTMPose &pose2)
 }
 
 /** @return a geometry Point corresponding to a UTM point. */
-static inline geometry_msgs::Point toGeometry(const UTMPoint &from)
+static inline geometry_msgs::msg::Point toGeometry(const UTMPoint &from)
 {
-  geometry_msgs::Point to;
+  geometry_msgs::msg::Point to;
   to.x = from.easting;
   to.y = from.northing;
   to.z = from.altitude;
   return to;
 }
 
 /** @return a geometry Pose corresponding to a UTM pose. */
-static inline geometry_msgs::Pose toGeometry(const UTMPose &from)
+static inline geometry_msgs::msg::Pose toGeometry(const UTMPose &from)
 {
-  geometry_msgs::Pose to;
+  geometry_msgs::msg::Pose to;
   to.position = toGeometry(from.position);
   to.orientation = from.orientation;
   return to;

geodesy/include/geodesy/wgs84.h

@@ -40,10 +40,11 @@
 
 #include <limits>
 #include <ctype.h>
-#include <geographic_msgs/GeoPoint.h>
-#include <geographic_msgs/GeoPose.h>
-#include <sensor_msgs/NavSatFix.h>
-#include <tf/tf.h>
+#include "geographic_msgs/msg/geo_point.hpp"
+#include "geographic_msgs/msg/geo_pose.hpp"
+#include "sensor_msgs/msg/nav_sat_fix.hpp"
+
+#define TF_QUATERNION_TOLERANCE 0.1
 
 /** @file
 
@@ -82,8 +83,8 @@ namespace geodesy
    *  @param from WGS 84 point message.
    *  @param to another point.
    */
-  static inline void fromMsg(const geographic_msgs::GeoPoint &from,
-                             geographic_msgs::GeoPoint &to)
+  static inline void fromMsg(const geographic_msgs::msg::GeoPoint &from,
+                             geographic_msgs::msg::GeoPoint &to)
   {
     convert(from, to);
   }
@@ -93,26 +94,26 @@ namespace geodesy
    *  @param from WGS 84 pose message.
    *  @param to another pose.
    */
-  static inline void fromMsg(const geographic_msgs::GeoPose &from,
-                             geographic_msgs::GeoPose &to)
+  static inline void fromMsg(const geographic_msgs::msg::GeoPose &from,
+                             geographic_msgs::msg::GeoPose &to)
   {
     convert(from, to);
   }
 
   /** @return true if no altitude specified. */
-  static inline bool is2D(const geographic_msgs::GeoPoint &pt)
+  static inline bool is2D(const geographic_msgs::msg::GeoPoint &pt)
   {
     return (pt.altitude != pt.altitude);
   }
 
   /** @return true if pose has no altitude. */
-  static inline bool is2D(const geographic_msgs::GeoPose &pose)
+  static inline bool is2D(const geographic_msgs::msg::GeoPose &pose)
   {
     return is2D(pose.position);
   }
 
   /** @return true if point is valid. */
-  static inline bool isValid(const geographic_msgs::GeoPoint &pt)
+  static inline bool isValid(const geographic_msgs::msg::GeoPoint &pt)
   {
     if (pt.latitude < -90.0 || pt.latitude > 90.0)
       return false;
@@ -124,14 +125,14 @@ namespace geodesy
   }
 
   /** @return true if pose is valid. */
-  static inline bool isValid(const geographic_msgs::GeoPose &pose)
+  static inline bool isValid(const geographic_msgs::msg::GeoPose &pose)
   {
     // check that orientation quaternion is normalized
     double len2 = (pose.orientation.x * pose.orientation.x
                    + pose.orientation.y * pose.orientation.y
                    + pose.orientation.z * pose.orientation.z
                    + pose.orientation.w * pose.orientation.w);
-    if (fabs(len2 - 1.0) > tf::QUATERNION_TOLERANCE)
+    if (fabs(len2 - 1.0) > TF_QUATERNION_TOLERANCE)
       return false;
 
     return isValid(pose.position);
@@ -144,60 +145,60 @@ namespace geodesy
    *  Normalizes the longitude to [-180, 180).
    *  Clamps latitude to [-90, 90].
    */
-  static inline void normalize(geographic_msgs::GeoPoint &pt)
+  static inline void normalize(geographic_msgs::msg::GeoPoint &pt)
   {
     pt.longitude =
       (fmod(fmod((pt.longitude + 180.0), 360.0) + 360.0, 360.0) - 180.0);
     pt.latitude = std::min(std::max(pt.latitude, -90.0), 90.0);
   }
 
   /** @return a 2D WGS 84 geodetic point message. */
-  static inline geographic_msgs::GeoPoint toMsg(double lat, double lon)
+  static inline geographic_msgs::msg::GeoPoint toMsg(double lat, double lon)
   {
-    geographic_msgs::GeoPoint pt;
+    geographic_msgs::msg::GeoPoint pt;
     pt.latitude = lat;
     pt.longitude = lon;
     pt.altitude = std::numeric_limits<double>::quiet_NaN();
     return pt;
   }
 
   /** @return a 3D WGS 84 geodetic point message. */
-  static inline geographic_msgs::GeoPoint
+  static inline geographic_msgs::msg::GeoPoint
     toMsg(double lat, double lon, double alt)
   {
-    geographic_msgs::GeoPoint pt;
+    geographic_msgs::msg::GeoPoint pt;
     pt.latitude = lat;
     pt.longitude = lon;
     pt.altitude = alt;
     return pt;
   }
 
   /** @return a WGS 84 geodetic point message from a NavSatFix. */
-  static inline geographic_msgs::GeoPoint
-    toMsg(const sensor_msgs::NavSatFix &fix)
+  static inline geographic_msgs::msg::GeoPoint
+    toMsg(const sensor_msgs::msg::NavSatFix &fix)
   {
-    geographic_msgs::GeoPoint pt;
+    geographic_msgs::msg::GeoPoint pt;
     pt.latitude = fix.latitude;
     pt.longitude = fix.longitude;
     pt.altitude = fix.altitude;
     return pt;
   }
 
   /** @return a WGS 84 geodetic point message from another. */
-  static inline geographic_msgs::GeoPoint
-    toMsg(const geographic_msgs::GeoPoint &from)
+  static inline geographic_msgs::msg::GeoPoint
+    toMsg(const geographic_msgs::msg::GeoPoint &from)
   {
     return from;
   }
 
   /** @return a WGS 84 geodetic pose message from a point and a
    *          quaternion.
    */
-  static inline geographic_msgs::GeoPose
-    toMsg(const geographic_msgs::GeoPoint &pt,
-          const geometry_msgs::Quaternion &q)
+  static inline geographic_msgs::msg::GeoPose
+    toMsg(const geographic_msgs::msg::GeoPoint &pt,
+          const geometry_msgs::msg::Quaternion &q)
   {
-    geographic_msgs::GeoPose pose;
+    geographic_msgs::msg::GeoPose pose;
     pose.position = pt;
     pose.orientation = q;
     return pose;
@@ -206,19 +207,19 @@ namespace geodesy
   /** @return a WGS 84 geodetic pose message from a NavSatFix and a
    *          quaternion.
    */
-  static inline geographic_msgs::GeoPose
-    toMsg(const sensor_msgs::NavSatFix &fix,
-          const geometry_msgs::Quaternion &q)
+  static inline geographic_msgs::msg::GeoPose
+    toMsg(const sensor_msgs::msg::NavSatFix &fix,
+          const geometry_msgs::msg::Quaternion &q)
   {
-    geographic_msgs::GeoPose pose;
+    geographic_msgs::msg::GeoPose pose;
     pose.position = toMsg(fix);
     pose.orientation = q;
     return pose;
   }
 
   /** @return a WGS 84 geodetic pose message from another. */
-  static inline geographic_msgs::GeoPose
-    toMsg(const geographic_msgs::GeoPose &from)
+  static inline geographic_msgs::msg::GeoPose
+    toMsg(const geographic_msgs::msg::GeoPose &from)
   {
     return from;
   }