collision_distance_field, port to ROS 2

moveit_core/collision_distance_field/CMakeLists.txt

@@ -1,11 +1,6 @@
 set(MOVEIT_LIB_NAME moveit_collision_distance_field)
 
-if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
-  # clang is picky about new virtual functions hiding existing ones
-  add_compile_options(-Wno-overloaded-virtual)
-endif()
-
-add_library(${MOVEIT_LIB_NAME}
+add_library(${MOVEIT_LIB_NAME} SHARED
   src/collision_distance_field_types.cpp
   src/collision_common_distance_field.cpp
   src/collision_robot_distance_field.cpp
@@ -14,23 +9,54 @@ add_library(${MOVEIT_LIB_NAME}
   src/collision_world_hybrid.cpp
 )
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-target_link_libraries(${MOVEIT_LIB_NAME}
-  moveit_planning_scene moveit_distance_field
-  ${catkin_LIBRARIES} ${Boost_LIBRARIES})
-add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
 
-if(CATKIN_ENABLE_TESTING)
-  find_package(moveit_resources REQUIRED)
-  include_directories(${moveit_resources_INCLUDE_DIRS})
-
-  catkin_add_gtest(test_collision_distance_field test/test_collision_distance_field.cpp)
-  target_link_libraries(test_collision_distance_field  ${MOVEIT_LIB_NAME})
-endif()
+ament_target_dependencies(${MOVEIT_LIB_NAME}
+  moveit_planning_scene
+  moveit_distance_field
+  urdf
+  visualization_msgs
+  tf2_eigen
+  angles
+)
 
+target_link_libraries(${MOVEIT_LIB_NAME}  
+  moveit_planning_scene
+  moveit_distance_field
+  moveit_collision_detection
+  moveit_robot_state
+  ${geometric_shapes_LIBRARIES}
+)
+
 install(TARGETS ${MOVEIT_LIB_NAME}
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+        LIBRARY DESTINATION lib
+        ARCHIVE DESTINATION lib
 )
 
-install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
+install(DIRECTORY include/ DESTINATION include)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(moveit_resources REQUIRED)
+  include_directories(${moveit_resources_INCLUDE_DIRS})
+  find_package(resource_retriever REQUIRED)
+
+  if(WIN32)
+    # TODO add windows paths
+  else()
+    set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../planning_scene;${CMAKE_CURRENT_BINARY_DIR}/../distance_field;${CMAKE_CURRENT_BINARY_DIR}/../collision_detection;${CMAKE_CURRENT_BINARY_DIR}/../robot_state;${CMAKE_CURRENT_BINARY_DIR}/../robot_model;${CMAKE_CURRENT_BINARY_DIR}/../utils")
+  endif()
+
+  ament_add_gtest(test_collision_distance_field test/test_collision_distance_field.cpp)
+  target_link_libraries(test_collision_distance_field
+    ${MOVEIT_LIB_NAME}
+    moveit_collision_detection
+    moveit_collision_distance_field
+    moveit_robot_state
+    ${geometric_shapes_LIBRARIES}
+    ${OCTOMAP_LIBRARIES}
+    ${srdfdom_LIBRARIES}
+    resource_retriever::resource_retriever
+    moveit_distance_field
+    moveit_planning_scene
+  )
+endif()

moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_common_distance_field.h

@@ -42,6 +42,7 @@
 #include <moveit/collision_detection/collision_common.h>
 #include <moveit/collision_detection/collision_world.h>
 #include <moveit/collision_distance_field/collision_distance_field_types.h>
+#include "rclcpp/rclcpp.hpp"
 
 namespace collision_detection
 {
@@ -178,6 +179,6 @@ PosedBodyPointDecompositionVectorPtr getAttachedBodyPointDecomposition(const rob
                                                                        double resolution);
 
 void getBodySphereVisualizationMarkers(const GroupStateRepresentationPtr& gsr, const std::string& reference_frame,
-                                       visualization_msgs::MarkerArray& body_marker_array);
+                                       visualization_msgs::msg::MarkerArray& body_marker_array);
 }
 #endif

moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h

@@ -51,11 +51,16 @@
 #include <moveit/macros/class_forward.h>
 #include <moveit/distance_field/distance_field.h>
 #include <moveit/distance_field/propagation_distance_field.h>
-#include <visualization_msgs/MarkerArray.h>
-#include <ros/console.h>
+#include <visualization_msgs/msg/marker_array.hpp>
+#include "rclcpp/clock.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/time.hpp"
+#include "rclcpp/utilities.hpp"
 
 namespace collision_detection
 {
+static rclcpp::Logger LOGGER_COLLISION_DISTANCE_FIELD =
+    rclcpp::get_logger("moveit").get_child("collision_distance_field");
 enum CollisionType
 {
   NONE = 0,
@@ -406,7 +411,7 @@ class PosedBodySphereDecompositionVector
   {
     if (i >= decomp_vector_.size())
     {
-      ROS_INFO_NAMED("collision_distance_field", "No body decomposition");
+      RCLCPP_INFO(LOGGER_COLLISION_DISTANCE_FIELD, "No body decomposition");
       return empty_ptr_;
     }
     return decomp_vector_[i];
@@ -416,7 +421,7 @@ class PosedBodySphereDecompositionVector
   {
     if (ind >= decomp_vector_.size())
     {
-      ROS_WARN_NAMED("collision_distance_field", "Can't update pose");
+      RCLCPP_WARN(LOGGER_COLLISION_DISTANCE_FIELD, "Can't update pose");
       return;
     }
     decomp_vector_[ind]->updatePose(pose);
@@ -469,7 +474,7 @@ class PosedBodyPointDecompositionVector
   {
     if (i >= decomp_vector_.size())
     {
-      ROS_INFO_NAMED("collision_distance_field", "No body decomposition");
+      RCLCPP_INFO(LOGGER_COLLISION_DISTANCE_FIELD, "No body decomposition");
       return empty_ptr_;
     }
     return decomp_vector_[i];
@@ -483,7 +488,7 @@ class PosedBodyPointDecompositionVector
     }
     else
     {
-      ROS_WARN_NAMED("collision_distance_field", "Can't update pose");
+      RCLCPP_WARN(LOGGER_COLLISION_DISTANCE_FIELD, "Can't update pose");
       return;
     }
   }
@@ -509,20 +514,20 @@ struct ProximityInfo
 bool doBoundingSpheresIntersect(const PosedBodySphereDecompositionConstPtr& p1,
                                 const PosedBodySphereDecompositionConstPtr& p2);
 
-void getCollisionSphereMarkers(const std_msgs::ColorRGBA& color, const std::string& frame_id, const std::string& ns,
-                               const ros::Duration& dur,
+void getCollisionSphereMarkers(const std_msgs::msg::ColorRGBA& color, const std::string& frame_id,
+                               const std::string& ns, const rclcpp::Duration& dur,
                                const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
-                               visualization_msgs::MarkerArray& arr);
+                               visualization_msgs::msg::MarkerArray& arr);
 
-void getProximityGradientMarkers(const std::string& frame_id, const std::string& ns, const ros::Duration& dur,
+void getProximityGradientMarkers(const std::string& frame_id, const std::string& ns, const rclcpp::Duration& dur,
                                  const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
                                  const std::vector<PosedBodySphereDecompositionVectorPtr>& posed_vector_decompositions,
-                                 const std::vector<GradientInfo>& gradients, visualization_msgs::MarkerArray& arr);
+                                 const std::vector<GradientInfo>& gradients, visualization_msgs::msg::MarkerArray& arr);
 
-void getCollisionMarkers(const std::string& frame_id, const std::string& ns, const ros::Duration& dur,
+void getCollisionMarkers(const std::string& frame_id, const std::string& ns, const rclcpp::Duration& dur,
                          const std::vector<PosedBodySphereDecompositionPtr>& posed_decompositions,
                          const std::vector<PosedBodySphereDecompositionVectorPtr>& posed_vector_decompositions,
-                         const std::vector<GradientInfo>& gradients, visualization_msgs::MarkerArray& arr);
+                         const std::vector<GradientInfo>& gradients, visualization_msgs::msg::MarkerArray& arr);
 }
 
 #endif

moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_robot_distance_field.h

@@ -43,6 +43,7 @@
 #include <moveit/collision_distance_field/collision_common_distance_field.h>
 #include <moveit/planning_scene/planning_scene.h>
 #include <boost/thread/mutex.hpp>
+#include "rclcpp/rclcpp.hpp"
 
 namespace collision_detection
 {
@@ -54,6 +55,9 @@ static const double DEFAULT_RESOLUTION = .02;
 static const double DEFAULT_COLLISION_TOLERANCE = 0.0;
 static const double DEFAULT_MAX_PROPOGATION_DISTANCE = .25;
 
+static rclcpp::Logger LOGGER_COLLISION_ROBOT_DISTANCE_FIELD =
+    LOGGER_COLLISION_DISTANCE_FIELD.get_child("collision_robot_distance_field");
+
 MOVEIT_CLASS_FORWARD(CollisionRobotDistanceField);
 
 class CollisionRobotDistanceField : public CollisionRobot
@@ -102,37 +106,37 @@ class CollisionRobotDistanceField : public CollisionRobot
   void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
                           const moveit::core::RobotState& state1, const moveit::core::RobotState& state2) const override
   {
-    ROS_ERROR_NAMED("collision_distance_field", "Not implemented");
+    RCLCPP_ERROR(LOGGER_COLLISION_ROBOT_DISTANCE_FIELD, "Not implemented");
   };
 
   void checkSelfCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
                           const moveit::core::RobotState& state1, const moveit::core::RobotState& state2,
                           const collision_detection::AllowedCollisionMatrix& acm) const override
   {
-    ROS_ERROR_NAMED("collision_distance_field", "Not implemented");
+    RCLCPP_ERROR(LOGGER_COLLISION_ROBOT_DISTANCE_FIELD, "Not implemented");
   };
 
   void checkOtherCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
                            const moveit::core::RobotState& state, const CollisionRobot& other_robot,
                            const moveit::core::RobotState& other_state) const override
   {
-    ROS_ERROR_NAMED("collision_distance_field", "Not implemented");
+    RCLCPP_ERROR(LOGGER_COLLISION_ROBOT_DISTANCE_FIELD, "Not implemented");
   };
 
   void checkOtherCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
                            const moveit::core::RobotState& state, const CollisionRobot& other_robot,
                            const moveit::core::RobotState& other_state,
                            const collision_detection::AllowedCollisionMatrix& acm) const override
   {
-    ROS_ERROR_NAMED("collision_distance_field", "Not implemented");
+    RCLCPP_ERROR(LOGGER_COLLISION_ROBOT_DISTANCE_FIELD, "Not implemented");
   };
 
   void checkOtherCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
                            const moveit::core::RobotState& state1, const moveit::core::RobotState& state2,
                            const CollisionRobot& other_robot, const moveit::core::RobotState& other_state1,
                            const moveit::core::RobotState& other_state2) const override
   {
-    ROS_ERROR_NAMED("collision_distance_field", "Not implemented");
+    RCLCPP_ERROR(LOGGER_COLLISION_ROBOT_DISTANCE_FIELD, "Not implemented");
   };
 
   void checkOtherCollision(const collision_detection::CollisionRequest& req, collision_detection::CollisionResult& res,
@@ -141,11 +145,11 @@ class CollisionRobotDistanceField : public CollisionRobot
                            const moveit::core::RobotState& other_state2,
                            const collision_detection::AllowedCollisionMatrix& acm) const override
   {
-    ROS_ERROR_NAMED("collision_distance_field", "Not implemented");
+    RCLCPP_ERROR(LOGGER_COLLISION_ROBOT_DISTANCE_FIELD, "Not implemented");
   };
 
   void createCollisionModelMarker(const moveit::core::RobotState& state,
-                                  visualization_msgs::MarkerArray& model_markers) const;
+                                  visualization_msgs::msg::MarkerArray& model_markers) const;
 
   virtual double distanceSelf(const moveit::core::RobotState& state) const
   {
@@ -171,13 +175,13 @@ class CollisionRobotDistanceField : public CollisionRobot
   void distanceSelf(const DistanceRequest& req, DistanceResult& res,
                     const robot_state::RobotState& state) const override
   {
-    ROS_ERROR_NAMED("collision_distance_field", "Not implemented");
+    RCLCPP_ERROR(LOGGER_COLLISION_ROBOT_DISTANCE_FIELD, "Not implemented");
   }
 
   void distanceOther(const DistanceRequest& req, DistanceResult& res, const robot_state::RobotState& state,
                      const CollisionRobot& other_robot, const robot_state::RobotState& other_state) const override
   {
-    ROS_ERROR_NAMED("collision_distance_field", "Not implemented");
+    RCLCPP_ERROR(LOGGER_COLLISION_ROBOT_DISTANCE_FIELD, "Not implemented");
   }
 
   DistanceFieldCacheEntryConstPtr getLastDistanceFieldEntry() const

moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_world_distance_field.h

@@ -41,11 +41,14 @@
 #include <moveit/collision_detection/collision_world.h>
 #include <moveit/collision_distance_field/collision_distance_field_types.h>
 #include <moveit/collision_distance_field/collision_robot_distance_field.h>
+#include <chrono>
+#include "rclcpp/rclcpp.hpp"
 
 namespace collision_detection
 {
 MOVEIT_CLASS_FORWARD(CollisionWorldDistanceField)
-
+static rclcpp::Logger LOGGER_COLLISION_WORLD_DISTANCE_FIELD =
+    LOGGER_COLLISION_DISTANCE_FIELD.get_child("collision_world_distance_field");
 class CollisionWorldDistanceField : public CollisionWorld
 {
 public:
@@ -143,12 +146,12 @@ class CollisionWorldDistanceField : public CollisionWorld
   void distanceRobot(const DistanceRequest& req, DistanceResult& res, const CollisionRobot& robot,
                      const robot_state::RobotState& state) const override
   {
-    ROS_ERROR_NAMED("collision_distance_field", "Not implemented");
+    RCLCPP_ERROR(LOGGER_COLLISION_DISTANCE_FIELD, "Not implemented");
   }
 
   void distanceWorld(const DistanceRequest& req, DistanceResult& res, const CollisionWorld& world) const override
   {
-    ROS_ERROR_NAMED("collision_distance_field", "Not implemented");
+    RCLCPP_ERROR(LOGGER_COLLISION_DISTANCE_FIELD, "Not implemented");
   }
 
   void setWorld(const WorldPtr& world) override;

moveit_core/collision_distance_field/src/collision_common_distance_field.cpp

@@ -35,7 +35,6 @@
 /* Author: E. Gil Jones */
 
 #include <moveit/collision_distance_field/collision_common_distance_field.h>
-#include <ros/console.h>
 #include <boost/thread/mutex.hpp>
 #include <tf2_eigen/tf2_eigen.h>
 #include <memory>
@@ -129,39 +128,39 @@ PosedBodyPointDecompositionVectorPtr getAttachedBodyPointDecomposition(const rob
 }
 
 void getBodySphereVisualizationMarkers(const GroupStateRepresentationConstPtr& gsr, const std::string& reference_frame,
-                                       visualization_msgs::MarkerArray& body_marker_array)
+                                       visualization_msgs::msg::MarkerArray& body_marker_array)
 {
   // creating namespaces
   std::string robot_ns = gsr->dfce_->group_name_ + "_sphere_decomposition";
   std::string attached_ns = "attached_sphere_decomposition";
 
   // creating colors
-  std_msgs::ColorRGBA robot_color;
+  std_msgs::msg::ColorRGBA robot_color;
   robot_color.r = 0;
   robot_color.b = 0.8f;
   robot_color.g = 0;
   robot_color.a = 0.5;
 
-  std_msgs::ColorRGBA attached_color;
+  std_msgs::msg::ColorRGBA attached_color;
   attached_color.r = 1;
   attached_color.g = 1;
   attached_color.b = 0;
   attached_color.a = 0.5;
 
   // creating sphere marker
-  visualization_msgs::Marker sphere_marker;
+  visualization_msgs::msg::Marker sphere_marker;
   sphere_marker.header.frame_id = reference_frame;
-  sphere_marker.header.stamp = ros::Time(0);
+  sphere_marker.header.stamp = rclcpp::Time(0);
   sphere_marker.ns = robot_ns;
   sphere_marker.id = 0;
-  sphere_marker.type = visualization_msgs::Marker::SPHERE;
-  sphere_marker.action = visualization_msgs::Marker::ADD;
+  sphere_marker.type = visualization_msgs::msg::Marker::SPHERE;
+  sphere_marker.action = visualization_msgs::msg::Marker::ADD;
   sphere_marker.pose.orientation.x = 0;
   sphere_marker.pose.orientation.y = 0;
   sphere_marker.pose.orientation.z = 0;
   sphere_marker.pose.orientation.w = 1;
   sphere_marker.color = robot_color;
-  sphere_marker.lifetime = ros::Duration(0);
+  sphere_marker.lifetime = rclcpp::Duration(0, 0);
 
   const moveit::core::RobotState& state = *(gsr->dfce_->state_);
   unsigned int id = 0;
@@ -194,15 +193,15 @@ void getBodySphereVisualizationMarkers(const GroupStateRepresentationConstPtr& g
     const moveit::core::AttachedBody* att = state.getAttachedBody(gsr->dfce_->attached_body_names_[i]);
     if (!att)
     {
-      ROS_WARN("Attached body '%s' was not found, skipping sphere "
-               "decomposition visualization",
-               gsr->dfce_->attached_body_names_[i].c_str());
+      RCLCPP_WARN(LOGGER_COLLISION_DISTANCE_FIELD, "Attached body '%s' was not found, skipping sphere "
+                                                   "decomposition visualization",
+                  gsr->dfce_->attached_body_names_[i].c_str());
       continue;
     }
 
     if (gsr->attached_body_decompositions_[i]->getSize() != att->getShapes().size())
     {
-      ROS_WARN("Attached body size discrepancy");
+      RCLCPP_WARN(LOGGER_COLLISION_DISTANCE_FIELD, "Attached body size discrepancy");
       continue;
     }