37 #ifndef MOVEIT_PLANNING_SCENE_MONITOR_PLANNING_SCENE_MONITOR_ 38 #define MOVEIT_PLANNING_SCENE_MONITOR_PLANNING_SCENE_MONITOR_ 51 #include <boost/noncopyable.hpp> 52 #include <boost/thread/shared_mutex.hpp> 53 #include <boost/thread/recursive_mutex.hpp> 115 const std::string& name =
"");
124 const std::string& name =
"");
132 PlanningSceneMonitor(
const planning_scene::PlanningScenePtr& scene,
const std::string& robot_description,
134 const std::string& name =
"");
143 const robot_model_loader::RobotModelLoaderPtr& rml,
145 const std::string& name =
"");
157 const robot_model_loader::RobotModelLoaderPtr& rml,
const ros::NodeHandle& nh,
159 const std::string& name =
"");
211 bool updatesScene(
const planning_scene::PlanningSceneConstPtr& scene)
const;
217 bool updatesScene(
const planning_scene::PlanningScenePtr& scene)
const;
267 const std::string& planning_scene_topic = MONITORED_PLANNING_SCENE_TOPIC);
303 void startStateMonitor(
const std::string& joint_states_topic = DEFAULT_JOINT_STATES_TOPIC,
304 const std::string& attached_objects_topic = DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC);
333 void startSceneMonitor(
const std::string& scene_topic = DEFAULT_PLANNING_SCENE_TOPIC);
353 const std::string& planning_scene_world_topic = DEFAULT_PLANNING_SCENE_WORLD_TOPIC,
354 const bool load_octomap_monitor =
true);
409 void initialize(
const planning_scene::PlanningScenePtr& scene);
509 typedef std::map<
const robot_model::LinkModel*,
510 std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > >
512 typedef std::map<
const robot_state::AttachedBody*,
513 std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > >
515 typedef std::map<std::string, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, const Eigen::Affine3d*> > >
535 void onStateUpdate(
const sensor_msgs::JointStateConstPtr& joint_state);
602 : planning_scene_monitor_(planning_scene_monitor)
609 return planning_scene_monitor_;
612 operator bool()
const 614 return planning_scene_monitor_ && planning_scene_monitor_->getPlanningScene();
617 operator const planning_scene::PlanningSceneConstPtr&()
const 622 const planning_scene::PlanningSceneConstPtr&
operator->()
const 629 : planning_scene_monitor_(planning_scene_monitor)
636 if (planning_scene_monitor_)
637 lock_.reset(
new SingleUnlock(planning_scene_monitor_.get(), read_only));
647 : planning_scene_monitor_(planning_scene_monitor), read_only_(read_only)
650 planning_scene_monitor_->lockSceneRead();
652 planning_scene_monitor_->lockSceneWrite();
657 planning_scene_monitor_->unlockSceneRead();
659 planning_scene_monitor_->unlockSceneWrite();
698 operator const planning_scene::PlanningScenePtr&()
700 return planning_scene_monitor_->getPlanningScene();
705 return planning_scene_monitor_->getPlanningScene();
std::map< const robot_state::AttachedBody *, std::vector< std::pair< occupancy_map_monitor::ShapeHandle, std::size_t > > > AttachedBodyShapeHandles
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_
CurrentStateMonitorPtr & getStateMonitorNonConst()
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()
double getDefaultRobotPadding() const
Get the default robot padding.
const PlanningSceneMonitorPtr & getPlanningSceneMonitor()
void getUpdatedFrameTransforms(std::vector< geometry_msgs::TransformStamped > &transforms)
std::map< const robot_model::LinkModel *, std::vector< std::pair< occupancy_map_monitor::ShapeHandle, std::size_t > > > LinkShapeHandles
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...
void initialize(bool read_only)
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...
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)
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.
const robot_model_loader::RobotModelLoaderPtr & getRobotModelLoader() const
Get the user kinematic model loader.
const robot_model::RobotModelConstPtr & getRobotModel() const
ros::Duration shape_transform_cache_lookup_wait_time_
the amount of time to wait when looking up transforms
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_
double getDefaultRobotScale() const
Get the default robot scaling.
double getPlanningScenePublishingFrequency() const
Get the maximum frequency at which planning scenes are published (Hz)
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.
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.
const planning_scene::PlanningScenePtr & getPlanningScene()
Avoid this function! Returns an unsafe pointer to the current planning scene.
LockedPlanningSceneRO(const PlanningSceneMonitorPtr &planning_scene_monitor)
const planning_scene::PlanningSceneConstPtr & getPlanningScene() const
Avoid this function! Returns an unsafe pointer to the current planning scene.
void excludeAttachedBodyFromOctree(const robot_state::AttachedBody *attached_body)
const planning_scene::PlanningSceneConstPtr & operator->() const
ros::Subscriber attached_collision_object_subscriber_
std::unique_ptr< boost::thread > publish_planning_scene_
const CurrentStateMonitorPtr & getStateMonitor() const
Get the stored instance of the stored current state monitor.
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_
double getStateUpdateFrequency() const
Get the maximum frequency (Hz) at which the current state of the planning scene is updated...
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...
double getDefaultObjectPadding() const
Get the default object padding.
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
std::map< std::string, std::vector< std::pair< occupancy_map_monitor::ShapeHandle, const Eigen::Affine3d * > > > CollisionBodyShapeHandles
double publish_planning_scene_frequency_
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.
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...
void updateSceneWithCurrentState()
Update the scene using the monitored state. This function is automatically called when an update to t...
const planning_scene::PlanningScenePtr & operator->()
PlanningSceneMonitor * planning_scene_monitor_
ros::WallTimer state_update_timer_
timer for state updates.
PlanningSceneMonitorPtr planning_scene_monitor_
SceneUpdateType publish_update_types_
void includeRobotLinksInOctree()
double getDefaultAttachedObjectPadding() const
Get the default attached padding.
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...
LockedPlanningSceneRO(const PlanningSceneMonitorPtr &planning_scene_monitor, bool read_only)
This is a convenience class for obtaining access to an instance of a locked PlanningScene.
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()
MOVEIT_CLASS_FORWARD(CurrentStateMonitor)
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.
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_
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.
This is used to load the collision plugin.
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.
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.
double default_object_padd_
default object padding
bool newPlanningSceneMessage(const moveit_msgs::PlanningScene &scene)
ros::Publisher planning_scene_publisher_
const std::string & getRobotDescription() const
Get the stored robot description.
CollisionBodyShapeHandles collision_body_shape_handles_
const std::string & getName() const
Get the name of this monitor.
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
LockedPlanningSceneRW(const PlanningSceneMonitorPtr &planning_scene_monitor)
ros::Subscriber planning_scene_subscriber_
SingleUnlock(PlanningSceneMonitor *planning_scene_monitor, bool read_only)
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)
const boost::shared_ptr< tf::Transformer > & getTFClient() const
Get the instance of the TF client that was passed to the constructor of this class.
This is a convenience class for obtaining access to an instance of a locked PlanningScene.
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_
const ros::Time & getLastUpdateTime() const
Return the time when the last update was made to the planning scene (by any monitor) ...
The entire scene was updated.