sbpl_arm_planner::SBPLArmPlannerNode Class Reference

#include <sbpl_arm_planner_node.h>

Public Member Functions
bool	init ()
	Initialize the planner node.
bool	planKinematicPath (motion_planning_msgs::GetMotionPlan::Request &req, motion_planning_msgs::GetMotionPlan::Response &res)
	Planning service call back function.
int	run ()
	Run the node!
	SBPLArmPlannerNode ()
	Constructor-doesn't initialize class, you need to run init().
	~SBPLArmPlannerNode ()
Private Member Functions
void	attachedObjectCallback (const mapping_msgs::AttachedCollisionObjectConstPtr &attached_object)
void	attachObject (const mapping_msgs::CollisionObject &obj)
void	collisionMapCallback (const mapping_msgs::CollisionMapConstPtr &collision_map)
	Callback function that gets called when a new collision map is available. It updates the internal distance field with the map and visualizes it (if desired).
void	collisionObjectCallback (const mapping_msgs::CollisionObjectConstPtr &collision_object)
bool	computeFK (const std::vector< double > &jnt_pos, geometry_msgs::Pose &pose)
	Compute FK using pr2_fk service.
void	computeFKwithKDL (const std::vector< double > &jnt_pos, geometry_msgs::Pose &pose, int joint_num)
	Compute FK using the KDL library.
bool	computeIK (const geometry_msgs::Pose &pose_stamped_msg, std::vector< double > jnt_pos, std::vector< double > &solution)
	Compute IK using pr2_ik service.
void	displayARAStarStates ()
	Display states expanded by ARA* search.
void	displayShortestPath ()
	Computes and visualizes the 3D path to goal returned by heuristic.
bool	initChain (std::string robot_description)
	Initialize KDL Chain for using FK.
bool	initializePlannerAndEnvironment ()
	Initialize the SBPL planner and the sbpl_arm_planner environment.
bool	isGoalConstraintSatisfied (const std::vector< double > &angles, const motion_planning_msgs::Constraints &goal)
	Check if path satisfies goal constraints.
bool	plan (std::vector< trajectory_msgs::JointTrajectoryPoint > &arm_path)
	Retrieve plan from sbpl.
bool	planToPosition (motion_planning_msgs::GetMotionPlan::Request &req, motion_planning_msgs::GetMotionPlan::Response &res)
	Plan a path to a cartesian goal(s).
void	printPath (const std::vector< trajectory_msgs::JointTrajectoryPoint > &arm_path)
	Print out the trajectory along with the end effector coords of each waypoint.
void	printPath (FILE *fOut, const std::vector< std::vector< double > > path)
	Print the trajectory to a file.
void	setArmToMapTransform (std::string &map_frame)
	Set the transform in the environment class (remove this).
bool	setGoalPosition (const motion_planning_msgs::Constraints &goals)
	Set cartesian goal(s).
bool	setStart (const sensor_msgs::JointState &start_state)
	Set start configuration.
void	updateMapFromCollisionMap (const mapping_msgs::CollisionMapConstPtr &collision_map)
	Callback function that's called by the collision map topic. It reformats the collision map for the sbpl grid.
void	visualizeAttachedObject (const std::vector< double > &angles)
void	visualizeAttachedObject (trajectory_msgs::JointTrajectory &traj_msg, int throttle)
void	visualizeCollisionObjects ()
	Visualize voxels occupying known collision objects.
void	visualizeElbowPoses ()
void	visualizeGoalPosition (const motion_planning_msgs::Constraints &goal)
	Publish a visualization marker to display the goal end effector position in rviz.
Private Attributes
double	allocated_time_
	Maximum time for planner to return a solution.
KDL::Chain	arm_chain_
std::string	arm_description_filename_
std::string	arm_name_
bool	attached_object_
std::string	attached_object_frame_
VisualizeArm *	aviz_
std::vector< std::vector < double > >	col_objects_
tf::MessageFilter < mapping_msgs::CollisionMap > *	collision_map_filter_
message_filters::Subscriber < mapping_msgs::CollisionMap >	collision_map_subscriber_
std::string	collision_map_topic_
ros::Subscriber	collision_object_subscriber_
boost::mutex	colmap_mutex_
SBPLCollisionSpace *	cspace_
std::vector< std::vector < double > >	dpath_
double	env_resolution_
std::string	fk_service_name_
	The name of the forward kinematics service.
bool	forward_search_
OccupancyGrid *	grid_
std::string	ik_service_name_
	The name of the inverse kinematics service.
KDL::JntArray	jnt_pos_
KDL::ChainFkSolverPos_recursive *	jnt_to_pose_solver_
	A FK solver. (The FK service is currently being used).
std::vector< std::string >	joint_names_
KDL::Frame	kdl_transform_
std::string	map_frame_
MDPConfig	mdp_cfg_
std::string	mprims_filename_
ros::NodeHandle	node_handle_
std::string	node_name_
int	num_joints_
std::map< std::string, mapping_msgs::CollisionObject >	object_map_
boost::mutex	object_mutex_
ros::Subscriber	object_subscriber_
SBPLPlanner *	planner_
bool	planner_initialized_
std::string	planning_joint_
ros::ServiceServer	planning_service_
bool	print_path_
std::string	reference_frame_
std::string	robot_description_
ros::NodeHandle	root_handle_
EnvironmentROBARM3D	sbpl_arm_env_
bool	search_mode_
std::string	side_
	"r" - right arm, "l" - left arm
tf::TransformListener	tf_
int	throttle_
std::string	trajectory_type_
tf::StampedTransform	transform_
bool	use_first_solution_
bool	use_research_heuristic_
bool	visualize_collision_model_trajectory_
bool	visualize_expanded_states_
bool	visualize_goal_
bool	visualize_heuristic_
bool	visualize_trajectory_
double	waypoint_time_
	Time interval sent with each waypoint in trajectory to movearm.

Detailed Description

Definition at line 47 of file sbpl_arm_planner_node.h.

Constructor & Destructor Documentation

SBPLArmPlannerNode::SBPLArmPlannerNode ( )

Constructor-doesn't initialize class, you need to run init().

Initializers -------------------------------------------------------------

Definition at line 41 of file sbpl_arm_planner_node.cpp.

SBPLArmPlannerNode::~SBPLArmPlannerNode ( )

Definition at line 52 of file sbpl_arm_planner_node.cpp.

Member Function Documentation

void SBPLArmPlannerNode::attachedObjectCallback ( const mapping_msgs::AttachedCollisionObjectConstPtr & attached_object ) [private]

Definition at line 246 of file sbpl_arm_planner_node.cpp.

void SBPLArmPlannerNode::attachObject ( const mapping_msgs::CollisionObject & obj ) [private]

Definition at line 328 of file sbpl_arm_planner_node.cpp.

void SBPLArmPlannerNode::collisionMapCallback ( const mapping_msgs::CollisionMapConstPtr & collision_map ) [private]

Callback function that gets called when a new collision map is available. It updates the internal distance field with the map and visualizes it (if desired).

Callbacks ----------------------------------------------------------------

Definition at line 202 of file sbpl_arm_planner_node.cpp.

void SBPLArmPlannerNode::collisionObjectCallback ( const mapping_msgs::CollisionObjectConstPtr & collision_object ) [private]

Definition at line 305 of file sbpl_arm_planner_node.cpp.

bool SBPLArmPlannerNode::computeFK	(	const std::vector< double > &	jnt_pos,
		geometry_msgs::Pose &	pose
	)			`[private]`

Compute FK using pr2_fk service.

Definition at line 860 of file sbpl_arm_planner_node.cpp.

void SBPLArmPlannerNode::computeFKwithKDL	(	const std::vector< double > &	jnt_pos,
		geometry_msgs::Pose &	pose,
		int	joint_num
	)			`[private]`

Compute FK using the KDL library.

Definition at line 897 of file sbpl_arm_planner_node.cpp.

bool SBPLArmPlannerNode::computeIK	(	const geometry_msgs::Pose &	pose_stamped_msg,
		std::vector< double >	jnt_pos,
		std::vector< double > &	solution
	)			`[private]`

Compute IK using pr2_ik service.

Definition at line 811 of file sbpl_arm_planner_node.cpp.

void SBPLArmPlannerNode::displayARAStarStates ( ) [private]

Display states expanded by ARA* search.

Definition at line 966 of file sbpl_arm_planner_node.cpp.

void SBPLArmPlannerNode::displayShortestPath ( ) [private]

Computes and visualizes the 3D path to goal returned by heuristic.

Definition at line 1077 of file sbpl_arm_planner_node.cpp.

bool SBPLArmPlannerNode::init ( )

Initialize the planner node.

Definition at line 64 of file sbpl_arm_planner_node.cpp.

bool SBPLArmPlannerNode::initChain ( std::string robot_description ) [private]

Initialize KDL Chain for using FK.

Definition at line 918 of file sbpl_arm_planner_node.cpp.

bool SBPLArmPlannerNode::initializePlannerAndEnvironment ( ) [private]

Initialize the SBPL planner and the sbpl_arm_planner environment.

Definition at line 150 of file sbpl_arm_planner_node.cpp.

bool SBPLArmPlannerNode::isGoalConstraintSatisfied	(	const std::vector< double > &	angles,
		const motion_planning_msgs::Constraints &	goal
	)			`[private]`

Check if path satisfies goal constraints.

Definition at line 725 of file sbpl_arm_planner_node.cpp.

bool SBPLArmPlannerNode::plan ( std::vector< trajectory_msgs::JointTrajectoryPoint > & arm_path ) [private]

Retrieve plan from sbpl.

Definition at line 671 of file sbpl_arm_planner_node.cpp.

bool SBPLArmPlannerNode::planKinematicPath	(	motion_planning_msgs::GetMotionPlan::Request &	req,
		motion_planning_msgs::GetMotionPlan::Response &	res
	)

Planning service call back function.

Definition at line 651 of file sbpl_arm_planner_node.cpp.

bool SBPLArmPlannerNode::planToPosition	(	motion_planning_msgs::GetMotionPlan::Request &	req,
		motion_planning_msgs::GetMotionPlan::Response &	res
	)			`[private]`