Public Member Functions | Protected Member Functions | Private Attributes
teb_local_planner::TebLocalPlannerROS Class Reference

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:
Inheritance graph
[legend]

List of all members.

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::Costmap2Dcostmap_
 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::Costmap2DROScostmap_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::Pointfootprint_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::TransformListenertf_
 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

Default constructor of the teb plugin.

Definition at line 64 of file teb_local_planner_ros.cpp.

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_velWill 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 $ R = v/omega $
  • 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:
    vtranslational velocity [m/s]
    omegarotational velocity [rad/s]
    wheelbasedistance between both axles (drive shaft and steering axle), the value might be negative for back_wheeled robots
    min_turning_radiusSpecify 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_msgpointer 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_planThe global plan
local_goalCurrent local goal
current_goal_idxIndex of the current (local) goal pose in the global plan
[out]tf_plan_to_globalTransformation between the global plan and the global planning frame
moving_average_lengthnumber 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:
valuedouble value type
full_param_namethis 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.

Get the current robot footprint/contour model.

Parameters:
nhconst 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:
nameThe name of the instance
tfPointer to a transform listener
costmap_rosCost map representing occupied and free space

Implements nav_core::BaseLocalPlanner.

Definition at line 81 of file teb_local_planner_ros.cpp.

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_xmlrpcshould 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_namethis 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:
tfA reference to a transform listener
global_poseThe global pose of the robot
[in,out]global_planThe plan to be transformed
dist_behind_robotDistance 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:
configReference to the dynamic reconfigure config
levelDynamic 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]vxThe translational velocity that should be saturated.
[in,out]vyStrafing velocity which can be nonzero for holonomic robots
[in,out]omegaThe angular velocity that should be saturated.
max_vel_xMaximum translational velocity for forward driving
max_vel_yMaximum strafing velocity (for holonomic robots)
max_vel_thetaMaximum (absolute) angular velocity
max_vel_x_backwardsMaximum 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_planThe 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.

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_veltf::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:
tfA reference to a transform listener
global_planThe plan to be transformed
global_poseThe global pose of the robot
costmapA reference to the costmap being used so the window size for transforming can be computed
global_frameThe frame to transform the plan to
max_plan_lengthSpecify maximum length (cumulative Euclidean distances) of the transformed plan [if <=0: disabled; the length is also bounded by the local costmap size!]
[out]transformed_planPopulated with the transformed plan
[out]current_goal_idxIndex of the current (local) goal pose in the global plan
[out]tf_plan_to_globalTransformation 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.

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.

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.

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_separationminimum 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_radiusInscribed radius of the RobotFootprintModel for optimization
costmap_inscribed_radiusInscribed radius of the footprint model used for the costmap
min_obst_distdesired distance to obstacles

Definition at line 812 of file teb_local_planner_ros.cpp.


Member Data Documentation

Config class that stores and manages all related parameters.

Definition at line 359 of file teb_local_planner_ros.h.

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

Definition at line 350 of file teb_local_planner_ros.h.

Store the current costmap_converter.

Definition at line 366 of file teb_local_planner_ros.h.

Load costmap converter plugins at runtime.

Definition at line 365 of file teb_local_planner_ros.h.

Definition at line 358 of file teb_local_planner_ros.h.

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

Definition at line 349 of file teb_local_planner_ros.h.

Mutex that locks the obstacle array (multi-threaded)

Definition at line 370 of file teb_local_planner_ros.h.

Subscriber for custom obstacles received via a ObstacleMsg.

Definition at line 369 of file teb_local_planner_ros.h.

Copy of the most recent obstacle message.

Definition at line 371 of file teb_local_planner_ros.h.

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 368 of file teb_local_planner_ros.h.

Store the footprint of the robot.

Definition at line 380 of file teb_local_planner_ros.h.

The frame in which the controller will run.

Definition at line 384 of file teb_local_planner_ros.h.

std::vector<geometry_msgs::PoseStamped> teb_local_planner::TebLocalPlannerROS::global_plan_ [private]

Store the current global plan.

Definition at line 361 of file teb_local_planner_ros.h.

store whether the goal is reached or not

Definition at line 376 of file teb_local_planner_ros.h.

Keeps track about the correct initialization of this class.

Definition at line 388 of file teb_local_planner_ros.h.

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

Definition at line 378 of file teb_local_planner_ros.h.

Obstacle vector that should be considered during local trajectory optimization.

Definition at line 355 of file teb_local_planner_ros.h.

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

Definition at line 363 of file teb_local_planner_ros.h.

Instance of the underlying optimal planner class.

Definition at line 354 of file teb_local_planner_ros.h.

Used as the base frame id of the robot.

Definition at line 385 of file teb_local_planner_ros.h.

The radius of the circumscribed circle of the robot.

Definition at line 382 of file teb_local_planner_ros.h.

Store current robot goal.

Definition at line 374 of file teb_local_planner_ros.h.

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

Definition at line 381 of file teb_local_planner_ros.h.

Store current robot pose.

Definition at line 373 of file teb_local_planner_ros.h.

geometry_msgs::Twist teb_local_planner::TebLocalPlannerROS::robot_vel_ [private]

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

Definition at line 375 of file teb_local_planner_ros.h.

pointer to Transform Listener

Definition at line 351 of file teb_local_planner_ros.h.

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

Definition at line 377 of file teb_local_planner_ros.h.

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

Definition at line 356 of file teb_local_planner_ros.h.

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

Definition at line 357 of file teb_local_planner_ros.h.


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


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Sat Jun 8 2019 20:21:34