Speed Filter (ros-navigation#2074)

* Add speed limit filter

* Fix review items

* Remain only percent speed limit support

* Small review fixes

* Add error message for unknown cells on mask

* Fix missed items

* Ensure SpeedFilter is using accurate in time transform
instead of latest in TF Buffer

* Add costmap_filter_info_server into param description

* Set transform_tolerance to 0.1

* Update comment to be more informative

doc/parameters/param_list.md

@@ -178,8 +178,19 @@ When `plugins` parameter is not overridden, the following default plugins are lo
 | Parameter | Default | Description |
 | ----------| --------| ------------|
 | `<filter name>`.enabled | true | Whether it is enabled |
-| `<filter name>`.filter_info_topic | N/A | Name of the CostmapFilterInfo topic having filter-related information |
-| `<filter name>`.transform_tolerance | 0.0 | TF tolerance |
+| `<filter name>`.filter_info_topic | N/A | Name of the incoming CostmapFilterInfo topic having filter-related information |
+| `<filter name>`.transform_tolerance | 0.1 | TF tolerance |
+
+## speed filter
+
+* `<filter name>`: Name corresponding to the `nav2_costmap_2d::SpeedFilter` plugin. This name gets defined in `plugins`.
+
+| Parameter | Default | Description |
+| ----------| --------| ------------|
+| `<filter name>`.enabled | true | Whether it is enabled |
+| `<filter name>`.filter_info_topic | N/A | Name of the incoming CostmapFilterInfo topic having filter-related information |
+| `<filter name>`.speed_limit_topic | "speed_limit" | Topic to publish speed limit to |
+| `<filter name>`.transform_tolerance | 0.1 | TF tolerance |
 
 # controller_server
 
@@ -195,6 +206,7 @@ When `plugins` parameter is not overridden, the following default plugins are lo
 | min_x_velocity_threshold | 0.0001 | Minimum X velocity to use (m/s) |
 | min_y_velocity_threshold | 0.0001 | Minimum Y velocity to use (m/s) |
 | min_theta_velocity_threshold | 0.0001 | Minimum angular velocity to use (rad/s) |
+| speed_limit_topic | "speed_limit" | Speed limiting topic name to subscribe |
 
 **NOTE:** When `controller_plugins` parameter is overridden, each plugin namespace defined in the list needs to have a `plugin` parameter defining the type of plugin to be loaded in the namespace.
 
@@ -453,6 +465,16 @@ When `controller_plugins`\`progress_checker_plugin`\`goal_checker_plugin` parame
 | topic_name | "map" | topic  to publish loaded map to |
 | frame_id | "map" | Frame to publish loaded map in |
 
+## costmap_filter_info_server
+
+| 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) |
+| 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` |
+| multiplier | 1.0 | Multiplier of `OccupancyGrid` mask value -> filter space value linear conversion which is being proceeded as: `filter_space_value = base + multiplier * mask_value` |
+
 # planner_server
 
 | Parameter | Default | Description |

nav2_controller/include/nav2_controller/nav2_controller.hpp

@@ -27,6 +27,7 @@
 #include "nav2_costmap_2d/costmap_2d_ros.hpp"
 #include "tf2_ros/transform_listener.h"
 #include "nav2_msgs/action/follow_path.hpp"
+#include "nav2_msgs/msg/speed_limit.hpp"
 #include "nav_2d_utils/odom_subscriber.hpp"
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav2_util/simple_action_server.hpp"
@@ -196,6 +197,7 @@ class ControllerServer : public nav2_util::LifecycleNode
   // Publishers and subscribers
   std::unique_ptr<nav_2d_utils::OdomSubscriber> odom_sub_;
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr vel_publisher_;
+  rclcpp::Subscription<nav2_msgs::msg::SpeedLimit>::SharedPtr speed_limit_sub_;
 
   // Progress Checker Plugin
   pluginlib::ClassLoader<nav2_core::ProgressChecker> progress_checker_loader_;
@@ -229,6 +231,13 @@ class ControllerServer : public nav2_util::LifecycleNode
 
   // Whether we've published the single controller warning yet
   geometry_msgs::msg::Pose end_pose_;
+
+private:
+  /**
+    * @brief Callback for speed limiting messages
+    * @param msg Shared pointer to nav2_msgs::msg::SpeedLimit
+    */
+  void speedLimitCallback(const nav2_msgs::msg::SpeedLimit::SharedPtr msg);
 };
 
 }  // namespace nav2_controller

nav2_controller/src/nav2_controller.cpp

@@ -53,6 +53,8 @@ ControllerServer::ControllerServer()
   declare_parameter("min_y_velocity_threshold", rclcpp::ParameterValue(0.0001));
   declare_parameter("min_theta_velocity_threshold", rclcpp::ParameterValue(0.0001));
 
+  declare_parameter("speed_limit_topic", rclcpp::ParameterValue("speed_limit"));
+
   // The costmap node is used in the implementation of the controller
   costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
     "local_costmap", std::string{get_namespace()}, "local_costmap");
@@ -105,6 +107,9 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
   get_parameter("min_theta_velocity_threshold", min_theta_velocity_threshold_);
   RCLCPP_INFO(get_logger(), "Controller frequency set to %.4fHz", controller_frequency_);
 
+  std::string speed_limit_topic;
+  get_parameter("speed_limit_topic", speed_limit_topic);
+
   costmap_ros_->on_configure(state);
 
   try {
@@ -170,6 +175,11 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)
     rclcpp_node_, "follow_path",
     std::bind(&ControllerServer::computeControl, this));
 
+  // Set subscribtion to the speed limiting topic
+  speed_limit_sub_ = create_subscription<nav2_msgs::msg::SpeedLimit>(
+    speed_limit_topic, rclcpp::QoS(10),
+    std::bind(&ControllerServer::speedLimitCallback, this, std::placeholders::_1));
+
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -230,6 +240,7 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & state)
   action_server_.reset();
   odom_sub_.reset();
   vel_publisher_.reset();
+  speed_limit_sub_.reset();
   action_server_.reset();
   goal_checker_->reset();
 
@@ -452,4 +463,16 @@ bool ControllerServer::getRobotPose(geometry_msgs::msg::PoseStamped & pose)
   return true;
 }
 
+void ControllerServer::speedLimitCallback(const nav2_msgs::msg::SpeedLimit::SharedPtr msg)
+{
+  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);
+  }
+}
+
 }  // namespace nav2_controller

nav2_core/include/nav2_core/controller.hpp

@@ -112,6 +112,12 @@ class Controller
   virtual geometry_msgs::msg::TwistStamped computeVelocityCommands(
     const geometry_msgs::msg::PoseStamped & pose,
     const geometry_msgs::msg::Twist & velocity) = 0;
+
+  /**
+   * @brief Limits the maximum linear speed of the robot.
+   * @param speed_limit expressed in percentage from maximum robot speed.
+   */
+  virtual void setSpeedLimit(const double & speed_limit) = 0;
 };
 
 }  // namespace nav2_core

nav2_costmap_2d/CMakeLists.txt

@@ -98,6 +98,7 @@ target_link_libraries(layers
 
 add_library(filters SHARED
   plugins/costmap_filters/keepout_filter.cpp
+  plugins/costmap_filters/speed_filter.cpp
 )
 ament_target_dependencies(filters
   ${dependencies}

nav2_costmap_2d/costmap_plugins.xml

@@ -18,9 +18,12 @@
   </library>
 
   <library path="filters">
-    <class type="nav2_costmap_2d::KeepoutFilter"  base_class_type="nav2_costmap_2d::Layer">
+    <class type="nav2_costmap_2d::KeepoutFilter"   base_class_type="nav2_costmap_2d::Layer">
       <description>Prevents the robot from appearing in keepout zones marked on map.</description>
     </class>
+    <class type="nav2_costmap_2d::SpeedFilter"     base_class_type="nav2_costmap_2d::Layer">
+      <description>Restricts maximum speed of robot in speed-limit zones marked on map.</description>
+    </class>
   </library>
 </class_libraries>
 

nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp

@@ -47,9 +47,6 @@
 namespace nav2_costmap_2d
 {
 
-static constexpr double BASE_DEFAULT = 0.0;
-static constexpr double MULTIPLIER_DEFAULT = 1.0;
-
 /**
  * @brief: CostmapFilter basic class. It is inherited from Layer in order to avoid
  * hidden problems when the shared handling of costmap_ resource (PR #1936)

nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/filter_values.hpp

@@ -0,0 +1,62 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2020 Samsung Research Russia
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the <ORGANIZATION> nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Alexey Merzlyakov
+ *********************************************************************/
+
+#ifndef NAV2_COSTMAP_2D__FILTER_VALUES_HPP_
+#define NAV2_COSTMAP_2D__FILTER_VALUES_HPP_
+
+/** Provides constants used in costmap filters */
+
+namespace nav2_costmap_2d
+{
+
+/** Types of costmap filter */
+static constexpr uint8_t KEEPOUT_FILTER = 0;
+static constexpr uint8_t SPEED_FILTER_PERCENT = 1;
+static constexpr uint8_t SPEED_FILTER_ABSOLUTE = 2;
+
+/** Default values for base and multiplier */
+static constexpr double BASE_DEFAULT = 0.0;
+static constexpr double MULTIPLIER_DEFAULT = 1.0;
+
+/** Speed filter constants */
+static constexpr int8_t SPEED_MASK_UNKNOWN = -1;
+static constexpr int8_t SPEED_MASK_NO_LIMIT = 0;
+static constexpr double NO_SPEED_LIMIT = 0.0;
+
+}  // namespace nav2_costmap_2d
+
+#endif  // NAV2_COSTMAP_2D__FILTER_VALUES_HPP_

nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp

@@ -0,0 +1,120 @@
+/*********************************************************************
+ *
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2020 Samsung Research Russia
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the <ORGANIZATION> nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ * Author: Alexey Merzlyakov
+ *********************************************************************/
+
+#ifndef NAV2_COSTMAP_2D__SPEED_FILTER_HPP_
+#define NAV2_COSTMAP_2D__SPEED_FILTER_HPP_
+
+#include "nav2_costmap_2d/costmap_filters/costmap_filter.hpp"
+
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "nav2_msgs/msg/costmap_filter_info.hpp"
+#include "nav2_msgs/msg/speed_limit.hpp"
+
+#include <memory>
+
+namespace nav2_costmap_2d
+{
+
+class SpeedFilter : public CostmapFilter
+{
+public:
+  SpeedFilter();
+
+  void initializeFilter(
+    const std::string & filter_info_topic);
+
+  void process(
+    nav2_costmap_2d::Costmap2D & master_grid,
+    int min_i, int min_j, int max_i, int max_j,
+    const geometry_msgs::msg::Pose2D & pose);
+
+  void resetFilter();
+
+  bool isActive();
+
+protected:
+  /**
+   * @brief:  Transforms robot pose from current layer frame to mask frame
+   * @param:  pose Robot pose in costmap frame
+   * @param:  mask_pose Robot pose in mask frame
+   * @return: True if the transformation was successful, false otherwise
+   */
+  bool transformPose(
+    const geometry_msgs::msg::Pose2D & pose,
+    geometry_msgs::msg::Pose2D & mask_pose) const;
+
+  /**
+   * @brief: Convert from world coordinates to mask coordinates.
+     Similar to Costmap2D::worldToMap() method but works directly with OccupancyGrid-s.
+   * @param  wx The x world coordinate
+   * @param  wy The y world coordinate
+   * @param  mx Will be set to the associated mask x coordinate
+   * @param  my Will be set to the associated mask y coordinate
+   * @return True if the conversion was successful (legal bounds) false otherwise
+   */
+  bool worldToMask(double wx, double wy, unsigned int & mx, unsigned int & my) const;
+
+  /**
+   * @brief  Get the data of a cell in the filter mask
+   * @param  mx The x coordinate of the cell
+   * @param  my The y coordinate of the cell
+   * @return The data of the selected cell
+   */
+  inline int8_t getMaskData(
+    const unsigned int mx, const unsigned int my) const;
+
+private:
+  void filterInfoCallback(const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg);
+  void maskCallback(const nav_msgs::msg::OccupancyGrid::SharedPtr msg);
+
+  rclcpp::Subscription<nav2_msgs::msg::CostmapFilterInfo>::SharedPtr filter_info_sub_;
+  rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::SharedPtr mask_sub_;
+
+  rclcpp_lifecycle::LifecyclePublisher<nav2_msgs::msg::SpeedLimit>::SharedPtr speed_limit_pub_;
+
+  nav_msgs::msg::OccupancyGrid::SharedPtr filter_mask_;
+
+  std::string mask_frame_;  // Frame where mask located in
+  std::string global_frame_;  // Frame of currnet layer (master_grid)
+
+  double base_, multiplier_;
+  double speed_limit_, speed_limit_prev_;
+};
+
+}  // namespace nav2_costmap_2d
+
+#endif  // NAV2_COSTMAP_2D__SPEED_FILTER_HPP_

nav2_costmap_2d/plugins/costmap_filters/costmap_filter.cpp

@@ -60,21 +60,20 @@ void CostmapFilter::onInitialize()
     throw std::runtime_error{"Failed to lock node"};
   }
 
-  // Declare parameters
+  // Declare common for all costmap filters parameters
   declareParameter("enabled", rclcpp::ParameterValue(true));
   declareParameter("filter_info_topic");
-  declareParameter("transform_tolerance", rclcpp::ParameterValue(0.0));
+  declareParameter("transform_tolerance", rclcpp::ParameterValue(0.1));
 
   // Get parameters
   node->get_parameter(name_ + "." + "enabled", enabled_);
   try {
     filter_info_topic_ = node->get_parameter(name_ + "." + "filter_info_topic").as_string();
   } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) {
-    RCLCPP_ERROR(node->get_logger(), "filter_info_topic parameter is not set");
+    RCLCPP_ERROR(logger_, "filter_info_topic parameter is not set");
     throw ex;
   }
   double transform_tolerance;
-  // This is global for whole costmap parameter
   node->get_parameter(name_ + "." + "transform_tolerance", transform_tolerance);
   transform_tolerance_ = tf2::durationFromSec(transform_tolerance);
 }