#include <collision_proximity_space.h>

Public Types
enum	TrajectoryPointType { StartCollision, EndCollision, Middle, None }
enum	TrajectorySafety { ErrorUnsafe, StartUnsafe, MiddleUnsafe, EndUnsafe, MeshToMeshSafe, InCollisionSafe }
Public Member Functions
	CollisionProximitySpace (const std::string &robot_description_name, bool register_with_environment_server=true, bool use_signed_environment_field=false, bool use_signed_self_field=false)
planning_environment::CollisionModelsInterface *	getCollisionModelsInterface () const
std::vector< std::string >	getCurrentAttachedBodyNames () const
const std::string &	getCurrentGroupName () const
std::vector< std::string >	getCurrentLinkNames () const
bool	getEnvironmentCollisions (std::vector< bool > &collisions, bool stop_at_first=false) const
bool	getEnvironmentProximityGradients (std::vector< GradientInfo > &gradients, bool subtract_radii=false) const
bool	getIntraGroupCollisions (std::vector< bool > &collisions, bool stop_at_first=false) const
bool	getIntraGroupProximityGradients (std::vector< GradientInfo > &gradients, bool subtract_radii=false) const
tf::Transform	getInverseWorldTransform (const planning_models::KinematicState &state) const
void	getProximityGradientMarkers (const std::vector< std::string > &link_names, const std::vector< std::string > &attached_body_names, const std::vector< GradientInfo > &gradients, const std::string &ns, visualization_msgs::MarkerArray &arr) const
bool	getSelfCollisions (std::vector< bool > &collisions, bool stop_at_first=false) const
bool	getSelfProximityGradients (std::vector< GradientInfo > &gradients, bool subtract_radii=false) const
bool	getStateCollisions (bool &in_collision, std::vector< CollisionType > &collisions) const
bool	getStateGradients (std::vector< GradientInfo > &gradients, bool subtract_radii=false) const
bool	isEnvironmentCollision () const
bool	isIntraGroupCollision () const
bool	isSelfCollision () const
bool	isStateInCollision () const
TrajectorySafety	isTrajectorySafe (const trajectory_msgs::JointTrajectory &trajectory, const arm_navigation_msgs::Constraints &goal_constraints, const arm_navigation_msgs::Constraints &path_constraints, const std::string &groupName)
void	revertAfterGroupQueries ()
void	revertPlanningSceneCallback ()
void	setCollisionTolerance (double tol)
void	setCurrentGroupState (const planning_models::KinematicState &state)
bool	setPlanningScene (const arm_navigation_msgs::PlanningScene &planning_scene)
void	setPlanningSceneCallback (const arm_navigation_msgs::PlanningScene &scene)
void	setupForGroupQueries (const std::string &group_name, const arm_navigation_msgs::RobotState &rob_state, std::vector< std::string > &link_names, std::vector< std::string > &attached_body_names)
void	visualizeBoundingCylinders (const std::vector< std::string > &object_names) const
void	visualizeCollisions (const std::vector< std::string > &link_names, const std::vector< std::string > &attached_body_names, const std::vector< CollisionType > collisions) const
void	visualizeDistanceField (distance_field::DistanceField< distance_field::PropDistanceFieldVoxel > *distance_field) const
void	visualizeDistanceFieldPlane (distance_field::DistanceField< distance_field::PropDistanceFieldVoxel > *distance_field) const
void	visualizeObjectSpheres (const std::vector< std::string > &object_names) const
void	visualizeObjectVoxels (const std::vector< std::string > &object_names) const
	~CollisionProximitySpace ()
Private Member Functions
void	deleteAllAttachedObjectDecompositions ()
void	deleteAllStaticObjectDecompositions ()
bool	getGroupLinkAndAttachedBodyNames (const std::string &group_name, const planning_models::KinematicState &state, std::vector< std::string > &link_names, std::vector< unsigned int > &link_indices, std::vector< std::string > &attached_body_names, std::vector< unsigned int > &attached_body_link_indices) const
void	loadDefaultCollisionOperations ()
void	loadRobotBodyDecompositions ()
void	prepareEnvironmentDistanceField (const planning_models::KinematicState &state)
void	prepareSelfDistanceField (const std::vector< std::string > &link_names, const planning_models::KinematicState &state)
void	setBodyPosesGivenKinematicState (const planning_models::KinematicState &state)
void	setBodyPosesToCurrent ()
void	setDistanceFieldForGroupQueries (const std::string &group_name, const planning_models::KinematicState &state)
bool	setupGradientStructures (const std::vector< std::string > &link_names, const std::vector< std::string > &attached_body_names, std::vector< GradientInfo > &gradients) const
void	syncObjectsWithCollisionSpace (const planning_models::KinematicState &state)
bool	updateSphereLocations (const std::vector< std::string > &link_names, const std::vector< std::string > &attached_body_names, std::vector< GradientInfo > &gradients)
Private Attributes
std::map< std::string, std::map< std::string, bool > >	attached_object_collision_links_
std::map< std::string, BodyDecompositionVector * >	attached_object_map_
std::map< std::string, BodyDecomposition * >	body_decomposition_map_
planning_environment::CollisionModelsInterface *	collision_models_interface_
std::vector< std::vector < double > >	colors_
std::vector < BodyDecompositionVector * >	current_attached_body_decompositions_
std::vector< unsigned int >	current_attached_body_indices_
std::vector< std::string >	current_attached_body_names_
std::vector< GradientInfo >	current_gradients_
std::string	current_group_name_
std::vector< std::vector< bool > >	current_intra_group_collision_links_
std::vector< BodyDecomposition * >	current_link_body_decompositions_
std::vector< unsigned int >	current_link_indices_
std::vector< std::string >	current_link_names_
std::vector< bool >	current_self_excludes_
std::map< std::string, std::map< std::string, bool > >	enabled_self_collision_links_
distance_field::DistanceField < distance_field::PropDistanceFieldVoxel > *	environment_distance_field_
boost::recursive_mutex	group_queries_lock_
std::map< std::string, std::map< std::string, bool > >	intra_group_collision_links_
double	max_environment_distance_
double	max_self_distance_
double	origin_x_
double	origin_y_
double	origin_z_
ros::NodeHandle	priv_handle_
double	resolution_
ros::NodeHandle	root_handle_
distance_field::DistanceField < distance_field::PropDistanceFieldVoxel > *	self_distance_field_
std::map< std::string, bool >	self_excludes_
double	size_x_
double	size_y_
double	size_z_
std::map< std::string, BodyDecompositionVector * >	static_object_map_
double	tolerance_
double	undefined_distance_
ros::Publisher	vis_distance_field_marker_publisher_
ros::Publisher	vis_marker_array_publisher_
ros::Publisher	vis_marker_publisher_

Detailed Description

Definition at line 58 of file collision_proximity_space.h.

Member Enumeration Documentation

enum collision_proximity::CollisionProximitySpace::TrajectoryPointType

Enumerator:

StartCollision
EndCollision
Middle
None

Definition at line 63 of file collision_proximity_space.h.

enum collision_proximity::CollisionProximitySpace::TrajectorySafety

Enumerator:

ErrorUnsafe
StartUnsafe
MiddleUnsafe
EndUnsafe
MeshToMeshSafe
InCollisionSafe

Definition at line 68 of file collision_proximity_space.h.

Constructor & Destructor Documentation

CollisionProximitySpace::CollisionProximitySpace	(	const std::string &	robot_description_name,
		bool	register_with_environment_server = `true`,
		bool	use_signed_environment_field = `false`,
		bool	use_signed_self_field = `false`
	)

Definition at line 64 of file collision_proximity_space.cpp.

CollisionProximitySpace::~CollisionProximitySpace ( )

Definition at line 124 of file collision_proximity_space.cpp.

Member Function Documentation

void CollisionProximitySpace::deleteAllAttachedObjectDecompositions ( ) [private]

Definition at line 148 of file collision_proximity_space.cpp.

void CollisionProximitySpace::deleteAllStaticObjectDecompositions ( ) [private]

Definition at line 138 of file collision_proximity_space.cpp.

planning_environment::CollisionModelsInterface* collision_proximity::CollisionProximitySpace::getCollisionModelsInterface ( ) const [inline]

Definition at line 169 of file collision_proximity_space.h.

std::vector<std::string> collision_proximity::CollisionProximitySpace::getCurrentAttachedBodyNames ( ) const [inline]

Definition at line 184 of file collision_proximity_space.h.

const std::string& collision_proximity::CollisionProximitySpace::getCurrentGroupName ( ) const [inline]

Definition at line 175 of file collision_proximity_space.h.

std::vector<std::string> collision_proximity::CollisionProximitySpace::getCurrentLinkNames ( ) const [inline]

Definition at line 179 of file collision_proximity_space.h.

bool CollisionProximitySpace::getEnvironmentCollisions	(	std::vector< bool > &	collisions,
		bool	stop_at_first = `false`
	)		const

Definition at line 1094 of file collision_proximity_space.cpp.

bool CollisionProximitySpace::getEnvironmentProximityGradients	(	std::vector< GradientInfo > &	gradients,
		bool	subtract_radii = `false`
	)		const

Definition at line 1123 of file collision_proximity_space.cpp.

bool CollisionProximitySpace::getGroupLinkAndAttachedBodyNames	(	const std::string &	group_name,
		const planning_models::KinematicState &	state,
		std::vector< std::string > &	link_names,
		std::vector< unsigned int > &	link_indices,
		std::vector< std::string > &	attached_body_names,
		std::vector< unsigned int > &	attached_body_link_indices
	)		const `[private]`

Definition at line 660 of file collision_proximity_space.cpp.

bool CollisionProximitySpace::getIntraGroupCollisions	(	std::vector< bool > &	collisions,
		bool	stop_at_first = `false`
	)		const

Definition at line 928 of file collision_proximity_space.cpp.

bool CollisionProximitySpace::getIntraGroupProximityGradients	(	std::vector< GradientInfo > &	gradients,
		bool	subtract_radii = `false`
	)		const

Definition at line 968 of file collision_proximity_space.cpp.

tf::Transform CollisionProximitySpace::getInverseWorldTransform ( const planning_models::KinematicState & state ) const

Definition at line 439 of file collision_proximity_space.cpp.

void CollisionProximitySpace::getProximityGradientMarkers	(	const std::vector< std::string > &	link_names,
		const std::vector< std::string > &	attached_body_names,
		const std::vector< GradientInfo > &	gradients,
		const std::string &	ns,
		visualization_msgs::MarkerArray &	arr
	)		const

Definition at line 1147 of file collision_proximity_space.cpp.

bool CollisionProximitySpace::getSelfCollisions	(	std::vector< bool > &	collisions,
		bool	stop_at_first = `false`
	)		const

Definition at line 1033 of file collision_proximity_space.cpp.

bool CollisionProximitySpace::getSelfProximityGradients	(	std::vector< GradientInfo > &	gradients,
		bool	subtract_radii = `false`
	)		const

Definition at line 1062 of file collision_proximity_space.cpp.

bool CollisionProximitySpace::getStateCollisions	(	bool &	in_collision,
		std::vector< CollisionType > &	collisions
	)		const

Definition at line 802 of file collision_proximity_space.cpp.

bool CollisionProximitySpace::getStateGradients	(	std::vector< GradientInfo > &	gradients,
		bool	subtract_radii = `false`
	)		const

Definition at line 826 of file collision_proximity_space.cpp.

bool CollisionProximitySpace::isEnvironmentCollision ( void ) const

Definition at line 1088 of file collision_proximity_space.cpp.

bool CollisionProximitySpace::isIntraGroupCollision ( ) const

Definition at line 922 of file collision_proximity_space.cpp.

bool CollisionProximitySpace::isSelfCollision ( void ) const

Definition at line 1027 of file collision_proximity_space.cpp.

bool CollisionProximitySpace::isStateInCollision ( ) const

Definition at line 795 of file collision_proximity_space.cpp.

CollisionProximitySpace::TrajectorySafety CollisionProximitySpace::isTrajectorySafe	(	const trajectory_msgs::JointTrajectory &	trajectory,
		const arm_navigation_msgs::Constraints &	goal_constraints,
		const arm_navigation_msgs::Constraints &	path_constraints,
		const std::string &	groupName
	)

Definition at line 1207 of file collision_proximity_space.cpp.

void CollisionProximitySpace::loadDefaultCollisionOperations ( ) [private]

Definition at line 161 of file collision_proximity_space.cpp.

void CollisionProximitySpace::loadRobotBodyDecompositions ( ) [private]

Definition at line 249 of file collision_proximity_space.cpp.

void CollisionProximitySpace::prepareEnvironmentDistanceField ( const planning_models::KinematicState & state ) [private]

Definition at line 505 of file collision_proximity_space.cpp.

void CollisionProximitySpace::prepareSelfDistanceField	(	const std::vector< std::string > &	link_names,
		const planning_models::KinematicState &	state
	)		`[private]`

Definition at line 526 of file collision_proximity_space.cpp.

void collision_proximity::CollisionProximitySpace::revertAfterGroupQueries ( )

void CollisionProximitySpace::revertPlanningSceneCallback ( )

Definition at line 374 of file collision_proximity_space.cpp.

void CollisionProximitySpace::setBodyPosesGivenKinematicState ( const planning_models::KinematicState & state ) [private]

Definition at line 409 of file collision_proximity_space.cpp.

void collision_proximity::CollisionProximitySpace::setBodyPosesToCurrent ( ) [private]

void collision_proximity::CollisionProximitySpace::setCollisionTolerance ( double tol ) [inline]

Definition at line 189 of file collision_proximity_space.h.

void CollisionProximitySpace::setCurrentGroupState ( const planning_models::KinematicState & state )

Definition at line 384 of file collision_proximity_space.cpp.

void CollisionProximitySpace::setDistanceFieldForGroupQueries	(	const std::string &	group_name,
		const planning_models::KinematicState &	state
	)		`[private]`

Definition at line 487 of file collision_proximity_space.cpp.

bool CollisionProximitySpace::setPlanningScene ( const arm_navigation_msgs::PlanningScene & planning_scene )

Definition at line 270 of file collision_proximity_space.cpp.

void CollisionProximitySpace::setPlanningSceneCallback ( const arm_navigation_msgs::PlanningScene & scene )

Definition at line 274 of file collision_proximity_space.cpp.

void CollisionProximitySpace::setupForGroupQueries	(	const std::string &	group_name,
		const arm_navigation_msgs::RobotState &	rob_state,
		std::vector< std::string > &	link_names,
		std::vector< std::string > &	attached_body_names
	)

Definition at line 287 of file collision_proximity_space.cpp.

bool CollisionProximitySpace::setupGradientStructures	(	const std::vector< std::string > &	link_names,
		const std::vector< std::string > &	attached_body_names,
		std::vector< GradientInfo > &	gradients
	)		const `[private]`

Definition at line 749 of file collision_proximity_space.cpp.

void CollisionProximitySpace::syncObjectsWithCollisionSpace ( const planning_models::KinematicState & state ) [private]

Definition at line 449 of file collision_proximity_space.cpp.

bool CollisionProximitySpace::updateSphereLocations	(	const std::vector< std::string > &	link_names,
		const std::vector< std::string > &	attached_body_names,
		std::vector< GradientInfo > &	gradients
	)		`[private]`

Definition at line 714 of file collision_proximity_space.cpp.

void CollisionProximitySpace::visualizeBoundingCylinders ( const std::vector< std::string > & object_names ) const

Definition at line 1698 of file collision_proximity_space.cpp.

void CollisionProximitySpace::visualizeCollisions	(	const std::vector< std::string > &	link_names,
		const std::vector< std::string > &	attached_body_names,
		const std::vector< CollisionType >	collisions
	)		const

Definition at line 1515 of file collision_proximity_space.cpp.

void CollisionProximitySpace::visualizeDistanceField ( distance_field::DistanceField< distance_field::PropDistanceFieldVoxel > * distance_field ) const

Definition at line 1398 of file collision_proximity_space.cpp.

void CollisionProximitySpace::visualizeDistanceFieldPlane ( distance_field::DistanceField< distance_field::PropDistanceFieldVoxel > * distance_field ) const