Port reach study to ROS2

moveit_reach_plugins/CMakeLists.txt

@@ -1,61 +1,49 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(moveit_reach_plugins)
 
 add_compile_options(-std=c++11)
 
-find_package(catkin REQUIRED COMPONENTS
-  eigen_conversions
-  interactive_markers
-  moveit_core
-  moveit_msgs
-  moveit_ros_planning_interface
-  pluginlib
-  reach_core
-  reach_msgs
-  visualization_msgs
-  xmlrpcpp
-)
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
+find_package(tf2_eigen REQUIRED)
+find_package(interactive_markers REQUIRED)
+find_package(moveit_core REQUIRED)
+find_package(moveit_msgs REQUIRED)
+find_package(moveit_ros_planning_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(reach_core REQUIRED)
+find_package(reach_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
+find_package(geometric_shapes REQUIRED)
+#find_package(pcl_conversions REQUIRED)
+find_package(PCL REQUIRED)
 
-catkin_package(
-  INCLUDE_DIRS
-    include
-  LIBRARIES
-    ${PROJECT_NAME}_utils
-    evaluation_plugins
-    ik_solver_plugins
-    reach_display_plugins
-  CATKIN_DEPENDS
-    eigen_conversions
-    interactive_markers
-    moveit_core
-    moveit_msgs
-    moveit_ros_planning_interface
-    reach_core
-    reach_msgs
-    pluginlib
-    visualization_msgs
-    xmlrpcpp
-)
 
 ###########
 ## BUILD ##
 ###########
 
 include_directories(
   include
-  ${catkin_INCLUDE_DIRS}
+  ${PCL_INCLUDE_DIRS}
+  ${reach_core_INCLUDE_DIRS}
 )
 
 # Utils Library
 add_library(${PROJECT_NAME}_utils
   src/utils.cpp
 )
-add_dependencies(${PROJECT_NAME}_utils
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
+target_include_directories(${PROJECT_NAME}_utils
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
 )
-target_link_libraries(${PROJECT_NAME}_utils
-  ${catkin_LIBRARIES}
+ament_target_dependencies(${PROJECT_NAME}_utils
+  geometric_shapes
+  moveit_msgs
+  reach_msgs
+  tf2_eigen
 )
 
 # Evaluation Plugins
@@ -64,53 +52,77 @@ add_library(evaluation_plugins
   src/evaluation/joint_penalty_moveit.cpp
   src/evaluation/distance_penalty_moveit.cpp
 )
-add_dependencies(evaluation_plugins
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
+target_include_directories(evaluation_plugins
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
 )
 target_link_libraries(evaluation_plugins
-  ${catkin_LIBRARIES}
   ${PROJECT_NAME}_utils
+  ${PCL_LIBRARIES}
 )
+ament_target_dependencies(evaluation_plugins
+  geometric_shapes
+  moveit_msgs
+  reach_msgs
+  tf2_eigen
+  moveit_ros_planning_interface
+  moveit_core
+  pluginlib
+)
+pluginlib_export_plugin_description_file(reach_core eval_plugin_description.xml)
 
 # MoveIt IK Solver Plugin
 add_library(ik_solver_plugins
   src/ik/moveit_ik_solver.cpp
   src/ik/discretized_moveit_ik_solver.cpp
 )
-add_dependencies(ik_solver_plugins
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
+target_include_directories(ik_solver_plugins
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
 )
 target_link_libraries(ik_solver_plugins
-  ${catkin_LIBRARIES}
   ${PROJECT_NAME}_utils
 )
+ament_target_dependencies(ik_solver_plugins
+  moveit_ros_planning_interface
+  geometric_shapes
+  reach_core
+  pluginlib
+  rclcpp
+  tf2_eigen
+)
+pluginlib_export_plugin_description_file(reach_core ik_plugin_description.xml)
+target_compile_definitions(ik_solver_plugins PRIVATE "MOVEIT_REACH_PLUGINS_BUILDING_LIBRARY")
 
 # MoveIt Reach Display Plugin
 add_library(reach_display_plugins
   src/display/moveit_reach_display.cpp
 )
-add_dependencies(reach_display_plugins
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
+target_include_directories(reach_display_plugins
+  PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
 )
 target_link_libraries(reach_display_plugins
-  ${catkin_LIBRARIES}
   ${PROJECT_NAME}_utils
+  ${PCL_LIBRARIES}
+  ik_solver_plugins
 )
+pluginlib_export_plugin_description_file(reach_core display_plugin_description.xml)
 
 # IK Plugin Test
-add_executable(ik_plugin_test
-  test/plugin_test_node.cpp
-)
-add_dependencies(ik_plugin_test
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
-)
-target_link_libraries(ik_plugin_test
-  ${catkin_LIBRARIES}
-)
+#add_executable(ik_plugin_test
+#  test/plugin_test_node.cpp
+#)
+#add_dependencies(ik_plugin_test
+#  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+#  ${catkin_EXPORTED_TARGETS}
+#)
+#target_link_libraries(ik_plugin_test
+#  ${catkin_LIBRARIES}
+#)
 
 #############
 ## INSTALL ##
@@ -122,15 +134,24 @@ install(
     evaluation_plugins
     ik_solver_plugins
     reach_display_plugins
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
 )
 
-install(DIRECTORY include/${PROJECT_NAME}/
-  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+install(DIRECTORY include/
+  DESTINATION include/
 )
 
-install(FILES plugin_description.xml
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+ament_export_libraries(
+  ${PROJECT_NAME}_utils
+  evaluation_plugins
+  ik_solver_plugins
+  reach_display_plugins
+)
+
+ament_export_include_directories(
+  include
 )
+ament_package()

moveit_reach_plugins/display_plugin_description.xml

@@ -0,0 +1,9 @@
+<!-- Display Plugins -->
+<library path="reach_display_plugins">
+  <!-- MoveIt Reach Display -->
+  <class name="moveit_reach_plugins/display/MoveItReachDisplay" type="moveit_reach_plugins::display::MoveItReachDisplay" base_class_type="reach::plugins::DisplayBase">
+    <description>
+      A reach study display plugin using the MoveIt framework
+    </description>
+  </class>
+</library>

moveit_reach_plugins/plugin_description.xml → moveit_reach_plugins/eval_plugin_description.xml

@@ -1,5 +1,5 @@
 <!-- Evalution Plugins -->
-<library path="lib/libevaluation_plugins">
+<library path="evaluation_plugins">
   <!-- Manipulability MoveIt -->
   <class name="moveit_reach_plugins/evaluation/ManipulabilityMoveIt" type="moveit_reach_plugins::evaluation::ManipulabilityMoveIt" base_class_type="reach::plugins::EvaluationBase">
     <description>
@@ -29,30 +29,3 @@
     </description>
   </class>
 </library>
-
-<!-- IK Plugins -->
-<library path="lib/libik_solver_plugins">
-  <!-- MoveIt IK Solver -->
-  <class name="moveit_reach_plugins/ik/MoveItIKSolver" type="moveit_reach_plugins::ik::MoveItIKSolver" base_class_type="reach::plugins::IKSolverBase">
-    <description>
-      An inverse kinematics solver plugin which utilizes the MoveIt framework for solving robot inverse kinematics with respect to a given planning environment
-    </description>
-  </class>
-  <!-- Discretized MoveIt IK Solver -->
-  <class name="moveit_reach_plugins/ik/DiscretizedMoveItIKSolver" type="moveit_reach_plugins::ik::DiscretizedMoveItIKSolver" base_class_type="reach::plugins::IKSolverBase">
-    <description>
-      An inverse kinematics solver plugin which utilizes the MoveIt framework for solving robot inverse kinematics with respect to a given planning environment
-      This plugin discretizes the target pose around the Z-axis and outputs the solution with the highest score
-    </description>
-  </class>
-</library>
-
-<!-- Display Plugins -->
-<library path="lib/libreach_display_plugins">
-  <!-- MoveIt Reach Display -->
-  <class name="moveit_reach_plugins/display/MoveItReachDisplay" type="moveit_reach_plugins::display::MoveItReachDisplay" base_class_type="reach::plugins::DisplayBase">
-    <description>
-      A reach study display plugin using the MoveIt framework
-    </description>
-  </class>
-</library>

moveit_reach_plugins/ik_plugin_description.xml

@@ -0,0 +1,16 @@
+<!-- IK Plugins -->
+<library path="ik_solver_plugins">
+  <!-- MoveIt IK Solver -->
+  <class name="moveit_reach_plugins/ik/MoveItIKSolver" type="moveit_reach_plugins::ik::MoveItIKSolver" base_class_type="reach::plugins::IKSolverBase">
+    <description>
+      An inverse kinematics solver plugin which utilizes the MoveIt framework for solving robot inverse kinematics with respect to a given planning environment
+    </description>
+  </class>
+  <!-- Discretized MoveIt IK Solver -->
+  <class name="moveit_reach_plugins/ik/DiscretizedMoveItIKSolver" type="moveit_reach_plugins::ik::DiscretizedMoveItIKSolver" base_class_type="reach::plugins::IKSolverBase">
+    <description>
+      An inverse kinematics solver plugin which utilizes the MoveIt framework for solving robot inverse kinematics with respect to a given planning environment
+      This plugin discretizes the target pose around the Z-axis and outputs the solution with the highest score
+    </description>
+  </class>
+</library>

moveit_reach_plugins/include/moveit_reach_plugins/display/moveit_reach_display.h

@@ -17,16 +17,9 @@
 #define MOVEIT_REACH_PLUGINS_MOVEIT_REACH_DISPLAY_H
 
 #include <reach_core/plugins/reach_display_base.h>
+#include <moveit_msgs/msg/planning_scene.hpp>
 
-namespace moveit
-{
-namespace core
-{
-class RobotModel;
-typedef std::shared_ptr<const RobotModel> RobotModelConstPtr;
-class JointModelGroup;
-}
-}
+#include <rclcpp/rclcpp.hpp>
 
 namespace planning_scene
 {
@@ -36,6 +29,10 @@ typedef std::shared_ptr<PlanningScene> PlanningScenePtr;
 
 namespace moveit_reach_plugins
 {
+    namespace
+    {
+        const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_reach_plugins.MoveItReachDisplay");
+    }
 namespace display
 {
 
@@ -45,10 +42,12 @@ class MoveItReachDisplay : public reach::plugins::DisplayBase
 
   MoveItReachDisplay();
 
-  virtual bool initialize(XmlRpc::XmlRpcValue& config) override;
+  bool initialize(std::string& name, rclcpp::Node::SharedPtr node, const std::shared_ptr<const moveit::core::RobotModel> model) override;
 
   virtual void showEnvironment() override;
 
+  virtual void showEnvironment(const std::vector<std::string> & names, const std::vector<double>& positions) override;
+
   virtual void updateRobotPose(const std::map<std::string, double>& pose) override;
 
 private:
@@ -59,13 +58,11 @@ class MoveItReachDisplay : public reach::plugins::DisplayBase
 
   const moveit::core::JointModelGroup* jmg_;
 
-  std::string collision_mesh_filename_;
-
+  std::string collision_mesh_package_;
+  std::string collision_mesh_filename_path_;
   std::string collision_mesh_frame_;
 
-  ros::NodeHandle nh_;
-
-  ros::Publisher scene_pub_;
+  rclcpp::Publisher<moveit_msgs::msg::PlanningScene>::SharedPtr scene_pub_;
 };
 
 } // namespace display

moveit_reach_plugins/include/moveit_reach_plugins/evaluation/distance_penalty_moveit.h

@@ -17,17 +17,7 @@
 #define MOVEIT_REACH_PLUGINS_EVALUATION_DISTANCE_PENALTY_MOVEIT_H
 
 #include <reach_core/plugins/evaluation_base.h>
-#include <moveit_msgs/PlanningScene.h>
-
-namespace moveit
-{
-namespace core
-{
-class RobotModel;
-typedef std::shared_ptr<const RobotModel> RobotModelConstPtr;
-class JointModelGroup;
-}
-}
+#include <moveit_msgs/msg/planning_scene.hpp>
 
 namespace planning_scene
 {
@@ -37,6 +27,10 @@ typedef std::shared_ptr<PlanningScene> PlanningScenePtr;
 
 namespace moveit_reach_plugins
 {
+    namespace
+    {
+        const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_reach_plugins.DistancePenaltyMoveIt");
+    }
 namespace evaluation
 {
 
@@ -46,7 +40,7 @@ class DistancePenaltyMoveIt : public reach::plugins::EvaluationBase
 
   DistancePenaltyMoveIt();
 
-  virtual bool initialize(XmlRpc::XmlRpcValue& config) override;
+  virtual bool initialize(std::string& name, rclcpp::Node::SharedPtr node,const std::shared_ptr<const moveit::core::RobotModel> model) override;
 
   virtual double calculateScore(const std::map<std::string, double>& pose) override;
 
@@ -62,7 +56,8 @@ class DistancePenaltyMoveIt : public reach::plugins::EvaluationBase
 
   int exponent_;
 
-  std::string collision_mesh_filename_;
+    std::string collision_mesh_package_;
+    std::string collision_mesh_filename_path_;
 
   std::string collision_mesh_frame_;