From 05039b8df9a55e80ad45c4faf3155a391d1613ae Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Fri, 26 Apr 2019 16:37:09 +0200 Subject: [PATCH 1/6] Port planning_scene_interface to ROS2 --- moveit_ros/planning_interface/CMakeLists.txt | 4 +- .../planning_scene_interface/CMakeLists.txt | 24 +-- .../planning_scene_interface.h | 12 +- .../src/planning_scene_interface.cpp | 193 ++++++++++-------- 4 files changed, 125 insertions(+), 108 deletions(-) diff --git a/moveit_ros/planning_interface/CMakeLists.txt b/moveit_ros/planning_interface/CMakeLists.txt index e2e1aefc14..838a223acd 100644 --- a/moveit_ros/planning_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/CMakeLists.txt @@ -16,7 +16,7 @@ find_package(moveit_msgs REQUIRED) find_package(moveit_ros_planning REQUIRED) # find_package(moveit_ros_warehouse REQUIRED) # find_package(moveit_ros_manipulation REQUIRED) -# find_package(moveit_ros_move_group REQUIRED) +find_package(moveit_ros_move_group REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_eigen REQUIRED) @@ -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/planning_scene_interface/CMakeLists.txt b/moveit_ros/planning_interface/planning_scene_interface/CMakeLists.txt index 7658b2583e..35441338ac 100644 --- 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/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h b/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h index bbbd0fb5df..5a909f021d 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h +++ b/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h @@ -39,10 +39,10 @@ #include #include -#include -#include -#include -#include +#include +#include +#include +#include namespace moveit { @@ -82,7 +82,7 @@ class PlanningSceneInterface }; /** \brief Get the poses from the objects identified by the given object ids list. */ - std::map getObjectPoses(const std::vector& object_ids); + std::map getObjectPoses(const std::vector& 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. diff --git a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp index 81e6da28a1..24c451944c 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp +++ b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp @@ -36,9 +36,9 @@ #include #include -#include -#include -#include +#include +#include +#include "rclcpp/rclcpp.hpp" #include namespace moveit @@ -50,32 +50,38 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl public: explicit PlanningSceneInterfaceImpl(const std::string& ns = "") { - node_handle_ = ros::NodeHandle(ns); + node_ = rclcpp::Node::make_shared(ns); planning_scene_service_ = - node_handle_.serviceClient(move_group::GET_PLANNING_SCENE_SERVICE_NAME); + node_->create_client(move_group::GET_PLANNING_SCENE_SERVICE_NAME); apply_planning_scene_service_ = - node_handle_.serviceClient(move_group::APPLY_PLANNING_SCENE_SERVICE_NAME); - planning_scene_diff_publisher_ = node_handle_.advertise("planning_scene", 1); + node_->create_client(move_group::APPLY_PLANNING_SCENE_SERVICE_NAME); + + planning_scene_diff_publisher_ = node_->create_publisher("planning_scene", 1); } std::vector getKnownObjectNames(bool with_type) { - moveit_msgs::srv::GetPlanningScene::Request request; - moveit_msgs::srv::GetPlanningScene::Response response; + auto request = std::make_shared(); + auto response = std::make_shared(); + std::vector result; - request.components.components = request.components.WORLD_OBJECT_NAMES; - if (!planning_scene_service_.call(request, response)) + request->components.components = request->components.WORLD_OBJECT_NAMES; + auto res = planning_scene_service_->async_send_request(request); + if (rclcpp::spin_until_future_complete(node_, res) != + rclcpp::executor::FutureReturnCode::SUCCESS) + { return result; + } if (with_type) { - for (std::size_t i = 0; i < response.scene.world.collision_objects.size(); ++i) - if (!response.scene.world.collision_objects[i].type.key.empty()) - result.push_back(response.scene.world.collision_objects[i].id); + for (std::size_t i = 0; i < response->scene.world.collision_objects.size(); ++i) + if (!response->scene.world.collision_objects[i].type.key.empty()) + result.push_back(response->scene.world.collision_objects[i].id); } else { - for (std::size_t i = 0; i < response.scene.world.collision_objects.size(); ++i) - result.push_back(response.scene.world.collision_objects[i].id); + for (std::size_t i = 0; i < response->scene.world.collision_objects.size(); ++i) + result.push_back(response->scene.world.collision_objects[i].id); } return result; } @@ -83,82 +89,87 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl std::vector getKnownObjectNamesInROI(double minx, double miny, double minz, double maxx, double maxy, double maxz, bool with_type, std::vector& types) { - moveit_msgs::srv::GetPlanningScene::Request request; - moveit_msgs::srv::GetPlanningScene::Response response; + auto request = std::make_shared(); + auto response = std::make_shared(); + std::vector result; - request.components.components = request.components.WORLD_OBJECT_GEOMETRY; - if (!planning_scene_service_.call(request, response)) + request->components.components = request->components.WORLD_OBJECT_GEOMETRY; + auto res = planning_scene_service_->async_send_request(request); + if (rclcpp::spin_until_future_complete(node_, res) != + rclcpp::executor::FutureReturnCode::SUCCESS) { - ROS_WARN_NAMED("planning_scene_interface", "Could not call planning scene service to get object names"); + RCLCPP_WARN(node_->get_logger(), "Could not call planning scene service to get object names"); return result; } - for (std::size_t i = 0; i < response.scene.world.collision_objects.size(); ++i) + for (std::size_t i = 0; i < response->scene.world.collision_objects.size(); ++i) { - if (with_type && response.scene.world.collision_objects[i].type.key.empty()) + if (with_type && response->scene.world.collision_objects[i].type.key.empty()) continue; - if (response.scene.world.collision_objects[i].mesh_poses.empty() && - response.scene.world.collision_objects[i].primitive_poses.empty()) + if (response->scene.world.collision_objects[i].mesh_poses.empty() && + response->scene.world.collision_objects[i].primitive_poses.empty()) continue; bool good = true; - for (std::size_t j = 0; j < response.scene.world.collision_objects[i].mesh_poses.size(); ++j) - if (!(response.scene.world.collision_objects[i].mesh_poses[j].position.x >= minx && - response.scene.world.collision_objects[i].mesh_poses[j].position.x <= maxx && - response.scene.world.collision_objects[i].mesh_poses[j].position.y >= miny && - response.scene.world.collision_objects[i].mesh_poses[j].position.y <= maxy && - response.scene.world.collision_objects[i].mesh_poses[j].position.z >= minz && - response.scene.world.collision_objects[i].mesh_poses[j].position.z <= maxz)) + for (std::size_t j = 0; j < response->scene.world.collision_objects[i].mesh_poses.size(); ++j) + if (!(response->scene.world.collision_objects[i].mesh_poses[j].position.x >= minx && + response->scene.world.collision_objects[i].mesh_poses[j].position.x <= maxx && + response->scene.world.collision_objects[i].mesh_poses[j].position.y >= miny && + response->scene.world.collision_objects[i].mesh_poses[j].position.y <= maxy && + response->scene.world.collision_objects[i].mesh_poses[j].position.z >= minz && + response->scene.world.collision_objects[i].mesh_poses[j].position.z <= maxz)) { good = false; break; } - for (std::size_t j = 0; j < response.scene.world.collision_objects[i].primitive_poses.size(); ++j) - if (!(response.scene.world.collision_objects[i].primitive_poses[j].position.x >= minx && - response.scene.world.collision_objects[i].primitive_poses[j].position.x <= maxx && - response.scene.world.collision_objects[i].primitive_poses[j].position.y >= miny && - response.scene.world.collision_objects[i].primitive_poses[j].position.y <= maxy && - response.scene.world.collision_objects[i].primitive_poses[j].position.z >= minz && - response.scene.world.collision_objects[i].primitive_poses[j].position.z <= maxz)) + for (std::size_t j = 0; j < response->scene.world.collision_objects[i].primitive_poses.size(); ++j) + if (!(response->scene.world.collision_objects[i].primitive_poses[j].position.x >= minx && + response->scene.world.collision_objects[i].primitive_poses[j].position.x <= maxx && + response->scene.world.collision_objects[i].primitive_poses[j].position.y >= miny && + response->scene.world.collision_objects[i].primitive_poses[j].position.y <= maxy && + response->scene.world.collision_objects[i].primitive_poses[j].position.z >= minz && + response->scene.world.collision_objects[i].primitive_poses[j].position.z <= maxz)) { good = false; break; } if (good) { - result.push_back(response.scene.world.collision_objects[i].id); + result.push_back(response->scene.world.collision_objects[i].id); if (with_type) - types.push_back(response.scene.world.collision_objects[i].type.key); + types.push_back(response->scene.world.collision_objects[i].type.key); } } return result; } - std::map getObjectPoses(const std::vector& object_ids) + std::map getObjectPoses(const std::vector& object_ids) { - moveit_msgs::srv::GetPlanningScene::Request request; - moveit_msgs::srv::GetPlanningScene::Response response; - std::map result; - request.components.components = request.components.WORLD_OBJECT_GEOMETRY; - if (!planning_scene_service_.call(request, response)) + auto request = std::make_shared(); + auto response = std::make_shared(); + std::map result; + request->components.components = request->components.WORLD_OBJECT_GEOMETRY; + auto res = planning_scene_service_->async_send_request(request); + if (rclcpp::spin_until_future_complete(node_, res) != + rclcpp::executor::FutureReturnCode::SUCCESS) { - ROS_WARN_NAMED("planning_scene_interface", "Could not call planning scene service to get object names"); + RCLCPP_WARN(node_->get_logger(), "Could not call planning scene service to get object names"); return result; } - for (std::size_t i = 0; i < response.scene.world.collision_objects.size(); ++i) + for (std::size_t i = 0; i < response->scene.world.collision_objects.size(); ++i) { - if (std::find(object_ids.begin(), object_ids.end(), response.scene.world.collision_objects[i].id) != + if (std::find(object_ids.begin(), object_ids.end(), response->scene.world.collision_objects[i].id) != object_ids.end()) { - if (response.scene.world.collision_objects[i].mesh_poses.empty() && - response.scene.world.collision_objects[i].primitive_poses.empty()) + if (response->scene.world.collision_objects[i].mesh_poses.empty() && + response->scene.world.collision_objects[i].primitive_poses.empty()) continue; - if (!response.scene.world.collision_objects[i].mesh_poses.empty()) - result[response.scene.world.collision_objects[i].id] = - response.scene.world.collision_objects[i].mesh_poses[0]; + if (!response->scene.world.collision_objects[i].mesh_poses.empty()) + result[response->scene.world.collision_objects[i].id] = + response->scene.world.collision_objects[i].mesh_poses[0]; else - result[response.scene.world.collision_objects[i].id] = - response.scene.world.collision_objects[i].primitive_poses[0]; + result[response->scene.world.collision_objects[i].id] = + response->scene.world.collision_objects[i].primitive_poses[0]; } } return result; @@ -166,23 +177,25 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl std::map getObjects(const std::vector& object_ids) { - moveit_msgs::srv::GetPlanningScene::Request request; - moveit_msgs::srv::GetPlanningScene::Response response; + auto request = std::make_shared(); + auto response = std::make_shared(); std::map result; - request.components.components = request.components.WORLD_OBJECT_GEOMETRY; - if (!planning_scene_service_.call(request, response)) + request->components.components = request->components.WORLD_OBJECT_GEOMETRY; + auto res = planning_scene_service_->async_send_request(request); + if (rclcpp::spin_until_future_complete(node_, res) != + rclcpp::executor::FutureReturnCode::SUCCESS) { - ROS_WARN_NAMED("planning_scene_interface", "Could not call planning scene service to get object geometries"); + RCLCPP_WARN(node_->get_logger(), "Could not call planning scene service to get object geometries"); return result; } - for (std::size_t i = 0; i < response.scene.world.collision_objects.size(); ++i) + for (std::size_t i = 0; i < response->scene.world.collision_objects.size(); ++i) { if (object_ids.empty() || - std::find(object_ids.begin(), object_ids.end(), response.scene.world.collision_objects[i].id) != + std::find(object_ids.begin(), object_ids.end(), response->scene.world.collision_objects[i].id) != object_ids.end()) { - result[response.scene.world.collision_objects[i].id] = response.scene.world.collision_objects[i]; + result[response->scene.world.collision_objects[i].id] = response->scene.world.collision_objects[i]; } } return result; @@ -191,25 +204,27 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl std::map getAttachedObjects(const std::vector& object_ids) { - moveit_msgs::srv::GetPlanningScene::Request request; - moveit_msgs::srv::GetPlanningScene::Response response; + auto request = std::make_shared(); + auto response = std::make_shared(); std::map result; - request.components.components = request.components.ROBOT_STATE_ATTACHED_OBJECTS; - if (!planning_scene_service_.call(request, response)) + request->components.components = request->components.ROBOT_STATE_ATTACHED_OBJECTS; + auto res = planning_scene_service_->async_send_request(request); + if (rclcpp::spin_until_future_complete(node_, res) != + rclcpp::executor::FutureReturnCode::SUCCESS) { - ROS_WARN_NAMED("planning_scene_interface", + RCLCPP_WARN(node_->get_logger(), "Could not call planning scene service to get attached object geometries"); return result; } - for (std::size_t i = 0; i < response.scene.robot_state.attached_collision_objects.size(); ++i) + for (std::size_t i = 0; i < response->scene.robot_state.attached_collision_objects.size(); ++i) { if (object_ids.empty() || std::find(object_ids.begin(), object_ids.end(), - response.scene.robot_state.attached_collision_objects[i].object.id) != object_ids.end()) + response->scene.robot_state.attached_collision_objects[i].object.id) != object_ids.end()) { - result[response.scene.robot_state.attached_collision_objects[i].object.id] = - response.scene.robot_state.attached_collision_objects[i]; + result[response->scene.robot_state.attached_collision_objects[i].object.id] = + response->scene.robot_state.attached_collision_objects[i]; } } return result; @@ -217,15 +232,17 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl bool applyPlanningScene(const moveit_msgs::msg::PlanningScene& planning_scene) { - moveit_msgs::srv::ApplyPlanningScene::Request request; - moveit_msgs::srv::ApplyPlanningScene::Response response; - request.scene = planning_scene; - if (!apply_planning_scene_service_.call(request, response)) + auto request = std::make_shared(); + auto response = std::make_shared(); + request->scene = planning_scene; + auto res = apply_planning_scene_service_->async_send_request(request); + if (rclcpp::spin_until_future_complete(node_, res) != + rclcpp::executor::FutureReturnCode::SUCCESS) { - ROS_WARN_NAMED("planning_scene_interface", "Failed to call ApplyPlanningScene service"); + RCLCPP_WARN(node_->get_logger(), "Failed to call ApplyPlanningScene service"); return false; } - return response.success; + return response->success; } void addCollisionObjects(const std::vector& collision_objects, @@ -244,7 +261,7 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl } planning_scene.is_diff = true; - planning_scene_diff_publisher_.publish(planning_scene); + planning_scene_diff_publisher_->publish(planning_scene); } void removeCollisionObjects(const std::vector& object_ids) const @@ -258,14 +275,14 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl planning_scene.world.collision_objects.push_back(object); } planning_scene.is_diff = true; - planning_scene_diff_publisher_.publish(planning_scene); + planning_scene_diff_publisher_->publish(planning_scene); } private: - ros::NodeHandle node_handle_; - ros::ServiceClient planning_scene_service_; - ros::ServiceClient apply_planning_scene_service_; - ros::Publisher planning_scene_diff_publisher_; + rclcpp::Node::SharedPtr node_; + rclcpp::Client::SharedPtr planning_scene_service_; + rclcpp::Client::SharedPtr apply_planning_scene_service_; + rclcpp::Publisher::SharedPtr planning_scene_diff_publisher_; robot_model::RobotModelConstPtr robot_model_; }; @@ -292,7 +309,7 @@ std::vector PlanningSceneInterface::getKnownObjectNamesInROI(double return impl_->getKnownObjectNamesInROI(minx, miny, minz, maxx, maxy, maxz, with_type, types); } -std::map +std::map PlanningSceneInterface::getObjectPoses(const std::vector& object_ids) { return impl_->getObjectPoses(object_ids); @@ -321,7 +338,7 @@ bool PlanningSceneInterface::applyCollisionObject(const moveit_msgs::msg::Collis } bool PlanningSceneInterface::applyCollisionObject(const moveit_msgs::msg::CollisionObject& collision_object, - const std_msgs::ColorRGBA& object_color) + const std_msgs::msg::ColorRGBA& object_color) { moveit_msgs::msg::PlanningScene ps; ps.robot_state.is_diff = true; From 192b68e15fb4d994788b892b0a1ba5131c1a24f9 Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Fri, 26 Apr 2019 16:41:56 +0200 Subject: [PATCH 2/6] Apply clang-format --- .../planning_scene_interface.h | 10 +++--- .../src/planning_scene_interface.cpp | 34 ++++++++----------- 2 files changed, 20 insertions(+), 24 deletions(-) diff --git a/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h b/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h index 5a909f021d..2ded7dfea3 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h +++ b/moveit_ros/planning_interface/planning_scene_interface/include/moveit/planning_scene_interface/planning_scene_interface.h @@ -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& attached_collision_objects); + bool applyAttachedCollisionObjects( + const std::vector& 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& collision_objects, - const std::vector& object_colors = std::vector()) const; + void addCollisionObjects(const std::vector& collision_objects, + const std::vector& object_colors = + std::vector()) const; /** \brief Remove collision objects from the world via /planning_scene. diff --git a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp index 24c451944c..d64aa189c6 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp +++ b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp @@ -67,8 +67,7 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl std::vector result; request->components.components = request->components.WORLD_OBJECT_NAMES; auto res = planning_scene_service_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, res) != - rclcpp::executor::FutureReturnCode::SUCCESS) + if (rclcpp::spin_until_future_complete(node_, res) != rclcpp::executor::FutureReturnCode::SUCCESS) { return result; } @@ -95,8 +94,7 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl std::vector result; request->components.components = request->components.WORLD_OBJECT_GEOMETRY; auto res = planning_scene_service_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, res) != - rclcpp::executor::FutureReturnCode::SUCCESS) + if (rclcpp::spin_until_future_complete(node_, res) != rclcpp::executor::FutureReturnCode::SUCCESS) { RCLCPP_WARN(node_->get_logger(), "Could not call planning scene service to get object names"); return result; @@ -149,8 +147,7 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl std::map result; request->components.components = request->components.WORLD_OBJECT_GEOMETRY; auto res = planning_scene_service_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, res) != - rclcpp::executor::FutureReturnCode::SUCCESS) + if (rclcpp::spin_until_future_complete(node_, res) != rclcpp::executor::FutureReturnCode::SUCCESS) { RCLCPP_WARN(node_->get_logger(), "Could not call planning scene service to get object names"); return result; @@ -182,8 +179,7 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl std::map result; request->components.components = request->components.WORLD_OBJECT_GEOMETRY; auto res = planning_scene_service_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, res) != - rclcpp::executor::FutureReturnCode::SUCCESS) + if (rclcpp::spin_until_future_complete(node_, res) != rclcpp::executor::FutureReturnCode::SUCCESS) { RCLCPP_WARN(node_->get_logger(), "Could not call planning scene service to get object geometries"); return result; @@ -209,11 +205,9 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl std::map result; request->components.components = request->components.ROBOT_STATE_ATTACHED_OBJECTS; auto res = planning_scene_service_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, res) != - rclcpp::executor::FutureReturnCode::SUCCESS) + if (rclcpp::spin_until_future_complete(node_, res) != rclcpp::executor::FutureReturnCode::SUCCESS) { - RCLCPP_WARN(node_->get_logger(), - "Could not call planning scene service to get attached object geometries"); + RCLCPP_WARN(node_->get_logger(), "Could not call planning scene service to get attached object geometries"); return result; } @@ -236,8 +230,7 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl auto response = std::make_shared(); request->scene = planning_scene; auto res = apply_planning_scene_service_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, res) != - rclcpp::executor::FutureReturnCode::SUCCESS) + if (rclcpp::spin_until_future_complete(node_, res) != rclcpp::executor::FutureReturnCode::SUCCESS) { RCLCPP_WARN(node_->get_logger(), "Failed to call ApplyPlanningScene service"); return false; @@ -352,8 +345,9 @@ bool PlanningSceneInterface::applyCollisionObject(const moveit_msgs::msg::Collis return applyPlanningScene(ps); } -bool PlanningSceneInterface::applyCollisionObjects(const std::vector& collision_objects, - const std::vector& object_colors) +bool PlanningSceneInterface::applyCollisionObjects( + const std::vector& collision_objects, + const std::vector& object_colors) { moveit_msgs::msg::PlanningScene ps; ps.robot_state.is_diff = true; @@ -372,7 +366,8 @@ bool PlanningSceneInterface::applyCollisionObjects(const std::vectorapplyPlanningScene(ps); } -void PlanningSceneInterface::addCollisionObjects(const std::vector& collision_objects, - const std::vector& object_colors) const +void PlanningSceneInterface::addCollisionObjects( + const std::vector& collision_objects, + const std::vector& object_colors) const { impl_->addCollisionObjects(collision_objects, object_colors); } From 66f9b31c456f919fc25f458e3d21f00fb237bada Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Thu, 2 May 2019 11:12:25 +0200 Subject: [PATCH 3/6] Update move_group cmakelist --- moveit_ros/move_group/CMakeLists.txt | 26 ++++++++++++++------------ 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/moveit_ros/move_group/CMakeLists.txt b/moveit_ros/move_group/CMakeLists.txt index 30eedebbf0..c9c7860250 100644 --- 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,16 +47,18 @@ 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 @@ -64,10 +66,10 @@ ament_target_dependencies(moveit_move_group_default_capabilities moveit_move_gro # 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) From dcd9932527999e4df35061e01756494cdde2fc39 Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Thu, 2 May 2019 16:48:08 +0200 Subject: [PATCH 4/6] Update planning_interface package.xml --- moveit_ros/planning_interface/package.xml | 37 ++++++++--------------- 1 file changed, 12 insertions(+), 25 deletions(-) diff --git a/moveit_ros/planning_interface/package.xml b/moveit_ros/planning_interface/package.xml index f202bc58b6..38a1881517 100644 --- a/moveit_ros/planning_interface/package.xml +++ b/moveit_ros/planning_interface/package.xml @@ -20,31 +20,18 @@ ament_cmake - moveit_ros_planning - - - - rclcpp - rclpy - geometry_msgs - moveit_msgs - tf2 - tf2_eigen - tf2_geometry_msgs - tf2_ros - - moveit_ros_planning - - - - rclcpp - rclpy - geometry_msgs - moveit_msgs - tf2 - tf2_eigen - tf2_geometry_msgs - tf2_ros + moveit_ros_planning + + moveit_ros_move_group + + rclcpp + rclpy + geometry_msgs + moveit_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros ament_cmake_gtest moveit_resources From 4f3bfcba44d39a7fe079011c8bed794f88f654b9 Mon Sep 17 00:00:00 2001 From: ahcorde Date: Mon, 6 May 2019 18:37:17 +0200 Subject: [PATCH 5/6] resource retriever from sources Update osx-compile-moveit2.sh Update osx-install-ros2-binary.sh Update moveit2.repos Update package.xml Update osx-compile-moveit2.sh From 2a85265ce72bfba0464ea165aa623541397b5a20 Mon Sep 17 00:00:00 2001 From: LanderU Date: Tue, 7 May 2019 14:19:14 +0200 Subject: [PATCH 6/6] Restore moveit2.repos and remove it from OSX --- moveit2.repos | 12 ++++++++++++ osx-compile-moveit2.sh | 1 + 2 files changed, 13 insertions(+) diff --git a/moveit2.repos b/moveit2.repos index 7cbb30004d..723bfa1f29 100644 --- a/moveit2.repos +++ b/moveit2.repos @@ -1,4 +1,8 @@ repositories: + moveit_msgs: + type: git + url: https://github.com/AcutronicRobotics/moveit_msgs + version: ros2 geometry2: type: git url: https://github.com/AcutronicRobotics/geometry2 @@ -11,6 +15,14 @@ repositories: type: git url: https://github.com/AcutronicRobotics/eigen_stl_containers version: ros2 + object_recognition_msgs: + type: git + url: https://github.com/AcutronicRobotics/object_recognition_msgs + version: master + octomap_msgs: + type: git + url: https://github.com/AcutronicRobotics/octomap_msgs + version: ros2 random_numbers: type: git url: https://github.com/AcutronicRobotics/random_numbers diff --git a/osx-compile-moveit2.sh b/osx-compile-moveit2.sh index 5643e1be48..32e775bab6 100644 --- a/osx-compile-moveit2.sh +++ b/osx-compile-moveit2.sh @@ -10,6 +10,7 @@ mkdir -p /tmp/ros2_ws/src cp -r $dir /tmp/ros2_ws/src/moveit2 cd /tmp/ros2_ws && wget https://raw.githubusercontent.com/AcutronicRobotics/moveit2/master/moveit2.repos vcs import src < moveit2.repos +rm -rf src/moveit_msgs src/octomap_msgs src/object_recognition_msgs export OPENSSL_ROOT_DIR="/usr/local/opt/openssl" #Ignore packages