Implements both nav_core::BaseLocalPlanner and mbf_costmap_core::CostmapController abstract interfaces, so the teb_local_planner plugin can be used both in move_base and move_base_flex (MBF). More...

#include <teb_local_planner_ros.h>

Inheritance diagram for teb_local_planner::TebLocalPlannerROS:

[legend]

Public Member Functions
bool	cancel ()
	Requests the planner to cancel, e.g. if it takes too much time. More...

uint32_t	computeVelocityCommands (const geometry_msgs::PoseStamped &pose, const geometry_msgs::TwistStamped &velocity, geometry_msgs::TwistStamped &cmd_vel, std::string &message)
	Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base. More...

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. More...

void	initialize (std::string name, tf2_ros::Buffer tf, costmap_2d::Costmap2DROS costmap_ros)
	Initializes the teb plugin. More...

bool	isGoalReached ()
	Check if the goal pose has been achieved. More...

bool	isGoalReached (double xy_tolerance, double yaw_tolerance)
	Dummy version to satisfy MBF API. More...

bool	setPlan (const std::vector< geometry_msgs::PoseStamped > &orig_global_plan)
	Set the plan that the teb local planner is following. More...

	TebLocalPlannerROS ()
	Default constructor of the teb plugin. More...

	~TebLocalPlannerROS ()
	Destructor of the plugin. More...

Public Member Functions inherited from nav_core::BaseLocalPlanner
virtual	~BaseLocalPlanner ()

Public Member Functions inherited from mbf_costmap_core::CostmapController
virtual void	initialize (std::string name, ::TF tf, costmap_2d::Costmap2DROS costmap_ros)=0

virtual	~CostmapController ()

Public Member Functions inherited from mbf_abstract_core::AbstractController
virtual	~AbstractController ()

Public utility functions/methods
costmap_2d::Costmap2DROS *	costmap_ros_
	Pointer to the costmap ros wrapper, received from the navigation stack. More...

costmap_2d::Costmap2D *	costmap_
	Pointer to the 2d costmap (obtained from the costmap ros wrapper) More...

tf2_ros::Buffer *	tf_
	pointer to tf buffer More...

PlannerInterfacePtr	planner_
	Instance of the underlying optimal planner class. More...

ObstContainer	obstacles_
	Obstacle vector that should be considered during local trajectory optimization. More...

ViaPointContainer	via_points_
	Container of via-points that should be considered during local trajectory optimization. More...

TebVisualizationPtr	visualization_
	Instance of the visualization class (local/global plan, obstacles, ...) More...

boost::shared_ptr< base_local_planner::CostmapModel >	costmap_model_

TebConfig	cfg_
	Config class that stores and manages all related parameters. More...

FailureDetector	failure_detector_
	Detect if the robot got stucked. More...

std::vector< geometry_msgs::PoseStamped >	global_plan_
	Store the current global plan. More...

base_local_planner::OdometryHelperRos	odom_helper_
	Provides an interface to receive the current velocity from the robot. More...

pluginlib::ClassLoader< costmap_converter::BaseCostmapToPolygons >	costmap_converter_loader_
	Load costmap converter plugins at runtime. More...

boost::shared_ptr< costmap_converter::BaseCostmapToPolygons >	costmap_converter_
	Store the current costmap_converter More...

boost::shared_ptr< dynamic_reconfigure::Server< TebLocalPlannerReconfigureConfig > >	dynamic_recfg_
	Dynamic reconfigure server to allow config modifications at runtime. More...

ros::Subscriber	custom_obst_sub_
	Subscriber for custom obstacles received via a ObstacleMsg. More...

boost::mutex	custom_obst_mutex_
	Mutex that locks the obstacle array (multi-threaded) More...

costmap_converter::ObstacleArrayMsg	custom_obstacle_msg_
	Copy of the most recent obstacle message. More...

ros::Subscriber	via_points_sub_
	Subscriber for custom via-points received via a Path msg. More...

bool	custom_via_points_active_
	Keep track whether valid via-points have been received from via_points_sub_. More...

boost::mutex	via_point_mutex_
	Mutex that locks the via_points container (multi-threaded) More...

PoseSE2	robot_pose_
	Store current robot pose. More...

PoseSE2	robot_goal_
	Store current robot goal. More...

geometry_msgs::Twist	robot_vel_
	Store current robot translational and angular velocity (vx, vy, omega) More...

bool	goal_reached_
	store whether the goal is reached or not More...

ros::Time	time_last_infeasible_plan_
	Store at which time stamp the last infeasible plan was detected. More...

int	no_infeasible_plans_
	Store how many times in a row the planner failed to find a feasible plan. More...

ros::Time	time_last_oscillation_
	Store at which time stamp the last oscillation was detected. More...

RotType	last_preferred_rotdir_
	Store recent preferred turning direction. More...

geometry_msgs::Twist	last_cmd_
	Store the last control command generated in computeVelocityCommands() More...

std::vector< geometry_msgs::Point >	footprint_spec_
	Store the footprint of the robot. More...

double	robot_inscribed_radius_
	The radius of the inscribed circle of the robot (collision possible) More...

double	robot_circumscribed_radius
	The radius of the circumscribed circle of the robot. More...

std::string	global_frame_
	The frame in which the controller will run. More...

std::string	robot_base_frame_
	Used as the base frame id of the robot. More...

std::string	name_
	For use with the ros nodehandle. More...

bool	initialized_
	Keeps track about the correct initialization of this class. More...

static Eigen::Vector2d	tfPoseToEigenVector2dTransRot (const tf::Pose &tf_vel)
	Transform a tf::Pose type into a Eigen::Vector2d containing the translational and angular velocities. More...

static RobotFootprintModelPtr	getRobotFootprintFromParamServer (const ros::NodeHandle &nh, const TebConfig &config)
	Get the current robot footprint/contour model. More...

static Point2dContainer	makeFootprintFromXMLRPC (XmlRpc::XmlRpcValue &footprint_xmlrpc, const std::string &full_param_name)
	Set the footprint from the given XmlRpcValue. More...

static double	getNumberFromXMLRPC (XmlRpc::XmlRpcValue &value, const std::string &full_param_name)
	Get a number from the given XmlRpcValue. More...

void	updateObstacleContainerWithCostmap ()
	Update internal obstacle vector based on occupied costmap cells. More...

void	updateObstacleContainerWithCostmapConverter ()
	Update internal obstacle vector based on polygons provided by a costmap_converter plugin. More...

void	updateObstacleContainerWithCustomObstacles ()
	Update internal obstacle vector based on custom messages received via subscriber. More...

void	updateViaPointsContainer (const std::vector< geometry_msgs::PoseStamped > &transformed_plan, double min_separation)
	Update internal via-point container based on the current reference plan. More...

void	reconfigureCB (TebLocalPlannerReconfigureConfig &config, uint32_t level)
	Callback for the dynamic_reconfigure node. More...

void	customObstacleCB (const costmap_converter::ObstacleArrayMsg::ConstPtr &obst_msg)
	Callback for custom obstacles that are not obtained from the costmap. More...

void	customViaPointsCB (const nav_msgs::Path::ConstPtr &via_points_msg)
	Callback for custom via-points. More...

bool	pruneGlobalPlan (const tf2_ros::Buffer &tf, const geometry_msgs::PoseStamped &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. More...

bool	transformGlobalPlan (const tf2_ros::Buffer &tf, const std::vector< geometry_msgs::PoseStamped > &global_plan, const geometry_msgs::PoseStamped &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, geometry_msgs::TransformStamped tf_plan_to_global=NULL) const
	Transforms the global plan of the robot from the planner frame to the local frame (modified). More...

double	estimateLocalGoalOrientation (const std::vector< geometry_msgs::PoseStamped > &global_plan, const geometry_msgs::PoseStamped &local_goal, int current_goal_idx, const geometry_msgs::TransformStamped &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. More...

void	saturateVelocity (double &vx, double &vy, double &omega, double max_vel_x, double max_vel_y, double max_vel_trans, double max_vel_theta, double max_vel_x_backwards) const
	Saturate the translational and angular velocity to given limits. More...

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. More...

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. More...

void	configureBackupModes (std::vector< geometry_msgs::PoseStamped > &transformed_plan, int &goal_idx)

Additional Inherited Members
Public Types inherited from mbf_costmap_core::CostmapController
typedef boost::shared_ptr< ::mbf_costmap_core::CostmapController >	Ptr

Public Types inherited from mbf_abstract_core::AbstractController
typedef boost::shared_ptr< ::mbf_abstract_core::AbstractController >	Ptr

Protected Member Functions inherited from nav_core::BaseLocalPlanner
	BaseLocalPlanner ()

Protected Member Functions inherited from mbf_costmap_core::CostmapController
	CostmapController ()

Protected Member Functions inherited from mbf_abstract_core::AbstractController
	AbstractController ()

Detailed Description

Implements both nav_core::BaseLocalPlanner and mbf_costmap_core::CostmapController abstract interfaces, so the teb_local_planner plugin can be used both in move_base and move_base_flex (MBF).

Todo:: Escape behavior, more efficient obstacle handling

Definition at line 129 of file teb_local_planner_ros.h.

Constructor & Destructor Documentation

◆ TebLocalPlannerROS()

teb_local_planner::TebLocalPlannerROS::TebLocalPlannerROS ( )

Default constructor of the teb plugin.

Definition at line 105 of file teb_local_planner_ros.cpp.

◆ ~TebLocalPlannerROS()

teb_local_planner::TebLocalPlannerROS::~TebLocalPlannerROS ( )

Destructor of the plugin.

Definition at line 113 of file teb_local_planner_ros.cpp.

Member Function Documentation

◆ cancel()

bool teb_local_planner::TebLocalPlannerROS::cancel ( )

inlinevirtual

Requests the planner to cancel, e.g. if it takes too much time.

Remarks: New on MBF API

Returns: True if a cancel has been successfully requested, false if not implemented.

Implements mbf_costmap_core::CostmapController.

Definition at line 214 of file teb_local_planner_ros.h.

◆ computeVelocityCommands() [1/2]

uint32_t teb_local_planner::TebLocalPlannerROS::computeVelocityCommands	(	const geometry_msgs::PoseStamped &	pose,
		const geometry_msgs::TwistStamped &	velocity,
		geometry_msgs::TwistStamped &	cmd_vel,
		std::string &	message
	)

virtual

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

Remarks: Extended version for MBF API

Parameters

pose	the current pose of the robot.
velocity	the current velocity of the robot.
cmd_vel	Will be filled with the velocity command to be passed to the robot base.
message	Optional more detailed outcome as a string

Returns: Result code as described on ExePath action result: SUCCESS = 0 1..9 are reserved as plugin specific non-error results FAILURE = 100 Unspecified failure, only used for old, non-mfb_core based plugins CANCELED = 101 NO_VALID_CMD = 102 PAT_EXCEEDED = 103 COLLISION = 104 OSCILLATION = 105 ROBOT_STUCK = 106 MISSED_GOAL = 107 MISSED_PATH = 108 BLOCKED_PATH = 109 INVALID_PATH = 110 TF_ERROR = 111 NOT_INITIALIZED = 112 INVALID_PLUGIN = 113 INTERNAL_ERROR = 114 121..149 are reserved as plugin specific errors

Implements mbf_costmap_core::CostmapController.

Definition at line 272 of file teb_local_planner_ros.cpp.

◆ computeVelocityCommands() [2/2]

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 262 of file teb_local_planner_ros.cpp.

◆ configureBackupModes()

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

protected

Definition at line 982 of file teb_local_planner_ros.cpp.

◆ convertTransRotVelToSteeringAngle()

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 958 of file teb_local_planner_ros.cpp.

◆ customObstacleCB()

void teb_local_planner::TebLocalPlannerROS::customObstacleCB ( const costmap_converter::ObstacleArrayMsg::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 1056 of file teb_local_planner_ros.cpp.

◆ customViaPointsCB()

void teb_local_planner::TebLocalPlannerROS::customViaPointsCB ( const nav_msgs::Path::ConstPtr & via_points_msg )

protected

Callback for custom via-points.

Parameters

via_points_msg pointer to the message containing a list of via-points

Definition at line 1062 of file teb_local_planner_ros.cpp.

◆ estimateLocalGoalOrientation()

double teb_local_planner::TebLocalPlannerROS::estimateLocalGoalOrientation	(	const std::vector< geometry_msgs::PoseStamped > &	global_plan,
		const geometry_msgs::PoseStamped &	local_goal,
		int	current_goal_idx,
		const geometry_msgs::TransformStamped &	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 863 of file teb_local_planner_ros.cpp.

◆ getNumberFromXMLRPC()

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 1241 of file teb_local_planner_ros.cpp.

◆ getRobotFootprintFromParamServer()

RobotFootprintModelPtr teb_local_planner::TebLocalPlannerROS::getRobotFootprintFromParamServer	(	const ros::NodeHandle &	nh,
		const TebConfig &	config
	)

static

Get the current robot footprint/contour model.

Parameters

nh	const reference to the local ros::NodeHandle
config	const reference to the current configuration

Returns: Robot footprint model used for optimization

Definition at line 1082 of file teb_local_planner_ros.cpp.

◆ initialize()

void teb_local_planner::TebLocalPlannerROS::initialize	(	std::string	name,
		tf2_ros::Buffer *	tf,
		costmap_2d::Costmap2DROS *	costmap_ros
	)

virtual

Initializes the teb plugin.

Parameters

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

Implements nav_core::BaseLocalPlanner.

Definition at line 129 of file teb_local_planner_ros.cpp.

◆ isGoalReached() [1/2]

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 501 of file teb_local_planner_ros.cpp.

◆ isGoalReached() [2/2]

bool teb_local_planner::TebLocalPlannerROS::isGoalReached	(	double	xy_tolerance,
		double	yaw_tolerance
	)

inlinevirtual

Dummy version to satisfy MBF API.

Implements mbf_costmap_core::CostmapController.

Definition at line 207 of file teb_local_planner_ros.h.

◆ makeFootprintFromXMLRPC()

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 1204 of file teb_local_planner_ros.cpp.

◆ pruneGlobalPlan()

bool teb_local_planner::TebLocalPlannerROS::pruneGlobalPlan	(	const tf2_ros::Buffer &	tf,
		const geometry_msgs::PoseStamped &	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 tf buffer
	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 693 of file teb_local_planner_ros.cpp.

◆ reconfigureCB()

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 117 of file teb_local_planner_ros.cpp.

◆ saturateVelocity()

void teb_local_planner::TebLocalPlannerROS::saturateVelocity	(	double &	vx,
		double &	vy,
		double &	omega,
		double	max_vel_x,
		double	max_vel_y,
		double	max_vel_trans,
		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_trans	Maximum translational velocity for holonomic robots
	max_vel_theta	Maximum (absolute) angular velocity
	max_vel_x_backwards	Maximum translational velocity for backwards driving

Definition at line 910 of file teb_local_planner_ros.cpp.

◆ setPlan()

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 mbf_costmap_core::CostmapController.

Definition at line 239 of file teb_local_planner_ros.cpp.

◆ tfPoseToEigenVector2dTransRot()

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 684 of file teb_local_planner_ros.cpp.

◆ transformGlobalPlan()

bool teb_local_planner::TebLocalPlannerROS::transformGlobalPlan	(	const tf2_ros::Buffer &	tf,
		const std::vector< geometry_msgs::PoseStamped > &	global_plan,
		const geometry_msgs::PoseStamped &	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`,
		geometry_msgs::TransformStamped *	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 tf buffer
	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 737 of file teb_local_planner_ros.cpp.

◆ updateObstacleContainerWithCostmap()

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 514 of file teb_local_planner_ros.cpp.

◆ updateObstacleContainerWithCostmapConverter()

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 542 of file teb_local_planner_ros.cpp.

◆ updateObstacleContainerWithCustomObstacles()

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 588 of file teb_local_planner_ros.cpp.

◆ updateViaPointsContainer()

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 663 of file teb_local_planner_ros.cpp.

◆ validateFootprints()

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 972 of file teb_local_planner_ros.cpp.

Member Data Documentation

◆ cfg_

TebConfig teb_local_planner::TebLocalPlannerROS::cfg_

private

Config class that stores and manages all related parameters.

Definition at line 445 of file teb_local_planner_ros.h.

◆ costmap_

costmap_2d::Costmap2D* teb_local_planner::TebLocalPlannerROS::costmap_

private

Pointer to the 2d costmap (obtained from the costmap ros wrapper)

Definition at line 436 of file teb_local_planner_ros.h.

◆ costmap_converter_

boost::shared_ptr<costmap_converter::BaseCostmapToPolygons> teb_local_planner::TebLocalPlannerROS::costmap_converter_

private

Store the current costmap_converter

Definition at line 453 of file teb_local_planner_ros.h.

◆ costmap_converter_loader_

pluginlib::ClassLoader<costmap_converter::BaseCostmapToPolygons> teb_local_planner::TebLocalPlannerROS::costmap_converter_loader_

private

Load costmap converter plugins at runtime.

Definition at line 452 of file teb_local_planner_ros.h.

◆ costmap_model_

boost::shared_ptr<base_local_planner::CostmapModel> teb_local_planner::TebLocalPlannerROS::costmap_model_

private

Definition at line 444 of file teb_local_planner_ros.h.

◆ costmap_ros_

costmap_2d::Costmap2DROS* teb_local_planner::TebLocalPlannerROS::costmap_ros_

private

Pointer to the costmap ros wrapper, received from the navigation stack.

Definition at line 435 of file teb_local_planner_ros.h.

◆ custom_obst_mutex_

boost::mutex teb_local_planner::TebLocalPlannerROS::custom_obst_mutex_

private

Mutex that locks the obstacle array (multi-threaded)

Definition at line 457 of file teb_local_planner_ros.h.

◆ custom_obst_sub_

ros::Subscriber teb_local_planner::TebLocalPlannerROS::custom_obst_sub_

private

Subscriber for custom obstacles received via a ObstacleMsg.

Definition at line 456 of file teb_local_planner_ros.h.

◆ custom_obstacle_msg_

costmap_converter::ObstacleArrayMsg teb_local_planner::TebLocalPlannerROS::custom_obstacle_msg_

private

Copy of the most recent obstacle message.

Definition at line 458 of file teb_local_planner_ros.h.

◆ custom_via_points_active_

bool teb_local_planner::TebLocalPlannerROS::custom_via_points_active_

private

Keep track whether valid via-points have been received from via_points_sub_.

Definition at line 461 of file teb_local_planner_ros.h.

◆ dynamic_recfg_

boost::shared_ptr< dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig> > teb_local_planner::TebLocalPlannerROS::dynamic_recfg_

private

Dynamic reconfigure server to allow config modifications at runtime.

Definition at line 455 of file teb_local_planner_ros.h.

◆ failure_detector_

FailureDetector teb_local_planner::TebLocalPlannerROS::failure_detector_

private

Detect if the robot got stucked.

Definition at line 446 of file teb_local_planner_ros.h.

◆ footprint_spec_

std::vector<geometry_msgs::Point> teb_local_planner::TebLocalPlannerROS::footprint_spec_

private

Store the footprint of the robot.

Definition at line 474 of file teb_local_planner_ros.h.

◆ global_frame_

std::string teb_local_planner::TebLocalPlannerROS::global_frame_

private

The frame in which the controller will run.

Definition at line 478 of file teb_local_planner_ros.h.

◆ global_plan_

std::vector<geometry_msgs::PoseStamped> teb_local_planner::TebLocalPlannerROS::global_plan_

private

Store the current global plan.

Definition at line 448 of file teb_local_planner_ros.h.

◆ goal_reached_

bool teb_local_planner::TebLocalPlannerROS::goal_reached_

private

store whether the goal is reached or not

Definition at line 467 of file teb_local_planner_ros.h.

◆ initialized_

bool teb_local_planner::TebLocalPlannerROS::initialized_

private

Keeps track about the correct initialization of this class.

Definition at line 483 of file teb_local_planner_ros.h.

◆ last_cmd_

geometry_msgs::Twist teb_local_planner::TebLocalPlannerROS::last_cmd_

private

Store the last control command generated in computeVelocityCommands()

Definition at line 472 of file teb_local_planner_ros.h.

◆ last_preferred_rotdir_

RotType teb_local_planner::TebLocalPlannerROS::last_preferred_rotdir_

private

Store recent preferred turning direction.

Definition at line 471 of file teb_local_planner_ros.h.

◆ name_

std::string teb_local_planner::TebLocalPlannerROS::name_

private

For use with the ros nodehandle.

Definition at line 480 of file teb_local_planner_ros.h.

◆ no_infeasible_plans_

int teb_local_planner::TebLocalPlannerROS::no_infeasible_plans_

private

Store how many times in a row the planner failed to find a feasible plan.

Definition at line 469 of file teb_local_planner_ros.h.

◆ obstacles_

ObstContainer teb_local_planner::TebLocalPlannerROS::obstacles_

private

Obstacle vector that should be considered during local trajectory optimization.

Definition at line 441 of file teb_local_planner_ros.h.

◆ odom_helper_

base_local_planner::OdometryHelperRos teb_local_planner::TebLocalPlannerROS::odom_helper_

private

Provides an interface to receive the current velocity from the robot.

Definition at line 450 of file teb_local_planner_ros.h.

◆ planner_

PlannerInterfacePtr teb_local_planner::TebLocalPlannerROS::planner_

private

Instance of the underlying optimal planner class.

Definition at line 440 of file teb_local_planner_ros.h.

◆ robot_base_frame_

std::string teb_local_planner::TebLocalPlannerROS::robot_base_frame_

private

Used as the base frame id of the robot.

Definition at line 479 of file teb_local_planner_ros.h.

◆ robot_circumscribed_radius

double teb_local_planner::TebLocalPlannerROS::robot_circumscribed_radius

private

The radius of the circumscribed circle of the robot.

Definition at line 476 of file teb_local_planner_ros.h.

◆ robot_goal_

PoseSE2 teb_local_planner::TebLocalPlannerROS::robot_goal_

private

Store current robot goal.

Definition at line 465 of file teb_local_planner_ros.h.

◆ robot_inscribed_radius_

double teb_local_planner::TebLocalPlannerROS::robot_inscribed_radius_

private

The radius of the inscribed circle of the robot (collision possible)

Definition at line 475 of file teb_local_planner_ros.h.

◆ robot_pose_

PoseSE2 teb_local_planner::TebLocalPlannerROS::robot_pose_

private

Store current robot pose.

Definition at line 464 of file teb_local_planner_ros.h.

◆ robot_vel_

geometry_msgs::Twist teb_local_planner::TebLocalPlannerROS::robot_vel_

private

Store current robot translational and angular velocity (vx, vy, omega)

Definition at line 466 of file teb_local_planner_ros.h.

◆ tf_

tf2_ros::Buffer* teb_local_planner::TebLocalPlannerROS::tf_

private

pointer to tf buffer

Definition at line 437 of file teb_local_planner_ros.h.

◆ time_last_infeasible_plan_

ros::Time teb_local_planner::TebLocalPlannerROS::time_last_infeasible_plan_

private

Store at which time stamp the last infeasible plan was detected.

Definition at line 468 of file teb_local_planner_ros.h.

◆ time_last_oscillation_

ros::Time teb_local_planner::TebLocalPlannerROS::time_last_oscillation_

private

Store at which time stamp the last oscillation was detected.

Definition at line 470 of file teb_local_planner_ros.h.

◆ via_point_mutex_

boost::mutex teb_local_planner::TebLocalPlannerROS::via_point_mutex_

private

Mutex that locks the via_points container (multi-threaded)

Definition at line 462 of file teb_local_planner_ros.h.

◆ via_points_

ViaPointContainer teb_local_planner::TebLocalPlannerROS::via_points_

private

Container of via-points that should be considered during local trajectory optimization.

Definition at line 442 of file teb_local_planner_ros.h.

◆ via_points_sub_

ros::Subscriber teb_local_planner::TebLocalPlannerROS::via_points_sub_

private

Subscriber for custom via-points received via a Path msg.

Definition at line 460 of file teb_local_planner_ros.h.

◆ visualization_

TebVisualizationPtr teb_local_planner::TebLocalPlannerROS::visualization_

private

Instance of the visualization class (local/global plan, obstacles, ...)

Definition at line 443 of file teb_local_planner_ros.h.

The documentation for this class was generated from the following files:

Public Member Functions

Public utility functions/methods

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ TebLocalPlannerROS()

◆ ~TebLocalPlannerROS()

Member Function Documentation

◆ cancel()

◆ computeVelocityCommands() [1/2]

◆ computeVelocityCommands() [2/2]

◆ configureBackupModes()

◆ convertTransRotVelToSteeringAngle()

◆ customObstacleCB()

◆ customViaPointsCB()

◆ estimateLocalGoalOrientation()

◆ getNumberFromXMLRPC()

◆ getRobotFootprintFromParamServer()

◆ initialize()

◆ isGoalReached() [1/2]

◆ isGoalReached() [2/2]

◆ makeFootprintFromXMLRPC()

◆ pruneGlobalPlan()

◆ reconfigureCB()

◆ saturateVelocity()

◆ setPlan()

◆ tfPoseToEigenVector2dTransRot()

◆ transformGlobalPlan()

◆ updateObstacleContainerWithCostmap()

◆ updateObstacleContainerWithCostmapConverter()

◆ updateObstacleContainerWithCustomObstacles()

◆ updateViaPointsContainer()

◆ validateFootprints()

Member Data Documentation

◆ cfg_

◆ costmap_

◆ costmap_converter_

◆ costmap_converter_loader_

◆ costmap_model_

◆ costmap_ros_

◆ custom_obst_mutex_

◆ custom_obst_sub_

◆ custom_obstacle_msg_

◆ custom_via_points_active_

◆ dynamic_recfg_

◆ failure_detector_

◆ footprint_spec_

◆ global_frame_

◆ global_plan_

◆ goal_reached_

◆ initialized_

◆ last_cmd_

◆ last_preferred_rotdir_

◆ name_

◆ no_infeasible_plans_

◆ obstacles_

◆ odom_helper_

◆ planner_

◆ robot_base_frame_

◆ robot_circumscribed_radius

◆ robot_goal_

◆ robot_inscribed_radius_

◆ robot_pose_

◆ robot_vel_

◆ tf_

◆ time_last_infeasible_plan_

◆ time_last_oscillation_

◆ via_point_mutex_

◆ via_points_

◆ via_points_sub_

◆ visualization_