Classes | Public Types | Public Member Functions | Static Public Attributes | Protected Types | Protected Member Functions | Protected Attributes | Private Member Functions | Private Attributes
planning_scene_monitor::PlanningSceneMonitor Class Reference

PlanningSceneMonitor Subscribes to the topic planning_scene. More...

#include <planning_scene_monitor.h>

Inheritance diagram for planning_scene_monitor::PlanningSceneMonitor:
Inheritance graph
[legend]

List of all members.

Classes

class  DynamicReconfigureImpl

Public Types

enum  SceneUpdateType {
  UPDATE_NONE = 0, UPDATE_STATE = 1, UPDATE_TRANSFORMS = 2, UPDATE_GEOMETRY = 4,
  UPDATE_SCENE = 8 + UPDATE_STATE + UPDATE_TRANSFORMS + UPDATE_GEOMETRY
}

Public Member Functions

void addUpdateCallback (const boost::function< void(SceneUpdateType)> &fn)
 Add a function to be called when an update to the scene is received.
void clearOctomap ()
void clearUpdateCallbacks ()
 Clear the functions to be called when an update to the scene is received.
double getDefaultAttachedObjectPadding () const
 Get the default attached padding.
double getDefaultObjectPadding () const
 Get the default object padding.
double getDefaultRobotPadding () const
 Get the default robot padding.
double getDefaultRobotScale () const
 Get the default robot scaling.
const ros::TimegetLastUpdateTime () const
 Return the time when the last update was made to the planning scene (by any monitor)
void getMonitoredTopics (std::vector< std::string > &topics) const
 Get the topic names that the monitor is listening to.
const std::string & getName () const
 Get the name of this monitor.
const
planning_scene::PlanningScenePtr & 
getPlanningScene ()
 Avoid this function! Returns an unsafe pointer to the current planning scene.
const
planning_scene::PlanningSceneConstPtr & 
getPlanningScene () const
 Avoid this function! Returns an unsafe pointer to the current planning scene.
double getPlanningScenePublishingFrequency () const
 Get the maximum frequency at which planning scenes are published (Hz)
const std::string & getRobotDescription () const
 Get the stored robot description.
const
robot_model::RobotModelConstPtr & 
getRobotModel () const
const
robot_model_loader::RobotModelLoaderPtr & 
getRobotModelLoader () const
 Get the user kinematic model loader.
const CurrentStateMonitorPtr & getStateMonitor () const
 Get the stored instance of the stored current state monitor.
double getStateUpdateFrequency () const
 Get the maximum frequency (Hz) at which the current state of the planning scene is updated.
const boost::shared_ptr
< tf::Transformer > & 
getTFClient () const
 Get the instance of the TF client that was passed to the constructor of this class.
void lockSceneRead ()
 Lock the scene for reading (multiple threads can lock for reading at the same time)
void lockSceneWrite ()
 Lock the scene for writing (only one thread can lock for writing and no other thread can lock for reading)
void monitorDiffs (bool flag)
 By default, the maintained planning scene does not reason about diffs. When the flag passed in is true, the maintained scene starts counting diffs. Future updates to the planning scene will be stored as diffs and can be retrieved as such. Setting the flag to false restores the default behaviour. Maintaining diffs is automatically enabled when publishing planning scenes.
bool newPlanningSceneMessage (const moveit_msgs::PlanningScene &scene)
 PlanningSceneMonitor (const std::string &robot_description, const boost::shared_ptr< tf::Transformer > &tf=boost::shared_ptr< tf::Transformer >(), const std::string &name="")
 Constructor.
 PlanningSceneMonitor (const robot_model_loader::RobotModelLoaderPtr &rml, const boost::shared_ptr< tf::Transformer > &tf=boost::shared_ptr< tf::Transformer >(), const std::string &name="")
 Constructor.
 PlanningSceneMonitor (const planning_scene::PlanningScenePtr &scene, const std::string &robot_description, const boost::shared_ptr< tf::Transformer > &tf=boost::shared_ptr< tf::Transformer >(), const std::string &name="")
 Constructor.
 PlanningSceneMonitor (const planning_scene::PlanningScenePtr &scene, const robot_model_loader::RobotModelLoaderPtr &rml, const boost::shared_ptr< tf::Transformer > &tf=boost::shared_ptr< tf::Transformer >(), const std::string &name="")
 Constructor.
void publishDebugInformation (bool flag)
bool requestPlanningSceneState (const std::string &service_name=DEFAULT_PLANNING_SCENE_SERVICE)
 Request planning scene state using a service call.
void setPlanningScenePublishingFrequency (double hz)
 Set the maximum frequency at which planning scenes are being published.
void setStateUpdateFrequency (double hz)
 Update the scene using the monitored state at a specified frequency, in Hz. This function has an effect only when updates from the CurrentStateMonitor are received at a higher frequency. In that case, the updates are throttled down, so that they do not exceed a maximum update frequency specified here.
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 scene. Diffs are sent afterwards on updates specified by the event bitmask. For UPDATE_SCENE, the full scene is always sent.
void startSceneMonitor (const std::string &scene_topic=DEFAULT_PLANNING_SCENE_TOPIC)
 Start the scene monitor.
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 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. Additionally, this function starts the OccupancyMapMonitor as well.
void stopPublishingPlanningScene ()
 Stop publishing the maintained planning scene.
void stopSceneMonitor ()
 Stop the scene monitor.
void stopStateMonitor ()
 Stop the state monitor.
void stopWorldGeometryMonitor ()
 Stop the world geometry monitor.
void triggerSceneUpdateEvent (SceneUpdateType update_type)
 This function is called every time there is a change to the planning scene.
void unlockSceneRead ()
 Unlock the scene from reading (multiple threads can lock for reading at the same time)
void unlockSceneWrite ()
 Lock the scene from writing (only one thread can lock for writing and no other thread can lock for reading)
void updateFrameTransforms ()
 Update the transforms for the frames that are not part of the kinematic model using tf. Examples of these frames are the "map" and "odom_combined" transforms. This function is automatically called when data that uses transforms is received. However, this function should also be called before starting a planning request, for example.
void updateSceneWithCurrentState ()
 Update the scene using the monitored state. This function is automatically called when an update to the current state is received (if startStateMonitor() has been called). The updates are throttled to a maximum update frequency however, which is set by setStateUpdateFrequency().
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.
bool updatesScene (const planning_scene::PlanningScenePtr &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.
bool waitForCurrentRobotState (const ros::Time &t, double wait_time=1.)
 Wait for robot state to become more recent than time t.
 ~PlanningSceneMonitor ()

Static Public Attributes

static const std::string DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC = "attached_collision_object"
 The name of the topic used by default for attached collision objects.
static const std::string DEFAULT_COLLISION_OBJECT_TOPIC = "collision_object"
 The name of the topic used by default for receiving collision objects in the world.
static const std::string DEFAULT_JOINT_STATES_TOPIC = "joint_states"
 The name of the topic used by default for receiving joint states.
static const std::string DEFAULT_PLANNING_SCENE_SERVICE = "get_planning_scene"
 The name of the service used by default for requesting full planning scene state.
static const std::string DEFAULT_PLANNING_SCENE_TOPIC = "planning_scene"
 The name of the topic used by default for receiving full planning scenes or planning scene diffs.
static const std::string DEFAULT_PLANNING_SCENE_WORLD_TOPIC = "world"
static const std::string MONITORED_PLANNING_SCENE_TOPIC = "scene"

Protected Types

typedef std::map< const
robot_state::AttachedBody
*, std::vector< std::pair
< occupancy_map_monitor::ShapeHandle,
std::size_t > > > 
AttachedBodyShapeHandles
typedef std::map< std::string,
std::vector< std::pair
< occupancy_map_monitor::ShapeHandle,
Eigen::Affine3d * > > > 
CollisionBodyShapeHandles
typedef std::map< const
robot_model::LinkModel
*, std::vector< std::pair
< occupancy_map_monitor::ShapeHandle,
std::size_t > > > 
LinkShapeHandles

Protected Member Functions

void attachObjectCallback (const moveit_msgs::AttachedCollisionObjectConstPtr &obj)
 Callback for a new attached object msg.
void collisionObjectCallback (const moveit_msgs::CollisionObjectConstPtr &obj)
 Callback for a new collision object msg.
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 configureCollisionMatrix (const planning_scene::PlanningScenePtr &scene)
 Configure the collision matrix for a particular scene.
void configureDefaultPadding ()
 Configure the default padding.
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.
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 excludeAttachedBodiesFromOctree ()
void excludeAttachedBodyFromOctree (const robot_state::AttachedBody *attached_body)
void excludeRobotLinksFromOctree ()
void excludeWorldObjectFromOctree (const collision_detection::World::ObjectConstPtr &obj)
void excludeWorldObjectsFromOctree ()
bool getShapeTransformCache (const std::string &target_frame, const ros::Time &target_time, occupancy_map_monitor::ShapeTransformCache &cache) const
void includeAttachedBodiesInOctree ()
void includeAttachedBodyInOctree (const robot_state::AttachedBody *attached_body)
void includeRobotLinksInOctree ()
void includeWorldObjectInOctree (const collision_detection::World::ObjectConstPtr &obj)
void includeWorldObjectsInOctree ()
void initialize (const planning_scene::PlanningScenePtr &scene)
 Initialize the planning scene monitor.
void newPlanningSceneWorldCallback (const moveit_msgs::PlanningSceneWorldConstPtr &world)
 Callback for a new planning scene world.
void octomapUpdateCallback ()
 Callback for octomap updates.

Protected Attributes

AttachedBodyShapeHandles attached_body_shape_handles_
ros::Subscriber attached_collision_object_subscriber_
CollisionBodyShapeHandles collision_body_shape_handles_
boost::scoped_ptr
< tf::MessageFilter
< moveit_msgs::CollisionObject > > 
collision_object_filter_
boost::scoped_ptr
< message_filters::Subscriber
< moveit_msgs::CollisionObject > > 
collision_object_subscriber_
CurrentStateMonitorPtr current_state_monitor_
double default_attached_padd_
 default attached padding
double default_object_padd_
 default object padding
std::map< std::string, double > default_robot_link_padd_
 default robot link padding
std::map< std::string, double > default_robot_link_scale_
 default robot link scale
double default_robot_padd_
 default robot padding
double default_robot_scale_
 default robot scaling
ros::Time last_update_time_
LinkShapeHandles link_shape_handles_
std::string monitor_name_
 The name of this scene monitor.
SceneUpdateType new_scene_update_
boost::condition_variable_any new_scene_update_condition_
ros::NodeHandle nh_
 mutex for stored scene
boost::scoped_ptr
< occupancy_map_monitor::OccupancyMapMonitor
octomap_monitor_
planning_scene::PlanningScenePtr parent_scene_
ros::Publisher planning_scene_publisher_
ros::Subscriber planning_scene_subscriber_
ros::Subscriber planning_scene_world_subscriber_
boost::scoped_ptr< boost::thread > publish_planning_scene_
double publish_planning_scene_frequency_
SceneUpdateType publish_update_types_
std::string robot_description_
ros::NodeHandle root_nh_
planning_scene::PlanningScenePtr scene_
planning_scene::PlanningSceneConstPtr scene_const_
boost::shared_mutex scene_update_mutex_
 if diffs are monitored, this is the pointer to the parent scene
boost::recursive_mutex shape_handles_lock_
boost::shared_ptr
< tf::Transformer
tf_
std::vector< boost::function
< void(SceneUpdateType)> > 
update_callbacks_
boost::recursive_mutex update_lock_
 lock access to update_callbacks_

Private Member Functions

void getUpdatedFrameTransforms (std::vector< geometry_msgs::TransformStamped > &transforms)
 Last time the state was updated.
void newPlanningSceneCallback (const moveit_msgs::PlanningSceneConstPtr &scene)
void onStateUpdate (const sensor_msgs::JointStateConstPtr &joint_state)
void scenePublishingThread ()
void stateUpdateTimerCallback (const ros::WallTimerEvent &event)

Private Attributes

collision_detection::CollisionPluginLoader collision_loader_
ros::WallDuration dt_state_update_
 the amount of time to wait in between updates to the robot state
ros::WallTime last_state_update_
 Last time the state was updated from current_state_monitor_.
DynamicReconfigureImplreconfigure_impl_
robot_model_loader::RobotModelLoaderPtr rm_loader_
robot_model::RobotModelConstPtr robot_model_
ros::Duration shape_transform_cache_lookup_wait_time_
 the amount of time to wait when looking up transforms
boost::mutex state_pending_mutex_
volatile bool state_update_pending_
 True when we need to update the RobotState from current_state_monitor_.
ros::WallTimer state_update_timer_
 timer for state updates.

Detailed Description

PlanningSceneMonitor Subscribes to the topic planning_scene.

Definition at line 61 of file planning_scene_monitor.h.


Member Typedef Documentation

typedef std::map<const robot_state::AttachedBody*, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > > planning_scene_monitor::PlanningSceneMonitor::AttachedBodyShapeHandles [protected]

Definition at line 489 of file planning_scene_monitor.h.

typedef std::map<std::string, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, Eigen::Affine3d*> > > planning_scene_monitor::PlanningSceneMonitor::CollisionBodyShapeHandles [protected]

Definition at line 491 of file planning_scene_monitor.h.

typedef std::map<const robot_model::LinkModel*, std::vector<std::pair<occupancy_map_monitor::ShapeHandle, std::size_t> > > planning_scene_monitor::PlanningSceneMonitor::LinkShapeHandles [protected]

Definition at line 486 of file planning_scene_monitor.h.


Member Enumeration Documentation

Enumerator:
UPDATE_NONE 

No update.

UPDATE_STATE 

The state in the monitored scene was updated.

UPDATE_TRANSFORMS 

The maintained set of fixed transforms in the monitored scene was updated.

UPDATE_GEOMETRY 

The geometry of the scene was updated. This includes receiving new octomaps, collision objects, attached objects, scene geometry, etc.

UPDATE_SCENE 

The entire scene was updated.

Definition at line 64 of file planning_scene_monitor.h.


Constructor & Destructor Documentation

planning_scene_monitor::PlanningSceneMonitor::PlanningSceneMonitor ( const std::string &  robot_description,
const boost::shared_ptr< tf::Transformer > &  tf = boost::shared_ptr<tf::Transformer>(),
const std::string &  name = "" 
)

Constructor.

Parameters:
robot_descriptionThe name of the ROS parameter that contains the URDF (in string format)
tfA pointer to a tf::Transformer
nameA name identifying this planning scene monitor

Definition at line 115 of file planning_scene_monitor.cpp.

planning_scene_monitor::PlanningSceneMonitor::PlanningSceneMonitor ( const robot_model_loader::RobotModelLoaderPtr &  rml,
const boost::shared_ptr< tf::Transformer > &  tf = boost::shared_ptr<tf::Transformer>(),
const std::string &  name = "" 
)

Constructor.

Parameters:
rmlA pointer to a kinematic model loader
tfA pointer to a tf::Transformer
nameA name identifying this planning scene monitor

Definition at line 134 of file planning_scene_monitor.cpp.

planning_scene_monitor::PlanningSceneMonitor::PlanningSceneMonitor ( const planning_scene::PlanningScenePtr &  scene,
const std::string &  robot_description,
const boost::shared_ptr< tf::Transformer > &  tf = boost::shared_ptr<tf::Transformer>(),
const std::string &  name = "" 
)

Constructor.

Parameters:
sceneThe scene instance to maintain up to date with monitored information
robot_descriptionThe name of the ROS parameter that contains the URDF (in string format)
tfA pointer to a tf::Transformer
nameA name identifying this planning scene monitor

Definition at line 124 of file planning_scene_monitor.cpp.

planning_scene_monitor::PlanningSceneMonitor::PlanningSceneMonitor ( const planning_scene::PlanningScenePtr &  scene,
const robot_model_loader::RobotModelLoaderPtr &  rml,
const boost::shared_ptr< tf::Transformer > &  tf = boost::shared_ptr<tf::Transformer>(),
const std::string &  name = "" 
)

Constructor.

Parameters:
sceneThe scene instance to maintain up to date with monitored information
rmlA pointer to a kinematic model loader
tfA pointer to a tf::Transformer
nameA name identifying this planning scene monitor

Definition at line 142 of file planning_scene_monitor.cpp.

Definition at line 150 of file planning_scene_monitor.cpp.


Member Function Documentation

Add a function to be called when an update to the scene is received.

Definition at line 1258 of file planning_scene_monitor.cpp.

void planning_scene_monitor::PlanningSceneMonitor::attachObjectCallback ( const moveit_msgs::AttachedCollisionObjectConstPtr &  obj) [protected]

Callback for a new attached object msg.

Definition at line 626 of file planning_scene_monitor.cpp.

Definition at line 495 of file planning_scene_monitor.cpp.

Clear the functions to be called when an update to the scene is received.

Definition at line 1265 of file planning_scene_monitor.cpp.

void planning_scene_monitor::PlanningSceneMonitor::collisionObjectCallback ( const moveit_msgs::CollisionObjectConstPtr &  obj) [protected]

Callback for a new collision object msg.

Definition at line 611 of file planning_scene_monitor.cpp.

void planning_scene_monitor::PlanningSceneMonitor::collisionObjectFailTFCallback ( const moveit_msgs::CollisionObjectConstPtr &  obj,
tf::filter_failure_reasons::FilterFailureReason  reason 
) [protected]

Callback for a new collision object msg that failed to pass the TF filter.

Definition at line 603 of file planning_scene_monitor.cpp.

void planning_scene_monitor::PlanningSceneMonitor::configureCollisionMatrix ( const planning_scene::PlanningScenePtr &  scene) [protected]

Configure the collision matrix for a particular scene.

Definition at line 1357 of file planning_scene_monitor.cpp.

Configure the default padding.

Definition at line 1401 of file planning_scene_monitor.cpp.

void planning_scene_monitor::PlanningSceneMonitor::currentStateAttachedBodyUpdateCallback ( robot_state::AttachedBody *  attached_body,
bool  just_attached 
) [protected]

Callback for a change for an attached object of the current state of the planning scene.

Definition at line 825 of file planning_scene_monitor.cpp.

Callback for a change in the world maintained by the planning scene.

Definition at line 837 of file planning_scene_monitor.cpp.

Definition at line 705 of file planning_scene_monitor.cpp.

void planning_scene_monitor::PlanningSceneMonitor::excludeAttachedBodyFromOctree ( const robot_state::AttachedBody *  attached_body) [protected]

Definition at line 742 of file planning_scene_monitor.cpp.

Definition at line 641 of file planning_scene_monitor.cpp.

Definition at line 783 of file planning_scene_monitor.cpp.

Definition at line 732 of file planning_scene_monitor.cpp.

Get the default attached padding.

Definition at line 229 of file planning_scene_monitor.h.

Get the default object padding.

Definition at line 223 of file planning_scene_monitor.h.

Get the default robot padding.

Definition at line 211 of file planning_scene_monitor.h.

Get the default robot scaling.

Definition at line 217 of file planning_scene_monitor.h.

Return the time when the last update was made to the planning scene (by any monitor)

Definition at line 348 of file planning_scene_monitor.h.

void planning_scene_monitor::PlanningSceneMonitor::getMonitoredTopics ( std::vector< std::string > &  topics) const

Get the topic names that the monitor is listening to.

Definition at line 405 of file planning_scene_monitor.cpp.

const std::string& planning_scene_monitor::PlanningSceneMonitor::getName ( ) const [inline]

Get the name of this monitor.

Definition at line 148 of file planning_scene_monitor.h.

const planning_scene::PlanningScenePtr& planning_scene_monitor::PlanningSceneMonitor::getPlanningScene ( ) [inline]

Avoid this function! Returns an unsafe pointer to the current planning scene.

Warning:
Most likely you do not want to call this function directly. PlanningSceneMonitor has a background thread which repeatedly updates and clobbers various contents of its internal PlanningScene instance. This function just returns a pointer to that dynamic internal object. The correct thing is usually to use a LockedPlanningSceneRO or LockedPlanningSceneRW, which locks the PlanningSceneMonitor and provides safe access to the PlanningScene object.
See also:
LockedPlanningSceneRO
LockedPlanningSceneRW.
Returns:
A pointer to the current planning scene.

Definition at line 178 of file planning_scene_monitor.h.

const planning_scene::PlanningSceneConstPtr& planning_scene_monitor::PlanningSceneMonitor::getPlanningScene ( ) const [inline]

Avoid this function! Returns an unsafe pointer to the current planning scene.

Warning:
Most likely you do not want to call this function directly. PlanningSceneMonitor has a background thread which repeatedly updates and clobbers various contents of its internal PlanningScene instance. This function just returns a pointer to that dynamic internal object. The correct thing is usually to use a LockedPlanningSceneRO or LockedPlanningSceneRW, which locks the PlanningSceneMonitor and provides safe access to the PlanningScene object.
See also:
LockedPlanningSceneRO
LockedPlanningSceneRW.
Returns:
A pointer to the current planning scene.

Definition at line 186 of file planning_scene_monitor.h.

Get the maximum frequency at which planning scenes are published (Hz)

Definition at line 260 of file planning_scene_monitor.h.

Get the stored robot description.

Returns:
An instance of the stored robot description

Definition at line 205 of file planning_scene_monitor.h.

const robot_model::RobotModelConstPtr& planning_scene_monitor::PlanningSceneMonitor::getRobotModel ( ) const [inline]

Definition at line 159 of file planning_scene_monitor.h.

const robot_model_loader::RobotModelLoaderPtr& planning_scene_monitor::PlanningSceneMonitor::getRobotModelLoader ( ) const [inline]

Get the user kinematic model loader.

Definition at line 154 of file planning_scene_monitor.h.

bool planning_scene_monitor::PlanningSceneMonitor::getShapeTransformCache ( const std::string &  target_frame,
const ros::Time target_time,
occupancy_map_monitor::ShapeTransformCache cache 
) const [protected]

Definition at line 958 of file planning_scene_monitor.cpp.

const CurrentStateMonitorPtr& planning_scene_monitor::PlanningSceneMonitor::getStateMonitor ( ) const [inline]

Get the stored instance of the stored current state monitor.

Returns:
An instance of the stored current state monitor

Definition at line 267 of file planning_scene_monitor.h.

Get the maximum frequency (Hz) at which the current state of the planning scene is updated.

Definition at line 301 of file planning_scene_monitor.h.

const boost::shared_ptr<tf::Transformer>& planning_scene_monitor::PlanningSceneMonitor::getTFClient ( ) const [inline]

Get the instance of the TF client that was passed to the constructor of this class.

Definition at line 235 of file planning_scene_monitor.h.

void planning_scene_monitor::PlanningSceneMonitor::getUpdatedFrameTransforms ( std::vector< geometry_msgs::TransformStamped > &  transforms) [private]

Last time the state was updated.

Definition at line 1278 of file planning_scene_monitor.cpp.

Definition at line 690 of file planning_scene_monitor.cpp.

void planning_scene_monitor::PlanningSceneMonitor::includeAttachedBodyInOctree ( const robot_state::AttachedBody *  attached_body) [protected]

Definition at line 765 of file planning_scene_monitor.cpp.

Definition at line 677 of file planning_scene_monitor.cpp.

Definition at line 807 of file planning_scene_monitor.cpp.

Definition at line 717 of file planning_scene_monitor.cpp.

void planning_scene_monitor::PlanningSceneMonitor::initialize ( const planning_scene::PlanningScenePtr &  scene) [protected]

Initialize the planning scene monitor.

Parameters:
sceneThe scene instance to fill with data (an instance is allocated if the one passed in is not allocated)

Definition at line 170 of file planning_scene_monitor.cpp.

Lock the scene for reading (multiple threads can lock for reading at the same time)

Definition at line 907 of file planning_scene_monitor.cpp.

Lock the scene for writing (only one thread can lock for writing and no other thread can lock for reading)

Definition at line 921 of file planning_scene_monitor.cpp.

By default, the maintained planning scene does not reason about diffs. When the flag passed in is true, the maintained scene starts counting diffs. Future updates to the planning scene will be stored as diffs and can be retrieved as such. Setting the flag to false restores the default behaviour. Maintaining diffs is automatically enabled when publishing planning scenes.

Definition at line 250 of file planning_scene_monitor.cpp.

void planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneCallback ( const moveit_msgs::PlanningSceneConstPtr &  scene) [private]

Definition at line 489 of file planning_scene_monitor.cpp.

bool planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneMessage ( const moveit_msgs::PlanningScene &  scene)

Definition at line 502 of file planning_scene_monitor.cpp.

void planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneWorldCallback ( const moveit_msgs::PlanningSceneWorldConstPtr &  world) [protected]

Callback for a new planning scene world.

Definition at line 578 of file planning_scene_monitor.cpp.

Callback for octomap updates.

Definition at line 1184 of file planning_scene_monitor.cpp.

void planning_scene_monitor::PlanningSceneMonitor::onStateUpdate ( const sensor_msgs::JointStateConstPtr &  joint_state) [private]

Definition at line 1131 of file planning_scene_monitor.cpp.

Definition at line 1351 of file planning_scene_monitor.cpp.

Request planning scene state using a service call.

Parameters:
service_nameThe name of the service to use for requesting the planning scene. This must be a service of type moveit_msgs::GetPlanningScene and is usually called "/get_planning_scene".

Definition at line 457 of file planning_scene_monitor.cpp.

Definition at line 324 of file planning_scene_monitor.cpp.

Set the maximum frequency at which planning scenes are being published.

Definition at line 1271 of file planning_scene_monitor.cpp.

Update the scene using the monitored state at a specified frequency, in Hz. This function has an effect only when updates from the CurrentStateMonitor are received at a higher frequency. In that case, the updates are throttled down, so that they do not exceed a maximum update frequency specified here.

Parameters:
hzthe update frequency. By default this is 10Hz.

Definition at line 1208 of file planning_scene_monitor.cpp.

Start publishing the maintained planning scene. The first message set out is a complete planning scene. Diffs are sent afterwards on updates specified by the event bitmask. For UPDATE_SCENE, the full scene is always sent.

Definition at line 311 of file planning_scene_monitor.cpp.

Start the scene monitor.

Parameters:
scene_topicThe name of the planning scene topic

Definition at line 935 of file planning_scene_monitor.cpp.

void planning_scene_monitor::PlanningSceneMonitor::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.

Parameters:
joint_states_topicthe topic to listen to for joint states
attached_objects_topicthe topic to listen to for attached collision objects

Definition at line 1086 of file planning_scene_monitor.cpp.

void planning_scene_monitor::PlanningSceneMonitor::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. Additionally, this function starts the OccupancyMapMonitor as well.

Parameters:
collision_objects_topicThe topic on which to listen for collision objects
planning_scene_world_topicThe topic to listen to for world scene geometry
load_octomap_monitorFlag to disable octomap monitor if desired

Definition at line 1011 of file planning_scene_monitor.cpp.

Definition at line 1158 of file planning_scene_monitor.cpp.

Stop publishing the maintained planning scene.

Definition at line 297 of file planning_scene_monitor.cpp.

Stop the scene monitor.

Definition at line 949 of file planning_scene_monitor.cpp.

Stop the state monitor.

Definition at line 1116 of file planning_scene_monitor.cpp.

Stop the world geometry monitor.

Definition at line 1068 of file planning_scene_monitor.cpp.

This function is called every time there is a change to the planning scene.

Definition at line 446 of file planning_scene_monitor.cpp.

Unlock the scene from reading (multiple threads can lock for reading at the same time)

Definition at line 914 of file planning_scene_monitor.cpp.

Lock the scene from writing (only one thread can lock for writing and no other thread can lock for reading)

Definition at line 928 of file planning_scene_monitor.cpp.

Update the transforms for the frames that are not part of the kinematic model using tf. Examples of these frames are the "map" and "odom_combined" transforms. This function is automatically called when data that uses transforms is received. However, this function should also be called before starting a planning request, for example.

Definition at line 1333 of file planning_scene_monitor.cpp.

Update the scene using the monitored state. This function is automatically called when an update to the current state is received (if startStateMonitor() has been called). The updates are throttled to a maximum update frequency however, which is set by setStateUpdateFrequency().

Definition at line 1233 of file planning_scene_monitor.cpp.

bool planning_scene_monitor::PlanningSceneMonitor::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.

Definition at line 440 of file planning_scene_monitor.cpp.

bool planning_scene_monitor::PlanningSceneMonitor::updatesScene ( const planning_scene::PlanningScenePtr &  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.

Definition at line 435 of file planning_scene_monitor.cpp.

Wait for robot state to become more recent than time t.

If there is no state monitor active, there will be no scene updates. Hence, you can specify a timeout to wait for those updates. Default is 1s.

Definition at line 856 of file planning_scene_monitor.cpp.


Member Data Documentation

Definition at line 494 of file planning_scene_monitor.h.

Definition at line 473 of file planning_scene_monitor.h.

Definition at line 495 of file planning_scene_monitor.h.

Definition at line 547 of file planning_scene_monitor.h.

boost::scoped_ptr<tf::MessageFilter<moveit_msgs::CollisionObject> > planning_scene_monitor::PlanningSceneMonitor::collision_object_filter_ [protected]

Definition at line 476 of file planning_scene_monitor.h.

Definition at line 475 of file planning_scene_monitor.h.

Definition at line 482 of file planning_scene_monitor.h.

const std::string planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC = "attached_collision_object" [static]

The name of the topic used by default for attached collision objects.

Definition at line 87 of file planning_scene_monitor.h.

default attached padding

Definition at line 455 of file planning_scene_monitor.h.

The name of the topic used by default for receiving collision objects in the world.

Definition at line 90 of file planning_scene_monitor.h.

The name of the topic used by default for receiving joint states.

Definition at line 84 of file planning_scene_monitor.h.

default object padding

Definition at line 453 of file planning_scene_monitor.h.

const std::string planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_SERVICE = "get_planning_scene" [static]

The name of the service used by default for requesting full planning scene state.

Definition at line 100 of file planning_scene_monitor.h.

The name of the topic used by default for receiving full planning scenes or planning scene diffs.

Definition at line 97 of file planning_scene_monitor.h.

The name of the topic used by default for receiving geometry information about a planning scene (complete overwrite of world geometry)

Definition at line 94 of file planning_scene_monitor.h.

default robot link padding

Definition at line 457 of file planning_scene_monitor.h.

default robot link scale

Definition at line 459 of file planning_scene_monitor.h.

default robot padding

Definition at line 449 of file planning_scene_monitor.h.

default robot scaling

Definition at line 451 of file planning_scene_monitor.h.

the amount of time to wait in between updates to the robot state

Definition at line 528 of file planning_scene_monitor.h.

Last time the state was updated from current_state_monitor_.

Definition at line 542 of file planning_scene_monitor.h.

List of callbacks to trigger when updates are received

Definition at line 502 of file planning_scene_monitor.h.

Definition at line 493 of file planning_scene_monitor.h.

The name of this scene monitor.

Definition at line 436 of file planning_scene_monitor.h.

The name of the topic used by default for publishing the monitored planning scene (this is without "/" in the name, so the topic is prefixed by the node name)

Definition at line 104 of file planning_scene_monitor.h.

Definition at line 466 of file planning_scene_monitor.h.

Definition at line 467 of file planning_scene_monitor.h.

mutex for stored scene

Definition at line 443 of file planning_scene_monitor.h.

Definition at line 479 of file planning_scene_monitor.h.

planning_scene::PlanningScenePtr planning_scene_monitor::PlanningSceneMonitor::parent_scene_ [protected]

Definition at line 440 of file planning_scene_monitor.h.

Definition at line 462 of file planning_scene_monitor.h.

Definition at line 470 of file planning_scene_monitor.h.

Definition at line 471 of file planning_scene_monitor.h.

boost::scoped_ptr<boost::thread> planning_scene_monitor::PlanningSceneMonitor::publish_planning_scene_ [protected]

Definition at line 463 of file planning_scene_monitor.h.

Definition at line 464 of file planning_scene_monitor.h.

Definition at line 465 of file planning_scene_monitor.h.

Definition at line 549 of file planning_scene_monitor.h.

robot_model_loader::RobotModelLoaderPtr planning_scene_monitor::PlanningSceneMonitor::rm_loader_ [private]

Definition at line 544 of file planning_scene_monitor.h.

Definition at line 446 of file planning_scene_monitor.h.

robot_model::RobotModelConstPtr planning_scene_monitor::PlanningSceneMonitor::robot_model_ [private]

Definition at line 545 of file planning_scene_monitor.h.

Definition at line 444 of file planning_scene_monitor.h.

planning_scene::PlanningScenePtr planning_scene_monitor::PlanningSceneMonitor::scene_ [protected]

Definition at line 438 of file planning_scene_monitor.h.

planning_scene::PlanningSceneConstPtr planning_scene_monitor::PlanningSceneMonitor::scene_const_ [protected]

Definition at line 439 of file planning_scene_monitor.h.

if diffs are monitored, this is the pointer to the parent scene

Definition at line 441 of file planning_scene_monitor.h.

boost::recursive_mutex planning_scene_monitor::PlanningSceneMonitor::shape_handles_lock_ [mutable, protected]

Definition at line 496 of file planning_scene_monitor.h.

the amount of time to wait when looking up transforms

Definition at line 533 of file planning_scene_monitor.h.

Definition at line 520 of file planning_scene_monitor.h.

True when we need to update the RobotState from current_state_monitor_.

Definition at line 524 of file planning_scene_monitor.h.

timer for state updates.

Definition at line 538 of file planning_scene_monitor.h.

Definition at line 445 of file planning_scene_monitor.h.

std::vector<boost::function<void(SceneUpdateType)> > planning_scene_monitor::PlanningSceneMonitor::update_callbacks_ [protected]

Definition at line 500 of file planning_scene_monitor.h.

lock access to update_callbacks_

Definition at line 499 of file planning_scene_monitor.h.


The documentation for this class was generated from the following files:


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Jun 19 2019 19:24:16