Merge pull request moveit#59 from AcutronicRobotics/planning_scene_in…

…terface Port planning_scene_interface to ROS2
JafarAbdi · May 7, 2019 · ada03e9 · ada03e9
2 parents 58ff7bc + 2a85265
commit ada03e9
Show file tree

Hide file tree

Showing 6 changed files with 157 additions and 155 deletions.
diff --git a/moveit_ros/move_group/CMakeLists.txt b/moveit_ros/move_group/CMakeLists.txt
@@ -13,13 +13,13 @@ find_package(Boost REQUIRED)
 find_package(ament_cmake REQUIRED)
 find_package(moveit_core REQUIRED)
 find_package(moveit_ros_planning REQUIRED)
+find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_srvs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 
-
 include_directories(include)
 include_directories(${rclcpp_INCLUDE_DIRS}
                     ${Boost_INCLUDE_DIRS}
@@ -47,27 +47,29 @@ add_library(moveit_move_group_default_capabilities
   # src/default_capabilities/apply_planning_scene_service_capability.cpp
   # src/default_capabilities/clear_octomap_service_capability.cpp
 )
-# set_target_properties(moveit_move_group_default_capabilities PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-
-ament_target_dependencies(moveit_move_group_capabilities_base
-  rclcpp
-  Boost
-  moveit_core)
+set_target_properties(moveit_move_group_default_capabilities PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+
+# ament_target_dependencies(moveit_move_group_capabilities_base
+#   rclcpp
+#   Boost
+#   moveit_core)
+# ament_target_dependencies(moveit_move_group_default_capabilities moveit_move_group_capabilities_base
+#   rclcpp
+#   Boost)
+target_link_libraries(moveit_move_group_capabilities_base ${rclcpp_LIBRARIES} ${Boost_LIBRARIES})
 # target_link_libraries(move_group moveit_move_group_capabilities_base ${rclcpp_LIBRARIES} ${Boost_LIBRARIES})
-ament_target_dependencies(moveit_move_group_default_capabilities moveit_move_group_capabilities_base
-  rclcpp
-  Boost)
+target_link_libraries(moveit_move_group_default_capabilities moveit_move_group_capabilities_base ${rclcpp_LIBRARIES} ${Boost_LIBRARIES})
 # target_link_libraries(list_move_group_capabilities ${rclcpp_LIBRARIES} ${Boost_LIBRARIES})
 
 # install(TARGETS move_group list_move_group_capabilities moveit_move_group_capabilities_base moveit_move_group_default_capabilities
 #   ARCHIVE DESTINATION lib
 #   LIBRARY DESTINATION lib
 #   RUNTIME DESTINATION bin)
 #TODO (anasarrak): Rever back for the install that is above
-install(TARGETS moveit_move_group_capabilities_base
+install(TARGETS moveit_move_group_capabilities_base moveit_move_group_default_capabilities
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
-  RUNTIME DESTINATION bin)
+  RUNTIME DESTINATION lib/${PROJECT_NAME})
 
 install(DIRECTORY include/ DESTINATION include)
 

diff --git a/moveit_ros/planning_interface/CMakeLists.txt b/moveit_ros/planning_interface/CMakeLists.txt
@@ -83,7 +83,7 @@ ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDENCIES})
 
 # add_subdirectory(py_bindings_tools)
 add_subdirectory(common_planning_interface_objects)
-# add_subdirectory(planning_scene_interface)
+add_subdirectory(planning_scene_interface)
 add_subdirectory(move_group_interface)
 # add_subdirectory(robot_interface)
 # add_subdirectory(test)

diff --git a/moveit_ros/planning_interface/package.xml b/moveit_ros/planning_interface/package.xml
@@ -20,31 +20,18 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <build_depend>moveit_ros_planning</build_depend>
-  <!-- <build_depend>moveit_ros_warehouse</build_depend> TODO: uncomment when ported-->
-  <build_depend>moveit_ros_move_group</build_depend>
-  <!-- <build_depend>moveit_ros_manipulation</build_depend> TODO: uncomment when ported -->
-  <build_depend>rclcpp</build_depend>
-  <build_depend>rclpy</build_depend>
-  <build_depend>geometry_msgs</build_depend>
-  <build_depend>moveit_msgs</build_depend>
-  <build_depend>tf2</build_depend>
-  <build_depend>tf2_eigen</build_depend>
-  <build_depend>tf2_geometry_msgs</build_depend>
-  <build_depend>tf2_ros</build_depend>
-
-  <exec_depend>moveit_ros_planning</exec_depend>
-  <!-- <exec_depend>moveit_ros_warehouse</exec_depend> TODO: uncomment when ported-->
-  <exec_depend>moveit_ros_move_group</exec_depend>
-  <!-- <exec_depend>moveit_ros_manipulation</exec_depend> TODO: uncomment when ported -->
-  <exec_depend>rclcpp</exec_depend>
-  <exec_depend>rclpy</exec_depend>
-  <exec_depend>geometry_msgs</exec_depend>
-  <exec_depend>moveit_msgs</exec_depend>
-  <exec_depend>tf2</exec_depend>
-  <exec_depend>tf2_eigen</exec_depend>
-  <exec_depend>tf2_geometry_msgs</exec_depend>
-  <exec_depend>tf2_ros</exec_depend>
+  <depend>moveit_ros_planning</depend>
+  <!-- <depend>moveit_ros_warehouse</depend> TODO: uncomment when ported-->
+  <depend>moveit_ros_move_group</depend>
+  <!-- <depend>moveit_ros_manipulation</depend> TODO: uncomment when ported -->
+  <depend>rclcpp</depend>
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+  <depend>moveit_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_eigen</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>moveit_resources</test_depend>

diff --git a/moveit_ros/planning_interface/planning_scene_interface/CMakeLists.txt b/moveit_ros/planning_interface/planning_scene_interface/CMakeLists.txt
@@ -2,20 +2,20 @@ set(MOVEIT_LIB_NAME moveit_planning_scene_interface)
 
 add_library(${MOVEIT_LIB_NAME} src/planning_scene_interface.cpp)
 set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-target_link_libraries(${MOVEIT_LIB_NAME} moveit_common_planning_interface_objects ${catkin_LIBRARIES} ${Boost_LIBRARIES})
+target_link_libraries(${MOVEIT_LIB_NAME} moveit_common_planning_interface_objects ${rclcpp_LIBRARIES} ${Boost_LIBRARIES})
 
-add_library(${MOVEIT_LIB_NAME}_python src/wrap_python_planning_scene_interface.cpp)
-set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
-target_link_libraries(${MOVEIT_LIB_NAME}_python ${MOVEIT_LIB_NAME} ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} moveit_py_bindings_tools)
-set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES OUTPUT_NAME _moveit_planning_scene_interface PREFIX "")
-set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}")
+# add_library(${MOVEIT_LIB_NAME}_python src/wrap_python_planning_scene_interface.cpp)
+# set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+# target_link_libraries(${MOVEIT_LIB_NAME}_python ${MOVEIT_LIB_NAME} ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} moveit_py_bindings_tools)
+# set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES OUTPUT_NAME _moveit_planning_scene_interface PREFIX "")
+# set_target_properties(${MOVEIT_LIB_NAME}_python PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}")
 
 install(TARGETS ${MOVEIT_LIB_NAME}
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib)
 
-install(TARGETS ${MOVEIT_LIB_NAME}_python
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION})
+# install(TARGETS ${MOVEIT_LIB_NAME}_python
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION})
 
-install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
+install(DIRECTORY include/ DESTINATION install)
diff --git a/...anning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h b/...anning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h
@@ -39,10 +39,10 @@
 
 #include <moveit/macros/class_forward.h>
 #include <moveit/robot_state/robot_state.h>
-#include <moveit_msgs/ObjectColor.h>
-#include <moveit_msgs/CollisionObject.h>
-#include <moveit_msgs/AttachedCollisionObject.h>
-#include <moveit_msgs/PlanningScene.h>
+#include <moveit_msgs/msg/object_color.hpp>
+#include <moveit_msgs/msg/collision_object.hpp>
+#include <moveit_msgs/msg/attached_collision_object.hpp>
+#include <moveit_msgs/msg/planning_scene.hpp>
 
 namespace moveit
 {
@@ -82,7 +82,7 @@ class PlanningSceneInterface
   };
 
   /** \brief Get the poses from the objects identified by the given object ids list. */
-  std::map<std::string, geometry_msgs::Pose> getObjectPoses(const std::vector<std::string>& object_ids);
+  std::map<std::string, geometry_msgs::msg::Pose> getObjectPoses(const std::vector<std::string>& object_ids);
 
   /** \brief Get the objects identified by the given object ids list. If no ids are provided, return all the known
    * objects. */
@@ -101,7 +101,7 @@ class PlanningSceneInterface
   /** \brief Apply collision object to the planning scene of the move_group node synchronously.
       Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group's monitored scene */
   bool applyCollisionObject(const moveit_msgs::msg::CollisionObject& collision_object,
-                            const std_msgs::ColorRGBA& object_color);
+                            const std_msgs::msg::ColorRGBA& object_color);
 
   /** \brief Apply collision objects to the planning scene of the move_group node synchronously.
       Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group's monitored scene.
@@ -116,8 +116,8 @@ class PlanningSceneInterface
 
   /** \brief Apply attached collision objects to the planning scene of the move_group node synchronously.
       Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group's monitored scene */
-  bool
-  applyAttachedCollisionObjects(const std::vector<moveit_msgs::msg::AttachedCollisionObject>& attached_collision_objects);
+  bool applyAttachedCollisionObjects(
+      const std::vector<moveit_msgs::msg::AttachedCollisionObject>& attached_collision_objects);
 
   /** \brief Update the planning_scene of the move_group node with the given ps synchronously.
       Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group's monitored scene */
@@ -128,9 +128,9 @@ class PlanningSceneInterface
 
       The update runs asynchronously. If you need the objects to be available *directly* after you called this function,
       consider using `applyCollisionObjects` instead. */
-  void addCollisionObjects(
-      const std::vector<moveit_msgs::msg::CollisionObject>& collision_objects,
-      const std::vector<moveit_msgs::msg::ObjectColor>& object_colors = std::vector<moveit_msgs::msg::ObjectColor>()) const;
+  void addCollisionObjects(const std::vector<moveit_msgs::msg::CollisionObject>& collision_objects,
+                           const std::vector<moveit_msgs::msg::ObjectColor>& object_colors =
+                               std::vector<moveit_msgs::msg::ObjectColor>()) const;
 
   /** \brief Remove collision objects from the world via /planning_scene.