Port collision_detection to ROS2

moveit_core/collision_detection/CMakeLists.txt

@@ -1,6 +1,6 @@
 set(MOVEIT_LIB_NAME moveit_collision_detection)
 
-add_library(${MOVEIT_LIB_NAME}
+add_library(${MOVEIT_LIB_NAME} SHARED
   src/allvalid/collision_robot_allvalid.cpp
   src/allvalid/collision_world_allvalid.cpp
   src/collision_matrix.cpp
@@ -11,26 +11,64 @@ add_library(${MOVEIT_LIB_NAME}
   src/world.cpp
   src/world_diff.cpp
 )
-set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-
-target_link_libraries(${MOVEIT_LIB_NAME} moveit_robot_state ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})
-add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
+set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION})
+ament_target_dependencies(${MOVEIT_LIB_NAME}
+  moveit_robot_state
+  rclcpp
+  rmw_implementation
+  urdf
+  urdfdom
+  urdfdom_headers
+  visualization_msgs
+  tf2_eigen
+  Boost
+  geometric_shapes
+)
 
 # unit tests
-if(CATKIN_ENABLE_TESTING)
-  catkin_add_gtest(test_world test/test_world.cpp)
-  target_link_libraries(test_world ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})
+if(BUILD_TESTING)
+	find_package(resource_retriever REQUIRED)
+	if(WIN32)
+		# set(append_library_dirs "$<TARGET_FILE_DIR:${PROJECT_NAME}>;$<TARGET_FILE_DIR:${PROJECT_NAME}_TestPlugins1>")
+	else()
+		set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../robot_model;${CMAKE_CURRENT_BINARY_DIR}/../utils")
+	endif()
 
-  catkin_add_gtest(test_world_diff test/test_world_diff.cpp)
-  target_link_libraries(test_world_diff ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})
+  ament_add_gtest(test_world test/test_world.cpp
+		APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  target_link_libraries(test_world
+    ${MOVEIT_LIB_NAME}
+    ${urdfdom}
+    ${urdfdom_headers}
+    ${Boost_LIBRARIES}
+		${geometric_shapes_LIBRARIES}
+	)
 
-  catkin_add_gtest(test_all_valid test/test_all_valid.cpp)
-  target_link_libraries(test_all_valid ${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})
-endif()
+	ament_add_gtest(test_world_diff test/test_world_diff.cpp
+		APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  target_link_libraries(test_world_diff
+    ${MOVEIT_LIB_NAME}
+    ${urdfdom}
+    ${urdfdom_headers}
+    ${Boost_LIBRARIES}
+		${geometric_shapes_LIBRARIES}
+	)
 
+	ament_add_gtest(test_all_valid test/test_all_valid.cpp
+		APPEND_LIBRARY_DIRS "${append_library_dirs}")
+  target_link_libraries(test_all_valid
+    ${MOVEIT_LIB_NAME}
+		moveit_robot_model
+    ${urdfdom}
+    ${urdfdom_headers}
+    ${Boost_LIBRARIES}
+		${geometric_shapes_LIBRARIES}
+		resource_retriever::resource_retriever
+	)
+endif()
 
 install(TARGETS ${MOVEIT_LIB_NAME}
-        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+        LIBRARY DESTINATION lib
+        ARCHIVE DESTINATION lib)
 
-install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
+install(DIRECTORY include/ DESTINATION include)

moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_detector_allocator_allvalid.h

@@ -45,12 +45,12 @@ namespace collision_detection
 {
 /** \brief An allocator for AllValid collision detectors */
 class CollisionDetectorAllocatorAllValid
-    : public CollisionDetectorAllocatorTemplate<CollisionWorldAllValid, CollisionRobotAllValid,
-                                                CollisionDetectorAllocatorAllValid>
+  : public CollisionDetectorAllocatorTemplate<CollisionWorldAllValid, CollisionRobotAllValid,
+                                              CollisionDetectorAllocatorAllValid>
 {
 public:
   static const std::string NAME;  // defined in collision_world_allvalid.cpp
 };
-}
+}  // namespace collision_detection
 
 #endif

moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_robot_allvalid.h

@@ -83,6 +83,6 @@ class CollisionRobotAllValid : public CollisionRobot
   void distanceOther(const DistanceRequest& req, DistanceResult& res, const robot_state::RobotState& state,
                      const CollisionRobot& other_robot, const robot_state::RobotState& other_state) const override;
 };
-}
+}  // namespace collision_detection
 
 #endif

moveit_core/collision_detection/include/moveit/collision_detection/allvalid/collision_world_allvalid.h

@@ -74,6 +74,6 @@ class CollisionWorldAllValid : public CollisionWorld
   virtual double distanceWorld(const CollisionWorld& world, const AllowedCollisionMatrix& acm) const;
   void distanceWorld(const DistanceRequest& req, DistanceResult& res, const CollisionWorld& world) const override;
 };
-}
+}  // namespace collision_detection
 
 #endif

moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h

@@ -65,7 +65,7 @@ enum Type
   /** \brief A body in the environment */
   WORLD_OBJECT
 };
-}
+}  // namespace BodyTypes
 
 /** \brief The types of bodies that are considered for collision */
 typedef BodyTypes::Type BodyType;
@@ -381,6 +381,6 @@ struct DistanceResult
     distances.clear();
   }
 };
-}
+}  // namespace collision_detection
 
 #endif

moveit_core/collision_detection/include/moveit/collision_detection/collision_detector_allocator.h

@@ -107,6 +107,6 @@ class CollisionDetectorAllocatorTemplate : public CollisionDetectorAllocator
     return CollisionDetectorAllocatorPtr(new CollisionDetectorAllocatorType());
   }
 };
-}
+}  // namespace collision_detection
 
 #endif

moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h

@@ -149,7 +149,7 @@ class AllowedCollisionMatrix
    *  @param name2 name of second element
    *  @param fn A callback function that is used to decide if collisions are allowed between the two elements is
    * expected here */
-  void setEntry(const std::string& name1, const std::string& name2, const DecideContactFn& fn);
+  void setEntry(const std::string& name1, const std::string& name2, DecideContactFn& fn);
 
   /** @brief Set the entries corresponding to a name. With each of the the known names in the collision matrix, form a
    * pair using the name
@@ -222,7 +222,7 @@ class AllowedCollisionMatrix
    *  @param name The name of the element for which to set the default value
    *  @param fn A callback function that is used to decide if collisions are allowed between \e name and some other
    * element is expected here. */
-  void setDefaultEntry(const std::string& name, const DecideContactFn& fn);
+  void setDefaultEntry(const std::string& name, DecideContactFn& fn);
 
   /** @brief Get the type of the allowed collision between to be considered by default for an element. Return true if a
    * default value was
@@ -277,6 +277,6 @@ class AllowedCollisionMatrix
   std::map<std::string, AllowedCollision::Type> default_entries_;
   std::map<std::string, DecideContactFn> default_allowed_contacts_;
 };
-}
+}  // namespace collision_detection
 
 #endif

moveit_core/collision_detection/include/moveit/collision_detection/collision_octomap_filter.h

@@ -61,6 +61,6 @@ namespace collision_detection
 int refineContactNormals(const World::ObjectConstPtr& object, CollisionResult& res,
                          double cell_bbx_search_distance = 1.0, double allowed_angle_divergence = 0.0,
                          bool estimate_depth = false, double iso_value = 0.5, double metaball_radius_multiple = 1.5);
-}
+}  // namespace collision_detection
 
 #endif

moveit_core/collision_detection/include/moveit/collision_detection/collision_robot.h

@@ -308,6 +308,6 @@ class CollisionRobot
   /** @brief The internally maintained map (from link names to scaling)*/
   std::map<std::string, double> link_scale_;
 };
-}
+}  // namespace collision_detection
 
 #endif

moveit_core/collision_detection/include/moveit/collision_detection/collision_tools.h

@@ -40,24 +40,29 @@
 #include <moveit/collision_detection/collision_common.h>
 #include <moveit_msgs/msg/cost_source.hpp>
 #include <moveit_msgs/msg/contact_information.hpp>
-#include <visualization_msgs/MarkerArray.h>
+#include <visualization_msgs/msg/marker_array.hpp>
+
+#include "rclcpp/rclcpp.hpp"
+#include "rcl/time.h"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/time.hpp"
 
 namespace collision_detection
 {
-void getCollisionMarkersFromContacts(visualization_msgs::MarkerArray& arr, const std::string& frame_id,
-                                     const CollisionResult::ContactMap& con, const std_msgs::ColorRGBA& color,
-                                     const ros::Duration& lifetime, const double radius = 0.035);
+void getCollisionMarkersFromContacts(visualization_msgs::msg::MarkerArray& arr, const std::string& frame_id,
+                                     const CollisionResult::ContactMap& con, const std_msgs::msg::ColorRGBA& color,
+                                     const rclcpp::Duration& lifetime, const double radius = 0.035);
 
-void getCollisionMarkersFromContacts(visualization_msgs::MarkerArray& arr, const std::string& frame_id,
+void getCollisionMarkersFromContacts(visualization_msgs::msg::MarkerArray& arr, const std::string& frame_id,
                                      const CollisionResult::ContactMap& con);
 
 /// \todo add a class for managing cost sources
-void getCostMarkers(visualization_msgs::MarkerArray& arr, const std::string& frame_id,
+void getCostMarkers(visualization_msgs::msg::MarkerArray& arr, const std::string& frame_id,
                     std::set<CostSource>& cost_sources);
 
-void getCostMarkers(visualization_msgs::MarkerArray& arr, const std::string& frame_id,
-                    std::set<CostSource>& cost_sources, const std_msgs::ColorRGBA& color,
-                    const ros::Duration& lifetime);
+void getCostMarkers(visualization_msgs::msg::MarkerArray& arr, const std::string& frame_id,
+                    std::set<CostSource>& cost_sources, const std_msgs::msg::ColorRGBA& color,
+                    const rclcpp::Duration& lifetime);
 
 double getTotalCost(const std::set<CostSource>& cost_sources);
 
@@ -67,10 +72,10 @@ void intersectCostSources(std::set<CostSource>& cost_sources, const std::set<Cos
                           const std::set<CostSource>& b);
 void removeOverlapping(std::set<CostSource>& cost_sources, double overlap_fraction);
 
-bool getSensorPositioning(geometry_msgs::Point& point, const std::set<CostSource>& cost_sources);
+bool getSensorPositioning(geometry_msgs::msg::Point& point, const std::set<CostSource>& cost_sources);
 
 void costSourceToMsg(const CostSource& cost_source, moveit_msgs::msg::CostSource& msg);
 void contactToMsg(const Contact& contact, moveit_msgs::msg::ContactInformation& msg);
-}
+}  // namespace collision_detection
 
 #endif

moveit_core/collision_detection/include/moveit/collision_detection/collision_world.h

@@ -277,6 +277,6 @@ class CollisionWorld : private boost::noncopyable
   WorldPtr world_;             // The world.  Always valid.  Never NULL.
   WorldConstPtr world_const_;  // always same as world_
 };
-}
+}  // namespace collision_detection
 
 #endif

moveit_core/collision_detection/include/moveit/collision_detection/world.h

@@ -38,7 +38,6 @@
 #define MOVEIT_COLLISION_DETECTION_WORLD_
 
 #include <moveit/macros/class_forward.h>
-
 #include <string>
 #include <vector>
 #include <map>
@@ -274,6 +273,6 @@ class World
   };
   std::vector<Observer*> observers_;
 };
-}
+}  // namespace collision_detection
 
 #endif

moveit_core/collision_detection/include/moveit/collision_detection/world_diff.h

@@ -125,6 +125,6 @@ class WorldDiff
   /* used to unregister the notifier */
   WorldWeakPtr world_;
 };
-}
+}  // namespace collision_detection
 
 #endif

moveit_core/collision_detection/src/allvalid/collision_robot_allvalid.cpp

@@ -35,6 +35,10 @@
 /* Author: Ioan Sucan, Jia Pan */
 
 #include <moveit/collision_detection/allvalid/collision_robot_allvalid.h>
+#include "rclcpp/rclcpp.hpp"
+
+// Logger
+rclcpp::Logger LOGGER_COLLISION_ROBOT_ALLVALID = rclcpp::get_logger("moveit").get_child("collision_robot_allvalid");;
 
 collision_detection::CollisionRobotAllValid::CollisionRobotAllValid(const robot_model::RobotModelConstPtr& robot_model,
                                                                     double padding, double scale)
@@ -51,7 +55,7 @@ void collision_detection::CollisionRobotAllValid::checkSelfCollision(const Colli
 {
   res.collision = false;
   if (req.verbose)
-    ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
+    RCLCPP_INFO(LOGGER_COLLISION_ROBOT_ALLVALID, "Using AllValid collision detection. No collision checking is performed.");
 }
 
 void collision_detection::CollisionRobotAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
@@ -60,7 +64,7 @@ void collision_detection::CollisionRobotAllValid::checkSelfCollision(const Colli
 {
   res.collision = false;
   if (req.verbose)
-    ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
+    RCLCPP_INFO(LOGGER_COLLISION_ROBOT_ALLVALID, "Using AllValid collision detection. No collision checking is performed.");
 }
 
 void collision_detection::CollisionRobotAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
@@ -69,7 +73,7 @@ void collision_detection::CollisionRobotAllValid::checkSelfCollision(const Colli
 {
   res.collision = false;
   if (req.verbose)
-    ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
+    RCLCPP_INFO(LOGGER_COLLISION_ROBOT_ALLVALID, "Using AllValid collision detection. No collision checking is performed.");
 }
 
 void collision_detection::CollisionRobotAllValid::checkSelfCollision(const CollisionRequest& req, CollisionResult& res,
@@ -79,7 +83,7 @@ void collision_detection::CollisionRobotAllValid::checkSelfCollision(const Colli
 {
   res.collision = false;
   if (req.verbose)
-    ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
+    RCLCPP_INFO(LOGGER_COLLISION_ROBOT_ALLVALID, "Using AllValid collision detection. No collision checking is performed.");
 }
 
 void collision_detection::CollisionRobotAllValid::checkOtherCollision(const CollisionRequest& req, CollisionResult& res,
@@ -89,7 +93,7 @@ void collision_detection::CollisionRobotAllValid::checkOtherCollision(const Coll
 {
   res.collision = false;
   if (req.verbose)
-    ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
+    RCLCPP_INFO(LOGGER_COLLISION_ROBOT_ALLVALID, "Using AllValid collision detection. No collision checking is performed.");
 }
 
 void collision_detection::CollisionRobotAllValid::checkOtherCollision(const CollisionRequest& req, CollisionResult& res,
@@ -100,7 +104,7 @@ void collision_detection::CollisionRobotAllValid::checkOtherCollision(const Coll
 {
   res.collision = false;
   if (req.verbose)
-    ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
+    RCLCPP_INFO(LOGGER_COLLISION_ROBOT_ALLVALID, "Using AllValid collision detection. No collision checking is performed.");
 }
 
 void collision_detection::CollisionRobotAllValid::checkOtherCollision(const CollisionRequest& req, CollisionResult& res,
@@ -112,7 +116,7 @@ void collision_detection::CollisionRobotAllValid::checkOtherCollision(const Coll
 {
   res.collision = false;
   if (req.verbose)
-    ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
+    RCLCPP_INFO(LOGGER_COLLISION_ROBOT_ALLVALID, "Using AllValid collision detection. No collision checking is performed.");
 }
 
 void collision_detection::CollisionRobotAllValid::checkOtherCollision(const CollisionRequest& req, CollisionResult& res,
@@ -125,7 +129,7 @@ void collision_detection::CollisionRobotAllValid::checkOtherCollision(const Coll
 {
   res.collision = false;
   if (req.verbose)
-    ROS_INFO_NAMED("collision_detection", "Using AllValid collision detection. No collision checking is performed.");
+    RCLCPP_INFO(LOGGER_COLLISION_ROBOT_ALLVALID, "Using AllValid collision detection. No collision checking is performed.");
 }
 
 double collision_detection::CollisionRobotAllValid::distanceSelf(const robot_state::RobotState& state) const