planning_environment::KinematicModelStateMonitor Class Reference

KinematicModelStateMonitor is a class that monitors the robot state for the kinematic model defined in RobotModels If the pose is not included, the robot state is the frame of the link it attaches to the world. If the pose is included, the frame of the robot is the one in which the pose is published. More...

#include <kinematic_model_state_monitor.h>

Inheritance diagram for planning_environment::KinematicModelStateMonitor:

[legend]

List of all members.

Public Member Functions
virtual void	advertiseServices ()
bool	allJointsUpdated (ros::Duration dur=ros::Duration()) const
bool	convertKinematicStateToRobotState (const planning_models::KinematicState &state, motion_planning_msgs::RobotState &robot_state) const
bool	getCachedJointStateValues (const ros::Time &time, std::map< std::string, double > &ret_map) const
std::map< std::string, double >	getCurrentJointStateValues () const
bool	getCurrentRobotState (motion_planning_msgs::RobotState &robot_state) const
boost::shared_ptr < planning_models::KinematicModel >	getKinematicModel (void) const
	Get the kinematic model that is being used to check for validity.
const std::string &	getRobotFrameId (void) const
	Return the frame id of the state.
RobotModels *	getRobotModels (void) const
	Get the instance of RobotModels that is being used.
double	getTotalVelocity (void) const
	Return the maintained robot velocity (square root of sum of squares of velocity at each joint).
tf::TransformListener *	getTransformListener (void) const
	Return the transform listener.
const std::string &	getWorldFrameId (void) const
	Return the world frame id.
bool	haveState (void) const
	Return true if a full joint state has been received (including pose, if pose inclusion is enabled).
bool	isJointStateUpdated (double sec) const
	Return true if a joint state has been received in the last sec seconds. If sec < 10us, this function always returns true.
bool	isPoseUpdated (double sec) const
	Return true if a pose has been received in the last sec seconds. If sec < 10us, this function always returns true.
bool	isStateMonitorStarted (void) const
	Check if the state monitor is currently started.
	KinematicModelStateMonitor (RobotModels rm, tf::TransformListener tf)
const ros::Time &	lastJointStateUpdate (void) const
	Return the time of the last state update.
const ros::Time &	lastPoseUpdate (void) const
	Return the time of the last pose update.
void	printRobotState (void) const
	Output the current state as ROS INFO.
bool	setKinematicStateToTime (const ros::Time &time, planning_models::KinematicState &state) const
void	setOnStateUpdateCallback (const boost::function< void(void)> &callback)
	Define a callback for when the state is changed.
void	setRobotStateAndComputeTransforms (const motion_planning_msgs::RobotState &robot_state, planning_models::KinematicState &state) const
void	setStateValuesFromCurrentValues (planning_models::KinematicState &state) const
	Sets the model used for collision/valditity checking to the current state values.
void	startStateMonitor (void)
	Start the state monitor. By default, the monitor is started after creation.
void	stopStateMonitor (void)
	Stop the state monitor.
void	waitForState (void) const
	Wait until a full joint state is received (including pose, if pose inclusion is enabled).
virtual	~KinematicModelStateMonitor (void)
Protected Member Functions
void	jointStateCallback (const sensor_msgs::JointStateConstPtr &jointState)
void	setupRSM (void)
Protected Attributes
std::map< std::string, double >	current_joint_state_map_
boost::recursive_mutex	current_joint_values_lock_
std::string	floatingJoint_
bool	haveJointState_
bool	havePose_
double	joint_state_cache_allowed_difference_
double	joint_state_cache_time_
std::list< std::pair < ros::Time, std::map < std::string, double > > >	joint_state_map_cache_
ros::Subscriber	jointStateSubscriber_
boost::shared_ptr < planning_models::KinematicModel >	kmodel_
std::map< std::string, ros::Time >	last_joint_update_
ros::Time	lastJointStateUpdate_
ros::Time	lastPoseUpdate_
ros::NodeHandle	nh_
boost::function< void(void)>	onStateUpdate_
std::string	planarJoint_
tf::Pose	pose_
bool	printed_out_of_date_
RobotModels *	rm_
std::string	robot_frame_
double	robotVelocity_
ros::NodeHandle	root_handle_
bool	stateMonitorStarted_
tf::TransformListener *	tf_

Detailed Description

KinematicModelStateMonitor is a class that monitors the robot state for the kinematic model defined in RobotModels If the pose is not included, the robot state is the frame of the link it attaches to the world. If the pose is included, the frame of the robot is the one in which the pose is published.

Definition at line 60 of file kinematic_model_state_monitor.h.

Constructor & Destructor Documentation

planning_environment::KinematicModelStateMonitor::KinematicModelStateMonitor	(	RobotModels *	rm,
		tf::TransformListener *	tf
	)			`[inline]`

Definition at line 64 of file kinematic_model_state_monitor.h.

virtual planning_environment::KinematicModelStateMonitor::~KinematicModelStateMonitor ( void ) [inline, virtual]

Definition at line 71 of file kinematic_model_state_monitor.h.

Member Function Documentation

virtual void planning_environment::KinematicModelStateMonitor::advertiseServices ( ) [inline, virtual]

Once everything is functional this advertises any services that the monitor may offer

Reimplemented in planning_environment::CollisionSpaceMonitor.

Definition at line 88 of file kinematic_model_state_monitor.h.

bool planning_environment::KinematicModelStateMonitor::allJointsUpdated ( ros::Duration dur = ros::Duration() ) const

Definition at line 229 of file kinematic_model_state_monitor.cpp.

bool planning_environment::KinematicModelStateMonitor::convertKinematicStateToRobotState	(	const planning_models::KinematicState &	state,
		motion_planning_msgs::RobotState &	robot_state
	)			const

Definition at line 308 of file kinematic_model_state_monitor.cpp.

bool planning_environment::KinematicModelStateMonitor::getCachedJointStateValues	(	const ros::Time &	time,
		std::map< std::string, double > &	ret_map
	)			const

Definition at line 340 of file kinematic_model_state_monitor.cpp.

std::map<std::string, double> planning_environment::KinematicModelStateMonitor::getCurrentJointStateValues ( ) const [inline]

Definition at line 186 of file kinematic_model_state_monitor.h.

bool planning_environment::KinematicModelStateMonitor::getCurrentRobotState ( motion_planning_msgs::RobotState & robot_state ) const

Definition at line 290 of file kinematic_model_state_monitor.cpp.

boost::shared_ptr<planning_models::KinematicModel> planning_environment::KinematicModelStateMonitor::getKinematicModel ( void ) const [inline]

Get the kinematic model that is being used to check for validity.

Definition at line 100 of file kinematic_model_state_monitor.h.

const std::string& planning_environment::KinematicModelStateMonitor::getRobotFrameId ( void ) const [inline]

Return the frame id of the state.

Definition at line 124 of file kinematic_model_state_monitor.h.

RobotModels* planning_environment::KinematicModelStateMonitor::getRobotModels ( void ) const [inline]

Get the instance of RobotModels that is being used.

Definition at line 106 of file kinematic_model_state_monitor.h.

double planning_environment::KinematicModelStateMonitor::getTotalVelocity ( void ) const [inline]

Return the maintained robot velocity (square root of sum of squares of velocity at each joint).

Definition at line 112 of file kinematic_model_state_monitor.h.

tf::TransformListener* planning_environment::KinematicModelStateMonitor::getTransformListener ( void ) const [inline]

Return the transform listener.

Definition at line 118 of file kinematic_model_state_monitor.h.

const std::string& planning_environment::KinematicModelStateMonitor::getWorldFrameId ( void ) const [inline]

Return the world frame id.

Definition at line 130 of file kinematic_model_state_monitor.h.

bool planning_environment::KinematicModelStateMonitor::haveState ( void ) const [inline]

Return true if a full joint state has been received (including pose, if pose inclusion is enabled).

Definition at line 136 of file kinematic_model_state_monitor.h.

bool planning_environment::KinematicModelStateMonitor::isJointStateUpdated ( double sec ) const

Return true if a joint state has been received in the last sec seconds. If sec < 10us, this function always returns true.

Definition at line 412 of file kinematic_model_state_monitor.cpp.

bool planning_environment::KinematicModelStateMonitor::isPoseUpdated ( double sec ) const

Return true if a pose has been received in the last sec seconds. If sec < 10us, this function always returns true.

Definition at line 442 of file kinematic_model_state_monitor.cpp.

bool planning_environment::KinematicModelStateMonitor::isStateMonitorStarted ( void ) const [inline]

Check if the state monitor is currently started.

Definition at line 82 of file kinematic_model_state_monitor.h.

void planning_environment::KinematicModelStateMonitor::jointStateCallback ( const sensor_msgs::JointStateConstPtr & jointState ) [protected]

Definition at line 93 of file kinematic_model_state_monitor.cpp.

const ros::Time& planning_environment::KinematicModelStateMonitor::lastJointStateUpdate ( void ) const [inline]

Return the time of the last state update.

Definition at line 142 of file kinematic_model_state_monitor.h.

const ros::Time& planning_environment::KinematicModelStateMonitor::lastPoseUpdate ( void ) const [inline]

Return the time of the last pose update.

Definition at line 148 of file kinematic_model_state_monitor.h.

void planning_environment::KinematicModelStateMonitor::printRobotState ( void ) const

Output the current state as ROS INFO.

bool planning_environment::KinematicModelStateMonitor::setKinematicStateToTime	(	const ros::Time &	time,
		planning_models::KinematicState &	state
	)			const

Definition at line 297 of file kinematic_model_state_monitor.cpp.

void planning_environment::KinematicModelStateMonitor::setOnStateUpdateCallback ( const boost::function< void(void)> & callback ) [inline]

Define a callback for when the state is changed.

Definition at line 94 of file kinematic_model_state_monitor.h.

void planning_environment::KinematicModelStateMonitor::setRobotStateAndComputeTransforms	(	const motion_planning_msgs::RobotState &	robot_state,
		planning_models::KinematicState &	state
	)			const