# Obstacle Avoidance and DWB Controller

## Related Readings

* [Github repo of nav2\_dwb\_controller](https://github.com/ros-planning/navigation2/tree/humble/nav2_dwb_controller)
* [ROS Document - dwa\_local\_planner](http://wiki.ros.org/dwa_local_planner)
* [The Dynamic Window Approach to Collision Avoidance](https://www.ri.cmu.edu/pub_files/pub1/fox_dieter_1997_1/fox_dieter_1997_1.pdf)
* [Planning and Control in Unstructured Terrain](https://citeseerx.ist.psu.edu/document?repid=rep1\&type=pdf\&doi=dabdbb636f02d3cff3d546bd1bdae96a058ba4bc)
* [Local Path Planning: Dynamic Window Approach With Virtual Manipulators Considering Dynamic Obstacles](https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=\&arnumber=9707826)

## 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))
$$

<figure><img src="/files/WcTTSiTWxyhG8lulyKnT" alt=""><figcaption></figcaption></figure>

## 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](https://github.com/ros-planning/navigation2/blob/humble/nav2_controller/src/controller_server.cpp#L47).  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`.&#x20;

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](https://navigation.ros.org/configuration/packages/configuring-dwb-controller.html#trajectory-critics).

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](https://github.com/ros-planning/navigation2/blob/humble/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp))
* Limited acceleration trajectory generator ([LimitedAccelGenerator](https://github.com/ros-planning/navigation2/blob/humble/nav2_dwb_controller/dwb_plugins/include/dwb_plugins/limited_accel_generator.hpp))

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`:&#x20;

```
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](https://github.com/ros-planning/navigation2/tree/humble/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](https://github.com/ros-planning/navigation2/blob/humble/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp) 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](https://github.com/ros-planning/navigation2/blob/humble/nav2_dwb_controller/dwb_critics/src/base_obstacle.cpp):

```
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.&#x20;

*<mark style="color:green;">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.</mark>*

(っ◔◡◔)っ  :heart: give support :arrow\_right:  [**Stripe**](https://donate.stripe.com/8wMeYddJy7d4fK0145) :arrow\_right:

{% embed url="<https://www.buymeacoffee.com/learnros2>" %}


---

# Agent Instructions: Querying This Documentation

If you need additional information that is not directly available in this page, you can query the documentation dynamically by asking a question.

Perform an HTTP GET request on the current page URL with the `ask` query parameter:

```
GET https://www.learnros2.com/ros/navigation-and-planning/navigation2-implementation-overview/obstacle-avoidance-and-dwb-controller.md?ask=<question>
```

The question should be specific, self-contained, and written in natural language.
The response will contain a direct answer to the question and relevant excerpts and sources from the documentation.

Use this mechanism when the answer is not explicitly present in the current page, you need clarification or additional context, or you want to retrieve related documentation sections.