PlanningSceneMonitor Subscribes to the topic planning_scene. More...
#include <planning_scene_monitor.h>
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 | 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::Time & | getLastUpdateTime () 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 () |
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. | |
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. | |
~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 = "planning_scene_world" |
The name of the topic used by default for receiving geometry information about a planning scene (complete overwrite of world geometry) | |
static const std::string | MONITORED_PLANNING_SCENE_TOPIC = "monitored_planning_scene" |
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) | |
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_ |
List of callbacks to trigger when updates are received. | |
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 | newPlanningSceneMessage (const moveit_msgs::PlanningScene &scene) |
void | onStateUpdate (const sensor_msgs::JointStateConstPtr &joint_state) |
void | scenePublishingThread () |
void | stateUpdateTimerCallback (const ros::WallTimerEvent &event) |
Private Attributes | |
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_. | |
DynamicReconfigureImpl * | reconfigure_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. |
PlanningSceneMonitor Subscribes to the topic planning_scene.
Definition at line 58 of file planning_scene_monitor.h.
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 449 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 450 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 448 of file planning_scene_monitor.h.
Definition at line 62 of file planning_scene_monitor.h.
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.
robot_description | The name of the ROS parameter that contains the URDF (in string format) |
tf | A pointer to a tf::Transformer |
name | A name identifying this planning scene monitor |
Definition at line 112 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.
rml | A pointer to a kinematic model loader |
tf | A pointer to a tf::Transformer |
name | A name identifying this planning scene monitor |
Definition at line 127 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.
scene | The scene instance to maintain up to date with monitored information |
robot_description | The name of the ROS parameter that contains the URDF (in string format) |
tf | A pointer to a tf::Transformer |
name | A name identifying this planning scene monitor |
Definition at line 119 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.
scene | The scene instance to maintain up to date with monitored information |
rml | A pointer to a kinematic model loader |
tf | A pointer to a tf::Transformer |
name | A name identifying this planning scene monitor |
Definition at line 134 of file planning_scene_monitor.cpp.
Definition at line 141 of file planning_scene_monitor.cpp.
void planning_scene_monitor::PlanningSceneMonitor::addUpdateCallback | ( | const boost::function< void(SceneUpdateType)> & | fn | ) |
Add a function to be called when an update to the scene is received.
Definition at line 1117 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 572 of file planning_scene_monitor.cpp.
Clear the functions to be called when an update to the scene is received.
Definition at line 1124 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 558 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 551 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 1209 of file planning_scene_monitor.cpp.
void planning_scene_monitor::PlanningSceneMonitor::configureDefaultPadding | ( | ) | [protected] |
Configure the default padding.
Definition at line 1251 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 756 of file planning_scene_monitor.cpp.
void planning_scene_monitor::PlanningSceneMonitor::currentWorldObjectUpdateCallback | ( | const collision_detection::World::ObjectConstPtr & | object, |
collision_detection::World::Action | action | ||
) | [protected] |
Callback for a change in the world maintained by the planning scene.
Definition at line 767 of file planning_scene_monitor.cpp.
void planning_scene_monitor::PlanningSceneMonitor::excludeAttachedBodiesFromOctree | ( | ) | [protected] |
Definition at line 642 of file planning_scene_monitor.cpp.
void planning_scene_monitor::PlanningSceneMonitor::excludeAttachedBodyFromOctree | ( | const robot_state::AttachedBody * | attached_body | ) | [protected] |
Definition at line 677 of file planning_scene_monitor.cpp.
void planning_scene_monitor::PlanningSceneMonitor::excludeRobotLinksFromOctree | ( | ) | [protected] |
Definition at line 586 of file planning_scene_monitor.cpp.
void planning_scene_monitor::PlanningSceneMonitor::excludeWorldObjectFromOctree | ( | const collision_detection::World::ObjectConstPtr & | obj | ) | [protected] |
Definition at line 716 of file planning_scene_monitor.cpp.
void planning_scene_monitor::PlanningSceneMonitor::excludeWorldObjectsFromOctree | ( | ) | [protected] |
Definition at line 668 of file planning_scene_monitor.cpp.
double planning_scene_monitor::PlanningSceneMonitor::getDefaultAttachedObjectPadding | ( | ) | const [inline] |
Get the default attached padding.
Definition at line 224 of file planning_scene_monitor.h.
double planning_scene_monitor::PlanningSceneMonitor::getDefaultObjectPadding | ( | ) | const [inline] |
Get the default object padding.
Definition at line 218 of file planning_scene_monitor.h.
double planning_scene_monitor::PlanningSceneMonitor::getDefaultRobotPadding | ( | ) | const [inline] |
Get the default robot padding.
Definition at line 206 of file planning_scene_monitor.h.
double planning_scene_monitor::PlanningSceneMonitor::getDefaultRobotScale | ( | ) | const [inline] |
Get the default robot scaling.
Definition at line 212 of file planning_scene_monitor.h.
const ros::Time& planning_scene_monitor::PlanningSceneMonitor::getLastUpdateTime | ( | ) | const [inline] |
Return the time when the last update was made to the planning scene (by any monitor)
Definition at line 328 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 364 of file planning_scene_monitor.cpp.
const std::string& planning_scene_monitor::PlanningSceneMonitor::getName | ( | void | ) | const [inline] |
Get the name of this monitor.
Definition at line 143 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.
Definition at line 173 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.
Definition at line 181 of file planning_scene_monitor.h.
double planning_scene_monitor::PlanningSceneMonitor::getPlanningScenePublishingFrequency | ( | ) | const [inline] |
Get the maximum frequency at which planning scenes are published (Hz)
Definition at line 252 of file planning_scene_monitor.h.
const std::string& planning_scene_monitor::PlanningSceneMonitor::getRobotDescription | ( | ) | const [inline] |
Get the stored robot description.
Definition at line 200 of file planning_scene_monitor.h.
const robot_model::RobotModelConstPtr& planning_scene_monitor::PlanningSceneMonitor::getRobotModel | ( | ) | const [inline] |
Definition at line 154 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 149 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 836 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.
Definition at line 259 of file planning_scene_monitor.h.
Get the maximum frequency (Hz) at which the current state of the planning scene is updated.
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 230 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 1136 of file planning_scene_monitor.cpp.
void planning_scene_monitor::PlanningSceneMonitor::includeAttachedBodiesInOctree | ( | ) | [protected] |
Definition at line 628 of file planning_scene_monitor.cpp.
void planning_scene_monitor::PlanningSceneMonitor::includeAttachedBodyInOctree | ( | const robot_state::AttachedBody * | attached_body | ) | [protected] |
Definition at line 699 of file planning_scene_monitor.cpp.
void planning_scene_monitor::PlanningSceneMonitor::includeRobotLinksInOctree | ( | ) | [protected] |
Definition at line 615 of file planning_scene_monitor.cpp.
void planning_scene_monitor::PlanningSceneMonitor::includeWorldObjectInOctree | ( | const collision_detection::World::ObjectConstPtr & | obj | ) | [protected] |
Definition at line 739 of file planning_scene_monitor.cpp.
void planning_scene_monitor::PlanningSceneMonitor::includeWorldObjectsInOctree | ( | ) | [protected] |
Definition at line 654 of file planning_scene_monitor.cpp.
void planning_scene_monitor::PlanningSceneMonitor::initialize | ( | const planning_scene::PlanningScenePtr & | scene | ) | [protected] |
Initialize the planning scene monitor.
scene | The scene instance to fill with data (an instance is allocated if the one passed in is not allocated) |
Definition at line 161 of file planning_scene_monitor.cpp.
Lock the scene for reading (multiple threads can lock for reading at the same time)
Definition at line 786 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 800 of file planning_scene_monitor.cpp.
void planning_scene_monitor::PlanningSceneMonitor::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.
Definition at line 235 of file planning_scene_monitor.cpp.
void planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneCallback | ( | const moveit_msgs::PlanningSceneConstPtr & | scene | ) | [private] |
Definition at line 453 of file planning_scene_monitor.cpp.
void planning_scene_monitor::PlanningSceneMonitor::newPlanningSceneMessage | ( | const moveit_msgs::PlanningScene & | scene | ) | [private] |
Definition at line 458 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 527 of file planning_scene_monitor.cpp.
void planning_scene_monitor::PlanningSceneMonitor::octomapUpdateCallback | ( | ) | [protected] |
Callback for octomap updates.
Definition at line 1045 of file planning_scene_monitor.cpp.
void planning_scene_monitor::PlanningSceneMonitor::onStateUpdate | ( | const sensor_msgs::JointStateConstPtr & | joint_state | ) | [private] |
Definition at line 993 of file planning_scene_monitor.cpp.
void planning_scene_monitor::PlanningSceneMonitor::publishDebugInformation | ( | bool | flag | ) |
Definition at line 1203 of file planning_scene_monitor.cpp.
bool planning_scene_monitor::PlanningSceneMonitor::requestPlanningSceneState | ( | const std::string & | service_name = DEFAULT_PLANNING_SCENE_SERVICE | ) |
Request planning scene state using a service call.
service_name | The 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 414 of file planning_scene_monitor.cpp.
void planning_scene_monitor::PlanningSceneMonitor::scenePublishingThread | ( | ) | [private] |
Definition at line 305 of file planning_scene_monitor.cpp.
Set the maximum frequency at which planning scenes are being published.
Definition at line 1130 of file planning_scene_monitor.cpp.
void planning_scene_monitor::PlanningSceneMonitor::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.
hz | the update frequency. By default this is 10Hz. |
Definition at line 1069 of file planning_scene_monitor.cpp.
void planning_scene_monitor::PlanningSceneMonitor::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.
Definition at line 293 of file planning_scene_monitor.cpp.
void planning_scene_monitor::PlanningSceneMonitor::startSceneMonitor | ( | const std::string & | scene_topic = DEFAULT_PLANNING_SCENE_TOPIC | ) |
Start the scene monitor.
scene_topic | The name of the planning scene topic |
Definition at line 814 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.
joint_states_topic | the topic to listen to for joint states |
attached_objects_topic | the topic to listen to for attached collision objects |
Definition at line 951 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.
collision_objects_topic | The topic on which to listen for collision objects |
planning_scene_world_topic | The topic to listen to for world scene geometry |
load_octomap_monitor | Flag to disable octomap monitor if desired |
Definition at line 884 of file planning_scene_monitor.cpp.
void planning_scene_monitor::PlanningSceneMonitor::stateUpdateTimerCallback | ( | const ros::WallTimerEvent & | event | ) | [private] |
Definition at line 1019 of file planning_scene_monitor.cpp.
Stop publishing the maintained planning scene.
Definition at line 279 of file planning_scene_monitor.cpp.
Stop the scene monitor.
Definition at line 827 of file planning_scene_monitor.cpp.
Stop the state monitor.
Definition at line 978 of file planning_scene_monitor.cpp.
Stop the world geometry monitor.
Definition at line 932 of file planning_scene_monitor.cpp.
void planning_scene_monitor::PlanningSceneMonitor::triggerSceneUpdateEvent | ( | SceneUpdateType | update_type | ) |
This function is called every time there is a change to the planning scene.
Definition at line 403 of file planning_scene_monitor.cpp.
Unlock the scene from reading (multiple threads can lock for reading at the same time)
Definition at line 793 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 807 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 1185 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 1094 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 398 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 393 of file planning_scene_monitor.cpp.
AttachedBodyShapeHandles planning_scene_monitor::PlanningSceneMonitor::attached_body_shape_handles_ [protected] |
Definition at line 453 of file planning_scene_monitor.h.
ros::Subscriber planning_scene_monitor::PlanningSceneMonitor::attached_collision_object_subscriber_ [protected] |
Definition at line 436 of file planning_scene_monitor.h.
CollisionBodyShapeHandles planning_scene_monitor::PlanningSceneMonitor::collision_body_shape_handles_ [protected] |
Definition at line 454 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 439 of file planning_scene_monitor.h.
boost::scoped_ptr<message_filters::Subscriber<moveit_msgs::CollisionObject> > planning_scene_monitor::PlanningSceneMonitor::collision_object_subscriber_ [protected] |
Definition at line 438 of file planning_scene_monitor.h.
CurrentStateMonitorPtr planning_scene_monitor::PlanningSceneMonitor::current_state_monitor_ [protected] |
Definition at line 445 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 84 of file planning_scene_monitor.h.
double planning_scene_monitor::PlanningSceneMonitor::default_attached_padd_ [protected] |
default attached padding
Definition at line 418 of file planning_scene_monitor.h.
const std::string planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC = "collision_object" [static] |
The name of the topic used by default for receiving collision objects in the world.
Definition at line 87 of file planning_scene_monitor.h.
const std::string planning_scene_monitor::PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC = "joint_states" [static] |
The name of the topic used by default for receiving joint states.
Definition at line 81 of file planning_scene_monitor.h.
double planning_scene_monitor::PlanningSceneMonitor::default_object_padd_ [protected] |
default object padding
Definition at line 416 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 96 of file planning_scene_monitor.h.
const std::string planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC = "planning_scene" [static] |
The name of the topic used by default for receiving full planning scenes or planning scene diffs.
Definition at line 93 of file planning_scene_monitor.h.
const std::string planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC = "planning_scene_world" [static] |
The name of the topic used by default for receiving geometry information about a planning scene (complete overwrite of world geometry)
Definition at line 90 of file planning_scene_monitor.h.
std::map<std::string, double> planning_scene_monitor::PlanningSceneMonitor::default_robot_link_padd_ [protected] |
default robot link padding
Definition at line 420 of file planning_scene_monitor.h.
std::map<std::string, double> planning_scene_monitor::PlanningSceneMonitor::default_robot_link_scale_ [protected] |
default robot link scale
Definition at line 422 of file planning_scene_monitor.h.
double planning_scene_monitor::PlanningSceneMonitor::default_robot_padd_ [protected] |
default robot padding
Definition at line 412 of file planning_scene_monitor.h.
double planning_scene_monitor::PlanningSceneMonitor::default_robot_scale_ [protected] |
default robot scaling
Definition at line 414 of file planning_scene_monitor.h.
the amount of time to wait in between updates to the robot state
Definition at line 491 of file planning_scene_monitor.h.
Last time the state was updated from current_state_monitor_.
Definition at line 505 of file planning_scene_monitor.h.
List of callbacks to trigger when updates are received.
Definition at line 460 of file planning_scene_monitor.h.
Definition at line 452 of file planning_scene_monitor.h.
std::string planning_scene_monitor::PlanningSceneMonitor::monitor_name_ [protected] |
The name of this scene monitor.
Definition at line 399 of file planning_scene_monitor.h.
const std::string planning_scene_monitor::PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC = "monitored_planning_scene" [static] |
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 99 of file planning_scene_monitor.h.
Definition at line 429 of file planning_scene_monitor.h.
boost::condition_variable_any planning_scene_monitor::PlanningSceneMonitor::new_scene_update_condition_ [protected] |
Definition at line 430 of file planning_scene_monitor.h.
mutex for stored scene
Definition at line 406 of file planning_scene_monitor.h.
boost::scoped_ptr<occupancy_map_monitor::OccupancyMapMonitor> planning_scene_monitor::PlanningSceneMonitor::octomap_monitor_ [protected] |
Definition at line 442 of file planning_scene_monitor.h.
planning_scene::PlanningScenePtr planning_scene_monitor::PlanningSceneMonitor::parent_scene_ [protected] |
Definition at line 403 of file planning_scene_monitor.h.
Definition at line 425 of file planning_scene_monitor.h.
ros::Subscriber planning_scene_monitor::PlanningSceneMonitor::planning_scene_subscriber_ [protected] |
Definition at line 433 of file planning_scene_monitor.h.
ros::Subscriber planning_scene_monitor::PlanningSceneMonitor::planning_scene_world_subscriber_ [protected] |
Definition at line 434 of file planning_scene_monitor.h.
boost::scoped_ptr<boost::thread> planning_scene_monitor::PlanningSceneMonitor::publish_planning_scene_ [protected] |
Definition at line 426 of file planning_scene_monitor.h.
Definition at line 427 of file planning_scene_monitor.h.
Definition at line 428 of file planning_scene_monitor.h.
Definition at line 510 of file planning_scene_monitor.h.
robot_model_loader::RobotModelLoaderPtr planning_scene_monitor::PlanningSceneMonitor::rm_loader_ [private] |
Definition at line 507 of file planning_scene_monitor.h.
std::string planning_scene_monitor::PlanningSceneMonitor::robot_description_ [protected] |
Definition at line 409 of file planning_scene_monitor.h.
robot_model::RobotModelConstPtr planning_scene_monitor::PlanningSceneMonitor::robot_model_ [private] |
Definition at line 508 of file planning_scene_monitor.h.
Definition at line 407 of file planning_scene_monitor.h.
Definition at line 401 of file planning_scene_monitor.h.
planning_scene::PlanningSceneConstPtr planning_scene_monitor::PlanningSceneMonitor::scene_const_ [protected] |
Definition at line 402 of file planning_scene_monitor.h.
boost::shared_mutex planning_scene_monitor::PlanningSceneMonitor::scene_update_mutex_ [protected] |
if diffs are monitored, this is the pointer to the parent scene
Definition at line 404 of file planning_scene_monitor.h.
boost::recursive_mutex planning_scene_monitor::PlanningSceneMonitor::shape_handles_lock_ [mutable, protected] |
Definition at line 455 of file planning_scene_monitor.h.
ros::Duration planning_scene_monitor::PlanningSceneMonitor::shape_transform_cache_lookup_wait_time_ [private] |
the amount of time to wait when looking up transforms
Definition at line 496 of file planning_scene_monitor.h.
boost::mutex planning_scene_monitor::PlanningSceneMonitor::state_pending_mutex_ [private] |
Definition at line 483 of file planning_scene_monitor.h.
volatile bool planning_scene_monitor::PlanningSceneMonitor::state_update_pending_ [private] |
True when we need to update the RobotState from current_state_monitor_.
Definition at line 487 of file planning_scene_monitor.h.
timer for state updates.
Definition at line 501 of file planning_scene_monitor.h.
boost::shared_ptr<tf::Transformer> planning_scene_monitor::PlanningSceneMonitor::tf_ [protected] |
Definition at line 408 of file planning_scene_monitor.h.
std::vector<boost::function<void(SceneUpdateType)> > planning_scene_monitor::PlanningSceneMonitor::update_callbacks_ [protected] |
Definition at line 459 of file planning_scene_monitor.h.
boost::recursive_mutex planning_scene_monitor::PlanningSceneMonitor::update_lock_ [protected] |
lock access to update_callbacks_
Definition at line 458 of file planning_scene_monitor.h.