Constraint samplers

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

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,17 +38,18 @@
 #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
 {
 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);
+                           visualization_msgs::msg::MarkerArray& markers);
 
 double countSamplesPerSecond(const ConstraintSamplerPtr& sampler, const robot_state::RobotState& reference_state);
 

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

@@ -36,13 +36,15 @@
 
 #include <moveit/constraint_samplers/constraint_sampler.h>
 
+rclcpp::Logger LOGGER_CONSTRAINT_SAMPLERS = rclcpp::get_logger("moveit").get_child("constraint_samplers");
+
 constraint_samplers::ConstraintSampler::ConstraintSampler(const planning_scene::PlanningSceneConstPtr& scene,
                                                           const std::string& group_name)
   : is_valid_(false), scene_(scene), jmg_(scene->getRobotModel()->getJointModelGroup(group_name)), verbose_(false)
 {
   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,16 +63,16 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni
   if (!jmg)
     return constraint_samplers::ConstraintSamplerPtr();
   std::stringstream ss;
-  ss << constr;
-  ROS_DEBUG_NAMED("constraint_samplers",
+  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. "
+    RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, "There are joint constraints specified. "
                                            "Attempting to construct a JointConstraintSampler for group '%s'",
                     jmg->getName().c_str());
 
@@ -108,7 +110,7 @@ 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'",
+        RCLCPP_DEBUG(LOGGER_CONTRAINT_SAMPLERS_MANAGER, "Allocated a sampler satisfying joint constraints for group '%s'",
                         jmg->getName().c_str());
         return sampler;
       }
@@ -121,7 +123,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni
       JointConstraintSamplerPtr sampler(new JointConstraintSampler(scene, jmg->getName()));
       if (sampler->configure(jc))
       {
-        ROS_DEBUG_NAMED("constraint_samplers",
+        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());
@@ -142,7 +144,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni
   // should be used
   if (ik_alloc)
   {
-    ROS_DEBUG_NAMED("constraint_samplers", "There is an IK allocator for '%s'. "
+    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());
 
@@ -177,7 +179,7 @@ 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' "
+                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,7 +211,7 @@ 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' "
+            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,7 +240,7 @@ 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' "
+            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,7 +260,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni
     }
     else if (used_l.size() > 1)
     {
-      ROS_DEBUG_NAMED("constraint_samplers",
+      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
@@ -290,7 +292,7 @@ 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'. "
+    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());
 
@@ -321,12 +323,12 @@ 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'",
+        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', "
+          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;
@@ -336,7 +338,7 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni
     }
     if (some_sampler_valid)
     {
-      ROS_DEBUG_NAMED("constraint_samplers", "Constructing sampler for group '%s' as a union of %zu samplers",
+      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));
     }
@@ -345,12 +347,12 @@ constraint_samplers::ConstraintSamplerManager::selectDefaultSampler(const planni
   // 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'",
+    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();
 }