PlanningComponentsVisualizer Class Reference

List of all members.

Classes
struct	GroupCollection
struct	SelectableMarker
	Contains data for selectable markers. Stored in a large map. More...
struct	StateTrajectoryDisplay
Public Types
enum	IKControlType { StartPosition, EndPosition }
	Used to decide which kinematic state the user is controlling. The planner plans from start to end. More...
enum	InteractiveMarkerType { EndEffectorControl, JointControl, CollisionObject }
	May be for IK control, joint control, or collision objects. More...
Public Member Functions
void	createCollisionPole (int num, Pose pose)
	Creates a collision pole with the given ID at location given by pose. Creates a selectable marker too.
void	createSelectableJointMarkers (GroupCollection &gc)
	Given the group collection, creates a set of selectable markers for each joint in the group.
void	deleteJointMarkers (GroupCollection &gc)
void	deselectMarker (SelectableMarker &marker, btTransform transform)
	Removes the 6DOF control of the given marker and replaces it with a menu.
void	determinePitchRollConstraintsGivenState (const PlanningComponentsVisualizer::GroupCollection &gc, const planning_models::KinematicState &state, arm_navigation_msgs::OrientationConstraint &goal_constraint, arm_navigation_msgs::OrientationConstraint &path_constraint) const
bool	doesGroupHaveGoodIKSolution (const string &group) const
bool	doesGroupHaveGoodTrajectory (const string &group, const string &source) const
bool	filterPlannerTrajectory (PlanningComponentsVisualizer::GroupCollection &gc)
size_t	getNumPlanningGroups ()
GroupCollection *	getPlanningGroup (unsigned int i)
bool	isGroupName (const string &name)
	Returns true if the string of the given name corresponds to a kinematic chain in the robot.
bool	isValidJointName (GroupCollection &gc, string name)
void	makeInteractive1DOFRotationMarker (btTransform transform, btVector3 axis, string name, string description, float scale=1.0f, float angle=0.0f)
void	makeInteractive1DOFTranslationMarker (btTransform transform, btVector3 axis, string name, string description, float scale=1.0f, float value=0.0f)
void	makeInteractive6DOFMarker (bool fixed, btTransform transform, string name, string description, float scale=1.0f, bool pole=false)
	Creates an interactive 6DOF marker control.
InteractiveMarkerControl &	makeInteractiveBoxControl (InteractiveMarker &msg, float alpha=1.0f)
	Creates a clickable, box shaped marker.
InteractiveMarkerControl &	makeInteractiveCylinderControl (InteractiveMarker &msg, float alpha=1.0f)
	Creates a clickable, cylinder shaped marker.
Marker	makeMarkerBox (InteractiveMarker &msg, float alpha=1.0f)
	Creates a box shaped marker.
Marker	makeMarkerCylinder (InteractiveMarker &msg, float alpha=1.0f)
	Creates a cylinder shaped marker.
Marker	makeMarkerSphere (InteractiveMarker &msg)
	Creates a sphere-shaped marker.
void	makePoleContextMenu (btTransform transform, string name, string description, float scale=1.0f)
	Wrapper for makeSelectableMarker which assumes we are creating a collision pole.
void	makeSelectableMarker (InteractiveMarkerType type, btTransform transform, string name, string description, float scale=1.0f, bool publish=true)
	Creates a marker that is initially a clickable menu. Upon selection it turns into a 6DOF control.
void	makeTopLevelMenu ()
void	moveEndEffectorMarkers (double vx, double vy, double vz, double vr, double vp, double vw, bool coll_aware=true)
void	moveThroughTrajectory (PlanningComponentsVisualizer::GroupCollection &gc, const string &source_name, int step)
int	nextCollisionPole ()
	Generates a new collision pole ID.
	PlanningComponentsVisualizer ()
bool	planToEndEffectorState (PlanningComponentsVisualizer::GroupCollection &gc)
bool	playTrajectory (PlanningComponentsVisualizer::GroupCollection &gc, const string &source_name, const trajectory_msgs::JointTrajectory &traj)
void	processInteractiveFeedback (const InteractiveMarkerFeedbackConstPtr &feedback)
	Main function that handles interactive marker feedback of all kinds.
void	publishJointStates ()
void	randomlyPerturb (PlanningComponentsVisualizer::GroupCollection &gc)
void	refreshEnvironment ()
	Sends all collision pole changes and changes to the robot state to the planning environment.
MenuHandler::EntryHandle	registerMenuEntry (MenuHandler &handler, MenuEntryMap &map, string name)
	Creates a new menu entry for the given menu handler, putting its handle in the given map.
MenuHandler::EntryHandle	registerSubMenuEntry (MenuHandler &handler, MenuEntryMap &map, string name, MenuHandler::EntryHandle subMenuHandle)
	Creates a new menu entry for the given menu handler, putting its handle in the given map.
void	removeCollisionPole (int num)
	deletes the collision pole with the given ID.
void	removeCollisionPoleByName (string id)
	deletes the collision pole with the given name.
void	resetToLastGoodState (GroupCollection &gc)
bool	selectableMarkerExists (string name)
	Returns true if a selectable marker of the given name exists in the marker map.
void	selectMarker (SelectableMarker &marker, btTransform transform)
	Removes the menu marker given and replaces it with a 6DOF marker.
void	selectPlanningGroup (unsigned int entry)
void	sendMarkers ()
void	sendPlanningScene ()
void	sendTransforms ()
void	setJointState (GroupCollection &gc, std::string &joint_name, btTransform value)
	Sets the kinematic state of a joint in the given group collection.
void	setNewEndEffectorPosition (GroupCollection &gc, btTransform &cur, bool coll_aware)
bool	solveIKForEndEffectorPose (PlanningComponentsVisualizer::GroupCollection &gc, bool coll_aware=true, bool constrain_pitch_and_roll=false, double change_redundancy=0.0)
void	updateJointStates (GroupCollection &gc)
	Sends the joint states of the given group collection to the robot state publisher.
	~PlanningComponentsVisualizer ()
Protected Member Functions
void	deleteKinematicStates ()
btTransform	toBulletTransform (geometry_msgs::Pose pose)
Pose	toGeometryPose (btTransform transform)
Protected Attributes
CollisionModels *	cm_
bool	collision_aware_
MenuHandler::EntryHandle	collision_aware_handle_
map< string, arm_navigation_msgs::CollisionObject >	collision_poles_
	Map of collision pole names to messages sent to ROS.
bool	constrain_rp_
MenuHandler::EntryHandle	constrain_rp_handle_
string	current_group_name_
MenuHandler::EntryHandle	end_position_handle_
map< string, GroupCollection >	group_map_
MenuHandler::EntryHandle	ik_control_handle_
IKControlType	ik_control_type_
boost::shared_ptr < InteractiveMarkerServer >	interactive_marker_server_
bool	is_ik_control_active_
bool	is_joint_control_active_
map< string, bool >	joint_clicked_map_
MenuHandler::EntryHandle	joint_control_handle_
map< string, btTransform >	joint_prev_transform_map_
boost::recursive_mutex	joint_state_lock_
ros::Publisher	joint_state_publisher_
map< string, Pose >	last_ee_poses_
	Maps end effector link names to their previously recorded poses.
sensor_msgs::JointState	last_joint_state_msg_
MotionPlanRequest	last_motion_plan_request_
boost::recursive_mutex	lock_
MenuMap	menu_entry_maps_
	Maps MenuHandles to their names. Used to determine which menu entry is selected.
MenuHandlerMap	menu_handler_map_
	Maps strings to menu handlers. This is used for convenience and extensibility.
ros::NodeHandle	nh_
int	num_collision_poles_
	Used to generate new IDs for collision poles. Not the actual number of poles.
ros::ServiceClient	planner_service_client_
map< string, double >	prev_joint_control_value_map_
MenuHandler::FeedbackCallback	process_function_ptr_
	Boost function pointer to the main interactive feedback function.
KinematicState *	robot_state_
map< string, SelectableMarker >	selectable_markers_
	Maps selectable marker names to a struct containing their information.
ros::ServiceClient	set_planning_scene_diff_client_
MenuHandler::EntryHandle	start_position_handle_
ros::ServiceClient	trajectory_filter_service_client_
tf::TransformBroadcaster	transform_broadcaster_
ros::Publisher	vis_marker_array_publisher_
ros::Publisher	vis_marker_publisher_

Detailed Description

Definition at line 89 of file planning_components_visualizer.cpp.

Member Enumeration Documentation

enum PlanningComponentsVisualizer::IKControlType

Used to decide which kinematic state the user is controlling. The planner plans from start to end.

Enumerator:

StartPosition
EndPosition

Definition at line 95 of file planning_components_visualizer.cpp.

enum PlanningComponentsVisualizer::InteractiveMarkerType

May be for IK control, joint control, or collision objects.

Enumerator:

EndEffectorControl
JointControl
CollisionObject

Definition at line 101 of file planning_components_visualizer.cpp.

Constructor & Destructor Documentation

PlanningComponentsVisualizer::PlanningComponentsVisualizer ( ) [inline]

Definition at line 248 of file planning_components_visualizer.cpp.

PlanningComponentsVisualizer::~PlanningComponentsVisualizer ( ) [inline]

Definition at line 403 of file planning_components_visualizer.cpp.

Member Function Documentation

void PlanningComponentsVisualizer::createCollisionPole	(	int	num,
		Pose	pose
	)			`[inline]`

Creates a collision pole with the given ID at location given by pose. Creates a selectable marker too.

Parameters:

	num	the ID of the pole. (must be unique)
	pose	the location and orientation of the pole.

Definition at line 446 of file planning_components_visualizer.cpp.

void PlanningComponentsVisualizer::createSelectableJointMarkers ( GroupCollection & gc ) [inline]

Given the group collection, creates a set of selectable markers for each joint in the group.

Parameters:

gc

the group collection to create joint markers for.

Definition at line 717 of file planning_components_visualizer.cpp.

void PlanningComponentsVisualizer::deleteJointMarkers ( GroupCollection & gc ) [inline]

Definition at line 705 of file planning_components_visualizer.cpp.

void PlanningComponentsVisualizer::deleteKinematicStates ( ) [inline, protected]

Definition at line 2363 of file planning_components_visualizer.cpp.

void PlanningComponentsVisualizer::deselectMarker	(	SelectableMarker &	marker,
		btTransform	transform
	)			`[inline]`

Removes the 6DOF control of the given marker and replaces it with a menu.

Parameters:

	marker	a reference to the selectablemarker struct.
	transform	location of the marker when it is de-selected.

Definition at line 2109 of file planning_components_visualizer.cpp.

void PlanningComponentsVisualizer::determinePitchRollConstraintsGivenState	(	const PlanningComponentsVisualizer::GroupCollection &	gc,
		const planning_models::KinematicState &	state,
		arm_navigation_msgs::OrientationConstraint &	goal_constraint,
		arm_navigation_msgs::OrientationConstraint &	path_constraint
	)			const `[inline]`

Definition at line 936 of file planning_components_visualizer.cpp.

bool PlanningComponentsVisualizer::doesGroupHaveGoodIKSolution ( const string & group ) const [inline]

Definition at line 2295 of file planning_components_visualizer.cpp.

bool PlanningComponentsVisualizer::doesGroupHaveGoodTrajectory	(	const string &	group,
		const string &	source
	)			const `[inline]`

Definition at line 2304 of file planning_components_visualizer.cpp.

bool PlanningComponentsVisualizer::filterPlannerTrajectory ( PlanningComponentsVisualizer::GroupCollection & gc ) [inline]

Definition at line 1253 of file planning_components_visualizer.cpp.

size_t PlanningComponentsVisualizer::getNumPlanningGroups ( ) [inline]

Returns the number of kinematic chains in the robot.

Returns:: size_t with number of chains of the robot.

Definition at line 2322 of file planning_components_visualizer.cpp.

GroupCollection* PlanningComponentsVisualizer::getPlanningGroup ( unsigned int i ) [inline]

Definition at line 2327 of file planning_components_visualizer.cpp.

bool PlanningComponentsVisualizer::isGroupName ( const string & name ) [inline]

Returns true if the string of the given name corresponds to a kinematic chain in the robot.

Parameters:

name

a string corresponding to a kinematic chain.

Returns:: true if the name is a kinematic chain, or false otherwise.

Definition at line 1944 of file planning_components_visualizer.cpp.

bool PlanningComponentsVisualizer::isValidJointName	(	GroupCollection &	gc,
		string	name
	)			`[inline]`

Definition at line 648 of file planning_components_visualizer.cpp.

void PlanningComponentsVisualizer::makeInteractive1DOFRotationMarker	(	btTransform	transform,
		btVector3	axis,
		string	name,
		string	description,
		float	scale = `1.0f`,
		float	angle = `0.0f`
	)			`[inline]`

Definition at line 2171 of file planning_components_visualizer.cpp.

void PlanningComponentsVisualizer::makeInteractive1DOFTranslationMarker	(	btTransform	transform,
		btVector3	axis,
		string	name,
		string	description,
		float	scale = `1.0f`,
		float	value = `0.0f`
	)			`[inline]`

Definition at line 2134 of file planning_components_visualizer.cpp.

void PlanningComponentsVisualizer::makeInteractive6DOFMarker	(	bool	fixed,
		btTransform	transform,
		string	name,
		string	description,
		float	scale = `1.0f`,
		bool	pole = `false`
	)			`[inline]`

Creates an interactive 6DOF marker control.

Parameters:

	fixed	if true, the axes remain fixed to the frame of the marker. Otherwise they rotate with input.
	transform	the location and rotation of the 6DOF marker.
	name	used internally to represent the marker. Must be unique.
	description	drawn above the marker as a label.
	scale	uniformly sets the size in meters of the marker.
	pole	if true, the marker is a large green cylinder. Otherwise it is a small white cube.

Definition at line 2217 of file planning_components_visualizer.cpp.

InteractiveMarkerControl& PlanningComponentsVisualizer::makeInteractiveBoxControl	(	InteractiveMarker &	msg,
		float	alpha = `1.0f`
	)			`[inline]`

Creates a clickable, box shaped marker.

Parameters:

	msg	the interactive marker to associate this box with.
	alpha	the transparency of the marker.

Returns:: the control (which is a clickable box)

Definition at line 1589 of file planning_components_visualizer.cpp.

InteractiveMarkerControl& PlanningComponentsVisualizer::makeInteractiveCylinderControl	(	InteractiveMarker &	msg,
		float	alpha = `1.0f`
	)			`[inline]`

Creates a clickable, cylinder shaped marker.

Parameters:

	msg	the interactive marker to associate this cylinder with.
	alpha	the transparency of the marker.

Returns:: the control (which is a clickable cylinder)

Definition at line 1604 of file planning_components_visualizer.cpp.

Marker PlanningComponentsVisualizer::makeMarkerBox	(	InteractiveMarker &	msg,
		float	alpha = `1.0f`
	)			`[inline]`

Creates a box shaped marker.

Parameters:

	msg	the interactive marker to associate this box with.
	alpha	the transparency of the marker.

Returns:: the marker (which is a box).

Definition at line 1523 of file planning_components_visualizer.cpp.

Marker PlanningComponentsVisualizer::makeMarkerCylinder	(	InteractiveMarker &	msg,
		float	alpha = `1.0f`
	)			`[inline]`

Creates a cylinder shaped marker.

Parameters:

	msg	the interactive marker to associate this cylinder with.
	alpha	the transparency of the marker.

Returns:: the marker (which is a cylinder).

Definition at line 1545 of file planning_components_visualizer.cpp.

Marker PlanningComponentsVisualizer::makeMarkerSphere ( InteractiveMarker & msg ) [inline]

Creates a sphere-shaped marker.

Parameters:

msg

the interactive marker to associate this sphere with.

Returns:: the marker (which is a sphere)

Definition at line 1566 of file planning_components_visualizer.cpp.

void PlanningComponentsVisualizer::makePoleContextMenu	(	btTransform	transform,
		string	name,
		string	description,
		float	scale = `1.0f`
	)			`[inline]`

Wrapper for makeSelectableMarker which assumes we are creating a collision pole.

Definition at line 1984 of file planning_components_visualizer.cpp.

void PlanningComponentsVisualizer::makeSelectableMarker	(	InteractiveMarkerType	type,
		btTransform	transform,
		string	name,
		string	description,
		float	scale = `1.0f`,
		bool	publish = `true`
	)			`[inline]`

Creates a marker that is initially a clickable menu. Upon selection it turns into a 6DOF control.

Parameters:

	type	the type (collision object, joint, IK control, etc.) of the selectable marker.
	transform	location and orientation of the marker.
	name	internal, unique representation of the marker (this is for the 6DOF control)
	description	displayed above the menu marker and the 6DOF marker.
	scale	uniformly sizes the marker and its controls
	publish	if true, the marker server will publish the marker. Otherwise, it will not.

Definition at line 1999 of file planning_components_visualizer.cpp.

void PlanningComponentsVisualizer::makeTopLevelMenu ( ) [inline]

Definition at line 1949 of file planning_components_visualizer.cpp.

void PlanningComponentsVisualizer::moveEndEffectorMarkers	(	double	vx,
		double	vy,
		double	vz,
		double	vr,
		double	vp,
		double	vw,
		bool	coll_aware = `true`
	)			`[inline]`

Definition at line 660 of file planning_components_visualizer.cpp.

void PlanningComponentsVisualizer::moveThroughTrajectory	(	PlanningComponentsVisualizer::GroupCollection &	gc,
		const string &	source_name,
		int	step
	)			`[inline]`

Definition at line 1323 of file planning_components_visualizer.cpp.

int PlanningComponentsVisualizer::nextCollisionPole ( ) [inline]

Generates a new collision pole ID.

Returns:: an int, which is unique.

Definition at line 414 of file planning_components_visualizer.cpp.

bool PlanningComponentsVisualizer::planToEndEffectorState ( PlanningComponentsVisualizer::GroupCollection & gc ) [inline]

Definition at line 1137 of file planning_components_visualizer.cpp.

bool PlanningComponentsVisualizer::playTrajectory	(	PlanningComponentsVisualizer::GroupCollection &	gc,
		const string &	source_name,
		const trajectory_msgs::JointTrajectory &	traj
	)			`[inline]`

Definition at line 1286 of file planning_components_visualizer.cpp.

void PlanningComponentsVisualizer::processInteractiveFeedback ( const InteractiveMarkerFeedbackConstPtr & feedback ) [inline]

Main function that handles interactive marker feedback of all kinds.

Parameters:

feedback

the feedback message received by ROS. TODO: Replace this with several feedback functions.

Definition at line 1631 of file planning_components_visualizer.cpp.

void PlanningComponentsVisualizer::publishJointStates ( ) [inline]

Definition at line 1120 of file planning_components_visualizer.cpp.

void PlanningComponentsVisualizer::randomlyPerturb ( PlanningComponentsVisualizer::GroupCollection & gc ) [inline]

Definition at line 1204 of file planning_components_visualizer.cpp.

void PlanningComponentsVisualizer::refreshEnvironment ( ) [inline]

Sends all collision pole changes and changes to the robot state to the planning environment.

Definition at line 1616 of file planning_components_visualizer.cpp.

MenuHandler::EntryHandle PlanningComponentsVisualizer::registerMenuEntry	(	MenuHandler &	handler,
		MenuEntryMap &	map,
		string	name
	)			`[inline]`

Creates a new menu entry for the given menu handler, putting its handle in the given map.

Parameters:

	handler	the menu handler to associate this entry with.
	map	the menu entry map to associate the name with a handle.
	the	name which appears in the menu.

Returns:: the handle that was registered

Definition at line 1380 of file planning_components_visualizer.cpp.

MenuHandler::EntryHandle PlanningComponentsVisualizer::registerSubMenuEntry	(	MenuHandler &	handler,
		MenuEntryMap &	map,
		string	name,
		MenuHandler::EntryHandle	subMenuHandle
	)			`[inline]`

Creates a new menu entry for the given menu handler, putting its handle in the given map.

Parameters:

	handler	the menu handler to associate this entry with.
	map	the menu entry map to associate the name with a handle.
	the	name which appears in the menu.
	subMenuHandle	a menu handle to an existing menu entry in the menu handler given.

Returns:: the handle that was registered

Definition at line 1365 of file planning_components_visualizer.cpp.

void PlanningComponentsVisualizer::removeCollisionPole ( int num ) [inline]

deletes the collision pole with the given ID.

Parameters:

num

the ID of the pole.

Definition at line 423 of file planning_components_visualizer.cpp.

void PlanningComponentsVisualizer::removeCollisionPoleByName ( string id ) [inline]

deletes the collision pole with the given name.

Parameters:

id

the name of the pole.

Definition at line 434 of file planning_components_visualizer.cpp.

void PlanningComponentsVisualizer::resetToLastGoodState ( GroupCollection & gc ) [inline]

Definition at line 908 of file planning_components_visualizer.cpp.

bool PlanningComponentsVisualizer::selectableMarkerExists ( string name ) [inline]

Returns true if a selectable marker of the given name exists in the marker map.

Parameters:

name

the unique identifier of the marker.

Returns:: true if the marker exists, or false otherwise.

Definition at line 2064 of file planning_components_visualizer.cpp.

void PlanningComponentsVisualizer::selectMarker	(	SelectableMarker &	marker,
		btTransform	transform
	)			`[inline]`

Removes the menu marker given and replaces it with a 6DOF marker.

Parameters:

	marker	a reference to the selectablemarker struct.
	transform	location to select the marker.

Definition at line 2074 of file planning_components_visualizer.cpp.

void PlanningComponentsVisualizer::selectPlanningGroup ( unsigned int entry ) [inline]

Definition at line 579 of file planning_components_visualizer.cpp.

void PlanningComponentsVisualizer::sendMarkers ( ) [inline]

Definition at line 1387 of file planning_components_visualizer.cpp.

void PlanningComponentsVisualizer::sendPlanningScene ( ) [inline]

Definition at line 471 of file planning_components_visualizer.cpp.

void PlanningComponentsVisualizer::sendTransforms ( ) [inline]

Definition at line 1496 of file planning_components_visualizer.cpp.

void PlanningComponentsVisualizer::setJointState	(	GroupCollection &	gc,
		std::string &	joint_name,
		btTransform	value
	)			`[inline]`

Sets the kinematic state of a joint in the given group collection.

Parameters:

	gc	the group collection that the joint is in.
	value,a	transform that the joint will attempt to match.

Definition at line 807 of file planning_components_visualizer.cpp.

void PlanningComponentsVisualizer::setNewEndEffectorPosition	(	GroupCollection &	gc,
		btTransform &	cur,
		bool	coll_aware
	)			`[inline]`

Definition at line 918 of file planning_components_visualizer.cpp.

bool PlanningComponentsVisualizer::solveIKForEndEffectorPose	(	PlanningComponentsVisualizer::GroupCollection &	gc,
		bool	coll_aware = `true`,
		bool	constrain_pitch_and_roll = `false`,
		double	change_redundancy = `0.0`
	)			`[inline]`