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;
263 std::vector<std::string> ignored_frames_vector;
264 nh_.
param(
"planning_scene_monitor_options/ignored_frames", ignored_frames_vector, std::vector<std::string>());
265 ignored_frames_ = std::set<std::string>(ignored_frames_vector.begin(), ignored_frames_vector.end());
286 scene_->setCollisionObjectUpdateCallback(
287 [
this](
const collision_detection::World::ObjectConstPtr&
object,
296 "Stopping planning scene diff publisher");
306 if (!
scene_->getName().empty())
308 if (
scene_->getName()[
scene_->getName().length() - 1] ==
'+')
321 std::unique_ptr<boost::thread> copy;
332 const std::string& planning_scene_topic)
338 ROS_INFO_NAMED(
LOGNAME,
"Publishing maintained planning scene on '%s'", planning_scene_topic.c_str());
350 moveit_msgs::PlanningScene msg;
355 scene_->getPlanningSceneMsg(msg);
363 moveit_msgs::PlanningScene msg;
364 bool publish_msg =
false;
365 bool is_full =
false;
382 scene_->getPlanningSceneDiffMsg(msg);
385 msg.robot_state.attached_collision_objects.clear();
386 msg.robot_state.is_diff =
true;
401 scene_->setCollisionObjectUpdateCallback(
402 [
this](
const collision_detection::World::ObjectConstPtr&
object,
414 scene_->getPlanningSceneMsg(msg);
452 bool sceneIsParentOf(
const planning_scene::PlanningSceneConstPtr& scene,
455 if (scene && scene.get() == possible_parent)
458 return sceneIsParentOf(scene->getParent(), possible_parent);
479 update_callback(update_type);
489 throw std::runtime_error(
"requestPlanningSceneState() to self-provided service: " + service_name);
494 moveit_msgs::GetPlanningScene srv;
503 if (client.
call(srv))
510 LOGNAME,
"Failed to call service %s, have you launched move_group or called psm.providePlanningSceneService()?",
511 service_name.c_str());
525 moveit_msgs::GetPlanningScene::Response& res)
527 if (req.components.components & moveit_msgs::PlanningSceneComponents::TRANSFORMS)
530 moveit_msgs::PlanningSceneComponents all_components;
531 all_components.components = UINT_MAX;
534 scene_->getPlanningSceneMsg(res.scene, req.components.components ? req.components : all_components);
546 bool removed =
false;
549 removed =
scene_->getWorldNonConst()->removeObject(
scene_->OCTOMAP_NS);
575 std::string old_scene_name;
586 old_scene_name =
scene_->getName();
591 if (scene.robot_state.is_diff)
601 result =
scene_->usePlanningSceneMsg(scene);
606 if (!scene.is_diff && scene.world.octomap.octomap.data.empty())
625 bool no_other_scene_upd = (scene.name.empty() || scene.name == old_scene_name) &&
626 scene.allowed_collision_matrix.entry_names.empty() && scene.link_padding.empty() &&
627 scene.link_scale.empty();
628 if (no_other_scene_upd)
634 if (!scene.fixed_frame_transforms.empty())
640 if (!scene.robot_state.attached_collision_objects.empty() || !
static_cast<bool>(scene.robot_state.is_diff))
657 scene_->getWorldNonConst()->clearObjects();
658 scene_->processPlanningSceneWorldMsg(*world);
661 if (world->octomap.octomap.data.empty())
682 if (!
scene_->processCollisionObjectMsg(*obj))
696 scene_->processAttachedCollisionObjectMsg(*obj);
710 const std::vector<const moveit::core::LinkModel*>& links =
getRobotModel()->getLinkModelsWithCollisionGeometry();
715 std::vector<shapes::ShapeConstPtr>
shapes = link->getShapes();
716 for (std::size_t j = 0; j <
shapes.size(); ++j)
746 std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>& link_shape_handle :
748 for (std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>& it : link_shape_handle.second)
762 std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>>>& attached_body_shape_handle :
764 for (std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>& it : attached_body_shape_handle.second)
775 std::vector<const moveit::core::AttachedBody*> ab;
776 scene_->getCurrentState().getAttachedBodies(ab);
789 for (std::pair<
const std::string, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>>>&
791 for (std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Isometry3d*>& it :
792 collision_body_shape_handle.second)
802 for (
const std::pair<const std::string, collision_detection::World::ObjectPtr>& it : *
scene_->getWorld())
813 for (std::size_t i = 0; i < attached_body->
getShapes().size(); ++i)
838 for (std::pair<occupancy_map_monitor::ShapeHandle, std::size_t>& shape_handle : it->second)
853 for (std::size_t i = 0; i <
obj->shapes_.size(); ++i)
878 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())
1222 if (hz > std::numeric_limits<double>::epsilon())
1248 std::vector<std::string> missing;
1252 std::string missing_str = boost::algorithm::join(missing,
", ");
1254 missing_str.c_str());
1259 if (!skip_update_if_locked)
1261 else if (!ulock.try_lock())
1268 scene_->getCurrentStateNonConst().update();
1286 boost::recursive_mutex::scoped_lock lock(
update_lock_);
1293 boost::recursive_mutex::scoped_lock lock(
update_lock_);
1306 const std::string& target =
getRobotModel()->getModelFrame();
1308 std::vector<std::string> all_frame_names;
1309 tf_buffer_->_getFrameStrings(all_frame_names);
1310 for (
const std::string& all_frame_name : all_frame_names)
1315 geometry_msgs::TransformStamped
f;
1323 << all_frame_name <<
"' to planning frame '" << target <<
"' (" << ex.what()
1327 f.header.frame_id = all_frame_name;
1328 f.child_frame_id = target;
1329 transforms.push_back(f);
1340 std::vector<geometry_msgs::TransformStamped> transforms;
1344 scene_->getTransformsNonConst().setTransforms(transforms);
1381 if (coll_ops.
size() == 0)
1387 for (
int i = 0; i < coll_ops.
size(); ++i)
1389 if (!coll_ops[i].hasMember(
"object1") || !coll_ops[i].hasMember(
"object2") || !coll_ops[i].hasMember(
"operation"))
1394 acm.
setEntry(std::string(coll_ops[i][
"object1"]), std::string(coll_ops[i][
"object2"]),
1395 std::string(coll_ops[i][
"operation"]) ==
"disable");
1413 const std::string old_robot_description =
1417 if (
nh_.
hasParam(old_robot_description +
"_planning/default_robot_padding") ||
1418 nh_.
hasParam(old_robot_description +
"_planning/default_robot_scale") ||
1419 nh_.
hasParam(old_robot_description +
"_planning/default_object_padding") ||
1420 nh_.
hasParam(old_robot_description +
"_planning/default_attached_padding") ||
1421 nh_.
hasParam(old_robot_description +
"_planning/default_robot_link_padding") ||
1422 nh_.
hasParam(old_robot_description +
"_planning/default_robot_link_scale"))
1425 "Old parameter path: '"
1428 "New parameter path: '"
1431 "Ignoring old parameters. Please update your moveit config!");
1440 std::map<std::string, double>());
1442 std::map<std::string, double>());