Merge pull request moveit#60 from AcutronicRobotics/constraint_sample…

…r_manager_loader Port constraint_sampler_manager_loader to ROS2
JafarAbdi · May 7, 2019 · 0b79bbf · 0b79bbf
2 parents ada03e9 + e678658
commit 0b79bbf
Show file tree

Hide file tree

Showing 5 changed files with 26 additions and 17 deletions.
diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp
@@ -170,7 +170,7 @@ bool ompl_interface::OMPLInterface::loadConstraintApproximations()
 void ompl_interface::OMPLInterface::loadConstraintSamplers()
 {
   constraint_sampler_manager_loader_.reset(
-      new constraint_sampler_manager_loader::ConstraintSamplerManagerLoader(constraint_sampler_manager_));
+      new constraint_sampler_manager_loader::ConstraintSamplerManagerLoader(constraint_sampler_manager_,nh_));
 }
 
 bool ompl_interface::OMPLInterface::loadPlannerConfiguration(

diff --git a/moveit_ros/planning/CMakeLists.txt b/moveit_ros/planning/CMakeLists.txt
@@ -79,7 +79,7 @@ add_subdirectory(rdf_loader)
 add_subdirectory(collision_plugin_loader)
 add_subdirectory(kinematics_plugin_loader)
 add_subdirectory(robot_model_loader)
-# add_subdirectory(constraint_sampler_manager_loader)
+add_subdirectory(constraint_sampler_manager_loader)
 add_subdirectory(planning_pipeline)
 # add_subdirectory(planning_request_adapter_plugins)
 add_subdirectory(planning_scene_monitor)

diff --git a/moveit_ros/planning/constraint_sampler_manager_loader/CMakeLists.txt b/moveit_ros/planning/constraint_sampler_manager_loader/CMakeLists.txt
@@ -2,7 +2,11 @@ set(MOVEIT_LIB_NAME moveit_constraint_sampler_manager_loader)
 
 add_library(${MOVEIT_LIB_NAME} src/constraint_sampler_manager_loader.cpp)
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES})
+target_link_libraries(${MOVEIT_LIB_NAME} ${rclcpp_LIBRARIES})
 
-install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_GLOBAL_LIB_DESTINATION})
-install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
+install(TARGETS ${MOVEIT_LIB_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin)
+
+install(DIRECTORY include/ DESTINATION include)
diff --git a/...ader/include/moveit/constraint_sampler_manager_loader/constraint_sampler_manager_loader.h b/...ader/include/moveit/constraint_sampler_manager_loader/constraint_sampler_manager_loader.h
@@ -48,7 +48,8 @@ class ConstraintSamplerManagerLoader
 {
 public:
   ConstraintSamplerManagerLoader(
-      const constraint_samplers::ConstraintSamplerManagerPtr& csm = constraint_samplers::ConstraintSamplerManagerPtr());
+      const constraint_samplers::ConstraintSamplerManagerPtr& csm = constraint_samplers::ConstraintSamplerManagerPtr(),
+      const std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("constraint_sampler_manager_loader"));
 
   const constraint_samplers::ConstraintSamplerManagerPtr& getConstraintSamplerManager() const
   {

diff --git a/..._ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp b/..._ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp
@@ -36,7 +36,7 @@
 
 #include <moveit/constraint_sampler_manager_loader/constraint_sampler_manager_loader.h>
 #include <pluginlib/class_loader.hpp>
-#include <ros/ros.h>
+#include "rclcpp/rclcpp.hpp"
 #include <boost/tokenizer.hpp>
 #include <memory>
 
@@ -45,11 +45,15 @@ namespace constraint_sampler_manager_loader
 class ConstraintSamplerManagerLoader::Helper
 {
 public:
-  Helper(const constraint_samplers::ConstraintSamplerManagerPtr& csm) : nh_("~")
+  Helper(const constraint_samplers::ConstraintSamplerManagerPtr& csm, const std::shared_ptr<rclcpp::Node> node)
+    : node_(node)
   {
+    auto parameters_constraint_sampler = std::make_shared<rclcpp::SyncParametersClient>(node_);
+
     std::string constraint_samplers;
-    if (nh_.getParam("constraint_samplers", constraint_samplers))
+    if (parameters_constraint_sampler->has_parameter("constraint_samplers"))
     {
+      constraint_samplers = node_->get_parameter("constraint_samplers").get_value<std::string>();
       try
       {
         constraint_sampler_plugin_loader_.reset(
@@ -58,7 +62,7 @@ class ConstraintSamplerManagerLoader::Helper
       }
       catch (pluginlib::PluginlibException& ex)
       {
-        ROS_ERROR_STREAM("Exception while creating constraint sampling plugin loader " << ex.what());
+        RCLCPP_ERROR(node_->get_logger(), "Exception while creating constraint sampling plugin loader %s", ex.what());
         return;
       }
       boost::char_separator<char> sep(" ");
@@ -70,27 +74,27 @@ class ConstraintSamplerManagerLoader::Helper
           constraint_samplers::ConstraintSamplerAllocatorPtr csa =
               constraint_sampler_plugin_loader_->createUniqueInstance(*beg);
           csm->registerSamplerAllocator(csa);
-          ROS_INFO("Loaded constraint sampling plugin %s", std::string(*beg).c_str());
+          RCLCPP_INFO(node_->get_logger(), "Loaded constraint sampling plugin %s", std::string(*beg).c_str());
         }
         catch (pluginlib::PluginlibException& ex)
         {
-          ROS_ERROR_STREAM("Exception while planning adapter plugin '" << *beg << "': " << ex.what());
+          RCLCPP_ERROR(node_->get_logger(), "Exception while planning adapter plugin '%s': %s",
+                       std::string(*beg).c_str(), ex.what());
         }
       }
     }
   }
 
 private:
-  ros::NodeHandle nh_;
+  rclcpp::Node::SharedPtr node_;
   std::unique_ptr<pluginlib::ClassLoader<constraint_samplers::ConstraintSamplerAllocator> >
       constraint_sampler_plugin_loader_;
 };
-
-ConstraintSamplerManagerLoader::ConstraintSamplerManagerLoader(
-    const constraint_samplers::ConstraintSamplerManagerPtr& csm)
+    ConstraintSamplerManagerLoader::ConstraintSamplerManagerLoader(
+        const constraint_samplers::ConstraintSamplerManagerPtr& csm, const std::shared_ptr<rclcpp::Node> node)
   : constraint_sampler_manager_(csm ? csm : constraint_samplers::ConstraintSamplerManagerPtr(
                                                 new constraint_samplers::ConstraintSamplerManager()))
-  , impl_(new Helper(constraint_sampler_manager_))
+  , impl_(new Helper(constraint_sampler_manager_, node))
 {
 }
 }  // namespace constraint_sampler_manager_loader