41 #include <moveit_msgs/GetPlanningScene.h>
43 #include <dynamic_reconfigure/server.h>
44 #include <moveit_ros_planning/PlanningSceneMonitorDynamicReconfigureConfig.h>
51 #include <boost/algorithm/string/join.hpp>
57 using namespace moveit_ros_planning;
59 static const std::string
LOGNAME =
"planning_scene_monitor";
61 class PlanningSceneMonitor::DynamicReconfigureImpl
64 DynamicReconfigureImpl(PlanningSceneMonitor* owner)
65 : owner_(owner), dynamic_reconfigure_server_(
ros::NodeHandle(decideNamespace(owner->
getName())))
67 dynamic_reconfigure_server_.setCallback(
68 [
this](
const auto& config, uint32_t level) { dynamicReconfigureCallback(config, level); });
74 static std::string decideNamespace(
const std::string& name)
76 std::string ns =
"~/" +
name;
77 std::replace(ns.begin(), ns.end(),
' ',
'_');
78 std::transform(ns.begin(), ns.end(), ns.begin(), ::tolower);
82 while (
ros::service::exists(ns + boost::lexical_cast<std::string>(c) +
"/set_parameters",
false))
84 ns += boost::lexical_cast<std::string>(c);
89 void dynamicReconfigureCallback(
const PlanningSceneMonitorDynamicReconfigureConfig& config, uint32_t )
92 if (config.publish_geometry_updates)
94 if (config.publish_state_updates)
96 if (config.publish_transforms_updates)
98 if (config.publish_planning_scene)
100 owner_->setPlanningScenePublishingFrequency(config.publish_planning_scene_hz);
101 owner_->startPublishingPlanningScene(event);
104 owner_->stopPublishingPlanningScene();
108 dynamic_reconfigure::Server<PlanningSceneMonitorDynamicReconfigureConfig> dynamic_reconfigure_server_;
120 const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
const std::string& name)
126 const std::string& robot_description,
127 const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
const std::string& name)
134 const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
const std::string& name)
140 const robot_model_loader::RobotModelLoaderPtr& rm_loader,
141 const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
const std::string& name)
152 const robot_model_loader::RobotModelLoaderPtr& rm_loader,
153 const ros::NodeHandle& nh,
const std::shared_ptr<tf2_ros::Buffer>& tf_buffer,
154 const std::string& name)
200 scene_ = std::make_shared<planning_scene::PlanningScene>(
rm_loader_->getModel());
208 scene_->getCollisionEnvNonConst()->setLinkPadding(it.first, it.second);
212 scene_->getCollisionEnvNonConst()->setLinkScale(it.first, it.second);
214 scene_->propogateRobotPadding();
231 scene_->setCollisionObjectUpdateCallback(
249 double temp_wait_time = 0.05;
282 scene_->setCollisionObjectUpdateCallback(
283 [
this](
const collision_detection::World::ObjectConstPtr&
object,
292 "Stopping planning scene diff publisher");
302 if (!
scene_->getName().empty())
304 if (
scene_->getName()[
scene_->getName().length() - 1] ==
'+')
317 std::unique_ptr<boost::thread> copy;
328 const std::string& planning_scene_topic)
334 ROS_INFO_NAMED(
LOGNAME,
"Publishing maintained planning scene on '%s'", planning_scene_topic.c_str());
346 moveit_msgs::PlanningScene msg;
351 scene_->getPlanningSceneMsg(msg);
359 moveit_msgs::PlanningScene msg;
360 bool publish_msg =
false;
361 bool is_full =
false;
378 scene_->getPlanningSceneDiffMsg(msg);
381 msg.robot_state.attached_collision_objects.clear();
382 msg.robot_state.is_diff =
true;
397 scene_->setCollisionObjectUpdateCallback(
398 [
this](
const collision_detection::World::ObjectConstPtr&
object,
410 scene_->getPlanningSceneMsg(msg);
448 bool sceneIsParentOf(
const planning_scene::PlanningSceneConstPtr& scene,
451 if (scene && scene.get() == possible_parent)
454 return sceneIsParentOf(scene->getParent(), possible_parent);
475 update_callback(update_type);
485 throw std::runtime_error(
"requestPlanningSceneState() to self-provided service: " + service_name);
490 moveit_msgs::GetPlanningScene srv;
499 if (client.
call(srv))
506 LOGNAME,
"Failed to call service %s, have you launched move_group or called psm.providePlanningSceneService()?",
507 service_name.c_str());
521 moveit_msgs::GetPlanningScene::Response& res)
523 if (req.components.components & moveit_msgs::PlanningSceneComponents::TRANSFORMS)
526 moveit_msgs::PlanningSceneComponents all_components;
527 all_components.components = UINT_MAX;
530 scene_->getPlanningSceneMsg(res.scene, req.components.components ? req.components : all_components);
542 bool removed =
false;
545 removed =
scene_->getWorldNonConst()->removeObject(
scene_->OCTOMAP_NS);
571 std::string old_scene_name;
582 old_scene_name =
scene_->getName();
595 result =
scene_->setPlanningSceneDiffMsg(scene);
600 if (!scene.is_diff && scene.world.octomap.octomap.data.empty())
619 bool no_other_scene_upd = (scene.name.empty() || scene.name == old_scene_name) &&
620 scene.allowed_collision_matrix.entry_names.empty() && scene.link_padding.empty() &&
621 scene.link_scale.empty();
622 if (no_other_scene_upd)
628 if (!scene.fixed_frame_transforms.empty())
634 if (!scene.robot_state.attached_collision_objects.empty() || !
static_cast<bool>(scene.robot_state.is_diff))
651 scene_->getWorldNonConst()->clearObjects();
652 scene_->processPlanningSceneWorldMsg(*world);
655 if (world->octomap.octomap.data.empty())
676 if (!
scene_->processCollisionObjectMsg(*obj))
690 scene_->processAttachedCollisionObjectMsg(*obj);
704 const std::vector<const moveit::core::LinkModel*>& links =
getRobotModel()->getLinkModelsWithCollisionGeometry();
709 std::vector<shapes::ShapeConstPtr>
shapes = link->getShapes();
710 for (std::size_t j = 0; j <
shapes.size(); ++j)
740 std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>& link_shape_handle :
742 for (std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>& it : link_shape_handle.second)
756 std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>& attached_body_shape_handle :
758 for (std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>& it : attached_body_shape_handle.second)
769 std::vector<const moveit::core::AttachedBody*> ab;
770 scene_->getCurrentState().getAttachedBodies(ab);
783 for (std::pair<
const std::string, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>>>&
785 for (std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>& it :
786 collision_body_shape_handle.second)
796 for (
const std::pair<const std::string, collision_detection::World::ObjectPtr>& it : *
scene_->getWorld())
807 for (std::size_t i = 0; i < attached_body->
getShapes().size(); ++i)
832 for (std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>& shape_handle : it->second)
847 for (std::size_t i = 0; i <
obj->shapes_.size(); ++i)
872 for (std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>& shape_handle : it->second)
1001 if (!scene_topic.empty())
1028 std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>& link_shape_handle :
1031 tf_buffer_->canTransform(target_frame, link_shape_handle.first->getName(), target_time,
1034 tf_buffer_->lookupTransform(target_frame, link_shape_handle.first->getName(), target_time));
1035 for (std::size_t j = 0; j < link_shape_handle.second.size(); ++j)
1036 cache[link_shape_handle.second[j].first] =
1037 ttr * link_shape_handle.first->getCollisionOriginTransforms()[link_shape_handle.second[j].second];
1040 std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>&
1043 tf_buffer_->canTransform(target_frame, attached_body_shape_handle.first->getAttachedLinkName(), target_time,
1046 target_frame, attached_body_shape_handle.first->getAttachedLinkName(), target_time));
1047 for (std::size_t k = 0; k < attached_body_shape_handle.second.size(); ++k)
1048 cache[attached_body_shape_handle.second[k].first] =
1050 attached_body_shape_handle.first->getShapePosesInLinkFrame()[attached_body_shape_handle.second[k].second];
1053 tf_buffer_->canTransform(target_frame,
scene_->getPlanningFrame(), target_time,
1057 for (
const std::pair<
const std::string,
1058 std::vector<std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>>>&
1060 for (
const std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>& it :
1061 collision_body_shape_handle.second)
1062 cache[it.first] =
transform * (*it.second);
1074 const std::string& planning_scene_world_topic,
1075 const bool load_octomap_monitor)
1078 ROS_INFO_NAMED(
LOGNAME,
"Starting world geometry update monitor for collision objects, attached objects, octomap "
1082 if (!collision_objects_topic.empty())
1089 if (!planning_scene_world_topic.empty())
1098 if (load_octomap_monitor)
1103 std::make_unique<occupancy_map_monitor::OccupancyMapMonitor>(
tf_buffer_,
scene_->getPlanningFrame());
1135 const std::string& attached_objects_topic)
1143 [
this](
const sensor_msgs::JointStateConstPtr& state) {
onStateUpdate(state); });
1152 if (!attached_objects_topic.empty())
1263 if (hz > std::numeric_limits<double>::epsilon())
1289 std::vector<std::string> missing;
1293 std::string missing_str = boost::algorithm::join(missing,
", ");
1295 missing_str.c_str());
1303 scene_->getCurrentStateNonConst().update();
1313 boost::recursive_mutex::scoped_lock lock(
update_lock_);
1320 boost::recursive_mutex::scoped_lock lock(
update_lock_);
1333 const std::string& target =
getRobotModel()->getModelFrame();
1335 std::vector<std::string> all_frame_names;
1336 tf_buffer_->_getFrameStrings(all_frame_names);
1337 for (
const std::string& all_frame_name : all_frame_names)
1339 if (all_frame_name == target ||
getRobotModel()->hasLinkModel(all_frame_name))
1342 geometry_msgs::TransformStamped
f;
1350 << all_frame_name <<
"' to planning frame '" << target <<
"' (" << ex.what()
1354 f.header.frame_id = all_frame_name;
1355 f.child_frame_id = target;
1356 transforms.push_back(f);
1367 std::vector<geometry_msgs::TransformStamped> transforms;
1371 scene_->getTransformsNonConst().setTransforms(transforms);
1408 if (coll_ops.
size() == 0)
1414 for (
int i = 0; i < coll_ops.
size(); ++i)
1416 if (!coll_ops[i].hasMember(
"object1") || !coll_ops[i].hasMember(
"object2") || !coll_ops[i].hasMember(
"operation"))
1421 acm.
setEntry(std::string(coll_ops[i][
"object1"]), std::string(coll_ops[i][
"object2"]),
1422 std::string(coll_ops[i][
"operation"]) ==
"disable");
1440 const std::string old_robot_description =
1444 if (
nh_.
hasParam(old_robot_description +
"_planning/default_robot_padding") ||
1445 nh_.
hasParam(old_robot_description +
"_planning/default_robot_scale") ||
1446 nh_.
hasParam(old_robot_description +
"_planning/default_object_padding") ||
1447 nh_.
hasParam(old_robot_description +
"_planning/default_attached_padding") ||
1448 nh_.
hasParam(old_robot_description +
"_planning/default_robot_link_padding") ||
1449 nh_.
hasParam(old_robot_description +
"_planning/default_robot_link_scale"))
1452 "Old parameter path: '"
1455 "New parameter path: '"
1458 "Ignoring old parameters. Please update your moveit config!");
1467 std::map<std::string, double>());
1469 std::map<std::string, double>());