Port distance_field to ROS 2 (#64)

* distance_field, adapt logging #21
* moveit_core distance field - library as SHARED
* Fixing distance_field tests
* moveit_core distance field - Fixing logger variable
* moveit_core distance_field - converting time to seconds

moveit_core/distance_field/CMakeLists.txt

@@ -1,25 +1,47 @@
 set(MOVEIT_LIB_NAME moveit_distance_field)
 
-add_library(${MOVEIT_LIB_NAME}
+add_library(${MOVEIT_LIB_NAME} SHARED
   src/distance_field.cpp
   src/find_internal_points.cpp
   src/propagation_distance_field.cpp
-  )
+)
+
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 
-target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})
-add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
+ament_target_dependencies(${MOVEIT_LIB_NAME}
+  boost
+  urdfdom
+  urdfdom_headers
+  visualization_msgs
+  tf2_eigen)
+
+target_link_libraries(${MOVEIT_LIB_NAME}
+  ${Boost_LIBRARIES}
+  ${geometric_shapes_LIBRARIES}
+)
 
 install(TARGETS ${MOVEIT_LIB_NAME}
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+        LIBRARY DESTINATION lib
+        ARCHIVE DESTINATION lib)
+install(DIRECTORY include/ DESTINATION include)
 
-install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
 
-if(CATKIN_ENABLE_TESTING)
-  catkin_add_gtest(test_voxel_grid test/test_voxel_grid.cpp)
-  target_link_libraries(test_voxel_grid ${MOVEIT_LIB_NAME})
+  ament_add_gtest(test_voxel_grid test/test_voxel_grid.cpp)
+  target_link_libraries(test_voxel_grid
+    ${MOVEIT_LIB_NAME}
+  )
+
+  ament_add_gtest(test_distance_field test/test_distance_field.cpp)
 
-  catkin_add_gtest(test_distance_field test/test_distance_field.cpp)
-  target_link_libraries(test_distance_field ${MOVEIT_LIB_NAME})
+  target_link_libraries(test_distance_field
+    ${MOVEIT_LIB_NAME}
+    ${visualization_msgs_LIBRARIES}
+    ${tf2_eigen_LIBRARIES}
+    ${Boost_LIBRARIES}
+    ${geometric_shapes_LIBRARIES}
+    ${OCTOMAP_LIBRARIES}
+    ${console_bridge_LIBRARIES}
+  )
 endif()

moveit_core/distance_field/include/moveit/distance_field/distance_field.h

@@ -42,12 +42,13 @@
 #include <moveit/distance_field/voxel_grid.h>
 #include <vector>
 #include <list>
-#include <visualization_msgs/Marker.h>
-#include <visualization_msgs/MarkerArray.h>
+#include <visualization_msgs/msg/marker.hpp>
+#include <visualization_msgs/msg/marker_array.hpp>
 #include <Eigen/Core>
 #include <Eigen/Geometry>
 #include <eigen_stl_containers/eigen_stl_containers.h>
 #include <moveit/macros/class_forward.h>
+#include "rclcpp/rclcpp.hpp"
 
 namespace shapes
 {
@@ -192,7 +193,7 @@ class DistanceField
   void addShapeToField(const shapes::Shape* shape, const Eigen::Isometry3d& pose);
 
   // DEPRECATED form
-  MOVEIT_DEPRECATED void addShapeToField(const shapes::Shape* shape, const geometry_msgs::Pose& pose);
+  MOVEIT_DEPRECATED void addShapeToField(const shapes::Shape* shape, const geometry_msgs::msg::Pose& pose);
 
   /**
    * \brief Adds an octree to the distance field.  Cells that are
@@ -231,8 +232,8 @@ class DistanceField
                         const Eigen::Isometry3d& new_pose);
 
   // DEPRECATED form
-  MOVEIT_DEPRECATED void moveShapeInField(const shapes::Shape* shape, const geometry_msgs::Pose& old_pose,
-                                          const geometry_msgs::Pose& new_pose);
+  MOVEIT_DEPRECATED void moveShapeInField(const shapes::Shape* shape, const geometry_msgs::msg::Pose& old_pose,
+                                          const geometry_msgs::msg::Pose& new_pose);
 
   /**
    * \brief All points corresponding to the shape are removed from the
@@ -246,7 +247,7 @@ class DistanceField
   void removeShapeFromField(const shapes::Shape* shape, const Eigen::Isometry3d& pose);
 
   // DEPRECATED form
-  MOVEIT_DEPRECATED void removeShapeFromField(const shapes::Shape* shape, const geometry_msgs::Pose& pose);
+  MOVEIT_DEPRECATED void removeShapeFromField(const shapes::Shape* shape, const geometry_msgs::msg::Pose& pose);
 
   /**
    * \brief Resets all points in the distance field to an uninitialize
@@ -430,7 +431,7 @@ class DistanceField
    * \brief Get an iso-surface for visualization in rviz.  The
    * iso-surface shows every cell that has a distance in a given
    * range in the distance field.  The cells are added as a
-   * visualization_msgs::Marker::CUBE_LIST in the namespace
+   * visualization_msgs::msg::Marker::CUBE_LIST in the namespace
    * "distance_field".
    *
    * @param [in] min_distance Cells of less than this distance will not be added to the marker
@@ -440,13 +441,13 @@ class DistanceField
    * @param [out] marker The marker that will contain the indicated cells.
    */
   void getIsoSurfaceMarkers(double min_distance, double max_distance, const std::string& frame_id,
-                            const ros::Time stamp, visualization_msgs::Marker& marker) const;
+                            const rclcpp::Time stamp, visualization_msgs::msg::Marker& marker) const;
 
   /**
    * \brief Populates the supplied marker array with a series of
    * arrows representing gradients of cells that are within the
    * supplied range in terms of distance.  The markers will be
-   * visualization_msgs::Marker::ARROW in the namespace
+   * visualization_msgs::msg::Marker::ARROW in the namespace
    * "distance_field_gradient".
    *
    * @param [in] min_distance Cells of less than this distance will not be added to the marker
@@ -455,8 +456,8 @@ class DistanceField
    * @param [in] stamp The stamp to use in the header of the marker
    * @param [out] marker_array The marker array to populate
    */
-  void getGradientMarkers(double min_radius, double max_radius, const std::string& frame_id, const ros::Time& stamp,
-                          visualization_msgs::MarkerArray& marker_array) const;
+  void getGradientMarkers(double min_radius, double max_radius, const std::string& frame_id, const rclcpp::Time& stamp,
+                          visualization_msgs::msg::MarkerArray& marker_array) const;
 
   /**
    * \brief Populates a marker with a slice of the distance field in a
@@ -484,8 +485,8 @@ class DistanceField
    * @param [out] marker The marker that will contain the indicated cells.
    */
   void getPlaneMarkers(PlaneVisualizationType type, double length, double width, double height,
-                       const Eigen::Vector3d& origin, const std::string& frame_id, const ros::Time stamp,
-                       visualization_msgs::Marker& marker) const;
+                       const Eigen::Vector3d& origin, const std::string& frame_id, const rclcpp::Time stamp,
+                       visualization_msgs::msg::Marker& marker) const;
   /**
    * \brief A function that populates the marker with three planes -
    * one each along the XY, XZ, and YZ axes.  For each of the planes,
@@ -500,10 +501,10 @@ class DistanceField
    * the marker.
    *
    * @param [out] marker The marker, which will be populated with a
-   * visualization_msgs::Marker::CUBE_LIST .
+   * visualization_msgs::msg::Marker::CUBE_LIST .
    */
-  void getProjectionPlanes(const std::string& frame_id, const ros::Time& stamp, double max_distance,
-                           visualization_msgs::Marker& marker) const;
+  void getProjectionPlanes(const std::string& frame_id, const rclcpp::Time& stamp, double max_distance,
+                           visualization_msgs::msg::Marker& marker) const;
 
   /**
    * \brief Gets the distance field size along the X dimension in meters
@@ -613,8 +614,8 @@ class DistanceField
    *
    * @param [in] max_distance The distance past which all cells will be fully white
    */
-  void setPoint(int xCell, int yCell, int zCell, double dist, geometry_msgs::Point& point, std_msgs::ColorRGBA& color,
-                double max_distance) const;
+  void setPoint(int xCell, int yCell, int zCell, double dist, geometry_msgs::msg::Point& point,
+                std_msgs::msg::ColorRGBA& color, double max_distance) const;
 
   double size_x_;            /**< \brief X size of the distance field */
   double size_y_;            /**< \brief Y size of the distance field */

moveit_core/distance_field/include/moveit/distance_field/propagation_distance_field.h

@@ -44,6 +44,7 @@
 #include <Eigen/Core>
 #include <set>
 #include <octomap/octomap.h>
+#include "rclcpp/rclcpp.hpp"
 
 namespace EigenSTL
 {

moveit_core/distance_field/src/distance_field.cpp

@@ -38,12 +38,14 @@
 #include <moveit/distance_field/find_internal_points.h>
 #include <geometric_shapes/body_operations.h>
 #include <tf2_eigen/tf2_eigen.h>
-#include <ros/console.h>
 #include <octomap/octomap.h>
 #include <octomap/OcTree.h>
+#include "rclcpp/rclcpp.hpp"
 
 namespace distance_field
 {
+rclcpp::Logger LOGGER_DISTANCE_FIELD = rclcpp::get_logger("moveit").get_child("distance_field");
+
 DistanceField::DistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x,
                              double origin_y, double origin_z)
   : size_x_(size_x)
@@ -86,15 +88,15 @@ double DistanceField::getDistanceGradient(double x, double y, double z, double&
 }
 
 void DistanceField::getIsoSurfaceMarkers(double min_distance, double max_distance, const std::string& frame_id,
-                                         const ros::Time stamp, visualization_msgs::Marker& inf_marker) const
+                                         const rclcpp::Time stamp, visualization_msgs::msg::Marker& inf_marker) const
 {
   inf_marker.points.clear();
   inf_marker.header.frame_id = frame_id;
   inf_marker.header.stamp = stamp;
   inf_marker.ns = "distance_field";
   inf_marker.id = 1;
-  inf_marker.type = visualization_msgs::Marker::CUBE_LIST;
-  inf_marker.action = visualization_msgs::Marker::MODIFY;
+  inf_marker.type = visualization_msgs::msg::Marker::CUBE_LIST;
+  inf_marker.action = visualization_msgs::msg::Marker::MODIFY;
   inf_marker.scale.x = resolution_;
   inf_marker.scale.y = resolution_;
   inf_marker.scale.z = resolution_;
@@ -130,7 +132,8 @@ void DistanceField::getIsoSurfaceMarkers(double min_distance, double max_distanc
 }
 
 void DistanceField::getGradientMarkers(double min_distance, double max_distance, const std::string& frame_id,
-                                       const ros::Time& stamp, visualization_msgs::MarkerArray& marker_array) const
+                                       const rclcpp::Time& stamp,
+                                       visualization_msgs::msg::MarkerArray& marker_array) const
 {
   Eigen::Vector3d unit_x(1, 0, 0);
   Eigen::Vector3d unit_y(0, 1, 0);
@@ -154,15 +157,15 @@ void DistanceField::getGradientMarkers(double min_distance, double max_distance,
 
         if (in_bounds && distance >= min_distance && distance <= max_distance && gradient.norm() > 0)
         {
-          visualization_msgs::Marker marker;
+          visualization_msgs::msg::Marker marker;
 
           marker.header.frame_id = frame_id;
           marker.header.stamp = stamp;
 
           marker.ns = "distance_field_gradient";
           marker.id = id++;
-          marker.type = visualization_msgs::Marker::ARROW;
-          marker.action = visualization_msgs::Marker::ADD;
+          marker.type = visualization_msgs::msg::Marker::ARROW;
+          marker.action = visualization_msgs::msg::Marker::ADD;
 
           marker.pose.position.x = world_x;
           marker.pose.position.y = world_y;
@@ -201,7 +204,7 @@ bool DistanceField::getShapePoints(const shapes::Shape* shape, const Eigen::Isom
     const shapes::OcTree* oc = dynamic_cast<const shapes::OcTree*>(shape);
     if (!oc)
     {
-      ROS_ERROR_NAMED("distance_field", "Problem dynamic casting shape that claims to be OcTree");
+      RCLCPP_ERROR(LOGGER_DISTANCE_FIELD, "Problem dynamic casting shape that claims to be OcTree");
       return false;
     }
     getOcTreePoints(oc->octree.get(), points);
@@ -224,7 +227,7 @@ void DistanceField::addShapeToField(const shapes::Shape* shape, const Eigen::Iso
 }
 
 // DEPRECATED
-void DistanceField::addShapeToField(const shapes::Shape* shape, const geometry_msgs::Pose& pose)
+void DistanceField::addShapeToField(const shapes::Shape* shape, const geometry_msgs::msg::Pose& pose)
 {
   Eigen::Isometry3d pose_e;
   tf2::fromMsg(pose, pose_e);
@@ -289,7 +292,7 @@ void DistanceField::moveShapeInField(const shapes::Shape* shape, const Eigen::Is
 {
   if (shape->type == shapes::OCTREE)
   {
-    ROS_WARN_NAMED("distance_field", "Move shape not supported for Octree");
+    RCLCPP_WARN(LOGGER_DISTANCE_FIELD, "Move shape not supported for Octree");
     return;
   }
   bodies::Body* body = bodies::createBodyFromShape(shape);
@@ -304,8 +307,8 @@ void DistanceField::moveShapeInField(const shapes::Shape* shape, const Eigen::Is
 }
 
 // DEPRECATED
-void DistanceField::moveShapeInField(const shapes::Shape* shape, const geometry_msgs::Pose& old_pose,
-                                     const geometry_msgs::Pose& new_pose)
+void DistanceField::moveShapeInField(const shapes::Shape* shape, const geometry_msgs::msg::Pose& old_pose,
+                                     const geometry_msgs::msg::Pose& new_pose)
 {
   Eigen::Isometry3d old_pose_e, new_pose_e;
   tf2::fromMsg(old_pose, old_pose_e);
@@ -324,23 +327,23 @@ void DistanceField::removeShapeFromField(const shapes::Shape* shape, const Eigen
 }
 
 // DEPRECATED
-void DistanceField::removeShapeFromField(const shapes::Shape* shape, const geometry_msgs::Pose& pose)
+void DistanceField::removeShapeFromField(const shapes::Shape* shape, const geometry_msgs::msg::Pose& pose)
 {
   Eigen::Isometry3d pose_e;
   tf2::fromMsg(pose, pose_e);
   removeShapeFromField(shape, pose_e);
 }
 
 void DistanceField::getPlaneMarkers(PlaneVisualizationType type, double length, double width, double height,
-                                    const Eigen::Vector3d& origin, const std::string& frame_id, const ros::Time stamp,
-                                    visualization_msgs::Marker& plane_marker) const
+                                    const Eigen::Vector3d& origin, const std::string& frame_id,
+                                    const rclcpp::Time stamp, visualization_msgs::msg::Marker& plane_marker) const
 {
   plane_marker.header.frame_id = frame_id;
   plane_marker.header.stamp = stamp;
   plane_marker.ns = "distance_field_plane";
   plane_marker.id = 1;
-  plane_marker.type = visualization_msgs::Marker::CUBE_LIST;
-  plane_marker.action = visualization_msgs::Marker::ADD;
+  plane_marker.type = visualization_msgs::msg::Marker::CUBE_LIST;
+  plane_marker.action = visualization_msgs::msg::Marker::ADD;
   plane_marker.scale.x = resolution_;
   plane_marker.scale.y = resolution_;
   plane_marker.scale.z = resolution_;
@@ -442,8 +445,8 @@ void DistanceField::getPlaneMarkers(PlaneVisualizationType type, double length,
   }
 }
 
-void DistanceField::setPoint(int xCell, int yCell, int zCell, double dist, geometry_msgs::Point& point,
-                             std_msgs::ColorRGBA& color, double max_distance) const
+void DistanceField::setPoint(int xCell, int yCell, int zCell, double dist, geometry_msgs::msg::Point& point,
+                             std_msgs::msg::ColorRGBA& color, double max_distance) const
 {
   double wx, wy, wz;
   gridToWorld(xCell, yCell, zCell, wx, wy, wz);
@@ -457,8 +460,8 @@ void DistanceField::setPoint(int xCell, int yCell, int zCell, double dist, geome
   color.b = dist / max_distance;  // dist/max_distance * 0.1;
 }
 
-void DistanceField::getProjectionPlanes(const std::string& frame_id, const ros::Time& stamp, double max_dist,
-                                        visualization_msgs::Marker& marker) const
+void DistanceField::getProjectionPlanes(const std::string& frame_id, const rclcpp::Time& stamp, double max_dist,
+                                        visualization_msgs::msg::Marker& marker) const
 {
   int max_x_cell = getXNumCells();
   int max_y_cell = getYNumCells();
@@ -503,8 +506,8 @@ void DistanceField::getProjectionPlanes(const std::string& frame_id, const ros::
   marker.header.stamp = stamp;
   marker.ns = "distance_field_projection_plane";
   marker.id = 1;
-  marker.type = visualization_msgs::Marker::CUBE_LIST;
-  marker.action = visualization_msgs::Marker::MODIFY;
+  marker.type = visualization_msgs::msg::Marker::CUBE_LIST;
+  marker.action = visualization_msgs::msg::Marker::MODIFY;
   marker.scale.x = getResolution();
   marker.scale.y = getResolution();
   marker.scale.z = getResolution();
@@ -557,4 +560,4 @@ void DistanceField::getProjectionPlanes(const std::string& frame_id, const ros::
     delete[] z_projection;
 }
 
-}  // end of namespace distance_field
+}  // end of namespace distance_field