planning_environment::PlanningMonitor Class Reference

#include <planning_monitor.h>

Inheritance diagram for planning_environment::PlanningMonitor:

Public Types
enum	{ COLLISION_TEST = 1, PATH_CONSTRAINTS_TEST = 2, GOAL_CONSTRAINTS_TEST = 4, JOINT_LIMITS_TEST = 8, CHECK_FULL_TRAJECTORY = 16 }
	Mask for validity testing functions. More...
Public Member Functions
bool	broadcastCollisions ()
bool	checkGoalConstraints (const planning_models::KinematicState *state, bool verbose) const
bool	checkPathConstraints (const planning_models::KinematicState *state, bool verbose) const
void	clearAllowedContacts (void)
	Clear the set of allowed contacts.
void	clearConstraints (void)
	Clear previously set constraints.
int	closestStateOnTrajectory (const trajectory_msgs::JointTrajectory &trajectory, motion_planning_msgs::RobotState &robot_state, unsigned int start, unsigned int end, motion_planning_msgs::ArmNavigationErrorCodes &error_code) const
	Find the index of the state on the path segment that is closest to a given state.
int	closestStateOnTrajectory (const trajectory_msgs::JointTrajectory &trajectory, motion_planning_msgs::RobotState &robot_state, motion_planning_msgs::ArmNavigationErrorCodes &error_code) const
	Find the index of the state on the path that is closest to a given state.
const std::vector < collision_space::EnvironmentModel::AllowedContact > &	getAllowedContacts (void) const
	Get the set of contacts allowed when collision checking.
void	getChildLinks (const std::vector< std::string > &joints, std::vector< std::string > &link_names)
double	getExpectedJointStateUpdateInterval (void) const
	Return the maximum amount of time that is allowed to pass between updates to the state.
double	getExpectedMapUpdateInterval (void) const
	Return the maximum amount of time that is allowed to pass between updates to the map.
double	getExpectedPoseUpdateInterval (void) const
	Return the maximum amount of time that is allowed to pass between updates to the pose.
const motion_planning_msgs::Constraints &	getGoalConstraints (void) const
	Get the kinematic constraints the monitor uses when checking a path's last state (the goal).
void	getOrderedCollisionOperationsForOnlyCollideLinks (const std::vector< std::string > &collision_check_links, const motion_planning_msgs::OrderedCollisionOperations &requested_collision_operations, motion_planning_msgs::OrderedCollisionOperations &result_collision_operations)
const motion_planning_msgs::Constraints &	getPathConstraints (void) const
	Get the kinematic constraints the monitor uses when checking a path.
bool	isEnvironmentSafe (motion_planning_msgs::ArmNavigationErrorCodes &error_code) const
	Return true if recent enough data is available so that planning is considered safe.
bool	isStateValid (const motion_planning_msgs::RobotState &robot_state, const int test, bool verbose, motion_planning_msgs::ArmNavigationErrorCodes &error_code)
bool	isTrajectoryValid (const trajectory_msgs::JointTrajectory &trajectory, motion_planning_msgs::RobotState &robot_state, unsigned int start, unsigned int end, const int test, bool verbose, motion_planning_msgs::ArmNavigationErrorCodes &error_code, std::vector< motion_planning_msgs::ArmNavigationErrorCodes > &trajectory_error_codes)
	This function takes a path and checks a segment of the path for collisions and violations of path constraints, goal constraints and joint limits.
bool	isTrajectoryValid (const trajectory_msgs::JointTrajectory &trajectory, motion_planning_msgs::RobotState &robot_state, const int test, bool verbose, motion_planning_msgs::ArmNavigationErrorCodes &error_code, std::vector< motion_planning_msgs::ArmNavigationErrorCodes > &trajectory_error_codes)
	This function takes a path and checks for collisions and violations of path constraints, goal constraints and joint limits.
	PlanningMonitor (CollisionModels cm, tf::TransformListener tf)
bool	prepareForValidityChecks (const std::vector< std::string > &joint_names, const motion_planning_msgs::OrderedCollisionOperations &ordered_collision_operations, const std::vector< motion_planning_msgs::AllowedContactSpecification > &allowed_contacts, const motion_planning_msgs::Constraints &path_constraints, const motion_planning_msgs::Constraints &goal_constraints, const std::vector< motion_planning_msgs::LinkPadding > &link_padding, motion_planning_msgs::ArmNavigationErrorCodes &error_code)
void	printAllowedContacts (std::ostream &out=std::cout)
	Print allowed contacts.
void	printConstraints (std::ostream &out=std::cout)
	Print active constraints.
void	revertToDefaultState ()
void	setAllowedContacts (const std::vector< collision_space::EnvironmentModel::AllowedContact > &allowedContacts)
	Set the set of contacts allowed when collision checking.
void	setAllowedContacts (const std::vector< motion_planning_msgs::AllowedContactSpecification > &allowedContacts)
	Set the set of contacts allowed when collision checking.
void	setCollisionCheck (const std::string link_name, bool state)
void	setCollisionCheckAll (bool state)
void	setCollisionCheckLinks (const std::vector< std::string > &link_names, bool state)
void	setCollisionCheckOnlyLinks (const std::vector< std::string > &link_names, bool state)
bool	setGoalConstraints (const motion_planning_msgs::Constraints &kc, motion_planning_msgs::ArmNavigationErrorCodes &error_code)
	Set the kinematic constraints the monitor should use when checking a path's last state (the goal).
void	setOnCollisionContactCallback (const boost::function< void(collision_space::EnvironmentModel::Contact &)> &callback)
	Set a callback to be called when a collision is found.
bool	setPathConstraints (const motion_planning_msgs::Constraints &kc, motion_planning_msgs::ArmNavigationErrorCodes &error_code)
	Set the kinematic constraints the monitor should use when checking a path.
bool	transformConstraintsToFrame (motion_planning_msgs::Constraints &kc, const std::string &target, motion_planning_msgs::ArmNavigationErrorCodes &error_code) const
	Transform the frames in which constraints are specified to the one requested.
bool	transformJointToFrame (double &value, const std::string &joint_name, std::string &frame_id, const std::string &target, motion_planning_msgs::ArmNavigationErrorCodes &error_code) const
	Transform the kinematic joint to the frame requested.
bool	transformTrajectoryToFrame (trajectory_msgs::JointTrajectory &kp, motion_planning_msgs::RobotState &robot_state, const std::string &target, motion_planning_msgs::ArmNavigationErrorCodes &error_code) const
	Transform the path to the frame requested.
virtual	~PlanningMonitor (void)
Protected Member Functions
int	closestStateOnTrajectoryAux (const trajectory_msgs::JointTrajectory &trajectory, unsigned int start, unsigned int end, motion_planning_msgs::ArmNavigationErrorCodes &error_code) const
	Find the index of the state on the path that is closest to a given state assuming the path is in the frame of the model.
bool	isTrajectoryValidAux (const trajectory_msgs::JointTrajectory &trajectory, motion_planning_msgs::RobotState &robot_state, unsigned int start, unsigned int end, const int test, bool verbose, motion_planning_msgs::ArmNavigationErrorCodes &error_code, std::vector< motion_planning_msgs::ArmNavigationErrorCodes > &trajectory_error_codes)
	Check the path assuming it is in the frame of the model.
void	loadParams (void)
	Load ROS parameters.
bool	transformJoint (const std::string &name, unsigned int index, double &param, std::string &frame_id, const std::string &target, motion_planning_msgs::ArmNavigationErrorCodes &error_code) const
	Transform the joint parameters (if needed) to a target frame.
Protected Attributes
std::vector < collision_space::EnvironmentModel::AllowedContact >	allowedContacts_
ros::Publisher	display_collision_pose_publisher_
ros::Publisher	display_state_validity_publisher_
motion_planning_msgs::Constraints	goal_constraints_
double	intervalCollisionMap_
double	intervalPose_
double	intervalState_
int	num_contacts_allowable_contacts_test_
int	num_contacts_for_display_
boost::function< void(collision_space::EnvironmentModel::Contact &)>	onCollisionContact_
	User callback when a collision is found.
motion_planning_msgs::Constraints	path_constraints_

Detailed Description

PlanningMonitor is a class which in addition to being aware of a robot model, and the collision model is also aware of constraints and can check the validity of states and paths.

Definition at line 53 of file planning_monitor.h.

Member Enumeration Documentation

anonymous enum

Mask for validity testing functions.

Enumerator:

COLLISION_TEST
PATH_CONSTRAINTS_TEST
GOAL_CONSTRAINTS_TEST
JOINT_LIMITS_TEST
CHECK_FULL_TRAJECTORY

Definition at line 69 of file planning_monitor.h.

Constructor & Destructor Documentation

planning_environment::PlanningMonitor::PlanningMonitor	(	CollisionModels *	cm,
		tf::TransformListener *	tf
	)			`[inline]`

Definition at line 57 of file planning_monitor.h.

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

Definition at line 64 of file planning_monitor.h.

Member Function Documentation

bool planning_environment::PlanningMonitor::broadcastCollisions ( )

Definition at line 759 of file planning_monitor.cpp.

bool planning_environment::PlanningMonitor::checkGoalConstraints	(	const planning_models::KinematicState *	state,
		bool	verbose
	)			const

Definition at line 983 of file planning_monitor.cpp.

bool planning_environment::PlanningMonitor::checkPathConstraints	(	const planning_models::KinematicState *	state,
		bool	verbose
	)			const

Definition at line 973 of file planning_monitor.cpp.

void planning_environment::PlanningMonitor::clearAllowedContacts ( void )

Clear the set of allowed contacts.

Definition at line 847 of file planning_monitor.cpp.

void planning_environment::PlanningMonitor::clearConstraints ( void )

Clear previously set constraints.

Definition at line 135 of file planning_monitor.cpp.

int planning_environment::PlanningMonitor::closestStateOnTrajectory	(	const trajectory_msgs::JointTrajectory &	trajectory,
		motion_planning_msgs::RobotState &	robot_state,
		unsigned int	start,
		unsigned int	end,
		motion_planning_msgs::ArmNavigationErrorCodes &	error_code
	)			const

Find the index of the state on the path segment that is closest to a given state.

Definition at line 476 of file planning_monitor.cpp.

int planning_environment::PlanningMonitor::closestStateOnTrajectory	(	const trajectory_msgs::JointTrajectory &	trajectory,
		motion_planning_msgs::RobotState &	robot_state,
		motion_planning_msgs::ArmNavigationErrorCodes &	error_code
	)			const

Find the index of the state on the path that is closest to a given state.

Definition at line 469 of file planning_monitor.cpp.

int planning_environment::PlanningMonitor::closestStateOnTrajectoryAux	(	const trajectory_msgs::JointTrajectory &	trajectory,
		unsigned int	start,
		unsigned int	end,
		motion_planning_msgs::ArmNavigationErrorCodes &	error_code
	)			const `[protected]`

Find the index of the state on the path that is closest to a given state assuming the path is in the frame of the model.

Definition at line 507 of file planning_monitor.cpp.

const std::vector< collision_space::EnvironmentModel::AllowedContact > & planning_environment::PlanningMonitor::getAllowedContacts ( void ) const

Get the set of contacts allowed when collision checking.

Definition at line 810 of file planning_monitor.cpp.

void planning_environment::PlanningMonitor::getChildLinks	(	const std::vector< std::string > &	joints,
		std::vector< std::string > &	link_names
	)

Definition at line 872 of file planning_monitor.cpp.

double planning_environment::PlanningMonitor::getExpectedJointStateUpdateInterval ( void ) const [inline]

Return the maximum amount of time that is allowed to pass between updates to the state.

Definition at line 215 of file planning_monitor.h.

double planning_environment::PlanningMonitor::getExpectedMapUpdateInterval ( void ) const [inline]

Return the maximum amount of time that is allowed to pass between updates to the map.

Definition at line 209 of file planning_monitor.h.

double planning_environment::PlanningMonitor::getExpectedPoseUpdateInterval ( void ) const [inline]

Return the maximum amount of time that is allowed to pass between updates to the pose.

Definition at line 221 of file planning_monitor.h.

const motion_planning_msgs::Constraints& planning_environment::PlanningMonitor::getGoalConstraints ( void ) const [inline]

Get the kinematic constraints the monitor uses when checking a path's last state (the goal).

Definition at line 156 of file planning_monitor.h.

void planning_environment::PlanningMonitor::getOrderedCollisionOperationsForOnlyCollideLinks	(	const std::vector< std::string > &	collision_check_links,
		const motion_planning_msgs::OrderedCollisionOperations &	requested_collision_operations,
		motion_planning_msgs::OrderedCollisionOperations &	result_collision_operations
	)

Definition at line 899 of file planning_monitor.cpp.

const motion_planning_msgs::Constraints& planning_environment::PlanningMonitor::getPathConstraints ( void ) const [inline]

Get the kinematic constraints the monitor uses when checking a path.

Definition at line 146 of file planning_monitor.h.

bool planning_environment::PlanningMonitor::isEnvironmentSafe ( motion_planning_msgs::ArmNavigationErrorCodes & error_code ) const

Return true if recent enough data is available so that planning is considered safe.

Definition at line 107 of file planning_monitor.cpp.

bool planning_environment::PlanningMonitor::isStateValid	(	const motion_planning_msgs::RobotState &	robot_state,
		const int	test,
		bool	verbose,
		motion_planning_msgs::ArmNavigationErrorCodes &	error_code
	)

Definition at line 368 of file planning_monitor.cpp.

bool planning_environment::PlanningMonitor::isTrajectoryValid	(	const trajectory_msgs::JointTrajectory &	trajectory,
		motion_planning_msgs::RobotState &	robot_state,
		unsigned int	start,
		unsigned int	end,
		const int	test,
		bool	verbose,
		motion_planning_msgs::ArmNavigationErrorCodes &	error_code,
		std::vector< motion_planning_msgs::ArmNavigationErrorCodes > &	trajectory_error_codes
	)

This function takes a path and checks a segment of the path for collisions and violations of path constraints, goal constraints and joint limits.

Parameters:

	The	input path.
	The	robot state, joint values specified here are over-written by corresponding joint values in the JointPath message.
	The	test flag used to specify which set of tests should be run. The choices are collision test, path constraints test, goal constraints test and joint limits test. E.g. to specify that tests should be run for collision, path and goal constraints, set test = planning_environment_msgs::IsPlanValid::Request::COLLISION_TEST \| planning_environment_msgs::IsPlanValid::Request::PATH_CONSTRAINTS_TEST \| planning_environment_msgs::IsPlanValid::Request::GOAL_CONSTRAINTS_TEST;
	Set	verbosity level

Returns:: True if state satisfies all the constraints, false otherwise.

Definition at line 562 of file planning_monitor.cpp.

bool planning_environment::PlanningMonitor::isTrajectoryValid	(	const trajectory_msgs::JointTrajectory &	trajectory,
		motion_planning_msgs::RobotState &	robot_state,
		const int	test,
		bool	verbose,
		motion_planning_msgs::ArmNavigationErrorCodes &	error_code,
		std::vector< motion_planning_msgs::ArmNavigationErrorCodes > &	trajectory_error_codes
	)

This function takes a path and checks for collisions and violations of path constraints, goal constraints and joint limits.

Parameters:

	The	input path.
	The	robot state, joint values specified here are over-written by corresponding joint values in the JointPath message.
	The	test flag used to specify which set of tests should be run. The choices are collision test, path constraints test, goal constraints test and joint limits test. E.g. to specify that tests should be run for collision, path and goal constraints, set test = planning_environment_msgs::IsPlanValid::Request::COLLISION_TEST \| planning_environment_msgs::IsPlanValid::Request::PATH_CONSTRAINTS_TEST \| planning_environment_msgs::IsPlanValid::Request::GOAL_CONSTRAINTS_TEST;
	Set	verbosity level

Returns:: True if state satisfies all the constraints, false otherwise.

Definition at line 546 of file planning_monitor.cpp.

bool planning_environment::PlanningMonitor::isTrajectoryValidAux	(	const trajectory_msgs::JointTrajectory &	trajectory,
		motion_planning_msgs::RobotState &	robot_state,
		unsigned int	start,
		unsigned int	end,
		const int	test,
		bool	verbose,
		motion_planning_msgs::ArmNavigationErrorCodes &	error_code,
		std::vector< motion_planning_msgs::ArmNavigationErrorCodes > &	trajectory_error_codes
	)			`[protected]`

Check the path assuming it is in the frame of the model.

Definition at line 596 of file planning_monitor.cpp.

void planning_environment::PlanningMonitor::loadParams ( void ) [protected]

Load ROS parameters.

Author:: Ioan Sucan, Sachin Chitta

Definition at line 42 of file planning_monitor.cpp.

bool planning_environment::PlanningMonitor::prepareForValidityChecks	(	const std::vector< std::string > &	joint_names,
		const motion_planning_msgs::OrderedCollisionOperations &	ordered_collision_operations,
		const std::vector< motion_planning_msgs::AllowedContactSpecification > &	allowed_contacts,
		const motion_planning_msgs::Constraints &	path_constraints,
		const motion_planning_msgs::Constraints &	goal_constraints,
		const std::vector< motion_planning_msgs::LinkPadding > &	link_padding,
		motion_planning_msgs::ArmNavigationErrorCodes &	error_code
	)

Definition at line 54 of file planning_monitor.cpp.

void planning_environment::PlanningMonitor::printAllowedContacts ( std::ostream & out = std::cout )

Print allowed contacts.

Definition at line 815 of file planning_monitor.cpp.

void planning_environment::PlanningMonitor::printConstraints ( std::ostream & out = std::cout )

Print active constraints.

Definition at line 827 of file planning_monitor.cpp.

void planning_environment::PlanningMonitor::revertToDefaultState ( )

Definition at line 99 of file planning_monitor.cpp.

void planning_environment::PlanningMonitor::setAllowedContacts ( const std::vector< collision_space::EnvironmentModel::AllowedContact > & allowedContacts )

Set the set of contacts allowed when collision checking.

Definition at line 794 of file planning_monitor.cpp.

void planning_environment::PlanningMonitor::setAllowedContacts ( const std::vector< motion_planning_msgs::AllowedContactSpecification > & allowedContacts )

Set the set of contacts allowed when collision checking.

Definition at line 799 of file planning_monitor.cpp.

void planning_environment::PlanningMonitor::setCollisionCheck	(	const std::string	link_name,
		bool	state
	)

Definition at line 852 of file planning_monitor.cpp.

void planning_environment::PlanningMonitor::setCollisionCheckAll ( bool state )

Definition at line 857 of file planning_monitor.cpp.

void planning_environment::PlanningMonitor::setCollisionCheckLinks	(	const std::vector< std::string > &	link_names,
		bool	state
	)

Definition at line 862 of file planning_monitor.cpp.

void planning_environment::PlanningMonitor::setCollisionCheckOnlyLinks	(	const std::vector< std::string > &	link_names,
		bool	state
	)

Definition at line 867 of file planning_monitor.cpp.

bool planning_environment::PlanningMonitor::setGoalConstraints	(	const motion_planning_msgs::Constraints &	kc,
		motion_planning_msgs::ArmNavigationErrorCodes &	error_code
	)

Set the kinematic constraints the monitor should use when checking a path's last state (the goal).

Definition at line 155 of file planning_monitor.cpp.

void planning_environment::PlanningMonitor::setOnCollisionContactCallback ( const boost::function< void(collision_space::EnvironmentModel::Contact &)> & callback ) [inline]

Set a callback to be called when a collision is found.

Definition at line 203 of file planning_monitor.h.

bool planning_environment::PlanningMonitor::setPathConstraints	(	const motion_planning_msgs::Constraints &	kc,
		motion_planning_msgs::ArmNavigationErrorCodes &	error_code
	)

Set the kinematic constraints the monitor should use when checking a path.

Definition at line 148 of file planning_monitor.cpp.

bool planning_environment::PlanningMonitor::transformConstraintsToFrame	(	motion_planning_msgs::Constraints &	kc,
		const std::string &	target,
		motion_planning_msgs::ArmNavigationErrorCodes &	error_code
	)			const

Transform the frames in which constraints are specified to the one requested.

Definition at line 162 of file planning_monitor.cpp.

bool planning_environment::PlanningMonitor::transformJoint	(	const std::string &	name,
		unsigned int	index,
		double &	param,
		std::string &	frame_id,
		const std::string &	target,
		motion_planning_msgs::ArmNavigationErrorCodes &	error_code
	)			const `[protected]`

Transform the joint parameters (if needed) to a target frame.

Definition at line 348 of file planning_monitor.cpp.

bool planning_environment::PlanningMonitor::transformJointToFrame	(	double &	value,
		const std::string &	joint_name,
		std::string &	frame_id,
		const std::string &	target,
		motion_planning_msgs::ArmNavigationErrorCodes &	error_code
	)			const

Transform the kinematic joint to the frame requested.

Definition at line 339 of file planning_monitor.cpp.

bool planning_environment::PlanningMonitor::transformTrajectoryToFrame	(	trajectory_msgs::JointTrajectory &	kp,
		motion_planning_msgs::RobotState &	robot_state,
		const std::string &	target,
		motion_planning_msgs::ArmNavigationErrorCodes &	error_code
	)			const