#include <cart_local_planner.h>

Inheritance diagram for cart_local_planner::CartLocalPlanner:

Public Member Functions
	CartLocalPlanner ()
bool	computeVelocityCommands (geometry_msgs::Twist &cmd_vel)
void	initialize (std::string name, tf::TransformListener tf, costmap_2d::Costmap2DROS costmap_ros)
bool	isGoalReached ()
bool	setPlan (const std::vector< geometry_msgs::PoseStamped > &global_plan)
Protected Types
enum	CONTROL_MODE { REGULAR }
enum	FILTER_OPTIONS { GLOBAL_SCALING = 0x1, CART_ERR_SCALING = GLOBAL_SCALING << 1, ALL = 0xffff }
Protected Member Functions
double	baseTwistFromError ()
	* Sets twist_base_ based on robot_pose_error_, returns twist magnitude
double	cartTwistFromError ()
	* Sets twist_cart_ based on cart_pose_error_, returns twist magnitutde
bool	checkTrajectoryMonotonic (const std::vector< unsigned int > &indices)
virtual bool	checkTwists ()
virtual bool	checkTwistsMonotonic ()
virtual void	controlModeAction ()
virtual void	filterTwistsCombined (int filter_options)
void	freeze ()
	* set zero twist to arms and base */
bool	getNextFewWaypointsIndices (const std::vector< geometry_msgs::PoseStamped > &current_plan, const int &current_waypoint_index, const int &max_num_waypoints, const double &max_translation, const double &max_rotation, std::vector< unsigned int > &waypoint_indices)
virtual void	initialization_extras ()
	A place to implement any additional initialization required for the planner. Gets called at the end of initialize()
void	invalidPoseCallback (const std_msgs::Empty::ConstPtr &msg)
	* callback for invalid pose notification */
geometry_msgs::Twist	mapBaseTwistToCart (const geometry_msgs::Twist &twist_base)
	* map twist_base to cart frame, thanks to KDL's frametwist operator /
void	odomCallback (const nav_msgs::Odometry::ConstPtr &msg)
	* callback for base controller odometry */
void	publishDebugPose (geometry_msgs::Pose &p)
void	publishDebugPose (tf::Pose &p)
void	publishDebugTwist (geometry_msgs::Twist &t)
int	setCartGoalFromWaypoint (double min_dist, double min_x_component)
void	setCartPoseGoal (const tf::Stamped< tf::Pose > &goal)
virtual void	setControlMode ()
	Sets the internal control mode for the computeVelocityCommands switch based on the current state of the robot and the cart.
virtual void	setGoalPoses ()
void	setRobotPoseGoal (const tf::Stamped< tf::Pose > &goal)
void	setYawFromVec (const tf::Pose &pose1, const tf::Pose &pose2, tf::Pose &res)
	* makes a new pose with yaw set from xy coordinate deltas (atan) */
std::vector< geometry_msgs::Point >	transformFootprint (const geometry_msgs::PolygonStamped &poly) const
	* converts poly to footprint in the costmap global frame */
bool	transformGlobalPlan (const tf::TransformListener &tf, const std::vector< geometry_msgs::PoseStamped > &global_plan, const costmap_2d::Costmap2DROS &costmap, const std::string &global_frame, std::vector< geometry_msgs::PoseStamped > &transformed_plan)
	* Transforms global_plan to plan in global frame, and does pruning */
Static Protected Member Functions
static void	scaleTwist2D (geometry_msgs::Twist &t, double scale)
	* scale twist by constant factor */
Protected Attributes
nav_msgs::Odometry	base_odom_
CostmapTrajectoryChecker	cart_collision_checker_
tf::Stamped< tf::Pose >	cart_pose_actual_
geometry_msgs::Pose2D	cart_pose_error_
tf::Stamped< tf::Pose >	cart_pose_goal_
ros::Publisher	cart_pose_pub_
pose_range_2D	cart_range
ros::Publisher	cart_twist_pub_
enum CONTROL_MODE	control_mode_
costmap_2d::Costmap2DROS *	costmap_ros_
unsigned int	current_waypoint_
bool	debug_print_
double	dt_
std::vector < geometry_msgs::PoseStamped >	global_plan_
ros::Time	goal_reached_time_
bool	initialized_
boost::mutex	invalid_pose_mutex_
ros::Subscriber	invalid_pose_sub_
double	K_rot_base_
double	K_rot_cart_
double	K_trans_base_
double	K_trans_cart_
ros::Time	last_invalid_pose_time_
ros::NodeHandle	nh_
int	num_traj_steps_
boost::mutex	odom_lock_
ros::Subscriber	odom_sub_
std::vector < geometry_msgs::PoseStamped >	original_global_plan_
ros::Publisher	pose2D_pub_
CostmapTrajectoryChecker	robot_collision_checker_
tf::Stamped< tf::Pose >	robot_pose_actual_
tf::Pose	robot_pose_error_
tf::Stamped< tf::Pose >	robot_pose_goal_
double	rot_stopped_velocity_
boost::shared_ptr < cart_local_planner::SBPLSubscriber < cart_pushing_msgs::RobotCartPath > >	sbpl_subscriber_
bool	subscribe_sbpl_plan_
tf::TransformBroadcaster	tb_
tf::TransformListener *	tf_
double	tolerance_rot_
double	tolerance_timeout_
double	tolerance_trans_
double	trans_stopped_velocity_
geometry_msgs::Twist *	twist_base_
geometry_msgs::Twist	twist_base_max_
geometry_msgs::TwistStamped	twist_cart_
geometry_msgs::Twist	twist_cart_max_
ros::Publisher	vel_pub_

Detailed Description

Definition at line 81 of file cart_local_planner.h.

Member Enumeration Documentation

enum cart_local_planner::CartLocalPlanner::CONTROL_MODE [protected]

Enumerator:

REGULAR

Reimplemented in cart_local_planner::FixedFrontCartPlanner, and cart_local_planner::HolonomicCartPlanner.

Definition at line 202 of file cart_local_planner.h.

enum cart_local_planner::CartLocalPlanner::FILTER_OPTIONS [protected]

Enumerator:

GLOBAL_SCALING
CART_ERR_SCALING
ALL

Reimplemented in cart_local_planner::FixedFrontCartPlanner, and cart_local_planner::HolonomicCartPlanner.

Definition at line 196 of file cart_local_planner.h.

Constructor & Destructor Documentation

cart_local_planner::CartLocalPlanner::CartLocalPlanner ( )

Definition at line 135 of file cart_local_planner.cpp.

Member Function Documentation

double cart_local_planner::CartLocalPlanner::baseTwistFromError ( ) [protected]

* Sets twist_base_ based on robot_pose_error_, returns twist magnitude

Definition at line 695 of file cart_local_planner.cpp.

double cart_local_planner::CartLocalPlanner::cartTwistFromError ( ) [protected]

* Sets twist_cart_ based on cart_pose_error_, returns twist magnitutde

Definition at line 704 of file cart_local_planner.cpp.

bool cart_local_planner::CartLocalPlanner::checkTrajectoryMonotonic ( const std::vector< unsigned int > & indices ) [protected]

Definition at line 353 of file cart_local_planner.cpp.

bool cart_local_planner::CartLocalPlanner::checkTwists ( ) [protected, virtual]

Check twists for collision in costmap

Returns:: True if not in collision

Definition at line 568 of file cart_local_planner.cpp.

bool cart_local_planner::CartLocalPlanner::checkTwistsMonotonic ( ) [protected, virtual]

Check twists for collision in costmap

Returns:: True if not in collision

Definition at line 602 of file cart_local_planner.cpp.

bool cart_local_planner::CartLocalPlanner::computeVelocityCommands ( geometry_msgs::Twist & cmd_vel ) [virtual]

Get the current pose of the robot in the fixed frame

Get the current pose of the cart in the base frame

Save member copies of cart and robot states (the slow way, because TF is annoying)

Alias cmd_vel with our local reference

Set goals for the robot and cart given the current state

Set a control mode based on current state

Generate controls for current control mode

Check final twists before returning

Implements nav_core::BaseLocalPlanner.

Definition at line 242 of file cart_local_planner.cpp.

void cart_local_planner::CartLocalPlanner::controlModeAction ( ) [protected, virtual]

Implements what to do during computeVelocityCommands for each control mode. This provides a greater degree of control than the default supported by the base_local_planner API, for problems such as rotating in place or evading collisions that the global planner doesn't know about (e.g. cart collisions).

Reimplemented in cart_local_planner::FixedFrontCartPlanner, and cart_local_planner::HolonomicCartPlanner.

Definition at line 458 of file cart_local_planner.cpp.

void cart_local_planner::CartLocalPlanner::filterTwistsCombined ( int filter_options ) [protected, virtual]

Applies requested operations on the robot and cart twists

Parameters:

filter_options An int containing OR'ed together FILTER_OPTIONS

1: Scale everything by its max;

2: Scale base vel based on cart error (turns out to be a very handy trick for keeping the cart on track - this is basically a gaussian)

Reimplemented in cart_local_planner::FixedFrontCartPlanner, and cart_local_planner::HolonomicCartPlanner.

Definition at line 521 of file cart_local_planner.cpp.

void cart_local_planner::CartLocalPlanner::freeze ( ) [protected]

* set zero twist to arms and base */

Definition at line 841 of file cart_local_planner.cpp.

bool cart_local_planner::CartLocalPlanner::getNextFewWaypointsIndices	(	const std::vector< geometry_msgs::PoseStamped > &	current_plan,
		const int &	current_waypoint_index,
		const int &	max_num_waypoints,
		const double &	max_translation,
		const double &	max_rotation,
		std::vector< unsigned int > &	waypoint_indices
	)		`[protected]`

Definition at line 403 of file cart_local_planner.cpp.

void cart_local_planner::CartLocalPlanner::initialization_extras ( ) [protected, virtual]

A place to implement any additional initialization required for the planner. Gets called at the end of initialize()

Reimplemented in cart_local_planner::FixedFrontCartPlanner, and cart_local_planner::HolonomicCartPlanner.

Definition at line 238 of file cart_local_planner.cpp.

void cart_local_planner::CartLocalPlanner::initialize	(	std::string	name,
		tf::TransformListener *	tf,
		costmap_2d::Costmap2DROS *	costmap_ros
	)		`[virtual]`

Implements nav_core::BaseLocalPlanner.

Definition at line 147 of file cart_local_planner.cpp.

void cart_local_planner::CartLocalPlanner::invalidPoseCallback ( const std_msgs::Empty::ConstPtr & msg ) [protected]

* callback for invalid pose notification */

Definition at line 909 of file cart_local_planner.cpp.

bool cart_local_planner::CartLocalPlanner::isGoalReached ( ) [virtual]

Implements nav_core::BaseLocalPlanner.

Definition at line 915 of file cart_local_planner.cpp.

gm::Twist cart_local_planner::CartLocalPlanner::mapBaseTwistToCart ( const geometry_msgs::Twist & twist_base ) [protected]

* map twist_base to cart frame, thanks to KDL's frame*twist operator */

Definition at line 861 of file cart_local_planner.cpp.

void cart_local_planner::CartLocalPlanner::odomCallback ( const nav_msgs::Odometry::ConstPtr & msg ) [protected]

* callback for base controller odometry */

Definition at line 901 of file cart_local_planner.cpp.

void cart_local_planner::CartLocalPlanner::publishDebugPose ( geometry_msgs::Pose & p ) [protected]

Definition at line 893 of file cart_local_planner.cpp.

void cart_local_planner::CartLocalPlanner::publishDebugPose ( tf::Pose & p ) [protected]

void cart_local_planner::CartLocalPlanner::publishDebugTwist ( geometry_msgs::Twist & t ) [protected]

Definition at line 885 of file cart_local_planner.cpp.

void cart_local_planner::CartLocalPlanner::scaleTwist2D	(	geometry_msgs::Twist &	t,
		double	scale
	)		`[static, protected]`

* scale twist by constant factor */

Definition at line 854 of file cart_local_planner.cpp.

int cart_local_planner::CartLocalPlanner::setCartGoalFromWaypoint	(	double	min_dist,
		double	min_x_component
	)		`[protected]`

Picks a future waypoint on the global plan for the cart based on the distance from the robot

Parameters:

min_dist	The minimum distance a future waypoint must be in order to qualify as a cart goal pose
min_x_component	The amount of min_dist that must be in the X direction

Returns:: The waypoint number chosen to set the target cart pose, or -1 on failure

Definition at line 644 of file cart_local_planner.cpp.

void cart_local_planner::CartLocalPlanner::setCartPoseGoal ( const tf::Stamped< tf::Pose > & goal ) [protected]

Definition at line 443 of file cart_local_planner.cpp.

void cart_local_planner::CartLocalPlanner::setControlMode ( ) [protected, virtual]

Sets the internal control mode for the computeVelocityCommands switch based on the current state of the robot and the cart.

Reimplemented in cart_local_planner::FixedFrontCartPlanner, and cart_local_planner::HolonomicCartPlanner.

Definition at line 453 of file cart_local_planner.cpp.

void cart_local_planner::CartLocalPlanner::setGoalPoses ( ) [protected, virtual]

Sets goal poses for the cart and the robot given current state and way-point

Definition at line 482 of file cart_local_planner.cpp.

bool cart_local_planner::CartLocalPlanner::setPlan ( const std::vector< geometry_msgs::PoseStamped > & global_plan ) [virtual]

Implements nav_core::BaseLocalPlanner.

Definition at line 739 of file cart_local_planner.cpp.

void cart_local_planner::CartLocalPlanner::setRobotPoseGoal ( const tf::Stamped< tf::Pose > & goal ) [protected]

Definition at line 437 of file cart_local_planner.cpp.

void cart_local_planner::CartLocalPlanner::setYawFromVec	(	const tf::Pose &	pose1,
		const tf::Pose &	pose2,
		tf::Pose &	res
	)		`[protected]`

* makes a new pose with yaw set from xy coordinate deltas (atan) */

Definition at line 716 of file cart_local_planner.cpp.

vector< gm::Point > cart_local_planner::CartLocalPlanner::transformFootprint ( const geometry_msgs::PolygonStamped & poly ) const [protected]

* converts poly to footprint in the costmap global frame */

Definition at line 823 of file cart_local_planner.cpp.

bool cart_local_planner::CartLocalPlanner::transformGlobalPlan	(	const tf::TransformListener &	tf,
		const std::vector< geometry_msgs::PoseStamped > &	global_plan,
		const costmap_2d::Costmap2DROS &	costmap,
		const std::string &	global_frame,
		std::vector< geometry_msgs::PoseStamped > &	transformed_plan
	)		`[protected]`