Constraint samplers (#60)

moveit_core/constraint_samplers/CMakeLists.txt

@@ -1,6 +1,6 @@
 set(MOVEIT_LIB_NAME moveit_constraint_samplers)
 
-add_library(${MOVEIT_LIB_NAME}
+add_library(${MOVEIT_LIB_NAME} SHARED
   src/constraint_sampler.cpp
   src/constraint_sampler_manager.cpp
   src/constraint_sampler_tools.cpp
@@ -9,40 +9,73 @@ add_library(${MOVEIT_LIB_NAME}
 )
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 
-target_link_libraries(${MOVEIT_LIB_NAME}
+ament_target_dependencies(${MOVEIT_LIB_NAME}
   moveit_robot_state
   moveit_kinematic_constraints
   moveit_kinematics_base
   moveit_planning_scene
-  ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})
-add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
+  urdf
+  urdfdom
+  urdfdom_headers
+  visualization_msgs
+)
+
+target_link_libraries(${MOVEIT_LIB_NAME}
+  moveit_robot_trajectory
+  moveit_robot_state
+  moveit_kinematic_constraints
+)
 
 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)
 
-if(CATKIN_ENABLE_TESTING)
+if(BUILD_TESTING)
   find_package(orocos_kdl REQUIRED)
   find_package(angles REQUIRED)
   find_package(tf2_kdl REQUIRED)
   find_package(moveit_resources REQUIRED)
 
-include_directories(${orocos_kdl_INCLUDE_DIRS} ${angles_INCLUDE_DIRS} ${tf2_kdl_INCLUDE_DIRS} ${moveit_resources_INCLUDE_DIRS})
+  include_directories(${orocos_kdl_INCLUDE_DIRS} ${angles_INCLUDE_DIRS} ${tf2_kdl_INCLUDE_DIRS} ${moveit_resources_INCLUDE_DIRS})
+
+  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}/../utils;${CMAKE_CURRENT_BINARY_DIR}/../robot_state;${CMAKE_CURRENT_BINARY_DIR}/../planning_scene;${CMAKE_CURRENT_BINARY_DIR}/../robot_model;${CMAKE_CURRENT_BINARY_DIR}/../kinematics_constraint")
+  endif()
 
-  catkin_add_gtest(test_constraint_samplers
+  ament_add_gtest(test_constraint_samplers
     test/test_constraint_samplers.cpp
     test/pr2_arm_kinematics_plugin.cpp
     test/pr2_arm_ik.cpp
   )
+
+  target_include_directories(test_constraint_samplers PUBLIC
+    ${geometry_msgs_INCLUDE_DIRS}
+  )
+
   target_link_libraries(test_constraint_samplers
+    moveit_constraint_samplers
+    moveit_kinematic_constraints
+    moveit_planning_scene
     moveit_test_utils
+    moveit_utils
+    moveit_robot_state
     ${MOVEIT_LIB_NAME}
-    ${catkin_LIBRARIES}
     ${angles_LIBRARIES}
-    ${orocos_kdl_LIBRARIES}
+    ${urdf_LIBRARIES}
+    ${tf2_LIBRARIES}
+    ${tf2_kdl_LIBRARIES}
+    ${tf2_ros_LIBRARIES}
+    ${kdl_parser_LIBRARIES}
+    ${OCTOMAP_LIBRARIES}
+    ${orcos_kdl_LIBRARIES}
     ${urdfdom_LIBRARIES}
     ${urdfdom_headers_LIBRARIES}
+    ${rclcpp_headers_LIBRARIES}
+    ${geometric_shapes_LIBRARIES}
   )
 endif()

moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler.h

@@ -40,7 +40,7 @@
 #include <moveit/planning_scene/planning_scene.h>
 #include <moveit/kinematic_constraints/kinematic_constraint.h>
 #include <vector>
-
+#include "rclcpp/rclcpp.hpp"
 /**
  * \brief The constraint samplers namespace contains a number of
  * methods for generating samples based on a constraint or set of
@@ -52,7 +52,7 @@
 namespace constraint_samplers
 {
 MOVEIT_CLASS_FORWARD(ConstraintSampler);
-
+static rclcpp::Logger LOGGER_CONSTRAINT_SAMPLERS = rclcpp::get_logger("moveit").get_child("constraint_samplers");
 /**
  * \brief ConstraintSampler is an abstract base class that allows the
  * sampling of a kinematic state for a particular group of a robot.

moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_allocator.h

@@ -39,6 +39,7 @@
 
 #include <moveit/constraint_samplers/constraint_sampler.h>
 #include <moveit/macros/class_forward.h>
+#include "rclcpp/rclcpp.hpp"
 
 namespace constraint_samplers
 {

moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_manager.h

@@ -39,6 +39,9 @@
 
 #include <moveit/constraint_samplers/constraint_sampler_allocator.h>
 #include <moveit/macros/class_forward.h>
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/clock.hpp"
+#include "rclcpp/duration.hpp"
 
 namespace constraint_samplers
 {

moveit_core/constraint_samplers/include/moveit/constraint_samplers/constraint_sampler_tools.h

@@ -38,22 +38,26 @@
 #define MOVEIT_CONSTRAINT_SAMPLERS_CONSTRAINT_SAMPLER_TOOLS_
 
 #include <moveit/constraint_samplers/constraint_sampler.h>
-#include <visualization_msgs/MarkerArray.h>
+#include <visualization_msgs/msg/marker_array.hpp>
+#include "rclcpp/rclcpp.hpp"
 
 namespace constraint_samplers
 {
+rclcpp::Logger LOGGER_CONTRAINT_SAMPLERS_TOOLS = rclcpp::get_logger("moveit").get_child("constraint_samplers");
+
 void visualizeDistribution(const ConstraintSamplerPtr& sampler, const robot_state::RobotState& reference_state,
                            const std::string& link_name, unsigned int sample_count,
-                           visualization_msgs::MarkerArray& markers);
+                           visualization_msgs::msg::MarkerArray& markers);
 
-void visualizeDistribution(const moveit_msgs::msg::Constraints& constr, const planning_scene::PlanningSceneConstPtr& scene,
-                           const std::string& group, const std::string& link_name, unsigned int sample_count,
-                           visualization_msgs::MarkerArray& markers);
+void visualizeDistribution(const moveit_msgs::msg::Constraints& constr,
+                           const planning_scene::PlanningSceneConstPtr& scene, const std::string& group,
+                           const std::string& link_name, unsigned int sample_count,
+                           visualization_msgs::msg::MarkerArray& markers);
 
 double countSamplesPerSecond(const ConstraintSamplerPtr& sampler, const robot_state::RobotState& reference_state);
 
-double countSamplesPerSecond(const moveit_msgs::msg::Constraints& constr, const planning_scene::PlanningSceneConstPtr& scene,
-                             const std::string& group);
+double countSamplesPerSecond(const moveit_msgs::msg::Constraints& constr,
+                             const planning_scene::PlanningSceneConstPtr& scene, const std::string& group);
 }
 
 #endif

moveit_core/constraint_samplers/include/moveit/constraint_samplers/default_constraint_samplers.h

@@ -40,6 +40,7 @@
 #include <moveit/constraint_samplers/constraint_sampler.h>
 #include <moveit/macros/class_forward.h>
 #include <random_numbers/random_numbers.h>
+#include "rclcpp/rclcpp.hpp"
 
 namespace constraint_samplers
 {
@@ -507,7 +508,7 @@ class IKConstraintSampler : public ConstraintSampler
    *
    * @return True if IK returns successfully with the timeout, and otherwise false.
    */
-  bool callIK(const geometry_msgs::Pose& ik_query,
+  bool callIK(const geometry_msgs::msg::Pose& ik_query,
               const kinematics::KinematicsBase::IKCallbackFn& adapted_ik_validity_callback, double timeout,
               robot_state::RobotState& state, bool use_as_seed);
   bool sampleHelper(robot_state::RobotState& state, const robot_state::RobotState& reference_state,

moveit_core/constraint_samplers/include/moveit/constraint_samplers/union_constraint_sampler.h

@@ -38,6 +38,7 @@
 #define MOVEIT_CONSTRAINT_SAMPLERS_DEFAULT_UNION_CONSTRAINT_SAMPLER_
 
 #include <moveit/constraint_samplers/constraint_sampler.h>
+#include "rclcpp/rclcpp.hpp"
 
 namespace constraint_samplers
 {

moveit_core/constraint_samplers/src/constraint_sampler.cpp

@@ -42,7 +42,7 @@ constraint_samplers::ConstraintSampler::ConstraintSampler(const planning_scene::
 {
   if (!jmg_)
   {
-    ROS_ERROR_NAMED("constraint_samplers", "A JointModelGroup should have been specified for the constraint sampler");
+    RCLCPP_ERROR(LOGGER_CONSTRAINT_SAMPLERS, "A JointModelGroup should have been specified for the constraint sampler");
   }
 }
 

moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp

@@ -39,6 +39,8 @@
 #include <moveit/constraint_samplers/union_constraint_sampler.h>
 #include <sstream>
 
+rclcpp::Logger LOGGER_CONTRAINT_SAMPLERS_MANAGER = rclcpp::get_logger("moveit").get_child("constraint_samplers");
+
 constraint_samplers::ConstraintSamplerPtr
 constraint_samplers::ConstraintSamplerManager::selectSampler(const planning_scene::PlanningSceneConstPtr& scene,
                                                              const std::string& group_name,
@@ -61,18 +63,18 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni
   if (!jmg)
     return constraint_samplers::ConstraintSamplerPtr();
   std::stringstream ss;
-  ss << constr;
-  ROS_DEBUG_NAMED("constraint_samplers",
-                  "Attempting to construct constrained state sampler for group '%s', using constraints:\n%s.\n",
-                  jmg->getName().c_str(), ss.str().c_str());
+  ss << constr.name;
+  RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER,
+               "Attempting to construct constrained state sampler for group '%s', using constraints:\n%s.\n",
+               jmg->getName().c_str(), ss.str().c_str());
 
   ConstraintSamplerPtr joint_sampler;  // location to put chosen joint sampler if needed
   // if there are joint constraints, we could possibly get a sampler from those
   if (!constr.joint_constraints.empty())
   {
-    ROS_DEBUG_NAMED("constraint_samplers", "There are joint constraints specified. "
-                                           "Attempting to construct a JointConstraintSampler for group '%s'",
-                    jmg->getName().c_str());
+    RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, "There are joint constraints specified. "
+                                                    "Attempting to construct a JointConstraintSampler for group '%s'",
+                 jmg->getName().c_str());
 
     std::map<std::string, bool> joint_coverage;
     for (std::size_t i = 0; i < jmg->getVariableNames().size(); ++i)
@@ -108,8 +110,8 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni
       JointConstraintSamplerPtr sampler(new JointConstraintSampler(scene, jmg->getName()));
       if (sampler->configure(jc))
       {
-        ROS_DEBUG_NAMED("constraint_samplers", "Allocated a sampler satisfying joint constraints for group '%s'",
-                        jmg->getName().c_str());
+        RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER,
+                     "Allocated a sampler satisfying joint constraints for group '%s'", jmg->getName().c_str());
         return sampler;
       }
     }
@@ -121,10 +123,10 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni
       JointConstraintSamplerPtr sampler(new JointConstraintSampler(scene, jmg->getName()));
       if (sampler->configure(jc))
       {
-        ROS_DEBUG_NAMED("constraint_samplers",
-                        "Temporary sampler satisfying joint constraints for group '%s' allocated. "
-                        "Looking for different types of constraints before returning though.",
-                        jmg->getName().c_str());
+        RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER,
+                     "Temporary sampler satisfying joint constraints for group '%s' allocated. "
+                     "Looking for different types of constraints before returning though.",
+                     jmg->getName().c_str());
         joint_sampler = sampler;
       }
     }
@@ -142,9 +144,10 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni
   // should be used
   if (ik_alloc)
   {
-    ROS_DEBUG_NAMED("constraint_samplers", "There is an IK allocator for '%s'. "
-                                           "Checking for corresponding position and/or orientation constraints",
-                    jmg->getName().c_str());
+    RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER,
+                 "There is an IK allocator for '%s'. "
+                 "Checking for corresponding position and/or orientation constraints",
+                 jmg->getName().c_str());
 
     // keep track of which links we constrained
     std::map<std::string, IKConstraintSamplerPtr> used_l;
@@ -177,9 +180,10 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni
               {
                 // assign the link to a new constraint sampler
                 used_l[constr.position_constraints[p].link_name] = iks;
-                ROS_DEBUG_NAMED("constraint_samplers", "Allocated an IK-based sampler for group '%s' "
-                                                       "satisfying position and orientation constraints on link '%s'",
-                                jmg->getName().c_str(), constr.position_constraints[p].link_name.c_str());
+                RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER,
+                             "Allocated an IK-based sampler for group '%s' "
+                             "satisfying position and orientation constraints on link '%s'",
+                             jmg->getName().c_str(), constr.position_constraints[p].link_name.c_str());
               }
             }
           }
@@ -209,9 +213,9 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni
           if (use)
           {
             used_l[constr.position_constraints[p].link_name] = iks;
-            ROS_DEBUG_NAMED("constraint_samplers", "Allocated an IK-based sampler for group '%s' "
-                                                   "satisfying position constraints on link '%s'",
-                            jmg->getName().c_str(), constr.position_constraints[p].link_name.c_str());
+            RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, "Allocated an IK-based sampler for group '%s' "
+                                                            "satisfying position constraints on link '%s'",
+                         jmg->getName().c_str(), constr.position_constraints[p].link_name.c_str());
           }
         }
       }
@@ -238,9 +242,9 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni
           if (use)
           {
             used_l[constr.orientation_constraints[o].link_name] = iks;
-            ROS_DEBUG_NAMED("constraint_samplers", "Allocated an IK-based sampler for group '%s' "
-                                                   "satisfying orientation constraints on link '%s'",
-                            jmg->getName().c_str(), constr.orientation_constraints[o].link_name.c_str());
+            RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, "Allocated an IK-based sampler for group '%s' "
+                                                            "satisfying orientation constraints on link '%s'",
+                         jmg->getName().c_str(), constr.orientation_constraints[o].link_name.c_str());
           }
         }
       }
@@ -258,9 +262,9 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni
     }
     else if (used_l.size() > 1)
     {
-      ROS_DEBUG_NAMED("constraint_samplers",
-                      "Too many IK-based samplers for group '%s'. Keeping the one with minimal sampling volume",
-                      jmg->getName().c_str());
+      RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER,
+                   "Too many IK-based samplers for group '%s'. Keeping the one with minimal sampling volume",
+                   jmg->getName().c_str());
       // find the sampler with the smallest sampling volume; delete the rest
       IKConstraintSamplerPtr iks = used_l.begin()->second;
       double msv = iks->getSamplingVolume();
@@ -290,9 +294,10 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni
   // we now check to see if we can use samplers from subgroups
   if (!ik_subgroup_alloc.empty())
   {
-    ROS_DEBUG_NAMED("constraint_samplers", "There are IK allocators for subgroups of group '%s'. "
-                                           "Checking for corresponding position and/or orientation constraints",
-                    jmg->getName().c_str());
+    RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER,
+                 "There are IK allocators for subgroups of group '%s'. "
+                 "Checking for corresponding position and/or orientation constraints",
+                 jmg->getName().c_str());
 
     bool some_sampler_valid = false;
 
@@ -321,36 +326,39 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni
       // if some matching constraints were found, construct the allocator
       if (!sub_constr.orientation_constraints.empty() || !sub_constr.position_constraints.empty())
       {
-        ROS_DEBUG_NAMED("constraint_samplers", "Attempting to construct a sampler for the '%s' subgroup of '%s'",
-                        it->first->getName().c_str(), jmg->getName().c_str());
+        RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER,
+                     "Attempting to construct a sampler for the '%s' subgroup of '%s'", it->first->getName().c_str(),
+                     jmg->getName().c_str());
         ConstraintSamplerPtr cs = selectDefaultSampler(scene, it->first->getName(), sub_constr);
         if (cs)
         {
-          ROS_DEBUG_NAMED("constraint_samplers", "Constructed a sampler for the joints corresponding to group '%s', "
-                                                 "but part of group '%s'",
-                          it->first->getName().c_str(), jmg->getName().c_str());
+          RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER,
+                       "Constructed a sampler for the joints corresponding to group '%s', "
+                       "but part of group '%s'",
+                       it->first->getName().c_str(), jmg->getName().c_str());
           some_sampler_valid = true;
           samplers.push_back(cs);
         }
       }
     }
     if (some_sampler_valid)
     {
-      ROS_DEBUG_NAMED("constraint_samplers", "Constructing sampler for group '%s' as a union of %zu samplers",
-                      jmg->getName().c_str(), samplers.size());
+      RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, "Constructing sampler for group '%s' as a union of %zu samplers",
+                   jmg->getName().c_str(), samplers.size());
       return ConstraintSamplerPtr(new UnionConstraintSampler(scene, jmg->getName(), samplers));
     }
   }
 
   // if we've gotten here, just return joint sampler
   if (joint_sampler)
   {
-    ROS_DEBUG_NAMED("constraint_samplers", "Allocated a sampler satisfying joint constraints for group '%s'",
-                    jmg->getName().c_str());
+    RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, "Allocated a sampler satisfying joint constraints for group '%s'",
+                 jmg->getName().c_str());
     return joint_sampler;
   }
 
-  ROS_DEBUG_NAMED("constraint_samplers", "No constraints sampler allocated for group '%s'", jmg->getName().c_str());
+  RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, "No constraints sampler allocated for group '%s'",
+               jmg->getName().c_str());
 
   return ConstraintSamplerPtr();
 }