Port kinematic_constraints to ROS2 (#42)

moveit_core/collision_detection/CMakeLists.txt

@@ -11,7 +11,19 @@ add_library(${MOVEIT_LIB_NAME} SHARED
   src/world.cpp
   src/world_diff.cpp
 )
+
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION})
+ament_target_dependencies(${MOVEIT_LIB_NAME}
+  moveit_robot_state
+  rclcpp
+  rmw_implementation
+  urdf
+  urdfdom
+  urdfdom_headers
+  visualization_msgs
+  tf2_eigen
+  geometric_shapes
+)
 
 ament_target_dependencies(${MOVEIT_LIB_NAME}
   moveit_robot_state

moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h

@@ -40,7 +40,6 @@
 #include <moveit/collision_detection/collision_common.h>
 #include <moveit/macros/class_forward.h>
 #include <moveit_msgs/msg/allowed_collision_matrix.hpp>
-#include <boost/function.hpp>
 #include <iostream>
 #include <vector>
 #include <string>
@@ -70,7 +69,7 @@ enum Type
 
 /** \brief Signature of predicate that decides whether a contact is allowed or not (when AllowedCollision::Type is
  * CONDITIONAL) */
-typedef boost::function<bool(collision_detection::Contact&)> DecideContactFn;
+typedef std::function<bool(collision_detection::Contact&)> DecideContactFn;
 
 MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix);
 

moveit_core/kinematic_constraints/CMakeLists.txt

@@ -1,25 +1,61 @@
 set(MOVEIT_LIB_NAME moveit_kinematic_constraints)
 
-add_library(${MOVEIT_LIB_NAME}
+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_model;${CMAKE_CURRENT_BINARY_DIR}/../robot_state;${CMAKE_CURRENT_BINARY_DIR}/../collision_detection_fcl")
+endif()
+
+add_library(${MOVEIT_LIB_NAME} SHARED
   src/kinematic_constraint.cpp
-  src/utils.cpp)
+  src/utils.cpp
+)
+
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
 
+ament_target_dependencies(${MOVEIT_LIB_NAME}
+  urdf
+  urdfdom
+  urdfdom_headers
+  tf2_geometry_msgs
+  visualization_msgs
+  tf2_eigen
+)
+
 target_link_libraries(${MOVEIT_LIB_NAME}
-  moveit_robot_model moveit_kinematics_base moveit_robot_state moveit_collision_detection_fcl moveit_utils
-  ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})
-add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
+  moveit_collision_detection_fcl
+  moveit_kinematics_base
+  moveit_robot_state
+  moveit_robot_model
+)
 
 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(ament_cmake_gtest REQUIRED)
   find_package(moveit_resources REQUIRED)
+  find_package(resource_retriever REQUIRED)
+
   include_directories(${moveit_resources_INCLUDE_DIRS})
 
-  catkin_add_gtest(test_constraints test/test_constraints.cpp)
-  target_link_libraries(test_constraints moveit_test_utils ${MOVEIT_LIB_NAME})
+  ament_add_gtest(test_constraints test/test_constraints.cpp
+    APPEND_LIBRARY_DIRS "${append_library_dirs}"
+  )
+
+  target_link_libraries(test_constraints
+    moveit_kinematic_constraints
+    moveit_collision_detection_fcl
+    moveit_robot_model
+    moveit_robot_state
+    moveit_utils
+    moveit_test_utils
+    ${geometric_shapes_LIBRARIES}
+    resource_retriever::resource_retriever
+    ${MOVEIT_LIB_NAME}
+  )
 endif()

moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h

@@ -49,6 +49,10 @@
 #include <iostream>
 #include <vector>
 
+#include "rclcpp/clock.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/duration.hpp"
+
 /** \brief Representation and evaluation of kinematic constraints */
 namespace kinematic_constraints
 {
@@ -814,7 +818,7 @@ class VisibilityConstraint : public KinematicConstraint
    * @param [in] state The state from which to produce the markers
    * @param [out] markers The marker array to which the markers will be added
    */
-  void getMarkers(const robot_state::RobotState& state, visualization_msgs::MarkerArray& markers) const;
+  void getMarkers(const robot_state::RobotState& state, visualization_msgs::msg::MarkerArray& markers) const;
 
   bool enabled() const override;
   ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const override;

moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h

@@ -38,9 +38,9 @@
 #define MOVEIT_KINEMATIC_CONSTRAINTS_UTILS_
 
 #include <moveit_msgs/msg/motion_plan_request.hpp>
-#include <geometry_msgs/PointStamped.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/QuaternionStamped.h>
+#include <geometry_msgs/msg/point_stamped.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/quaternion_stamped.hpp>
 #include <moveit/robot_state/robot_state.h>
 #include <limits>
 
@@ -118,7 +118,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const robot_state::RobotS
  *
  * @return A full constraint message containing both constraints
  */
-moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, const geometry_msgs::PoseStamped& pose,
+moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, const geometry_msgs::msg::PoseStamped& pose,
                                                   double tolerance_pos = 1e-3, double tolerance_angle = 1e-2);
 
 /**
@@ -136,7 +136,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n
  *
  * @return A full constraint message containing both constraints
  */
-moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, const geometry_msgs::PoseStamped& pose,
+moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, const geometry_msgs::msg::PoseStamped& pose,
                                                   const std::vector<double>& tolerance_pos,
                                                   const std::vector<double>& tolerance_angle);
 
@@ -152,7 +152,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n
  * @return A full constraint message containing the orientation constraint
  */
 moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name,
-                                                  const geometry_msgs::QuaternionStamped& quat,
+                                                  const geometry_msgs::msg::QuaternionStamped& quat,
                                                   double tolerance = 1e-2);
 
 /**
@@ -169,8 +169,8 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n
  * @return A full constraint message containing the position constraint
  */
 moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name,
-                                                  const geometry_msgs::Point& reference_point,
-                                                  const geometry_msgs::PointStamped& goal_point,
+                                                  const geometry_msgs::msg::Point& reference_point,
+                                                  const geometry_msgs::msg::PointStamped& goal_point,
                                                   double tolerance = 1e-3);
 
 /**
@@ -186,7 +186,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n
  * @return A full constraint message containing the position constraint
  */
 moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name,
-                                                  const geometry_msgs::PointStamped& goal_point,
+                                                  const geometry_msgs::msg::PointStamped& goal_point,
                                                   double tolerance = 1e-3);
 
 /**
@@ -200,7 +200,8 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n
  *
  * @return was the construction successful?
  */
-bool constructConstraints(XmlRpc::XmlRpcValue& params, moveit_msgs::msg::Constraints& constraints);
+ // TODO rework this function
+// bool constructConstraints(XmlRpc::XmlRpcValue& params, moveit_msgs::msg::Constraints& constraints);
 }
 
 #endif