39 #include <moveit_msgs/GetPlanningScene.h> 40 #include <moveit_msgs/ApplyPlanningScene.h> 48 static const std::string
LOGNAME =
"planning_scene_interface";
69 if (!planning_scene_service_.exists() || !apply_planning_scene_service_.exists())
71 throw std::runtime_error(
"ROS services not available");
78 moveit_msgs::GetPlanningScene::Request request;
79 moveit_msgs::GetPlanningScene::Response
response;
80 std::vector<std::string> result;
81 request.components.components = request.components.WORLD_OBJECT_NAMES;
86 for (std::size_t i = 0; i < response.scene.world.collision_objects.size(); ++i)
87 if (!response.scene.world.collision_objects[i].type.key.empty())
88 result.push_back(response.scene.world.collision_objects[i].id);
92 for (std::size_t i = 0; i < response.scene.world.collision_objects.size(); ++i)
93 result.push_back(response.scene.world.collision_objects[i].id);
99 double maxz,
bool with_type, std::vector<std::string>& types)
101 moveit_msgs::GetPlanningScene::Request request;
102 moveit_msgs::GetPlanningScene::Response
response;
103 std::vector<std::string> result;
104 request.components.components = request.components.WORLD_OBJECT_GEOMETRY;
107 ROS_WARN_NAMED(LOGNAME,
"Could not call planning scene service to get object names");
111 for (std::size_t i = 0; i < response.scene.world.collision_objects.size(); ++i)
113 if (with_type && response.scene.world.collision_objects[i].type.key.empty())
115 if (response.scene.world.collision_objects[i].mesh_poses.empty() &&
116 response.scene.world.collision_objects[i].primitive_poses.empty())
119 for (std::size_t j = 0; j < response.scene.world.collision_objects[i].mesh_poses.size(); ++j)
120 if (!(response.scene.world.collision_objects[i].mesh_poses[j].position.x >= minx &&
121 response.scene.world.collision_objects[i].mesh_poses[j].position.x <= maxx &&
122 response.scene.world.collision_objects[i].mesh_poses[j].position.y >= miny &&
123 response.scene.world.collision_objects[i].mesh_poses[j].position.y <= maxy &&
124 response.scene.world.collision_objects[i].mesh_poses[j].position.z >= minz &&
125 response.scene.world.collision_objects[i].mesh_poses[j].position.z <= maxz))
130 for (std::size_t j = 0; j < response.scene.world.collision_objects[i].primitive_poses.size(); ++j)
131 if (!(response.scene.world.collision_objects[i].primitive_poses[j].position.x >= minx &&
132 response.scene.world.collision_objects[i].primitive_poses[j].position.x <= maxx &&
133 response.scene.world.collision_objects[i].primitive_poses[j].position.y >= miny &&
134 response.scene.world.collision_objects[i].primitive_poses[j].position.y <= maxy &&
135 response.scene.world.collision_objects[i].primitive_poses[j].position.z >= minz &&
136 response.scene.world.collision_objects[i].primitive_poses[j].position.z <= maxz))
143 result.push_back(response.scene.world.collision_objects[i].id);
145 types.push_back(response.scene.world.collision_objects[i].type.key);
151 std::map<std::string, geometry_msgs::Pose>
getObjectPoses(
const std::vector<std::string>& object_ids)
153 moveit_msgs::GetPlanningScene::Request request;
154 moveit_msgs::GetPlanningScene::Response
response;
155 std::map<std::string, geometry_msgs::Pose> result;
156 request.components.components = request.components.WORLD_OBJECT_GEOMETRY;
159 ROS_WARN_NAMED(LOGNAME,
"Could not call planning scene service to get object names");
163 for (std::size_t i = 0; i < response.scene.world.collision_objects.size(); ++i)
165 if (std::find(object_ids.begin(), object_ids.end(), response.scene.world.collision_objects[i].id) !=
168 if (response.scene.world.collision_objects[i].mesh_poses.empty() &&
169 response.scene.world.collision_objects[i].primitive_poses.empty())
171 if (!response.scene.world.collision_objects[i].mesh_poses.empty())
172 result[response.scene.world.collision_objects[i].id] =
173 response.scene.world.collision_objects[i].mesh_poses[0];
175 result[response.scene.world.collision_objects[i].id] =
176 response.scene.world.collision_objects[i].primitive_poses[0];
182 std::map<std::string, moveit_msgs::CollisionObject>
getObjects(
const std::vector<std::string>& object_ids)
184 moveit_msgs::GetPlanningScene::Request request;
185 moveit_msgs::GetPlanningScene::Response
response;
186 std::map<std::string, moveit_msgs::CollisionObject> result;
187 request.components.components = request.components.WORLD_OBJECT_GEOMETRY;
190 ROS_WARN_NAMED(LOGNAME,
"Could not call planning scene service to get object geometries");
194 for (std::size_t i = 0; i < response.scene.world.collision_objects.size(); ++i)
196 if (object_ids.empty() || std::find(object_ids.begin(), object_ids.end(),
197 response.scene.world.collision_objects[i].id) != object_ids.end())
199 result[response.scene.world.collision_objects[i].id] = response.scene.world.collision_objects[i];
205 std::map<std::string, moveit_msgs::AttachedCollisionObject>
208 moveit_msgs::GetPlanningScene::Request request;
209 moveit_msgs::GetPlanningScene::Response
response;
210 std::map<std::string, moveit_msgs::AttachedCollisionObject> result;
211 request.components.components = request.components.ROBOT_STATE_ATTACHED_OBJECTS;
214 ROS_WARN_NAMED(LOGNAME,
"Could not call planning scene service to get attached object geometries");
218 for (std::size_t i = 0; i < response.scene.robot_state.attached_collision_objects.size(); ++i)
220 if (object_ids.empty() ||
221 std::find(object_ids.begin(), object_ids.end(),
222 response.scene.robot_state.attached_collision_objects[i].object.id) != object_ids.end())
224 result[response.scene.robot_state.attached_collision_objects[i].object.id] =
225 response.scene.robot_state.attached_collision_objects[i];
233 moveit_msgs::ApplyPlanningScene::Request request;
234 moveit_msgs::ApplyPlanningScene::Response
response;
235 request.scene = planning_scene;
238 ROS_WARN_NAMED(LOGNAME,
"Failed to call ApplyPlanningScene service");
241 return response.success;
245 const std::vector<moveit_msgs::ObjectColor>& object_colors)
const 248 planning_scene.world.collision_objects = collision_objects;
249 planning_scene.object_colors = object_colors;
251 for (
size_t i = 0; i < planning_scene.object_colors.size(); ++i)
253 if (planning_scene.object_colors[i].id.empty() && i < collision_objects.size())
254 planning_scene.object_colors[i].id = collision_objects[i].id;
259 planning_scene.is_diff =
true;
266 moveit_msgs::CollisionObject object;
267 for (std::size_t i = 0; i < object_ids.size(); ++i)
269 object.id = object_ids[i];
270 object.operation =
object.REMOVE;
271 planning_scene.world.collision_objects.push_back(
object);
273 planning_scene.is_diff =
true;
312 double maxx,
double maxy,
double maxz,
314 std::vector<std::string>& types)
319 std::map<std::string, geometry_msgs::Pose>
325 std::map<std::string, moveit_msgs::CollisionObject>
331 std::map<std::string, moveit_msgs::AttachedCollisionObject>
339 moveit_msgs::PlanningScene ps;
340 ps.robot_state.is_diff =
true;
342 ps.world.collision_objects.reserve(1);
343 ps.world.collision_objects.push_back(collision_object);
348 const std_msgs::ColorRGBA& object_color)
350 moveit_msgs::PlanningScene ps;
351 ps.robot_state.is_diff =
true;
353 ps.world.collision_objects.reserve(1);
354 ps.world.collision_objects.push_back(collision_object);
355 moveit_msgs::ObjectColor oc;
356 oc.id = collision_object.id;
357 oc.color = object_color;
358 ps.object_colors.push_back(oc);
363 const std::vector<moveit_msgs::ObjectColor>& object_colors)
365 moveit_msgs::PlanningScene ps;
366 ps.robot_state.is_diff =
true;
368 ps.world.collision_objects = collision_objects;
369 ps.object_colors = object_colors;
371 for (
size_t i = 0; i < ps.object_colors.size(); ++i)
373 if (ps.object_colors[i].id.empty() && i < collision_objects.size())
374 ps.object_colors[i].id = collision_objects[i].id;
384 moveit_msgs::PlanningScene ps;
385 ps.robot_state.is_diff =
true;
387 ps.robot_state.attached_collision_objects.reserve(1);
388 ps.robot_state.attached_collision_objects.push_back(collision_object);
393 const std::vector<moveit_msgs::AttachedCollisionObject>& collision_objects)
395 moveit_msgs::PlanningScene ps;
396 ps.robot_state.is_diff =
true;
398 ps.robot_state.attached_collision_objects = collision_objects;
408 const std::vector<moveit_msgs::ObjectColor>& object_colors)
const ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
std::map< std::string, geometry_msgs::Pose > getObjectPoses(const std::vector< std::string > &object_ids)
bool applyCollisionObject(const moveit_msgs::CollisionObject &collision_object)
Apply collision object to the planning scene of the move_group node synchronously. Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group's monitored scene.
std::vector< std::string > getKnownObjectNames(bool with_type)
PlanningSceneInterfaceImpl(const std::string &ns="", bool wait=true)
static const std::string GET_PLANNING_SCENE_SERVICE_NAME
#define ROS_WARN_NAMED(name,...)
bool applyCollisionObjects(const std::vector< moveit_msgs::CollisionObject > &collision_objects, const std::vector< moveit_msgs::ObjectColor > &object_colors=std::vector< moveit_msgs::ObjectColor >())
Apply collision objects to the planning scene of the move_group node synchronously. Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group's monitored scene. If object_colors do not specify an id, the corresponding object id from collision_objects is used.
bool call(MReq &req, MRes &res)
ros::ServiceClient apply_planning_scene_service_
std::map< std::string, moveit_msgs::CollisionObject > getObjects(const std::vector< std::string > &object_ids=std::vector< std::string >())
Get the objects identified by the given object ids list. If no ids are provided, return all the known...
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)
PlanningSceneInterfaceImpl * impl_
static const std::string APPLY_PLANNING_SCENE_SERVICE_NAME
std::map< std::string, geometry_msgs::Pose > getObjectPoses(const std::vector< std::string > &object_ids)
Get the poses from the objects identified by the given object ids list.
void publish(const boost::shared_ptr< M > &message) const
bool applyAttachedCollisionObjects(const std::vector< moveit_msgs::AttachedCollisionObject > &attached_collision_objects)
Apply attached collision objects to the planning scene of the move_group node synchronously. Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group's monitored scene.
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
bool applyPlanningScene(const moveit_msgs::PlanningScene &ps)
Update the planning_scene of the move_group node with the given ps synchronously. Other PlanningScene...
void waitForService(ros::ServiceClient &srv)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::ServiceClient planning_scene_service_
void removeCollisionObjects(const std::vector< std::string > &object_ids) const
Remove collision objects from the world via /planning_scene.
void removeCollisionObjects(const std::vector< std::string > &object_ids) const
std::map< std::string, moveit_msgs::AttachedCollisionObject > getAttachedObjects(const std::vector< std::string > &object_ids=std::vector< std::string >())
Get the attached objects identified by the given object ids list. If no ids are provided, return all the attached objects.
bool applyAttachedCollisionObject(const moveit_msgs::AttachedCollisionObject &attached_collision_object)
Apply attached collision object to the planning scene of the move_group node synchronously. Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group's monitored scene.
void addCollisionObjects(const std::vector< moveit_msgs::CollisionObject > &collision_objects, const std::vector< moveit_msgs::ObjectColor > &object_colors) const
std::vector< std::string > getKnownObjectNames(bool with_type=false)
Get the names of all known objects in the world. If with_type is set to true, only return objects tha...
bool applyPlanningScene(const moveit_msgs::PlanningScene &planning_scene)
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)
Get the names of known objects in the world that are located within a bounding region (specified in t...
~PlanningSceneInterface()
ros::NodeHandle node_handle_
PlanningSceneInterface(const std::string &ns="", bool wait=true)
std::map< std::string, moveit_msgs::AttachedCollisionObject > getAttachedObjects(const std::vector< std::string > &object_ids)
std::map< std::string, moveit_msgs::CollisionObject > getObjects(const std::vector< std::string > &object_ids)
ros::Publisher planning_scene_diff_publisher_
robot_model::RobotModelConstPtr robot_model_
const std::string response
void addCollisionObjects(const std::vector< moveit_msgs::CollisionObject > &collision_objects, const std::vector< moveit_msgs::ObjectColor > &object_colors=std::vector< moveit_msgs::ObjectColor >()) const
Add collision objects to the world via /planning_scene. Make sure object.operation is set to object...
#define ROS_WARN_STREAM_NAMED(name, args)