Port common_planning_interface_objects to ROS 2 (moveit#159)

JafarAbdi · Jan 31, 2020 · 0d50f50 · 0d50f50
2 parents d366cfb + 019854c
commit 0d50f50
Show file tree

Hide file tree

Showing 6 changed files with 86 additions and 105 deletions.
diff --git a/moveit_ros/planning_interface/CMakeLists.txt b/moveit_ros/planning_interface/CMakeLists.txt
@@ -8,22 +8,20 @@ if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE)
   set(CMAKE_BUILD_TYPE Release)
 endif()
 
-find_package(catkin REQUIRED COMPONENTS
-  moveit_msgs
-  moveit_ros_planning
-  moveit_ros_warehouse
-  moveit_ros_manipulation
-  moveit_ros_move_group
-  geometry_msgs
-  tf2
-  tf2_eigen
-  tf2_geometry_msgs
-  tf2_ros
-  roscpp
-  actionlib
-  rospy
-  rosconsole
-)
+find_package(ament_cmake REQUIRED)
+find_package(moveit_msgs REQUIRED)
+find_package(moveit_ros_planning REQUIRED)
+# find_package(moveit_ros_warehouse REQUIRED)
+# find_package(moveit_ros_manipulation REQUIRED)
+# find_package(moveit_ros_move_group REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_eigen REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(rclpy REQUIRED)
 
 find_package(PythonInterp REQUIRED)
 find_package(PythonLibs "${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}" REQUIRED)
@@ -44,52 +42,42 @@ find_package(Boost REQUIRED COMPONENTS
   thread
 )
 find_package(Eigen3 REQUIRED)
-find_package(eigenpy REQUIRED)
+# find_package(eigenpy REQUIRED)
 
 set(THIS_PACKAGE_INCLUDE_DIRS
-  py_bindings_tools/include
+#  py_bindings_tools/include
   common_planning_interface_objects/include
-  planning_scene_interface/include
-  move_group_interface/include
-  moveit_cpp/include
+#  planning_scene_interface/include
+#  move_group_interface/include
+#  moveit_cpp/include
 )
 
-catkin_python_setup()
-
-catkin_package(
-  LIBRARIES
-    moveit_common_planning_interface_objects
-    moveit_planning_scene_interface
-    moveit_move_group_interface
-    moveit_cpp
-  INCLUDE_DIRS
-    ${THIS_PACKAGE_INCLUDE_DIRS}
-  CATKIN_DEPENDS
-    actionlib
-    moveit_ros_planning
-    moveit_ros_warehouse
-    moveit_ros_manipulation
-    moveit_ros_move_group
-    geometry_msgs
-    moveit_msgs
-    tf2_geometry_msgs
-  DEPENDS
-    EIGEN3
-)
+# catkin_python_setup()
 
-include_directories(${THIS_PACKAGE_INCLUDE_DIRS}
-                    ${Boost_INCLUDE_DIRS}
-                    ${catkin_INCLUDE_DIRS})
 
-include_directories(SYSTEM
-                    ${EIGEN3_INCLUDE_DIRS}
-                    ${eigenpy_INCLUDE_DIRS}
-                    ${PYTHON_INCLUDE_DIRS})
+include_directories(${THIS_PACKAGE_INCLUDE_DIRS})
 
-add_subdirectory(py_bindings_tools)
+# add_subdirectory(py_bindings_tools)
 add_subdirectory(common_planning_interface_objects)
-add_subdirectory(planning_scene_interface)
-add_subdirectory(move_group_interface)
-add_subdirectory(robot_interface)
-add_subdirectory(test)
-add_subdirectory(moveit_cpp)
+# add_subdirectory(planning_scene_interface)
+# add_subdirectory(move_group_interface)
+# add_subdirectory(robot_interface)
+# add_subdirectory(test)
+# add_subdirectory(moveit_cpp)
+
+ament_export_include_directories(include)
+ament_export_dependencies(moveit_msgs)
+ament_export_dependencies(moveit_ros_planning)
+ament_export_dependencies(geometry_msgs)
+ament_export_dependencies(tf2)
+ament_export_dependencies(tf2_eigen)
+ament_export_dependencies(tf2_geometry_msgs)
+ament_export_dependencies(tf2_ros)
+ament_export_dependencies(rclcpp)
+ament_export_dependencies(rclcpp_action)
+ament_export_dependencies(rclpy)
+ament_export_dependencies(PythonInterp)
+ament_export_dependencies(PythonLibs)
+ament_export_dependencies(Eigen3)
+ament_export_dependencies(Boost)
+ament_package()
diff --git a/moveit_ros/planning_interface/COLCON_IGNORE b/moveit_ros/planning_interface/COLCON_IGNORE
diff --git a/moveit_ros/planning_interface/common_planning_interface_objects/CMakeLists.txt b/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})
diff --git a/...nning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h b/...nning_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
diff --git a/moveit_ros/planning_interface/common_planning_interface_objects/src/common_objects.cpp b/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
diff --git a/moveit_ros/planning_interface/package.xml b/moveit_ros/planning_interface/package.xml
@@ -15,28 +15,31 @@
   <url type="bugtracker">https://github.com/ros-planning/moveit/issues</url>
   <url type="repository">https://github.com/ros-planning/moveit</url>
 
-  <buildtool_depend>catkin</buildtool_depend>
-  <buildtool_depend>python-catkin-pkg</buildtool_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>moveit_ros_planning</depend>
-  <depend>moveit_ros_warehouse</depend>
-  <depend>moveit_ros_move_group</depend>
-  <depend>moveit_ros_manipulation</depend>
-  <depend>roscpp</depend>
-  <depend>rospy</depend>
-  <depend>rosconsole</depend>
-  <depend>actionlib</depend>
+  <!-- TODO(JafarAbdi): uncomment after porting moveit_ros_warehouse -->
+  <!--depend>moveit_ros_warehouse</depend-->
+  <!-- TODO(JafarAbdi): uncomment after porting moveit_ros_move_group -->
+  <!-- depend>moveit_ros_move_group</depend-->
+  <!-- TODO(JafarAbdi): uncomment after porting moveit_ros_manipulation -->
+  <!--depend>moveit_ros_manipulation</depend-->
+  <depend>rclcpp</depend>
+  <depend>rclpy</depend>
+  <depend>rclcpp_action</depend>
   <depend>geometry_msgs</depend>
   <depend>moveit_msgs</depend>
   <depend>tf2</depend>
   <depend>tf2_eigen</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>python</depend>
-  <depend>eigenpy</depend>
+  <!--depend>eigenpy</depend-->
 
   <build_depend>eigen</build_depend>
 
   <test_depend>moveit_resources</test_depend>
-  <test_depend>rostest</test_depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
 </package>