Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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 <ros/ros.h>
00041
00042 namespace moveit
00043 {
00044 namespace planning_interface
00045 {
00046
00047 class PlanningSceneInterface::PlanningSceneInterfaceImpl
00048 {
00049 public:
00050
00051 PlanningSceneInterfaceImpl()
00052 {
00053 planning_scene_service_ = node_handle_.serviceClient<moveit_msgs::GetPlanningScene>(move_group::GET_PLANNING_SCENE_SERVICE_NAME);
00054 planning_scene_diff_publisher_ = node_handle_.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
00055 }
00056
00057 std::vector<std::string> getKnownObjectNames(bool with_type)
00058 {
00059 moveit_msgs::GetPlanningScene::Request request;
00060 moveit_msgs::GetPlanningScene::Response response;
00061 std::vector<std::string> result;
00062 request.components.components = request.components.WORLD_OBJECT_NAMES;
00063 if (!planning_scene_service_.call(request, response))
00064 return result;
00065 if (with_type)
00066 {
00067 for (std::size_t i = 0 ; i < response.scene.world.collision_objects.size() ; ++i)
00068 if (!response.scene.world.collision_objects[i].type.key.empty())
00069 result.push_back(response.scene.world.collision_objects[i].id);
00070 }
00071 else
00072 {
00073 for (std::size_t i = 0 ; i < response.scene.world.collision_objects.size() ; ++i)
00074 result.push_back(response.scene.world.collision_objects[i].id);
00075 }
00076 return result;
00077 }
00078
00079 std::vector<std::string> getKnownObjectNamesInROI(double minx, double miny, double minz, double maxx, double maxy, double maxz, bool with_type, std::vector<std::string> &types)
00080 {
00081 moveit_msgs::GetPlanningScene::Request request;
00082 moveit_msgs::GetPlanningScene::Response response;
00083 std::vector<std::string> result;
00084 request.components.components = request.components.WORLD_OBJECT_GEOMETRY;
00085 if (!planning_scene_service_.call(request, response))
00086 {
00087 ROS_WARN("Could not call planning scene service to get object names");
00088 return result;
00089 }
00090
00091 for (std::size_t i = 0; i < response.scene.world.collision_objects.size() ; ++i)
00092 {
00093 if (with_type && response.scene.world.collision_objects[i].type.key.empty())
00094 continue;
00095 if (response.scene.world.collision_objects[i].mesh_poses.empty() &&
00096 response.scene.world.collision_objects[i].primitive_poses.empty())
00097 continue;
00098 bool good = true;
00099 for (std::size_t j = 0 ; j < response.scene.world.collision_objects[i].mesh_poses.size() ; ++j)
00100 if (!(response.scene.world.collision_objects[i].mesh_poses[j].position.x >= minx &&
00101 response.scene.world.collision_objects[i].mesh_poses[j].position.x <= maxx &&
00102 response.scene.world.collision_objects[i].mesh_poses[j].position.y >= miny &&
00103 response.scene.world.collision_objects[i].mesh_poses[j].position.y <= maxy &&
00104 response.scene.world.collision_objects[i].mesh_poses[j].position.z >= minz &&
00105 response.scene.world.collision_objects[i].mesh_poses[j].position.z <= maxz))
00106 {
00107 good = false;
00108 break;
00109 }
00110 for (std::size_t j = 0 ; j < response.scene.world.collision_objects[i].primitive_poses.size() ; ++j)
00111 if (!(response.scene.world.collision_objects[i].primitive_poses[j].position.x >= minx &&
00112 response.scene.world.collision_objects[i].primitive_poses[j].position.x <= maxx &&
00113 response.scene.world.collision_objects[i].primitive_poses[j].position.y >= miny &&
00114 response.scene.world.collision_objects[i].primitive_poses[j].position.y <= maxy &&
00115 response.scene.world.collision_objects[i].primitive_poses[j].position.z >= minz &&
00116 response.scene.world.collision_objects[i].primitive_poses[j].position.z <= maxz))
00117 {
00118 good = false;
00119 break;
00120 }
00121 if (good)
00122 {
00123 result.push_back(response.scene.world.collision_objects[i].id);
00124 if(with_type)
00125 types.push_back(response.scene.world.collision_objects[i].type.key);
00126 }
00127 }
00128 return result;
00129 }
00130
00131 std::map<std::string, geometry_msgs::Pose> getObjectPoses(const std::vector<std::string> &object_ids)
00132 {
00133 moveit_msgs::GetPlanningScene::Request request;
00134 moveit_msgs::GetPlanningScene::Response response;
00135 std::map<std::string, geometry_msgs::Pose> result;
00136 request.components.components = request.components.WORLD_OBJECT_GEOMETRY;
00137 if (!planning_scene_service_.call(request, response))
00138 {
00139 ROS_WARN("Could not call planning scene service to get object names");
00140 return result;
00141 }
00142
00143 for(std::size_t i=0; i < object_ids.size(); ++i)
00144 {
00145 for (std::size_t j=0; j < response.scene.world.collision_objects.size() ; ++j)
00146 {
00147 if(response.scene.world.collision_objects[j].id == object_ids[i])
00148 {
00149 if (response.scene.world.collision_objects[j].mesh_poses.empty() &&
00150 response.scene.world.collision_objects[j].primitive_poses.empty())
00151 continue;
00152 if(!response.scene.world.collision_objects[j].mesh_poses.empty())
00153 result[response.scene.world.collision_objects[j].id] = response.scene.world.collision_objects[j].mesh_poses[0];
00154 else
00155 result[response.scene.world.collision_objects[j].id] = response.scene.world.collision_objects[j].primitive_poses[0];
00156 }
00157 }
00158 }
00159 return result;
00160 }
00161
00162 void addCollisionObjects(const std::vector<moveit_msgs::CollisionObject> &collision_objects) const
00163 {
00164 moveit_msgs::PlanningScene planning_scene;
00165 planning_scene.world.collision_objects = collision_objects;
00166 planning_scene.is_diff = true;
00167 planning_scene_diff_publisher_.publish(planning_scene);
00168 }
00169
00170 void removeCollisionObjects(const std::vector<std::string> &object_ids) const
00171 {
00172 moveit_msgs::PlanningScene planning_scene;
00173 moveit_msgs::CollisionObject object;
00174 for(std::size_t i=0; i < object_ids.size(); ++i)
00175 {
00176 object.id = object_ids[i];
00177 object.operation = object.REMOVE;
00178 planning_scene.world.collision_objects.push_back(object);
00179 }
00180 planning_scene.is_diff = true;
00181 planning_scene_diff_publisher_.publish(planning_scene);
00182 }
00183
00184 private:
00185
00186 ros::NodeHandle node_handle_;
00187 ros::ServiceClient planning_scene_service_;
00188 ros::Publisher planning_scene_diff_publisher_;
00189 robot_model::RobotModelConstPtr robot_model_;
00190 };
00191
00192 PlanningSceneInterface::PlanningSceneInterface()
00193 {
00194 impl_ = new PlanningSceneInterfaceImpl();
00195 }
00196
00197 PlanningSceneInterface::~PlanningSceneInterface()
00198 {
00199 delete impl_;
00200 }
00201
00202 std::vector<std::string> PlanningSceneInterface::getKnownObjectNames(bool with_type)
00203 {
00204 return impl_->getKnownObjectNames(with_type);
00205 }
00206
00207 std::vector<std::string> PlanningSceneInterface::getKnownObjectNamesInROI(double minx, double miny, double minz, double maxx, double maxy, double maxz, bool with_type, std::vector<std::string> &types)
00208 {
00209 return impl_->getKnownObjectNamesInROI(minx, miny, minz, maxx, maxy, maxz, with_type, types);
00210 }
00211
00212 std::map<std::string, geometry_msgs::Pose> PlanningSceneInterface::getObjectPoses(const std::vector<std::string> &object_ids)
00213 {
00214 return impl_->getObjectPoses(object_ids);
00215 }
00216
00217 void PlanningSceneInterface::addCollisionObjects(const std::vector<moveit_msgs::CollisionObject> &collision_objects) const
00218 {
00219 return impl_->addCollisionObjects(collision_objects);
00220 }
00221
00222 void PlanningSceneInterface::removeCollisionObjects(const std::vector<std::string> &object_ids) const
00223 {
00224 return impl_->removeCollisionObjects(object_ids);
00225 }
00226
00227 }
00228 }