Class Backoff

Class Documentation

class Backoff

Class implementing backoff behavior for robot navigation.

The Backoff class provides recovery behavior when the robot encounters a human in a place where both agents (human and robot) get stuck without any progress towards to goal. It implements methods to calculate and execute backoff maneuvers to help the robot recover from such situations.

Public Functions

Backoff(costmap_2d::Costmap2DROS *costmap_ros)

Constructor.

~Backoff()

Destructor.

void initialize(costmap_2d::Costmap2DROS *costmap_ros)

Initializes the backoff behavior with a costmap.

Parameters:

costmap_ros – Pointer to the costmap ROS wrapper

bool setbackGoal(geometry_msgs::PoseStamped goal)

Sets a new backoff goal position.

Parameters:

goal – The goal pose to back off to

Returns:

True if goal was successfully set

bool timeOut()

Checks if backoff behavior has timed out.

Returns:

True if timeout occurred

bool startRecovery()

Initiates the recovery behavior.

Returns:

True if recovery was successfully started

inline bool checkNewGoal() const

Checks if a new goal has been received.

Returns:

True if there is a new goal

bool isBackoffGoalReached(geometry_msgs::Pose2D &robot_pose) const

Checks if the backoff goal position has been reached.

Parameters:

robot_pose – Current pose of the robot

Returns:

True if goal has been reached

inline void initializeOffsets(double r)

Initializes the grid offsets for obstacle checking.

Sets up two grids of points (left and right) around the robot’s circumference used for checking nearby obstacles when planning backoff maneuvers.

Parameters:

r – Robot’s circumscribed radius

Private Functions

void goalCB(const geometry_msgs::PoseStamped::ConstPtr &goal)

Callback for processing new goal messages.

Parameters:

goal – Pointer to the received goal message

void loadRosParamFromNodeHandle(const ros::NodeHandle &private_nh)

Loads ROS parameters from the node handle.

Parameters:

private_nh – Private node handle containing parameters

Private Members

std::string map_frame_

Name of the map frame.

std::string footprint_frame_

Name of the robot’s footprint frame.

std::string ns_

Namespace for the node.

std::string publish_goal_topic_

Topic name for publishing backoff goals.

std::string current_goal_topic_

Topic name for current goal.

std::string get_plan_srv_name_

Service name for path planning.

double backoff_timeout_

Maximum allowed time for backoff maneuver.

geometry_msgs::PoseStamped goal_

Current navigation goal.

geometry_msgs::PoseStamped old_goal_

Previous navigation goal.

tf::TransformListener tf_

Transform listener for coordinate transformations.

costmap_2d::Costmap2DROS *costmap_ros_

Pointer to the costmap ROS wrapper.

costmap_2d::Costmap2D *costmap_

Pointer to the 2D costmap.

double robot_circumscribed_radius_

Radius of circle encompassing the robot.

ros::Publisher goal_pub_

Publisher for sending backoff goal poses.

ros::Publisher poly_pub_l_

Publisher for visualizing left grid points.

ros::Publisher poly_pub_r_

Publisher for visualizing right grid points.

ros::Subscriber goal_sub_

Subscriber for receiving navigation goals.

ros::ServiceClient get_plan_client_

Service client for requesting path plans.

geometry_msgs::Transform start_pose_

Initial robot pose when backoff maneuver starts.

bool visualize_backoff_

Enable visualization of backoff grids.

bool self_published_

Whether the goal was published by this node.

bool new_goal_

Whether a new navigation goal was received.

ros::Time last_time_

Time of the last update.

tf::Transform start_pose_tr_

Initial pose stored as transform.

tf::StampedTransform robot_to_map_tf_

Transform from robot base to map frame.

std::shared_ptr<base_local_planner::CostmapModel> costmap_model_

Model for collision checking with costmap.

geometry_msgs::PoseStamped backoff_goal_

Goal pose for backoff maneuver.

std::vector<geometry_msgs::Point> left_grid_

Grid points for left obstacle checks.

std::vector<geometry_msgs::Point> right_grid_

Grid points for right obstacle checks.

Private Static Functions

static inline double normalize_angle(double angle_radians)

Normalizes an angle to the range [-π, π].

Parameters:

angle_radians – Angle to normalize (in radians)

Returns:

Normalized angle in radians