[ros2-master] Fix build for last nav2 main branch (#274)

* build with current nav2 master

* setSpeedLimit override to conform to API, needs to be implemented

* build with new setSpeedLimit interface (todo: implementation)

See ros-navigation/navigation2#2149

* setSpeedLimit implementation

teb_local_planner/include/teb_local_planner/teb_config.h

@@ -92,6 +92,10 @@ class TebConfig
   //! Robot related parameters
   struct Robot
   {
+    double base_max_vel_x; //!< Maximum translational velocity of the robot before speed limit is applied
+    double base_max_vel_x_backwards; //!< Maximum translational velocity of the robot for driving backwards before speed limit is applied
+    double base_max_vel_y; //!< Maximum strafing velocity of the robot (should be zero for non-holonomic robots!) before speed limit is applied
+    double base_max_vel_theta; //!< Maximum angular velocity of the robot before speed limit is applied
     double max_vel_x; //!< Maximum translational velocity of the robot
     double max_vel_x_backwards; //!< Maximum translational velocity of the robot for driving backwards
     double max_vel_y; //!< Maximum strafing velocity of the robot (should be zero for non-holonomic robots!)
@@ -258,6 +262,10 @@ class TebConfig
     robot.max_vel_x_backwards = 0.2;
     robot.max_vel_y = 0.0;
     robot.max_vel_theta = 0.3;
+    robot.base_max_vel_x = robot.max_vel_x;
+    robot.base_max_vel_x_backwards = robot.base_max_vel_x_backwards;
+    robot.base_max_vel_y = robot.base_max_vel_y;
+    robot.base_max_vel_theta = robot.base_max_vel_theta;
     robot.acc_lim_x = 0.5;
     robot.acc_lim_y = 0.5;
     robot.acc_lim_theta = 0.5;

teb_local_planner/include/teb_local_planner/teb_local_planner_ros.h

@@ -66,6 +66,7 @@
 
 // costmap
 #include <costmap_converter/costmap_converter_interface.h>
+#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
 
 #include <nav2_util/lifecycle_node.hpp>
 #include <nav2_costmap_2d/costmap_2d_ros.hpp>
@@ -106,7 +107,7 @@ class TebLocalPlannerROS : public nav2_core::Controller
    * @param costmap_ros Cost map representing occupied and free space
    */
   void configure(
-    const rclcpp_lifecycle::LifecycleNode::WeakPtr & node,
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
     std::string name,
     const std::shared_ptr<tf2_ros::Buffer> & tf,
     const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> & costmap_ros) override;
@@ -348,10 +349,19 @@ class TebLocalPlannerROS : public nav2_core::Controller
 
   void configureBackupModes(std::vector<geometry_msgs::msg::PoseStamped>& transformed_plan,  int& goal_idx);
 
+  /**
+   * @brief Limits the maximum linear speed of the robot.
+   * @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,  const bool & percentage);
+
 private:
   // Definition of member variables
-  nav2_util::LifecycleNode::WeakPtr nh_;
-  rclcpp::Logger logger_;
+  rclcpp_lifecycle::LifecycleNode::WeakPtr nh_;
+  rclcpp::Logger logger_{rclcpp::get_logger("TEBLocalPlanner")};
   rclcpp::Clock::SharedPtr clock_;
   rclcpp::Node::SharedPtr intra_proc_node_;
   // external objects (store weak pointers)

teb_local_planner/src/teb_local_planner_ros.cpp

@@ -66,7 +66,7 @@ namespace teb_local_planner
 
 
 TebLocalPlannerROS::TebLocalPlannerROS() 
-    : nh_(nullptr), costmap_ros_(nullptr), tf_(nullptr), cfg_(new TebConfig()), costmap_model_(nullptr), intra_proc_node_(nullptr),
+    : costmap_ros_(nullptr), tf_(nullptr), cfg_(new TebConfig()), costmap_model_(nullptr), intra_proc_node_(nullptr),
                                            costmap_converter_loader_("costmap_converter", "costmap_converter::BaseCostmapToPolygons"),
                                            custom_via_points_active_(false), no_infeasible_plans_(0),
                                            last_preferred_rotdir_(RotType::none), initialized_(false)
@@ -201,11 +201,11 @@ void TebLocalPlannerROS::initialize(nav2_util::LifecycleNode::SharedPtr node)
 }
 
 void TebLocalPlannerROS::configure(
-    const rclcpp_lifecycle::LifecycleNode::WeakPtr & node,
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
     std::string name,
     const std::shared_ptr<tf2_ros::Buffer> & tf,
     const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> & costmap_ros) {
-  nh_ = node;
+  nh_ = parent;
 
   auto node = nh_.lock();
   logger_ = node->get_logger();
@@ -1008,7 +1008,43 @@ void TebLocalPlannerROS::configureBackupModes(std::vector<geometry_msgs::msg::Po
 
 }
 
-
+
+void TebLocalPlannerROS::setSpeedLimit(
+    const double & speed_limit, const bool & percentage)
+{
+  if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) {
+    // Restore default value
+    cfg_->robot.max_vel_x = cfg_->robot.base_max_vel_x;
+    cfg_->robot.base_max_vel_x_backwards = cfg_->robot.base_max_vel_x_backwards;
+    cfg_->robot.base_max_vel_y = cfg_->robot.base_max_vel_y;
+    cfg_->robot.base_max_vel_theta = cfg_->robot.base_max_vel_theta;
+  } else {
+    if (percentage) {
+      // Speed limit is expressed in % from maximum speed of robot
+      cfg_->robot.max_vel_x = cfg_->robot.base_max_vel_x * speed_limit / 100.0;
+      cfg_->robot.base_max_vel_x_backwards = cfg_->robot.base_max_vel_x_backwards * speed_limit / 100.0;
+      cfg_->robot.base_max_vel_y = cfg_->robot.base_max_vel_y * speed_limit / 100.0;
+      cfg_->robot.base_max_vel_theta = cfg_->robot.base_max_vel_theta * speed_limit / 100.0;
+    } else {
+      // Speed limit is expressed in absolute value
+      double max_speed_xy = std::max(
+            std::max(cfg_->robot.base_max_vel_x,cfg_->robot.base_max_vel_x_backwards),cfg_->robot.base_max_vel_y);
+      if (speed_limit < max_speed_xy) {
+        // 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.
+        // G. Doisy: not sure if that's applicable to base_max_vel_x_backwards.
+        const double ratio = speed_limit / max_speed_xy;
+        cfg_->robot.max_vel_x = cfg_->robot.base_max_vel_x * ratio;
+        cfg_->robot.base_max_vel_x_backwards = cfg_->robot.base_max_vel_x_backwards * ratio;
+        cfg_->robot.base_max_vel_y = cfg_->robot.base_max_vel_y * ratio;
+        cfg_->robot.base_max_vel_theta = cfg_->robot.base_max_vel_theta * ratio;
+      }
+    }
+  }
+}
+
 void TebLocalPlannerROS::customObstacleCB(const costmap_converter_msgs::msg::ObstacleArrayMsg::ConstSharedPtr obst_msg)
 {
   std::lock_guard<std::mutex> l(custom_obst_mutex_);