Add support of absolute limits to Speed Filter (#2149)

* Add support of absolute limits to Speed Filter

* Fix review items

* Fix comment

* Support for nav2_regulated_pure_pursuit_controller

doc/parameters/param_list.md

@@ -474,7 +474,7 @@ When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parame
 
 | Parameter | Default | Description |
 | ----------| --------| ------------|
-| type | 0 | Type of costmap filter used. This is an enum for the type of filter this should be interpreted as. We provide the following pre-defined types: 0 - keepout zones / preferred lanes filter; 1 - speed filter, speed limit is specified in % of maximum speed; 2 - speed filter, speed limit is specified in absolute value (not implemented yet) |
+| type | 0 | Type of costmap filter used. This is an enum for the type of filter this should be interpreted as. We provide the following pre-defined types: 0 - keepout zones / preferred lanes filter; 1 - speed filter, speed limit is specified in % of maximum speed; 2 - speed filter, speed limit is specified in absolute value (m/s) |
 | filter_info_topic | "costmap_filter_info" | Topic to publish costmap filter information to |
 | mask_topic | "filter_mask" | Topic to publish filter mask to. The value of this parameter should be in accordance with `topic_name` parameter of `map_server` tuned to filter mask publishing |
 | base | 0.0 | Base of `OccupancyGrid` mask value -> filter space value linear conversion which is being proceeded as: `filter_space_value = base + multiplier * mask_value` |

nav2_controller/src/nav2_controller.cpp

@@ -467,11 +467,7 @@ void ControllerServer::speedLimitCallback(const nav2_msgs::msg::SpeedLimit::Shar
 {
   ControllerMap::iterator it;
   for (it = controllers_.begin(); it != controllers_.end(); ++it) {
-    if (!msg->percentage) {
-      RCLCPP_ERROR(get_logger(), "Speed limit in absolute values is not implemented yet");
-      return;
-    }
-    it->second->setSpeedLimit(msg->speed_limit);
+    it->second->setSpeedLimit(msg->speed_limit, msg->percentage);
   }
 }
 

nav2_core/include/nav2_core/controller.hpp

@@ -115,9 +115,12 @@ class Controller
 
   /**
    * @brief Limits the maximum linear speed of the robot.
-   * @param speed_limit expressed in percentage from maximum robot speed.
+   * @param speed_limit expressed in absolute value (in m/s)
+   * or in percentage from maximum robot speed.
+   * @param percentage Setting speed limit in percentage if true
+   * or in absolute values in false case.
    */
-  virtual void setSpeedLimit(const double & speed_limit) = 0;
+  virtual void setSpeedLimit(const double & speed_limit, const bool & percentage) = 0;
 };
 
 }  // namespace nav2_core

nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp

@@ -112,6 +112,7 @@ class SpeedFilter : public CostmapFilter
   std::string global_frame_;  // Frame of currnet layer (master_grid)
 
   double base_, multiplier_;
+  bool percentage_;
   double speed_limit_, speed_limit_prev_;
 };
 

nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp

@@ -90,6 +90,7 @@ void SpeedFilter::initializeFilter(
   // Reset speed conversion states
   base_ = BASE_DEFAULT;
   multiplier_ = MULTIPLIER_DEFAULT;
+  percentage_ = false;
 }
 
 void SpeedFilter::filterInfoCallback(
@@ -120,15 +121,19 @@ void SpeedFilter::filterInfoCallback(
   multiplier_ = msg->multiplier;
   if (msg->type == SPEED_FILTER_PERCENT) {
     // Using speed limit in % of maximum speed
+    percentage_ = true;
     RCLCPP_INFO(
       logger_,
       "SpeedFilter: Using expressed in a percent from maximum speed"
       "speed_limit = %f + filter_mask_data * %f",
       base_, multiplier_);
   } else if (msg->type == SPEED_FILTER_ABSOLUTE) {
-    // Speed limit in absolute values is not implemented yet
-    RCLCPP_ERROR(logger_, "SpeedFilter: Speed limit in absolute values is not implemented yet");
-    return;
+    // Using speed limit in m/s
+    percentage_ = false;
+    RCLCPP_INFO(
+      logger_,
+      "SpeedFilter: Using absolute speed_limit = %f + filter_mask_data * %f",
+      base_, multiplier_);
   } else {
     RCLCPP_ERROR(logger_, "SpeedFilter: Mode is not supported");
     return;
@@ -278,30 +283,30 @@ void SpeedFilter::process(
   } else {
     // Normal case: speed_mask_data in range of [1..100]
     speed_limit_ = speed_mask_data * multiplier_ + base_;
-    if (speed_limit_ < 0.0) {
-      RCLCPP_WARN(
-        logger_,
-        "SpeedFilter: Speed limit in filter_mask[%i, %i] is less than 0%%, "
-        "which can not be true. Setting it to 0%% value.",
-        mask_robot_i, mask_robot_j);
-      speed_limit_ = NO_SPEED_LIMIT;
-    }
-    if (speed_limit_ > 100.0) {
-      RCLCPP_WARN(
-        logger_,
-        "SpeedFilter: Speed limit in filter_mask[%i, %i] is higher than 100%%, "
-        "which can not be true. Setting it to 100%% value.",
-        mask_robot_i, mask_robot_j);
-      speed_limit_ = 100.0;
+    if (percentage_) {
+      if (speed_limit_ < 0.0 || speed_limit_ > 100.0) {
+        RCLCPP_WARN(
+          logger_,
+          "SpeedFilter: Speed limit in filter_mask[%i, %i] is %f%%, "
+          "out of bounds of [0, 100]. Setting it to no-limit value.",
+          mask_robot_i, mask_robot_j, speed_limit_);
+        speed_limit_ = NO_SPEED_LIMIT;
+      }
+    } else {
+      if (speed_limit_ < 0.0) {
+        RCLCPP_WARN(
+          logger_,
+          "SpeedFilter: Speed limit in filter_mask[%i, %i] is less than 0 m/s, "
+          "which can not be true. Setting it to no-limit value.",
+          mask_robot_i, mask_robot_j);
+        speed_limit_ = NO_SPEED_LIMIT;
+      }
     }
   }
 
   if (speed_limit_ != speed_limit_prev_) {
     if (speed_limit_ != NO_SPEED_LIMIT) {
-      RCLCPP_DEBUG(
-        logger_,
-        "SpeedFilter: Speed limit is set to %f%% of maximum speed",
-        speed_limit_);
+      RCLCPP_DEBUG(logger_, "SpeedFilter: Speed limit is set to %f", speed_limit_);
     } else {
       RCLCPP_DEBUG(logger_, "SpeedFilter: Speed limit is set to its default value");
     }
@@ -311,7 +316,7 @@ void SpeedFilter::process(
       std::make_unique<nav2_msgs::msg::SpeedLimit>();
     msg->header.frame_id = global_frame_;
     msg->header.stamp = clock_->now();
-    msg->percentage = true;
+    msg->percentage = percentage_;
     msg->speed_limit = speed_limit_;
     speed_limit_pub_->publish(std::move(msg));
 

nav2_costmap_2d/test/unit/speed_filter_test.cpp

@@ -223,7 +223,7 @@ class TestNode : public ::testing::Test
     uint8_t type, double base, double multiplier,
     double tr_x, double tr_y);
   void testOutOfMask(uint8_t type, double base, double multiplier);
-  void testIncorrectPercentage(uint8_t type, double base, double multiplier);
+  void testIncorrectLimits(uint8_t type, double base, double multiplier);
 
   void reset();
 
@@ -420,16 +420,20 @@ void TestNode::verifySpeedLimit(
   // expected_limit is being calculated by using float32 base and multiplier
   double expected_limit = cost * multiplier + base;
   if (type == nav2_costmap_2d::SPEED_FILTER_PERCENT) {
-    if (expected_limit < 0.0) {
+    if (expected_limit < 0.0 || expected_limit > 100.0) {
       expected_limit = nav2_costmap_2d::NO_SPEED_LIMIT;
     }
-    if (expected_limit > 100.0) {
-      expected_limit = 100.0;
-    }
     EXPECT_TRUE(speed_limit->percentage);
     EXPECT_TRUE(speed_limit->speed_limit >= 0.0);
     EXPECT_TRUE(speed_limit->speed_limit <= 100.0);
     EXPECT_NEAR(speed_limit->speed_limit, expected_limit, EPSILON);
+  } else if (type == nav2_costmap_2d::SPEED_FILTER_ABSOLUTE) {
+    if (expected_limit < 0.0) {
+      expected_limit = nav2_costmap_2d::NO_SPEED_LIMIT;
+    }
+    EXPECT_FALSE(speed_limit->percentage);
+    EXPECT_TRUE(speed_limit->speed_limit >= 0.0);
+    EXPECT_NEAR(speed_limit->speed_limit, expected_limit, EPSILON);
   } else {
     FAIL() << "The type of costmap filter is unknown";
   }
@@ -581,7 +585,7 @@ void TestNode::testOutOfMask(uint8_t type, double base, double multiplier)
   ASSERT_TRUE(speed_limit == old_speed_limit);
 }
 
-void TestNode::testIncorrectPercentage(uint8_t type, double base, double multiplier)
+void TestNode::testIncorrectLimits(uint8_t type, double base, double multiplier)
 {
   const int min_i = 0;
   const int min_j = 0;
@@ -593,14 +597,14 @@ void TestNode::testIncorrectPercentage(uint8_t type, double base, double multipl
 
   std::vector<std::tuple<unsigned int, unsigned int>> points;
 
-  // Some middle point corresponding to correct percentage
+  // Some middle point corresponding to correct speed limit value
   points.push_back(std::make_tuple(width_ / 2 - 1, height_ / 2 - 1));
-  // (0, 1) point corresponding to incorrect percentage: data = 1, percentage < 0%
+  // (0, 1) point corresponding to incorrect limit value: data = 1, value < 0
   points.push_back(std::make_tuple(0, 1));
-  // Some middle point corresponding to correct percentage
+  // Some middle point corresponding to correct speed limit value
   points.push_back(std::make_tuple(width_ / 2 - 1, height_ / 2 - 1));
-  // (width_ - 1, height_ - 1) point corresponding to incorrect percentage:
-  // data = 100, percentage > 100%
+  // (width_ - 1, height_ - 1) point corresponding to incorrect limit value:
+  // data = 100, value > 100
   points.push_back(std::make_tuple(width_ - 1, height_ - 1));
 
   for (auto it = points.begin(); it != points.end(); ++it) {
@@ -650,7 +654,7 @@ TEST_F(TestNode, testIncorrectPercentSpeedLimit)
   EXPECT_TRUE(createSpeedFilter("map"));
 
   // Test SpeedFilter
-  testIncorrectPercentage(nav2_costmap_2d::SPEED_FILTER_PERCENT, -50.0, 2.0);
+  testIncorrectLimits(nav2_costmap_2d::SPEED_FILTER_PERCENT, -50.0, 2.0);
 
   // Clean-up
   speed_filter_->resetFilter();
@@ -661,8 +665,26 @@ TEST_F(TestNode, testAbsoluteSpeedLimit)
 {
   // Initilize test system
   createMaps("map");
-  publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, -1.23, 4.5);
-  EXPECT_FALSE(createSpeedFilter("map"));
+  publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5);
+  EXPECT_TRUE(createSpeedFilter("map"));
+
+  // Test SpeedFilter
+  testFullMask(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5, NO_TRANSLATION, NO_TRANSLATION);
+
+  // Clean-up
+  speed_filter_->resetFilter();
+  reset();
+}
+
+TEST_F(TestNode, testIncorrectAbsoluteSpeedLimit)
+{
+  // Initilize test system
+  createMaps("map");
+  publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, -50.0, 2.0);
+  EXPECT_TRUE(createSpeedFilter("map"));
+
+  // Test SpeedFilter
+  testIncorrectLimits(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, -50.0, 2.0);
 
   // Clean-up
   speed_filter_->resetFilter();
@@ -688,7 +710,7 @@ TEST_F(TestNode, testInfoRePublish)
 {
   // Initilize test system
   createMaps("map");
-  publishMaps(nav2_costmap_2d::SPEED_FILTER_PERCENT, 1.2, 3.4);
+  publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5);
   EXPECT_TRUE(createSpeedFilter("map"));
 
   // Re-publish filter info (with incorrect base and multiplier)
@@ -708,15 +730,15 @@ TEST_F(TestNode, testMaskRePublish)
 {
   // Initilize test system
   createMaps("map");
-  publishMaps(nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.1, 0.2);
+  publishMaps(nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5);
   EXPECT_TRUE(createSpeedFilter("map"));
 
   // Re-publish filter mask and test that everything is working after
   rePublishMask();
 
   // Test SpeedFilter
   testSimpleMask(
-    nav2_costmap_2d::SPEED_FILTER_PERCENT, 0.1, 0.2, NO_TRANSLATION, NO_TRANSLATION);
+    nav2_costmap_2d::SPEED_FILTER_ABSOLUTE, 1.23, 4.5, NO_TRANSLATION, NO_TRANSLATION);
 
   // Clean-up
   speed_filter_->resetFilter();
@@ -727,7 +749,7 @@ TEST_F(TestNode, testIncorrectFilterType)
 {
   // Initilize test system
   createMaps("map");
-  publishMaps(INCORRECT_TYPE, -1.23, 4.5);
+  publishMaps(INCORRECT_TYPE, 1.23, 4.5);
   EXPECT_FALSE(createSpeedFilter("map"));
 
   // Clean-up

nav2_dwb_controller/dwb_core/include/dwb_core/dwb_local_planner.hpp

@@ -133,12 +133,15 @@ class DWBLocalPlanner : public nav2_core::Controller
 
   /**
    * @brief Limits the maximum linear speed of the robot.
-   * @param speed_limit expressed in percentage from maximum robot speed.
+   * @param speed_limit expressed in absolute value (in m/s)
+   * or in percentage from maximum robot speed.
+   * @param percentage Setting speed limit in percentage if true
+   * or in absolute values in false case.
    */
-  void setSpeedLimit(const double & speed_limit) override
+  void setSpeedLimit(const double & speed_limit, const bool & percentage) override
   {
     if (traj_generator_) {
-      traj_generator_->setSpeedLimit(speed_limit);
+      traj_generator_->setSpeedLimit(speed_limit, percentage);
     }
   }
 

nav2_dwb_controller/dwb_core/include/dwb_core/trajectory_generator.hpp

@@ -126,9 +126,12 @@ class TrajectoryGenerator
 
   /**
    * @brief Limits the maximum linear speed of the robot.
-   * @param speed_limit expressed in percentage from maximum robot speed.
+   * @param speed_limit expressed in absolute value (in m/s)
+   * or in percentage from maximum robot speed.
+   * @param percentage Setting speed limit in percentage if true
+   * or in absolute values in false case.
    */
-  virtual void setSpeedLimit(const double & speed_limit) = 0;
+  virtual void setSpeedLimit(const double & speed_limit, const bool & percentage) = 0;
 };
 
 }  // namespace dwb_core

nav2_dwb_controller/dwb_plugins/include/dwb_plugins/kinematic_parameters.hpp

@@ -113,7 +113,7 @@ class KinematicsHandler
 
   inline KinematicParameters getKinematics() {return *kinematics_.load();}
 
-  void setSpeedLimit(const double & speed_limit);
+  void setSpeedLimit(const double & speed_limit, const bool & percentage);
 
   using Ptr = std::shared_ptr<KinematicsHandler>;
 

nav2_dwb_controller/dwb_plugins/include/dwb_plugins/standard_traj_generator.hpp

@@ -70,12 +70,15 @@ class StandardTrajectoryGenerator : public dwb_core::TrajectoryGenerator
 
   /**
    * @brief Limits the maximum linear speed of the robot.
-   * @param speed_limit expressed in percentage from maximum robot speed.
+   * @param speed_limit expressed in absolute value (in m/s)
+   * or in percentage from maximum robot speed.
+   * @param percentage Setting speed limit in percentage if true
+   * or in absolute values in false case.
    */
-  void setSpeedLimit(const double & speed_limit) override
+  void setSpeedLimit(const double & speed_limit, const bool & percentage) override
   {
     if (kinematics_handler_) {
-      kinematics_handler_->setSpeedLimit(speed_limit);
+      kinematics_handler_->setSpeedLimit(speed_limit, percentage);
     }
   }
 

nav2_dwb_controller/dwb_plugins/src/kinematic_parameters.cpp

@@ -129,7 +129,8 @@ void KinematicsHandler::initialize(
   update_kinematics(kinematics);
 }
 
-void KinematicsHandler::setSpeedLimit(const double & speed_limit)
+void KinematicsHandler::setSpeedLimit(
+  const double & speed_limit, const bool & percentage)
 {
   KinematicParameters kinematics(*kinematics_.load());
 
@@ -140,11 +141,26 @@ void KinematicsHandler::setSpeedLimit(const double & speed_limit)
     kinematics.max_vel_y_ = kinematics.base_max_vel_y_;
     kinematics.max_vel_theta_ = kinematics.base_max_vel_theta_;
   } else {
-    // Speed limit is expressed in % from maximum speed of robot
-    kinematics.max_speed_xy_ = kinematics.base_max_speed_xy_ * speed_limit / 100.0;
-    kinematics.max_vel_x_ = kinematics.base_max_vel_x_ * speed_limit / 100.0;
-    kinematics.max_vel_y_ = kinematics.base_max_vel_y_ * speed_limit / 100.0;
-    kinematics.max_vel_theta_ = kinematics.base_max_vel_theta_ * speed_limit / 100.0;
+    if (percentage) {
+      // Speed limit is expressed in % from maximum speed of robot
+      kinematics.max_speed_xy_ = kinematics.base_max_speed_xy_ * speed_limit / 100.0;
+      kinematics.max_vel_x_ = kinematics.base_max_vel_x_ * speed_limit / 100.0;
+      kinematics.max_vel_y_ = kinematics.base_max_vel_y_ * speed_limit / 100.0;
+      kinematics.max_vel_theta_ = kinematics.base_max_vel_theta_ * speed_limit / 100.0;
+    } else {
+      // Speed limit is expressed in absolute value
+      if (speed_limit < kinematics.base_max_speed_xy_) {
+        kinematics.max_speed_xy_ = speed_limit;
+        // Handling components and angular velocity changes:
+        // Max velocities are being changed in the same proportion
+        // as absolute linear speed changed in order to preserve
+        // robot moving trajectories to be the same after speed change.
+        const double ratio = speed_limit / kinematics.base_max_speed_xy_;
+        kinematics.max_vel_x_ = kinematics.base_max_vel_x_ * ratio;
+        kinematics.max_vel_y_ = kinematics.base_max_vel_y_ * ratio;
+        kinematics.max_vel_theta_ = kinematics.base_max_vel_theta_ * ratio;
+      }
+    }
   }
 
   // Do not forget to update max_speed_xy_sq_ as well