Merge pull request #15 from AcutronicRobotics/distance_field

Port Distance field

README.md

@@ -54,7 +54,7 @@ The MoveIt! Motion Planning Framework **for ROS 2.0**
     - [x] planning_interface
     - [x] planning_request_adapter
     - [ ] trajectory_processing
-    - [ ] distance_field
+    - [x] distance_field
     - [ ] collision_distance_field
     - [ ] kinematics_metrics
     - [ ] dynamics_solver

moveit_core/CMakeLists.txt

@@ -91,7 +91,7 @@ set(THIS_PACKAGE_INCLUDE_DIRS
     collision_detection_fcl/include
     constraint_samplers/include
     controller_manager/include
-    # distance_field/include
+    distance_field/include
     # collision_distance_field/include
     # dynamics_solver/include
     kinematics_base/include
@@ -197,7 +197,7 @@ add_subdirectory(constraint_samplers)
 add_subdirectory(planning_interface)
 add_subdirectory(planning_request_adapter)
 add_subdirectory(trajectory_processing)
-# add_subdirectory(distance_field)
+add_subdirectory(distance_field)
 # add_subdirectory(collision_distance_field)
 # add_subdirectory(kinematics_metrics)
 # add_subdirectory(dynamics_solver)

moveit_core/distance_field/CMakeLists.txt

@@ -7,19 +7,25 @@ add_library(${MOVEIT_LIB_NAME}
   )
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 
-target_link_libraries(${MOVEIT_LIB_NAME} ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})
-add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
+ament_target_dependencies(${MOVEIT_LIB_NAME}
+  urdfdom
+  urdfdom_headers
+  visualization_msgs
+  tf2_eigen)
 
 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(CATKIN_ENABLE_TESTING)
-  catkin_add_gtest(test_voxel_grid test/test_voxel_grid.cpp)
-  target_link_libraries(test_voxel_grid ${MOVEIT_LIB_NAME})
-
-  catkin_add_gtest(test_distance_field test/test_distance_field.cpp)
-  target_link_libraries(test_distance_field ${MOVEIT_LIB_NAME})
-endif()
+# if(BUILD_TESTING)
+#   find_package(ament_cmake_gtest REQUIRED)
+#   ament_add_gtest(test_voxel_grid test/test_voxel_grid.cpp)
+#   ament_target_dependencies(test_voxel_grid
+#     ${MOVEIT_LIB_NAME})
+#   ament_add_gtest(test_distance_field test/test_distance_field.cpp)
+#   ament_target_dependencies(test_distance_field
+#     ${MOVEIT_LIB_NAME}
+#     visualization_msgs
+#     tf2_eigen)
+# 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,7 +614,7 @@ 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,
+  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 */

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,10 +38,11 @@
 #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>
 
+rclcpp::Logger logger = rclcpp::get_logger("distance_field");
+
 namespace distance_field
 {
 DistanceField::DistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x,
@@ -86,15 +87,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 +131,7 @@ 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 +155,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 +202,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, "Problem dynamic casting shape that claims to be OcTree");
       return false;
     }
     getOcTreePoints(oc->octree.get(), points);
@@ -224,7 +225,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 +290,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, "Move shape not supported for Octree");
     return;
   }
   bodies::Body* body = bodies::createBodyFromShape(shape);
@@ -304,8 +305,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 +325,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 +443,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 +458,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 +504,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 +558,4 @@ void DistanceField::getProjectionPlanes(const std::string& frame_id, const ros::
     delete[] z_projection;
 }
 
-}  // end of namespace distance_field
+}  // end of namespace distance_field