Port common_planning_interface_objects to ROS2

moveit_ros/planning_interface/common_planning_interface_objects/CMakeLists.txt

@@ -1,12 +1,18 @@
 set(MOVEIT_LIB_NAME moveit_common_planning_interface_objects)
 
-add_library(${MOVEIT_LIB_NAME} src/common_objects.cpp)
+add_library(${MOVEIT_LIB_NAME} SHARED src/common_objects.cpp)
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES} ${PYTHON_LIBRARIES} ${Boost_LIBRARIES})
+ament_target_dependencies(${MOVEIT_LIB_NAME}
+    rclcpp
+    moveit_ros_planning
+    tf2_ros
+)
 
 install(TARGETS ${MOVEIT_LIB_NAME}
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin)
 
-install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
+install(DIRECTORY include/ DESTINATION include)
+
+ament_export_libraries(${MOVEIT_LIB_NAME})

moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h

@@ -36,6 +36,8 @@
 
 #pragma once
 
+#include <memory>
+#include <tf2_ros/buffer.h>
 #include <moveit/planning_scene_monitor/current_state_monitor.h>
 
 namespace moveit
@@ -44,31 +46,19 @@ namespace planning_interface
 {
 std::shared_ptr<tf2_ros::Buffer> getSharedTF();
 
-robot_model::RobotModelConstPtr getSharedRobotModel(const std::string& robot_description);
-
-/**
-  @brief getSharedStateMonitor is a simpler version of getSharedStateMonitor(const robot_model::RobotModelConstPtr
-  &robot_model, const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
-    ros::NodeHandle nh = ros::NodeHandle() ). It calls this function using the default constructed ros::NodeHandle
-
-  @param robot_model
-  @param tf_buffer
-  @return
- */
-planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr& robot_model,
-                                                                     const std::shared_ptr<tf2_ros::Buffer>& tf_buffer);
+robot_model::RobotModelConstPtr getSharedRobotModel(const rclcpp::Node::SharedPtr& node,
+                                                    const std::string& robot_description);
 
 /**
   @brief getSharedStateMonitor
 
+  @param node A rclcpp::Node::SharePtr to pass node specific configurations, such as callbacks queues.
   @param robot_model
   @param tf_buffer
-  @param nh A ros::NodeHandle to pass node specific configurations, such as callbacks queues.
   @return
  */
-planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr& robot_model,
-                                                                     const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
-                                                                     const ros::NodeHandle& nh);
-
-}  // namespace planning interface
+planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const rclcpp::Node::SharedPtr& node,
+                                                                     const robot_model::RobotModelConstPtr& robot_model,
+                                                                     const std::shared_ptr<tf2_ros::Buffer>& tf_buffer);
+}  // namespace planning_interface
 }  // namespace moveit

moveit_ros/planning_interface/common_planning_interface_objects/src/common_objects.cpp

@@ -105,15 +105,16 @@ std::shared_ptr<tf2_ros::Buffer> getSharedTF()
   std::shared_ptr<tf2_ros::Buffer> buffer = s.tf_buffer_.lock();
   if (!buffer)
   {
-    tf2_ros::Buffer* raw = new tf2_ros::Buffer();
+    tf2_ros::Buffer* raw = new tf2_ros::Buffer(std::make_shared<rclcpp::Clock>(RCL_ROS_TIME));
     // assign custom deleter to also delete associated TransformListener
     buffer.reset(raw, Deleter(new tf2_ros::TransformListener(*raw)));
     s.tf_buffer_ = buffer;
   }
   return buffer;
 }
 
-robot_model::RobotModelConstPtr getSharedRobotModel(const std::string& robot_description)
+robot_model::RobotModelConstPtr getSharedRobotModel(const rclcpp::Node::SharedPtr& node,
+                                                    const std::string& robot_description)
 {
   SharedStorage& s = getSharedStorage();
   boost::mutex::scoped_lock slock(s.lock_);
@@ -123,23 +124,17 @@ robot_model::RobotModelConstPtr getSharedRobotModel(const std::string& robot_des
   {
     RobotModelLoader::Options opt(robot_description);
     opt.load_kinematics_solvers_ = true;
-    RobotModelLoaderPtr loader(new RobotModelLoader(opt));
+    RobotModelLoaderPtr loader(new RobotModelLoader(node, opt));
     // create an aliasing shared_ptr
     model = robot_model::RobotModelPtr(loader, loader->getModel().get());
     it->second = model;
   }
   return model;
 }
 
-CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr& robot_model,
+CurrentStateMonitorPtr getSharedStateMonitor(const rclcpp::Node::SharedPtr& node,
+                                             const robot_model::RobotModelConstPtr& robot_model,
                                              const std::shared_ptr<tf2_ros::Buffer>& tf_buffer)
-{
-  return getSharedStateMonitor(robot_model, tf_buffer, ros::NodeHandle());
-}
-
-CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr& robot_model,
-                                             const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
-                                             const ros::NodeHandle& nh)
 {
   SharedStorage& s = getSharedStorage();
   boost::mutex::scoped_lock slock(s.lock_);
@@ -148,11 +143,10 @@ CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstP
   if (!monitor)
   {
     // if there was no valid entry, create one
-    monitor.reset(new CurrentStateMonitor(robot_model, tf_buffer, nh));
+    monitor.reset(new CurrentStateMonitor(node, robot_model, tf_buffer));
     it->second = monitor;
   }
   return monitor;
 }
-
 }  // namespace planning_interface
 }  // namespace moveit

moveit_ros/planning_interface/package.xml

@@ -16,7 +16,6 @@
   <url type="repository">https://github.com/ros-planning/moveit</url>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
-  <buildtool_depend>python-catkin-pkg</buildtool_depend>
 
   <depend>moveit_ros_planning</depend>
   <!-- TODO(JafarAbdi): uncomment after porting moveit_ros_warehouse -->