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>
57 static const rclcpp::Logger LOGGER = rclcpp::get_logger(
"moveit_planning_scene.planning_scene");
78 if (Transforms::isFixedFrame(frame))
81 return knowsObjectFrame(frame.substr(1));
83 return knowsObjectFrame(frame);
86 const Eigen::Isometry3d&
getTransform(
const std::string& from_frame)
const override
93 bool knowsObjectFrame(
const std::string&
frame_id)
const
98 const PlanningScene* scene_;
117 const collision_detection::WorldPtr& world)
119 : robot_model_(robot_model), world_(world), world_const_(world)
125 const srdf::ModelConstSharedPtr& srdf_model,
const collision_detection::WorldPtr& world)
126 : world_(world), world_const_(world)
134 robot_model_ = createRobotModel(urdf_model, srdf_model);
143 if (current_world_object_update_callback_)
144 world_->removeObserver(current_world_object_update_observer_handle_);
147 void PlanningScene::initialize()
151 scene_transforms_ = std::make_shared<SceneTransforms>(
this);
153 robot_state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
154 robot_state_->setToDefaultValues();
155 robot_state_->update();
157 acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(*
getRobotModel()->getSRDF());
163 moveit::core::RobotModelPtr PlanningScene::createRobotModel(
const urdf::ModelInterfaceSharedPtr& urdf_model,
164 const srdf::ModelConstSharedPtr& srdf_model)
166 auto robot_model = std::make_shared<moveit::core::RobotModel>(urdf_model, srdf_model);
167 if (!robot_model || !robot_model->getRootJoint())
168 return moveit::core::RobotModelPtr();
178 if (!parent_->getName().empty())
179 name_ = parent_->getName() +
"+";
181 robot_model_ = parent_->robot_model_;
185 world_ = std::make_shared<collision_detection::World>(*parent_->world_);
186 world_const_ = world_;
189 world_diff_ = std::make_shared<collision_detection::WorldDiff>(world_);
192 collision_detector_->copyPadding(*parent_->collision_detector_);
197 PlanningScenePtr result =
scene->diff();
198 result->decoupleParent();
199 result->setName(
scene->getName());
205 return PlanningScenePtr(
new PlanningScene(shared_from_this()));
210 PlanningScenePtr result =
diff();
211 result->setPlanningSceneDiffMsg(msg);
217 cenv_->setLinkPadding(src.getCollisionEnv()->getLinkPadding());
218 cenv_->setLinkScale(src.getCollisionEnv()->getLinkScale());
222 const CollisionDetectorPtr& parent_detector)
225 CollisionDetectorPtr prev_coll_detector = collision_detector_;
228 collision_detector_ = std::make_shared<CollisionDetector>();
229 collision_detector_->alloc_ = allocator;
235 collision_detector_->cenv_ = collision_detector_->alloc_->allocateEnv(parent_detector->cenv_, world_);
236 collision_detector_->cenv_unpadded_ =
237 collision_detector_->alloc_->allocateEnv(parent_detector->cenv_unpadded_, world_);
241 collision_detector_->cenv_ = collision_detector_->alloc_->allocateEnv(world_,
getRobotModel());
242 collision_detector_->cenv_unpadded_ = collision_detector_->alloc_->allocateEnv(world_,
getRobotModel());
245 if (prev_coll_detector)
246 collision_detector_->copyPadding(*prev_coll_detector);
250 collision_detector_->cenv_const_ = collision_detector_->cenv_;
251 collision_detector_->cenv_unpadded_const_ = collision_detector_->cenv_unpadded_;
254 const collision_detection::CollisionEnvConstPtr&
259 RCLCPP_ERROR(LOGGER,
"Could not get CollisionRobot named '%s'. Returning active CollisionRobot '%s' instead",
260 collision_detector_name.c_str(), collision_detector_->alloc_->getName().c_str());
261 return collision_detector_->getCollisionEnv();
264 return collision_detector_->getCollisionEnv();
267 const collision_detection::CollisionEnvConstPtr&
273 "Could not get CollisionRobotUnpadded named '%s'. "
274 "Returning active CollisionRobotUnpadded '%s' instead",
275 collision_detector_name.c_str(), collision_detector_->alloc_->getName().c_str());
276 return collision_detector_->getCollisionEnvUnpadded();
279 return collision_detector_->getCollisionEnvUnpadded();
288 world_ = std::make_shared<collision_detection::World>(*parent_->world_);
289 world_const_ = world_;
290 world_diff_ = std::make_shared<collision_detection::WorldDiff>(world_);
291 if (current_world_object_update_callback_)
292 current_world_object_update_observer_handle_ = world_->addObserver(current_world_object_update_callback_);
297 scene_transforms_.reset();
298 robot_state_.reset();
300 object_colors_.reset();
301 object_types_.reset();
309 if (scene_transforms_)
310 scene->getTransformsNonConst().setAllTransforms(scene_transforms_->getAllTransforms());
314 scene->getCurrentStateNonConst() = *robot_state_;
316 std::vector<const moveit::core::AttachedBody*> attached_objs;
317 robot_state_->getAttachedBodies(attached_objs);
321 scene->setObjectType(attached_obj->getName(),
getObjectType(attached_obj->getName()));
328 scene->getAllowedCollisionMatrixNonConst() = *acm_;
330 collision_detection::CollisionEnvPtr active_cenv =
scene->getCollisionEnvNonConst();
331 active_cenv->setLinkPadding(collision_detector_->cenv_->getLinkPadding());
332 active_cenv->setLinkScale(collision_detector_->cenv_->getLinkScale());
336 for (
const std::pair<const std::string, collision_detection::World::Action>& it : *world_diff_)
340 scene->world_->removeObject(it.first);
341 scene->removeObjectColor(it.first);
342 scene->removeObjectType(it.first);
344 if (!
scene->getCurrentState().hasAttachedBody(it.first))
346 scene->getAllowedCollisionMatrixNonConst().removeEntry(it.first);
352 scene->world_->removeObject(obj.
id_);
438 const std::string& group_name)
const
464 for (collision_detection::CollisionResult::ContactMap::const_iterator it = contacts.begin(); it != contacts.end();
477 return collision_detector_->cenv_;
484 robot_state_ = std::make_shared<moveit::core::RobotState>(parent_->getCurrentState());
485 robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
487 robot_state_->update();
488 return *robot_state_;
493 auto state = std::make_shared<moveit::core::RobotState>(
getCurrentState());
500 current_state_attached_body_callback_ = callback;
502 robot_state_->setAttachedBodyUpdateCallback(callback);
507 if (current_world_object_update_callback_)
508 world_->removeObserver(current_world_object_update_observer_handle_);
510 current_world_object_update_observer_handle_ = world_->addObserver(callback);
511 current_world_object_update_callback_ = callback;
517 acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(parent_->getAllowedCollisionMatrix());
532 if (!scene_transforms_)
536 scene_transforms_ = std::make_shared<SceneTransforms>(
this);
537 scene_transforms_->setAllTransforms(parent_->getTransforms().getAllTransforms());
539 return *scene_transforms_;
544 scene_msg.name = name_;
546 scene_msg.is_diff =
true;
548 if (scene_transforms_)
549 scene_transforms_->copyTransforms(scene_msg.fixed_frame_transforms);
551 scene_msg.fixed_frame_transforms.clear();
557 scene_msg.robot_state = moveit_msgs::msg::RobotState();
559 scene_msg.robot_state.is_diff =
true;
562 acm_->getMessage(scene_msg.allowed_collision_matrix);
564 scene_msg.allowed_collision_matrix = moveit_msgs::msg::AllowedCollisionMatrix();
566 collision_detector_->cenv_->getPadding(scene_msg.link_padding);
567 collision_detector_->cenv_->getScale(scene_msg.link_scale);
569 scene_msg.object_colors.clear();
573 scene_msg.object_colors.resize(object_colors_->size());
574 for (ObjectColorMap::const_iterator it = object_colors_->begin(); it != object_colors_->end(); ++it, ++i)
576 scene_msg.object_colors[i].id = it->first;
577 scene_msg.object_colors[i].color = it->second;
581 scene_msg.world.collision_objects.clear();
582 scene_msg.world.octomap = octomap_msgs::msg::OctomapWithPose();
586 bool do_omap =
false;
587 for (
const std::pair<const std::string, collision_detection::World::Action>& it : *world_diff_)
594 if (!std::count_if(scene_msg.robot_state.attached_collision_objects.cbegin(),
595 scene_msg.robot_state.attached_collision_objects.cend(),
596 [&it](
const moveit_msgs::msg::AttachedCollisionObject& aco) {
597 return aco.object.id == it.first &&
598 aco.object.operation == moveit_msgs::msg::CollisionObject::ADD;
601 moveit_msgs::msg::CollisionObject co;
604 co.operation = moveit_msgs::msg::CollisionObject::REMOVE;
605 scene_msg.world.collision_objects.push_back(co);
610 scene_msg.world.collision_objects.emplace_back();
621 for (
const auto& collision_object : scene_msg.world.collision_objects)
623 if (parent_ && parent_->getCurrentState().hasAttachedBody(collision_object.id))
625 moveit_msgs::msg::AttachedCollisionObject aco;
626 aco.object.id = collision_object.id;
627 aco.object.operation = moveit_msgs::msg::CollisionObject::REMOVE;
628 scene_msg.robot_state.attached_collision_objects.push_back(aco);
635 class ShapeVisitorAddToCollisionObject :
public boost::static_visitor<void>
638 ShapeVisitorAddToCollisionObject(moveit_msgs::msg::CollisionObject* obj) :
boost::static_visitor<void>(), obj_(obj)
642 void setPoseMessage(
const geometry_msgs::msg::Pose* pose)
647 void operator()(
const shape_msgs::msg::Plane& shape_msg)
const
649 obj_->planes.push_back(shape_msg);
650 obj_->plane_poses.push_back(*pose_);
653 void operator()(
const shape_msgs::msg::Mesh& shape_msg)
const
655 obj_->meshes.push_back(shape_msg);
656 obj_->mesh_poses.push_back(*pose_);
659 void operator()(
const shape_msgs::msg::SolidPrimitive& shape_msg)
const
661 obj_->primitives.push_back(shape_msg);
662 obj_->primitive_poses.push_back(*pose_);
666 moveit_msgs::msg::CollisionObject* obj_;
667 const geometry_msgs::msg::Pose* pose_;
677 collision_obj.pose = tf2::toMsg(obj->pose_);
678 collision_obj.id = ns;
679 collision_obj.operation = moveit_msgs::msg::CollisionObject::ADD;
681 ShapeVisitorAddToCollisionObject sv(&collision_obj);
682 for (std::size_t j = 0; j < obj->shapes_.size(); ++j)
685 if (constructMsgFromShape(obj->shapes_[j].get(), sm))
687 geometry_msgs::msg::Pose
p = tf2::toMsg(obj->shape_poses_[j]);
688 sv.setPoseMessage(&
p);
689 boost::apply_visitor(sv, sm);
693 if (!collision_obj.primitives.empty() || !collision_obj.meshes.empty() || !collision_obj.planes.empty())
698 for (
const auto& frame_pair : obj->subframe_poses_)
700 collision_obj.subframe_names.push_back(frame_pair.first);
701 geometry_msgs::msg::Pose
p;
702 p = tf2::toMsg(frame_pair.second);
703 collision_obj.subframe_poses.push_back(
p);
711 collision_objs.clear();
712 const std::vector<std::string>& ids = world_->getObjectIds();
713 for (
const std::string&
id : ids)
716 collision_objs.emplace_back();
722 const std::string& ns)
const
724 std::vector<moveit_msgs::msg::AttachedCollisionObject> attached_collision_objs;
726 for (moveit_msgs::msg::AttachedCollisionObject& it : attached_collision_objs)
728 if (it.object.id == ns)
730 attached_collision_obj = it;
738 std::vector<moveit_msgs::msg::AttachedCollisionObject>& attached_collision_objs)
const
740 std::vector<const moveit::core::AttachedBody*> attached_bodies;
748 octomap.octomap = octomap_msgs::msg::Octomap();
753 if (map->shapes_.size() == 1)
755 const shapes::OcTree* o =
static_cast<const shapes::OcTree*
>(map->shapes_[0].get());
756 octomap_msgs::fullMapToMsg(*o->octree,
octomap.octomap);
757 octomap.origin = tf2::toMsg(map->shape_poses_[0]);
760 RCLCPP_ERROR(LOGGER,
"Unexpected number of shapes in octomap collision object. Not including '%s' object",
768 object_colors.clear();
773 object_colors.resize(cmap.size());
774 for (ObjectColorMap::const_iterator it = cmap.begin(); it != cmap.end(); ++it, ++i)
776 object_colors[i].id = it->first;
777 object_colors[i].color = it->second;
783 scene_msg.name = name_;
784 scene_msg.is_diff =
false;
803 const moveit_msgs::msg::PlanningSceneComponents& comp)
const
805 scene_msg.is_diff =
false;
806 if (comp.components & moveit_msgs::msg::PlanningSceneComponents::SCENE_SETTINGS)
808 scene_msg.name = name_;
812 if (comp.components & moveit_msgs::msg::PlanningSceneComponents::TRANSFORMS)
815 if (comp.components & moveit_msgs::msg::PlanningSceneComponents::ROBOT_STATE_ATTACHED_OBJECTS)
818 for (moveit_msgs::msg::AttachedCollisionObject& attached_collision_object :
819 scene_msg.robot_state.attached_collision_objects)
823 attached_collision_object.object.type =
getObjectType(attached_collision_object.object.id);
827 else if (comp.components & moveit_msgs::msg::PlanningSceneComponents::ROBOT_STATE)
832 if (comp.components & moveit_msgs::msg::PlanningSceneComponents::ALLOWED_COLLISION_MATRIX)
835 if (comp.components & moveit_msgs::msg::PlanningSceneComponents::LINK_PADDING_AND_SCALING)
841 if (comp.components & moveit_msgs::msg::PlanningSceneComponents::OBJECT_COLORS)
845 if (comp.components & moveit_msgs::msg::PlanningSceneComponents::WORLD_OBJECT_GEOMETRY)
847 else if (comp.components & moveit_msgs::msg::PlanningSceneComponents::WORLD_OBJECT_NAMES)
849 const std::vector<std::string>& ids = world_->getObjectIds();
850 scene_msg.world.collision_objects.clear();
851 scene_msg.world.collision_objects.reserve(ids.size());
852 for (
const std::string&
id : ids)
855 moveit_msgs::msg::CollisionObject co;
859 scene_msg.world.collision_objects.push_back(co);
864 if (comp.components & moveit_msgs::msg::PlanningSceneComponents::OCTOMAP)
870 out << name_ <<
'\n';
871 const std::vector<std::string>& ids = world_->getObjectIds();
872 for (
const std::string&
id : ids)
878 out <<
"* " <<
id <<
'\n';
880 writePoseToText(out, obj->pose_);
883 out << obj->shapes_.size() <<
'\n';
884 for (std::size_t j = 0; j < obj->shapes_.size(); ++j)
886 shapes::saveAsText(obj->shapes_[j].get(), out);
888 writePoseToText(out, obj->shape_poses_[j]);
892 out <<
c.r <<
" " <<
c.g <<
" " <<
c.b <<
" " <<
c.a <<
'\n';
895 out <<
"0 0 0 0" <<
'\n';
899 out << obj->subframe_poses_.size() <<
'\n';
900 for (
auto& pose_pair : obj->subframe_poses_)
902 out << pose_pair.first <<
'\n';
903 writePoseToText(out, pose_pair.second);
917 if (!in.good() || in.eof())
919 RCLCPP_ERROR(LOGGER,
"Bad input stream when loading scene geometry");
923 std::getline(in, name_);
926 auto pos = in.tellg();
930 std::getline(in, line);
931 }
while (in.good() && !in.eof() && (line.empty() || line[0] !=
'*'));
932 std::getline(in, line);
933 boost::algorithm::trim(line);
936 bool uses_new_scene_format = line.find(
' ') != std::string::npos;
939 Eigen::Isometry3d pose;
944 if (!in.good() || in.eof())
946 RCLCPP_ERROR(LOGGER,
"Bad input stream when loading marker in scene geometry");
951 std::string object_id;
952 std::getline(in, object_id);
953 if (!in.good() || in.eof())
955 RCLCPP_ERROR(LOGGER,
"Bad input stream when loading object_id in scene geometry");
958 boost::algorithm::trim(object_id);
962 if (uses_new_scene_format && !readPoseFromText(in, pose))
964 RCLCPP_ERROR(LOGGER,
"Failed to read object pose from scene file");
967 pose = offset * pose;
968 world_->setObjectPose(object_id, pose);
971 unsigned int shape_count;
973 for (std::size_t i = 0; i < shape_count && in.good() && !in.eof(); ++i)
975 const auto shape = shapes::ShapeConstPtr(shapes::constructShapeFromText(in));
978 RCLCPP_ERROR(LOGGER,
"Failed to load shape from scene file");
981 if (!readPoseFromText(in, pose))
983 RCLCPP_ERROR(LOGGER,
"Failed to read pose from scene file");
987 if (!(in >>
r >> g >> b >>
a))
989 RCLCPP_ERROR(LOGGER,
"Improperly formatted color in scene geometry file");
994 world_->addToObject(object_id, shape, pose);
995 if (
r > 0.0f || g > 0.0f || b > 0.0f ||
a > 0.0f)
997 std_msgs::msg::ColorRGBA color;
1008 if (uses_new_scene_format)
1011 unsigned int subframe_count;
1012 in >> subframe_count;
1013 for (std::size_t i = 0; i < subframe_count && in.good() && !in.eof(); ++i)
1015 std::string subframe_name;
1016 in >> subframe_name;
1017 if (!readPoseFromText(in, pose))
1019 RCLCPP_ERROR(LOGGER,
"Failed to read subframe pose from scene file");
1022 subframes[subframe_name] = pose;
1024 world_->setSubframesOfObject(object_id, subframes);
1027 else if (marker ==
".")
1034 RCLCPP_ERROR(LOGGER,
"Unknown marker in scene geometry file: %s ", marker.c_str());
1040 bool PlanningScene::readPoseFromText(std::istream& in, Eigen::Isometry3d& pose)
const
1042 double x,
y,
z, rx, ry, rz, rw;
1043 if (!(in >>
x >>
y >>
z))
1045 RCLCPP_ERROR(LOGGER,
"Improperly formatted translation in scene geometry file");
1048 if (!(in >> rx >> ry >> rz >> rw))
1050 RCLCPP_ERROR(LOGGER,
"Improperly formatted rotation in scene geometry file");
1053 pose = Eigen::Translation3d(
x,
y,
z) * Eigen::Quaterniond(rw, rx, ry, rz);
1057 void PlanningScene::writePoseToText(std::ostream& out,
const Eigen::Isometry3d& pose)
const
1059 out << pose.translation().x() <<
" " << pose.translation().y() <<
" " << pose.translation().z() <<
'\n';
1060 Eigen::Quaterniond
r(pose.linear());
1061 out <<
r.x() <<
" " <<
r.y() <<
" " <<
r.z() <<
" " <<
r.w() <<
'\n';
1068 moveit_msgs::msg::RobotState state_no_attached(state);
1069 state_no_attached.attached_collision_objects.clear();
1075 robot_state_ = std::make_shared<moveit::core::RobotState>(parent_->getCurrentState());
1076 robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
1083 for (std::size_t i = 0; i < state.attached_collision_objects.size(); ++i)
1085 if (!state.is_diff && state.attached_collision_objects[i].object.operation != moveit_msgs::msg::CollisionObject::ADD)
1087 RCLCPP_ERROR(LOGGER,
1088 "The specified RobotState is not marked as is_diff. "
1089 "The request to modify the object '%s' is not supported. Object is ignored.",
1090 state.attached_collision_objects[i].object.id.c_str());
1108 if (!scene_transforms_)
1110 scene_transforms_ = std::make_shared<SceneTransforms>(
this);
1111 scene_transforms_->setAllTransforms(parent_->getTransforms().getAllTransforms());
1116 robot_state_ = std::make_shared<moveit::core::RobotState>(parent_->getCurrentState());
1117 robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
1121 acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(parent_->getAllowedCollisionMatrix());
1123 world_diff_.reset();
1125 if (!object_colors_)
1128 parent_->getKnownObjectColors(kc);
1129 object_colors_ = std::make_unique<ObjectColorMap>(kc);
1134 parent_->getKnownObjectColors(kc);
1135 for (ObjectColorMap::const_iterator it = kc.begin(); it != kc.end(); ++it)
1136 if (object_colors_->find(it->first) == object_colors_->end())
1137 (*object_colors_)[it->first] = it->second;
1143 parent_->getKnownObjectTypes(kc);
1144 object_types_ = std::make_unique<ObjectTypeMap>(kc);
1149 parent_->getKnownObjectTypes(kc);
1150 for (ObjectTypeMap::const_iterator it = kc.begin(); it != kc.end(); ++it)
1151 if (object_types_->find(it->first) == object_types_->end())
1152 (*object_types_)[it->first] = it->second;
1162 RCLCPP_DEBUG(LOGGER,
"Adding planning scene diff");
1163 if (!scene_msg.name.empty())
1164 name_ = scene_msg.name;
1166 if (!scene_msg.robot_model_name.empty() && scene_msg.robot_model_name !=
getRobotModel()->
getName())
1167 RCLCPP_WARN(LOGGER,
"Setting the scene for model '%s' but model '%s' is loaded.",
1172 if (!scene_msg.fixed_frame_transforms.empty())
1174 if (!scene_transforms_)
1175 scene_transforms_ = std::make_shared<SceneTransforms>(
this);
1176 scene_transforms_->setTransforms(scene_msg.fixed_frame_transforms);
1180 if (!scene_msg.robot_state.multi_dof_joint_state.joint_names.empty() ||
1181 !scene_msg.robot_state.joint_state.name.empty() || !scene_msg.robot_state.attached_collision_objects.empty())
1185 if (!scene_msg.allowed_collision_matrix.entry_names.empty())
1186 acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(scene_msg.allowed_collision_matrix);
1188 if (!scene_msg.link_padding.empty() || !scene_msg.link_scale.empty())
1190 collision_detector_->cenv_->setPadding(scene_msg.link_padding);
1191 collision_detector_->cenv_->setScale(scene_msg.link_scale);
1195 for (
const moveit_msgs::msg::ObjectColor& object_color : scene_msg.object_colors)
1199 for (
const moveit_msgs::msg::CollisionObject& collision_object : scene_msg.world.collision_objects)
1203 if (!scene_msg.world.octomap.octomap.data.empty())
1211 RCLCPP_DEBUG(LOGGER,
"Setting new planning scene: '%s'", scene_msg.name.c_str());
1212 name_ = scene_msg.name;
1214 if (!scene_msg.robot_model_name.empty() && scene_msg.robot_model_name !=
getRobotModel()->
getName())
1215 RCLCPP_WARN(LOGGER,
"Setting the scene for model '%s' but model '%s' is loaded.",
1221 object_types_.reset();
1222 scene_transforms_->setTransforms(scene_msg.fixed_frame_transforms);
1224 acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(scene_msg.allowed_collision_matrix);
1225 collision_detector_->cenv_->setPadding(scene_msg.link_padding);
1226 collision_detector_->cenv_->setScale(scene_msg.link_scale);
1227 object_colors_ = std::make_unique<ObjectColorMap>();
1228 for (
const moveit_msgs::msg::ObjectColor& object_color : scene_msg.object_colors)
1230 world_->clearObjects();
1237 for (
const moveit_msgs::msg::CollisionObject& collision_object : world.collision_objects)
1245 if (scene_msg.is_diff)
1253 std::shared_ptr<collision_detection::OccMapTree> om =
1254 std::make_shared<collision_detection::OccMapTree>(map.resolution);
1257 octomap_msgs::readTree(om.get(), map);
1261 std::stringstream datastream;
1262 if (!map.data.empty())
1264 datastream.write((
const char*)&map.data[0], map.data.size());
1265 om->readData(datastream);
1276 if (map.data.empty())
1279 if (map.id !=
"OcTree")
1281 RCLCPP_ERROR(LOGGER,
"Received octomap is of type '%s' but type 'OcTree' is expected.", map.id.c_str());
1285 std::shared_ptr<collision_detection::OccMapTree> om =
createOctomap(map);
1286 if (!map.header.frame_id.empty())
1289 world_->addToObject(
OCTOMAP_NS, std::make_shared<const shapes::OcTree>(om), t);
1293 world_->addToObject(
OCTOMAP_NS, std::make_shared<const shapes::OcTree>(om), Eigen::Isometry3d::Identity());
1299 const std::vector<std::string>& object_ids = world_->getObjectIds();
1300 for (
const std::string& object_id : object_ids)
1303 world_->removeObject(object_id);
1315 if (map.octomap.data.empty())
1318 if (map.octomap.id !=
"OcTree")
1320 RCLCPP_ERROR(LOGGER,
"Received octomap is of type '%s' but type 'OcTree' is expected.", map.octomap.id.c_str());
1324 std::shared_ptr<collision_detection::OccMapTree> om =
createOctomap(map.octomap);
1327 Eigen::Isometry3d
p;
1328 PlanningScene::poseMsgToEigen(map.origin,
p);
1330 world_->addToObject(
OCTOMAP_NS, std::make_shared<const shapes::OcTree>(om),
p);
1338 if (map->shapes_.size() == 1)
1341 const shapes::OcTree* o =
static_cast<const shapes::OcTree*
>(map->shapes_[0].get());
1342 if (o->octree == octree)
1345 if (map->shape_poses_[0].isApprox(t, std::numeric_limits<double>::epsilon() * 100.0))
1353 shapes::ShapeConstPtr shape = map->shapes_[0];
1355 world_->moveShapeInObject(
OCTOMAP_NS, shape, t);
1363 world_->addToObject(
OCTOMAP_NS, std::make_shared<const shapes::OcTree>(octree), t);
1368 if (
object.
object.operation == moveit_msgs::msg::CollisionObject::ADD &&
1371 RCLCPP_ERROR(LOGGER,
"Unable to attach a body to link '%s' (link not found)",
object.link_name.c_str());
1377 RCLCPP_ERROR(LOGGER,
"The ID '%s' cannot be used for collision objects (name reserved)",
OCTOMAP_NS.c_str());
1383 robot_state_ = std::make_shared<moveit::core::RobotState>(parent_->getCurrentState());
1384 robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_);
1386 robot_state_->update();
1393 if (
object.
object.operation == moveit_msgs::msg::CollisionObject::ADD ||
1394 object.
object.operation == moveit_msgs::msg::CollisionObject::APPEND)
1400 Eigen::Isometry3d object_pose_in_link;
1401 std::vector<shapes::ShapeConstPtr>
shapes;
1402 EigenSTL::vector_Isometry3d shape_poses;
1409 if (
object.
object.operation == moveit_msgs::msg::CollisionObject::ADD &&
object.
object.primitives.empty() &&
1410 object.object.meshes.empty() &&
object.object.planes.empty())
1414 RCLCPP_DEBUG(LOGGER,
"Attaching world object '%s' to link '%s'",
object.
object.
id.c_str(),
1415 object.link_name.c_str());
1417 object_pose_in_link = robot_state_->getGlobalLinkTransform(link_model).inverse() * obj_in_world->pose_;
1418 shapes = obj_in_world->shapes_;
1419 shape_poses = obj_in_world->shape_poses_;
1420 subframe_poses = obj_in_world->subframe_poses_;
1424 RCLCPP_ERROR(LOGGER,
1425 "Attempting to attach object '%s' to link '%s' but no geometry specified "
1426 "and such an object does not exist in the collision world",
1427 object.
object.
id.c_str(),
object.link_name.c_str());
1433 Eigen::Isometry3d header_frame_to_object_pose;
1436 const Eigen::Isometry3d world_to_header_frame =
getFrameTransform(
object.
object.header.frame_id);
1437 const Eigen::Isometry3d link_to_header_frame =
1438 robot_state_->getGlobalLinkTransform(link_model).inverse() * world_to_header_frame;
1439 object_pose_in_link = link_to_header_frame * header_frame_to_object_pose;
1441 Eigen::Isometry3d subframe_pose;
1442 for (std::size_t i = 0; i <
object.object.subframe_poses.size(); ++i)
1444 PlanningScene::poseMsgToEigen(
object.
object.subframe_poses[i], subframe_pose);
1445 std::string
name =
object.object.subframe_names[i];
1446 subframe_poses[
name] = subframe_pose;
1452 RCLCPP_ERROR(LOGGER,
"There is no geometry to attach to link '%s' as part of attached body '%s'",
1453 object.link_name.c_str(),
object.object.id.c_str());
1457 if (!
object.
object.
type.db.empty() || !
object.object.type.key.empty())
1461 if (obj_in_world && world_->removeObject(
object.object.id))
1463 if (
object.
object.operation == moveit_msgs::msg::CollisionObject::ADD)
1464 RCLCPP_DEBUG(LOGGER,
"Removing world object with the same name as newly attached object: '%s'",
1465 object.
object.
id.c_str());
1468 "You tried to append geometry to an attached object "
1469 "that is actually a world object ('%s'). World geometry is ignored.",
1470 object.
object.
id.c_str());
1474 if (
object.
object.operation == moveit_msgs::msg::CollisionObject::ADD ||
1475 !robot_state_->hasAttachedBody(
object.object.id))
1477 if (robot_state_->clearAttachedBody(
object.object.id))
1478 RCLCPP_DEBUG(LOGGER,
1479 "The robot state already had an object named '%s' attached to link '%s'. "
1480 "The object was replaced.",
1481 object.
object.
id.c_str(),
object.link_name.c_str());
1482 robot_state_->attachBody(
object.
object.
id, object_pose_in_link,
shapes, shape_poses,
object.touch_links,
1483 object.link_name,
object.detach_posture, subframe_poses);
1484 RCLCPP_DEBUG(LOGGER,
"Attached object '%s' to link '%s'",
object.
object.
id.c_str(),
object.link_name.c_str());
1492 object_pose_in_link = ab->
getPose();
1497 trajectory_msgs::msg::JointTrajectory detach_posture =
1498 object.detach_posture.joint_names.empty() ? ab->
getDetachPosture() :
object.detach_posture;
1501 touch_links.insert(std::make_move_iterator(
object.touch_links.begin()),
1502 std::make_move_iterator(
object.touch_links.end()));
1504 robot_state_->clearAttachedBody(
object.
object.
id);
1505 robot_state_->attachBody(
object.
object.
id, object_pose_in_link,
shapes, shape_poses, touch_links,
1506 object.link_name, detach_posture, subframe_poses);
1507 RCLCPP_DEBUG(LOGGER,
"Appended things to object '%s' attached to link '%s'",
object.
object.
id.c_str(),
1508 object.link_name.c_str());
1514 RCLCPP_ERROR(LOGGER,
"Robot state is not compatible with robot model. This could be fatal.");
1517 else if (
object.
object.operation == moveit_msgs::msg::CollisionObject::REMOVE)
1520 std::vector<const moveit::core::AttachedBody*> attached_bodies;
1521 if (
object.
object.
id.empty())
1524 object.link_name.empty() ? nullptr :
getRobotModel()->getLinkModel(
object.link_name);
1526 robot_state_->getAttachedBodies(attached_bodies, link_model);
1528 robot_state_->getAttachedBodies(attached_bodies);
1537 RCLCPP_ERROR_STREAM(LOGGER,
"The AttachedCollisionObject message states the object is attached to "
1538 <<
object.link_name <<
", but it is actually attached to "
1540 <<
". Leave the link_name empty or specify the correct link.");
1543 attached_bodies.push_back(body);
1550 const std::string&
name = attached_body->getName();
1551 if (world_->hasObject(
name))
1554 "The collision world already has an object with the same name as the body about to be detached. "
1555 "NOT adding the detached body '%s' to the collision world.",
1556 object.
object.
id.c_str());
1560 const Eigen::Isometry3d& pose = attached_body->getGlobalPose();
1561 world_->addToObject(
name, pose, attached_body->getShapes(), attached_body->getShapePoses());
1562 world_->setSubframesOfObject(
name, attached_body->getSubframes());
1563 RCLCPP_DEBUG(LOGGER,
"Detached object '%s' from link '%s' and added it back in the collision world",
1564 name.c_str(),
object.link_name.c_str());
1567 robot_state_->clearAttachedBody(
name);
1569 if (!attached_bodies.empty() ||
object.object.id.empty())
1572 else if (
object.
object.operation == moveit_msgs::msg::CollisionObject::MOVE)
1574 RCLCPP_ERROR(LOGGER,
"Move for attached objects not yet implemented");
1578 RCLCPP_ERROR(LOGGER,
"Unknown collision object operation: %d",
object.
object.operation);
1588 RCLCPP_ERROR(LOGGER,
"The ID '%s' cannot be used for collision objects (name reserved)",
OCTOMAP_NS.c_str());
1592 if (
object.operation == moveit_msgs::msg::CollisionObject::ADD ||
1593 object.operation == moveit_msgs::msg::CollisionObject::APPEND)
1595 return processCollisionObjectAdd(
object);
1597 else if (
object.operation == moveit_msgs::msg::CollisionObject::REMOVE)
1599 return processCollisionObjectRemove(
object);
1601 else if (
object.operation == moveit_msgs::msg::CollisionObject::MOVE)
1603 return processCollisionObjectMove(
object);
1606 RCLCPP_ERROR(LOGGER,
"Unknown collision object operation: %d",
object.operation);
1610 void PlanningScene::poseMsgToEigen(
const geometry_msgs::msg::Pose& msg, Eigen::Isometry3d& out)
1612 Eigen::Translation3d translation(msg.position.x, msg.position.y, msg.position.z);
1613 Eigen::Quaterniond quaternion(msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z);
1614 if ((quaternion.x() == 0) && (quaternion.y() == 0) && (quaternion.z() == 0) && (quaternion.w() == 0))
1616 RCLCPP_WARN(LOGGER,
"Empty quaternion found in pose message. Setting to neutral orientation.");
1617 quaternion.setIdentity();
1621 quaternion.normalize();
1623 out = translation * quaternion;
1627 Eigen::Isometry3d& object_pose,
1628 std::vector<shapes::ShapeConstPtr>&
shapes,
1629 EigenSTL::vector_Isometry3d& shape_poses)
1631 if (
object.primitives.size() <
object.primitive_poses.size())
1633 RCLCPP_ERROR(LOGGER,
"More primitive shape poses than shapes in collision object message.");
1636 if (
object.meshes.size() <
object.mesh_poses.size())
1638 RCLCPP_ERROR(LOGGER,
"More mesh poses than meshes in collision object message.");
1641 if (
object.planes.size() <
object.plane_poses.size())
1643 RCLCPP_ERROR(LOGGER,
"More plane poses than planes in collision object message.");
1647 const int num_shapes =
object.primitives.size() +
object.meshes.size() +
object.planes.size();
1648 shapes.reserve(num_shapes);
1649 shape_poses.reserve(num_shapes);
1651 PlanningScene::poseMsgToEigen(
object.pose, object_pose);
1653 bool switch_object_pose_and_shape_pose =
false;
1654 if (num_shapes == 1)
1657 switch_object_pose_and_shape_pose =
true;
1662 &switch_object_pose_and_shape_pose](shapes::Shape* s,
const geometry_msgs::msg::Pose& pose_msg) {
1665 Eigen::Isometry3d pose;
1666 PlanningScene::poseMsgToEigen(pose_msg, pose);
1667 if (!switch_object_pose_and_shape_pose)
1668 shape_poses.emplace_back(std::move(pose));
1671 shape_poses.emplace_back(std::move(object_pose));
1674 shapes.emplace_back(shapes::ShapeConstPtr(s));
1677 auto treat_shape_vectors = [&
append](
const auto& shape_vector,
1678 const auto& shape_poses_vector,
1679 const std::string& shape_type) {
1680 if (shape_vector.size() > shape_poses_vector.size())
1682 RCLCPP_DEBUG_STREAM(LOGGER,
"Number of " << shape_type
1683 <<
" does not match number of poses "
1684 "in collision object message. Assuming identity.");
1685 for (std::size_t i = 0; i < shape_vector.size(); ++i)
1687 if (i >= shape_poses_vector.size())
1689 append(shapes::constructShapeFromMsg(shape_vector[i]),
1690 geometry_msgs::msg::Pose());
1693 append(shapes::constructShapeFromMsg(shape_vector[i]), shape_poses_vector[i]);
1697 for (std::size_t i = 0; i < shape_vector.size(); ++i)
1698 append(shapes::constructShapeFromMsg(shape_vector[i]), shape_poses_vector[i]);
1701 treat_shape_vectors(
object.primitives,
object.primitive_poses, std::string(
"primitive_poses"));
1702 treat_shape_vectors(
object.meshes,
object.mesh_poses, std::string(
"meshes"));
1703 treat_shape_vectors(
object.planes,
object.plane_poses, std::string(
"planes"));
1707 bool PlanningScene::processCollisionObjectAdd(
const moveit_msgs::msg::CollisionObject&
object)
1711 RCLCPP_ERROR_STREAM(LOGGER,
"Unknown frame: " <<
object.header.frame_id);
1715 if (
object.primitives.empty() &&
object.meshes.empty() &&
object.planes.empty())
1717 RCLCPP_ERROR(LOGGER,
"There are no shapes specified in the collision object message");
1722 if (
object.operation == moveit_msgs::msg::CollisionObject::ADD && world_->hasObject(
object.id))
1723 world_->removeObject(
object.
id);
1725 const Eigen::Isometry3d& world_to_object_header_transform =
getFrameTransform(
object.header.frame_id);
1726 Eigen::Isometry3d header_to_pose_transform;
1727 std::vector<shapes::ShapeConstPtr>
shapes;
1728 EigenSTL::vector_Isometry3d shape_poses;
1731 const Eigen::Isometry3d object_frame_transform = world_to_object_header_transform * header_to_pose_transform;
1733 world_->addToObject(
object.
id, object_frame_transform,
shapes, shape_poses);
1735 if (!
object.
type.key.empty() || !
object.type.db.empty())
1740 Eigen::Isometry3d subframe_pose;
1741 for (std::size_t i = 0; i <
object.subframe_poses.size(); ++i)
1743 PlanningScene::poseMsgToEigen(
object.subframe_poses[i], subframe_pose);
1744 std::string
name =
object.subframe_names[i];
1745 subframes[
name] = subframe_pose;
1747 world_->setSubframesOfObject(
object.
id, subframes);
1751 bool PlanningScene::processCollisionObjectRemove(
const moveit_msgs::msg::CollisionObject&
object)
1753 if (
object.
id.empty())
1759 if (!world_->removeObject(
object.id))
1761 RCLCPP_WARN_STREAM(LOGGER,
1762 "Tried to remove world object '" <<
object.
id <<
"', but it does not exist in this scene.");
1773 bool PlanningScene::processCollisionObjectMove(
const moveit_msgs::msg::CollisionObject&
object)
1775 if (world_->hasObject(
object.id))
1777 if (!
object.primitives.empty() || !
object.meshes.empty() || !
object.planes.empty())
1778 RCLCPP_WARN(LOGGER,
"Move operation for object '%s' ignores the geometry specified in the message.",
1781 const Eigen::Isometry3d& world_to_object_header_transform =
getFrameTransform(
object.header.frame_id);
1782 Eigen::Isometry3d header_to_pose_transform;
1784 PlanningScene::poseMsgToEigen(
object.pose, header_to_pose_transform);
1786 const Eigen::Isometry3d object_frame_transform = world_to_object_header_transform * header_to_pose_transform;
1787 world_->setObjectPose(
object.
id, object_frame_transform);
1791 RCLCPP_ERROR(LOGGER,
"World object '%s' does not exist. Cannot move.",
object.
id.c_str());
1820 const Eigen::Isometry3d& t2 =
getWorld()->getTransform(
frame_id, frame_found);
1846 if (object_types_->find(object_id) != object_types_->end())
1849 return parent_->hasObjectType(object_id);
1857 ObjectTypeMap::const_iterator it = object_types_->find(object_id);
1858 if (it != object_types_->end())
1862 return parent_->getObjectType(object_id);
1863 static const object_recognition_msgs::msg::ObjectType EMPTY;
1870 object_types_ = std::make_unique<ObjectTypeMap>();
1871 (*object_types_)[object_id] =
type;
1877 object_types_->erase(object_id);
1884 parent_->getKnownObjectTypes(kc);
1886 for (ObjectTypeMap::const_iterator it = object_types_->begin(); it != object_types_->end(); ++it)
1887 kc[it->first] = it->second;
1893 if (object_colors_->find(object_id) != object_colors_->end())
1896 return parent_->hasObjectColor(object_id);
1904 ObjectColorMap::const_iterator it = object_colors_->find(object_id);
1905 if (it != object_colors_->end())
1909 return parent_->getObjectColor(object_id);
1910 static const std_msgs::msg::ColorRGBA EMPTY;
1918 parent_->getKnownObjectColors(kc);
1920 for (ObjectColorMap::const_iterator it = object_colors_->begin(); it != object_colors_->end(); ++it)
1921 kc[it->first] = it->second;
1926 if (object_id.empty())
1928 RCLCPP_ERROR(LOGGER,
"Cannot set color of object with empty object_id.");
1931 if (!object_colors_)
1932 object_colors_ = std::make_unique<ObjectColorMap>();
1933 (*object_colors_)[object_id] = color;
1939 object_colors_->erase(object_id);
1970 if (state_feasibility_)
1974 return state_feasibility_(s, verbose);
1981 if (state_feasibility_)
1982 return state_feasibility_(state, verbose);
1987 const moveit_msgs::msg::Constraints& constr,
bool verbose)
const
1995 const moveit_msgs::msg::Constraints& constr,
bool verbose)
const
1997 kinematic_constraints::KinematicConstraintSetPtr ks(
2022 static const moveit_msgs::msg::Constraints EMP_CONSTRAINTS;
2028 static const moveit_msgs::msg::Constraints EMP_CONSTRAINTS;
2033 const std::string&
group,
bool verbose)
const
2041 const std::string&
group,
bool verbose)
const
2062 const moveit_msgs::msg::RobotTrajectory& trajectory,
const std::string&
group,
2063 bool verbose, std::vector<std::size_t>* invalid_index)
const
2065 static const moveit_msgs::msg::Constraints EMP_CONSTRAINTS;
2066 static const std::vector<moveit_msgs::msg::Constraints> EMP_CONSTRAINTS_VECTOR;
2067 return isPathValid(start_state, trajectory, EMP_CONSTRAINTS, EMP_CONSTRAINTS_VECTOR,
group, verbose, invalid_index);
2071 const moveit_msgs::msg::RobotTrajectory& trajectory,
2073 bool verbose, std::vector<std::size_t>* invalid_index)
const
2075 static const std::vector<moveit_msgs::msg::Constraints> EMP_CONSTRAINTS_VECTOR;
2080 const moveit_msgs::msg::RobotTrajectory& trajectory,
2082 const moveit_msgs::msg::Constraints& goal_constraints,
const std::string&
group,
2083 bool verbose, std::vector<std::size_t>* invalid_index)
const
2085 std::vector<moveit_msgs::msg::Constraints> goal_constraints_vector(1, goal_constraints);
2090 const moveit_msgs::msg::RobotTrajectory& trajectory,
2092 const std::vector<moveit_msgs::msg::Constraints>& goal_constraints,
2093 const std::string&
group,
bool verbose, std::vector<std::size_t>* invalid_index)
const
2104 const std::vector<moveit_msgs::msg::Constraints>& goal_constraints,
2105 const std::string&
group,
bool verbose, std::vector<std::size_t>* invalid_index)
const
2109 invalid_index->clear();
2113 for (std::size_t i = 0; i < n_wp; ++i)
2117 bool this_state_valid =
true;
2119 this_state_valid =
false;
2121 this_state_valid =
false;
2123 this_state_valid =
false;
2125 if (!this_state_valid)
2128 invalid_index->push_back(i);
2135 if (i + 1 == n_wp && !goal_constraints.empty())
2138 for (
const moveit_msgs::msg::Constraints& goal_constraint : goal_constraints)
2149 RCLCPP_INFO(LOGGER,
"Goal not satisfied");
2151 invalid_index->push_back(i);
2161 const moveit_msgs::msg::Constraints& goal_constraints,
const std::string&
group,
2162 bool verbose, std::vector<std::size_t>* invalid_index)
const
2164 std::vector<moveit_msgs::msg::Constraints> goal_constraints_vector(1, goal_constraints);
2170 bool verbose, std::vector<std::size_t>* invalid_index)
const
2172 static const std::vector<moveit_msgs::msg::Constraints> EMP_CONSTRAINTS_VECTOR;
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(trajectory, EMP_CONSTRAINTS, EMP_CONSTRAINTS_VECTOR,
group, verbose, invalid_index);
2185 std::set<collision_detection::CostSource>& costs,
double overlap_fraction)
const
2187 getCostSources(trajectory, max_costs, std::string(), costs, overlap_fraction);
2191 const std::string& group_name, std::set<collision_detection::CostSource>& costs,
2192 double overlap_fraction)
const
2198 std::set<collision_detection::CostSource> cs;
2199 std::set<collision_detection::CostSource> cs_start;
2201 for (std::size_t i = 0; i < n_wp; ++i)
2210 if (cs.size() <= max_costs)
2216 for (std::set<collision_detection::CostSource>::iterator it = cs.begin(); i < max_costs; ++it, ++i)
2225 std::set<collision_detection::CostSource>& costs)
const
2231 const std::string& group_name,
2232 std::set<collision_detection::CostSource>& costs)
const
2245 const std::vector<std::string>& objects =
getWorld()->getObjectIds();
2246 std::vector<const moveit::core::AttachedBody*> attached_bodies;
2250 out <<
"-----------------------------------------\n";
2251 out <<
"PlanningScene Known Objects:\n";
2252 out <<
" - Collision World Objects:\n ";
2253 for (
const std::string&
object : objects)
2255 out <<
"\t- " <<
object <<
"\n";
2258 out <<
" - Attached Bodies:\n";
2261 out <<
"\t- " << attached_body->getName() <<
"\n";
2263 out <<
"-----------------------------------------\n";
Definition of a structure for the allowed collision matrix. All elements in the collision world are r...
void removeEntry(const std::string &name1, const std::string &name2)
Remove an entry corresponding to a pair of elements. Nothing happens if the pair does not exist in th...
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...
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...
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.
static bool isEmpty(const moveit_msgs::msg::PlanningScene &msg)
Check if a message includes any information about a planning scene, or it is just a default,...
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
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.
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.
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.