Implements the actual abstract navigation stack routines of the teb_local_planner plugin. More...

#include <teb_local_planner_ros.h>

Inheritance diagram for teb_local_planner::TebLocalPlannerROS:

Public Member Functions
bool	computeVelocityCommands (geometry_msgs::Twist &cmd_vel)
	Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base.
void	initialize (std::string name, tf::TransformListener tf, costmap_2d::Costmap2DROS costmap_ros)
	Initializes the teb plugin.
bool	isGoalReached ()
	Check if the goal pose has been achieved.
bool	setPlan (const std::vector< geometry_msgs::PoseStamped > &orig_global_plan)
	Set the plan that the teb local planner is following.
	TebLocalPlannerROS ()
	Default constructor of the teb plugin.
	~TebLocalPlannerROS ()
	Destructor of the plugin.
Static Public Member Functions
Public utility functions/methods
static Eigen::Vector2d	tfPoseToEigenVector2dTransRot (const tf::Pose &tf_vel)
	Transform a tf::Pose type into a Eigen::Vector2d containing the translational and angular velocities.
static RobotFootprintModelPtr	getRobotFootprintFromParamServer (const ros::NodeHandle &nh)
	Get the current robot footprint/contour model.
static Point2dContainer	makeFootprintFromXMLRPC (XmlRpc::XmlRpcValue &footprint_xmlrpc, const std::string &full_param_name)
	Set the footprint from the given XmlRpcValue.
static double	getNumberFromXMLRPC (XmlRpc::XmlRpcValue &value, const std::string &full_param_name)
	Get a number from the given XmlRpcValue.
Protected Member Functions
void	configureBackupModes (std::vector< geometry_msgs::PoseStamped > &transformed_plan, int &goal_idx)
double	convertTransRotVelToSteeringAngle (double v, double omega, double wheelbase, double min_turning_radius=0) const
	Convert translational and rotational velocities to a steering angle of a carlike robot.
void	customObstacleCB (const teb_local_planner::ObstacleMsg::ConstPtr &obst_msg)
	Callback for custom obstacles that are not obtained from the costmap.
double	estimateLocalGoalOrientation (const std::vector< geometry_msgs::PoseStamped > &global_plan, const tf::Stamped< tf::Pose > &local_goal, int current_goal_idx, const tf::StampedTransform &tf_plan_to_global, int moving_average_length=3) const
	Estimate the orientation of a pose from the global_plan that is treated as a local goal for the local planner.
bool	pruneGlobalPlan (const tf::TransformListener &tf, const tf::Stamped< tf::Pose > &global_pose, std::vector< geometry_msgs::PoseStamped > &global_plan, double dist_behind_robot=1)
	Prune global plan such that already passed poses are cut off.
void	reconfigureCB (TebLocalPlannerReconfigureConfig &config, uint32_t level)
	Callback for the dynamic_reconfigure node.
void	saturateVelocity (double &vx, double &vy, double &omega, double max_vel_x, double max_vel_y, double max_vel_theta, double max_vel_x_backwards) const
	Saturate the translational and angular velocity to given limits.
bool	transformGlobalPlan (const tf::TransformListener &tf, const std::vector< geometry_msgs::PoseStamped > &global_plan, const tf::Stamped< tf::Pose > &global_pose, const costmap_2d::Costmap2D &costmap, const std::string &global_frame, double max_plan_length, std::vector< geometry_msgs::PoseStamped > &transformed_plan, int current_goal_idx=NULL, tf::StampedTransform tf_plan_to_global=NULL) const
	Transforms the global plan of the robot from the planner frame to the local frame (modified).
void	updateObstacleContainerWithCostmap ()
	Update internal obstacle vector based on occupied costmap cells.
void	updateObstacleContainerWithCostmapConverter ()
	Update internal obstacle vector based on polygons provided by a costmap_converter plugin.
void	updateObstacleContainerWithCustomObstacles ()
	Update internal obstacle vector based on custom messages received via subscriber.
void	updateViaPointsContainer (const std::vector< geometry_msgs::PoseStamped > &transformed_plan, double min_separation)
	Update internal via-point container based on the current reference plan.
void	validateFootprints (double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist)
	Validate current parameter values of the footprint for optimization, obstacle distance and the costmap footprint.
Private Attributes
TebConfig	cfg_
	Config class that stores and manages all related parameters.
costmap_2d::Costmap2D *	costmap_
	Pointer to the 2d costmap (obtained from the costmap ros wrapper)
boost::shared_ptr < costmap_converter::BaseCostmapToPolygons >	costmap_converter_
	Store the current costmap_converter.
pluginlib::ClassLoader < costmap_converter::BaseCostmapToPolygons >	costmap_converter_loader_
	Load costmap converter plugins at runtime.
boost::shared_ptr < base_local_planner::CostmapModel >	costmap_model_
costmap_2d::Costmap2DROS *	costmap_ros_
	Pointer to the costmap ros wrapper, received from the navigation stack.
boost::mutex	custom_obst_mutex_
	Mutex that locks the obstacle array (multi-threaded)
ros::Subscriber	custom_obst_sub_
	Subscriber for custom obstacles received via a ObstacleMsg.
ObstacleMsg	custom_obstacle_msg_
	Copy of the most recent obstacle message.
boost::shared_ptr < dynamic_reconfigure::Server < TebLocalPlannerReconfigureConfig > >	dynamic_recfg_
	Dynamic reconfigure server to allow config modifications at runtime.
std::vector< geometry_msgs::Point >	footprint_spec_
	Store the footprint of the robot.
std::string	global_frame_
	The frame in which the controller will run.
std::vector < geometry_msgs::PoseStamped >	global_plan_
	Store the current global plan.
bool	goal_reached_
	store whether the goal is reached or not
bool	initialized_
	Keeps track about the correct initialization of this class.
int	no_infeasible_plans_
	Store how many times in a row the planner failed to find a feasible plan.
ObstContainer	obstacles_
	Obstacle vector that should be considered during local trajectory optimization.
base_local_planner::OdometryHelperRos	odom_helper_
	Provides an interface to receive the current velocity from the robot.
PlannerInterfacePtr	planner_
	Instance of the underlying optimal planner class.
std::string	robot_base_frame_
	Used as the base frame id of the robot.
double	robot_circumscribed_radius
	The radius of the circumscribed circle of the robot.
PoseSE2	robot_goal_
	Store current robot goal.
double	robot_inscribed_radius_
	The radius of the inscribed circle of the robot (collision possible)
PoseSE2	robot_pose_
	Store current robot pose.
geometry_msgs::Twist	robot_vel_
	Store current robot translational and angular velocity (vx, vy, omega)
tf::TransformListener *	tf_
	pointer to Transform Listener
ros::Time	time_last_infeasible_plan_
	Store at which time stamp the last infeasible plan was detected.
ViaPointContainer	via_points_
	Container of via-points that should be considered during local trajectory optimization.
TebVisualizationPtr	visualization_
	Instance of the visualization class (local/global plan, obstacles, ...)

Detailed Description

Implements the actual abstract navigation stack routines of the teb_local_planner plugin.

Todo:: Escape behavior, more efficient obstacle handling

Definition at line 91 of file teb_local_planner_ros.h.

Constructor & Destructor Documentation

teb_local_planner::TebLocalPlannerROS::TebLocalPlannerROS ( )

Default constructor of the teb plugin.

Definition at line 64 of file teb_local_planner_ros.cpp.

teb_local_planner::TebLocalPlannerROS::~TebLocalPlannerROS ( )

Destructor of the plugin.

Definition at line 72 of file teb_local_planner_ros.cpp.

Member Function Documentation

bool teb_local_planner::TebLocalPlannerROS::computeVelocityCommands ( geometry_msgs::Twist & cmd_vel ) [virtual]

Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base.

Parameters:

cmd_vel Will be filled with the velocity command to be passed to the robot base

Returns:: True if a valid trajectory was found, false otherwise

Implements nav_core::BaseLocalPlanner.

Definition at line 204 of file teb_local_planner_ros.cpp.

void teb_local_planner::TebLocalPlannerROS::configureBackupModes	(	std::vector< geometry_msgs::PoseStamped > &	transformed_plan,
		int &	goal_idx
	)		`[protected]`

Definition at line 822 of file teb_local_planner_ros.cpp.

double teb_local_planner::TebLocalPlannerROS::convertTransRotVelToSteeringAngle	(	double	v,
		double	omega,
		double	wheelbase,
		double	min_turning_radius = `0`
	)		const `[protected]`

Convert translational and rotational velocities to a steering angle of a carlike robot.

The conversion is based on the following equations:

The turning radius is defined by
For a car like robot withe a distance L between both axles, the relation is: $tan(\phi) = L/R$

phi denotes the steering angle.

Remarks:: You might provide distances instead of velocities, since the temporal information is not required.

Parameters:

v	translational velocity [m/s]
omega	rotational velocity [rad/s]
wheelbase	distance between both axles (drive shaft and steering axle), the value might be negative for back_wheeled robots
min_turning_radius	Specify a lower bound on the turning radius

Returns:: Resulting steering angle in [rad] inbetween [-pi/2, pi/2]

Definition at line 798 of file teb_local_planner_ros.cpp.

void teb_local_planner::TebLocalPlannerROS::customObstacleCB ( const teb_local_planner::ObstacleMsg::ConstPtr & obst_msg ) [protected]

Callback for custom obstacles that are not obtained from the costmap.

Parameters:

obst_msg pointer to the message containing a list of polygon shaped obstacles

Definition at line 858 of file teb_local_planner_ros.cpp.

double teb_local_planner::TebLocalPlannerROS::estimateLocalGoalOrientation	(	const std::vector< geometry_msgs::PoseStamped > &	global_plan,
		const tf::Stamped< tf::Pose > &	local_goal,
		int	current_goal_idx,
		const tf::StampedTransform &	tf_plan_to_global,
		int	moving_average_length = `3`
	)		const `[protected]`

Estimate the orientation of a pose from the global_plan that is treated as a local goal for the local planner.

If the current (local) goal point is not the final one (global) substitute the goal orientation by the angle of the direction vector between the local goal and the subsequent pose of the global plan. This is often helpful, if the global planner does not consider orientations.
A moving average filter is utilized to smooth the orientation.

Parameters:

	global_plan	The global plan
	local_goal	Current local goal
	current_goal_idx	Index of the current (local) goal pose in the global plan
[out]	tf_plan_to_global	Transformation between the global plan and the global planning frame
	moving_average_length	number of future poses of the global plan to be taken into account

Returns:: orientation (yaw-angle) estimate

Definition at line 724 of file teb_local_planner_ros.cpp.

double teb_local_planner::TebLocalPlannerROS::getNumberFromXMLRPC	(	XmlRpc::XmlRpcValue &	value,
		const std::string &	full_param_name
	)		`[static]`

Get a number from the given XmlRpcValue.

Remarks:: This method is copied from costmap_2d/footprint.h, since it is not declared public in all ros distros; It is modified in order to return a container of Eigen::Vector2d instead of geometry_msgs::Point

Parameters:

value	double value type
full_param_name	this is the full name of the rosparam from which the footprint_xmlrpc value came. It is used only for reporting errors.

Returns:: double value

Definition at line 1023 of file teb_local_planner_ros.cpp.

RobotFootprintModelPtr teb_local_planner::TebLocalPlannerROS::getRobotFootprintFromParamServer ( const ros::NodeHandle & nh ) [static]

Get the current robot footprint/contour model.

Parameters:

nh	const reference to the local ros::NodeHandle

Returns:: Robot footprint model used for optimization

Definition at line 864 of file teb_local_planner_ros.cpp.

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

Initializes the teb plugin.

Parameters:

name	The name of the instance
tf	Pointer to a transform listener
costmap_ros	Cost map representing occupied and free space

Implements nav_core::BaseLocalPlanner.

Definition at line 81 of file teb_local_planner_ros.cpp.

bool teb_local_planner::TebLocalPlannerROS::isGoalReached ( ) [virtual]

Check if the goal pose has been achieved.

The actual check is performed in computeVelocityCommands(). Only the status flag is checked here.

Returns:: True if achieved, false otherwise

Implements nav_core::BaseLocalPlanner.

Definition at line 384 of file teb_local_planner_ros.cpp.

Point2dContainer teb_local_planner::TebLocalPlannerROS::makeFootprintFromXMLRPC	(	XmlRpc::XmlRpcValue &	footprint_xmlrpc,
		const std::string &	full_param_name
	)		`[static]`

Set the footprint from the given XmlRpcValue.

Remarks:: This method is copied from costmap_2d/footprint.h, since it is not declared public in all ros distros; It is modified in order to return a container of Eigen::Vector2d instead of geometry_msgs::Point

Parameters:

footprint_xmlrpc	should be an array of arrays, where the top-level array should have 3 or more elements, and the sub-arrays should all have exactly 2 elements (x and y coordinates).
full_param_name	this is the full name of the rosparam from which the footprint_xmlrpc value came. It is used only for reporting errors.

Returns:: container of vertices describing the polygon

Definition at line 986 of file teb_local_planner_ros.cpp.

bool teb_local_planner::TebLocalPlannerROS::pruneGlobalPlan	(	const tf::TransformListener &	tf,
		const tf::Stamped< tf::Pose > &	global_pose,
		std::vector< geometry_msgs::PoseStamped > &	global_plan,
		double	dist_behind_robot = `1`
	)		`[protected]`

Prune global plan such that already passed poses are cut off.

The pose of the robot is transformed into the frame of the global plan by taking the most recent tf transform. If no valid transformation can be found, the method returns false. The global plan is pruned until the distance to the robot is at least dist_behind_robot. If no pose within the specified treshold dist_behind_robot can be found, nothing will be pruned and the method returns false.

Remarks:: Do not choose dist_behind_robot too small (not smaller the cellsize of the map), otherwise nothing will be pruned.

Parameters:

	tf	A reference to a transform listener
	global_pose	The global pose of the robot
[in,out]	global_plan	The plan to be transformed
	dist_behind_robot	Distance behind the robot that should be kept [meters]

Returns:: true if the plan is pruned, false in case of a transform exception or if no pose cannot be found inside the threshold

Definition at line 544 of file teb_local_planner_ros.cpp.

void teb_local_planner::TebLocalPlannerROS::reconfigureCB	(	TebLocalPlannerReconfigureConfig &	config,
		uint32_t	level
	)		`[protected]`

Callback for the dynamic_reconfigure node.

This callback allows to modify parameters dynamically at runtime without restarting the node

Parameters:

config	Reference to the dynamic reconfigure config
level	Dynamic reconfigure level

Definition at line 76 of file teb_local_planner_ros.cpp.

void teb_local_planner::TebLocalPlannerROS::saturateVelocity	(	double &	vx,
		double &	vy,
		double &	omega,
		double	max_vel_x,
		double	max_vel_y,
		double	max_vel_theta,
		double	max_vel_x_backwards
	)		const `[protected]`

Saturate the translational and angular velocity to given limits.

The limit of the translational velocity for backwards driving can be changed independently. Do not choose max_vel_x_backwards <= 0. If no backward driving is desired, change the optimization weight for penalizing backwards driving instead.

Parameters:

[in,out]	vx	The translational velocity that should be saturated.
[in,out]	vy	Strafing velocity which can be nonzero for holonomic robots
[in,out]	omega	The angular velocity that should be saturated.
	max_vel_x	Maximum translational velocity for forward driving
	max_vel_y	Maximum strafing velocity (for holonomic robots)
	max_vel_theta	Maximum (absolute) angular velocity
	max_vel_x_backwards	Maximum translational velocity for backwards driving

Definition at line 770 of file teb_local_planner_ros.cpp.

bool teb_local_planner::TebLocalPlannerROS::setPlan ( const std::vector< geometry_msgs::PoseStamped > & orig_global_plan ) [virtual]

Set the plan that the teb local planner is following.

Parameters:

orig_global_plan The plan to pass to the local planner

Returns:: True if the plan was updated successfully, false otherwise

Implements nav_core::BaseLocalPlanner.

Definition at line 181 of file teb_local_planner_ros.cpp.

Eigen::Vector2d teb_local_planner::TebLocalPlannerROS::tfPoseToEigenVector2dTransRot ( const tf::Pose & tf_vel ) [static]

Transform a tf::Pose type into a Eigen::Vector2d containing the translational and angular velocities.

Translational velocities (x- and y-coordinates) are combined into a single translational velocity (first component).

Parameters:

tf_vel tf::Pose message containing a 1D or 2D translational velocity (x,y) and an angular velocity (yaw-angle)

Returns:: Translational and angular velocity combined into an Eigen::Vector2d

Definition at line 535 of file teb_local_planner_ros.cpp.

bool teb_local_planner::TebLocalPlannerROS::transformGlobalPlan	(	const tf::TransformListener &	tf,
		const std::vector< geometry_msgs::PoseStamped > &	global_plan,
		const tf::Stamped< tf::Pose > &	global_pose,
		const costmap_2d::Costmap2D &	costmap,
		const std::string &	global_frame,
		double	max_plan_length,
		std::vector< geometry_msgs::PoseStamped > &	transformed_plan,
		int *	current_goal_idx = `NULL`,
		tf::StampedTransform *	tf_plan_to_global = `NULL`
	)		const `[protected]`

Transforms the global plan of the robot from the planner frame to the local frame (modified).

The method replaces transformGlobalPlan as defined in base_local_planner/goal_functions.h such that the index of the current goal pose is returned as well as the transformation between the global plan and the planning frame.

Parameters:

	tf	A reference to a transform listener
	global_plan	The plan to be transformed
	global_pose	The global pose of the robot
	costmap	A reference to the costmap being used so the window size for transforming can be computed
	global_frame	The frame to transform the plan to
	max_plan_length	Specify maximum length (cumulative Euclidean distances) of the transformed plan [if <=0: disabled; the length is also bounded by the local costmap size!]
[out]	transformed_plan	Populated with the transformed plan
[out]	current_goal_idx	Index of the current (local) goal pose in the global plan
[out]	tf_plan_to_global	Transformation between the global plan and the global planning frame

Returns:: true if the global plan is transformed, false otherwise

Definition at line 589 of file teb_local_planner_ros.cpp.

void teb_local_planner::TebLocalPlannerROS::updateObstacleContainerWithCostmap ( ) [protected]

Update internal obstacle vector based on occupied costmap cells.

Remarks:: All occupied cells will be added as point obstacles.; All previous obstacles are cleared.

See also:: updateObstacleContainerWithCostmapConverter

Todo:

Include temporal coherence among obstacle msgs (id vector)

Include properties for dynamic obstacles (e.g. using constant velocity model)

Definition at line 397 of file teb_local_planner_ros.cpp.

void teb_local_planner::TebLocalPlannerROS::updateObstacleContainerWithCostmapConverter ( ) [protected]

Update internal obstacle vector based on polygons provided by a costmap_converter plugin.

Remarks:: Requires a loaded costmap_converter plugin.; All previous obstacles are cleared.

See also:: updateObstacleContainerWithCostmap

Definition at line 425 of file teb_local_planner_ros.cpp.

void teb_local_planner::TebLocalPlannerROS::updateObstacleContainerWithCustomObstacles ( ) [protected]

Update internal obstacle vector based on custom messages received via subscriber.

Remarks:: All previous obstacles are NOT cleared. Call this method after other update methods.

See also:: updateObstacleContainerWithCostmap, updateObstacleContainerWithCostmapConverter

Definition at line 460 of file teb_local_planner_ros.cpp.

void teb_local_planner::TebLocalPlannerROS::updateViaPointsContainer	(	const std::vector< geometry_msgs::PoseStamped > &	transformed_plan,
		double	min_separation
	)		`[protected]`

Update internal via-point container based on the current reference plan.

Remarks:: All previous via-points will be cleared.

Parameters:

transformed_plan	(local) portion of the global plan (which is already transformed to the planning frame)
min_separation	minimum separation between two consecutive via-points

Definition at line 514 of file teb_local_planner_ros.cpp.

void teb_local_planner::TebLocalPlannerROS::validateFootprints	(	double	opt_inscribed_radius,
		double	costmap_inscribed_radius,
		double	min_obst_dist
	)		`[protected]`

Validate current parameter values of the footprint for optimization, obstacle distance and the costmap footprint.

This method prints warnings if validation fails.

Remarks:: Currently, we only validate the inscribed radius of the footprints

Parameters:

opt_inscribed_radius	Inscribed radius of the RobotFootprintModel for optimization
costmap_inscribed_radius	Inscribed radius of the footprint model used for the costmap
min_obst_dist	desired distance to obstacles

Definition at line 812 of file teb_local_planner_ros.cpp.