37 #include <boost/algorithm/string.hpp>
41 #include <geometric_shapes/shape_operations.h>
48 #include <octomap_msgs/conversions.h>
49 #include <rclcpp/logger.hpp>
50 #include <rclcpp/logging.hpp>
51 #include <tf2_eigen/tf2_eigen.hpp>
76 void poseMsgToEigen(
const geometry_msgs::msg::Pose& msg, Eigen::Isometry3d& out)
78 Eigen::Translation3d translation(msg.position.x, msg.position.y, msg.position.z);
79 Eigen::Quaterniond quaternion(msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z);
80 quaternion.normalize();
81 out = translation * quaternion;
87 double x, y, z, rx, ry, rz, rw;
88 if (!(in >> x >> y >> z))
90 RCLCPP_ERROR(
getLogger(),
"Improperly formatted translation in scene geometry file");
93 if (!(in >> rx >> ry >> rz >> rw))
95 RCLCPP_ERROR(
getLogger(),
"Improperly formatted rotation in scene geometry file");
98 pose = Eigen::Translation3d(x, y, z) * Eigen::Quaterniond(rw, rx, ry, rz);
105 out << pose.translation().x() <<
' ' << pose.translation().y() <<
' ' << pose.translation().z() <<
'\n';
106 Eigen::Quaterniond r(pose.linear());
107 out << r.x() <<
' ' << r.y() <<
' ' << r.z() <<
' ' << r.w() <<
'\n';
127 if (Transforms::isFixedFrame(frame))
131 return knowsObjectFrame(frame.substr(1));
135 return knowsObjectFrame(frame);
139 const Eigen::Isometry3d&
getTransform(
const std::string& from_frame)
const override
146 bool knowsObjectFrame(
const std::string& frame_id)
const
148 return scene_->
getWorld()->knowsTransform(frame_id);
151 const PlanningScene* scene_;
155 const collision_detection::WorldPtr& world)
157 : robot_model_(robot_model), world_(world), world_const_(world)
163 const srdf::ModelConstSharedPtr& srdf_model,
const collision_detection::WorldPtr& world)
164 : world_(world), world_const_(world)
172 robot_model_ = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
173 if (!robot_model_ || !robot_model_->getRootJoint())
175 robot_model_ =
nullptr;
184 if (current_world_object_update_callback_)
185 world_->removeObserver(current_world_object_update_observer_handle_);
188 void PlanningScene::initialize()
192 scene_transforms_ = std::make_shared<SceneTransforms>(
this);
194 robot_state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
195 robot_state_->setToDefaultValues();
196 robot_state_->update();
198 acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(*
getRobotModel()->getSRDF());
208 if (!parent_->getName().empty())
209 name_ = parent_->getName() +
"+";
211 robot_model_ = parent_->robot_model_;
215 world_ = std::make_shared<collision_detection::World>(*parent_->world_);
216 world_const_ = world_;
219 world_diff_ = std::make_shared<collision_detection::WorldDiff>(world_);
222 collision_detector_->copyPadding(*parent_->collision_detector_);
227 PlanningScenePtr result = scene->diff();
228 result->decoupleParent();
229 result->setName(scene->getName());
235 return PlanningScenePtr(
new PlanningScene(shared_from_this()));
240 PlanningScenePtr result =
diff();
241 result->setPlanningSceneDiffMsg(msg);
247 cenv_->setLinkPadding(src.getCollisionEnv()->getLinkPadding());
248 cenv_->setLinkScale(src.getCollisionEnv()->getLinkScale());
252 const CollisionDetectorPtr& parent_detector)
255 CollisionDetectorPtr prev_coll_detector = collision_detector_;
258 collision_detector_ = std::make_shared<CollisionDetector>();
259 collision_detector_->alloc_ = allocator;
265 collision_detector_->cenv_ = collision_detector_->alloc_->allocateEnv(parent_detector->cenv_, world_);
266 collision_detector_->cenv_unpadded_ =
267 collision_detector_->alloc_->allocateEnv(parent_detector->cenv_unpadded_, world_);
271 collision_detector_->cenv_ = collision_detector_->alloc_->allocateEnv(world_,
getRobotModel());
272 collision_detector_->cenv_unpadded_ = collision_detector_->alloc_->allocateEnv(world_,
getRobotModel());
275 if (prev_coll_detector)
276 collision_detector_->copyPadding(*prev_coll_detector);
280 collision_detector_->cenv_const_ = collision_detector_->cenv_;
281 collision_detector_->cenv_unpadded_const_ = collision_detector_->cenv_unpadded_;
284 const collision_detection::CollisionEnvConstPtr&
289 RCLCPP_ERROR(
getLogger(),
"Could not get CollisionRobot named '%s'. Returning active CollisionRobot '%s' instead",
290 collision_detector_name.c_str(), collision_detector_->alloc_->getName().c_str());
291 return collision_detector_->getCollisionEnv();
294 return collision_detector_->getCollisionEnv();
297 const collision_detection::CollisionEnvConstPtr&
303 "Could not get CollisionRobotUnpadded named '%s'. "
304 "Returning active CollisionRobotUnpadded '%s' instead",
305 collision_detector_name.c_str(), collision_detector_->alloc_->getName().c_str());
306 return collision_detector_->getCollisionEnvUnpadded();
309 return collision_detector_->getCollisionEnvUnpadded();
318 world_ = std::make_shared<collision_detection::World>(*parent_->world_);
319 world_const_ = world_;
320 world_diff_ = std::make_shared<collision_detection::WorldDiff>(world_);
321 if (current_world_object_update_callback_)
322 current_world_object_update_observer_handle_ = world_->addObserver(current_world_object_update_callback_);
327 scene_transforms_.reset();
328 robot_state_.reset();
330 object_colors_.reset();
331 object_types_.reset();
339 if (scene_transforms_)
340 scene->getTransformsNonConst().setAllTransforms(scene_transforms_->getAllTransforms());
344 scene->getCurrentStateNonConst() = *robot_state_;
346 std::vector<const moveit::core::AttachedBody*> attached_objs;
347 robot_state_->getAttachedBodies(attached_objs);
351 scene->setObjectType(attached_obj->getName(),
getObjectType(attached_obj->getName()));
353 scene->setObjectColor(attached_obj->getName(),
getObjectColor(attached_obj->getName()));
358 scene->getAllowedCollisionMatrixNonConst() = *acm_;
360 collision_detection::CollisionEnvPtr active_cenv = scene->getCollisionEnvNonConst();
361 active_cenv->setLinkPadding(collision_detector_->cenv_->getLinkPadding());
362 active_cenv->setLinkScale(collision_detector_->cenv_->getLinkScale());
366 for (
const std::pair<const std::string, collision_detection::World::Action>& it : *world_diff_)
370 scene->world_->removeObject(it.first);
371 scene->removeObjectColor(it.first);
372 scene->removeObjectType(it.first);
377 scene->world_->removeObject(obj.
id_);
479 const std::string& group_name)
const
509 for (collision_detection::CollisionResult::ContactMap::const_iterator it = contacts.begin(); it != contacts.end();
524 return collision_detector_->cenv_;
531 robot_state_ = std::make_shared<moveit::core::RobotState>(parent_->getCurrentState());
532 robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
534 robot_state_->update();
535 return *robot_state_;
540 auto state = std::make_shared<moveit::core::RobotState>(
getCurrentState());
547 current_state_attached_body_callback_ = callback;
549 robot_state_->setAttachedBodyUpdateCallback(callback);
554 if (current_world_object_update_callback_)
555 world_->removeObserver(current_world_object_update_observer_handle_);
557 current_world_object_update_observer_handle_ = world_->addObserver(callback);
558 current_world_object_update_callback_ = callback;
564 acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(parent_->getAllowedCollisionMatrix());
584 if (!scene_transforms_)
588 scene_transforms_ = std::make_shared<SceneTransforms>(
this);
589 scene_transforms_->setAllTransforms(parent_->getTransforms().getAllTransforms());
591 return *scene_transforms_;
596 scene_msg.name = name_;
598 scene_msg.is_diff =
true;
600 if (scene_transforms_)
602 scene_transforms_->copyTransforms(scene_msg.fixed_frame_transforms);
606 scene_msg.fixed_frame_transforms.clear();
615 scene_msg.robot_state = moveit_msgs::msg::RobotState();
617 scene_msg.robot_state.is_diff =
true;
621 acm_->getMessage(scene_msg.allowed_collision_matrix);
625 scene_msg.allowed_collision_matrix = moveit_msgs::msg::AllowedCollisionMatrix();
628 collision_detector_->cenv_->getPadding(scene_msg.link_padding);
629 collision_detector_->cenv_->getScale(scene_msg.link_scale);
631 scene_msg.object_colors.clear();
635 scene_msg.object_colors.resize(object_colors_->size());
636 for (ObjectColorMap::const_iterator it = object_colors_->begin(); it != object_colors_->end(); ++it, ++i)
638 scene_msg.object_colors[i].id = it->first;
639 scene_msg.object_colors[i].color = it->second;
643 scene_msg.world.collision_objects.clear();
644 scene_msg.world.octomap = octomap_msgs::msg::OctomapWithPose();
648 bool do_omap =
false;
649 for (
const std::pair<const std::string, collision_detection::World::Action>& it : *world_diff_)
658 if (!std::count_if(scene_msg.robot_state.attached_collision_objects.cbegin(),
659 scene_msg.robot_state.attached_collision_objects.cend(),
660 [&it](
const moveit_msgs::msg::AttachedCollisionObject& aco) {
661 return aco.object.id == it.first &&
662 aco.object.operation == moveit_msgs::msg::CollisionObject::ADD;
665 moveit_msgs::msg::CollisionObject co;
668 co.operation = moveit_msgs::msg::CollisionObject::REMOVE;
669 scene_msg.world.collision_objects.push_back(co);
674 scene_msg.world.collision_objects.emplace_back();
685 for (
const auto& collision_object : scene_msg.world.collision_objects)
687 if (parent_ && parent_->getCurrentState().hasAttachedBody(collision_object.id))
689 moveit_msgs::msg::AttachedCollisionObject aco;
690 aco.object.id = collision_object.id;
691 aco.object.operation = moveit_msgs::msg::CollisionObject::REMOVE;
692 scene_msg.robot_state.attached_collision_objects.push_back(aco);
699 class ShapeVisitorAddToCollisionObject :
public boost::static_visitor<void>
702 ShapeVisitorAddToCollisionObject(moveit_msgs::msg::CollisionObject* obj) : boost::static_visitor<void>(), obj_(obj)
706 void setPoseMessage(
const geometry_msgs::msg::Pose* pose)
711 void operator()(
const shape_msgs::msg::Plane& shape_msg)
const
713 obj_->planes.push_back(shape_msg);
714 obj_->plane_poses.push_back(*pose_);
717 void operator()(
const shape_msgs::msg::Mesh& shape_msg)
const
719 obj_->meshes.push_back(shape_msg);
720 obj_->mesh_poses.push_back(*pose_);
723 void operator()(
const shape_msgs::msg::SolidPrimitive& shape_msg)
const
725 obj_->primitives.push_back(shape_msg);
726 obj_->primitive_poses.push_back(*pose_);
730 moveit_msgs::msg::CollisionObject* obj_;
731 const geometry_msgs::msg::Pose* pose_;
741 collision_obj.pose = tf2::toMsg(obj->pose_);
742 collision_obj.id = ns;
743 collision_obj.operation = moveit_msgs::msg::CollisionObject::ADD;
745 ShapeVisitorAddToCollisionObject sv(&collision_obj);
746 for (std::size_t j = 0; j < obj->shapes_.size(); ++j)
749 if (constructMsgFromShape(obj->shapes_[j].get(), sm))
751 geometry_msgs::msg::Pose p = tf2::toMsg(obj->shape_poses_[j]);
752 sv.setPoseMessage(&p);
753 boost::apply_visitor(sv, sm);
757 if (!collision_obj.primitives.empty() || !collision_obj.meshes.empty() || !collision_obj.planes.empty())
762 for (
const auto& frame_pair : obj->subframe_poses_)
764 collision_obj.subframe_names.push_back(frame_pair.first);
765 geometry_msgs::msg::Pose p;
766 p = tf2::toMsg(frame_pair.second);
767 collision_obj.subframe_poses.push_back(p);
775 collision_objs.clear();
776 const std::vector<std::string>& ids = world_->getObjectIds();
777 for (
const std::string&
id : ids)
781 collision_objs.emplace_back();
788 const std::string& ns)
const
790 std::vector<moveit_msgs::msg::AttachedCollisionObject> attached_collision_objs;
792 for (moveit_msgs::msg::AttachedCollisionObject& it : attached_collision_objs)
794 if (it.object.id == ns)
796 attached_collision_obj = it;
804 std::vector<moveit_msgs::msg::AttachedCollisionObject>& attached_collision_objs)
const
806 std::vector<const moveit::core::AttachedBody*> attached_bodies;
814 octomap.octomap = octomap_msgs::msg::Octomap();
819 if (map->shapes_.size() == 1)
821 const shapes::OcTree* o =
static_cast<const shapes::OcTree*
>(map->shapes_[0].get());
822 octomap_msgs::fullMapToMsg(*o->octree,
octomap.octomap);
823 octomap.origin = tf2::toMsg(map->shape_poses_[0]);
826 RCLCPP_ERROR(
getLogger(),
"Unexpected number of shapes in octomap collision object. Not including '%s' object",
834 object_colors.clear();
839 object_colors.resize(cmap.size());
840 for (ObjectColorMap::const_iterator it = cmap.begin(); it != cmap.end(); ++it, ++i)
842 object_colors[i].id = it->first;
843 object_colors[i].color = it->second;
849 scene_msg.name = name_;
850 scene_msg.is_diff =
false;
869 const moveit_msgs::msg::PlanningSceneComponents& comp)
const
871 scene_msg.is_diff =
false;
872 if (comp.components & moveit_msgs::msg::PlanningSceneComponents::SCENE_SETTINGS)
874 scene_msg.name = name_;
878 if (comp.components & moveit_msgs::msg::PlanningSceneComponents::TRANSFORMS)
881 if (comp.components & moveit_msgs::msg::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS)
884 for (moveit_msgs::msg::AttachedCollisionObject& attached_collision_object :
885 scene_msg.robot_state.attached_collision_objects)
889 attached_collision_object.object.type =
getObjectType(attached_collision_object.object.id);
893 else if (comp.components & moveit_msgs::msg::PlanningSceneComponents::ROBOT_STATE)
898 if (comp.components & moveit_msgs::msg::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX)
901 if (comp.components & moveit_msgs::msg::PlanningSceneComponents::LINK_PADDING_AND_SCALING)
907 if (comp.components & moveit_msgs::msg::PlanningSceneComponents::OBJECT_COLORS)
911 if (comp.components & moveit_msgs::msg::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY)
915 else if (comp.components & moveit_msgs::msg::PlanningSceneComponents::WORLD_OBJECT_NAMES)
917 const std::vector<std::string>& ids = world_->getObjectIds();
918 scene_msg.world.collision_objects.clear();
919 scene_msg.world.collision_objects.reserve(ids.size());
920 for (
const std::string&
id : ids)
924 moveit_msgs::msg::CollisionObject co;
928 scene_msg.world.collision_objects.push_back(co);
934 if (comp.components & moveit_msgs::msg::PlanningSceneComponents::OCTOMAP)
940 out << name_ <<
'\n';
941 const std::vector<std::string>& ids = world_->getObjectIds();
942 for (
const std::string&
id : ids)
949 out <<
"* " <<
id <<
'\n';
954 out << obj->shapes_.size() <<
'\n';
955 for (std::size_t j = 0; j < obj->shapes_.size(); ++j)
957 shapes::saveAsText(obj->shapes_[j].get(), out);
963 out << c.r <<
' ' << c.g <<
' ' << c.b <<
' ' << c.a <<
'\n';
966 out <<
"0 0 0 0" <<
'\n';
970 out << obj->subframe_poses_.size() <<
'\n';
971 for (
auto& pose_pair : obj->subframe_poses_)
973 out << pose_pair.first <<
'\n';
989 if (!in.good() || in.eof())
991 RCLCPP_ERROR(
getLogger(),
"Bad input stream when loading scene geometry");
995 std::getline(in, name_);
998 auto pos = in.tellg();
1002 std::getline(in, line);
1003 }
while (in.good() && !in.eof() && (line.empty() || line[0] !=
'*'));
1004 std::getline(in, line);
1005 boost::algorithm::trim(line);
1008 bool uses_new_scene_format = line.find(
' ') != std::string::npos;
1011 Eigen::Isometry3d pose;
1016 if (!in.good() || in.eof())
1018 RCLCPP_ERROR(
getLogger(),
"Bad input stream when loading marker in scene geometry");
1023 std::string object_id;
1024 std::getline(in, object_id);
1025 if (!in.good() || in.eof())
1027 RCLCPP_ERROR(
getLogger(),
"Bad input stream when loading object_id in scene geometry");
1030 boost::algorithm::trim(object_id);
1036 RCLCPP_ERROR(
getLogger(),
"Failed to read object pose from scene file");
1039 pose = offset * pose;
1040 world_->setObjectPose(object_id, pose);
1043 unsigned int shape_count;
1045 for (std::size_t i = 0; i < shape_count && in.good() && !in.eof(); ++i)
1047 const auto shape = shapes::ShapeConstPtr(shapes::constructShapeFromText(in));
1050 RCLCPP_ERROR(
getLogger(),
"Failed to load shape from scene file");
1055 RCLCPP_ERROR(
getLogger(),
"Failed to read pose from scene file");
1059 if (!(in >> r >> g >> b >> a))
1061 RCLCPP_ERROR(
getLogger(),
"Improperly formatted color in scene geometry file");
1066 world_->addToObject(object_id, shape, pose);
1067 if (r > 0.0f || g > 0.0f || b > 0.0f || a > 0.0f)
1069 std_msgs::msg::ColorRGBA color;
1080 if (uses_new_scene_format)
1083 unsigned int subframe_count;
1084 in >> subframe_count;
1085 for (std::size_t i = 0; i < subframe_count && in.good() && !in.eof(); ++i)
1087 std::string subframe_name;
1088 in >> subframe_name;
1091 RCLCPP_ERROR(
getLogger(),
"Failed to read subframe pose from scene file");
1094 subframes[subframe_name] = pose;
1096 world_->setSubframesOfObject(object_id, subframes);
1099 else if (marker ==
".")
1106 RCLCPP_ERROR(
getLogger(),
"Unknown marker in scene geometry file: %s ", marker.c_str());
1116 moveit_msgs::msg::RobotState state_no_attached(state);
1117 state_no_attached.attached_collision_objects.clear();
1123 robot_state_ = std::make_shared<moveit::core::RobotState>(parent_->getCurrentState());
1124 robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
1131 for (std::size_t i = 0; i < state.attached_collision_objects.size(); ++i)
1133 if (!state.is_diff && state.attached_collision_objects[i].object.operation != moveit_msgs::msg::CollisionObject::ADD)
1136 "The specified RobotState is not marked as is_diff. "
1137 "The request to modify the object '%s' is not supported. Object is ignored.",
1138 state.attached_collision_objects[i].object.id.c_str());
1156 if (!scene_transforms_)
1158 scene_transforms_ = std::make_shared<SceneTransforms>(
this);
1159 scene_transforms_->setAllTransforms(parent_->getTransforms().getAllTransforms());
1164 robot_state_ = std::make_shared<moveit::core::RobotState>(parent_->getCurrentState());
1165 robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
1169 acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(parent_->getAllowedCollisionMatrix());
1171 world_diff_.reset();
1173 if (!object_colors_)
1176 parent_->getKnownObjectColors(kc);
1177 object_colors_ = std::make_unique<ObjectColorMap>(kc);
1182 parent_->getKnownObjectColors(kc);
1183 for (ObjectColorMap::const_iterator it = kc.begin(); it != kc.end(); ++it)
1185 if (object_colors_->find(it->first) == object_colors_->end())
1186 (*object_colors_)[it->first] = it->second;
1193 parent_->getKnownObjectTypes(kc);
1194 object_types_ = std::make_unique<ObjectTypeMap>(kc);
1199 parent_->getKnownObjectTypes(kc);
1200 for (ObjectTypeMap::const_iterator it = kc.begin(); it != kc.end(); ++it)
1202 if (object_types_->find(it->first) == object_types_->end())
1203 (*object_types_)[it->first] = it->second;
1214 RCLCPP_DEBUG(
getLogger(),
"Adding planning scene diff");
1215 if (!scene_msg.name.empty())
1216 name_ = scene_msg.name;
1218 if (!scene_msg.robot_model_name.empty() && scene_msg.robot_model_name !=
getRobotModel()->
getName())
1220 RCLCPP_WARN(
getLogger(),
"Setting the scene for model '%s' but model '%s' is loaded.",
1226 if (!scene_msg.fixed_frame_transforms.empty())
1228 if (!scene_transforms_)
1229 scene_transforms_ = std::make_shared<SceneTransforms>(
this);
1230 scene_transforms_->setTransforms(scene_msg.fixed_frame_transforms);
1234 if (!scene_msg.robot_state.multi_dof_joint_state.joint_names.empty() ||
1235 !scene_msg.robot_state.joint_state.name.empty() || !scene_msg.robot_state.attached_collision_objects.empty())
1239 if (!scene_msg.allowed_collision_matrix.entry_names.empty())
1240 acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(scene_msg.allowed_collision_matrix);
1242 if (!scene_msg.link_padding.empty() || !scene_msg.link_scale.empty())
1244 collision_detector_->cenv_->setPadding(scene_msg.link_padding);
1245 collision_detector_->cenv_->setScale(scene_msg.link_scale);
1249 for (
const moveit_msgs::msg::ObjectColor& object_color : scene_msg.object_colors)
1253 for (
const moveit_msgs::msg::CollisionObject& collision_object : scene_msg.world.collision_objects)
1257 if (!scene_msg.world.octomap.octomap.data.empty())
1265 RCLCPP_DEBUG(
getLogger(),
"Setting new planning scene: '%s'", scene_msg.name.c_str());
1266 name_ = scene_msg.name;
1268 if (!scene_msg.robot_model_name.empty() && scene_msg.robot_model_name !=
getRobotModel()->
getName())
1270 RCLCPP_WARN(
getLogger(),
"Setting the scene for model '%s' but model '%s' is loaded.",
1277 object_types_.reset();
1278 scene_transforms_->setTransforms(scene_msg.fixed_frame_transforms);
1280 acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(scene_msg.allowed_collision_matrix);
1281 collision_detector_->cenv_->setPadding(scene_msg.link_padding);
1282 collision_detector_->cenv_->setScale(scene_msg.link_scale);
1283 object_colors_ = std::make_unique<ObjectColorMap>();
1284 original_object_colors_ = std::make_unique<ObjectColorMap>();
1285 for (
const moveit_msgs::msg::ObjectColor& object_color : scene_msg.object_colors)
1287 world_->clearObjects();
1294 for (
const moveit_msgs::msg::CollisionObject& collision_object : world.collision_objects)
1302 if (scene_msg.is_diff)
1314 std::shared_ptr<collision_detection::OccMapTree> om =
1315 std::make_shared<collision_detection::OccMapTree>(map.resolution);
1318 octomap_msgs::readTree(om.get(), map);
1322 std::stringstream datastream;
1323 if (!map.data.empty())
1325 datastream.write(
reinterpret_cast<const char*
>(&map.data[0]), map.data.size());
1326 om->readData(datastream);
1337 if (map.data.empty())
1340 if (map.id !=
"OcTree")
1342 RCLCPP_ERROR(
getLogger(),
"Received octomap is of type '%s' but type 'OcTree' is expected.", map.id.c_str());
1346 std::shared_ptr<collision_detection::OccMapTree> om =
createOctomap(map);
1347 if (!map.header.frame_id.empty())
1350 world_->addToObject(
OCTOMAP_NS, std::make_shared<const shapes::OcTree>(om), t);
1354 world_->addToObject(
OCTOMAP_NS, std::make_shared<const shapes::OcTree>(om), Eigen::Isometry3d::Identity());
1360 const std::vector<std::string>& object_ids = world_->getObjectIds();
1361 for (
const std::string& object_id : object_ids)
1365 world_->removeObject(object_id);
1377 if (map.octomap.data.empty())
1380 if (map.octomap.id !=
"OcTree")
1382 RCLCPP_ERROR(
getLogger(),
"Received octomap is of type '%s' but type 'OcTree' is expected.", map.octomap.id.c_str());
1386 std::shared_ptr<collision_detection::OccMapTree> om =
createOctomap(map.octomap);
1389 Eigen::Isometry3d p;
1392 world_->addToObject(
OCTOMAP_NS, std::make_shared<const shapes::OcTree>(om), p);
1400 if (map->shapes_.size() == 1)
1403 const shapes::OcTree* o =
static_cast<const shapes::OcTree*
>(map->shapes_[0].get());
1404 if (o->octree == octree)
1407 if (map->shape_poses_[0].isApprox(t, std::numeric_limits<double>::epsilon() * 100.0))
1417 shapes::ShapeConstPtr shape = map->shapes_[0];
1419 world_->moveShapeInObject(
OCTOMAP_NS, shape, t);
1427 world_->addToObject(
OCTOMAP_NS, std::make_shared<const shapes::OcTree>(octree), t);
1432 if (
object.
object.operation == moveit_msgs::msg::CollisionObject::ADD &&
1435 RCLCPP_ERROR(
getLogger(),
"Unable to attach a body to link '%s' (link not found)",
object.link_name.c_str());
1441 RCLCPP_ERROR(
getLogger(),
"The ID '%s' cannot be used for collision objects (name reserved)",
OCTOMAP_NS.c_str());
1447 robot_state_ = std::make_shared<moveit::core::RobotState>(parent_->getCurrentState());
1448 robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
1450 robot_state_->update();
1457 if (
object.
object.operation == moveit_msgs::msg::CollisionObject::ADD ||
1458 object.
object.operation == moveit_msgs::msg::CollisionObject::APPEND)
1464 Eigen::Isometry3d object_pose_in_link;
1465 std::vector<shapes::ShapeConstPtr>
shapes;
1466 EigenSTL::vector_Isometry3d shape_poses;
1473 if (
object.
object.operation == moveit_msgs::msg::CollisionObject::ADD &&
object.
object.primitives.empty() &&
1474 object.object.meshes.empty() &&
object.object.planes.empty())
1478 RCLCPP_DEBUG(
getLogger(),
"Attaching world object '%s' to link '%s'",
object.
object.
id.c_str(),
1479 object.link_name.c_str());
1481 object_pose_in_link = robot_state_->getGlobalLinkTransform(link_model).inverse() * obj_in_world->pose_;
1482 shapes = obj_in_world->shapes_;
1483 shape_poses = obj_in_world->shape_poses_;
1484 subframe_poses = obj_in_world->subframe_poses_;
1489 "Attempting to attach object '%s' to link '%s' but no geometry specified "
1490 "and such an object does not exist in the collision world",
1491 object.
object.
id.c_str(),
object.link_name.c_str());
1497 Eigen::Isometry3d header_frame_to_object_pose;
1500 const Eigen::Isometry3d world_to_header_frame =
getFrameTransform(
object.
object.header.frame_id);
1501 const Eigen::Isometry3d link_to_header_frame =
1502 robot_state_->getGlobalLinkTransform(link_model).inverse() * world_to_header_frame;
1503 object_pose_in_link = link_to_header_frame * header_frame_to_object_pose;
1505 Eigen::Isometry3d subframe_pose;
1506 for (std::size_t i = 0; i <
object.object.subframe_poses.size(); ++i)
1509 std::string
name =
object.object.subframe_names[i];
1510 subframe_poses[
name] = subframe_pose;
1516 RCLCPP_ERROR(
getLogger(),
"There is no geometry to attach to link '%s' as part of attached body '%s'",
1517 object.link_name.c_str(),
object.object.id.c_str());
1521 if (!
object.
object.type.db.empty() || !
object.object.type.key.empty())
1525 if (obj_in_world && world_->removeObject(
object.object.id))
1527 if (
object.
object.operation == moveit_msgs::msg::CollisionObject::ADD)
1529 RCLCPP_DEBUG(
getLogger(),
"Removing world object with the same name as newly attached object: '%s'",
1530 object.
object.
id.c_str());
1535 "You tried to append geometry to an attached object "
1536 "that is actually a world object ('%s'). World geometry is ignored.",
1537 object.
object.
id.c_str());
1542 if (
object.
object.operation == moveit_msgs::msg::CollisionObject::ADD ||
1543 !robot_state_->hasAttachedBody(
object.object.id))
1545 if (robot_state_->clearAttachedBody(
object.object.id))
1548 "The robot state already had an object named '%s' attached to link '%s'. "
1549 "The object was replaced.",
1550 object.
object.
id.c_str(),
object.link_name.c_str());
1552 robot_state_->attachBody(
object.
object.
id, object_pose_in_link,
shapes, shape_poses,
object.touch_links,
1553 object.link_name,
object.detach_posture, subframe_poses);
1554 RCLCPP_DEBUG(
getLogger(),
"Attached object '%s' to link '%s'",
object.
object.
id.c_str(),
1555 object.link_name.c_str());
1563 object_pose_in_link = ab->
getPose();
1568 trajectory_msgs::msg::JointTrajectory detach_posture =
1569 object.detach_posture.joint_names.empty() ? ab->
getDetachPosture() :
object.detach_posture;
1572 touch_links.insert(std::make_move_iterator(
object.touch_links.begin()),
1573 std::make_move_iterator(
object.touch_links.end()));
1575 robot_state_->clearAttachedBody(
object.
object.
id);
1576 robot_state_->attachBody(
object.
object.
id, object_pose_in_link,
shapes, shape_poses, touch_links,
1577 object.link_name, detach_posture, subframe_poses);
1578 RCLCPP_DEBUG(
getLogger(),
"Appended things to object '%s' attached to link '%s'",
object.
object.
id.c_str(),
1579 object.link_name.c_str());
1585 RCLCPP_ERROR(
getLogger(),
"Robot state is not compatible with robot model. This could be fatal.");
1588 else if (
object.
object.operation == moveit_msgs::msg::CollisionObject::REMOVE)
1591 std::vector<const moveit::core::AttachedBody*> attached_bodies;
1592 if (
object.
object.
id.empty())
1595 object.link_name.empty() ? nullptr :
getRobotModel()->getLinkModel(
object.link_name);
1598 robot_state_->getAttachedBodies(attached_bodies, link_model);
1602 robot_state_->getAttachedBodies(attached_bodies);
1612 RCLCPP_ERROR_STREAM(
getLogger(),
"The AttachedCollisionObject message states the object is attached to "
1613 <<
object.link_name <<
", but it is actually attached to "
1615 <<
". Leave the link_name empty or specify the correct link.");
1618 attached_bodies.push_back(body);
1625 const std::string&
name = attached_body->getName();
1626 if (world_->hasObject(
name))
1629 "The collision world already has an object with the same name as the body about to be detached. "
1630 "NOT adding the detached body '%s' to the collision world.",
1631 object.
object.
id.c_str());
1635 const Eigen::Isometry3d& pose = attached_body->getGlobalPose();
1636 world_->addToObject(
name, pose, attached_body->getShapes(), attached_body->getShapePoses());
1637 world_->setSubframesOfObject(
name, attached_body->getSubframes());
1642 if (original_object_color.has_value())
1644 setObjectColor(attached_body->getName(), original_object_color.value());
1647 RCLCPP_DEBUG(
getLogger(),
"Detached object '%s' from link '%s' and added it back in the collision world",
1648 name.c_str(),
object.link_name.c_str());
1651 robot_state_->clearAttachedBody(
name);
1653 if (!attached_bodies.empty() ||
object.object.id.empty())
1656 else if (
object.
object.operation == moveit_msgs::msg::CollisionObject::MOVE)
1658 RCLCPP_ERROR(
getLogger(),
"Move for attached objects not yet implemented");
1662 RCLCPP_ERROR(
getLogger(),
"Unknown collision object operation: %d",
object.
object.operation);
1672 RCLCPP_ERROR(
getLogger(),
"The ID '%s' cannot be used for collision objects (name reserved)",
OCTOMAP_NS.c_str());
1676 if (
object.operation == moveit_msgs::msg::CollisionObject::ADD ||
1677 object.operation == moveit_msgs::msg::CollisionObject::APPEND)
1679 return processCollisionObjectAdd(
object);
1681 else if (
object.operation == moveit_msgs::msg::CollisionObject::REMOVE)
1683 return processCollisionObjectRemove(
object);
1685 else if (
object.operation == moveit_msgs::msg::CollisionObject::MOVE)
1687 return processCollisionObjectMove(
object);
1690 RCLCPP_ERROR(
getLogger(),
"Unknown collision object operation: %d",
object.operation);
1695 Eigen::Isometry3d& object_pose,
1696 std::vector<shapes::ShapeConstPtr>&
shapes,
1697 EigenSTL::vector_Isometry3d& shape_poses)
1699 if (
object.primitives.size() <
object.primitive_poses.size())
1701 RCLCPP_ERROR(
getLogger(),
"More primitive shape poses than shapes in collision object message.");
1704 if (
object.meshes.size() <
object.mesh_poses.size())
1706 RCLCPP_ERROR(
getLogger(),
"More mesh poses than meshes in collision object message.");
1709 if (
object.planes.size() <
object.plane_poses.size())
1711 RCLCPP_ERROR(
getLogger(),
"More plane poses than planes in collision object message.");
1715 const int num_shapes =
object.primitives.size() +
object.meshes.size() +
object.planes.size();
1716 shapes.reserve(num_shapes);
1717 shape_poses.reserve(num_shapes);
1721 bool switch_object_pose_and_shape_pose =
false;
1722 if (num_shapes == 1)
1726 switch_object_pose_and_shape_pose =
true;
1732 &switch_object_pose_and_shape_pose](shapes::Shape* s,
const geometry_msgs::msg::Pose& pose_msg) {
1735 Eigen::Isometry3d pose;
1737 if (!switch_object_pose_and_shape_pose)
1739 shape_poses.emplace_back(std::move(pose));
1743 shape_poses.emplace_back(std::move(object_pose));
1746 shapes.emplace_back(shapes::ShapeConstPtr(s));
1749 auto treat_shape_vectors = [&
append](
const auto& shape_vector,
1750 const auto& shape_poses_vector,
1751 const std::string& shape_type) {
1752 if (shape_vector.size() > shape_poses_vector.size())
1754 RCLCPP_DEBUG_STREAM(
getLogger(),
"Number of " << shape_type
1755 <<
" does not match number of poses "
1756 "in collision object message. Assuming identity.");
1757 for (std::size_t i = 0; i < shape_vector.size(); ++i)
1759 if (i >= shape_poses_vector.size())
1761 append(shapes::constructShapeFromMsg(shape_vector[i]),
1762 geometry_msgs::msg::Pose());
1765 append(shapes::constructShapeFromMsg(shape_vector[i]), shape_poses_vector[i]);
1770 for (std::size_t i = 0; i < shape_vector.size(); ++i)
1771 append(shapes::constructShapeFromMsg(shape_vector[i]), shape_poses_vector[i]);
1775 treat_shape_vectors(
object.primitives,
object.primitive_poses, std::string(
"primitive_poses"));
1776 treat_shape_vectors(
object.meshes,
object.mesh_poses, std::string(
"meshes"));
1777 treat_shape_vectors(
object.planes,
object.plane_poses, std::string(
"planes"));
1781 bool PlanningScene::processCollisionObjectAdd(
const moveit_msgs::msg::CollisionObject&
object)
1785 RCLCPP_ERROR_STREAM(
getLogger(),
"Unknown frame: " <<
object.header.frame_id);
1789 if (
object.primitives.empty() &&
object.meshes.empty() &&
object.planes.empty())
1791 RCLCPP_ERROR(
getLogger(),
"There are no shapes specified in the collision object message");
1796 if (
object.operation == moveit_msgs::msg::CollisionObject::ADD && world_->hasObject(
object.id))
1797 world_->removeObject(
object.
id);
1799 const Eigen::Isometry3d& world_to_object_header_transform =
getFrameTransform(
object.header.frame_id);
1800 Eigen::Isometry3d header_to_pose_transform;
1801 std::vector<shapes::ShapeConstPtr>
shapes;
1802 EigenSTL::vector_Isometry3d shape_poses;
1805 const Eigen::Isometry3d object_frame_transform = world_to_object_header_transform * header_to_pose_transform;
1807 world_->addToObject(
object.
id, object_frame_transform,
shapes, shape_poses);
1809 if (!
object.type.key.empty() || !
object.type.db.empty())
1814 Eigen::Isometry3d subframe_pose;
1815 for (std::size_t i = 0; i <
object.subframe_poses.size(); ++i)
1818 std::string
name =
object.subframe_names[i];
1819 subframes[
name] = subframe_pose;
1821 world_->setSubframesOfObject(
object.
id, subframes);
1825 bool PlanningScene::processCollisionObjectRemove(
const moveit_msgs::msg::CollisionObject&
object)
1827 if (
object.
id.empty())
1833 if (!world_->removeObject(
object.id))
1836 "Tried to remove world object '" <<
object.
id <<
"', but it does not exist in this scene.");
1846 bool PlanningScene::processCollisionObjectMove(
const moveit_msgs::msg::CollisionObject&
object)
1848 if (world_->hasObject(
object.id))
1850 if (!
object.primitives.empty() || !
object.meshes.empty() || !
object.planes.empty())
1852 RCLCPP_WARN(
getLogger(),
"Move operation for object '%s' ignores the geometry specified in the message.",
1856 const Eigen::Isometry3d& world_to_object_header_transform =
getFrameTransform(
object.header.frame_id);
1857 Eigen::Isometry3d header_to_pose_transform;
1861 const Eigen::Isometry3d object_frame_transform = world_to_object_header_transform * header_to_pose_transform;
1862 world_->setObjectPose(
object.
id, object_frame_transform);
1866 RCLCPP_ERROR(
getLogger(),
"World object '%s' does not exist. Cannot move.",
object.
id.c_str());
1888 const std::string& frame_id)
const
1890 if (!frame_id.empty() && frame_id[0] ==
'/')
1901 const Eigen::Isometry3d& t2 =
getWorld()->getTransform(frame_id, frame_found);
1914 if (!frame_id.empty() && frame_id[0] ==
'/')
1919 if (
getWorld()->knowsTransform(frame_id))
1928 if (object_types_->find(object_id) != object_types_->end())
1932 return parent_->hasObjectType(object_id);
1940 ObjectTypeMap::const_iterator it = object_types_->find(object_id);
1941 if (it != object_types_->end())
1945 return parent_->getObjectType(object_id);
1946 static const object_recognition_msgs::msg::ObjectType EMPTY;
1953 object_types_ = std::make_unique<ObjectTypeMap>();
1954 (*object_types_)[object_id] = type;
1960 object_types_->erase(object_id);
1967 parent_->getKnownObjectTypes(kc);
1970 for (ObjectTypeMap::const_iterator it = object_types_->begin(); it != object_types_->end(); ++it)
1971 kc[it->first] = it->second;
1979 if (object_colors_->find(object_id) != object_colors_->end())
1983 return parent_->hasObjectColor(object_id);
1991 ObjectColorMap::const_iterator it = object_colors_->find(object_id);
1992 if (it != object_colors_->end())
1996 return parent_->getObjectColor(object_id);
1997 static const std_msgs::msg::ColorRGBA EMPTY;
2003 if (original_object_colors_)
2005 ObjectColorMap::const_iterator it = original_object_colors_->find(object_id);
2006 if (it != original_object_colors_->end())
2009 return std::nullopt;
2016 parent_->getKnownObjectColors(kc);
2019 for (ObjectColorMap::const_iterator it = object_colors_->begin(); it != object_colors_->end(); ++it)
2020 kc[it->first] = it->second;
2026 if (object_id.empty())
2028 RCLCPP_ERROR(
getLogger(),
"Cannot set color of object with empty object_id.");
2031 if (!object_colors_)
2032 object_colors_ = std::make_unique<ObjectColorMap>();
2033 (*object_colors_)[object_id] = color;
2036 if (!original_object_colors_)
2037 original_object_colors_ = std::make_unique<ObjectColorMap>();
2039 (*original_object_colors_)[object_id] = color;
2045 object_colors_->erase(object_id);
2080 if (state_feasibility_)
2084 return state_feasibility_(s, verbose);
2091 if (state_feasibility_)
2092 return state_feasibility_(state, verbose);
2097 const moveit_msgs::msg::Constraints& constr,
bool verbose)
const
2105 const moveit_msgs::msg::Constraints& constr,
bool verbose)
const
2107 kinematic_constraints::KinematicConstraintSetPtr ks(
2136 static const moveit_msgs::msg::Constraints EMP_CONSTRAINTS;
2137 return isStateValid(state, EMP_CONSTRAINTS, group, verbose);
2142 static const moveit_msgs::msg::Constraints EMP_CONSTRAINTS;
2143 return isStateValid(state, EMP_CONSTRAINTS, group, verbose);
2147 const std::string& group,
bool verbose)
const
2155 const std::string& group,
bool verbose)
const
2176 const moveit_msgs::msg::RobotTrajectory& trajectory,
const std::string& group,
2177 bool verbose, std::vector<std::size_t>* invalid_index)
const
2179 static const moveit_msgs::msg::Constraints EMP_CONSTRAINTS;
2180 static const std::vector<moveit_msgs::msg::Constraints> EMP_CONSTRAINTS_VECTOR;
2181 return isPathValid(start_state, trajectory, EMP_CONSTRAINTS, EMP_CONSTRAINTS_VECTOR, group, verbose, invalid_index);
2185 const moveit_msgs::msg::RobotTrajectory& trajectory,
2186 const moveit_msgs::msg::Constraints& path_constraints,
const std::string& group,
2187 bool verbose, std::vector<std::size_t>* invalid_index)
const
2189 static const std::vector<moveit_msgs::msg::Constraints> EMP_CONSTRAINTS_VECTOR;
2190 return isPathValid(start_state, trajectory, path_constraints, EMP_CONSTRAINTS_VECTOR, group, verbose, invalid_index);
2194 const moveit_msgs::msg::RobotTrajectory& trajectory,
2195 const moveit_msgs::msg::Constraints& path_constraints,
2196 const moveit_msgs::msg::Constraints& goal_constraints,
const std::string& group,
2197 bool verbose, std::vector<std::size_t>* invalid_index)
const
2199 std::vector<moveit_msgs::msg::Constraints> goal_constraints_vector(1, goal_constraints);
2200 return isPathValid(start_state, trajectory, path_constraints, goal_constraints_vector, group, verbose, invalid_index);
2204 const moveit_msgs::msg::RobotTrajectory& trajectory,
2205 const moveit_msgs::msg::Constraints& path_constraints,
2206 const std::vector<moveit_msgs::msg::Constraints>& goal_constraints,
2207 const std::string& group,
bool verbose, std::vector<std::size_t>* invalid_index)
const
2213 return isPathValid(t, path_constraints, goal_constraints, group, verbose, invalid_index);
2217 const moveit_msgs::msg::Constraints& path_constraints,
2218 const std::vector<moveit_msgs::msg::Constraints>& goal_constraints,
2219 const std::string& group,
bool verbose, std::vector<std::size_t>* invalid_index)
const
2223 invalid_index->clear();
2227 for (std::size_t i = 0; i < n_wp; ++i)
2231 bool this_state_valid =
true;
2233 this_state_valid =
false;
2235 this_state_valid =
false;
2237 this_state_valid =
false;
2239 if (!this_state_valid)
2243 invalid_index->push_back(i);
2253 if (i + 1 == n_wp && !goal_constraints.empty())
2256 for (
const moveit_msgs::msg::Constraints& goal_constraint : goal_constraints)
2267 RCLCPP_INFO(
getLogger(),
"Goal not satisfied");
2269 invalid_index->push_back(i);
2278 const moveit_msgs::msg::Constraints& path_constraints,
2279 const moveit_msgs::msg::Constraints& goal_constraints,
const std::string& group,
2280 bool verbose, std::vector<std::size_t>* invalid_index)
const
2282 std::vector<moveit_msgs::msg::Constraints> goal_constraints_vector(1, goal_constraints);
2283 return isPathValid(trajectory, path_constraints, goal_constraints_vector, group, verbose, invalid_index);
2287 const moveit_msgs::msg::Constraints& path_constraints,
const std::string& group,
2288 bool verbose, std::vector<std::size_t>* invalid_index)
const
2290 static const std::vector<moveit_msgs::msg::Constraints> EMP_CONSTRAINTS_VECTOR;
2291 return isPathValid(trajectory, path_constraints, EMP_CONSTRAINTS_VECTOR, group, verbose, invalid_index);
2295 bool verbose, std::vector<std::size_t>* invalid_index)
const
2297 static const moveit_msgs::msg::Constraints EMP_CONSTRAINTS;
2298 static const std::vector<moveit_msgs::msg::Constraints> EMP_CONSTRAINTS_VECTOR;
2299 return isPathValid(trajectory, EMP_CONSTRAINTS, EMP_CONSTRAINTS_VECTOR, group, verbose, invalid_index);
2303 std::set<collision_detection::CostSource>& costs,
double overlap_fraction)
const
2305 getCostSources(trajectory, max_costs, std::string(), costs, overlap_fraction);
2309 const std::string& group_name, std::set<collision_detection::CostSource>& costs,
2310 double overlap_fraction)
const
2316 std::set<collision_detection::CostSource> cs;
2317 std::set<collision_detection::CostSource> cs_start;
2319 for (std::size_t i = 0; i < n_wp; ++i)
2328 if (cs.size() <= max_costs)
2336 for (std::set<collision_detection::CostSource>::iterator it = cs.begin(); i < max_costs; ++it, ++i)
2345 std::set<collision_detection::CostSource>& costs)
const
2351 const std::string& group_name,
2352 std::set<collision_detection::CostSource>& costs)
const
2365 const std::vector<std::string>& objects =
getWorld()->getObjectIds();
2366 std::vector<const moveit::core::AttachedBody*> attached_bodies;
2370 out <<
"-----------------------------------------\n";
2371 out <<
"PlanningScene Known Objects:\n";
2372 out <<
" - Collision World Objects:\n ";
2373 for (
const std::string&
object : objects)
2375 out <<
"\t- " <<
object <<
'\n';
2378 out <<
" - Attached Bodies:\n";
2381 out <<
"\t- " << attached_body->getName() <<
'\n';
2383 out <<
"-----------------------------------------\n";
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
void getMessage(moveit_msgs::msg::AllowedCollisionMatrix &msg) const
Get the allowed collision matrix as a message.
static CollisionDetectorAllocatorPtr create()
World::ObjectConstPtr ObjectConstPtr
std::function< void(const ObjectConstPtr &, Action)> ObserverCallbackFn
A class that contains many different constraints, and can check RobotState *versus the full set....
bool empty() const
Returns whether or not there are any constraints in the set.
bool add(const moveit_msgs::msg::Constraints &c, const moveit::core::Transforms &tf)
Add all known constraints.
ConstraintEvaluationResult decide(const moveit::core::RobotState &state, bool verbose=false) const
Determines whether all constraints are satisfied by state, returning a single evaluation result.
This may be thrown during construction of objects if errors occur.
Object defining bodies that can be attached to robot links.
const std::string & getAttachedLinkName() const
Get the name of the link this body is attached to.
const std::set< std::string > & getTouchLinks() const
Get the links that the attached body is allowed to touch.
const trajectory_msgs::msg::JointTrajectory & getDetachPosture() const
Return the posture that is necessary for the object to be released, (if any). This is useful for exam...
const moveit::core::FixedTransformsMap & getSubframes() const
Get subframes of this object (relative to the object pose). The returned transforms are guaranteed to...
const std::vector< shapes::ShapeConstPtr > & getShapes() const
Get the shapes that make up this attached body.
const Eigen::Isometry3d & getPose() const
Get the pose of the attached body relative to the parent link.
const EigenSTL::vector_Isometry3d & getShapePoses() const
Get the shape poses (the transforms to the shapes of this body, relative to the pose)....
A link from the robot. Contains the constant transform applied to the link and its geometry.
Representation of a robot's state. This includes position, velocity, acceleration and effort.
void getAttachedBodies(std::vector< const AttachedBody * > &attached_bodies) const
Get all bodies attached to the model corresponding to this state.
const Eigen::Isometry3d & getFrameTransform(const std::string &frame_id, bool *frame_found=nullptr)
Get the transformation matrix from the model frame (root of model) to the frame identified by frame_i...
void update(bool force=false)
Update all transforms.
bool knowsFrameTransform(const std::string &frame_id) const
Check if a transformation matrix from the model frame (root of model) to frame frame_id is known.
This class maintains the representation of the environment as seen by a planning instance....
void getAttachedCollisionObjectMsgs(std::vector< moveit_msgs::msg::AttachedCollisionObject > &attached_collision_objs) const
Construct a vector of messages (attached_collision_objects) with the attached collision object data f...
std::optional< std_msgs::msg::ColorRGBA > getOriginalObjectColor(const std::string &id) const
Tries to get the original color of an object, if one has been set before.
bool usePlanningSceneMsg(const moveit_msgs::msg::PlanningScene &scene)
Call setPlanningSceneMsg() or setPlanningSceneDiffMsg() depending on how the is_diff member of the me...
void getPlanningSceneMsg(moveit_msgs::msg::PlanningScene &scene) const
Construct a message (scene) with all the necessary data so that the scene can be later reconstructed ...
collision_detection::AllowedCollisionMatrix & getAllowedCollisionMatrixNonConst()
Get the allowed collision matrix.
bool isStateFeasible(const moveit_msgs::msg::RobotState &state, bool verbose=false) const
Check if a given state is feasible, in accordance to the feasibility predicate specified by setStateF...
void setAllowedCollisionMatrix(const collision_detection::AllowedCollisionMatrix &acm)
Set the allowed collision matrix.
static PlanningScenePtr clone(const PlanningSceneConstPtr &scene)
Clone a planning scene. Even if the scene scene depends on a parent, the cloned scene will not.
bool processCollisionObjectMsg(const moveit_msgs::msg::CollisionObject &object)
void setCurrentState(const moveit_msgs::msg::RobotState &state)
Set the current robot state to be state. If not all joint values are specified, the previously mainta...
moveit::core::RobotStatePtr getCurrentStateUpdated(const moveit_msgs::msg::RobotState &update) const
Get a copy of the current state with components overwritten by the state message update.
const std::string & getName() const
Get the name of the planning scene. This is empty by default.
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,...
const object_recognition_msgs::msg::ObjectType & getObjectType(const std::string &id) const
void getObjectColorMsgs(std::vector< moveit_msgs::msg::ObjectColor > &object_colors) const
Construct a vector of messages (object_colors) with the colors of the objects from the planning_scene...
const collision_detection::CollisionEnvPtr & getCollisionEnvNonConst()
Get the representation of the collision robot This can be used to set padding and link scale on the a...
bool getCollisionObjectMsg(moveit_msgs::msg::CollisionObject &collision_obj, const std::string &ns) const
Construct a message (collision_object) with the collision object data from the planning_scene for the...
const std::string & getPlanningFrame() const
Get the frame in which planning is performed.
bool isPathValid(const moveit_msgs::msg::RobotState &start_state, const moveit_msgs::msg::RobotTrajectory &trajectory, const std::string &group="", bool verbose=false, std::vector< std::size_t > *invalid_index=nullptr) const
Check if a given path is valid. Each state is checked for validity (collision avoidance and feasibili...
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...
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 getCollisionObjectMsgs(std::vector< moveit_msgs::msg::CollisionObject > &collision_objs) const
Construct a vector of messages (collision_objects) with the collision object data for all objects in ...
moveit::core::RobotState & getCurrentStateNonConst()
Get the state at which the robot is assumed to be.
bool isStateValid(const moveit_msgs::msg::RobotState &state, const std::string &group="", bool verbose=false) const
Check if a given state is valid. This means checking for collisions and feasibility....
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...
void processOctomapMsg(const octomap_msgs::msg::OctomapWithPose &map)
void getPlanningSceneDiffMsg(moveit_msgs::msg::PlanningScene &scene) const
Fill the message scene with the differences between this instance of PlanningScene with respect to th...
void getKnownObjectColors(ObjectColorMap &kc) const
bool loadGeometryFromStream(std::istream &in)
Load the geometry of the planning scene from a stream.
PlanningScenePtr diff() const
Return a new child PlanningScene that uses this one as parent.
const collision_detection::CollisionEnvConstPtr & getCollisionEnvUnpadded() const
Get the active collision detector for the robot.
bool setPlanningSceneDiffMsg(const moveit_msgs::msg::PlanningScene &scene)
Apply changes to this planning scene as diffs, even if the message itself is not marked as being a di...
bool processAttachedCollisionObjectMsg(const moveit_msgs::msg::AttachedCollisionObject &object)
bool processPlanningSceneWorldMsg(const moveit_msgs::msg::PlanningSceneWorld &world)
void setObjectColor(const std::string &id, const std_msgs::msg::ColorRGBA &color)
const collision_detection::AllowedCollisionMatrix & getAllowedCollisionMatrix() const
Get the allowed collision matrix.
void getKnownObjectTypes(ObjectTypeMap &kc) const
PlanningScene(const PlanningScene &)=delete
PlanningScene cannot be copy-constructed.
bool hasObjectColor(const std::string &id) const
bool getOctomapMsg(octomap_msgs::msg::OctomapWithPose &octomap) const
Construct a message (octomap) with the octomap data from the planning_scene.
void clearDiffs()
Clear the diffs accumulated for this planning scene, with respect to: the parent PlanningScene (if it...
void setAttachedBodyUpdateCallback(const moveit::core::AttachedBodyCallback &callback)
Set the callback to be triggered when changes are made to the current scene state.
void setObjectType(const std::string &id, const object_recognition_msgs::msg::ObjectType &type)
void printKnownObjects(std::ostream &out=std::cout) const
Outputs debug information about the planning scene contents.
const std::string getCollisionDetectorName() const
void pushDiffs(const PlanningScenePtr &scene)
If there is a parent specified for this scene, then the diffs with respect to that parent are applied...
bool shapesAndPosesFromCollisionObjectMessage(const moveit_msgs::msg::CollisionObject &object, Eigen::Isometry3d &object_pose_in_header_frame, std::vector< shapes::ShapeConstPtr > &shapes, EigenSTL::vector_Isometry3d &shape_poses)
Takes the object message and returns the object pose, shapes and shape poses. If the object pose is e...
bool hasObjectType(const std::string &id) const
void decoupleParent()
Make sure that all the data maintained in this scene is local. All unmodified data is copied from the...
void allocateCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr &allocator)
Allocate a new collision detector and replace the previous one if there was any.
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...
const moveit::core::RobotState & getCurrentState() const
Get the state at which the robot is assumed to be.
void removeAllCollisionObjects()
Clear all collision objects in planning scene.
const collision_detection::WorldConstPtr & getWorld() const
Get the representation of 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)....
const moveit::core::Transforms & getTransforms() const
Get the set of fixed transforms from known frames to the planning frame.
bool isStateConstrained(const moveit_msgs::msg::RobotState &state, const moveit_msgs::msg::Constraints &constr, bool verbose=false) const
Check if a given state satisfies a set of constraints.
bool setPlanningSceneMsg(const moveit_msgs::msg::PlanningScene &scene)
Set this instance of a planning scene to be the same as the one serialized in the scene message,...
bool getAttachedCollisionObjectMsg(moveit_msgs::msg::AttachedCollisionObject &attached_collision_obj, const std::string &ns) const
Construct a message (attached_collision_object) with the attached collision object data from the plan...
void saveGeometryToStream(std::ostream &out) const
Save the geometry of the planning scene to a stream, as plain text.
void getCollidingLinks(std::vector< std::string > &links)
Get the names of the links that are involved in collisions for the current state.
const moveit::core::RobotModelConstPtr & getRobotModel() const
Get the kinematic model for which the planning scene is maintained.
moveit::core::Transforms & getTransformsNonConst()
Get the set of fixed transforms from known frames to the planning frame.
void checkSelfCollision(const collision_detection::CollisionRequest &req, collision_detection::CollisionResult &res)
Check whether the current state is in self collision.
const collision_detection::CollisionEnvConstPtr & getCollisionEnv() const
Get the active collision environment.
void removeObjectColor(const std::string &id)
static const std::string OCTOMAP_NS
static const std::string DEFAULT_SCENE_NAME
void setCollisionObjectUpdateCallback(const collision_detection::World::ObserverCallbackFn &callback)
Set the callback to be triggered when changes are made to the current scene world.
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...
const std_msgs::msg::ColorRGBA & getObjectColor(const std::string &id) const
Gets the current color of an object.
friend struct CollisionDetector
Enumerates the available collision detectors.
void processOctomapPtr(const std::shared_ptr< const octomap::OcTree > &octree, const Eigen::Isometry3d &t)
void removeObjectType(const std::string &id)
Maintain a sequence of waypoints and the time durations between these waypoints.
RobotTrajectory & setRobotTrajectoryMsg(const moveit::core::RobotState &reference_state, const trajectory_msgs::msg::JointTrajectory &trajectory)
Copy the content of the trajectory message into this class. The trajectory message itself is not requ...
std::size_t getWayPointCount() const
const moveit::core::RobotState & getWayPoint(std::size_t index) const
@ ROBOT_LINK
A link on the robot.
rclcpp::Logger getLogger()
std::shared_ptr< OccMapTree > OccMapTreePtr
void removeCostSources(std::set< CostSource > &cost_sources, const std::set< CostSource > &cost_sources_to_remove, double overlap_fraction)
void removeOverlapping(std::set< CostSource > &cost_sources, double overlap_fraction)
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::msg::RobotState &robot_state, bool copy_attached_bodies=true)
Convert a MoveIt robot state to a robot state message.
bool isEmpty(const moveit_msgs::msg::PlanningScene &msg)
Check if a message includes any information about a planning scene, or whether it is empty.
void attachedBodiesToAttachedCollisionObjectMsgs(const std::vector< const AttachedBody * > &attached_bodies, std::vector< moveit_msgs::msg::AttachedCollisionObject > &attached_collision_objs)
Convert AttachedBodies to AttachedCollisionObjects.
std::map< std::string, Eigen::Isometry3d, std::less< std::string >, Eigen::aligned_allocator< std::pair< const std::string, Eigen::Isometry3d > > > FixedTransformsMap
Map frame names to the transformation matrix that can transform objects from the frame name to the pl...
std::function< void(AttachedBody *body, bool attached)> AttachedBodyCallback
bool robotStateMsgToRobotState(const Transforms &tf, const moveit_msgs::msg::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 update(moveit::core::RobotState *self, bool force, std::string &category)
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
void poseMsgToEigen(const geometry_msgs::msg::Pose &msg, Eigen::Isometry3d &out)
void writePoseToText(std::ostream &out, const Eigen::Isometry3d &pose)
Write a pose to text.
bool readPoseFromText(std::istream &in, Eigen::Isometry3d &pose)
Read a pose from text.
This namespace includes the central class for representing planning contexts.
std::map< std::string, std_msgs::msg::ColorRGBA > ObjectColorMap
A map from object names (e.g., attached bodies, collision objects) to their colors.
std::map< std::string, object_recognition_msgs::msg::ObjectType > ObjectTypeMap
A map from object names (e.g., attached bodies, collision objects) to their types.
collision_detection::OccMapTreePtr createOctomap(const octomap_msgs::msg::Octomap &map)
std::string append(const std::string &left, const std::string &right)
Representation of a collision checking request.
std::string group_name
The group name to check collisions for (optional; if empty, assume the complete robot)....
bool verbose
Flag indicating whether information about detected collisions should be reported.
std::size_t max_cost_sources
When costs are computed, this value defines how many of the top cost sources should be returned.
bool contacts
If true, compute contacts. Otherwise only a binary collision yes/no is reported.
bool cost
If true, a collision cost is computed.
std::size_t max_contacts
Overall maximum number of contacts to compute.
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...
Representation of a collision checking result.
std::map< std::pair< std::string, std::string >, std::vector< Contact > > ContactMap
A map returning the pairs of body ids in contact, plus their contact details.
std::set< CostSource > cost_sources
These are the individual cost sources when costs are computed.
bool collision
True if collision was found, false otherwise.
A representation of an object.
moveit::core::FixedTransformsMap subframe_poses_
Transforms from the object pose to subframes on the object. Use them to define points of interest on ...
Eigen::Isometry3d pose_
The object's pose. All shapes and subframes are defined relative to this frame. This frame is returne...
EIGEN_MAKE_ALIGNED_OPERATOR_NEW std::string id_
The id for this object.
EigenSTL::vector_Isometry3d shape_poses_
The poses of the corresponding entries in shapes_, relative to the object pose.
std::vector< shapes::ShapeConstPtr > shapes_
All the shapes making up this object.
bool satisfied
Whether or not the constraint or constraints were satisfied.