40 #include <moveit_msgs/GetPlanningScene.h> 42 #include <dynamic_reconfigure/server.h> 43 #include <moveit_ros_planning/PlanningSceneMonitorDynamicReconfigureConfig.h> 53 static const std::string
LOGNAME =
"planning_scene_monitor";
59 : owner_(owner), dynamic_reconfigure_server_(
ros::NodeHandle(decideNamespace(owner->
getName())))
61 dynamic_reconfigure_server_.setCallback(
62 boost::bind(&DynamicReconfigureImpl::dynamicReconfigureCallback,
this, _1, _2));
70 std::string ns =
"~/" + name;
71 std::replace(ns.begin(), ns.end(),
' ',
'_');
72 std::transform(ns.begin(), ns.end(), ns.begin(), ::tolower);
76 while (
ros::service::exists(ns + boost::lexical_cast<std::string>(c) +
"/set_parameters",
false))
78 ns += boost::lexical_cast<std::string>(c);
86 if (config.publish_geometry_updates)
88 if (config.publish_state_updates)
90 if (config.publish_transforms_updates)
92 if (config.publish_planning_scene)
94 owner_->setPlanningScenePublishingFrequency(config.publish_planning_scene_hz);
95 owner_->startPublishingPlanningScene(event);
98 owner_->stopPublishingPlanningScene();
120 const std::string& robot_description,
133 const robot_model_loader::RobotModelLoaderPtr& rm_loader,
145 const robot_model_loader::RobotModelLoaderPtr& rm_loader,
147 const std::string& name)
160 scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
206 scene_->getCollisionRobotNonConst()->setLinkPadding(it->first, it->second);
211 scene_->getCollisionRobotNonConst()->setLinkScale(it->first, it->second);
213 scene_->propogateRobotPadding();
224 scene_->setAttachedBodyUpdateCallback(
226 scene_->setCollisionObjectUpdateCallback(
242 double temp_wait_time = 0.05;
266 scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
272 scene_->setAttachedBodyUpdateCallback(
274 scene_->setCollisionObjectUpdateCallback(
282 ROS_WARN_NAMED(LOGNAME,
"Diff monitoring was stopped while publishing planning scene diffs. " 283 "Stopping planning scene diff publisher");
293 if (!
scene_->getName().empty())
295 if (
scene_->getName()[
scene_->getName().length() - 1] ==
'+')
308 std::unique_ptr<boost::thread> copy;
314 ROS_INFO_NAMED(LOGNAME,
"Stopped publishing maintained planning scene.");
319 const std::string& planning_scene_topic)
325 ROS_INFO_NAMED(LOGNAME,
"Publishing maintained planning scene on '%s'", planning_scene_topic.c_str());
337 moveit_msgs::PlanningScene msg;
342 scene_->getPlanningSceneMsg(msg);
345 ROS_DEBUG_NAMED(LOGNAME,
"Published the full planning scene: '%s'", msg.name.c_str());
350 moveit_msgs::PlanningScene msg;
351 bool publish_msg =
false;
352 bool is_full =
false;
369 scene_->getPlanningSceneDiffMsg(msg);
376 scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
380 scene_->setAttachedBodyUpdateCallback(
382 scene_->setCollisionObjectUpdateCallback(
394 scene_->getPlanningSceneMsg(msg);
408 ROS_DEBUG_NAMED(LOGNAME,
"Published full planning scene: '%s'", msg.name.c_str());
433 bool sceneIsParentOf(
const planning_scene::PlanningSceneConstPtr& scene,
436 if (scene && scene.get() == possible_parent)
439 return sceneIsParentOf(scene->getParent(), possible_parent);
469 moveit_msgs::GetPlanningScene srv;
470 srv.request.components.components =
471 srv.request.components.SCENE_SETTINGS | srv.request.components.ROBOT_STATE |
472 srv.request.components.ROBOT_STATE_ATTACHED_OBJECTS | srv.request.components.WORLD_OBJECT_NAMES |
473 srv.request.components.WORLD_OBJECT_GEOMETRY | srv.request.components.OCTOMAP |
474 srv.request.components.TRANSFORMS | srv.request.components.ALLOWED_COLLISION_MATRIX |
475 srv.request.components.LINK_PADDING_AND_SCALING | srv.request.components.OBJECT_COLORS;
484 if (client.
call(srv))
490 ROS_INFO_NAMED(LOGNAME,
"Failed to call service %s, have you launched move_group? at %s:%d", service_name.c_str(),
517 std::string old_scene_name;
528 old_scene_name =
scene_->getName();
529 result =
scene_->usePlanningSceneMsg(scene);
532 if (!scene.is_diff && scene.world.octomap.octomap.data.empty())
545 scene_->setAttachedBodyUpdateCallback(robot_state::AttachedBodyCallback());
550 scene_->setAttachedBodyUpdateCallback(
552 scene_->setCollisionObjectUpdateCallback(
565 bool no_other_scene_upd = (scene.name.empty() || scene.name == old_scene_name) &&
566 scene.allowed_collision_matrix.entry_names.empty() && scene.link_padding.empty() &&
567 scene.link_scale.empty();
568 if (no_other_scene_upd)
574 if (!scene.fixed_frame_transforms.empty())
580 if (!scene.robot_state.attached_collision_objects.empty() || scene.robot_state.is_diff ==
false)
597 scene_->getWorldNonConst()->clearObjects();
598 scene_->processPlanningSceneWorldMsg(*world);
601 if (world->octomap.octomap.data.empty())
629 scene_->processCollisionObjectMsg(*obj);
643 scene_->processAttachedCollisionObjectMsg(*obj);
657 const std::vector<const robot_model::LinkModel*>& links =
getRobotModel()->getLinkModelsWithCollisionGeometry();
660 for (std::size_t i = 0; i < links.size(); ++i)
662 std::vector<shapes::ShapeConstPtr>
shapes = links[i]->getShapes();
663 for (std::size_t j = 0; j < shapes.size(); ++j)
693 for (std::size_t i = 0; i < it->second.size(); ++i)
708 for (std::size_t k = 0; k < it->second.size(); ++k)
719 std::vector<const robot_state::AttachedBody*> ab;
720 scene_->getCurrentState().getAttachedBodies(ab);
721 for (std::size_t i = 0; i < ab.size(); ++i)
735 for (std::size_t k = 0; k < it->second.size(); ++k)
757 for (std::size_t i = 0; i < attached_body->getShapes().size(); ++i)
769 ROS_DEBUG_NAMED(LOGNAME,
"Excluding attached body '%s' from monitored octomap", attached_body->getName().c_str());
782 for (std::size_t k = 0; k < it->second.size(); ++k)
784 ROS_DEBUG_NAMED(LOGNAME,
"Including attached body '%s' in monitored octomap", attached_body->getName().c_str());
797 for (std::size_t i = 0; i < obj->shapes_.size(); ++i)
809 ROS_DEBUG_NAMED(LOGNAME,
"Excluding collision object '%s' from monitored octomap", obj->id_.c_str());
822 for (std::size_t k = 0; k < it->second.size(); ++k)
824 ROS_DEBUG_NAMED(LOGNAME,
"Including collision object '%s' in monitored octomap", obj->id_.c_str());
909 ROS_WARN_NAMED(LOGNAME,
"Maybe failed to update robot state, time diff: %.3fs",
951 if (!scene_topic.empty())
981 tf_->lookupTransform(target_frame, it->first->getName(), target_time, tr);
984 for (std::size_t j = 0; j < it->second.size(); ++j)
985 cache[it->second[j].first] = ttr * it->first->getCollisionOriginTransforms()[it->second[j].second];
991 tf_->waitForTransform(target_frame, it->first->getAttachedLinkName(), target_time,
993 tf_->lookupTransform(target_frame, it->first->getAttachedLinkName(), target_time, tr);
994 Eigen::Affine3d transform;
996 for (std::size_t k = 0; k < it->second.size(); ++k)
997 cache[it->second[k].first] = transform * it->first->getFixedTransforms()[it->second[k].second];
1001 tf_->waitForTransform(target_frame,
scene_->getPlanningFrame(), target_time,
1003 tf_->lookupTransform(target_frame,
scene_->getPlanningFrame(), target_time, tr);
1004 Eigen::Affine3d transform;
1008 for (std::size_t k = 0; k < it->second.size(); ++k)
1009 cache[it->second[k].first] = transform * (*it->second[k].second);
1021 const std::string& planning_scene_world_topic,
1022 const bool load_octomap_monitor)
1028 if (!collision_objects_topic.empty())
1039 ROS_INFO_NAMED(LOGNAME,
"Listening to '%s' using message notifier with target frame '%s'",
1051 if (!planning_scene_world_topic.empty())
1055 ROS_INFO_NAMED(LOGNAME,
"Listening to '%s' for planning scene world geometry",
1060 if (load_octomap_monitor)
1096 const std::string& attached_objects_topic)
1112 if (!attached_objects_topic.empty())
1117 ROS_INFO_NAMED(LOGNAME,
"Listening to '%s' for attached collision objects",
1122 ROS_ERROR_NAMED(LOGNAME,
"Cannot monitor robot state because planning scene is not configured");
1156 last_robot_state_update_wall_time_ = n;
1183 "performPendingStateUpdate: " << fmod(last_robot_state_update_wall_time_.toSec(), 10));
1223 if (hz > std::numeric_limits<double>::epsilon())
1249 std::vector<std::string> missing;
1253 std::string missing_str = boost::algorithm::join(missing,
", ");
1255 missing_str.c_str());
1263 scene_->getCurrentStateNonConst().update();
1273 boost::recursive_mutex::scoped_lock lock(
update_lock_);
1280 boost::recursive_mutex::scoped_lock lock(
update_lock_);
1287 ROS_DEBUG_NAMED(LOGNAME,
"Maximum frquency for publishing a planning scene is now %lf Hz",
1293 const std::string& target =
getRobotModel()->getModelFrame();
1295 std::vector<std::string> all_frame_names;
1296 tf_->getFrameStrings(all_frame_names);
1297 for (std::size_t i = 0; i < all_frame_names.size(); ++i)
1299 const std::string& frame_no_slash = (!all_frame_names[i].empty() && all_frame_names[i][0] ==
'/') ?
1300 all_frame_names[i].substr(1) :
1302 const std::string& frame_with_slash =
1303 (!all_frame_names[i].empty() && all_frame_names[i][0] !=
'/') ?
'/' + all_frame_names[i] : all_frame_names[i];
1305 if (frame_with_slash == target ||
getRobotModel()->hasLinkModel(frame_no_slash))
1309 std::string err_string;
1310 if (
tf_->getLatestCommonTime(target, all_frame_names[i], stamp, &err_string) !=
tf::NO_ERROR)
1313 << all_frame_names[i] <<
"' and planning frame '" << target <<
"' (" 1314 << err_string <<
")");
1321 tf_->lookupTransform(target, all_frame_names[i], stamp, t);
1326 << all_frame_names[i] <<
"' to planning frame '" << target <<
"' (" 1327 << ex.what() <<
")");
1330 geometry_msgs::TransformStamped
f;
1331 f.header.frame_id = frame_with_slash;
1332 f.child_frame_id = target;
1333 f.transform.translation.x = t.
getOrigin().
x();
1334 f.transform.translation.y = t.
getOrigin().
y();
1335 f.transform.translation.z = t.
getOrigin().
z();
1337 f.transform.rotation.x = q.x();
1338 f.transform.rotation.y = q.y();
1339 f.transform.rotation.z = q.z();
1340 f.transform.rotation.w = q.w();
1341 transforms.push_back(f);
1352 std::vector<geometry_msgs::TransformStamped> transforms;
1356 scene_->getTransformsNonConst().setTransforms(transforms);
1379 ROS_DEBUG_NAMED(LOGNAME,
"No additional default collision operations specified");
1382 ROS_DEBUG_NAMED(LOGNAME,
"Reading additional default collision operations");
1389 ROS_WARN_NAMED(LOGNAME,
"default_collision_operations is not an array");
1393 if (coll_ops.
size() == 0)
1395 ROS_WARN_NAMED(LOGNAME,
"No collision operations in default collision operations");
1399 for (
int i = 0; i < coll_ops.
size(); ++i)
1401 if (!coll_ops[i].hasMember(
"object1") || !coll_ops[i].
hasMember(
"object2") || !coll_ops[i].
hasMember(
"operation"))
1403 ROS_WARN_NAMED(LOGNAME,
"All collision operations must have two objects and an operation");
1406 acm.
setEntry(std::string(coll_ops[i][
"object1"]), std::string(coll_ops[i][
"object2"]),
1407 std::string(coll_ops[i][
"operation"]) ==
"disable");
1424 static const std::string robot_description =
1432 std::map<std::string, double>());
1434 std::map<std::string, double>());
void startSceneMonitor(const std::string &scene_topic=DEFAULT_PLANNING_SCENE_TOPIC)
Start the scene monitor.
boost::recursive_mutex update_lock_
lock access to update_callbacks_
std::map< ShapeHandle, Eigen::Affine3d, std::less< ShapeHandle >, Eigen::aligned_allocator< std::pair< const ShapeHandle, Eigen::Affine3d > > > ShapeTransformCache
void clearUpdateCallbacks()
Clear the functions to be called when an update to the scene is received.
void scenePublishingThread()
#define ROS_INFO_NAMED(name,...)
#define ROS_WARN_THROTTLE_NAMED(rate, name,...)
void mergeVertices(double threshold)
void getUpdatedFrameTransforms(std::vector< geometry_msgs::TransformStamped > &transforms)
#define ROS_DEBUG_STREAM_NAMED(name, args)
void publish(const boost::shared_ptr< M > &message) const
static bool isEmpty(const moveit_msgs::PlanningScene &msg)
double default_robot_scale_
default robot scaling
boost::mutex state_pending_mutex_
The geometry of the scene was updated. This includes receiving new octomaps, collision objects...
#define ROS_WARN_NAMED(name,...)
static const std::string LOGNAME
robot_model_loader::RobotModelLoaderPtr rm_loader_
void lockSceneWrite()
Lock the scene for writing (only one thread can lock for writing and no other thread can lock for rea...
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ros::Subscriber planning_scene_world_subscriber_
static const std::string DEFAULT_PLANNING_SCENE_SERVICE
The name of the service used by default for requesting full planning scene state. ...
void onStateUpdate(const sensor_msgs::JointStateConstPtr &joint_state)
void setPeriod(const WallDuration &period, bool reset=true)
std::string resolveName(const std::string &name, bool remap=true) const
SceneUpdateType new_scene_update_
void stopStateMonitor()
Stop the state monitor.
void excludeWorldObjectsFromOctree()
ros::Time last_robot_motion_time_
Last time the state was updated.
void addUpdateCallback(const boost::function< void(SceneUpdateType)> &fn)
Add a function to be called when an update to the scene is received.
boost::shared_lock< boost::shared_mutex > ReadLock
Monitors the joint_states topic and tf to maintain the current state of the robot.
bool call(MReq &req, MRes &res)
const robot_model::RobotModelConstPtr & getRobotModel() const
ros::Duration shape_transform_cache_lookup_wait_time_
the amount of time to wait when looking up transforms
std::string getName(void *handle)
void octomapUpdateCallback()
Callback for octomap updates.
void configureDefaultPadding()
Configure the default padding.
std::shared_ptr< ros::AsyncSpinner > spinner_
void triggerSceneUpdateEvent(SceneUpdateType update_type)
This function is called every time there is a change to the planning scene.
void stopPublishingPlanningScene()
Stop publishing the maintained planning scene.
void lockSceneRead()
Lock the scene for reading (multiple threads can lock for reading at the same time) ...
DynamicReconfigureImpl * reconfigure_impl_
std::map< std::string, ObjectPtr >::const_iterator const_iterator
Type const & getType() const
static const std::string OCTOMAP_NS
double default_robot_padd_
default robot padding
void excludeRobotLinksFromOctree()
void collisionObjectCallback(const moveit_msgs::CollisionObjectConstPtr &obj)
Callback for a new collision object msg.
static const std::string DEFAULT_PLANNING_SCENE_WORLD_TOPIC
void unlockSceneWrite()
Lock the scene from writing (only one thread can lock for writing and no other thread can lock for re...
PlanningSceneMonitor(const std::string &robot_description, const boost::shared_ptr< tf::Transformer > &tf=boost::shared_ptr< tf::Transformer >(), const std::string &name="")
Constructor.
CurrentStateMonitorPtr current_state_monitor_
The state in the monitored scene was updated.
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
robot_model::RobotModelConstPtr robot_model_
std::unique_ptr< message_filters::Subscriber< moveit_msgs::CollisionObject > > collision_object_subscriber_
void setStateUpdateFrequency(double hz)
Update the scene using the monitored state at a specified frequency, in Hz. This function has an effe...
void currentStateAttachedBodyUpdateCallback(robot_state::AttachedBody *attached_body, bool just_attached)
Callback for a change for an attached object of the current state of the planning scene...
bool updatesScene(const planning_scene::PlanningSceneConstPtr &scene) const
Return true if the scene scene can be updated directly or indirectly by this monitor. This function will return true if the pointer of the scene is the same as the one maintained, or if a parent of the scene is the one maintained.
boost::function< void(const ObjectConstPtr &, Action)> ObserverCallbackFn
TFSIMD_FORCE_INLINE const tfScalar & x() const
void excludeAttachedBodyFromOctree(const robot_state::AttachedBody *attached_body)
#define ROS_DEBUG_NAMED(name,...)
void setupScene(ros::NodeHandle &nh, const planning_scene::PlanningScenePtr &scene)
This can be called on a new planning scene to setup the collision detector.
ros::Subscriber attached_collision_object_subscriber_
std::unique_ptr< boost::thread > publish_planning_scene_
void getMonitoredTopics(std::vector< std::string > &topics) const
Get the topic names that the monitor is listening to.
void updateFrameTransforms()
Update the transforms for the frames that are not part of the kinematic model using tf...
ros::WallTime last_robot_state_update_wall_time_
Last time the state was updated from current_state_monitor_.
std::unique_ptr< tf::MessageFilter< moveit_msgs::CollisionObject > > collision_object_filter_
TFSIMD_FORCE_INLINE const tfScalar & z() const
void startWorldGeometryMonitor(const std::string &collision_objects_topic=DEFAULT_COLLISION_OBJECT_TOPIC, const std::string &planning_scene_world_topic=DEFAULT_PLANNING_SCENE_WORLD_TOPIC, const bool load_octomap_monitor=true)
Start listening for objects in the world, the collision map and attached collision objects...
bool waitForExistence(ros::Duration timeout=ros::Duration(-1))
WallDuration & fromSec(double t)
std::unique_ptr< occupancy_map_monitor::OccupancyMapMonitor > octomap_monitor_
PlanningSceneMonitor Subscribes to the topic planning_scene.
std::map< std::string, double > default_robot_link_scale_
default robot link scale
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
double publish_planning_scene_frequency_
void setCallbackQueue(CallbackQueueInterface *queue)
bool waitForCurrentRobotState(const ros::Time &t, double wait_time=1.)
Wait for robot state to become more recent than time t.
ros::NodeHandle nh_
Last time the robot has moved.
void initialize(const planning_scene::PlanningScenePtr &scene)
Initialize the planning scene monitor.
std::string getTopic() const
static const std::string DEFAULT_PLANNING_SCENE_TOPIC
The name of the topic used by default for receiving full planning scenes or planning scene diffs...
TFSIMD_FORCE_INLINE const tfScalar & y() const
void updateSceneWithCurrentState()
Update the scene using the monitored state. This function is automatically called when an update to t...
ros::WallTimer state_update_timer_
timer for state updates.
#define ROS_ERROR_THROTTLE_NAMED(rate, name,...)
WallTimer createWallTimer(WallDuration period, void(T::*callback)(const WallTimerEvent &), T *obj, bool oneshot=false, bool autostart=true) const
SceneUpdateType publish_update_types_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void includeRobotLinksInOctree()
void startStateMonitor(const std::string &joint_states_topic=DEFAULT_JOINT_STATES_TOPIC, const std::string &attached_objects_topic=DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC)
Start the current state monitor.
void configureCollisionMatrix(const planning_scene::PlanningScenePtr &scene)
Configure the collision matrix for a particular scene.
boost::condition_variable_any new_scene_update_condition_
boost::shared_ptr< tf::Transformer > tf_
void startPublishingPlanningScene(SceneUpdateType event, const std::string &planning_scene_topic=MONITORED_PLANNING_SCENE_TOPIC)
Start publishing the maintained planning scene. The first message set out is a complete planning scen...
void unlockSceneRead()
Unlock the scene from reading (multiple threads can lock for reading at the same time) ...
void includeWorldObjectsInOctree()
void stateUpdateTimerCallback(const ros::WallTimerEvent &event)
ros::Time last_update_time_
mutex for stored scene
static const std::string DEFAULT_COLLISION_OBJECT_TOPIC
The name of the topic used by default for receiving collision objects in the world.
void includeAttachedBodiesInOctree()
bool hasMember(const std::string &name) const
std::vector< boost::function< void(SceneUpdateType)> > update_callbacks_
std::string robot_description_
void collisionObjectFailTFCallback(const moveit_msgs::CollisionObjectConstPtr &obj, tf::filter_failure_reasons::FilterFailureReason reason)
Callback for a new collision object msg that failed to pass the TF filter.
void setEntry(const std::string &name1, const std::string &name2, bool allowed)
boost::recursive_mutex shape_handles_lock_
boost::shared_mutex scene_update_mutex_
if diffs are monitored, this is the pointer to the parent scene
void monitorDiffs(bool flag)
By default, the maintained planning scene does not reason about diffs. When the flag passed in is tru...
planning_scene::PlanningScenePtr parent_scene_
bool getParam(const std::string &key, std::string &s) const
ros::CallbackQueue queue_
bool getShapeTransformCache(const std::string &target_frame, const ros::Time &target_time, occupancy_map_monitor::ShapeTransformCache &cache) const
void stopSceneMonitor()
Stop the scene monitor.
void stopWorldGeometryMonitor()
Stop the world geometry monitor.
void excludeWorldObjectFromOctree(const collision_detection::World::ObjectConstPtr &obj)
LinkShapeHandles link_shape_handles_
bool requestPlanningSceneState(const std::string &service_name=DEFAULT_PLANNING_SCENE_SERVICE)
Request planning scene state using a service call.
#define ROS_ERROR_NAMED(name,...)
void includeWorldObjectInOctree(const collision_detection::World::ObjectConstPtr &obj)
The maintained set of fixed transforms in the monitored scene was updated.
void publishDebugInformation(bool flag)
double default_attached_padd_
default attached padding
std::string monitor_name_
The name of this scene monitor.
void transformTFToEigen(const tf::Transform &t, Eigen::Isometry3d &e)
double default_object_padd_
default object padding
ROSCPP_DECL bool exists(const std::string &service_name, bool print_failure_reason)
bool newPlanningSceneMessage(const moveit_msgs::PlanningScene &scene)
ros::Publisher planning_scene_publisher_
CollisionBodyShapeHandles collision_body_shape_handles_
CallbackQueueInterface * getCallbackQueue() const
bool hasParam(const std::string &key) const
void currentWorldObjectUpdateCallback(const collision_detection::World::ObjectConstPtr &object, collision_detection::World::Action action)
Callback for a change in the world maintained by the planning scene.
void setPlanningScenePublishingFrequency(double hz)
Set the maximum frequency at which planning scenes are being published.
static const std::string MONITORED_PLANNING_SCENE_TOPIC
static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC
The name of the topic used by default for attached collision objects.
std::map< std::string, double > default_robot_link_padd_
default robot link padding
ros::Subscriber planning_scene_subscriber_
volatile bool state_update_pending_
True when we need to update the RobotState from current_state_monitor_.
void includeAttachedBodyInOctree(const robot_state::AttachedBody *attached_body)
AttachedBodyShapeHandles attached_body_shape_handles_
void newPlanningSceneCallback(const moveit_msgs::PlanningSceneConstPtr &scene)
void attachObjectCallback(const moveit_msgs::AttachedCollisionObjectConstPtr &obj)
Callback for a new attached object msg.
planning_scene::PlanningSceneConstPtr scene_const_
planning_scene::PlanningScenePtr scene_
void excludeAttachedBodiesFromOctree()
void newPlanningSceneWorldCallback(const moveit_msgs::PlanningSceneWorldConstPtr &world)
Callback for a new planning scene world.
static const std::string DEFAULT_JOINT_STATES_TOPIC
The name of the topic used by default for receiving joint states.
ros::WallDuration dt_state_update_
the amount of time to wait in between updates to the robot state
collision_detection::CollisionPluginLoader collision_loader_
The entire scene was updated.
#define ROS_WARN_STREAM_NAMED(name, args)