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<moveit_msgs::ApplyPlanningScene>("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

@@ -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
   )
 

moveit_ros/move_group/default_capabilities_plugin_description.xml

@@ -48,6 +48,12 @@
     </description>
   </class>
 
+  <class name="move_group/ApplyPlanningSceneService" type="move_group::ApplyPlanningSceneService" base_class_type="move_group::MoveGroupCapability">
+    <description>
+      Provide a ROS service for blocking updates to the planning scene
+    </description>
+  </class>
+
   <class name="move_group/ClearOctomapService" type="move_group::ClearOctomapService" base_class_type="move_group::MoveGroupCapability">
     <description>
       Provide a ROS service that allows for clearing the octomap

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
 
 }

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 <moveit/move_group/capability_names.h>
+
+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/class_loader.h>
+CLASS_LOADER_REGISTER_CLASS(move_group::ApplyPlanningSceneService, move_group::MoveGroupCapability)

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 <moveit/move_group/move_group_capability.h>
+#include <moveit_msgs/ApplyPlanningScene.h>
+
+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_