planning_scene_interface.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan, Sachin Chitta */
00036 
00037 #include <moveit/planning_scene_interface/planning_scene_interface.h>
00038 #include <moveit/move_group/capability_names.h>
00039 #include <moveit_msgs/GetPlanningScene.h>
00040 #include <moveit_msgs/ApplyPlanningScene.h>
00041 #include <ros/ros.h>
00042 #include <algorithm>
00043 
00044 namespace moveit
00045 {
00046 namespace planning_interface
00047 {
00048 class PlanningSceneInterface::PlanningSceneInterfaceImpl
00049 {
00050 public:
00051   PlanningSceneInterfaceImpl()
00052   {
00053     planning_scene_service_ =
00054         node_handle_.serviceClient<moveit_msgs::GetPlanningScene>(move_group::GET_PLANNING_SCENE_SERVICE_NAME);
00055     apply_planning_scene_service_ =
00056         node_handle_.serviceClient<moveit_msgs::ApplyPlanningScene>(move_group::APPLY_PLANNING_SCENE_SERVICE_NAME);
00057     planning_scene_diff_publisher_ = node_handle_.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
00058   }
00059 
00060   std::vector<std::string> getKnownObjectNames(bool with_type)
00061   {
00062     moveit_msgs::GetPlanningScene::Request request;
00063     moveit_msgs::GetPlanningScene::Response response;
00064     std::vector<std::string> result;
00065     request.components.components = request.components.WORLD_OBJECT_NAMES;
00066     if (!planning_scene_service_.call(request, response))
00067       return result;
00068     if (with_type)
00069     {
00070       for (std::size_t i = 0; i < response.scene.world.collision_objects.size(); ++i)
00071         if (!response.scene.world.collision_objects[i].type.key.empty())
00072           result.push_back(response.scene.world.collision_objects[i].id);
00073     }
00074     else
00075     {
00076       for (std::size_t i = 0; i < response.scene.world.collision_objects.size(); ++i)
00077         result.push_back(response.scene.world.collision_objects[i].id);
00078     }
00079     return result;
00080   }
00081 
00082   std::vector<std::string> getKnownObjectNamesInROI(double minx, double miny, double minz, double maxx, double maxy,
00083                                                     double maxz, bool with_type, std::vector<std::string>& types)
00084   {
00085     moveit_msgs::GetPlanningScene::Request request;
00086     moveit_msgs::GetPlanningScene::Response response;
00087     std::vector<std::string> result;
00088     request.components.components = request.components.WORLD_OBJECT_GEOMETRY;
00089     if (!planning_scene_service_.call(request, response))
00090     {
00091       ROS_WARN_NAMED("planning_scene_interface", "Could not call planning scene service to get object names");
00092       return result;
00093     }
00094 
00095     for (std::size_t i = 0; i < response.scene.world.collision_objects.size(); ++i)
00096     {
00097       if (with_type && response.scene.world.collision_objects[i].type.key.empty())
00098         continue;
00099       if (response.scene.world.collision_objects[i].mesh_poses.empty() &&
00100           response.scene.world.collision_objects[i].primitive_poses.empty())
00101         continue;
00102       bool good = true;
00103       for (std::size_t j = 0; j < response.scene.world.collision_objects[i].mesh_poses.size(); ++j)
00104         if (!(response.scene.world.collision_objects[i].mesh_poses[j].position.x >= minx &&
00105               response.scene.world.collision_objects[i].mesh_poses[j].position.x <= maxx &&
00106               response.scene.world.collision_objects[i].mesh_poses[j].position.y >= miny &&
00107               response.scene.world.collision_objects[i].mesh_poses[j].position.y <= maxy &&
00108               response.scene.world.collision_objects[i].mesh_poses[j].position.z >= minz &&
00109               response.scene.world.collision_objects[i].mesh_poses[j].position.z <= maxz))
00110         {
00111           good = false;
00112           break;
00113         }
00114       for (std::size_t j = 0; j < response.scene.world.collision_objects[i].primitive_poses.size(); ++j)
00115         if (!(response.scene.world.collision_objects[i].primitive_poses[j].position.x >= minx &&
00116               response.scene.world.collision_objects[i].primitive_poses[j].position.x <= maxx &&
00117               response.scene.world.collision_objects[i].primitive_poses[j].position.y >= miny &&
00118               response.scene.world.collision_objects[i].primitive_poses[j].position.y <= maxy &&
00119               response.scene.world.collision_objects[i].primitive_poses[j].position.z >= minz &&
00120               response.scene.world.collision_objects[i].primitive_poses[j].position.z <= maxz))
00121         {
00122           good = false;
00123           break;
00124         }
00125       if (good)
00126       {
00127         result.push_back(response.scene.world.collision_objects[i].id);
00128         if (with_type)
00129           types.push_back(response.scene.world.collision_objects[i].type.key);
00130       }
00131     }
00132     return result;
00133   }
00134 
00135   std::map<std::string, geometry_msgs::Pose> getObjectPoses(const std::vector<std::string>& object_ids)
00136   {
00137     moveit_msgs::GetPlanningScene::Request request;
00138     moveit_msgs::GetPlanningScene::Response response;
00139     std::map<std::string, geometry_msgs::Pose> result;
00140     request.components.components = request.components.WORLD_OBJECT_GEOMETRY;
00141     if (!planning_scene_service_.call(request, response))
00142     {
00143       ROS_WARN_NAMED("planning_scene_interface", "Could not call planning scene service to get object names");
00144       return result;
00145     }
00146 
00147     for (std::size_t i = 0; i < response.scene.world.collision_objects.size(); ++i)
00148     {
00149       if (std::find(object_ids.begin(), object_ids.end(), response.scene.world.collision_objects[i].id) !=
00150           object_ids.end())
00151       {
00152         if (response.scene.world.collision_objects[i].mesh_poses.empty() &&
00153             response.scene.world.collision_objects[i].primitive_poses.empty())
00154           continue;
00155         if (!response.scene.world.collision_objects[i].mesh_poses.empty())
00156           result[response.scene.world.collision_objects[i].id] =
00157               response.scene.world.collision_objects[i].mesh_poses[0];
00158         else
00159           result[response.scene.world.collision_objects[i].id] =
00160               response.scene.world.collision_objects[i].primitive_poses[0];
00161       }
00162     }
00163     return result;
00164   }
00165 
00166   std::map<std::string, moveit_msgs::CollisionObject> getObjects(const std::vector<std::string>& object_ids)
00167   {
00168     moveit_msgs::GetPlanningScene::Request request;
00169     moveit_msgs::GetPlanningScene::Response response;
00170     std::map<std::string, moveit_msgs::CollisionObject> result;
00171     request.components.components = request.components.WORLD_OBJECT_GEOMETRY;
00172     if (!planning_scene_service_.call(request, response))
00173     {
00174       ROS_WARN_NAMED("planning_scene_interface", "Could not call planning scene service to get object geometries");
00175       return result;
00176     }
00177 
00178     for (std::size_t i = 0; i < response.scene.world.collision_objects.size(); ++i)
00179     {
00180       if (object_ids.empty() ||
00181           std::find(object_ids.begin(), object_ids.end(), response.scene.world.collision_objects[i].id) !=
00182               object_ids.end())
00183       {
00184         result[response.scene.world.collision_objects[i].id] = response.scene.world.collision_objects[i];
00185       }
00186     }
00187     return result;
00188   }
00189 
00190   std::map<std::string, moveit_msgs::AttachedCollisionObject>
00191   getAttachedObjects(const std::vector<std::string>& object_ids)
00192   {
00193     moveit_msgs::GetPlanningScene::Request request;
00194     moveit_msgs::GetPlanningScene::Response response;
00195     std::map<std::string, moveit_msgs::AttachedCollisionObject> result;
00196     request.components.components = request.components.ROBOT_STATE_ATTACHED_OBJECTS;
00197     if (!planning_scene_service_.call(request, response))
00198     {
00199       ROS_WARN_NAMED("planning_scene_interface", "Could not call planning scene service to get attached object "
00200                                                  "geometries");
00201       return result;
00202     }
00203 
00204     for (std::size_t i = 0; i < response.scene.robot_state.attached_collision_objects.size(); ++i)
00205     {
00206       if (object_ids.empty() ||
00207           std::find(object_ids.begin(), object_ids.end(),
00208                     response.scene.robot_state.attached_collision_objects[i].object.id) != object_ids.end())
00209       {
00210         result[response.scene.robot_state.attached_collision_objects[i].object.id] =
00211             response.scene.robot_state.attached_collision_objects[i];
00212       }
00213     }
00214     return result;
00215   }
00216 
00217   bool applyPlanningScene(const moveit_msgs::PlanningScene& planning_scene)
00218   {
00219     moveit_msgs::ApplyPlanningScene::Request request;
00220     moveit_msgs::ApplyPlanningScene::Response response;
00221     request.scene = planning_scene;
00222     if (!apply_planning_scene_service_.call(request, response))
00223     {
00224       ROS_WARN_NAMED("planning_scene_interface", "Failed to call ApplyPlanningScene service");
00225       return false;
00226     }
00227     return response.success;
00228   }
00229 
00230   void addCollisionObjects(const std::vector<moveit_msgs::CollisionObject>& collision_objects) const
00231   {
00232     moveit_msgs::PlanningScene planning_scene;
00233     planning_scene.world.collision_objects = collision_objects;
00234     planning_scene.is_diff = true;
00235     planning_scene_diff_publisher_.publish(planning_scene);
00236   }
00237 
00238   void removeCollisionObjects(const std::vector<std::string>& object_ids) const
00239   {
00240     moveit_msgs::PlanningScene planning_scene;
00241     moveit_msgs::CollisionObject object;
00242     for (std::size_t i = 0; i < object_ids.size(); ++i)
00243     {
00244       object.id = object_ids[i];
00245       object.operation = object.REMOVE;
00246       planning_scene.world.collision_objects.push_back(object);
00247     }
00248     planning_scene.is_diff = true;
00249     planning_scene_diff_publisher_.publish(planning_scene);
00250   }
00251 
00252 private:
00253   ros::NodeHandle node_handle_;
00254   ros::ServiceClient planning_scene_service_;
00255   ros::ServiceClient apply_planning_scene_service_;
00256   ros::Publisher planning_scene_diff_publisher_;
00257   robot_model::RobotModelConstPtr robot_model_;
00258 };
00259 
00260 PlanningSceneInterface::PlanningSceneInterface()
00261 {
00262   impl_ = new PlanningSceneInterfaceImpl();
00263 }
00264 
00265 PlanningSceneInterface::~PlanningSceneInterface()
00266 {
00267   delete impl_;
00268 }
00269 
00270 std::vector<std::string> PlanningSceneInterface::getKnownObjectNames(bool with_type)
00271 {
00272   return impl_->getKnownObjectNames(with_type);
00273 }
00274 
00275 std::vector<std::string> PlanningSceneInterface::getKnownObjectNamesInROI(double minx, double miny, double minz,
00276                                                                           double maxx, double maxy, double maxz,
00277                                                                           bool with_type,
00278                                                                           std::vector<std::string>& types)
00279 {
00280   return impl_->getKnownObjectNamesInROI(minx, miny, minz, maxx, maxy, maxz, with_type, types);
00281 }
00282 
00283 std::map<std::string, geometry_msgs::Pose>
00284 PlanningSceneInterface::getObjectPoses(const std::vector<std::string>& object_ids)
00285 {
00286   return impl_->getObjectPoses(object_ids);
00287 }
00288 
00289 std::map<std::string, moveit_msgs::CollisionObject>
00290 PlanningSceneInterface::getObjects(const std::vector<std::string>& object_ids)
00291 {
00292   return impl_->getObjects(object_ids);
00293 }
00294 
00295 std::map<std::string, moveit_msgs::AttachedCollisionObject>
00296 PlanningSceneInterface::getAttachedObjects(const std::vector<std::string>& object_ids)
00297 {
00298   return impl_->getAttachedObjects(object_ids);
00299 }
00300 
00301 bool PlanningSceneInterface::applyCollisionObject(const moveit_msgs::CollisionObject& collision_object)
00302 {
00303   moveit_msgs::PlanningScene ps;
00304   ps.robot_state.is_diff = true;
00305   ps.is_diff = true;
00306   ps.world.collision_objects.reserve(1);
00307   ps.world.collision_objects.push_back(collision_object);
00308   return applyPlanningScene(ps);
00309 }
00310 
00311 bool PlanningSceneInterface::applyCollisionObjects(const std::vector<moveit_msgs::CollisionObject>& collision_objects)
00312 {
00313   moveit_msgs::PlanningScene ps;
00314   ps.robot_state.is_diff = true;
00315   ps.is_diff = true;
00316   ps.world.collision_objects = collision_objects;
00317   return applyPlanningScene(ps);
00318 }
00319 
00320 bool PlanningSceneInterface::applyAttachedCollisionObject(const moveit_msgs::AttachedCollisionObject& collision_object)
00321 {
00322   moveit_msgs::PlanningScene ps;
00323   ps.robot_state.is_diff = true;
00324   ps.is_diff = true;
00325   ps.robot_state.attached_collision_objects.reserve(1);
00326   ps.robot_state.attached_collision_objects.push_back(collision_object);
00327   return applyPlanningScene(ps);
00328 }
00329 
00330 bool PlanningSceneInterface::applyAttachedCollisionObjects(
00331     const std::vector<moveit_msgs::AttachedCollisionObject>& attached_collision_objects)
00332 {
00333   moveit_msgs::PlanningScene ps;
00334   ps.robot_state.is_diff = true;
00335   ps.is_diff = true;
00336   ps.robot_state.attached_collision_objects = attached_collision_objects;
00337   return applyPlanningScene(ps);
00338 }
00339 
00340 bool PlanningSceneInterface::applyPlanningScene(const moveit_msgs::PlanningScene& ps)
00341 {
00342   impl_->applyPlanningScene(ps);
00343 }
00344 
00345 void PlanningSceneInterface::addCollisionObjects(
00346     const std::vector<moveit_msgs::CollisionObject>& collision_objects) const
00347 {
00348   return impl_->addCollisionObjects(collision_objects);
00349 }
00350 
00351 void PlanningSceneInterface::removeCollisionObjects(const std::vector<std::string>& object_ids) const
00352 {
00353   return impl_->removeCollisionObjects(object_ids);
00354 }
00355 }
00356 }


planning_interface
Author(s): Ioan Sucan
autogenerated on Mon Jul 24 2017 02:22:06