37 #include <boost/algorithm/string.hpp> 56 const std::string
LOGNAME =
"planning_scene";
74 if (Transforms::isFixedFrame(frame))
82 const Eigen::Isometry3d&
getTransform(
const std::string& from_frame)
const override 92 collision_detection::World::ObjectConstPtr obj =
scene_->
getWorld()->getObject(object_id);
93 return obj->shape_poses_.size() == 1;
103 return msg.name.empty() && msg.fixed_frame_transforms.empty() && msg.allowed_collision_matrix.entry_names.empty() &&
104 msg.link_padding.empty() && msg.link_scale.empty() &&
isEmpty(msg.robot_state) &&
isEmpty(msg.world);
112 return static_cast<bool>(msg.is_diff) && msg.multi_dof_joint_state.joint_names.empty() &&
113 msg.joint_state.name.empty() && msg.attached_collision_objects.empty() && msg.joint_state.position.empty() &&
114 msg.joint_state.velocity.empty() && msg.joint_state.effort.empty() &&
115 msg.multi_dof_joint_state.transforms.empty() && msg.multi_dof_joint_state.twist.empty() &&
116 msg.multi_dof_joint_state.wrench.empty();
121 return msg.collision_objects.empty() && msg.octomap.octomap.data.empty();
125 const collision_detection::WorldPtr& world)
126 : robot_model_(robot_model), world_(world), world_const_(world)
166 const std::vector<std::string>& collision_links =
robot_model_->getLinkModelNamesWithCollisionGeometry();
167 acm_->setEntry(collision_links, collision_links,
false);
170 const std::vector<srdf::Model::DisabledCollision>& dc =
getRobotModel()->getSRDF()->getDisabledCollisionPairs();
171 for (std::vector<srdf::Model::DisabledCollision>::const_iterator it = dc.begin(); it != dc.end(); ++it)
172 acm_->setEntry(it->link1_, it->link2_,
true);
182 if (!robot_model || !robot_model->getRootJoint())
183 return robot_model::RobotModelPtr();
193 if (!
parent_->getName().empty())
209 const CollisionDetectorPtr& parent_detector = it->second;
210 CollisionDetectorPtr& detector =
collision_[it->first];
212 detector->alloc_ = parent_detector->alloc_;
213 detector->parent_ = parent_detector;
215 detector->cworld_ = detector->alloc_->allocateWorld(parent_detector->cworld_,
world_);
216 detector->cworld_const_ = detector->cworld_;
220 detector->crobot_.reset();
221 detector->crobot_const_.reset();
222 detector->crobot_unpadded_.reset();
223 detector->crobot_unpadded_const_.reset();
230 PlanningScenePtr result = scene->diff();
231 result->decoupleParent();
232 result->setName(scene->getName());
238 return PlanningScenePtr(
new PlanningScene(shared_from_this()));
243 PlanningScenePtr result =
diff();
244 result->setPlanningSceneDiffMsg(msg);
252 alloc_->allocateRobot(
parent_->getCollisionRobot());
253 crobot_const_ = crobot_;
278 if (it != scene.
parent_->collision_.end())
284 const std::string& name = allocator->getName();
285 CollisionDetectorPtr& detector =
collision_[name];
292 detector->alloc_ = allocator;
297 detector->findParent(*
this);
299 detector->cworld_ = detector->alloc_->allocateWorld(
world_);
300 detector->cworld_const_ = detector->cworld_;
306 detector->crobot_ = detector->alloc_->allocateRobot(
getRobotModel());
307 detector->crobot_const_ = detector->crobot_;
314 if (!detector->parent_)
316 detector->crobot_unpadded_ = detector->alloc_->allocateRobot(
getRobotModel());
317 detector->crobot_unpadded_const_ = detector->crobot_unpadded_;
326 CollisionDetectorPtr p;
357 "Cannot setActiveCollisionDetector to '%s' -- it has been added to PlanningScene. " 358 "Keeping existing active collision detector '%s'",
369 names.push_back(it->first);
372 const collision_detection::CollisionWorldConstPtr&
378 ROS_ERROR_NAMED(LOGNAME,
"Could not get CollisionWorld named '%s'. Returning active CollisionWorld '%s' instead",
383 return it->second->cworld_const_;
386 const collision_detection::CollisionRobotConstPtr&
392 ROS_ERROR_NAMED(LOGNAME,
"Could not get CollisionRobot named '%s'. Returning active CollisionRobot '%s' instead",
397 return it->second->getCollisionRobot();
400 const collision_detection::CollisionRobotConstPtr&
407 "Could not get CollisionRobotUnpadded named '%s'. " 408 "Returning active CollisionRobotUnpadded '%s' instead",
413 return it->second->getCollisionRobotUnpadded();
431 if (!it->second->parent_)
432 it->second->findParent(*
this);
434 if (it->second->parent_)
436 it->second->crobot_.reset();
437 it->second->crobot_const_.reset();
438 it->second->crobot_unpadded_.reset();
439 it->second->crobot_unpadded_const_.reset();
441 it->second->cworld_ = it->second->alloc_->allocateWorld(it->second->parent_->cworld_,
world_);
442 it->second->cworld_const_ = it->second->cworld_;
446 it->second->copyPadding(*
parent_->active_collision_);
448 it->second->cworld_ = it->second->alloc_->allocateWorld(
world_);
449 it->second->cworld_const_ = it->second->cworld_;
466 scene->getTransformsNonConst().setAllTransforms(
scene_transforms_->getAllTransforms());
472 std::vector<const moveit::core::AttachedBody*> attached_objs;
474 for (std::vector<const moveit::core::AttachedBody*>::const_iterator it = attached_objs.begin();
475 it != attached_objs.end(); ++it)
478 scene->setObjectType((*it)->getName(),
getObjectType((*it)->getName()));
480 scene->setObjectColor((*it)->getName(),
getObjectColor((*it)->getName()));
485 scene->getAllowedCollisionMatrixNonConst() = *
acm_;
489 collision_detection::CollisionRobotPtr active_crobot = scene->getCollisionRobotNonConst();
492 scene->propogateRobotPadding();
501 scene->world_->removeObject(it->first);
502 scene->removeObjectColor(it->first);
503 scene->removeObjectType(it->first);
508 scene->world_->removeObject(obj.
id_);
623 for (collision_detection::CollisionResult::ContactMap::const_iterator it = contacts.begin(); it != contacts.end();
625 for (std::size_t j = 0; j < it->second.size(); ++j)
628 links.push_back(it->second[j].body_name_1);
630 links.push_back(it->second[j].body_name_2);
709 scene_msg.name =
name_;
711 scene_msg.is_diff =
true;
716 scene_msg.fixed_frame_transforms.clear();
722 scene_msg.robot_state = moveit_msgs::RobotState();
723 scene_msg.robot_state.is_diff =
true;
727 acm_->getMessage(scene_msg.allowed_collision_matrix);
729 scene_msg.allowed_collision_matrix = moveit_msgs::AllowedCollisionMatrix();
738 scene_msg.link_padding.clear();
739 scene_msg.link_scale.clear();
742 scene_msg.object_colors.clear();
749 scene_msg.object_colors[i].id = it->first;
750 scene_msg.object_colors[i].color = it->second;
754 scene_msg.world.collision_objects.clear();
755 scene_msg.world.octomap = octomap_msgs::OctomapWithPose();
759 bool do_omap =
false;
766 moveit_msgs::CollisionObject co;
769 co.operation = moveit_msgs::CollisionObject::REMOVE;
770 scene_msg.world.collision_objects.push_back(co);
774 scene_msg.world.collision_objects.emplace_back();
785 class ShapeVisitorAddToCollisionObject :
public boost::static_visitor<void>
788 ShapeVisitorAddToCollisionObject(moveit_msgs::CollisionObject* obj) : boost::static_visitor<void>(),
obj_(obj)
792 void setPoseMessage(
const geometry_msgs::Pose* pose)
797 void operator()(
const shape_msgs::Plane& shape_msg)
const 799 obj_->planes.push_back(shape_msg);
803 void operator()(
const shape_msgs::Mesh& shape_msg)
const 805 obj_->meshes.push_back(shape_msg);
809 void operator()(
const shape_msgs::SolidPrimitive& shape_msg)
const 811 obj_->primitives.push_back(shape_msg);
816 moveit_msgs::CollisionObject*
obj_;
824 collision_obj.id = ns;
825 collision_obj.operation = moveit_msgs::CollisionObject::ADD;
829 ShapeVisitorAddToCollisionObject sv(&collision_obj);
830 for (std::size_t j = 0; j < obj->shapes_.size(); ++j)
835 geometry_msgs::Pose p =
tf2::toMsg(obj->shape_poses_[j]);
836 sv.setPoseMessage(&p);
837 boost::apply_visitor(sv, sm);
841 if (!collision_obj.primitives.empty() || !collision_obj.meshes.empty() || !collision_obj.planes.empty())
851 collision_objs.clear();
852 const std::vector<std::string>& ns =
world_->getObjectIds();
853 for (std::size_t i = 0; i < ns.size(); ++i)
856 collision_objs.emplace_back();
862 const std::string& ns)
const 864 std::vector<moveit_msgs::AttachedCollisionObject> attached_collision_objs;
866 for (std::size_t i = 0; i < attached_collision_objs.size(); ++i)
868 if (attached_collision_objs[i].
object.
id == ns)
870 attached_collision_obj = attached_collision_objs[i];
878 std::vector<moveit_msgs::AttachedCollisionObject>& attached_collision_objs)
const 880 std::vector<const moveit::core::AttachedBody*> attached_bodies;
888 octomap.octomap = octomap_msgs::Octomap();
893 if (map->shapes_.size() == 1)
897 octomap.origin =
tf2::toMsg(map->shape_poses_[0]);
900 ROS_ERROR_NAMED(LOGNAME,
"Unexpected number of shapes in octomap collision object. Not including '%s' object",
908 object_colors.clear();
913 object_colors.resize(cmap.size());
914 for (ObjectColorMap::const_iterator it = cmap.begin(); it != cmap.end(); ++it, ++i)
916 object_colors[i].id = it->first;
917 object_colors[i].color = it->second;
923 scene_msg.name =
name_;
924 scene_msg.is_diff =
false;
943 const moveit_msgs::PlanningSceneComponents& comp)
const 945 scene_msg.is_diff =
false;
946 if (comp.components & moveit_msgs::PlanningSceneComponents::SCENE_SETTINGS)
948 scene_msg.name =
name_;
952 if (comp.components & moveit_msgs::PlanningSceneComponents::TRANSFORMS)
955 if (comp.components & moveit_msgs::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS)
958 for (std::vector<moveit_msgs::AttachedCollisionObject>::iterator it =
959 scene_msg.robot_state.attached_collision_objects.begin();
960 it != scene_msg.robot_state.attached_collision_objects.end(); ++it)
968 else if (comp.components & moveit_msgs::PlanningSceneComponents::ROBOT_STATE)
973 if (comp.components & moveit_msgs::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX)
976 if (comp.components & moveit_msgs::PlanningSceneComponents::LINK_PADDING_AND_SCALING)
982 if (comp.components & moveit_msgs::PlanningSceneComponents::OBJECT_COLORS)
986 if (comp.components & moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY)
988 else if (comp.components & moveit_msgs::PlanningSceneComponents::WORLD_OBJECT_NAMES)
990 const std::vector<std::string>& ns =
world_->getObjectIds();
991 scene_msg.world.collision_objects.clear();
992 scene_msg.world.collision_objects.reserve(ns.size());
993 for (std::size_t i = 0; i < ns.size(); ++i)
996 moveit_msgs::CollisionObject co;
1000 scene_msg.world.collision_objects.push_back(co);
1005 if (comp.components & moveit_msgs::PlanningSceneComponents::OCTOMAP)
1011 out <<
name_ << std::endl;
1012 const std::vector<std::string>& ns =
world_->getObjectIds();
1013 for (std::size_t i = 0; i < ns.size(); ++i)
1019 out <<
"* " << ns[i] << std::endl;
1020 out << obj->shapes_.size() << std::endl;
1021 for (std::size_t j = 0; j < obj->shapes_.size(); ++j)
1024 out << obj->shape_poses_[j].translation().x() <<
" " << obj->shape_poses_[j].translation().y() <<
" " 1025 << obj->shape_poses_[j].translation().z() << std::endl;
1026 Eigen::Quaterniond
r(obj->shape_poses_[j].rotation());
1027 out <<
r.x() <<
" " <<
r.y() <<
" " <<
r.z() <<
" " <<
r.w() << std::endl;
1031 out << c.r <<
" " << c.g <<
" " << c.b <<
" " << c.a << std::endl;
1034 out <<
"0 0 0 0" << std::endl;
1038 out <<
"." << std::endl;
1048 if (!in.good() || in.eof())
1050 ROS_ERROR_NAMED(LOGNAME,
"Bad input stream when loading scene geometry");
1053 std::getline(in,
name_);
1058 if (!in.good() || in.eof())
1060 ROS_ERROR_NAMED(LOGNAME,
"Bad input stream when loading marker in scene geometry");
1066 std::getline(in, ns);
1067 if (!in.good() || in.eof())
1069 ROS_ERROR_NAMED(LOGNAME,
"Bad input stream when loading ns in scene geometry");
1072 boost::algorithm::trim(ns);
1073 unsigned int shape_count;
1075 for (std::size_t i = 0; i < shape_count && in.good() && !in.eof(); ++i)
1083 double x,
y,
z, rx, ry, rz, rw;
1084 if (!(in >> x >> y >> z))
1086 ROS_ERROR_NAMED(LOGNAME,
"Improperly formatted translation in scene geometry file");
1089 if (!(in >> rx >> ry >> rz >> rw))
1091 ROS_ERROR_NAMED(LOGNAME,
"Improperly formatted rotation in scene geometry file");
1095 if (!(in >> r >> g >> b >> a))
1097 ROS_ERROR_NAMED(LOGNAME,
"Improperly formatted color in scene geometry file");
1102 Eigen::Isometry3d pose = Eigen::Translation3d(x, y, z) * Eigen::Quaterniond(rw, rx, ry, rz);
1104 pose = offset * pose;
1105 world_->addToObject(ns, shape, pose);
1106 if (r > 0.0
f || g > 0.0
f || b > 0.0
f || a > 0.0
f)
1108 std_msgs::ColorRGBA color;
1118 else if (marker ==
".")
1135 moveit_msgs::RobotState state_no_attached(state);
1136 state_no_attached.attached_collision_objects.clear();
1150 for (std::size_t i = 0; i < state.attached_collision_objects.size(); ++i)
1152 if (!state.is_diff && state.attached_collision_objects[i].object.operation != moveit_msgs::CollisionObject::ADD)
1155 "The specified RobotState is not marked as is_diff. " 1156 "The request to modify the object '%s' is not supported. Object is ignored.",
1157 state.attached_collision_objects[i].object.id.c_str());
1192 if (!it->second->crobot_)
1194 it->second->crobot_ = it->second->alloc_->allocateRobot(it->second->parent_->getCollisionRobot());
1195 it->second->crobot_const_ = it->second->crobot_;
1197 if (!it->second->crobot_unpadded_)
1199 it->second->crobot_unpadded_ =
1200 it->second->alloc_->allocateRobot(it->second->parent_->getCollisionRobotUnpadded());
1201 it->second->crobot_unpadded_const_ = it->second->crobot_unpadded_;
1203 it->second->parent_.reset();
1210 parent_->getKnownObjectColors(kc);
1216 parent_->getKnownObjectColors(kc);
1217 for (ObjectColorMap::const_iterator it = kc.begin(); it != kc.end(); ++it)
1225 parent_->getKnownObjectTypes(kc);
1231 parent_->getKnownObjectTypes(kc);
1232 for (ObjectTypeMap::const_iterator it = kc.begin(); it != kc.end(); ++it)
1245 if (!scene_msg.name.empty())
1246 name_ = scene_msg.name;
1248 if (!scene_msg.robot_model_name.empty() && scene_msg.robot_model_name !=
getRobotModel()->getName())
1249 ROS_WARN_NAMED(LOGNAME,
"Setting the scene for model '%s' but model '%s' is loaded.",
1250 scene_msg.robot_model_name.c_str(),
getRobotModel()->getName().c_str());
1254 if (!scene_msg.fixed_frame_transforms.empty())
1262 if (!scene_msg.robot_state.multi_dof_joint_state.joint_names.empty() ||
1263 !scene_msg.robot_state.joint_state.name.empty() || !scene_msg.robot_state.attached_collision_objects.empty())
1267 if (!scene_msg.allowed_collision_matrix.entry_names.empty())
1270 if (!scene_msg.link_padding.empty() || !scene_msg.link_scale.empty())
1274 if (!it->second->crobot_)
1276 it->second->crobot_ = it->second->alloc_->allocateRobot(it->second->parent_->getCollisionRobot());
1277 it->second->crobot_const_ = it->second->crobot_;
1279 it->second->crobot_->setPadding(scene_msg.link_padding);
1280 it->second->crobot_->setScale(scene_msg.link_scale);
1285 for (std::size_t i = 0; i < scene_msg.object_colors.size(); ++i)
1286 setObjectColor(scene_msg.object_colors[i].id, scene_msg.object_colors[i].color);
1289 for (std::size_t i = 0; i < scene_msg.world.collision_objects.size(); ++i)
1293 if (!scene_msg.world.octomap.octomap.data.empty())
1301 ROS_DEBUG_NAMED(LOGNAME,
"Setting new planning scene: '%s'", scene_msg.name.c_str());
1302 name_ = scene_msg.name;
1304 if (!scene_msg.robot_model_name.empty() && scene_msg.robot_model_name !=
getRobotModel()->getName())
1305 ROS_WARN_NAMED(LOGNAME,
"Setting the scene for model '%s' but model '%s' is loaded.",
1306 scene_msg.robot_model_name.c_str(),
getRobotModel()->getName().c_str());
1317 if (!it->second->crobot_)
1319 it->second->crobot_ = it->second->alloc_->allocateRobot(it->second->parent_->getCollisionRobot());
1320 it->second->crobot_const_ = it->second->crobot_;
1322 it->second->crobot_->setPadding(scene_msg.link_padding);
1323 it->second->crobot_->setScale(scene_msg.link_scale);
1326 for (std::size_t i = 0; i < scene_msg.object_colors.size(); ++i)
1327 setObjectColor(scene_msg.object_colors[i].id, scene_msg.object_colors[i].color);
1335 for (std::size_t i = 0; i < world.collision_objects.size(); ++i)
1343 if (scene_msg.is_diff)
1354 if (map.data.empty())
1357 if (map.id !=
"OcTree")
1359 ROS_ERROR_NAMED(LOGNAME,
"Received octomap is of type '%s' but type 'OcTree' is expected.", map.id.c_str());
1364 if (!map.header.frame_id.empty())
1377 const std::vector<std::string>& object_ids =
world_->getObjectIds();
1378 for (std::size_t i = 0; i < object_ids.size(); ++i)
1381 world_->removeObject(object_ids[i]);
1392 if (map.octomap.data.empty())
1395 if (map.octomap.id !=
"OcTree")
1397 ROS_ERROR_NAMED(LOGNAME,
"Received octomap is of type '%s' but type 'OcTree' is expected.", map.octomap.id.c_str());
1403 Eigen::Isometry3d p;
1414 if (map->shapes_.size() == 1)
1421 if (map->shape_poses_[0].isApprox(t, std::numeric_limits<double>::epsilon() * 100.0))
1444 if (
object.
object.operation == moveit_msgs::CollisionObject::ADD && !
getRobotModel()->hasLinkModel(
object.link_name))
1446 ROS_ERROR_NAMED(LOGNAME,
"Unable to attach a body to link '%s' (link not found)",
object.link_name.c_str());
1463 if (
object.
object.operation == moveit_msgs::CollisionObject::ADD ||
1464 object.
object.operation == moveit_msgs::CollisionObject::APPEND)
1466 if (
object.
object.primitives.size() !=
object.object.primitive_poses.size())
1468 ROS_ERROR_NAMED(LOGNAME,
"Number of primitive shapes does not match number of poses " 1469 "in attached collision object message");
1473 if (
object.
object.meshes.size() !=
object.object.mesh_poses.size())
1475 ROS_ERROR_NAMED(LOGNAME,
"Number of meshes does not match number of poses " 1476 "in attached collision object message");
1480 if (
object.
object.planes.size() !=
object.object.plane_poses.size())
1482 ROS_ERROR_NAMED(LOGNAME,
"Number of planes does not match number of poses " 1483 "in attached collision object message");
1490 std::vector<shapes::ShapeConstPtr>
shapes;
1494 if (
object.
object.operation == moveit_msgs::CollisionObject::ADD &&
object.
object.primitives.empty() &&
1495 object.object.meshes.empty() &&
object.object.planes.empty())
1500 ROS_DEBUG_NAMED(LOGNAME,
"Attaching world object '%s' to link '%s'",
object.
object.
id.c_str(),
1501 object.link_name.c_str());
1504 shapes = obj->shapes_;
1505 poses = obj->shape_poses_;
1507 world_->removeObject(
object.
object.
id);
1510 const Eigen::Isometry3d& i_t =
robot_state_->getGlobalLinkTransform(lm).inverse();
1511 for (std::size_t i = 0; i < poses.size(); ++i)
1512 poses[i] = i_t * poses[i];
1517 "Attempting to attach object '%s' to link '%s' but no geometry specified " 1518 "and such an object does not exist in the collision world",
1519 object.
object.
id.c_str(),
object.link_name.c_str());
1526 if (
world_->removeObject(
object.object.id))
1528 if (
object.
object.operation == moveit_msgs::CollisionObject::ADD)
1529 ROS_DEBUG_NAMED(LOGNAME,
"Removing world object with the same name as newly attached object: '%s'",
1530 object.
object.
id.c_str());
1533 "You tried to append geometry to an attached object that is actually a world object ('%s'). " 1534 "World geometry is ignored.",
1535 object.
object.
id.c_str());
1538 for (std::size_t i = 0; i <
object.object.primitives.size(); ++i)
1543 Eigen::Isometry3d p;
1549 for (std::size_t i = 0; i <
object.object.meshes.size(); ++i)
1554 Eigen::Isometry3d p;
1560 for (std::size_t i = 0; i <
object.object.planes.size(); ++i)
1565 Eigen::Isometry3d p;
1573 if (
object.
object.
header.frame_id !=
object.link_name)
1575 const Eigen::Isometry3d&
t =
1577 for (std::size_t i = 0; i < poses.size(); ++i)
1578 poses[i] = t * poses[i];
1584 ROS_ERROR_NAMED(LOGNAME,
"There is no geometry to attach to link '%s' as part of attached body '%s'",
1585 object.link_name.c_str(),
object.object.id.c_str());
1589 if (!
object.
object.type.db.empty() || !
object.object.type.key.empty())
1592 if (
object.
object.operation == moveit_msgs::CollisionObject::ADD ||
1598 "The robot state already had an object named '%s' attached to link '%s'. " 1599 "The object was replaced.",
1600 object.
object.
id.c_str(),
object.link_name.c_str());
1601 robot_state_->attachBody(
object.
object.
id, shapes, poses,
object.touch_links,
object.link_name,
1602 object.detach_posture);
1603 ROS_DEBUG_NAMED(LOGNAME,
"Attached object '%s' to link '%s'",
object.
object.
id.c_str(),
1604 object.link_name.c_str());
1611 trajectory_msgs::JointTrajectory detach_posture =
1612 object.detach_posture.joint_names.empty() ? ab->
getDetachPosture() :
object.detach_posture;
1613 const std::set<std::string>& ab_touch_links = ab->
getTouchLinks();
1615 if (
object.touch_links.empty())
1616 robot_state_->attachBody(
object.object.id, shapes, poses, ab_touch_links,
object.link_name, detach_posture);
1618 robot_state_->attachBody(
object.
object.
id, shapes, poses,
object.touch_links,
object.link_name,
1620 ROS_DEBUG_NAMED(LOGNAME,
"Added shapes to object '%s' attached to link '%s'",
object.
object.
id.c_str(),
1621 object.link_name.c_str());
1627 ROS_ERROR_NAMED(LOGNAME,
"Robot state is not compatible with robot model. This could be fatal.");
1629 else if (
object.
object.operation == moveit_msgs::CollisionObject::REMOVE)
1631 std::vector<const robot_state::AttachedBody*> attached_bodies;
1632 if (
object.link_name.empty())
1634 if (
object.
object.
id.empty())
1640 attached_bodies.push_back(ab);
1648 if (
object.
object.
id.empty())
1657 attached_bodies.push_back(ab);
1662 for (std::size_t i = 0; i < attached_bodies.size(); ++i)
1664 std::vector<shapes::ShapeConstPtr>
shapes = attached_bodies[i]->getShapes();
1666 std::string name = attached_bodies[i]->getName();
1668 if (
world_->hasObject(name))
1670 "The collision world already has an object with the same name as the body about to be detached. " 1671 "NOT adding the detached body '%s' to the collision world.",
1672 object.
object.
id.c_str());
1675 world_->addToObject(name, shapes, poses);
1676 ROS_DEBUG_NAMED(LOGNAME,
"Detached object '%s' from link '%s' and added it back in the collision world",
1677 name.c_str(),
object.link_name.c_str());
1682 if (!attached_bodies.empty() ||
object.object.id.empty())
1685 else if (
object.
object.operation == moveit_msgs::CollisionObject::MOVE)
1687 ROS_ERROR_NAMED(LOGNAME,
"Move for attached objects not yet implemented");
1691 ROS_ERROR_NAMED(LOGNAME,
"Unknown collision object operation: %d",
object.
object.operation);
1705 if (
object.operation == moveit_msgs::CollisionObject::ADD ||
object.operation == moveit_msgs::CollisionObject::APPEND)
1709 else if (
object.operation == moveit_msgs::CollisionObject::REMOVE)
1713 else if (
object.operation == moveit_msgs::CollisionObject::MOVE)
1718 ROS_ERROR_NAMED(LOGNAME,
"Unknown collision object operation: %d",
object.operation);
1724 Eigen::Translation3d translation(msg.position.x, msg.position.y, msg.position.z);
1725 Eigen::Quaterniond quaternion(msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z);
1726 if ((quaternion.x() == 0) && (quaternion.y() == 0) && (quaternion.z() == 0) && (quaternion.w() == 0))
1728 ROS_WARN_NAMED(LOGNAME,
"Empty quaternion found in pose message. Setting to neutral orientation.");
1729 quaternion.setIdentity();
1733 quaternion.normalize();
1735 out = translation * quaternion;
1740 if (
object.primitives.empty() &&
object.meshes.empty() &&
object.planes.empty())
1742 ROS_ERROR_NAMED(LOGNAME,
"There are no shapes specified in the collision object message");
1746 if (
object.primitives.size() !=
object.primitive_poses.size())
1748 ROS_ERROR_NAMED(LOGNAME,
"Number of primitive shapes does not match number of poses " 1749 "in collision object message");
1753 if (
object.meshes.size() !=
object.mesh_poses.size())
1755 ROS_ERROR_NAMED(LOGNAME,
"Number of meshes does not match number of poses in collision object message");
1759 if (
object.planes.size() !=
object.plane_poses.size())
1761 ROS_ERROR_NAMED(LOGNAME,
"Number of planes does not match number of poses in collision object message");
1772 if (
object.operation == moveit_msgs::CollisionObject::ADD &&
world_->hasObject(
object.id))
1773 world_->removeObject(
object.id);
1777 for (std::size_t i = 0; i <
object.primitives.size(); ++i)
1782 Eigen::Isometry3d object_pose;
1787 for (std::size_t i = 0; i <
object.meshes.size(); ++i)
1792 Eigen::Isometry3d object_pose;
1797 for (std::size_t i = 0; i <
object.planes.size(); ++i)
1802 Eigen::Isometry3d object_pose;
1807 if (!
object.type.key.empty() || !
object.type.db.empty())
1814 if (
object.
id.empty())
1820 world_->removeObject(
object.
id);
1829 if (
world_->hasObject(
object.id))
1831 if (!
object.primitives.empty() || !
object.meshes.empty() || !
object.planes.empty())
1832 ROS_WARN_NAMED(LOGNAME,
"Move operation for object '%s' ignores the geometry specified in the message.",
1837 for (
const geometry_msgs::Pose& primitive_pose :
object.primitive_poses)
1839 Eigen::Isometry3d object_pose;
1841 new_poses.push_back(t * object_pose);
1843 for (
const geometry_msgs::Pose& mesh_pose :
object.mesh_poses)
1845 Eigen::Isometry3d object_pose;
1847 new_poses.push_back(t * object_pose);
1849 for (
const geometry_msgs::Pose& plane_pose :
object.plane_poses)
1851 Eigen::Isometry3d object_pose;
1853 new_poses.push_back(t * object_pose);
1856 collision_detection::World::ObjectConstPtr obj =
world_->getObject(
object.
id);
1857 if (obj->shapes_.size() == new_poses.size())
1859 std::vector<shapes::ShapeConstPtr>
shapes = obj->shapes_;
1861 world_->removeObject(
object.
id);
1862 world_->addToObject(
object.
id, shapes, new_poses);
1867 "Number of supplied poses (%zu) for object '%s' does not match number of shapes (%zu). " 1869 new_poses.size(),
object.id.c_str(), obj->shapes_.size());
1875 ROS_ERROR_NAMED(LOGNAME,
"World object '%s' does not exist. Cannot move.",
object.
id.c_str());
1893 const std::string& frame_id)
const 1895 if (!frame_id.empty() && frame_id[0] ==
'/')
1902 if (
getWorld()->hasObject(frame_id))
1904 collision_detection::World::ObjectConstPtr obj =
getWorld()->getObject(frame_id);
1905 if (obj->shape_poses_.size() > 1)
1907 ROS_WARN_NAMED(LOGNAME,
"More than one shapes in object '%s'. Using first one to decide transform",
1909 return obj->shape_poses_[0];
1911 else if (obj->shape_poses_.size() == 1)
1912 return obj->shape_poses_[0];
1924 if (!frame_id.empty() && frame_id[0] ==
'/')
1929 collision_detection::World::ObjectConstPtr obj =
getWorld()->getObject(frame_id);
1932 return obj->shape_poses_.size() == 1;
1943 return parent_->hasObjectType(object_id);
1951 ObjectTypeMap::const_iterator it =
object_types_->find(object_id);
1956 return parent_->getObjectType(object_id);
1957 static const object_recognition_msgs::ObjectType EMPTY;
1965 (*object_types_)[object_id] = type;
1978 parent_->getKnownObjectTypes(kc);
1981 kc[it->first] = it->second;
1990 return parent_->hasObjectColor(object_id);
1998 ObjectColorMap::const_iterator it =
object_colors_->find(object_id);
2003 return parent_->getObjectColor(object_id);
2004 static const std_msgs::ColorRGBA EMPTY;
2012 parent_->getKnownObjectColors(kc);
2015 kc[it->first] = it->second;
2020 if (object_id.empty())
2022 ROS_ERROR_NAMED(LOGNAME,
"Cannot set color of object with empty object_id.");
2027 (*object_colors_)[object_id] = color;
2090 kinematic_constraints::KinematicConstraintSetPtr ks(
2115 static const moveit_msgs::Constraints EMP_CONSTRAINTS;
2116 return isStateValid(state, EMP_CONSTRAINTS, group, verbose);
2121 static const moveit_msgs::Constraints EMP_CONSTRAINTS;
2122 return isStateValid(state, EMP_CONSTRAINTS, group, verbose);
2126 const std::string& group,
bool verbose)
const 2134 const std::string& group,
bool verbose)
const 2155 const moveit_msgs::RobotTrajectory& trajectory,
const std::string& group,
bool verbose,
2156 std::vector<std::size_t>* invalid_index)
const 2158 static const moveit_msgs::Constraints EMP_CONSTRAINTS;
2159 static const std::vector<moveit_msgs::Constraints> EMP_CONSTRAINTS_VECTOR;
2160 return isPathValid(start_state, trajectory, EMP_CONSTRAINTS, EMP_CONSTRAINTS_VECTOR, group, verbose, invalid_index);
2164 const moveit_msgs::RobotTrajectory& trajectory,
2165 const moveit_msgs::Constraints& path_constraints,
const std::string& group,
2166 bool verbose, std::vector<std::size_t>* invalid_index)
const 2168 static const std::vector<moveit_msgs::Constraints> EMP_CONSTRAINTS_VECTOR;
2169 return isPathValid(start_state, trajectory, path_constraints, EMP_CONSTRAINTS_VECTOR, group, verbose, invalid_index);
2173 const moveit_msgs::RobotTrajectory& trajectory,
2174 const moveit_msgs::Constraints& path_constraints,
2175 const moveit_msgs::Constraints& goal_constraints,
const std::string& group,
2176 bool verbose, std::vector<std::size_t>* invalid_index)
const 2178 std::vector<moveit_msgs::Constraints> goal_constraints_vector(1, goal_constraints);
2179 return isPathValid(start_state, trajectory, path_constraints, goal_constraints_vector, group, verbose, invalid_index);
2183 const moveit_msgs::RobotTrajectory& trajectory,
2184 const moveit_msgs::Constraints& path_constraints,
2185 const std::vector<moveit_msgs::Constraints>& goal_constraints,
const std::string& group,
2186 bool verbose, std::vector<std::size_t>* invalid_index)
const 2192 return isPathValid(t, path_constraints, goal_constraints, group, verbose, invalid_index);
2196 const moveit_msgs::Constraints& path_constraints,
2197 const std::vector<moveit_msgs::Constraints>& goal_constraints,
const std::string& group,
2198 bool verbose, std::vector<std::size_t>* invalid_index)
const 2202 invalid_index->clear();
2206 for (std::size_t i = 0; i < n_wp; ++i)
2210 bool this_state_valid =
true;
2212 this_state_valid =
false;
2214 this_state_valid =
false;
2216 this_state_valid =
false;
2218 if (!this_state_valid)
2221 invalid_index->push_back(i);
2228 if (i + 1 == n_wp && !goal_constraints.empty())
2231 for (std::size_t k = 0; k < goal_constraints.size(); ++k)
2244 invalid_index->push_back(i);
2253 const moveit_msgs::Constraints& path_constraints,
2254 const moveit_msgs::Constraints& goal_constraints,
const std::string& group,
2255 bool verbose, std::vector<std::size_t>* invalid_index)
const 2257 std::vector<moveit_msgs::Constraints> goal_constraints_vector(1, goal_constraints);
2258 return isPathValid(trajectory, path_constraints, goal_constraints_vector, group, verbose, invalid_index);
2262 const moveit_msgs::Constraints& path_constraints,
const std::string& group,
2263 bool verbose, std::vector<std::size_t>* invalid_index)
const 2265 static const std::vector<moveit_msgs::Constraints> EMP_CONSTRAINTS_VECTOR;
2266 return isPathValid(trajectory, path_constraints, EMP_CONSTRAINTS_VECTOR, group, verbose, invalid_index);
2270 bool verbose, std::vector<std::size_t>* invalid_index)
const 2272 static const moveit_msgs::Constraints EMP_CONSTRAINTS;
2273 static const std::vector<moveit_msgs::Constraints> EMP_CONSTRAINTS_VECTOR;
2274 return isPathValid(trajectory, EMP_CONSTRAINTS, EMP_CONSTRAINTS_VECTOR, group, verbose, invalid_index);
2278 std::set<collision_detection::CostSource>& costs,
double overlap_fraction)
const 2280 getCostSources(trajectory, max_costs, std::string(), costs, overlap_fraction);
2284 const std::string& group_name, std::set<collision_detection::CostSource>& costs,
2285 double overlap_fraction)
const 2291 std::set<collision_detection::CostSource> cs;
2292 std::set<collision_detection::CostSource> cs_start;
2294 for (std::size_t i = 0; i < n_wp; ++i)
2303 if (cs.size() <= max_costs)
2309 for (std::set<collision_detection::CostSource>::iterator it = cs.begin(); i < max_costs; ++it, ++i)
2318 std::set<collision_detection::CostSource>& costs)
const 2324 const std::string& group_name,
2325 std::set<collision_detection::CostSource>& costs)
const 2338 const std::vector<std::string>& objects =
getWorld()->getObjectIds();
2339 std::vector<const robot_state::AttachedBody*> attached_bodies;
2343 out <<
"-----------------------------------------\n";
2344 out <<
"PlanningScene Known Objects:\n";
2345 out <<
" - Collision World Objects:\n ";
2346 for (std::size_t i = 0; i < objects.size(); ++i)
2348 out <<
"\t- " << objects[i] <<
"\n";
2351 out <<
" - Attached Bodies:\n";
2352 for (std::size_t i = 0; i < attached_bodies.size(); ++i)
2354 out <<
"\t- " << attached_bodies[i]->getName() <<
"\n";
2356 out <<
"-----------------------------------------\n";
Maintain a diff list of changes that have happened to a World.
const collision_detection::AllowedCollisionMatrix & getAllowedCollisionMatrix() const
Get the allowed collision matrix.
std::map< std::string, World::Action >::const_iterator const_iterator
This may be thrown during construction of objects if errors occur.
bool hasObjectColor(const std::string &id) const
Representation of a collision checking request.
bool knowsFrameTransform(const std::string &frame_id) const
Check if a transformation matrix from the model frame to frame frame_id is known. ...
Core components of MoveIt!
bool isEmpty(const moveit_msgs::Constraints &constr)
Check if any constraints were specified.
robot_state::RobotState & getCurrentStateNonConst()
Get the state at which the robot is assumed to be.
bool loadGeometryFromStream(std::istream &in)
Load the geometry of the planning scene from a stream.
#define ROS_INFO_NAMED(name,...)
EigenSTL::vector_Isometry3d shape_poses_
The poses of the corresponding entries in shapes_.
bool isPathValid(const moveit_msgs::RobotState &start_state, const moveit_msgs::RobotTrajectory &trajectory, const std::string &group="", bool verbose=false, std::vector< std::size_t > *invalid_index=NULL) const
Check if a given path is valid. Each state is checked for validity (collision avoidance and feasibili...
void setObjectType(const std::string &id, const object_recognition_msgs::ObjectType &type)
void decoupleParent()
Make sure that all the data maintained in this scene is local. All unmodified data is copied from the...
void processOctomapMsg(const octomap_msgs::OctomapWithPose &map)
const std::set< std::string > & getTouchLinks() const
Get the links that the attached body is allowed to touch.
#define ROS_ERROR_STREAM_NAMED(name, args)
static CollisionDetectorAllocatorPtr create()
static bool isEmpty(const moveit_msgs::PlanningScene &msg)
Check if a message includes any information about a planning scene, or it is just a default...
#define ROS_WARN_NAMED(name,...)
void removeObjectColor(const std::string &id)
bool isStateFeasible(const moveit_msgs::RobotState &state, bool verbose=false) const
Check if a given state is feasible, in accordance to the feasibility predicate specified by setStateF...
StateFeasibilityFn state_feasibility_
bool getCollisionObjectMsg(moveit_msgs::CollisionObject &collision_obj, const std::string &ns) const
Construct a message (collision_object) with the collision object data from the planning_scene for the...
std::vector< shapes::ShapeConstPtr > shapes_
All the shapes making up this object.
void checkSelfCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res)
Check whether the current state is in self collision.
const robot_state::Transforms & getTransforms() const
Get the set of fixed transforms from known frames to the planning frame.
static const std::string DEFAULT_SCENE_NAME
bool knowsFrameTransform(const std::string &id) const
Check if a transform to the frame id is known. This will be known if id is a link name...
void getPlanningSceneDiffMsg(moveit_msgs::PlanningScene &scene) const
Fill the message scene with the differences between this instance of PlanningScene with respect to th...
bool hasObjectType(const std::string &id) const
bool processCollisionObjectAdd(const moveit_msgs::CollisionObject &object)
const collision_detection::CollisionRobotConstPtr & getCollisionRobot() const
void getKnownObjectColors(ObjectColorMap &kc) const
Maintain a representation of the environment.
static bool fullMapToMsg(const OctomapT &octomap, Octomap &msg)
ContactMap contacts
A map returning the pairs of body ids in contact, plus their contact details.
void saveGeometryToStream(std::ostream &out) const
Save the geometry of the planning scene to a stream, as plain text.
static octomap::AbstractOcTree * msgToMap(const Octomap &msg)
bool processAttachedCollisionObjectMsg(const moveit_msgs::AttachedCollisionObject &object)
A class that contains many different constraints, and can check RobotState *versus the full set...
bool processPlanningSceneWorldMsg(const moveit_msgs::PlanningSceneWorld &world)
void printKnownObjects(std::ostream &out=std::cout) const
Outputs debug information about the planning scene contents.
Representation of a collision checking result.
void removeObjectType(const std::string &id)
const collision_detection::CollisionRobotPtr & getCollisionRobotNonConst()
Get the representation of the collision robot This can be used to set padding and link scale on the a...
std::map< std::string, CollisionDetectorPtr >::const_iterator CollisionDetectorConstIterator
const object_recognition_msgs::ObjectType & getObjectType(const std::string &id) const
void setObjectColor(const std::string &id, const std_msgs::ColorRGBA &color)
void getCostSources(const robot_trajectory::RobotTrajectory &trajectory, std::size_t max_costs, std::set< collision_detection::CostSource > &costs, double overlap_fraction=0.9) const
Get the top max_costs cost sources for a specified trajectory. The resulting costs are stored in cost...
void getAttachedCollisionObjectMsgs(std::vector< moveit_msgs::AttachedCollisionObject > &attached_collision_objs) const
Construct a vector of messages (attached_collision_objects) with the attached collision object data f...
static const std::string OCTOMAP_NS
void getObjectColorMsgs(std::vector< moveit_msgs::ObjectColor > &object_colors) const
Construct a vector of messages (object_colors) with the colors of the objects from the planning_scene...
bool processCollisionObjectMsg(const moveit_msgs::CollisionObject &object)
geometry_msgs::TransformStamped t
void copyPadding(const CollisionDetector &src)
std::size_t getWayPointCount() const
void getKnownObjectTypes(ObjectTypeMap &kc) const
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
collision_detection::AllowedCollisionMatrixPtr acm_
static robot_model::RobotModelPtr createRobotModel(const urdf::ModelInterfaceSharedPtr &urdf_model, const srdf::ModelConstSharedPtr &srdf_model)
PlanningScene(const robot_model::RobotModelConstPtr &robot_model, const collision_detection::WorldPtr &world=collision_detection::WorldPtr(new collision_detection::World()))
construct using an existing RobotModel
const robot_model::RobotModelConstPtr & getRobotModel() const
Get the kinematic model for which the planning scene is maintained.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW std::string id_
The id for this object.
void setCollisionObjectUpdateCallback(const collision_detection::World::ObserverCallbackFn &callback)
Set the callback to be triggered when changes are made to the current scene world.
bool collision
True if collision was found, false otherwise.
robot_model::RobotModelConstPtr robot_model_
collision_detection::WorldConstPtr world_const_
static PlanningScenePtr clone(const PlanningSceneConstPtr &scene)
Clone a planning scene. Even if the scene scene depends on a parent, the cloned scene will not...
std::unique_ptr< ObjectColorMap > object_colors_
std::map< std::pair< std::string, std::string >, std::vector< Contact > > ContactMap
std::set< CostSource > cost_sources
These are the individual cost sources when costs are computed.
bool add(const moveit_msgs::Constraints &c, const robot_state::Transforms &tf)
Add all known constraints.
void processOctomapPtr(const std::shared_ptr< const octomap::OcTree > &octree, const Eigen::Isometry3d &t)
boost::function< void(const ObjectConstPtr &, Action)> ObserverCallbackFn
std::map< std::string, object_recognition_msgs::ObjectType > ObjectTypeMap
A map from object names (e.g., attached bodies, collision objects) to their types.
World::ObjectConstPtr ObjectConstPtr
bool satisfied
Whether or not the constraint or constraints were satisfied.
#define ROS_DEBUG_NAMED(name,...)
std::shared_ptr< const octomap::OcTree > octree
void getAttachedBodies(std::vector< const AttachedBody *> &attached_bodies) const
Get all bodies attached to the model corresponding to this state.
std::unique_ptr< ObjectTypeMap > object_types_
robot_state::Transforms & getTransformsNonConst()
Get the set of fixed transforms from known frames to the planning frame.
robot_state::RobotStatePtr robot_state_
std::size_t max_contacts
Overall maximum number of contacts to compute.
void getCollisionObjectMsgs(std::vector< moveit_msgs::CollisionObject > &collision_objs) const
Construct a vector of messages (collision_objects) with the collision object data for all objects in ...
A representation of an object.
bool verbose
Flag indicating whether information about detected collisions should be reported. ...
std::shared_ptr< const Model > ModelConstSharedPtr
void update(bool force=false)
Update all transforms.
robot_state::AttachedBodyCallback current_state_attached_body_callback_
boost::function< void(AttachedBody *body, bool attached)> AttachedBodyCallback
This class maintains the representation of the environment as seen by a planning instance. The environment geometry, the robot geometry and state are maintained.
void findParent(const PlanningScene &scene)
void getCollidingLinks(std::vector< std::string > &links)
Get the names of the links that are involved in collisions for the current state. ...
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
void propogateRobotPadding()
Copy scale and padding from active CollisionRobot to other CollisionRobots. This should be called aft...
std::map< std::string, std_msgs::ColorRGBA > ObjectColorMap
A map from object names (e.g., attached bodies, collision objects) to their colors.
void checkCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res)
Check whether the current state is in collision, and if needed, updates the collision transforms of t...
void fromMsg(const A &, B &b)
void getMessage(moveit_msgs::AllowedCollisionMatrix &msg) const
Get the allowed collision matrix as a message.
void setAttachedBodyUpdateCallback(const robot_state::AttachedBodyCallback &callback)
Set the callback to be triggered when changes are made to the current scene state.
std::string group_name
The group name to check collisions for (optional; if empty, assume the complete robot) ...
Maintain a sequence of waypoints and the time durations between these waypoints.
robot_state::RobotStatePtr getCurrentStateUpdated(const moveit_msgs::RobotState &update) const
Get a copy of the current state with components overwritten by the state message update.
void getCollisionDetectorNames(std::vector< std::string > &names) const
get the types of collision detector that have already been added. These are the types which can be pa...
void getPlanningSceneMsg(moveit_msgs::PlanningScene &scene) const
Construct a message (scene) with all the necessary data so that the scene can be later reconstructed ...
void getCollidingPairs(collision_detection::CollisionResult::ContactMap &contacts)
Get the names of the links that are involved in collisions for the current state. Update the link tra...
bool robotStateMsgToRobotState(const Transforms &tf, const moveit_msgs::RobotState &robot_state, RobotState &state, bool copy_attached_bodies=true)
Convert a robot state msg (with accompanying extra transforms) to a MoveIt! robot state...
void saveAsText(const Shape *shape, std::ostream &out)
void setActiveCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr &allocator, bool exclusive=false)
Set the type of collision detector to use. Calls addCollisionDetector() to add it if it has not alrea...
void removeOverlapping(std::set< CostSource > &cost_sources, double overlap_fraction)
bool contacts
If true, compute contacts. Otherwise only a binary collision yes/no is reported.
const std::string & getPlanningFrame() const
Get the frame in which planning is performed.
const collision_detection::CollisionRobotConstPtr & getCollisionRobotUnpadded() const
Get the active collision detector for the robot.
bool setPlanningSceneMsg(const moveit_msgs::PlanningScene &scene)
Set this instance of a planning scene to be the same as the one serialized in the scene message...
Shape * constructShapeFromMsg(const shape_msgs::SolidPrimitive &shape_msg)
const Eigen::Isometry3d & getFrameTransform(const std::string &frame_id)
Get the transformation matrix from the model frame to the frame identified by frame_id.
ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const
Determines whether all constraints are satisfied by state, returning a single evaluation result...
void removeCostSources(std::set< CostSource > &cost_sources, const std::set< CostSource > &cost_sources_to_remove, double overlap_fraction)
This namespace includes the central class for representing planning contexts.
bool getOctomapMsg(octomap_msgs::OctomapWithPose &octomap) const
Construct a message (octomap) with the octomap data from the planning_scene.
collision_detection::AllowedCollisionMatrix & getAllowedCollisionMatrixNonConst()
Get the allowed collision matrix.
moveit_msgs::CollisionObject * obj_
void removeAllCollisionObjects()
Clear all collision objects in planning scene.
CollisionDetectorPtr active_collision_
void attachedBodiesToAttachedCollisionObjectMsgs(const std::vector< const AttachedBody *> &attached_bodies, std::vector< moveit_msgs::AttachedCollisionObject > &attached_collision_objs)
Convert AttachedBodies to AttachedCollisionObjects.
std::size_t max_contacts_per_pair
Maximum number of contacts to compute per pair of bodies (multiple bodies may be in contact at differ...
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::RobotState &robot_state, bool copy_attached_bodies=true)
Convert a MoveIt! robot state to a robot state message.
bool isStateConstrained(const moveit_msgs::RobotState &state, const moveit_msgs::Constraints &constr, bool verbose=false) const
Check if a given state satisfies a set of constraints.
collision_detection::World::ObserverCallbackFn current_world_object_update_callback_
bool constructMsgFromShape(const Shape *shape, ShapeMsg &shape_msg)
void addCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr &allocator)
Add a new collision detector type.
collision_detection::WorldDiffPtr world_diff_
std::map< std::string, CollisionDetectorPtr > collision_
PlanningScenePtr diff() const
Return a new child PlanningScene that uses this one as parent.
void pushDiffs(const PlanningScenePtr &scene)
If there is a parent specified for this scene, then the diffs with respect to that parent are applied...
const std_msgs::ColorRGBA & getObjectColor(const std::string &id) const
bool processCollisionObjectRemove(const moveit_msgs::CollisionObject &object)
const geometry_msgs::Pose * pose_
bool isStateValid(const moveit_msgs::RobotState &state, const std::string &group="", bool verbose=false) const
Check if a given state is valid. This means checking for collisions and feasibility.
std::size_t max_cost_sources
When costs are computed, this value defines how many of the top cost sources should be returned...
bool usePlanningSceneMsg(const moveit_msgs::PlanningScene &scene)
Call setPlanningSceneMsg() or setPlanningSceneDiffMsg() depending on how the is_diff member of the me...
void clearDiffs()
Clear the diffs accumulated for this planning scene, with respect to the parent. This function is a n...
robot_state::TransformsPtr scene_transforms_
collision_detection::World::ObserverHandle current_world_object_update_observer_handle_
bool empty() const
Returns whether or not there are any constraints in the set.
void setCurrentState(const moveit_msgs::RobotState &state)
Set the current robot state to be state. If not all joint values are specified, the previously mainta...
const std::string LOGNAME
std::map< std::string, CollisionDetectorPtr >::iterator CollisionDetectorIterator
#define ROS_ERROR_NAMED(name,...)
collision_detection::WorldPtr world_
static void poseMsgToEigen(const geometry_msgs::Pose &msg, Eigen::Isometry3d &out)
Representation of a robot's state. This includes position, velocity, acceleration and effort...
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get the shapes that make up this attached body.
A link from the robot. Contains the constant transform applied to the link and its geometry...
Shape * constructShapeFromText(std::istream &in)
bool processCollisionObjectMove(const moveit_msgs::CollisionObject &object)
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
bool getAttachedCollisionObjectMsg(moveit_msgs::AttachedCollisionObject &attached_collision_obj, const std::string &ns) const
Construct a message (attached_collision_object) with the attached collision object data from the plan...
friend struct CollisionDetector
const collision_detection::CollisionRobotConstPtr & getCollisionRobot() const
Get the active collision detector for the robot.
Object defining bodies that can be attached to robot links. This is useful when handling objects pick...
const trajectory_msgs::JointTrajectory & getDetachPosture() const
Return the posture that is necessary for the object to be released, (if any). This is useful for exam...
const robot_state::RobotState & getWayPoint(std::size_t index) const
const robot_state::RobotState & getCurrentState() const
Get the state at which the robot is assumed to be.
const collision_detection::WorldConstPtr & getWorld() const
Get the representation of the world.
void setRobotTrajectoryMsg(const robot_state::RobotState &reference_state, const trajectory_msgs::JointTrajectory &trajectory)
Copy the content of the trajectory message into this class. The trajectory message itself is not requ...
PlanningSceneConstPtr parent_
boost::variant< shape_msgs::SolidPrimitive, shape_msgs::Mesh, shape_msgs::Plane > ShapeMsg
const collision_detection::CollisionWorldConstPtr & getCollisionWorld() const
Get the active collision detector for the world.
bool isStateColliding(const std::string &group="", bool verbose=false)
Check if the current state is in collision (with the environment or self collision). If a group name is specified, collision checking is done for that group only. Since the function is non-const, the current state transforms are updated before the collision check.
const EigenSTL::vector_Isometry3d & getFixedTransforms() const
Get the fixed transform (the transforms to the shapes associated with this body)
bool cost
If true, a collision cost is computed.
const Eigen::Isometry3d & getFrameTransform(const std::string &id) const
Get the transform corresponding to the frame id. This will be known if id is a link name...
bool setPlanningSceneDiffMsg(const moveit_msgs::PlanningScene &scene)
Apply changes to this planning scene as diffs, even if the message itself is not marked as being a di...
std::shared_ptr< const Shape > ShapeConstPtr
void checkCollisionUnpadded(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res)
Check whether the current state is in collision, but use a collision_detection::CollisionRobot instan...