From 2542d2868608a2255968fec2fb55f61ea381e2d2 Mon Sep 17 00:00:00 2001 From: v4hn Date: Mon, 13 Jun 2016 20:17:34 +0200 Subject: [PATCH] add apply_planning_scene capability This new capability allows to apply changes to a monitored planning scene and *blocks* until the changes are applied. This is meant to replace the quasi-standard pattern: ``` planning_scene_interface.addCollisionObjects(...) sleep(2.0) group.pick("object") ``` by ``` ros::ServiceClient client = n.serviceClient("apply_planning_scene"); client.call(...) group.pick("object") ``` This makes it much more convenient to add&interact with objects without useless and arbitrarily long sleeps to ensure planning scene updates succeeded. --- moveit_ros/move_group/CMakeLists.txt | 1 + ...efault_capabilities_plugin_description.xml | 6 ++ .../moveit/move_group/capability_names.h | 1 + ...pply_planning_scene_service_capability.cpp | 63 +++++++++++++++++ .../apply_planning_scene_service_capability.h | 67 +++++++++++++++++++ 5 files changed, 138 insertions(+) create mode 100644 moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.cpp create mode 100644 moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.h diff --git a/moveit_ros/move_group/CMakeLists.txt b/moveit_ros/move_group/CMakeLists.txt index ccb857215b..22bf320398 100644 --- a/moveit_ros/move_group/CMakeLists.txt +++ b/moveit_ros/move_group/CMakeLists.txt @@ -54,6 +54,7 @@ add_library(moveit_move_group_default_capabilities src/default_capabilities/state_validation_service_capability.cpp src/default_capabilities/cartesian_path_service_capability.cpp src/default_capabilities/get_planning_scene_service_capability.cpp + src/default_capabilities/apply_planning_scene_service_capability.cpp src/default_capabilities/clear_octomap_service_capability.cpp ) diff --git a/moveit_ros/move_group/default_capabilities_plugin_description.xml b/moveit_ros/move_group/default_capabilities_plugin_description.xml index 295e9ecec1..99932a590c 100644 --- a/moveit_ros/move_group/default_capabilities_plugin_description.xml +++ b/moveit_ros/move_group/default_capabilities_plugin_description.xml @@ -48,6 +48,12 @@ + + + Provide a ROS service for blocking updates to the planning scene + + + Provide a ROS service that allows for clearing the octomap diff --git a/moveit_ros/move_group/include/moveit/move_group/capability_names.h b/moveit_ros/move_group/include/moveit/move_group/capability_names.h index 556e617976..869355e8ab 100644 --- a/moveit_ros/move_group/include/moveit/move_group/capability_names.h +++ b/moveit_ros/move_group/include/moveit/move_group/capability_names.h @@ -53,6 +53,7 @@ static const std::string FK_SERVICE_NAME = "compute_fk"; // name of fk service static const std::string STATE_VALIDITY_SERVICE_NAME = "check_state_validity"; // name of the service that validates states static const std::string CARTESIAN_PATH_SERVICE_NAME = "compute_cartesian_path"; // name of the service that computes cartesian paths static const std::string GET_PLANNING_SCENE_SERVICE_NAME = "get_planning_scene"; // name of the service that can be used to query the planning scene +static const std::string APPLY_PLANNING_SCENE_SERVICE_NAME = "apply_planning_scene"; // name of the service that applies a given planning scene static const std::string CLEAR_OCTOMAP_SERVICE_NAME = "clear_octomap"; // name of the service that can be used to clear the octomap } diff --git a/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.cpp new file mode 100644 index 0000000000..9b74af8dea --- /dev/null +++ b/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.cpp @@ -0,0 +1,63 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, Michael 'v4hn' Goerner + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Michael Goerner */ + +#include "apply_planning_scene_service_capability.h" +#include + +move_group::ApplyPlanningSceneService::ApplyPlanningSceneService(): + MoveGroupCapability("ApplyPlanningSceneService") +{ +} + +void move_group::ApplyPlanningSceneService::initialize() +{ + service_ = root_node_handle_.advertiseService(APPLY_PLANNING_SCENE_SERVICE_NAME, &ApplyPlanningSceneService::applyScene, this); +} + +bool move_group::ApplyPlanningSceneService::applyScene(moveit_msgs::ApplyPlanningScene::Request &req, moveit_msgs::ApplyPlanningScene::Response &res) +{ + if (!context_->planning_scene_monitor_) + { + ROS_ERROR("Cannot apply PlanningScene as no scene is monitored."); + return true; + } + context_->planning_scene_monitor_->updateFrameTransforms(); + context_->planning_scene_monitor_->newPlanningSceneMessage(req.scene); + return true; +} + +#include +CLASS_LOADER_REGISTER_CLASS(move_group::ApplyPlanningSceneService, move_group::MoveGroupCapability) diff --git a/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.h b/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.h new file mode 100644 index 0000000000..5d0fc1adbe --- /dev/null +++ b/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.h @@ -0,0 +1,67 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, Michael 'v4hn' Goerner + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Michael 'v4hn' Goerner */ + +#ifndef MOVEIT_MOVE_GROUP_APPLY_PLANNING_SCENE_SERVICE_CAPABILITY_ +#define MOVEIT_MOVE_GROUP_APPLY_PLANNING_SCENE_SERVICE_CAPABILITY_ + +#include +#include + +namespace move_group +{ + +/** + * Provides the ability to update the shared planning scene + * with a remote blocking call using a ROS-Service + */ +class ApplyPlanningSceneService : public MoveGroupCapability +{ +public: + + ApplyPlanningSceneService(); + + virtual void initialize(); + +private: + + bool applyScene(moveit_msgs::ApplyPlanningScene::Request &req, moveit_msgs::ApplyPlanningScene::Response &res); + + ros::ServiceServer service_; +}; + +} + +#endif // MOVEIT_MOVE_GROUP_APPLY_PLANNING_SCENE_SERVICE_CAPABILITY_