Obstacle Avoidance and DWB Controller

Related Readings

• Github repo of nav2_dwb_controller

• ROS Document - dwa_local_planner

• The Dynamic Window Approach to Collision Avoidance

• Planning and Control in Unstructured Terrain

• Local Path Planning: Dynamic Window Approach With Virtual Manipulators Considering Dynamic Obstacles

Introduction

In this article, we will explore the implementation of the DWB controller in Navigation2, a critic-based and highly configurable variant of the Dynamic Window Approach (DWA) Algorithm. We will start with a brief overview of the DWA algorithm, followed by a detailed examination of the DWB controller's key components, including the local planner, trajectory generator, and base obstacle critic.

DWA Overview

DWA simplifies robot movement calculation by assuming constant translation and rotational velocity during short time intervals. Under this assumption, the robot's path is reduced to either a straight line or an arc during these periods, making collision detection straightforward. The algorithm searches a 2D velocity space to determine the next command. Velocity pairs leading to potential collisions form inaccessible areas, while the robot's current velocity and acceleration limits confine the reachable space to a rectangular region centered around the current velocity of the robot. An objective function determines the best velocity command. For example, the function used in the DWA paper is as follows:

$$G(v, w) = \sigma(\alpha \cdot \textrm{heading}(v, w) + \beta \cdot \textrm{dist}(v, w) + \gamma \cdot \textrm{velocity}(v, w))$$

How is DWB implemented?

In the previous section, we outlined three essential components of a dynamic-window-based algorithm:

• The dynamic window. This is determined by the robot's current velocity and the maximum accelerations.

• The inaccessible regions. These regions depend on the robot's current location and the locations of the obstacles, informed by sensor data and map information.

• The objective function. The objective function evaluates various velocity commands to identify the optimal one.

Core Components

The core component in the DWB controller is the DWBLocalPlanner class, instantiated in nav2_controller/src/controller_server.cpp. Rather than diving into the interactions between DWBLocalPlanner and other components in the navigation stack, this article focuses on the code that implements the DWA algorithm.

In the Nav2 codebase, the standard practice for initializing an instance is through the configure method. For instance, the configure method in the DWBLocalPlanner class requires three arguments: LifecycleNode, tf2, and Costmap2DROS. These objects provide the context and inputs for the DWBLocalPlanner class. Parameters are another type of input. They are adjustable settings used to fine-tune the planner's behavior. We will cover this topic in a separate article.

The outputs of the DWBLocalPlanner are handled by the DWBPublisher class, which is responsible for publishing computation results to specific topics. For example:

pub_->publishCostGrid(costmap_ros_, critics_);
pub_->publishEvaluation(results);
pub_->publishGlobalPlan(global_plan_);
pub_->publishLocalPlan(pose.header, best.traj);
pub_->publishTransformedPlan(transformed_plan);

Public Methods of Planner

The DWBLocalPlanner has the following public methods:

setPlan
setSpeedLimit
scoreTrajectory
computeVelocityCommands

The setPlan method suggests that the planner receives a global plan, which it may then publish or modify for further processing. The role of the setSpeedLimit method is self-explanatory: it sets the linear speed limit of the robot. Specifically, it sets the speed limit of the trajectory generator, the component responsible for simulating the robot's trajectory. The next two methods are related. scoreTrajectory is used to evaluate proposed trajectories when the planner computes the velocity commands. In the context of the DWA algorithm, an objective function is required to choose the optimal velocity command, and scoreTrajectory implements such a function.

Let's take a look at the method computeVelocityCommands. Setting aside variable initialization and error handling, here is a simplified overview of its implementation:

nav_2d_msgs::msg::Twist2DStamped
DWBLocalPlanner::computeVelocityCommands(
  const nav_2d_msgs::msg::Pose2DStamped & pose,
  const nav_2d_msgs::msg::Twist2D & velocity,
  std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> & results)
{

    nav_2d_msgs::msg::Path2D transformed_plan;
    nav_2d_msgs::msg::Pose2DStamped goal_pose;

    prepareGlobalPlan(pose, transformed_plan, goal_pose);

    nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
    std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));


    dwb_msgs::msg::TrajectoryScore best = coreScoringAlgorithm(pose.pose, velocity, results);

    // Return Value
    nav_2d_msgs::msg::Twist2DStamped cmd_vel;
    cmd_vel.header.stamp = clock_->now();
    cmd_vel.velocity = best.traj.velocity;

    lock.unlock();

    pub_->publishLocalPlan(pose.header, best.traj);
    pub_->publishCostGrid(costmap_ros_, critics_);

    return cmd_vel;
}

It first prepares a global plan based on the current pose and the goal pose. Then, it locks the cost map so that the map cannot be modified during the calculation of the trajectory score. Next, it invokes the scoring algorithm and selects the best one. Finally, it unlocks the cost map and publishes the local plan and the cost map.

Move on to the coreScoringAlgorithm. Setting aside variable initialization and the exception handling, the method has the following structure:

dwb_msgs::msg::TrajectoryScore
DWBLocalPlanner::coreScoringAlgorithm(
  const geometry_msgs::msg::Pose2D & pose,
  const nav_2d_msgs::msg::Twist2D velocity,
  std::shared_ptr<dwb_msgs::msg::LocalPlanEvaluation> & results)
{

  traj_generator_->startNewIteration(velocity);

  while (traj_generator_->hasMoreTwists()) {

    twist = traj_generator_->nextTwist();
    traj = traj_generator_->generateTrajectory(pose, velocity, twist);
    try {

      dwb_msgs::msg::TrajectoryScore score = scoreTrajectory(traj, best.total);

      tracker.addLegalTrajectory();
      if (results) {
        results->twists.push_back(score);
      }
      if (best.total < 0 || score.total < best.total) {
        best = score;
        if (results) {
          results->best_index = results->twists.size() - 1;
        }
      }
      if (worst.total < 0 || score.total > worst.total) {
        worst = score;
        if (results) {
          results->worst_index = results->twists.size() - 1;
        }
      }
  }

  return best;
}

It iterates through the candidate velocity command and generates the correspondent trajectory. Then it calculates the score for each trajectory and returns the best one. The key step in this function is the invocation of the method scoreTrajectory.

The method scoreTrajector is straightforward. It iterates through the critics and each of them produces a score. The total score is used as the score of the trajectory. Recall in the paper, the objective function is defined as follows:

$$G(v, w) = \sigma(\alpha \cdot \textrm{heading}(v, w) + \beta \cdot \textrm{dist}(v, w) + \gamma \cdot \textrm{velocity}(v, w))$$

In the formula, heading, dist, and velocity are critics and each produces a score. For example, the score produced by the heading critic is $\textrm{heading}(v, w)$. The coefficient $\alpha$ corresponds to the critic scale in the implementation. For a complete reference of critics, you can check out the official document.

So far, we've examined the part in the DWBLocalPlanner that computes the velocity commands. We've learned the scoring algorithm, which implements the objective function in the DWA pager, is based on critics and this is the reason why nav2 DWB controller is called critic-based controller. The remaining question is where is the code that determines the dynamic window and accessible regions? This is answered in the next section.

Trajectory Generator

Two trajectory generators provided in navigation 2:

• Standard trajectory generator (StandardTrajectoryGenerator)
• Limited acceleration trajectory generator (LimitedAccelGenerator)

It's worth noting that the LimitedAccelGenerator is a derived class of the StandardTrajectoryGenerator. One of the key methods in the trajectory generator class is the method generateTrajectory:

dwb_msgs::msg::Trajectory2D generateTrajectory(
    const geometry_msgs::msg::Pose2D & start_pose,
    const nav_2d_msgs::msg::Twist2D & start_vel,
    const nav_2d_msgs::msg::Twist2D & cmd_vel) override;

This method takes the start pose, start velocity, and the velocity command as inputs and generate a Trajectory2D object, whose definition is given as follows:

# cat ./nav2_dwb_controller/dwb_msgs/msg/Trajectory2D.msg

# For a given velocity command, the poses that the robot will go to in the allotted time.

# Input Velocity
nav_2d_msgs/Twist2D velocity
# Time difference between first and last poses
builtin_interfaces/Duration[] time_offsets
# Poses the robot will go to, given our kinematic model
geometry_msgs/Pose2D[] poses

The implementation of the generateTrajectory method is straightforward. It essentially simulates a path:

dwb_msgs::msg::Trajectory2D StandardTrajectoryGenerator::generateTrajectory(
  const geometry_msgs::msg::Pose2D & start_pose,
  const nav_2d_msgs::msg::Twist2D & start_vel,
  const nav_2d_msgs::msg::Twist2D & cmd_vel)
{
    dwb_msgs::msg::Trajectory2D traj;
    traj.velocity = cmd_vel;

    // simulate the trajectory
    geometry_msgs::msg::Pose2D pose = start_pose;
    nav_2d_msgs::msg::Twist2D vel = start_vel;
    double running_time = 0.0;
    std::vector<double> steps = getTimeSteps(cmd_vel);
    traj.poses.push_back(start_pose);


    for (double dt : steps) {
        //  calculate velocities
        vel = computeNewVelocity(cmd_vel, vel, dt);

        //  update the position of the robot using the velocities passed in
        pose = computeNewPosition(pose, vel, dt);

        traj.poses.push_back(pose);
        traj.time_offsets.push_back(rclcpp::Duration::from_seconds(running_time));
        running_time += dt;
    }  //  end for simulation steps

    if (include_last_point_) {
        traj.poses.push_back(pose);
        traj.time_offsets.push_back(rclcpp::Duration::from_seconds(running_time));
    }

    return traj;
}

Regarding the dynamic window in the DWA algorithm, it's implemented in the computeNewVelocity method, which is overridden by the LimitedAccelGenerator which takes the acceleration into account.

If you search for keywords such as "obstacle" or "avoidance" in the navigation2/nav2_dwb_controller/dwb_plugins directory, you won't find any results. This suggests that the obstacle avoidance and the related accessible region determination are handled by other parts of the code.

Base Obstacle Critic

You may still wonder how obstacle avoidance is handled by the code. Remember, the DWA algorithm introduces the concept of inaccessible regions in the velocity space, representing velocity commands that could lead to collisions.

In the nav2 implementation, given the current pose of the robot and the velocity command, the planner calculates a trajectory and submits it to the base obstacle critic for evaluation. If this trajectory intersects with any obstacle in the map, it is considered invalid and excluded from the selection of the optimal trajectory.

The base obstacle critic can obtain obstacle information through the cost map. Special cost values are defined in the costmap header file to indicate the nature of a cell in the map:

static constexpr unsigned char NO_INFORMATION = 255;
static constexpr unsigned char LETHAL_OBSTACLE = 254;
static constexpr unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
static constexpr unsigned char MAX_NON_OBSTACLE = 252;
static constexpr unsigned char FREE_SPACE = 0;

Recall that when the local planner computes the velocity, it locks the cost map object. Now we can see why it needs to be locked: we want to ensure that all critics use the same cost map for their evaluation.

Here, we only list the key part of the BaseObstacleCritic implementation. For more details, please refer to the source code:

double BaseObstacleCritic::scoreTrajectory(const dwb_msgs::msg::Trajectory2D & traj)
{
    double score = 0.0;

    for (unsigned int i = 0; i < traj.poses.size(); ++i) {
        double pose_score = scorePose(traj.poses[i]);
        // Optimized/branchless version of if (sum_scores_) score += pose_score,
        // else score = pose_score;
        score = static_cast<double>(sum_scores_) * score + pose_score;
    }
    return score;
}

double BaseObstacleCritic::scorePose(const geometry_msgs::msg::Pose2D & pose)
{
    unsigned int cell_x, cell_y;
    if (!costmap_->worldToMap(pose.x, pose.y, cell_x, cell_y)) {
        throw dwb_core::
              IllegalTrajectoryException(name_, "Trajectory Goes Off Grid.");
    }

    unsigned char cost = costmap_->getCost(cell_x, cell_y);

    if (!isValidCost(cost)) {
        throw dwb_core::
              IllegalTrajectoryException(name_, "Trajectory Hits Obstacle.");
    }
    return cost;
}

Summary

This article discusses the implementation of the DWB controller in navigation 2. It begins with an overview of the DWA algorithm and introduces key concepts such as velocity space, inaccessible regions, and the dynamic window. It then focuses on the DWBLocalPlanner, a key component in the DWB controller, and discusses the trajectory generator and the base obstacle critic. The article also highlights how these components are related to the steps of the DWA algorithm.

If you enjoy reading this article and wish to support my work, you can make a contribution through Stripe or Buy Me a Coffee. Your support means a lot to me and helps me to continue producing content that you like and find useful.

(っ◔◡◔)っ ❤️ give support ➡️ Stripe ➡️

LearnRos2Buy Me a Coffee