39 #include <moveit_msgs/GetPlanningScene.h>
40 #include <moveit_msgs/ApplyPlanningScene.h>
49 static const std::string
LOGNAME =
"planning_scene_interface";
51 class PlanningSceneInterface::PlanningSceneInterfaceImpl
72 throw std::runtime_error(
"ROS services not available");
79 moveit_msgs::GetPlanningScene::Request request;
80 moveit_msgs::GetPlanningScene::Response
response;
81 std::vector<std::string> result;
82 request.components.components = request.components.WORLD_OBJECT_NAMES;
87 for (
const moveit_msgs::CollisionObject& collision_object :
response.scene.world.collision_objects)
88 if (!collision_object.type.key.empty())
89 result.push_back(collision_object.id);
93 for (
const moveit_msgs::CollisionObject& collision_object :
response.scene.world.collision_objects)
94 result.push_back(collision_object.id);
100 double maxz,
bool with_type, std::vector<std::string>& types)
102 moveit_msgs::GetPlanningScene::Request request;
103 moveit_msgs::GetPlanningScene::Response
response;
104 std::vector<std::string> result;
105 request.components.components = request.components.WORLD_OBJECT_GEOMETRY;
112 for (
const moveit_msgs::CollisionObject& collision_object :
response.scene.world.collision_objects)
114 if (with_type && collision_object.type.key.empty())
116 if (collision_object.mesh_poses.empty() && collision_object.primitive_poses.empty())
120 geometry_msgs::Pose pose_tf;
121 geometry_msgs::TransformStamped
tf;
122 tf.header = collision_object.header;
123 tf.transform.translation.x = collision_object.pose.position.x;
124 tf.transform.translation.y = collision_object.pose.position.y;
125 tf.transform.translation.z = collision_object.pose.position.z;
126 tf.transform.rotation.x = collision_object.pose.orientation.x;
127 tf.transform.rotation.y = collision_object.pose.orientation.y;
128 tf.transform.rotation.z = collision_object.pose.orientation.z;
129 tf.transform.rotation.w = collision_object.pose.orientation.w;
132 for (
const geometry_msgs::Pose& mesh_pose : collision_object.mesh_poses)
135 if (!(pose_tf.position.x >= minx && pose_tf.position.x <= maxx && pose_tf.position.y >= miny &&
136 pose_tf.position.y <= maxy && pose_tf.position.z >= minz && pose_tf.position.z <= maxz))
142 for (
const geometry_msgs::Pose& primitive_pose : collision_object.primitive_poses)
145 if (!(pose_tf.position.x >= minx && pose_tf.position.x <= maxx && pose_tf.position.y >= miny &&
146 pose_tf.position.y <= maxy && pose_tf.position.z >= minz && pose_tf.position.z <= maxz))
154 result.push_back(collision_object.id);
156 types.push_back(collision_object.type.key);
162 std::map<std::string, geometry_msgs::Pose>
getObjectPoses(
const std::vector<std::string>& object_ids)
164 moveit_msgs::GetPlanningScene::Request request;
165 moveit_msgs::GetPlanningScene::Response
response;
166 std::map<std::string, geometry_msgs::Pose> result;
167 request.components.components = request.components.WORLD_OBJECT_GEOMETRY;
174 for (
const moveit_msgs::CollisionObject& collision_object :
response.scene.world.collision_objects)
176 if (std::find(object_ids.begin(), object_ids.end(), collision_object.id) != object_ids.end())
178 result[collision_object.id] = collision_object.pose;
184 std::map<std::string, moveit_msgs::CollisionObject>
getObjects(
const std::vector<std::string>& object_ids)
186 moveit_msgs::GetPlanningScene::Request request;
187 moveit_msgs::GetPlanningScene::Response
response;
188 std::map<std::string, moveit_msgs::CollisionObject> result;
189 request.components.components = request.components.WORLD_OBJECT_GEOMETRY;
196 for (
const moveit_msgs::CollisionObject& collision_object :
response.scene.world.collision_objects)
198 if (object_ids.empty() || std::find(object_ids.begin(), object_ids.end(), collision_object.id) != object_ids.end())
200 result[collision_object.id] = collision_object;
206 std::map<std::string, moveit_msgs::AttachedCollisionObject>
209 moveit_msgs::GetPlanningScene::Request request;
210 moveit_msgs::GetPlanningScene::Response
response;
211 std::map<std::string, moveit_msgs::AttachedCollisionObject> result;
212 request.components.components = request.components.ROBOT_STATE_ATTACHED_OBJECTS;
219 for (
const moveit_msgs::AttachedCollisionObject& attached_collision_object :
220 response.scene.robot_state.attached_collision_objects)
222 if (object_ids.empty() ||
223 std::find(object_ids.begin(), object_ids.end(), attached_collision_object.object.id) != object_ids.end())
225 result[attached_collision_object.object.id] = attached_collision_object;
233 moveit_msgs::GetPlanningScene::Request request;
234 moveit_msgs::GetPlanningScene::Response
response;
235 request.components.components = components;
239 return moveit_msgs::PlanningScene();
246 moveit_msgs::ApplyPlanningScene::Request request;
247 moveit_msgs::ApplyPlanningScene::Response
response;
257 void addCollisionObjects(
const std::vector<moveit_msgs::CollisionObject>& collision_objects,
258 const std::vector<moveit_msgs::ObjectColor>& object_colors)
const
266 if (
planning_scene.object_colors[i].id.empty() && i < collision_objects.size())
279 moveit_msgs::CollisionObject object;
280 for (
const std::string& object_id : object_ids)
282 object.id = object_id;
283 object.operation =
object.REMOVE;
292 moveit_msgs::PlanningScene clear_scene;
293 clear_scene.is_diff =
true;
294 clear_scene.robot_state.is_diff =
true;
295 clear_scene.robot_state.attached_collision_objects.resize(1);
296 clear_scene.robot_state.attached_collision_objects[0].object.operation = moveit_msgs::CollisionObject::REMOVE;
297 clear_scene.world.collision_objects.resize(1);
298 clear_scene.world.collision_objects[0].operation = moveit_msgs::CollisionObject::REMOVE;
338 double maxx,
double maxy,
double maxz,
340 std::vector<std::string>& types)
345 std::map<std::string, geometry_msgs::Pose>
351 std::map<std::string, moveit_msgs::CollisionObject>
357 std::map<std::string, moveit_msgs::AttachedCollisionObject>
365 moveit_msgs::PlanningScene ps;
366 ps.robot_state.is_diff =
true;
368 ps.world.collision_objects.reserve(1);
369 ps.world.collision_objects.push_back(collision_object);
374 const std_msgs::ColorRGBA& object_color)
376 moveit_msgs::PlanningScene ps;
377 ps.robot_state.is_diff =
true;
379 ps.world.collision_objects.reserve(1);
380 ps.world.collision_objects.push_back(collision_object);
381 moveit_msgs::ObjectColor oc;
382 oc.id = collision_object.id;
383 oc.color = object_color;
384 ps.object_colors.push_back(oc);
389 const std::vector<moveit_msgs::ObjectColor>& object_colors)
391 moveit_msgs::PlanningScene ps;
392 ps.robot_state.is_diff =
true;
394 ps.world.collision_objects = collision_objects;
395 ps.object_colors = object_colors;
397 for (
size_t i = 0; i < ps.object_colors.size(); ++i)
399 if (ps.object_colors[i].id.empty() && i < collision_objects.size())
400 ps.object_colors[i].id = collision_objects[i].id;
410 moveit_msgs::PlanningScene ps;
411 ps.robot_state.is_diff =
true;
413 ps.robot_state.attached_collision_objects.reserve(1);
414 ps.robot_state.attached_collision_objects.push_back(collision_object);
419 const std::vector<moveit_msgs::AttachedCollisionObject>& collision_objects)
421 moveit_msgs::PlanningScene ps;
422 ps.robot_state.is_diff =
true;
424 ps.robot_state.attached_collision_objects = collision_objects;
439 const std::vector<moveit_msgs::ObjectColor>& object_colors)
const