Class HATebLocalPlannerROS

Defined in File hateb_local_planner_ros.h

Inheritance Relationships

Base Types

public nav_core::BaseLocalPlanner
public mbf_costmap_core::CostmapController

Class Documentation

class HATebLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap_core::CostmapController

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

Public utility functions/methods

costmap_2d::Costmap2DROS *costmap_ros_: Pointer to the costmap ros wrapper, received from the navigation stack.

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

tf2_ros::Buffer *tf_: pointer to tf buffer

PlannerInterfacePtr planner_: Instance of the underlying optimal planner class.

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

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

std::map<uint64_t, ViaPointContainer> agents_via_points_map_: Map storing via points for each agent.

TebVisualizationPtr visualization_: Instance of the visualization class (local/global plan, obstacles, …)

boost::shared_ptr<base_local_planner::CostmapModel> costmap_model_: Costmap model for collision checking.

HATebConfig cfg_: Config class that stores and manages all related parameters.

HATebLocalPlannerReconfigureConfig config_: Dynamic reconfigure config class.

FailureDetector failure_detector_: Detect if the robot got stucked.

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

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

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

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

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

ros::Subscriber custom_obst_sub_: Subscriber for custom obstacles received via ObstacleMsg.

ros::Subscriber inv_humans_sub_: Subscriber for invisible humans data.

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

boost::mutex inv_human_mutex_: Mutex that locks the invisible humans array.

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

costmap_converter::ObstacleArrayMsg inv_humans_msg_: Copy of the most recent invisible humans message.

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

bool custom_via_points_active_: Keep track whether valid via-points have been received.

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

PoseSE2 robot_pose_: Store current robot pose.

PoseSE2 robot_goal_: Store current robot goal.

geometry_msgs::Twist robot_vel_: Store current robot velocities (vx, vy, omega)

bool goal_reached_: Store whether the goal is reached or not.

bool horizon_reduced_: Flag indicating if the planning horizon was temporarily reduced.

ros::Time horizon_reduced_stamp_: Time when the horizon was reduced.

ros::Time time_last_infeasible_plan_: Time stamp of last infeasible plan detection.

int no_infeasible_plans_: Number of consecutive infeasible plans.

ros::Time time_last_oscillation_: Time stamp of last oscillation detection.

RotType last_preferred_rotdir_: Store recent preferred turning direction.

geometry_msgs::Twist last_cmd_: Store the last control command generated.

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

double robot_inscribed_radius_: The radius of the inscribed circle of the robot.

double robot_circumscribed_radius_: The radius of the circumscribed circle of the robot.

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

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

bool initialized_: Flag to track proper initialization of this class.

ros::ServiceClient predict_agents_client_: Client for predicting agent trajectories.

ros::ServiceClient reset_agents_prediction_client_: Client for resetting agent predictions.

ros::ServiceClient publish_predicted_markers_client_: Client for publishing prediction markers.

std::string predict_srv_name_: Name of the prediction service.

std::string reset_prediction_srv_name_: Name of the reset prediction service.

std::string publish_makers_srv_name_: Name of the marker publishing service.

ros::ServiceServer optimize_server_: optimize service for calling standalone

ros::Time last_call_time_: Time of last service call.

ros::Time last_omega_sign_change_: Time of last angular velocity sign change.

double last_omega_: Last angular velocity value.

bool goal_ctrl_: Flag for goal control.

bool reset_states_: Flag for state reset.

int isMode_: Current planning mode.

std::string logs_: System log messages.

ros::Subscriber agents_sub_: Subscriber for tracked agents.

ros::Publisher log_pub_: Publisher for system logs.

std::string ns_: Namespace for multi-agent support.

std::string invisible_humans_sub_topic_: Name for invisible humans topic.

std::shared_ptr<agents::Agents> agents_ptr_: Pointer to agents management class.

std::shared_ptr<Backoff> backoff_ptr_: Pointer to backoff behavior class.

ModeSwitch bt_mode_switch_: Behavior tree mode switch handler.

static Eigen::Vector2d tfPoseToEigenVector2dTransRot(const tf::Pose &tf_vel)

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

static FootprintModelPtr getRobotFootprintFromParamServer(const ros::NodeHandle &nh, const HATebConfig &config)

Get the current robot footprint/contour model.

Parameters:: nh – const reference to the local ros::NodeHandle
Returns:: Robot footprint model used for optimization

static Point2dContainer makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue &footprint_xmlrpc, const std::string &full_param_name)

Set the footprint from the given XmlRpcValue.

Remark

This method is copied from costmap_2d/footprint.h, since it is not declared public in all ros distros

Remark

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

static double getNumberFromXMLRPC(XmlRpc::XmlRpcValue &value, const std::string &full_param_name)

Get a number from the given XmlRpcValue.

Remark

This method is copied from costmap_2d/footprint.h, since it is not declared public in all ros distros

Remark

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

void updateObstacleContainerWithCostmap()

Update internal obstacle vector based on occupied costmap cells.

Remark

All occupied cells will be added as point obstacles.

Remark

All previous obstacles are cleared.

void updateObstacleContainerWithCostmapConverter()

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

Remark

Requires a loaded costmap_converter plugin.

Remark

All previous obstacles are cleared.

See also

updateObstacleContainerWithCostmap

void updateObstacleContainerWithCustomObstacles()

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

Remark

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

void updateObstacleContainerWithInvHumans()

Update internal obstacle vector based on invisible human messages received via subscriber.

Remark

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

void updateViaPointsContainer(const std::vector<geometry_msgs::PoseStamped> &transformed_plan, double min_separation)

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

Remark

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

void updateAgentViaPointsContainers(const AgentPlanVelMap &transformed_agent_plan_vel_map, double min_separation)

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

Remark

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

void reconfigureCB(HATebLocalPlannerReconfigureConfig &config, uint32_t level)

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

void customObstacleCB(const costmap_converter::ObstacleArrayMsg::ConstPtr &obst_msg)

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

void InvHumansCB(const costmap_converter::ObstacleArrayMsg::ConstPtr &obst_msg)

Callback for invisible humans.

Parameters:: obst_msg – pointer to the message containing a list of circular obstacles

void customViaPointsCB(const nav_msgs::Path::ConstPtr &via_points_msg)

Callback for custom via-points.

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

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, PlanCombined &transformed_plan_combined, int *current_goal_idx = nullptr, geometry_msgs::TransformStamped *tf_plan_to_global = nullptr) const

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!]
transformed_plan – [out] Populated with the transformed plan
current_goal_idx – [out] Index of the current (local) goal pose in the global plan
tf_plan_to_global – [out] Transformation between the global plan and the global planning frame

Returns:

true if the global plan is transformed, false otherwise

bool transformAgentPlan(const tf2_ros::Buffer &tf2, const geometry_msgs::PoseStamped &robot_pose, const costmap_2d::Costmap2D &costmap, const std::string &global_frame, const std::vector<geometry_msgs::PoseWithCovarianceStamped> &agent_plan, AgentPlanCombined &transformed_agent_plan_combined, geometry_msgs::TwistStamped &transformed_agent_twist, tf2::Stamped<tf2::Transform> *tf_agent_plan_to_global = nullptr) const

Transforms the agent plan from the tracker frame to the local frame.

Parameters:

tf – A reference to a transform listener
agent_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
transformed_agent_plan – [out] Populated with the transformed plan
tf_agent_plan_to_global – [out] Transformation between the agent plan and the local planning frame

Returns:

true if the global plan is transformed, false otherwise

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)

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:

vx – [inout] The translational velocity that should be saturated.
vy – [inout] Strafing velocity which can be nonzero for holonomic robots
omega – [inout] 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

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

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

This method prints warnings if validation fails.

Remark

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

void resetAgentsPrediction()

Reset the prediction state for all tracked agents.

This method resets the prediction states for all agents being tracked by the planner. It calls the prediction reset service to clear any existing prediction data and prepare for new predictions.

bool tickTreeAndUpdatePlans(const geometry_msgs::PoseStamped &robot_pose, std::vector<AgentPlanCombined> &transformed_agent_plans, AgentPlanVelMap &transformed_agent_plan_vel_map)

Process the behavior tree and update agent plans.

This method ticks the behavior tree to evaluate the current planning mode and updates the transformed plans for all agents in the environment.

Parameters:

robot_pose – Current pose of the robot
transformed_agent_plans – Vector containing the transformed plans for all agents
transformed_agent_plan_vel_map – Map containing agent plans with their velocities

Returns:

True if plans were successfully updated, false otherwise

bool optimizeStandalone(cohan_msgs::Optimize::Request &req, cohan_msgs::Optimize::Response &res)

Perform standalone trajectory optimization.

This method provides a service interface for performing trajectory optimization independently of the main planning loop. Useful for analysis and debugging.

Parameters:

req – Service request containing optimization parameters
res – Service response containing optimization results

Returns:

True if optimization was successful, false otherwise

inline void lookupTwist(const std::string &tracking_frame, const std::string &observation_frame, const ros::Time &time, const ros::Duration &averaging_interval, geometry_msgs::Twist &twist) const

Look up twist information between frames with default reference point.

Simplified version that uses the origin of tracking_frame as the reference point and observation_frame as the reference frame.

Parameters:

tracking_frame – The frame being tracked
observation_frame – The frame from which we’re observing
time – The time at which to get the twist
averaging_interval – Time interval over which to average the twist
twist – [out] The resulting twist message

inline void lookupTwist(const std::string &tracking_frame, const std::string &observation_frame, const std::string &reference_frame, const tf2::Vector3 &reference_point, const std::string &reference_point_frame, const ros::Time &time, const ros::Duration &averaging_interval, geometry_msgs::Twist &twist) const

Look up twist information between frames with custom reference point.

This method computes the twist (linear and angular velocity) of one frame relative to another, allowing specification of a custom reference point and frame. It performs the necessary coordinate transformations and averages the motion over the specified time interval.

Parameters:

tracking_frame – The frame being tracked (whose motion we want to determine)
observation_frame – The frame from which we’re observing
reference_frame – The frame in which the twist should be expressed
reference_point – The point about which the twist should be computed
reference_point_frame – The frame in which the reference point is expressed
time – The time at which to get the twist
averaging_interval – Time interval over which to average the twist
twist – [out] The resulting twist message containing linear and angular velocities

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

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.

Remark

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
global_plan – [inout] 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

static 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)

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
tf_plan_to_global – [out] 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

static double convertTransRotVelToSteeringAngle(double v, double omega, double wheelbase, double min_turning_radius = 0)

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.

Remark

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]

static void validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist)

Public Functions

HATebLocalPlannerROS(): Default constructor of the teb plugin.

~HATebLocalPlannerROS() override: Destructor of the plugin.

HATebLocalPlannerROS(std::string name, tf2_ros::Buffer *tf, costmap_2d::Costmap2DROS *costmap_ros)

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

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

bool setPlan(const std::vector<geometry_msgs::PoseStamped> &orig_global_plan) override

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

bool computeVelocityCommands(geometry_msgs::Twist &cmd_vel) override

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

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

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

Remark

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

bool isGoalReached() override

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

inline bool isGoalReached(double xy_tolerance, double yaw_tolerance) override: Dummy version to satisfy MBF API.

inline bool cancel() override

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

Remark

New on MBF API

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