Merge pull request moveit#47 from AcutronicRobotics/common_planning_i…

…nterface_objects Port Common_planning_interface_objects to ROS2
JafarAbdi · Apr 17, 2019 · 27005bd · 27005bd
2 parents 2b106bc + 453142a
commit 27005bd
Show file tree

Hide file tree

Showing 5 changed files with 20 additions and 12 deletions.
diff --git a/moveit_ros/planning/CMakeLists.txt b/moveit_ros/planning/CMakeLists.txt
@@ -47,6 +47,7 @@ set(THIS_PACKAGE_INCLUDE_DIRS
 set(libraries
   moveit_rdf_loader
   moveit_kinematics_plugin_loader
+  moveit_robot_model_loader
   # moveit_robot_model_loader
   # moveit_constraint_sampler_manager_loader
   # moveit_planning_pipeline
@@ -64,6 +65,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   moveit_msgs
   tf2_msgs
   tf2_geometry_msgs
+  srdfdom
 )
 
 include_directories(${THIS_PACKAGE_INCLUDE_DIRS}
@@ -95,5 +97,5 @@ install(
 
 ament_export_include_directories(include)
 ament_export_libraries(${libraries})
-
+ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
 ament_package()
diff --git a/moveit_ros/planning_interface/CMakeLists.txt b/moveit_ros/planning_interface/CMakeLists.txt
@@ -13,7 +13,7 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(moveit_msgs REQUIRED)
 # TODO: uncomment this as submodules become available
-# find_package(moveit_ros_planning 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)
@@ -82,7 +82,7 @@ ament_export_include_directories(${THIS_PACKAGE_INCLUDE_DIRS})
 ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDENCIES})
 
 # add_subdirectory(py_bindings_tools)
-# add_subdirectory(common_planning_interface_objects)
+add_subdirectory(common_planning_interface_objects)
 # add_subdirectory(planning_scene_interface)
 # add_subdirectory(move_group_interface)
 # add_subdirectory(robot_interface)

diff --git a/moveit_ros/planning_interface/common_planning_interface_objects/CMakeLists.txt b/moveit_ros/planning_interface/common_planning_interface_objects/CMakeLists.txt
@@ -2,10 +2,14 @@ set(MOVEIT_LIB_NAME moveit_common_planning_interface_objects)
 
 add_library(${MOVEIT_LIB_NAME} 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
+  Boost
+  moveit_ros_planning
+  srdfdom)
 
 install(TARGETS ${MOVEIT_LIB_NAME}
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib)
 
-install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
+install(DIRECTORY include/ DESTINATION install)
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
@@ -38,6 +38,7 @@
 #define MOVEIT_PLANNING_INTERFACE_COMMON_OBJECTS_
 
 #include <moveit/planning_scene_monitor/current_state_monitor.h>
+#include "rclcpp/rclcpp.hpp"
 
 namespace moveit
 {
@@ -69,7 +70,7 @@ planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot
  */
 planning_scene_monitor::CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr& robot_model,
                                                                      const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
-                                                                     const ros::NodeHandle& nh);
+                                                                     const std::shared_ptr<rclcpp::Node> node);
 
 }  // 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
@@ -106,7 +106,8 @@ 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();
+    rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
+    tf2_ros::Buffer* raw = new tf2_ros::Buffer(clock);
     // assign custom deleter to also delete associated TransformListener
     buffer.reset(raw, Deleter(new tf2_ros::TransformListener(*raw)));
     s.tf_buffer_ = buffer;
@@ -135,12 +136,12 @@ robot_model::RobotModelConstPtr getSharedRobotModel(const std::string& robot_des
 CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr& robot_model,
                                              const std::shared_ptr<tf2_ros::Buffer>& tf_buffer)
 {
-  return getSharedStateMonitor(robot_model, tf_buffer, ros::NodeHandle());
+  return getSharedStateMonitor(robot_model, tf_buffer , rclcpp::Node::make_shared("default_common_object"));
 }
 
 CurrentStateMonitorPtr getSharedStateMonitor(const robot_model::RobotModelConstPtr& robot_model,
                                              const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
-                                             const ros::NodeHandle& nh)
+                                             const std::shared_ptr<rclcpp::Node> node)
 {
   SharedStorage& s = getSharedStorage();
   boost::mutex::scoped_lock slock(s.lock_);
@@ -149,7 +150,7 @@ 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(robot_model, tf_buffer, node));
     it->second = monitor;
   }
   return monitor;