From 7a5b5461af373b037d8a34fab2711cef37970f8c Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 24 Oct 2022 14:11:17 -0400 Subject: [PATCH 01/43] publish layers of costmap --- .../nav2_costmap_2d/costmap_2d_ros.hpp | 4 +- .../costmap_filters/costmap_filter.hpp | 6 +++ .../nav2_costmap_2d/inflation_layer.hpp | 5 ++ .../include/nav2_costmap_2d/layer.hpp | 5 ++ .../nav2_costmap_2d/obstacle_layer.hpp | 5 ++ .../nav2_costmap_2d/range_sensor_layer.hpp | 5 ++ .../include/nav2_costmap_2d/static_layer.hpp | 5 ++ .../include/nav2_costmap_2d/voxel_layer.hpp | 5 ++ nav2_costmap_2d/src/costmap_2d_ros.cpp | 50 ++++++++++++++++++- .../test/unit/declare_parameter_test.cpp | 1 + 10 files changed, 88 insertions(+), 3 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index 968bd9b8a4..305cb16408 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -319,7 +319,9 @@ class Costmap2DROS : public nav2_util::LifecycleNode // Publishers and subscribers rclcpp_lifecycle::LifecyclePublisher::SharedPtr footprint_pub_; - std::unique_ptr costmap_publisher_{nullptr}; + std::unique_ptr costmap_publisher_; + + std::vector> layer_publishers_; rclcpp::Subscription::SharedPtr footprint_sub_; rclcpp::Subscription::SharedPtr parameter_sub_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp index 35f53c6b96..c7095154d3 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp @@ -126,6 +126,12 @@ class CostmapFilter : public Layer */ bool isClearable() {return false;} + /** + * @brief If this layer can be published or not + */ + virtual bool isPublishable() {return false;} + + /** CostmapFilter API **/ /** * @brief: Initializes costmap filter. Creates subscriptions to filter-related topics diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp index 5f68dcaed1..ba97c48317 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -133,6 +133,11 @@ class InflationLayer : public Layer */ virtual bool isClearable() {return false;} + /** + * @brief If this layer can be published or not + */ + virtual bool isPublishable() {return false;} + /** * @brief Reset this costmap */ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp index 72125143ba..413ee5f5be 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp @@ -89,6 +89,11 @@ class Layer */ virtual bool isClearable() = 0; + /** + * @brief If publishing operations should be processed on this layer or not + */ + virtual bool isPublishable() = 0; + /** * @brief This is called by the LayeredCostmap to poll this plugin as to how * much of the costmap it needs to update. Each layer can increase diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp index 754b434930..6c1529cdbb 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp @@ -131,6 +131,11 @@ class ObstacleLayer : public CostmapLayer */ virtual bool isClearable() {return true;} + /** + * @brief If this layer can be published or not + */ + virtual bool isPublishable() {return true;} + /** * @brief Callback executed when a parameter change is detected * @param event ParameterEvent message diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp index 71cd4654a9..38f82aae06 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp @@ -123,6 +123,11 @@ class RangeSensorLayer : public CostmapLayer */ virtual bool isClearable() {return true;} + /** + * @brief If this layer can be published or not + */ + virtual bool isPublishable() {return true;} + /** * @brief Handle an incoming Range message to populate into costmap */ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index aec68fac1f..37204313b9 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -92,6 +92,11 @@ class StaticLayer : public CostmapLayer */ virtual bool isClearable() {return false;} + /** + * @brief If this layer can be published or not + */ + virtual bool isPublishable() {return false;} + /** * @brief Update the bounds of the master costmap by this layer's update dimensions * @param robot_x X pose of robot diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp index 14baefcbdd..6c5b8fef8b 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp @@ -127,6 +127,11 @@ class VoxelLayer : public ObstacleLayer */ virtual bool isClearable() {return true;} + /** + * @brief If this layer can be published or not + */ + virtual bool isPublishable() {return true;} + protected: /** * @brief Reset internal maps diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 50e74c511b..9f407620ce 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -210,6 +210,22 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) layered_costmap_->getCostmap(), global_frame_, "costmap", always_send_full_costmap_); + auto layers = layered_costmap_->getPlugins(); + + for (auto & layer : *layers) { + if (layer->isPublishable()) { + auto costmap_layer = std::static_pointer_cast(layer); + layer_publishers_.emplace_back( + std::make_unique( + shared_from_this(), + costmap_layer.get(), global_frame_, + layer->getName(), always_send_full_costmap_) + ); + } + } + + + // Set the footprint if (use_radius_) { setRobotFootprint(makeFootprintFromRadius(robot_radius_)); @@ -233,8 +249,13 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - costmap_publisher_->on_activate(); footprint_pub_->on_activate(); + costmap_publisher_->on_activate(); + + for (auto& layer_pub : layer_publishers_) + { + layer_pub->on_activate(); + } // First, make sure that the transform between the robot base frame // and the global frame is available @@ -280,8 +301,14 @@ Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Deactivating"); dyn_params_handler.reset(); - costmap_publisher_->on_deactivate(); + footprint_pub_->on_deactivate(); + costmap_publisher_->on_deactivate(); + + for (auto& layer_pub : layer_publishers_) + { + layer_pub->on_deactivate(); + } stop(); @@ -308,6 +335,11 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) costmap_publisher_.reset(); clear_costmap_service_.reset(); + for (auto& layer_pub : layer_publishers_) + { + layer_pub.reset(); + } + executor_thread_.reset(); return nav2_util::CallbackReturn::SUCCESS; } @@ -446,12 +478,24 @@ Costmap2DROS::mapUpdateLoop(double frequency) layered_costmap_->getBounds(&x0, &xn, &y0, &yn); costmap_publisher_->updateBounds(x0, xn, y0, yn); + //TODO (jwallace42): need bounds of each costmap layer? + for (auto& layer_pub : layer_publishers_) + { + layer_pub->updateBounds(x0, xn, y0, yn); + } + auto current_time = now(); if ((last_publish_ + publish_cycle_ < current_time) || // publish_cycle_ is due (current_time < last_publish_)) // time has moved backwards, probably due to a switch to sim_time // NOLINT { RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str()); costmap_publisher_->publishCostmap(); + + for (auto& layer_pub : layer_publishers_) + { + layer_pub->publishCostmap(); + } + last_publish_ = current_time; } } @@ -720,4 +764,6 @@ Costmap2DROS::dynamicParametersCallback(std::vector parameter return result; } + + } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/test/unit/declare_parameter_test.cpp b/nav2_costmap_2d/test/unit/declare_parameter_test.cpp index a8a717c8f9..caed25b4d6 100644 --- a/nav2_costmap_2d/test/unit/declare_parameter_test.cpp +++ b/nav2_costmap_2d/test/unit/declare_parameter_test.cpp @@ -34,6 +34,7 @@ class LayerWrapper : public nav2_costmap_2d::Layer void updateBounds(double, double, double, double *, double *, double *, double *) {} void updateCosts(nav2_costmap_2d::Costmap2D &, int, int, int, int) {} bool isClearable() {return false;} + bool isPublishable() {return false;} }; TEST(DeclareParameter, useValidParameter) From 0f2e97b3e8d26d00fe41c52908786bc0ec906058 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 24 Oct 2022 14:23:45 -0400 Subject: [PATCH 02/43] lint fix --- .../costmap_filters/costmap_filter.hpp | 1 + nav2_costmap_2d/src/costmap_2d_ros.cpp | 33 +++++++------------ 2 files changed, 13 insertions(+), 21 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp index c7095154d3..91e6a08460 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp @@ -40,6 +40,7 @@ #include #include +#include #include "geometry_msgs/msg/pose2_d.hpp" #include "std_srvs/srv/set_bool.hpp" diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 9f407620ce..450d433db6 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -216,16 +216,14 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) if (layer->isPublishable()) { auto costmap_layer = std::static_pointer_cast(layer); layer_publishers_.emplace_back( - std::make_unique( - shared_from_this(), - costmap_layer.get(), global_frame_, - layer->getName(), always_send_full_costmap_) - ); + std::make_unique( + shared_from_this(), + costmap_layer.get(), global_frame_, + layer->getName(), always_send_full_costmap_) + ); } } - - // Set the footprint if (use_radius_) { setRobotFootprint(makeFootprintFromRadius(robot_radius_)); @@ -252,8 +250,7 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/) footprint_pub_->on_activate(); costmap_publisher_->on_activate(); - for (auto& layer_pub : layer_publishers_) - { + for (auto & layer_pub : layer_publishers_) { layer_pub->on_activate(); } @@ -305,8 +302,7 @@ Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /*state*/) footprint_pub_->on_deactivate(); costmap_publisher_->on_deactivate(); - for (auto& layer_pub : layer_publishers_) - { + for (auto & layer_pub : layer_publishers_) { layer_pub->on_deactivate(); } @@ -335,8 +331,7 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) costmap_publisher_.reset(); clear_costmap_service_.reset(); - for (auto& layer_pub : layer_publishers_) - { + for (auto & layer_pub : layer_publishers_) { layer_pub.reset(); } @@ -478,9 +473,8 @@ Costmap2DROS::mapUpdateLoop(double frequency) layered_costmap_->getBounds(&x0, &xn, &y0, &yn); costmap_publisher_->updateBounds(x0, xn, y0, yn); - //TODO (jwallace42): need bounds of each costmap layer? - for (auto& layer_pub : layer_publishers_) - { + // TODO(jwallace42): need bounds of each costmap layer? + for (auto & layer_pub : layer_publishers_) { layer_pub->updateBounds(x0, xn, y0, yn); } @@ -491,8 +485,7 @@ Costmap2DROS::mapUpdateLoop(double frequency) RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str()); costmap_publisher_->publishCostmap(); - for (auto& layer_pub : layer_publishers_) - { + for (auto & layer_pub : layer_publishers_) { layer_pub->publishCostmap(); } @@ -764,6 +757,4 @@ Costmap2DROS::dynamicParametersCallback(std::vector parameter return result; } - - -} // namespace nav2_costmap_2d +} // namespace nav2_costmap_2d \ No newline at end of file From 7d950ebffe025a40b60bee09ccf734d26f9fd74f Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 24 Oct 2022 17:44:05 -0400 Subject: [PATCH 03/43] lint round 2 :) --- nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp | 2 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp index 413ee5f5be..dbe90204ef 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp @@ -92,7 +92,7 @@ class Layer /** * @brief If publishing operations should be processed on this layer or not */ - virtual bool isPublishable() = 0; + virtual bool isPublishable() = 0; /** * @brief This is called by the LayeredCostmap to poll this plugin as to how diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 450d433db6..8d8d76a20d 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -757,4 +757,4 @@ Costmap2DROS::dynamicParametersCallback(std::vector parameter return result; } -} // namespace nav2_costmap_2d \ No newline at end of file +} // namespace nav2_costmap_2d From 82481ab28666db65f3113161619adb5d0ee9365a Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 26 Oct 2022 15:12:07 -0400 Subject: [PATCH 04/43] code review --- nav2_costmap_2d/src/costmap_2d_ros.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 8d8d76a20d..3468069450 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -473,7 +473,6 @@ Costmap2DROS::mapUpdateLoop(double frequency) layered_costmap_->getBounds(&x0, &xn, &y0, &yn); costmap_publisher_->updateBounds(x0, xn, y0, yn); - // TODO(jwallace42): need bounds of each costmap layer? for (auto & layer_pub : layer_publishers_) { layer_pub->updateBounds(x0, xn, y0, yn); } From 447f80f9b62d91d6ccc0a2aaeef743c6e6e74738 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 2 Nov 2022 10:44:10 -0400 Subject: [PATCH 05/43] remove isPublishable --- .../costmap_filters/costmap_filter.hpp | 6 ------ .../include/nav2_costmap_2d/inflation_layer.hpp | 5 ----- nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp | 5 ----- .../include/nav2_costmap_2d/obstacle_layer.hpp | 5 ----- .../include/nav2_costmap_2d/range_sensor_layer.hpp | 5 ----- .../include/nav2_costmap_2d/static_layer.hpp | 5 ----- .../include/nav2_costmap_2d/voxel_layer.hpp | 5 ----- nav2_costmap_2d/src/costmap_2d_ros.cpp | 13 ++++++------- .../test/unit/declare_parameter_test.cpp | 1 - 9 files changed, 6 insertions(+), 44 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp index 91e6a08460..eb1630be26 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp @@ -127,12 +127,6 @@ class CostmapFilter : public Layer */ bool isClearable() {return false;} - /** - * @brief If this layer can be published or not - */ - virtual bool isPublishable() {return false;} - - /** CostmapFilter API **/ /** * @brief: Initializes costmap filter. Creates subscriptions to filter-related topics diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp index ba97c48317..5f68dcaed1 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -133,11 +133,6 @@ class InflationLayer : public Layer */ virtual bool isClearable() {return false;} - /** - * @brief If this layer can be published or not - */ - virtual bool isPublishable() {return false;} - /** * @brief Reset this costmap */ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp index dbe90204ef..72125143ba 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp @@ -89,11 +89,6 @@ class Layer */ virtual bool isClearable() = 0; - /** - * @brief If publishing operations should be processed on this layer or not - */ - virtual bool isPublishable() = 0; - /** * @brief This is called by the LayeredCostmap to poll this plugin as to how * much of the costmap it needs to update. Each layer can increase diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp index 6c1529cdbb..754b434930 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp @@ -131,11 +131,6 @@ class ObstacleLayer : public CostmapLayer */ virtual bool isClearable() {return true;} - /** - * @brief If this layer can be published or not - */ - virtual bool isPublishable() {return true;} - /** * @brief Callback executed when a parameter change is detected * @param event ParameterEvent message diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp index 38f82aae06..71cd4654a9 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/range_sensor_layer.hpp @@ -123,11 +123,6 @@ class RangeSensorLayer : public CostmapLayer */ virtual bool isClearable() {return true;} - /** - * @brief If this layer can be published or not - */ - virtual bool isPublishable() {return true;} - /** * @brief Handle an incoming Range message to populate into costmap */ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index 37204313b9..aec68fac1f 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -92,11 +92,6 @@ class StaticLayer : public CostmapLayer */ virtual bool isClearable() {return false;} - /** - * @brief If this layer can be published or not - */ - virtual bool isPublishable() {return false;} - /** * @brief Update the bounds of the master costmap by this layer's update dimensions * @param robot_x X pose of robot diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp index 6c5b8fef8b..14baefcbdd 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp @@ -127,11 +127,6 @@ class VoxelLayer : public ObstacleLayer */ virtual bool isClearable() {return true;} - /** - * @brief If this layer can be published or not - */ - virtual bool isPublishable() {return true;} - protected: /** * @brief Reset internal maps diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 3468069450..8d1ba15b4e 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -213,14 +213,13 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) auto layers = layered_costmap_->getPlugins(); for (auto & layer : *layers) { - if (layer->isPublishable()) { - auto costmap_layer = std::static_pointer_cast(layer); + auto costmap_layer = std::dynamic_pointer_cast(layer); + if (costmap_layer != nullptr) { layer_publishers_.emplace_back( - std::make_unique( - shared_from_this(), - costmap_layer.get(), global_frame_, - layer->getName(), always_send_full_costmap_) - ); + std::make_unique( + shared_from_this(), + costmap_layer.get(), global_frame_, + layer->getName(), always_send_full_costmap_)); } } diff --git a/nav2_costmap_2d/test/unit/declare_parameter_test.cpp b/nav2_costmap_2d/test/unit/declare_parameter_test.cpp index caed25b4d6..a8a717c8f9 100644 --- a/nav2_costmap_2d/test/unit/declare_parameter_test.cpp +++ b/nav2_costmap_2d/test/unit/declare_parameter_test.cpp @@ -34,7 +34,6 @@ class LayerWrapper : public nav2_costmap_2d::Layer void updateBounds(double, double, double, double *, double *, double *, double *) {} void updateCosts(nav2_costmap_2d::Costmap2D &, int, int, int, int) {} bool isClearable() {return false;} - bool isPublishable() {return false;} }; TEST(DeclareParameter, useValidParameter) From 11459a1a9060606c3ae292a2aa0534eabafe4f7d Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 4 Nov 2022 10:31:07 -0400 Subject: [PATCH 06/43] lint --- nav2_costmap_2d/src/costmap_2d_ros.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 8d1ba15b4e..d5e8eef093 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -216,10 +216,10 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) auto costmap_layer = std::dynamic_pointer_cast(layer); if (costmap_layer != nullptr) { layer_publishers_.emplace_back( - std::make_unique( - shared_from_this(), - costmap_layer.get(), global_frame_, - layer->getName(), always_send_full_costmap_)); + std::make_unique( + shared_from_this(), + costmap_layer.get(), global_frame_, + layer->getName(), always_send_full_costmap_)); } } From 4a222d62a591a1fc58e558f6bca0f809e25d8cb4 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 7 Nov 2022 15:07:47 -0500 Subject: [PATCH 07/43] test running --- .../integration/test_costmap_2d_publisher.cpp | 48 +++++++++++++++++++ 1 file changed, 48 insertions(+) create mode 100644 nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp diff --git a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp new file mode 100644 index 0000000000..38bc6c2e41 --- /dev/null +++ b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp @@ -0,0 +1,48 @@ +// +// Created by josh on 11/7/22. +// + +// Copyright (c) 2022 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include + +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_2d_publisher.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +//#include "../testing_helper.hpp" + +//class DummyCostmap2DLayer : public nav2_costmap_2d::Costmap2D +//{ +//public: +// DummyCostmap2DLayer() {} +//}; + +TEST(costmap_publisher_test, dummy_test) +{ + EXPECT_EQ(1, 1); +} + +//int main(int argc, char ** argv) +//{ +// // Create a a fake costmap +// nav2_costmap_2d::Costmap2D costmap_2d(100, 100, 0.05, 0, 0, nav2_costmap_2d::FREE_SPACE); +// costmap_2d.setCost(5, 5, nav2_costmap_2d::LETHAL_OBSTACLE); +// +//// printMap(costmap_2d); +// +// +// testing::InitGoogleTest(&argc, argv); +// return RUN_ALL_TESTS(); +//} \ No newline at end of file From 0285d20e10ffaa6630ced4f8574902a97e17d84a Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 7 Nov 2022 17:00:38 -0500 Subject: [PATCH 08/43] rough structure complete --- nav2_costmap_2d/src/costmap_2d_publisher.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index 906a9f0324..e571d576bd 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -183,6 +183,7 @@ void Costmap2DPublisher::prepareCostmap() void Costmap2DPublisher::publishCostmap() { + RCLCPP_INFO(logger_, "RUNNNUNUNUNNUN"); if (costmap_raw_pub_->get_subscription_count() > 0) { prepareCostmap(); costmap_raw_pub_->publish(std::move(costmap_raw_)); From c52243e5f86d6c69d2fc51e48cd24d3d0eafd8f1 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 15 Nov 2022 15:35:23 -0500 Subject: [PATCH 09/43] completed test --- .../test/integration/CMakeLists.txt | 22 +++ .../integration/test_costmap_2d_publisher.cpp | 150 +++++++++++++++--- 2 files changed, 151 insertions(+), 21 deletions(-) diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index ef0b148440..519643a0a5 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -64,6 +64,18 @@ target_link_libraries(dyn_params_tests nav2_costmap_2d_core ) +ament_add_gtest_executable(test_costmap_publisher_exec + test_costmap_2d_publisher.cpp +) +ament_target_dependencies(test_costmap_publisher_exec + ${dependencies} +) +target_link_libraries(test_costmap_publisher_exec + nav2_costmap_2d_core + nav2_costmap_2d_client + layers +) + ament_add_test(test_collision_checker GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" @@ -114,6 +126,16 @@ ament_add_test(range_tests TEST_EXECUTABLE=$ ) +ament_add_test(test_costmap_publisher_exec + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + ENV + TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml + TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} + TEST_EXECUTABLE=$ + ) + ## TODO(bpwilcox): this test (I believe) is intended to be launched with the simple_driving_test.xml, ## which has a dependency on rosbag playback # ament_add_gtest_executable(costmap_tester diff --git a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp index 38bc6c2e41..9331dafea7 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp @@ -18,31 +18,139 @@ #include +#include + #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/costmap_2d_publisher.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_costmap_2d/cost_values.hpp" -//#include "../testing_helper.hpp" -//class DummyCostmap2DLayer : public nav2_costmap_2d::Costmap2D -//{ -//public: -// DummyCostmap2DLayer() {} -//}; +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; -TEST(costmap_publisher_test, dummy_test) +class TestCostmap2dPublisher : public nav2_util::LifecycleNode { - EXPECT_EQ(1, 1); -} +public: + explicit TestCostmap2dPublisher(std::string name) + : LifecycleNode(name) + { + RCLCPP_INFO(get_logger(), "Constructing"); + } -//int main(int argc, char ** argv) -//{ -// // Create a a fake costmap -// nav2_costmap_2d::Costmap2D costmap_2d(100, 100, 0.05, 0, 0, nav2_costmap_2d::FREE_SPACE); -// costmap_2d.setCost(5, 5, nav2_costmap_2d::LETHAL_OBSTACLE); -// -//// printMap(costmap_2d); -// -// -// testing::InitGoogleTest(&argc, argv); -// return RUN_ALL_TESTS(); -//} \ No newline at end of file + ~TestCostmap2dPublisher() {} + + nav2_util::CallbackReturn + on_configure(const rclcpp_lifecycle::State &) + { + RCLCPP_INFO(get_logger(), "Configuring"); + + callback_group_ = create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + + costmap_ = std::make_shared( + 100, 100, 0.05, 0, 0, + nav2_costmap_2d::LETHAL_OBSTACLE); + + costmap_pub_ = std::make_shared( + shared_from_this(), + costmap_.get(), + "map", + "dummy_costmap", + true); + + rclcpp::SubscriptionOptions sub_option; + sub_option.callback_group = callback_group_; + + std::string topic_name = "dummy_costmap_raw"; + costmap_sub_ = this->create_subscription( + topic_name, + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&TestCostmap2dPublisher::costmapCallback, this, std::placeholders::_1), + sub_option); + + executor_ = std::make_shared(); + executor_->add_callback_group(callback_group_, get_node_base_interface()); + executor_thread_ = std::make_unique(executor_); + + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn + on_activate(const rclcpp_lifecycle::State &) + { + RCLCPP_INFO(get_logger(), "Activating"); + costmap_pub_->on_activate(); + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State &) + { + RCLCPP_INFO(get_logger(), "Deactivating"); + return nav2_util::CallbackReturn::SUCCESS; + } + + nav2_util::CallbackReturn + on_cleanup(const rclcpp_lifecycle::State &) + { + executor_thread_.reset(); + return nav2_util::CallbackReturn::SUCCESS; + } + + void publishCostmap() + { + costmap_pub_->publishCostmap(); + } + + void costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr costmap) + { + promise_.set_value(costmap); + } + + std::shared_ptr costmap_pub_; + rclcpp::Subscription::SharedPtr costmap_sub_; + std::shared_ptr costmap_; + + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + std::unique_ptr executor_thread_; + std::promise promise_; +}; + +class TestNode : public ::testing::Test +{ +public: + TestNode() + { + costmap_publisher_ = std::make_shared("test_costmap_publisher"); + costmap_publisher_->on_configure(costmap_publisher_->get_current_state()); + costmap_publisher_->on_activate(costmap_publisher_->get_current_state()); + } + + ~TestNode() + { + costmap_publisher_->on_deactivate(costmap_publisher_->get_current_state()); + costmap_publisher_->on_cleanup(costmap_publisher_->get_current_state()); + } + +protected: + std::shared_ptr costmap_publisher_; +}; + +TEST_F(TestNode, costmap_pub_test) +{ + costmap_publisher_->publishCostmap(); + auto future = costmap_publisher_->promise_.get_future(); + auto status = future.wait_for(std::chrono::seconds(5)); + EXPECT_TRUE(status == std::future_status::ready); + + auto costmap_raw = future.get(); + for (const auto cost : costmap_raw->data) { + EXPECT_TRUE(cost == nav2_costmap_2d::LETHAL_OBSTACLE); + } +} From f184acf7b2c13b0bdf93813f1deba44133ad6af3 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 15 Nov 2022 18:01:53 -0500 Subject: [PATCH 10/43] lint --- nav2_costmap_2d/test/integration/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index 519643a0a5..0768972a0b 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -134,7 +134,7 @@ ament_add_test(test_costmap_publisher_exec TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} TEST_EXECUTABLE=$ - ) +) ## TODO(bpwilcox): this test (I believe) is intended to be launched with the simple_driving_test.xml, ## which has a dependency on rosbag playback From b09ce66110a235411c5023be8bd5acb9f6eff9f8 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 15 Nov 2022 18:04:13 -0500 Subject: [PATCH 11/43] code review --- nav2_costmap_2d/src/costmap_2d_publisher.cpp | 1 - .../test/integration/test_costmap_2d_publisher.cpp | 4 ---- 2 files changed, 5 deletions(-) diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index e571d576bd..906a9f0324 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -183,7 +183,6 @@ void Costmap2DPublisher::prepareCostmap() void Costmap2DPublisher::publishCostmap() { - RCLCPP_INFO(logger_, "RUNNNUNUNUNNUN"); if (costmap_raw_pub_->get_subscription_count() > 0) { prepareCostmap(); costmap_raw_pub_->publish(std::move(costmap_raw_)); diff --git a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp index 9331dafea7..ad5a4ad370 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp @@ -1,7 +1,3 @@ -// -// Created by josh on 11/7/22. -// - // Copyright (c) 2022 Joshua Wallace // // Licensed under the Apache License, Version 2.0 (the "License"); From a663d58e37576e5bd094965d9fcc282710fb3831 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 15 Nov 2022 19:02:27 -0500 Subject: [PATCH 12/43] CI --- nav2_costmap_2d/src/costmap_2d_publisher.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index 906a9f0324..8726cddf79 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -188,7 +188,6 @@ void Costmap2DPublisher::publishCostmap() costmap_raw_pub_->publish(std::move(costmap_raw_)); } float resolution = costmap_->getResolution(); - if (always_send_full_costmap_ || grid_resolution != resolution || grid_width != costmap_->getSizeInCellsX() || grid_height != costmap_->getSizeInCellsY() || From ce48a592b4fec0a84fb68a149166af0a3374c5df Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Tue, 15 Nov 2022 19:02:39 -0500 Subject: [PATCH 13/43] CI --- nav2_costmap_2d/src/costmap_2d_publisher.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index 8726cddf79..867072dc00 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -188,6 +188,7 @@ void Costmap2DPublisher::publishCostmap() costmap_raw_pub_->publish(std::move(costmap_raw_)); } float resolution = costmap_->getResolution(); + if (always_send_full_costmap_ || grid_resolution != resolution || grid_width != costmap_->getSizeInCellsX() || grid_height != costmap_->getSizeInCellsY() || From b0dab49da89247b707a82f1880e196bc9edfaece Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 21 Nov 2022 17:01:40 -0500 Subject: [PATCH 14/43] linting --- nav2_costmap_2d/src/costmap_2d_publisher.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_costmap_2d/src/costmap_2d_publisher.cpp b/nav2_costmap_2d/src/costmap_2d_publisher.cpp index 867072dc00..87881bc7ca 100644 --- a/nav2_costmap_2d/src/costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/src/costmap_2d_publisher.cpp @@ -187,8 +187,8 @@ void Costmap2DPublisher::publishCostmap() prepareCostmap(); costmap_raw_pub_->publish(std::move(costmap_raw_)); } + float resolution = costmap_->getResolution(); - if (always_send_full_costmap_ || grid_resolution != resolution || grid_width != costmap_->getSizeInCellsX() || grid_height != costmap_->getSizeInCellsY() || From f3ab86230f99f0250397eb69d451ae894c453581 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 30 Nov 2022 09:57:40 -0500 Subject: [PATCH 15/43] completed pub test --- .../test/integration/costmap_tests_launch.py | 16 ++++++++++++ .../integration/test_costmap_2d_publisher.cpp | 26 ++++++++++++++++--- 2 files changed, 39 insertions(+), 3 deletions(-) diff --git a/nav2_costmap_2d/test/integration/costmap_tests_launch.py b/nav2_costmap_2d/test/integration/costmap_tests_launch.py index 5e8c19a485..706290d826 100755 --- a/nav2_costmap_2d/test/integration/costmap_tests_launch.py +++ b/nav2_costmap_2d/test/integration/costmap_tests_launch.py @@ -30,6 +30,20 @@ def main(argv=sys.argv[1:]): launchFile = os.path.join(os.getenv('TEST_LAUNCH_DIR'), 'costmap_map_server.launch.py') testExecutable = os.getenv('TEST_EXECUTABLE') + map_to_odom = launch_ros.actions.Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom'] + ) + + odom_to_base_link = launch_ros.actions.Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'odom', 'base_link'] + ) + lifecycle_manager = launch_ros.actions.Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', @@ -39,6 +53,8 @@ def main(argv=sys.argv[1:]): ld = LaunchDescription([ IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])), + map_to_odom, + odom_to_base_link, lifecycle_manager ]) diff --git a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp index ad5a4ad370..8cb7e66240 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp @@ -20,6 +20,8 @@ #include "nav2_costmap_2d/costmap_2d_publisher.hpp" #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_costmap_2d/cost_values.hpp" +#include "tf2_ros/transform_listener.h" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" class RclCppFixture { @@ -62,7 +64,7 @@ class TestCostmap2dPublisher : public nav2_util::LifecycleNode rclcpp::SubscriptionOptions sub_option; sub_option.callback_group = callback_group_; - std::string topic_name = "dummy_costmap_raw"; + std::string topic_name = "/dummy_costmap/static_layer_raw"; costmap_sub_ = this->create_subscription( topic_name, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), @@ -73,6 +75,15 @@ class TestCostmap2dPublisher : public nav2_util::LifecycleNode executor_->add_callback_group(callback_group_, get_node_base_interface()); executor_thread_ = std::make_unique(executor_); + costmap_ros_ = std::make_shared( + "dummy_costmap", + std::string{get_namespace()}, + "dummy_costmap", + get_parameter("use_sim_time").as_bool()); + costmap_thread_ = std::make_unique(costmap_ros_); + + costmap_ros_->configure(); + return nav2_util::CallbackReturn::SUCCESS; } @@ -81,6 +92,7 @@ class TestCostmap2dPublisher : public nav2_util::LifecycleNode { RCLCPP_INFO(get_logger(), "Activating"); costmap_pub_->on_activate(); + costmap_ros_->activate(); return nav2_util::CallbackReturn::SUCCESS; } @@ -88,6 +100,7 @@ class TestCostmap2dPublisher : public nav2_util::LifecycleNode on_deactivate(const rclcpp_lifecycle::State &) { RCLCPP_INFO(get_logger(), "Deactivating"); + costmap_ros_->deactivate(); return nav2_util::CallbackReturn::SUCCESS; } @@ -95,6 +108,8 @@ class TestCostmap2dPublisher : public nav2_util::LifecycleNode on_cleanup(const rclcpp_lifecycle::State &) { executor_thread_.reset(); + costmap_thread_.reset(); + costmap_ros_->deactivate(); return nav2_util::CallbackReturn::SUCCESS; } @@ -116,6 +131,9 @@ class TestCostmap2dPublisher : public nav2_util::LifecycleNode rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; std::unique_ptr executor_thread_; std::promise promise_; + + std::shared_ptr costmap_ros_; + std::unique_ptr costmap_thread_; }; class TestNode : public ::testing::Test @@ -146,7 +164,9 @@ TEST_F(TestNode, costmap_pub_test) EXPECT_TRUE(status == std::future_status::ready); auto costmap_raw = future.get(); - for (const auto cost : costmap_raw->data) { - EXPECT_TRUE(cost == nav2_costmap_2d::LETHAL_OBSTACLE); + + // Check that the first row is free space + for (int i = 0; i < 10; ++i) { + EXPECT_EQ(costmap_raw->data[0], nav2_costmap_2d::FREE_SPACE); } } From c2f9f0beecd933f869f74ab74f542eb5ba11aa8e Mon Sep 17 00:00:00 2001 From: Hao-Xuan Song <44140526+Cryst4L9527@users.noreply.github.com> Date: Mon, 24 Oct 2022 13:36:49 +0800 Subject: [PATCH 16/43] CostmapLayer::matchSize may be executed concurrently (#3250) * CostmapLayer::matchSize() add a mutex --- nav2_costmap_2d/src/costmap_layer.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_costmap_2d/src/costmap_layer.cpp b/nav2_costmap_2d/src/costmap_layer.cpp index 3f9da4013d..ad488c2618 100644 --- a/nav2_costmap_2d/src/costmap_layer.cpp +++ b/nav2_costmap_2d/src/costmap_layer.cpp @@ -55,6 +55,7 @@ void CostmapLayer::touch( void CostmapLayer::matchSize() { + std::lock_guard guard(*getMutex()); Costmap2D * master = layered_costmap_->getCostmap(); resizeMap( master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(), From 734001733d2cde49933c3eb2d7af32c4e718eaa1 Mon Sep 17 00:00:00 2001 From: jaeminSHIN <91681721+woawo1213@users.noreply.github.com> Date: Tue, 1 Nov 2022 13:45:31 +0900 Subject: [PATCH 17/43] Fix typo (#3262) --- nav2_constrained_smoother/README.md | 2 +- nav2_map_server/README.md | 4 ++-- nav2_simple_commander/README.md | 2 +- nav2_smac_planner/README.md | 2 +- nav2_theta_star_planner/README.md | 4 ++-- nav2_velocity_smoother/README.md | 4 ++-- 6 files changed, 9 insertions(+), 9 deletions(-) diff --git a/nav2_constrained_smoother/README.md b/nav2_constrained_smoother/README.md index d37eb51b63..d34274cda2 100644 --- a/nav2_constrained_smoother/README.md +++ b/nav2_constrained_smoother/README.md @@ -28,7 +28,7 @@ smoother_server: # Parameters used to improve obstacle avoidance near cusps (forward/reverse movement changes) # See the [docs page](https://navigation.ros.org/configuration/packages/configuring-constrained-smoother) for further clarification w_cost_cusp_multiplier: 3.0 # option to have higher weight during forward/reverse direction change which is often accompanied with dangerous rotations - cusp_zone_length: 2.5 # length of the section around cusp in which nodes use w_cost_cusp_multiplier (w_cost rises gradually inside the zone towards the cusp point, whose costmap weight eqals w_cost*w_cost_cusp_multiplier) + cusp_zone_length: 2.5 # length of the section around cusp in which nodes use w_cost_cusp_multiplier (w_cost rises gradually inside the zone towards the cusp point, whose costmap weight equals w_cost*w_cost_cusp_multiplier) # Points in robot frame to grab costmap values from. Format: [x1, y1, weight1, x2, y2, weight2, ...] # IMPORTANT: Requires much higher number of iterations to actually improve the path. Uncomment only if you really need it (highly elongated/asymmetric robots) diff --git a/nav2_map_server/README.md b/nav2_map_server/README.md index c0e7a2cf6a..94a6bd66af 100644 --- a/nav2_map_server/README.md +++ b/nav2_map_server/README.md @@ -18,7 +18,7 @@ Currently map server divides into tree parts: - `map_io` library `map_server` is responsible for loading the map from a file through command-line interface -or by using serice requests. +or by using service requests. `map_saver` saves the map into a file. Like `map_server`, it has an ability to save the map from command-line or by calling a service. @@ -27,7 +27,7 @@ command-line or by calling a service. in order to allow easily save/load map from external code just by calling necessary function. This library is also used by `map_loader` and `map_saver` to work. Currently it contains OccupancyGrid saving/loading functions moved from the rest part of map server code. -It is designed to be replaceble for a new IO library (e.g. for library with new map encoding +It is designed to be replaceable for a new IO library (e.g. for library with new map encoding method or any other library supporting costmaps, multifloor maps, etc...). ### CLI-usage diff --git a/nav2_simple_commander/README.md b/nav2_simple_commander/README.md index c4f398f77d..7af387eb02 100644 --- a/nav2_simple_commander/README.md +++ b/nav2_simple_commander/README.md @@ -92,7 +92,7 @@ This will bring up the robot in the AWS Warehouse in a reasonable position, laun ### Manually -The main benefit of this is to be able to launch alternative robot models or different navigation configurations than the default for a specific technology demonstation. As long as Nav2 and the simulation (or physical robot) is running, the simple python commander examples / demos don't care what the robot is or how it got there. Since the examples / demos do contain hard-programmed item locations or routes, you should still utilize the AWS Warehouse. Obviously these are easy to update if you wish to adapt these examples / demos to another environment. +The main benefit of this is to be able to launch alternative robot models or different navigation configurations than the default for a specific technology demonstration. As long as Nav2 and the simulation (or physical robot) is running, the simple python commander examples / demos don't care what the robot is or how it got there. Since the examples / demos do contain hard-programmed item locations or routes, you should still utilize the AWS Warehouse. Obviously these are easy to update if you wish to adapt these examples / demos to another environment. ``` bash # Terminal 1: launch your robot navigation and simulation (or physical robot). For example diff --git a/nav2_smac_planner/README.md b/nav2_smac_planner/README.md index 0396664f8e..1faa85bf38 100644 --- a/nav2_smac_planner/README.md +++ b/nav2_smac_planner/README.md @@ -37,7 +37,7 @@ The `SmacPlannerHybrid` is designed to work with: The `SmacPlannerLattice` is designed to work with: - Arbitrary shaped, non-circular robots requiring kinematically feasible planning with SE2 collision checking using the full capabilities of the drivetrain -- Flexibility to use other robot model types or with provided non-circular differental, ackermann, and omni support +- Flexibility to use other robot model types or with provided non-circular differential, ackermann, and omni support The `SmacPlanner2D` is designed to work with: - Circular, differential or omnidirectional robots diff --git a/nav2_theta_star_planner/README.md b/nav2_theta_star_planner/README.md index b8e7729904..722957f841 100644 --- a/nav2_theta_star_planner/README.md +++ b/nav2_theta_star_planner/README.md @@ -74,9 +74,9 @@ This planner uses the costs associated with each cell from the `global_costmap` Providing a gentle potential field over the region allows the planner to exchange the increase in path lengths for a decrease in the accumulated traversal cost, thus leading to an increase in the distance from the obstacles. This around a corner allows for naturally smoothing the turns and removes the requirement for an external path smoother. -`w_heuristic_cost` is an internal setting that is not user changable. It has been provided to have an admissible heuristic, restricting its value to the minimum of `w_euc_cost` and `1.0` to make sure the heuristic and travel costs are admissible and consistent. +`w_heuristic_cost` is an internal setting that is not user changeable. It has been provided to have an admissible heuristic, restricting its value to the minimum of `w_euc_cost` and `1.0` to make sure the heuristic and travel costs are admissible and consistent. -To begin with, you can set the parameters to its default values and then try increasing the value of `w_traversal_cost` to achieve those middling paths, but this would adversly make the paths less taut. So it is recommend ed that you simultaneously tune the value of `w_euc_cost`. Increasing `w_euc_cost` increases the tautness of the path, which leads to more straight line like paths (any-angled paths). Do note that the query time from the planner would increase for higher values of `w_traversal_cost` as more nodes would have to be expanded to lower the cost of the path, to counteract this you may also try setting `w_euc_cost` to a higher value and check if it reduces the query time. +To begin with, you can set the parameters to its default values and then try increasing the value of `w_traversal_cost` to achieve those middling paths, but this would adversely make the paths less taut. So it is recommended that you simultaneously tune the value of `w_euc_cost`. Increasing `w_euc_cost` increases the tautness of the path, which leads to more straight line like paths (any-angled paths). Do note that the query time from the planner would increase for higher values of `w_traversal_cost` as more nodes would have to be expanded to lower the cost of the path, to counteract this you may also try setting `w_euc_cost` to a higher value and check if it reduces the query time. Also note that changes to `w_traversal_cost` might cause slow downs, in case the number of node expanisions increase thus tuning it along with `w_euc_cost` is the way to go to. diff --git a/nav2_velocity_smoother/README.md b/nav2_velocity_smoother/README.md index bdd5527788..6ffbe5d372 100644 --- a/nav2_velocity_smoother/README.md +++ b/nav2_velocity_smoother/README.md @@ -3,7 +3,7 @@ The ``nav2_velocity_smoother`` is a package containing a lifecycle-component node for smoothing velocities sent by Nav2 to robot controllers. The aim of this package is to implement velocity, acceleration, and deadband smoothing from Nav2 to reduce wear-and-tear on robot motors and hardware controllers by smoothing out the accelerations/jerky movements that might be present with some local trajectory planners' control efforts. -It supports differential drive and omnidirectional robot platforms primarily, but is applicable to ackermann as well with some intepretations of ``Twist``. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). +It supports differential drive and omnidirectional robot platforms primarily, but is applicable to ackermann as well with some interpretations of ``Twist``. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/). See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-velocity-smoother.html) for additional parameter descriptions. @@ -39,7 +39,7 @@ There are two primary operation modes: open and closed loop. In open-loop, the node assumes that the robot was able to achieve the velocity send to it in the last command which was smoothed (which should be a good assumption if acceleration limits are set properly). This is useful when robot odometry is not particularly accurate or has significant latency relative to `smoothing_frequency` so there isn't a delay in the feedback loop. In closed-loop, the node will read from the odometry topic and apply a smoother over it to obtain the robot's current speed. -This will be used to determine the robot's current velocity and therefore achivable velocity targets by the velocity, acceleration, and deadband constraints using live data. +This will be used to determine the robot's current velocity and therefore achievable velocity targets by the velocity, acceleration, and deadband constraints using live data. ## Parameters From 15f5800c07e5d1d46c4b41a4f72e3fe1bf347278 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 2 Nov 2022 20:33:42 -0700 Subject: [PATCH 18/43] Adding new Nav2 Smoother: Savitzky-Golay Smoother (#3264) * initial prototype of the Savitzky Golay Filter Path Smoother * fixed indexing issue - tested working * updates for filter * adding unit tests for SG-filter smoother * adding lifecycle transitions --- nav2_smoother/CMakeLists.txt | 14 +- nav2_smoother/README.md | 2 + .../nav2_smoother/savitzky_golay_smoother.hpp | 108 ++++++ .../include/nav2_smoother/simple_smoother.hpp | 31 +- .../include/nav2_smoother/smoother_utils.hpp | 132 +++++++ nav2_smoother/plugins.xml | 6 + nav2_smoother/src/nav2_smoother.cpp | 1 + nav2_smoother/src/savitzky_golay_smoother.cpp | 198 +++++++++++ nav2_smoother/src/simple_smoother.cpp | 80 +---- nav2_smoother/test/CMakeLists.txt | 13 + .../test/test_savitzky_golay_smoother.cpp | 332 ++++++++++++++++++ nav2_smoother/test/test_simple_smoother.cpp | 1 + 12 files changed, 807 insertions(+), 111 deletions(-) create mode 100644 nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp create mode 100644 nav2_smoother/include/nav2_smoother/smoother_utils.hpp create mode 100644 nav2_smoother/src/savitzky_golay_smoother.cpp create mode 100644 nav2_smoother/test/test_savitzky_golay_smoother.cpp diff --git a/nav2_smoother/CMakeLists.txt b/nav2_smoother/CMakeLists.txt index 0df85f54bd..0ab7c83628 100644 --- a/nav2_smoother/CMakeLists.txt +++ b/nav2_smoother/CMakeLists.txt @@ -68,8 +68,18 @@ add_library(simple_smoother SHARED ament_target_dependencies(simple_smoother ${dependencies} ) + +# Savitzky Golay Smoother plugin +add_library(savitzky_golay_smoother SHARED + src/savitzky_golay_smoother.cpp +) +ament_target_dependencies(savitzky_golay_smoother + ${dependencies} +) + pluginlib_export_plugin_description_file(nav2_core plugins.xml) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights @@ -82,7 +92,7 @@ endif() rclcpp_components_register_nodes(${library_name} "nav2_smoother::SmootherServer") install( - TARGETS ${library_name} simple_smoother + TARGETS ${library_name} simple_smoother savitzky_golay_smoother ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -104,6 +114,6 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_libraries(${library_name} simple_smoother) +ament_export_libraries(${library_name} simple_smoother savitzky_golay_smoother) ament_export_dependencies(${dependencies}) ament_package() diff --git a/nav2_smoother/README.md b/nav2_smoother/README.md index da007a9096..c612dba43c 100644 --- a/nav2_smoother/README.md +++ b/nav2_smoother/README.md @@ -7,3 +7,5 @@ A smoothing module implementing the `nav2_behavior_tree::SmoothPath` interface i See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available smoother plugins. See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-smoother-server.html) for additional parameter descriptions. + +This package contains the Simple Smoother and Savitzky-Golay Smoother plugins. diff --git a/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp b/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp new file mode 100644 index 0000000000..090aa07af8 --- /dev/null +++ b/nav2_smoother/include/nav2_smoother/savitzky_golay_smoother.hpp @@ -0,0 +1,108 @@ +// Copyright (c) 2022, Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#ifndef NAV2_SMOOTHER__SAVITZKY_GOLAY_SMOOTHER_HPP_ +#define NAV2_SMOOTHER__SAVITZKY_GOLAY_SMOOTHER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "nav2_core/smoother.hpp" +#include "nav2_smoother/smoother_utils.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav_msgs/msg/path.hpp" +#include "angles/angles.h" +#include "tf2/utils.h" + +namespace nav2_smoother +{ + +/** + * @class nav2_smoother::SavitzkyGolaySmoother + * @brief A path smoother implementation using Savitzky Golay filters + */ +class SavitzkyGolaySmoother : public nav2_core::Smoother +{ +public: + /** + * @brief A constructor for nav2_smoother::SavitzkyGolaySmoother + */ + SavitzkyGolaySmoother() = default; + + /** + * @brief A destructor for nav2_smoother::SavitzkyGolaySmoother + */ + ~SavitzkyGolaySmoother() override = default; + + void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr &, + std::string name, std::shared_ptr, + std::shared_ptr, + std::shared_ptr) override; + + /** + * @brief Method to cleanup resources. + */ + void cleanup() override {} + + /** + * @brief Method to activate smoother and any threads involved in execution. + */ + void activate() override {} + + /** + * @brief Method to deactivate smoother and any threads involved in execution. + */ + void deactivate() override {} + + /** + * @brief Method to smooth given path + * + * @param path In-out path to be smoothed + * @param max_time Maximum duration smoothing should take + * @return If smoothing was completed (true) or interrupted by time limit (false) + */ + bool smooth( + nav_msgs::msg::Path & path, + const rclcpp::Duration & max_time) override; + +protected: + /** + * @brief Smoother method - does the smoothing on a segment + * @param path Reference to path + * @param reversing_segment Return if this is a reversing segment + * @param costmap Pointer to minimal costmap + * @param max_time Maximum time to compute, stop early if over limit + * @return If smoothing was successful + */ + bool smoothImpl( + nav_msgs::msg::Path & path, + bool & reversing_segment); + + bool do_refinement_; + int refinement_num_; + rclcpp::Logger logger_{rclcpp::get_logger("SGSmoother")}; +}; + +} // namespace nav2_smoother + +#endif // NAV2_SMOOTHER__SAVITZKY_GOLAY_SMOOTHER_HPP_ diff --git a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp index 4b841b3c09..95f19dc54c 100644 --- a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp @@ -24,6 +24,7 @@ #include #include "nav2_core/smoother.hpp" +#include "nav2_smoother/smoother_utils.hpp" #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/cost_values.hpp" #include "nav2_util/geometry_utils.hpp" @@ -35,19 +36,6 @@ namespace nav2_smoother { -/** - * @class nav2_smoother::PathSegment - * @brief A segment of a path in start/end indices - */ -struct PathSegment -{ - unsigned int start; - unsigned int end; -}; - -typedef std::vector::iterator PathIterator; -typedef std::vector::reverse_iterator ReversePathIterator; - /** * @class nav2_smoother::SimpleSmoother * @brief A path smoother implementation @@ -132,23 +120,6 @@ class SimpleSmoother : public nav2_core::Smoother geometry_msgs::msg::PoseStamped & msg, const unsigned int dim, const double & value); - /** - * @brief Finds the starting and end indices of path segments where - * the robot is traveling in the same direction (e.g. forward vs reverse) - * @param path Path in which to look for cusps - * @return Set of index pairs for each segment of the path in a given direction - */ - std::vector findDirectionalPathSegments(const nav_msgs::msg::Path & path); - - /** - * @brief For a given path, update the path point orientations based on smoothing - * @param path Path to approximate the path orientation in - * @param reversing_segment Return if this is a reversing segment - */ - inline void updateApproximatePathOrientations( - nav_msgs::msg::Path & path, - bool & reversing_segment); - double tolerance_, data_w_, smooth_w_; int max_its_, refinement_ctr_; bool do_refinement_; diff --git a/nav2_smoother/include/nav2_smoother/smoother_utils.hpp b/nav2_smoother/include/nav2_smoother/smoother_utils.hpp new file mode 100644 index 0000000000..67d7296353 --- /dev/null +++ b/nav2_smoother/include/nav2_smoother/smoother_utils.hpp @@ -0,0 +1,132 @@ +// Copyright (c) 2022, Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#ifndef NAV2_SMOOTHER__SMOOTHER_UTILS_HPP_ +#define NAV2_SMOOTHER__SMOOTHER_UTILS_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "nav2_core/smoother.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/cost_values.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav_msgs/msg/path.hpp" +#include "angles/angles.h" +#include "tf2/utils.h" + +namespace smoother_utils +{ + +/** + * @class nav2_smoother::PathSegment + * @brief A segment of a path in start/end indices + */ +struct PathSegment +{ + unsigned int start; + unsigned int end; +}; + +typedef std::vector::iterator PathIterator; +typedef std::vector::reverse_iterator ReversePathIterator; + +inline std::vector findDirectionalPathSegments( + const nav_msgs::msg::Path & path) +{ + std::vector segments; + PathSegment curr_segment; + curr_segment.start = 0; + + // Iterating through the path to determine the position of the cusp + for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { + // We have two vectors for the dot product OA and AB. Determining the vectors. + double oa_x = path.poses[idx].pose.position.x - + path.poses[idx - 1].pose.position.x; + double oa_y = path.poses[idx].pose.position.y - + path.poses[idx - 1].pose.position.y; + double ab_x = path.poses[idx + 1].pose.position.x - + path.poses[idx].pose.position.x; + double ab_y = path.poses[idx + 1].pose.position.y - + path.poses[idx].pose.position.y; + + // Checking for the existance of cusp, in the path, using the dot product. + double dot_product = (oa_x * ab_x) + (oa_y * ab_y); + if (dot_product < 0.0) { + curr_segment.end = idx; + segments.push_back(curr_segment); + curr_segment.start = idx; + } + + // Checking for the existance of a differential rotation in place. + double cur_theta = tf2::getYaw(path.poses[idx].pose.orientation); + double next_theta = tf2::getYaw(path.poses[idx + 1].pose.orientation); + double dtheta = angles::shortest_angular_distance(cur_theta, next_theta); + if (fabs(ab_x) < 1e-4 && fabs(ab_y) < 1e-4 && fabs(dtheta) > 1e-4) { + curr_segment.end = idx; + segments.push_back(curr_segment); + curr_segment.start = idx; + } + } + + curr_segment.end = path.poses.size() - 1; + segments.push_back(curr_segment); + return segments; +} + +inline void updateApproximatePathOrientations( + nav_msgs::msg::Path & path, + bool & reversing_segment) +{ + double dx, dy, theta, pt_yaw; + reversing_segment = false; + + // Find if this path segment is in reverse + dx = path.poses[2].pose.position.x - path.poses[1].pose.position.x; + dy = path.poses[2].pose.position.y - path.poses[1].pose.position.y; + theta = atan2(dy, dx); + pt_yaw = tf2::getYaw(path.poses[1].pose.orientation); + if (fabs(angles::shortest_angular_distance(pt_yaw, theta)) > M_PI_2) { + reversing_segment = true; + } + + // Find the angle relative the path position vectors + for (unsigned int i = 0; i != path.poses.size() - 1; i++) { + dx = path.poses[i + 1].pose.position.x - path.poses[i].pose.position.x; + dy = path.poses[i + 1].pose.position.y - path.poses[i].pose.position.y; + theta = atan2(dy, dx); + + // If points are overlapping, pass + if (fabs(dx) < 1e-4 && fabs(dy) < 1e-4) { + continue; + } + + // Flip the angle if this path segment is in reverse + if (reversing_segment) { + theta += M_PI; // orientationAroundZAxis will normalize + } + + path.poses[i].pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(theta); + } +} + +} // namespace smoother_utils + +#endif // NAV2_SMOOTHER__SMOOTHER_UTILS_HPP_ diff --git a/nav2_smoother/plugins.xml b/nav2_smoother/plugins.xml index c72f7c25c8..90960a51ef 100644 --- a/nav2_smoother/plugins.xml +++ b/nav2_smoother/plugins.xml @@ -4,4 +4,10 @@ Does a simple smoothing process with collision checking + + + + Does Savitzky-Golay smoothing + + diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index 27e88f17b3..f8f48c9886 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -1,5 +1,6 @@ // Copyright (c) 2019 RoboTech Vision // Copyright (c) 2019 Intel Corporation +// Copyright (c) 2022 Samsung Research America // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/nav2_smoother/src/savitzky_golay_smoother.cpp b/nav2_smoother/src/savitzky_golay_smoother.cpp new file mode 100644 index 0000000000..6a61196ab3 --- /dev/null +++ b/nav2_smoother/src/savitzky_golay_smoother.cpp @@ -0,0 +1,198 @@ +// Copyright (c) 2022, Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include "nav2_smoother/savitzky_golay_smoother.hpp" + +namespace nav2_smoother +{ + +using namespace smoother_utils; // NOLINT +using namespace nav2_util::geometry_utils; // NOLINT +using namespace std::chrono; // NOLINT +using nav2_util::declare_parameter_if_not_declared; + +void SavitzkyGolaySmoother::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, std::shared_ptr/*tf*/, + std::shared_ptr/*costmap_sub*/, + std::shared_ptr/*footprint_sub*/) +{ + auto node = parent.lock(); + logger_ = node->get_logger(); + + declare_parameter_if_not_declared( + node, name + ".do_refinement", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, name + ".refinement_num", rclcpp::ParameterValue(2)); + node->get_parameter(name + ".do_refinement", do_refinement_); + node->get_parameter(name + ".refinement_num", refinement_num_); +} + +bool SavitzkyGolaySmoother::smooth( + nav_msgs::msg::Path & path, + const rclcpp::Duration & max_time) +{ + steady_clock::time_point start = steady_clock::now(); + double time_remaining = max_time.seconds(); + + bool success = true, reversing_segment; + nav_msgs::msg::Path curr_path_segment; + curr_path_segment.header = path.header; + + std::vector path_segments = findDirectionalPathSegments(path); + + for (unsigned int i = 0; i != path_segments.size(); i++) { + if (path_segments[i].end - path_segments[i].start > 9) { + // Populate path segment + curr_path_segment.poses.clear(); + std::copy( + path.poses.begin() + path_segments[i].start, + path.poses.begin() + path_segments[i].end + 1, + std::back_inserter(curr_path_segment.poses)); + + // Make sure we're still able to smooth with time remaining + steady_clock::time_point now = steady_clock::now(); + time_remaining = max_time.seconds() - duration_cast>(now - start).count(); + + if (time_remaining <= 0.0) { + RCLCPP_WARN( + logger_, + "Smoothing time exceeded allowed duration of %0.2f.", max_time.seconds()); + return false; + } + + // Smooth path segment + success = success && smoothImpl(curr_path_segment, reversing_segment); + + // Assemble the path changes to the main path + std::copy( + curr_path_segment.poses.begin(), + curr_path_segment.poses.end(), + path.poses.begin() + path_segments[i].start); + } + } + + return success; +} + +bool SavitzkyGolaySmoother::smoothImpl( + nav_msgs::msg::Path & path, + bool & reversing_segment) +{ + // Must be at least 10 in length to enter function + const unsigned int & path_size = path.poses.size(); + + // 7-point SG filter + const std::array filter = { + -2.0 / 21.0, + 3.0 / 21.0, + 6.0 / 21.0, + 7.0 / 21.0, + 6.0 / 21.0, + 3.0 / 21.0, + -2.0 / 21.0}; + + auto applyFilter = [&](const std::vector & data) + -> geometry_msgs::msg::Point + { + geometry_msgs::msg::Point val; + for (unsigned int i = 0; i != filter.size(); i++) { + val.x += filter[i] * data[i].x; + val.y += filter[i] * data[i].y; + } + return val; + }; + + auto applyFilterOverAxes = + [&](std::vector & plan_pts) -> void + { + // Handle initial boundary conditions, first point is fixed + unsigned int idx = 1; + plan_pts[idx].pose.position = applyFilter( + { + plan_pts[idx - 1].pose.position, + plan_pts[idx - 1].pose.position, + plan_pts[idx - 1].pose.position, + plan_pts[idx].pose.position, + plan_pts[idx + 1].pose.position, + plan_pts[idx + 2].pose.position, + plan_pts[idx + 3].pose.position}); + + idx++; + plan_pts[idx].pose.position = applyFilter( + { + plan_pts[idx - 2].pose.position, + plan_pts[idx - 2].pose.position, + plan_pts[idx - 1].pose.position, + plan_pts[idx].pose.position, + plan_pts[idx + 1].pose.position, + plan_pts[idx + 2].pose.position, + plan_pts[idx + 3].pose.position}); + + // Apply nominal filter + for (idx = 3; idx < path_size - 4; ++idx) { + plan_pts[idx].pose.position = applyFilter( + { + plan_pts[idx - 3].pose.position, + plan_pts[idx - 2].pose.position, + plan_pts[idx - 1].pose.position, + plan_pts[idx].pose.position, + plan_pts[idx + 1].pose.position, + plan_pts[idx + 2].pose.position, + plan_pts[idx + 3].pose.position}); + } + + // Handle terminal boundary conditions, last point is fixed + idx++; + plan_pts[idx].pose.position = applyFilter( + { + plan_pts[idx - 3].pose.position, + plan_pts[idx - 2].pose.position, + plan_pts[idx - 1].pose.position, + plan_pts[idx].pose.position, + plan_pts[idx + 1].pose.position, + plan_pts[idx + 2].pose.position, + plan_pts[idx + 2].pose.position}); + + idx++; + plan_pts[idx].pose.position = applyFilter( + { + plan_pts[idx - 3].pose.position, + plan_pts[idx - 2].pose.position, + plan_pts[idx - 1].pose.position, + plan_pts[idx].pose.position, + plan_pts[idx + 1].pose.position, + plan_pts[idx + 1].pose.position, + plan_pts[idx + 1].pose.position}); + }; + + applyFilterOverAxes(path.poses); + + // Lets do additional refinement, it shouldn't take more than a couple milliseconds + if (do_refinement_) { + for (int i = 0; i < refinement_num_; i++) { + applyFilterOverAxes(path.poses); + } + } + + updateApproximatePathOrientations(path, reversing_segment); + return true; +} + +} // namespace nav2_smoother + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_smoother::SavitzkyGolaySmoother, nav2_core::Smoother) diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp index 59a81fd3b1..d04d88bc61 100644 --- a/nav2_smoother/src/simple_smoother.cpp +++ b/nav2_smoother/src/simple_smoother.cpp @@ -18,6 +18,7 @@ namespace nav2_smoother { +using namespace smoother_utils; // NOLINT using namespace nav2_util::geometry_utils; // NOLINT using namespace std::chrono; // NOLINT using nav2_util::declare_parameter_if_not_declared; @@ -214,85 +215,6 @@ void SimpleSmoother::setFieldByDim( } } -std::vector SimpleSmoother::findDirectionalPathSegments( - const nav_msgs::msg::Path & path) -{ - std::vector segments; - PathSegment curr_segment; - curr_segment.start = 0; - - // Iterating through the path to determine the position of the cusp - for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) { - // We have two vectors for the dot product OA and AB. Determining the vectors. - double oa_x = path.poses[idx].pose.position.x - - path.poses[idx - 1].pose.position.x; - double oa_y = path.poses[idx].pose.position.y - - path.poses[idx - 1].pose.position.y; - double ab_x = path.poses[idx + 1].pose.position.x - - path.poses[idx].pose.position.x; - double ab_y = path.poses[idx + 1].pose.position.y - - path.poses[idx].pose.position.y; - - // Checking for the existance of cusp, in the path, using the dot product. - double dot_product = (oa_x * ab_x) + (oa_y * ab_y); - if (dot_product < 0.0) { - curr_segment.end = idx; - segments.push_back(curr_segment); - curr_segment.start = idx; - } - - // Checking for the existance of a differential rotation in place. - double cur_theta = tf2::getYaw(path.poses[idx].pose.orientation); - double next_theta = tf2::getYaw(path.poses[idx + 1].pose.orientation); - double dtheta = angles::shortest_angular_distance(cur_theta, next_theta); - if (fabs(ab_x) < 1e-4 && fabs(ab_y) < 1e-4 && fabs(dtheta) > 1e-4) { - curr_segment.end = idx; - segments.push_back(curr_segment); - curr_segment.start = idx; - } - } - - curr_segment.end = path.poses.size() - 1; - segments.push_back(curr_segment); - return segments; -} - -void SimpleSmoother::updateApproximatePathOrientations( - nav_msgs::msg::Path & path, - bool & reversing_segment) -{ - double dx, dy, theta, pt_yaw; - reversing_segment = false; - - // Find if this path segment is in reverse - dx = path.poses[2].pose.position.x - path.poses[1].pose.position.x; - dy = path.poses[2].pose.position.y - path.poses[1].pose.position.y; - theta = atan2(dy, dx); - pt_yaw = tf2::getYaw(path.poses[1].pose.orientation); - if (fabs(angles::shortest_angular_distance(pt_yaw, theta)) > M_PI_2) { - reversing_segment = true; - } - - // Find the angle relative the path position vectors - for (unsigned int i = 0; i != path.poses.size() - 1; i++) { - dx = path.poses[i + 1].pose.position.x - path.poses[i].pose.position.x; - dy = path.poses[i + 1].pose.position.y - path.poses[i].pose.position.y; - theta = atan2(dy, dx); - - // If points are overlapping, pass - if (fabs(dx) < 1e-4 && fabs(dy) < 1e-4) { - continue; - } - - // Flip the angle if this path segment is in reverse - if (reversing_segment) { - theta += M_PI; // orientationAroundZAxis will normalize - } - - path.poses[i].pose.orientation = orientationAroundZAxis(theta); - } -} - } // namespace nav2_smoother #include "pluginlib/class_list_macros.hpp" diff --git a/nav2_smoother/test/CMakeLists.txt b/nav2_smoother/test/CMakeLists.txt index 67c98e3e43..623447b5f9 100644 --- a/nav2_smoother/test/CMakeLists.txt +++ b/nav2_smoother/test/CMakeLists.txt @@ -21,3 +21,16 @@ target_link_libraries(test_simple_smoother ament_target_dependencies(test_simple_smoother ${dependencies} ) + + +ament_add_gtest(test_savitzky_golay_smoother + test_savitzky_golay_smoother.cpp +) + +target_link_libraries(test_savitzky_golay_smoother + savitzky_golay_smoother +) + +ament_target_dependencies(test_savitzky_golay_smoother + ${dependencies} +) diff --git a/nav2_smoother/test/test_savitzky_golay_smoother.cpp b/nav2_smoother/test/test_savitzky_golay_smoother.cpp new file mode 100644 index 0000000000..58860d67db --- /dev/null +++ b/nav2_smoother/test/test_savitzky_golay_smoother.cpp @@ -0,0 +1,332 @@ +// Copyright (c) 2022, Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#include +#include +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_subscriber.hpp" +#include "nav2_msgs/msg/costmap.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_smoother/savitzky_golay_smoother.hpp" +#include "ament_index_cpp/get_package_share_directory.hpp" + +using namespace smoother_utils; // NOLINT +using namespace nav2_smoother; // NOLINT +using namespace std::chrono_literals; // NOLINT + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(SmootherTest, test_sg_smoother_basics) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr node = + std::make_shared("SmacSGSmootherTest"); + + std::shared_ptr costmap_msg = + std::make_shared(); + costmap_msg->header.stamp = node->now(); + costmap_msg->header.frame_id = "map"; + costmap_msg->data.resize(100 * 100); + costmap_msg->metadata.resolution = 0.05; + costmap_msg->metadata.size_x = 100; + costmap_msg->metadata.size_y = 100; + + std::weak_ptr parent = node; + std::shared_ptr dummy_costmap; + dummy_costmap = std::make_shared(parent, "dummy_topic"); + dummy_costmap->costmapCallback(costmap_msg); + + // Make smoother + std::shared_ptr dummy_tf; + std::shared_ptr dummy_footprint; + node->declare_parameter("test.do_refinement", rclcpp::ParameterValue(false)); + auto smoother = std::make_unique(); + smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint); + smoother->activate(); + rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1.0); // 1 seconds + + // Test regular path, should see no effective change + nav_msgs::msg::Path straight_regular_path, straight_regular_path_baseline; + straight_regular_path.header.frame_id = "map"; + straight_regular_path.header.stamp = node->now(); + straight_regular_path.poses.resize(11); + straight_regular_path.poses[0].pose.position.x = 0.5; + straight_regular_path.poses[0].pose.position.y = 0.1; + straight_regular_path.poses[1].pose.position.x = 0.5; + straight_regular_path.poses[1].pose.position.y = 0.2; + straight_regular_path.poses[2].pose.position.x = 0.5; + straight_regular_path.poses[2].pose.position.y = 0.3; + straight_regular_path.poses[3].pose.position.x = 0.5; + straight_regular_path.poses[3].pose.position.y = 0.4; + straight_regular_path.poses[4].pose.position.x = 0.5; + straight_regular_path.poses[4].pose.position.y = 0.5; + straight_regular_path.poses[5].pose.position.x = 0.5; + straight_regular_path.poses[5].pose.position.y = 0.6; + straight_regular_path.poses[6].pose.position.x = 0.5; + straight_regular_path.poses[6].pose.position.y = 0.7; + straight_regular_path.poses[7].pose.position.x = 0.5; + straight_regular_path.poses[7].pose.position.y = 0.8; + straight_regular_path.poses[8].pose.position.x = 0.5; + straight_regular_path.poses[8].pose.position.y = 0.9; + straight_regular_path.poses[9].pose.position.x = 0.5; + straight_regular_path.poses[9].pose.position.y = 1.0; + straight_regular_path.poses[10].pose.position.x = 0.5; + straight_regular_path.poses[10].pose.position.y = 1.1; + straight_regular_path_baseline = straight_regular_path; + + EXPECT_TRUE(smoother->smooth(straight_regular_path, max_time)); + for (uint i = 0; i != straight_regular_path.poses.size() - 1; i++) { + // Check distances are still the same + EXPECT_NEAR( + fabs( + straight_regular_path.poses[i].pose.position.y - + straight_regular_path_baseline.poses[i].pose.position.y), 0.0, 0.011); + } + + // Attempt smoothing with no time given, should fail + rclcpp::Duration no_time = rclcpp::Duration::from_seconds(-1.0); // 0 seconds + EXPECT_FALSE(smoother->smooth(straight_regular_path, no_time)); + + smoother->deactivate(); + smoother->cleanup(); +} + +TEST(SmootherTest, test_sg_smoother_noisey_path) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr node = + std::make_shared("SmacSGSmootherTest"); + + std::shared_ptr costmap_msg = + std::make_shared(); + costmap_msg->header.stamp = node->now(); + costmap_msg->header.frame_id = "map"; + costmap_msg->data.resize(100 * 100); + costmap_msg->metadata.resolution = 0.05; + costmap_msg->metadata.size_x = 100; + costmap_msg->metadata.size_y = 100; + + std::weak_ptr parent = node; + std::shared_ptr dummy_costmap; + dummy_costmap = std::make_shared(parent, "dummy_topic"); + dummy_costmap->costmapCallback(costmap_msg); + + // Make smoother + std::shared_ptr dummy_tf; + std::shared_ptr dummy_footprint; + node->declare_parameter("test.do_refinement", rclcpp::ParameterValue(false)); + auto smoother = std::make_unique(); + smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint); + rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1.0); // 1 seconds + + // Given nominal irregular/noisey path, test that the output is shorter and smoother + nav_msgs::msg::Path noisey_path, noisey_path_baseline; + noisey_path.header.frame_id = "map"; + noisey_path.header.stamp = node->now(); + noisey_path.poses.resize(11); + noisey_path.poses[0].pose.position.x = 0.5; + noisey_path.poses[0].pose.position.y = 0.1; + noisey_path.poses[1].pose.position.x = 0.5; + noisey_path.poses[1].pose.position.y = 0.2; + noisey_path.poses[2].pose.position.x = 0.5; + noisey_path.poses[2].pose.position.y = 0.3; + noisey_path.poses[3].pose.position.x = 0.5; + noisey_path.poses[3].pose.position.y = 0.4; + noisey_path.poses[4].pose.position.x = 0.5; + noisey_path.poses[4].pose.position.y = 0.5; + noisey_path.poses[5].pose.position.x = 0.5; + noisey_path.poses[5].pose.position.y = 0.6; + noisey_path.poses[6].pose.position.x = 0.5; + noisey_path.poses[6].pose.position.y = 0.7; + noisey_path.poses[7].pose.position.x = 0.5; + noisey_path.poses[7].pose.position.y = 0.8; + noisey_path.poses[8].pose.position.x = 0.5; + noisey_path.poses[8].pose.position.y = 0.9; + noisey_path.poses[9].pose.position.x = 0.5; + noisey_path.poses[9].pose.position.y = 1.0; + noisey_path.poses[10].pose.position.x = 0.5; + noisey_path.poses[10].pose.position.y = 1.1; + + // Add random but deterministic noises + std::random_device rd{}; + std::mt19937 gen{rd()}; + std::normal_distribution<> normal_distribution{0.0, 0.02}; + for (unsigned int i = 0; i != noisey_path.poses.size(); i++) { + auto noise = normal_distribution(gen); + noisey_path.poses[i].pose.position.x += noise; + } + + noisey_path_baseline = noisey_path; + EXPECT_TRUE(smoother->smooth(noisey_path, max_time)); + + // Compute metric, should be shorter if smoother + double length = 0; + double base_length = 0; + for (unsigned int i = 0; i != noisey_path.poses.size() - 1; i++) { + length += std::hypot( + noisey_path.poses[i + 1].pose.position.x - noisey_path.poses[i].pose.position.x, + noisey_path.poses[i + 1].pose.position.y - noisey_path.poses[i].pose.position.y); + base_length += std::hypot( + noisey_path_baseline.poses[i + 1].pose.position.x - + noisey_path_baseline.poses[i].pose.position.x, + noisey_path_baseline.poses[i + 1].pose.position.y - + noisey_path_baseline.poses[i].pose.position.y); + } + + EXPECT_LT(length, base_length); + + // Test again with refinement, even shorter and smoother + node->set_parameter(rclcpp::Parameter("test.do_refinement", rclcpp::ParameterValue(true))); + smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint); + nav_msgs::msg::Path noisey_path_refined = noisey_path_baseline; + EXPECT_TRUE(smoother->smooth(noisey_path_refined, max_time)); + + length = 0; + double non_refined_length = 0; + for (unsigned int i = 0; i != noisey_path.poses.size() - 1; i++) { + length += std::hypot( + noisey_path_refined.poses[i + 1].pose.position.x - + noisey_path_refined.poses[i].pose.position.x, + noisey_path_refined.poses[i + 1].pose.position.y - + noisey_path_refined.poses[i].pose.position.y); + non_refined_length += std::hypot( + noisey_path.poses[i + 1].pose.position.x - noisey_path_baseline.poses[i].pose.position.x, + noisey_path.poses[i + 1].pose.position.y - noisey_path_baseline.poses[i].pose.position.y); + } + + EXPECT_LT(length, base_length); +} + +TEST(SmootherTest, test_sg_smoother_reversing) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr node = + std::make_shared("SmacSGSmootherTest"); + + std::shared_ptr costmap_msg = + std::make_shared(); + costmap_msg->header.stamp = node->now(); + costmap_msg->header.frame_id = "map"; + costmap_msg->data.resize(100 * 100); + costmap_msg->metadata.resolution = 0.05; + costmap_msg->metadata.size_x = 100; + costmap_msg->metadata.size_y = 100; + + std::weak_ptr parent = node; + std::shared_ptr dummy_costmap; + dummy_costmap = std::make_shared(parent, "dummy_topic"); + dummy_costmap->costmapCallback(costmap_msg); + + // Make smoother + std::shared_ptr dummy_tf; + std::shared_ptr dummy_footprint; + node->declare_parameter("test.do_refinement", rclcpp::ParameterValue(false)); + auto smoother = std::make_unique(); + smoother->configure(parent, "test", dummy_tf, dummy_costmap, dummy_footprint); + rclcpp::Duration max_time = rclcpp::Duration::from_seconds(1.0); // 1 seconds + + // Test reversing / multiple segments via a cusp + nav_msgs::msg::Path cusp_path, cusp_path_baseline; + cusp_path.header.frame_id = "map"; + cusp_path.header.stamp = node->now(); + cusp_path.poses.resize(22); + cusp_path.poses[0].pose.position.x = 0.5; + cusp_path.poses[0].pose.position.y = 0.1; + cusp_path.poses[1].pose.position.x = 0.5; + cusp_path.poses[1].pose.position.y = 0.2; + cusp_path.poses[2].pose.position.x = 0.5; + cusp_path.poses[2].pose.position.y = 0.3; + cusp_path.poses[3].pose.position.x = 0.5; + cusp_path.poses[3].pose.position.y = 0.4; + cusp_path.poses[4].pose.position.x = 0.5; + cusp_path.poses[4].pose.position.y = 0.5; + cusp_path.poses[5].pose.position.x = 0.5; + cusp_path.poses[5].pose.position.y = 0.6; + cusp_path.poses[6].pose.position.x = 0.5; + cusp_path.poses[6].pose.position.y = 0.7; + cusp_path.poses[7].pose.position.x = 0.5; + cusp_path.poses[7].pose.position.y = 0.8; + cusp_path.poses[8].pose.position.x = 0.5; + cusp_path.poses[8].pose.position.y = 0.9; + cusp_path.poses[9].pose.position.x = 0.5; + cusp_path.poses[9].pose.position.y = 1.0; + cusp_path.poses[10].pose.position.x = 0.5; + cusp_path.poses[10].pose.position.y = 1.1; + cusp_path.poses[11].pose.position.x = 0.5; + cusp_path.poses[11].pose.position.y = 1.0; + cusp_path.poses[12].pose.position.x = 0.5; + cusp_path.poses[12].pose.position.y = 0.9; + cusp_path.poses[13].pose.position.x = 0.5; + cusp_path.poses[13].pose.position.y = 0.8; + cusp_path.poses[14].pose.position.x = 0.5; + cusp_path.poses[14].pose.position.y = 0.7; + cusp_path.poses[15].pose.position.x = 0.5; + cusp_path.poses[15].pose.position.y = 0.6; + cusp_path.poses[16].pose.position.x = 0.5; + cusp_path.poses[16].pose.position.y = 0.5; + cusp_path.poses[17].pose.position.x = 0.5; + cusp_path.poses[17].pose.position.y = 0.4; + cusp_path.poses[18].pose.position.x = 0.5; + cusp_path.poses[18].pose.position.y = 0.3; + cusp_path.poses[19].pose.position.x = 0.5; + cusp_path.poses[19].pose.position.y = 0.2; + cusp_path.poses[20].pose.position.x = 0.5; + cusp_path.poses[20].pose.position.y = 0.1; + cusp_path.poses[21].pose.position.x = 0.5; + cusp_path.poses[21].pose.position.y = 0.0; + + // Add random but deterministic noises + std::random_device rd{}; + std::mt19937 gen{rd()}; + std::normal_distribution<> normal_distribution{0.0, 0.02}; + for (unsigned int i = 0; i != cusp_path.poses.size(); i++) { + auto noise = normal_distribution(gen); + cusp_path.poses[i].pose.position.x += noise; + } + + cusp_path_baseline = cusp_path; + + EXPECT_TRUE(smoother->smooth(cusp_path, max_time)); + + // If it detected the cusp, the cusp point should be fixed + EXPECT_EQ(cusp_path.poses[10].pose.position.x, cusp_path_baseline.poses[10].pose.position.x); + EXPECT_EQ(cusp_path.poses[10].pose.position.y, cusp_path_baseline.poses[10].pose.position.y); + + // But the path also should be smoother / shorter + double length = 0; + double base_length = 0; + for (unsigned int i = 0; i != cusp_path.poses.size() - 1; i++) { + length += std::hypot( + cusp_path.poses[i + 1].pose.position.x - cusp_path.poses[i].pose.position.x, + cusp_path.poses[i + 1].pose.position.y - cusp_path.poses[i].pose.position.y); + base_length += std::hypot( + cusp_path_baseline.poses[i + 1].pose.position.x - + cusp_path_baseline.poses[i].pose.position.x, + cusp_path_baseline.poses[i + 1].pose.position.y - + cusp_path_baseline.poses[i].pose.position.y); + } + + EXPECT_LT(length, base_length); +} diff --git a/nav2_smoother/test/test_simple_smoother.cpp b/nav2_smoother/test/test_simple_smoother.cpp index fccc2c2a7c..7d9b56d457 100644 --- a/nav2_smoother/test/test_simple_smoother.cpp +++ b/nav2_smoother/test/test_simple_smoother.cpp @@ -28,6 +28,7 @@ #include "nav2_smoother/simple_smoother.hpp" #include "ament_index_cpp/get_package_share_directory.hpp" +using namespace smoother_utils; // NOLINT using namespace nav2_smoother; // NOLINT using namespace std::chrono_literals; // NOLINT From be85e52681e046afbd692f0fd67e4267bf5af9b4 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 2 Nov 2022 22:50:36 -0700 Subject: [PATCH 19/43] refactoring RPP a bit for cleanliness on way to ROSCon (#3265) * refactor for RPP on way to ROSCon * fixing header * fixing header * fixing header * fix edge cases test samplings * linting --- .../CMakeLists.txt | 5 +- .../collision_checker.hpp | 105 +++ .../parameter_handler.hpp | 104 +++ .../path_handler.hpp | 99 +++ .../regulated_pure_pursuit_controller.hpp | 123 +--- .../regulation_functions.hpp | 138 ++++ .../src/collision_checker.cpp | 174 +++++ .../src/parameter_handler.cpp | 261 ++++++++ .../src/path_handler.cpp | 137 ++++ .../src/regulated_pure_pursuit_controller.cpp | 600 ++---------------- .../test/test_regulated_pp.cpp | 51 +- 11 files changed, 1131 insertions(+), 666 deletions(-) create mode 100644 nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp create mode 100644 nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp create mode 100644 nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp create mode 100644 nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulation_functions.hpp create mode 100644 nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp create mode 100644 nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp create mode 100644 nav2_regulated_pure_pursuit_controller/src/path_handler.cpp diff --git a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt index 6c17481925..6f84b05382 100644 --- a/nav2_regulated_pure_pursuit_controller/CMakeLists.txt +++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt @@ -35,7 +35,10 @@ set(dependencies set(library_name nav2_regulated_pure_pursuit_controller) add_library(${library_name} SHARED - src/regulated_pure_pursuit_controller.cpp) + src/regulated_pure_pursuit_controller.cpp + src/collision_checker.cpp + src/parameter_handler.cpp + src/path_handler.cpp) ament_target_dependencies(${library_name} ${dependencies} diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp new file mode 100644 index 0000000000..baccc6ca20 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/collision_checker.hpp @@ -0,0 +1,105 @@ +// Copyright (c) 2022 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__COLLISION_CHECKER_HPP_ +#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__COLLISION_CHECKER_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_util/odometry_utils.hpp" +#include "geometry_msgs/msg/pose2_d.hpp" +#include "nav2_regulated_pure_pursuit_controller/parameter_handler.hpp" + +#include "nav2_core/controller_exceptions.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_costmap_2d/costmap_filters/filter_values.hpp" + +namespace nav2_regulated_pure_pursuit_controller +{ + +/** + * @class nav2_regulated_pure_pursuit_controller::CollisionChecker + * @brief Checks for collision based on a RPP control command + */ +class CollisionChecker +{ +public: + /** + * @brief Constructor for nav2_regulated_pure_pursuit_controller::CollisionChecker + */ + CollisionChecker( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::shared_ptr costmap_ros, Parameters * params); + + /** + * @brief Destrructor for nav2_regulated_pure_pursuit_controller::CollisionChecker + */ + ~CollisionChecker() = default; + + /** + * @brief Whether collision is imminent + * @param robot_pose Pose of robot + * @param carrot_pose Pose of carrot + * @param linear_vel linear velocity to forward project + * @param angular_vel angular velocity to forward project + * @param carrot_dist Distance to the carrot for PP + * @return Whether collision is imminent + */ + bool isCollisionImminent( + const geometry_msgs::msg::PoseStamped &, + const double &, const double &, + const double &); + + /** + * @brief checks for collision at projected pose + * @param x Pose of pose x + * @param y Pose of pose y + * @param theta orientation of Yaw + * @return Whether in collision + */ + bool inCollision( + const double & x, + const double & y, + const double & theta); + + /** + * @brief Cost at a point + * @param x Pose of pose x + * @param y Pose of pose y + * @return Cost of pose in costmap + */ + double costAtPose(const double & x, const double & y); + +protected: + rclcpp::Logger logger_ {rclcpp::get_logger("RPPCollisionChecker")}; + std::shared_ptr costmap_ros_; + nav2_costmap_2d::Costmap2D * costmap_; + std::unique_ptr> + footprint_collision_checker_; + Parameters * params_; + std::shared_ptr> carrot_arc_pub_; + rclcpp::Clock::SharedPtr clock_; +}; + +} // namespace nav2_regulated_pure_pursuit_controller + +#endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__COLLISION_CHECKER_HPP_ diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp new file mode 100644 index 0000000000..63e1d215e4 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/parameter_handler.hpp @@ -0,0 +1,104 @@ +// Copyright (c) 2022 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PARAMETER_HANDLER_HPP_ +#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PARAMETER_HANDLER_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav2_util/odometry_utils.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_util/node_utils.hpp" + +namespace nav2_regulated_pure_pursuit_controller +{ + +struct Parameters +{ + double desired_linear_vel, base_desired_linear_vel; + double lookahead_dist; + double rotate_to_heading_angular_vel; + double max_lookahead_dist; + double min_lookahead_dist; + double lookahead_time; + bool use_velocity_scaled_lookahead_dist; + double min_approach_linear_velocity; + double approach_velocity_scaling_dist; + double max_allowed_time_to_collision_up_to_carrot; + bool use_regulated_linear_velocity_scaling; + bool use_cost_regulated_linear_velocity_scaling; + double cost_scaling_dist; + double cost_scaling_gain; + double inflation_cost_scaling_factor; + double regulated_linear_scaling_min_radius; + double regulated_linear_scaling_min_speed; + bool use_rotate_to_heading; + double max_angular_accel; + double rotate_to_heading_min_angle; + bool allow_reversing; + double max_robot_pose_search_dist; + bool use_interpolation; + bool use_collision_detection; + double transform_tolerance; +}; + +/** + * @class nav2_regulated_pure_pursuit_controller::ParameterHandler + * @brief Handles parameters and dynamic parameters for RPP + */ +class ParameterHandler +{ +public: + /** + * @brief Constructor for nav2_regulated_pure_pursuit_controller::ParameterHandler + */ + ParameterHandler( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::string & plugin_name, + rclcpp::Logger & logger, const double costmap_size_x); + + /** + * @brief Destrructor for nav2_regulated_pure_pursuit_controller::ParameterHandler + */ + ~ParameterHandler() = default; + + std::mutex & getMutex() {return mutex_;} + + Parameters * getParams() {return ¶ms_;} + +protected: + /** + * @brief Callback executed when a parameter change is detected + * @param event ParameterEvent message + */ + rcl_interfaces::msg::SetParametersResult + dynamicParametersCallback(std::vector parameters); + + // Dynamic parameters handler + std::mutex mutex_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + Parameters params_; + std::string plugin_name_; + rclcpp::Logger logger_ {rclcpp::get_logger("RegulatedPurePursuitController")}; +}; + +} // namespace nav2_regulated_pure_pursuit_controller + +#endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PARAMETER_HANDLER_HPP_ diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp new file mode 100644 index 0000000000..22bc0266a6 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/path_handler.hpp @@ -0,0 +1,99 @@ +// Copyright (c) 2022 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PATH_HANDLER_HPP_ +#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PATH_HANDLER_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_util/odometry_utils.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_core/controller_exceptions.hpp" +#include "geometry_msgs/msg/pose2_d.hpp" + +namespace nav2_regulated_pure_pursuit_controller +{ + +/** + * @class nav2_regulated_pure_pursuit_controller::PathHandler + * @brief Handles input paths to transform them to local frames required + */ +class PathHandler +{ +public: + /** + * @brief Constructor for nav2_regulated_pure_pursuit_controller::PathHandler + */ + PathHandler( + tf2::Duration transform_tolerance, + std::shared_ptr tf, + std::shared_ptr costmap_ros); + + /** + * @brief Destrructor for nav2_regulated_pure_pursuit_controller::PathHandler + */ + ~PathHandler() = default; + + /** + * @brief Transforms global plan into same frame as pose and clips poses ineligible for lookaheadPoint + * Points ineligible to be selected as a lookahead point if they are any of the following: + * - Outside the local_costmap (collision avoidance cannot be assured) + * @param pose pose to transform + * @param max_robot_pose_search_dist Distance to search for matching nearest path point + * @return Path in new frame + */ + nav_msgs::msg::Path transformGlobalPlan( + const geometry_msgs::msg::PoseStamped & pose, + double max_robot_pose_search_dist); + + /** + * @brief Transform a pose to another frame. + * @param frame Frame ID to transform to + * @param in_pose Pose input to transform + * @param out_pose transformed output + * @return bool if successful + */ + bool transformPose( + const std::string frame, + const geometry_msgs::msg::PoseStamped & in_pose, + geometry_msgs::msg::PoseStamped & out_pose) const; + + void setPlan(const nav_msgs::msg::Path & path) {global_plan_ = path;} + + nav_msgs::msg::Path getPlan() {return global_plan_;} + +protected: + /** + * Get the greatest extent of the costmap in meters from the center. + * @return max of distance from center in meters to edge of costmap + */ + double getCostmapMaxExtent() const; + + rclcpp::Logger logger_ {rclcpp::get_logger("RPPPathHandler")}; + tf2::Duration transform_tolerance_; + std::shared_ptr tf_; + std::shared_ptr costmap_ros_; + nav_msgs::msg::Path global_plan_; +}; + +} // namespace nav2_regulated_pure_pursuit_controller + +#endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PATH_HANDLER_HPP_ diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 7c244d9393..7b28ca9720 100644 --- a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -22,14 +22,15 @@ #include #include -#include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_core/controller.hpp" #include "rclcpp/rclcpp.hpp" #include "pluginlib/class_loader.hpp" #include "pluginlib/class_list_macros.hpp" -#include "nav2_util/odometry_utils.hpp" -#include "nav2_util/geometry_utils.hpp" #include "geometry_msgs/msg/pose2_d.hpp" +#include "nav2_regulated_pure_pursuit_controller/path_handler.hpp" +#include "nav2_regulated_pure_pursuit_controller/collision_checker.hpp" +#include "nav2_regulated_pure_pursuit_controller/parameter_handler.hpp" +#include "nav2_regulated_pure_pursuit_controller/regulation_functions.hpp" namespace nav2_regulated_pure_pursuit_controller { @@ -111,28 +112,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller void setSpeedLimit(const double & speed_limit, const bool & percentage) override; protected: - /** - * @brief Transforms global plan into same frame as pose and clips poses ineligible for lookaheadPoint - * Points ineligible to be selected as a lookahead point if they are any of the following: - * - Outside the local_costmap (collision avoidance cannot be assured) - * @param pose pose to transform - * @return Path in new frame - */ - nav_msgs::msg::Path transformGlobalPlan( - const geometry_msgs::msg::PoseStamped & pose); - - /** - * @brief Transform a pose to another frame. - * @param frame Frame ID to transform to - * @param in_pose Pose input to transform - * @param out_pose transformed output - * @return bool if successful - */ - bool transformPose( - const std::string frame, - const geometry_msgs::msg::PoseStamped & in_pose, - geometry_msgs::msg::PoseStamped & out_pose) const; - /** * @brief Get lookahead distance * @param cmd the current speed to use to compute lookahead point @@ -175,48 +154,6 @@ class RegulatedPurePursuitController : public nav2_core::Controller double & linear_vel, double & angular_vel, const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed); - /** - * @brief Whether collision is imminent - * @param robot_pose Pose of robot - * @param carrot_pose Pose of carrot - * @param linear_vel linear velocity to forward project - * @param angular_vel angular velocity to forward project - * @param carrot_dist Distance to the carrot for PP - * @return Whether collision is imminent - */ - bool isCollisionImminent( - const geometry_msgs::msg::PoseStamped &, - const double &, const double &, - const double &); - - /** - * @brief checks for collision at projected pose - * @param x Pose of pose x - * @param y Pose of pose y - * @param theta orientation of Yaw - * @return Whether in collision - */ - bool inCollision( - const double & x, - const double & y, - const double & theta); - /** - * @brief Cost at a point - * @param x Pose of pose x - * @param y Pose of pose y - * @return Cost of pose in costmap - */ - double costAtPose(const double & x, const double & y); - - double approachVelocityScalingFactor( - const nav_msgs::msg::Path & path - ) const; - - void applyApproachVelocityScaling( - const nav_msgs::msg::Path & path, - double & linear_vel - ) const; - /** * @brief apply regulation constraints to the system * @param linear_vel robot command linear velocity input @@ -259,66 +196,24 @@ class RegulatedPurePursuitController : public nav2_core::Controller */ double findVelocitySignChange(const nav_msgs::msg::Path & transformed_plan); - /** - * Get the greatest extent of the costmap in meters from the center. - * @return max of distance from center in meters to edge of costmap - */ - double getCostmapMaxExtent() const; - - /** - * @brief Callback executed when a parameter change is detected - * @param event ParameterEvent message - */ - rcl_interfaces::msg::SetParametersResult - dynamicParametersCallback(std::vector parameters); - rclcpp_lifecycle::LifecycleNode::WeakPtr node_; std::shared_ptr tf_; std::string plugin_name_; std::shared_ptr costmap_ros_; nav2_costmap_2d::Costmap2D * costmap_; rclcpp::Logger logger_ {rclcpp::get_logger("RegulatedPurePursuitController")}; - rclcpp::Clock::SharedPtr clock_; - double desired_linear_vel_, base_desired_linear_vel_; - double lookahead_dist_; - double rotate_to_heading_angular_vel_; - double max_lookahead_dist_; - double min_lookahead_dist_; - double lookahead_time_; - bool use_velocity_scaled_lookahead_dist_; - tf2::Duration transform_tolerance_; - double min_approach_linear_velocity_; - double approach_velocity_scaling_dist_; - double control_duration_; - double max_allowed_time_to_collision_up_to_carrot_; - bool use_collision_detection_; - bool use_regulated_linear_velocity_scaling_; - bool use_cost_regulated_linear_velocity_scaling_; - double cost_scaling_dist_; - double cost_scaling_gain_; - double inflation_cost_scaling_factor_; - double regulated_linear_scaling_min_radius_; - double regulated_linear_scaling_min_speed_; - bool use_rotate_to_heading_; - double max_angular_accel_; - double rotate_to_heading_min_angle_; + Parameters * params_; double goal_dist_tol_; - bool allow_reversing_; - double max_robot_pose_search_dist_; - bool use_interpolation_; + double control_duration_; - nav_msgs::msg::Path global_plan_; std::shared_ptr> global_path_pub_; std::shared_ptr> carrot_pub_; std::shared_ptr> carrot_arc_pub_; - std::unique_ptr> - collision_checker_; - - // Dynamic parameters handler - std::mutex mutex_; - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + std::unique_ptr path_handler_; + std::unique_ptr param_handler_; + std::unique_ptr collision_checker_; }; } // namespace nav2_regulated_pure_pursuit_controller diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulation_functions.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulation_functions.hpp new file mode 100644 index 0000000000..61dca3487e --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulation_functions.hpp @@ -0,0 +1,138 @@ +// Copyright (c) 2022 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__REGULATION_FUNCTIONS_HPP_ +#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__REGULATION_FUNCTIONS_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_regulated_pure_pursuit_controller/parameter_handler.hpp" + +namespace nav2_regulated_pure_pursuit_controller +{ + +namespace heuristics +{ + +/** + * @brief apply curvature constraint regulation on the linear velocity + * @param raw_linear_velocity Raw linear velocity desired + * @param curvature Curvature of the current command to follow the path + * @param min_radius Minimum path radius to apply the heuristic + * @return Velocity after applying the curvature constraint + */ +inline double curvatureConstraint( + const double raw_linear_vel, const double curvature, const double min_radius) +{ + const double radius = fabs(1.0 / curvature); + if (radius < min_radius) { + return raw_linear_vel * (1.0 - (fabs(radius - min_radius) / min_radius)); + } else { + return raw_linear_vel; + } +} + +/** + * @brief apply cost constraint regulation on the linear velocity + * @param raw_linear_velocity Raw linear velocity desired + * @param pose_cost Cost at the robot pose + * @param costmap_ros Costmap object to query + * @param params Parameters + * @return Velocity after applying the curvature constraint + */ +inline double costConstraint( + const double raw_linear_vel, + const double pose_cost, + std::shared_ptr costmap_ros, + Parameters * params) +{ + using namespace nav2_costmap_2d; // NOLINT + + if (pose_cost != static_cast(NO_INFORMATION) && + pose_cost != static_cast(FREE_SPACE)) + { + const double & inscribed_radius = costmap_ros->getLayeredCostmap()->getInscribedRadius(); + const double min_distance_to_obstacle = (-1.0 / params->inflation_cost_scaling_factor) * + std::log(pose_cost / (INSCRIBED_INFLATED_OBSTACLE - 1)) + inscribed_radius; + + if (min_distance_to_obstacle < params->cost_scaling_dist) { + return raw_linear_vel * + (params->cost_scaling_gain * min_distance_to_obstacle / params->cost_scaling_dist); + } + } + + return raw_linear_vel; +} + +/** + * @brief Compute the scale factor to apply for linear velocity regulation on approach to goal + * @param transformed_path Path to use to calculate distances to goal + * @param approach_velocity_scaling_dist Minimum distance away to which to apply the heuristic + * @return A scale from 0.0-1.0 of the distance to goal scaled by minimum distance + */ +inline double approachVelocityScalingFactor( + const nav_msgs::msg::Path & transformed_path, + const double approach_velocity_scaling_dist) +{ + using namespace nav2_util::geometry_utils; // NOLINT + + // Waiting to apply the threshold based on integrated distance ensures we don't + // erroneously apply approach scaling on curvy paths that are contained in a large local costmap. + const double remaining_distance = calculate_path_length(transformed_path); + if (remaining_distance < approach_velocity_scaling_dist) { + auto & last = transformed_path.poses.back(); + // Here we will use a regular euclidean distance from the robot frame (origin) + // to get smooth scaling, regardless of path density. + return std::hypot(last.pose.position.x, last.pose.position.y) / approach_velocity_scaling_dist; + } else { + return 1.0; + } +} + +/** + * @brief Velocity on approach to goal heuristic regulation term + * @param constrained_linear_vel Linear velocity already constrained by heuristics + * @param path The path plan in the robot base frame coordinates + * @param min_approach_velocity Minimum velocity to use on approach to goal + * @param approach_velocity_scaling_dist Distance away from goal to start applying this heuristic + * @return Velocity after regulation via approach to goal slow-down + */ +inline double approachVelocityConstraint( + const double constrained_linear_vel, + const nav_msgs::msg::Path & path, + const double min_approach_velocity, + const double approach_velocity_scaling_dist) +{ + double velocity_scaling = approachVelocityScalingFactor(path, approach_velocity_scaling_dist); + double approach_vel = constrained_linear_vel * velocity_scaling; + + if (approach_vel < min_approach_velocity) { + approach_vel = min_approach_velocity; + } + + return std::min(constrained_linear_vel, approach_vel); +} + +} // namespace heuristics + +} // namespace nav2_regulated_pure_pursuit_controller + +#endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__REGULATION_FUNCTIONS_HPP_ diff --git a/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp b/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp new file mode 100644 index 0000000000..ac654fcab7 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/src/collision_checker.cpp @@ -0,0 +1,174 @@ +// Copyright (c) 2022 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +#include "nav2_regulated_pure_pursuit_controller/collision_checker.hpp" + +namespace nav2_regulated_pure_pursuit_controller +{ + +using namespace nav2_costmap_2d; // NOLINT + +CollisionChecker::CollisionChecker( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::shared_ptr costmap_ros, + Parameters * params) +{ + clock_ = node->get_clock(); + costmap_ros_ = costmap_ros; + costmap_ = costmap_ros_->getCostmap(); + params_ = params; + + // initialize collision checker and set costmap + footprint_collision_checker_ = std::make_unique>(costmap_); + footprint_collision_checker_->setCostmap(costmap_); + + carrot_arc_pub_ = node->create_publisher("lookahead_collision_arc", 1); + carrot_arc_pub_->on_activate(); +} + +bool CollisionChecker::isCollisionImminent( + const geometry_msgs::msg::PoseStamped & robot_pose, + const double & linear_vel, const double & angular_vel, + const double & carrot_dist) +{ + // Note(stevemacenski): This may be a bit unusual, but the robot_pose is in + // odom frame and the carrot_pose is in robot base frame. Just how the data comes to us + + // check current point is OK + if (inCollision( + robot_pose.pose.position.x, robot_pose.pose.position.y, + tf2::getYaw(robot_pose.pose.orientation))) + { + return true; + } + + // visualization messages + nav_msgs::msg::Path arc_pts_msg; + arc_pts_msg.header.frame_id = costmap_ros_->getGlobalFrameID(); + arc_pts_msg.header.stamp = robot_pose.header.stamp; + geometry_msgs::msg::PoseStamped pose_msg; + pose_msg.header.frame_id = arc_pts_msg.header.frame_id; + pose_msg.header.stamp = arc_pts_msg.header.stamp; + + double projection_time = 0.0; + if (fabs(linear_vel) < 0.01 && fabs(angular_vel) > 0.01) { + // rotating to heading at goal or toward path + // Equation finds the angular distance required for the largest + // part of the robot radius to move to another costmap cell: + // theta_min = 2.0 * sin ((res/2) / r_max) + // via isosceles triangle r_max-r_max-resolution, + // dividing by angular_velocity gives us a timestep. + double max_radius = costmap_ros_->getLayeredCostmap()->getCircumscribedRadius(); + projection_time = + 2.0 * sin((costmap_->getResolution() / 2) / max_radius) / fabs(angular_vel); + } else { + // Normal path tracking + projection_time = costmap_->getResolution() / fabs(linear_vel); + } + + const geometry_msgs::msg::Point & robot_xy = robot_pose.pose.position; + geometry_msgs::msg::Pose2D curr_pose; + curr_pose.x = robot_pose.pose.position.x; + curr_pose.y = robot_pose.pose.position.y; + curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation); + + // only forward simulate within time requested + int i = 1; + while (i * projection_time < params_->max_allowed_time_to_collision_up_to_carrot) { + i++; + + // apply velocity at curr_pose over distance + curr_pose.x += projection_time * (linear_vel * cos(curr_pose.theta)); + curr_pose.y += projection_time * (linear_vel * sin(curr_pose.theta)); + curr_pose.theta += projection_time * angular_vel; + + // check if past carrot pose, where no longer a thoughtfully valid command + if (hypot(curr_pose.x - robot_xy.x, curr_pose.y - robot_xy.y) > carrot_dist) { + break; + } + + // store it for visualization + pose_msg.pose.position.x = curr_pose.x; + pose_msg.pose.position.y = curr_pose.y; + pose_msg.pose.position.z = 0.01; + arc_pts_msg.poses.push_back(pose_msg); + + // check for collision at the projected pose + if (inCollision(curr_pose.x, curr_pose.y, curr_pose.theta)) { + carrot_arc_pub_->publish(arc_pts_msg); + return true; + } + } + + carrot_arc_pub_->publish(arc_pts_msg); + + return false; +} + +bool CollisionChecker::inCollision( + const double & x, + const double & y, + const double & theta) +{ + unsigned int mx, my; + + if (!costmap_->worldToMap(x, y, mx, my)) { + RCLCPP_WARN_THROTTLE( + logger_, *(clock_), 30000, + "The dimensions of the costmap is too small to successfully check for " + "collisions as far ahead as requested. Proceed at your own risk, slow the robot, or " + "increase your costmap size."); + return false; + } + + double footprint_cost = footprint_collision_checker_->footprintCostAtPose( + x, y, theta, costmap_ros_->getRobotFootprint()); + if (footprint_cost == static_cast(NO_INFORMATION) && + costmap_ros_->getLayeredCostmap()->isTrackingUnknown()) + { + return false; + } + + // if occupied or unknown and not to traverse unknown space + return footprint_cost >= static_cast(LETHAL_OBSTACLE); +} + + +double CollisionChecker::costAtPose(const double & x, const double & y) +{ + unsigned int mx, my; + + if (!costmap_->worldToMap(x, y, mx, my)) { + RCLCPP_FATAL( + logger_, + "The dimensions of the costmap is too small to fully include your robot's footprint, " + "thusly the robot cannot proceed further"); + throw nav2_core::ControllerException( + "RegulatedPurePursuitController: Dimensions of the costmap are too small " + "to encapsulate the robot footprint at current speeds!"); + } + + unsigned char cost = costmap_->getCost(mx, my); + return static_cast(cost); +} + +} // namespace nav2_regulated_pure_pursuit_controller diff --git a/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp new file mode 100644 index 0000000000..9bb4aa9427 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp @@ -0,0 +1,261 @@ +// Copyright (c) 2022 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +#include "nav2_regulated_pure_pursuit_controller/parameter_handler.hpp" + +namespace nav2_regulated_pure_pursuit_controller +{ + +using nav2_util::declare_parameter_if_not_declared; +using rcl_interfaces::msg::ParameterType; + +ParameterHandler::ParameterHandler( + rclcpp_lifecycle::LifecycleNode::SharedPtr node, + std::string & plugin_name, rclcpp::Logger & logger, + const double costmap_size_x) +{ + plugin_name_ = plugin_name; + logger_ = logger; + + declare_parameter_if_not_declared( + node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_lookahead_dist", rclcpp::ParameterValue(0.3)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_lookahead_dist", rclcpp::ParameterValue(0.9)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".lookahead_time", rclcpp::ParameterValue(1.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".rotate_to_heading_angular_vel", rclcpp::ParameterValue(1.8)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_velocity_scaled_lookahead_dist", + rclcpp::ParameterValue(false)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_approach_linear_velocity", rclcpp::ParameterValue(0.05)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".approach_velocity_scaling_dist", + rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", + rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", + rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".cost_scaling_dist", rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".cost_scaling_gain", rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".inflation_cost_scaling_factor", rclcpp::ParameterValue(3.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".regulated_linear_scaling_min_radius", rclcpp::ParameterValue(0.90)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".regulated_linear_scaling_min_speed", rclcpp::ParameterValue(0.25)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_rotate_to_heading", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".allow_reversing", rclcpp::ParameterValue(false)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_robot_pose_search_dist", + rclcpp::ParameterValue(costmap_size_x / 2.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_interpolation", + rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_collision_detection", + rclcpp::ParameterValue(true)); + + node->get_parameter(plugin_name_ + ".desired_linear_vel", params_.desired_linear_vel); + params_.base_desired_linear_vel = params_.desired_linear_vel; + node->get_parameter(plugin_name_ + ".lookahead_dist", params_.lookahead_dist); + node->get_parameter(plugin_name_ + ".min_lookahead_dist", params_.min_lookahead_dist); + node->get_parameter(plugin_name_ + ".max_lookahead_dist", params_.max_lookahead_dist); + node->get_parameter(plugin_name_ + ".lookahead_time", params_.lookahead_time); + node->get_parameter( + plugin_name_ + ".rotate_to_heading_angular_vel", + params_.rotate_to_heading_angular_vel); + node->get_parameter(plugin_name_ + ".transform_tolerance", params_.transform_tolerance); + node->get_parameter( + plugin_name_ + ".use_velocity_scaled_lookahead_dist", + params_.use_velocity_scaled_lookahead_dist); + node->get_parameter( + plugin_name_ + ".min_approach_linear_velocity", + params_.min_approach_linear_velocity); + node->get_parameter( + plugin_name_ + ".approach_velocity_scaling_dist", + params_.approach_velocity_scaling_dist); + if (params_.approach_velocity_scaling_dist > costmap_size_x / 2.0) { + RCLCPP_WARN( + logger_, "approach_velocity_scaling_dist is larger than forward costmap extent, " + "leading to permanent slowdown"); + } + node->get_parameter( + plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", + params_.max_allowed_time_to_collision_up_to_carrot); + node->get_parameter( + plugin_name_ + ".use_regulated_linear_velocity_scaling", + params_.use_regulated_linear_velocity_scaling); + node->get_parameter( + plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", + params_.use_cost_regulated_linear_velocity_scaling); + node->get_parameter(plugin_name_ + ".cost_scaling_dist", params_.cost_scaling_dist); + node->get_parameter(plugin_name_ + ".cost_scaling_gain", params_.cost_scaling_gain); + node->get_parameter( + plugin_name_ + ".inflation_cost_scaling_factor", + params_.inflation_cost_scaling_factor); + node->get_parameter( + plugin_name_ + ".regulated_linear_scaling_min_radius", + params_.regulated_linear_scaling_min_radius); + node->get_parameter( + plugin_name_ + ".regulated_linear_scaling_min_speed", + params_.regulated_linear_scaling_min_speed); + node->get_parameter(plugin_name_ + ".use_rotate_to_heading", params_.use_rotate_to_heading); + node->get_parameter( + plugin_name_ + ".rotate_to_heading_min_angle", params_.rotate_to_heading_min_angle); + node->get_parameter(plugin_name_ + ".max_angular_accel", params_.max_angular_accel); + node->get_parameter(plugin_name_ + ".allow_reversing", params_.allow_reversing); + node->get_parameter( + plugin_name_ + ".max_robot_pose_search_dist", + params_.max_robot_pose_search_dist); + node->get_parameter( + plugin_name_ + ".use_interpolation", + params_.use_interpolation); + node->get_parameter( + plugin_name_ + ".use_collision_detection", + params_.use_collision_detection); + + if (params_.inflation_cost_scaling_factor <= 0.0) { + RCLCPP_WARN( + logger_, "The value inflation_cost_scaling_factor is incorrectly set, " + "it should be >0. Disabling cost regulated linear velocity scaling."); + params_.use_cost_regulated_linear_velocity_scaling = false; + } + + /** Possible to drive in reverse direction if and only if + "use_rotate_to_heading" parameter is set to false **/ + + if (params_.use_rotate_to_heading && params_.allow_reversing) { + RCLCPP_WARN( + logger_, "Disabling reversing. Both use_rotate_to_heading and allow_reversing " + "parameter cannot be set to true. By default setting use_rotate_to_heading true"); + params_.allow_reversing = false; + } + + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &ParameterHandler::dynamicParametersCallback, + this, std::placeholders::_1)); +} + +rcl_interfaces::msg::SetParametersResult +ParameterHandler::dynamicParametersCallback( + std::vector parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + std::lock_guard lock_reinit(mutex_); + + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == plugin_name_ + ".inflation_cost_scaling_factor") { + if (parameter.as_double() <= 0.0) { + RCLCPP_WARN( + logger_, "The value inflation_cost_scaling_factor is incorrectly set, " + "it should be >0. Ignoring parameter update."); + continue; + } + params_.inflation_cost_scaling_factor = parameter.as_double(); + } else if (name == plugin_name_ + ".desired_linear_vel") { + params_.desired_linear_vel = parameter.as_double(); + params_.base_desired_linear_vel = parameter.as_double(); + } else if (name == plugin_name_ + ".lookahead_dist") { + params_.lookahead_dist = parameter.as_double(); + } else if (name == plugin_name_ + ".max_lookahead_dist") { + params_.max_lookahead_dist = parameter.as_double(); + } else if (name == plugin_name_ + ".min_lookahead_dist") { + params_.min_lookahead_dist = parameter.as_double(); + } else if (name == plugin_name_ + ".lookahead_time") { + params_.lookahead_time = parameter.as_double(); + } else if (name == plugin_name_ + ".rotate_to_heading_angular_vel") { + params_.rotate_to_heading_angular_vel = parameter.as_double(); + } else if (name == plugin_name_ + ".min_approach_linear_velocity") { + params_.min_approach_linear_velocity = parameter.as_double(); + } else if (name == plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot") { + params_.max_allowed_time_to_collision_up_to_carrot = parameter.as_double(); + } else if (name == plugin_name_ + ".cost_scaling_dist") { + params_.cost_scaling_dist = parameter.as_double(); + } else if (name == plugin_name_ + ".cost_scaling_gain") { + params_.cost_scaling_gain = parameter.as_double(); + } else if (name == plugin_name_ + ".regulated_linear_scaling_min_radius") { + params_.regulated_linear_scaling_min_radius = parameter.as_double(); + } else if (name == plugin_name_ + ".regulated_linear_scaling_min_speed") { + params_.regulated_linear_scaling_min_speed = parameter.as_double(); + } else if (name == plugin_name_ + ".max_angular_accel") { + params_.max_angular_accel = parameter.as_double(); + } else if (name == plugin_name_ + ".rotate_to_heading_min_angle") { + params_.rotate_to_heading_min_angle = parameter.as_double(); + } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") { + params_.use_velocity_scaled_lookahead_dist = parameter.as_bool(); + } else if (name == plugin_name_ + ".use_regulated_linear_velocity_scaling") { + params_.use_regulated_linear_velocity_scaling = parameter.as_bool(); + } else if (name == plugin_name_ + ".use_cost_regulated_linear_velocity_scaling") { + params_.use_cost_regulated_linear_velocity_scaling = parameter.as_bool(); + } else if (name == plugin_name_ + ".use_collision_detection") { + params_.use_collision_detection = parameter.as_bool(); + } else if (name == plugin_name_ + ".use_rotate_to_heading") { + if (parameter.as_bool() && params_.allow_reversing) { + RCLCPP_WARN( + logger_, "Both use_rotate_to_heading and allow_reversing " + "parameter cannot be set to true. Rejecting parameter update."); + continue; + } + params_.use_rotate_to_heading = parameter.as_bool(); + } else if (name == plugin_name_ + ".allow_reversing") { + if (params_.use_rotate_to_heading && parameter.as_bool()) { + RCLCPP_WARN( + logger_, "Both use_rotate_to_heading and allow_reversing " + "parameter cannot be set to true. Rejecting parameter update."); + continue; + } + params_.allow_reversing = parameter.as_bool(); + } + } + } + + result.successful = true; + return result; +} + +} // namespace nav2_regulated_pure_pursuit_controller diff --git a/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp new file mode 100644 index 0000000000..f4ceec759d --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp @@ -0,0 +1,137 @@ +// Copyright (c) 2022 Samsung Research America +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +#include "nav2_regulated_pure_pursuit_controller/path_handler.hpp" +#include "nav2_core/controller_exceptions.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/geometry_utils.hpp" + +namespace nav2_regulated_pure_pursuit_controller +{ + +using nav2_util::geometry_utils::euclidean_distance; + +PathHandler::PathHandler( + tf2::Duration transform_tolerance, + std::shared_ptr tf, + std::shared_ptr costmap_ros) +: transform_tolerance_(transform_tolerance), tf_(tf), costmap_ros_(costmap_ros) +{ +} + +double PathHandler::getCostmapMaxExtent() const +{ + const double max_costmap_dim_meters = std::max( + costmap_ros_->getCostmap()->getSizeInMetersX(), + costmap_ros_->getCostmap()->getSizeInMetersY()); + return max_costmap_dim_meters / 2.0; +} + +nav_msgs::msg::Path PathHandler::transformGlobalPlan( + const geometry_msgs::msg::PoseStamped & pose, + double max_robot_pose_search_dist) +{ + if (global_plan_.poses.empty()) { + throw nav2_core::InvalidPath("Received plan with zero length"); + } + + // let's get the pose of the robot in the frame of the plan + geometry_msgs::msg::PoseStamped robot_pose; + if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) { + throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame"); + } + + auto closest_pose_upper_bound = + nav2_util::geometry_utils::first_after_integrated_distance( + global_plan_.poses.begin(), global_plan_.poses.end(), max_robot_pose_search_dist); + + // First find the closest pose on the path to the robot + // bounded by when the path turns around (if it does) so we don't get a pose from a later + // portion of the path + auto transformation_begin = + nav2_util::geometry_utils::min_by( + global_plan_.poses.begin(), closest_pose_upper_bound, + [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) { + return euclidean_distance(robot_pose, ps); + }); + + // We'll discard points on the plan that are outside the local costmap + const double max_costmap_extent = getCostmapMaxExtent(); + auto transformation_end = std::find_if( + transformation_begin, global_plan_.poses.end(), + [&](const auto & pose) { + return euclidean_distance(pose, robot_pose) > max_costmap_extent; + }); + + // Lambda to transform a PoseStamped from global frame to local + auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { + geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; + stamped_pose.header.frame_id = global_plan_.header.frame_id; + stamped_pose.header.stamp = robot_pose.header.stamp; + stamped_pose.pose = global_plan_pose.pose; + if (!transformPose(costmap_ros_->getBaseFrameID(), stamped_pose, transformed_pose)) { + throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame"); + } + transformed_pose.pose.position.z = 0.0; + return transformed_pose; + }; + + // Transform the near part of the global plan into the robot's frame of reference. + nav_msgs::msg::Path transformed_plan; + std::transform( + transformation_begin, transformation_end, + std::back_inserter(transformed_plan.poses), + transformGlobalPoseToLocal); + transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); + transformed_plan.header.stamp = robot_pose.header.stamp; + + // Remove the portion of the global plan that we've already passed so we don't + // process it on the next iteration (this is called path pruning) + global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin); + + if (transformed_plan.poses.empty()) { + throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); + } + + return transformed_plan; +} + +bool PathHandler::transformPose( + const std::string frame, + const geometry_msgs::msg::PoseStamped & in_pose, + geometry_msgs::msg::PoseStamped & out_pose) const +{ + if (in_pose.header.frame_id == frame) { + out_pose = in_pose; + return true; + } + + try { + tf_->transform(in_pose, out_pose, frame, transform_tolerance_); + out_pose.header.frame_id = frame; + return true; + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(logger_, "Exception in transformPose: %s", ex.what()); + } + return false; +} + +} // namespace nav2_regulated_pure_pursuit_controller diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index d5c648bd1a..674a456c57 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -30,10 +30,7 @@ using std::hypot; using std::min; using std::max; using std::abs; -using nav2_util::declare_parameter_if_not_declared; -using nav2_util::geometry_utils::euclidean_distance; using namespace nav2_costmap_2d; // NOLINT -using rcl_interfaces::msg::ParameterType; namespace nav2_regulated_pure_pursuit_controller { @@ -54,157 +51,28 @@ void RegulatedPurePursuitController::configure( tf_ = tf; plugin_name_ = name; logger_ = node->get_logger(); - clock_ = node->get_clock(); - double transform_tolerance = 0.1; + // Handles storage and dynamic configuration of parameters. + // Returns pointer to data current param settings. + param_handler_ = std::make_unique( + node, plugin_name_, logger_, costmap_->getSizeInMetersX()); + params_ = param_handler_->getParams(); + + // Handles global path transformations + path_handler_ = std::make_unique( + tf2::durationFromSec(params_->transform_tolerance), tf_, costmap_ros_); + + // Checks for imminent collisions + collision_checker_ = std::make_unique(node, costmap_ros_, params_); + double control_frequency = 20.0; goal_dist_tol_ = 0.25; // reasonable default before first update - declare_parameter_if_not_declared( - node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue(0.6)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".min_lookahead_dist", rclcpp::ParameterValue(0.3)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_lookahead_dist", rclcpp::ParameterValue(0.9)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".lookahead_time", rclcpp::ParameterValue(1.5)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".rotate_to_heading_angular_vel", rclcpp::ParameterValue(1.8)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_velocity_scaled_lookahead_dist", - rclcpp::ParameterValue(false)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".min_approach_linear_velocity", rclcpp::ParameterValue(0.05)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".approach_velocity_scaling_dist", - rclcpp::ParameterValue(0.6)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", - rclcpp::ParameterValue(1.0)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_collision_detection", - rclcpp::ParameterValue(true)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", - rclcpp::ParameterValue(true)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".cost_scaling_dist", rclcpp::ParameterValue(0.6)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".cost_scaling_gain", rclcpp::ParameterValue(1.0)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".inflation_cost_scaling_factor", rclcpp::ParameterValue(3.0)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".regulated_linear_scaling_min_radius", rclcpp::ParameterValue(0.90)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".regulated_linear_scaling_min_speed", rclcpp::ParameterValue(0.25)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_rotate_to_heading", rclcpp::ParameterValue(true)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".allow_reversing", rclcpp::ParameterValue(false)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_robot_pose_search_dist", - rclcpp::ParameterValue(getCostmapMaxExtent())); - declare_parameter_if_not_declared( - node, plugin_name_ + ".use_interpolation", - rclcpp::ParameterValue(true)); - - node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_); - base_desired_linear_vel_ = desired_linear_vel_; - node->get_parameter(plugin_name_ + ".lookahead_dist", lookahead_dist_); - node->get_parameter(plugin_name_ + ".min_lookahead_dist", min_lookahead_dist_); - node->get_parameter(plugin_name_ + ".max_lookahead_dist", max_lookahead_dist_); - node->get_parameter(plugin_name_ + ".lookahead_time", lookahead_time_); - node->get_parameter( - plugin_name_ + ".rotate_to_heading_angular_vel", - rotate_to_heading_angular_vel_); - node->get_parameter(plugin_name_ + ".transform_tolerance", transform_tolerance); - node->get_parameter( - plugin_name_ + ".use_velocity_scaled_lookahead_dist", - use_velocity_scaled_lookahead_dist_); - node->get_parameter( - plugin_name_ + ".min_approach_linear_velocity", - min_approach_linear_velocity_); - node->get_parameter( - plugin_name_ + ".approach_velocity_scaling_dist", - approach_velocity_scaling_dist_); - if (approach_velocity_scaling_dist_ > costmap_->getSizeInMetersX() / 2.0) { - RCLCPP_WARN( - logger_, "approach_velocity_scaling_dist is larger than forward costmap extent, " - "leading to permanent slowdown"); - } - node->get_parameter( - plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot", - max_allowed_time_to_collision_up_to_carrot_); - node->get_parameter( - plugin_name_ + ".use_collision_detection", - use_collision_detection_); - node->get_parameter( - plugin_name_ + ".use_regulated_linear_velocity_scaling", - use_regulated_linear_velocity_scaling_); - node->get_parameter( - plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", - use_cost_regulated_linear_velocity_scaling_); - node->get_parameter(plugin_name_ + ".cost_scaling_dist", cost_scaling_dist_); - node->get_parameter(plugin_name_ + ".cost_scaling_gain", cost_scaling_gain_); - node->get_parameter( - plugin_name_ + ".inflation_cost_scaling_factor", - inflation_cost_scaling_factor_); - node->get_parameter( - plugin_name_ + ".regulated_linear_scaling_min_radius", - regulated_linear_scaling_min_radius_); - node->get_parameter( - plugin_name_ + ".regulated_linear_scaling_min_speed", - regulated_linear_scaling_min_speed_); - node->get_parameter(plugin_name_ + ".use_rotate_to_heading", use_rotate_to_heading_); - node->get_parameter(plugin_name_ + ".rotate_to_heading_min_angle", rotate_to_heading_min_angle_); - node->get_parameter(plugin_name_ + ".max_angular_accel", max_angular_accel_); - node->get_parameter(plugin_name_ + ".allow_reversing", allow_reversing_); node->get_parameter("controller_frequency", control_frequency); - node->get_parameter( - plugin_name_ + ".max_robot_pose_search_dist", - max_robot_pose_search_dist_); - node->get_parameter( - plugin_name_ + ".use_interpolation", - use_interpolation_); - - transform_tolerance_ = tf2::durationFromSec(transform_tolerance); control_duration_ = 1.0 / control_frequency; - if (inflation_cost_scaling_factor_ <= 0.0) { - RCLCPP_WARN( - logger_, "The value inflation_cost_scaling_factor is incorrectly set, " - "it should be >0. Disabling cost regulated linear velocity scaling."); - use_cost_regulated_linear_velocity_scaling_ = false; - } - - /** Possible to drive in reverse direction if and only if - "use_rotate_to_heading" parameter is set to false **/ - - if (use_rotate_to_heading_ && allow_reversing_) { - RCLCPP_WARN( - logger_, "Disabling reversing. Both use_rotate_to_heading and allow_reversing " - "parameter cannot be set to true. By default setting use_rotate_to_heading true"); - allow_reversing_ = false; - } - global_path_pub_ = node->create_publisher("received_global_plan", 1); carrot_pub_ = node->create_publisher("lookahead_point", 1); - carrot_arc_pub_ = node->create_publisher("lookahead_collision_arc", 1); - - // initialize collision checker and set costmap - collision_checker_ = std::make_unique>(costmap_); - collision_checker_->setCostmap(costmap_); } void RegulatedPurePursuitController::cleanup() @@ -216,7 +84,6 @@ void RegulatedPurePursuitController::cleanup() plugin_name_.c_str()); global_path_pub_.reset(); carrot_pub_.reset(); - carrot_arc_pub_.reset(); } void RegulatedPurePursuitController::activate() @@ -228,13 +95,6 @@ void RegulatedPurePursuitController::activate() plugin_name_.c_str()); global_path_pub_->on_activate(); carrot_pub_->on_activate(); - carrot_arc_pub_->on_activate(); - // Add callback for dynamic parameters - auto node = node_.lock(); - dyn_params_handler_ = node->add_on_set_parameters_callback( - std::bind( - &RegulatedPurePursuitController::dynamicParametersCallback, - this, std::placeholders::_1)); } void RegulatedPurePursuitController::deactivate() @@ -246,8 +106,6 @@ void RegulatedPurePursuitController::deactivate() plugin_name_.c_str()); global_path_pub_->on_deactivate(); carrot_pub_->on_deactivate(); - carrot_arc_pub_->on_deactivate(); - dyn_params_handler_.reset(); } std::unique_ptr RegulatedPurePursuitController::createCarrotMsg( @@ -266,10 +124,11 @@ double RegulatedPurePursuitController::getLookAheadDistance( { // If using velocity-scaled look ahead distances, find and clamp the dist // Else, use the static look ahead distance - double lookahead_dist = lookahead_dist_; - if (use_velocity_scaled_lookahead_dist_) { - lookahead_dist = fabs(speed.linear.x) * lookahead_time_; - lookahead_dist = std::clamp(lookahead_dist, min_lookahead_dist_, max_lookahead_dist_); + double lookahead_dist = params_->lookahead_dist; + if (params_->use_velocity_scaled_lookahead_dist) { + lookahead_dist = fabs(speed.linear.x) * params_->lookahead_time; + lookahead_dist = std::clamp( + lookahead_dist, params_->min_lookahead_dist, params_->max_lookahead_dist); } return lookahead_dist; @@ -280,7 +139,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity const geometry_msgs::msg::Twist & speed, nav2_core::GoalChecker * goal_checker) { - std::lock_guard lock_reinit(mutex_); + std::lock_guard lock_reinit(param_handler_->getMutex()); // Update for the current goal checker's state geometry_msgs::msg::Pose pose_tolerance; @@ -292,15 +151,17 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity } // Transform path to robot base frame - auto transformed_plan = transformGlobalPlan(pose); + auto transformed_plan = path_handler_->transformGlobalPlan( + pose, params_->max_robot_pose_search_dist); + global_path_pub_->publish(transformed_plan); // Find look ahead distance and point on path and publish double lookahead_dist = getLookAheadDistance(speed); // Check for reverse driving - if (allow_reversing_) { + if (params_->allow_reversing) { // Cusp check - double dist_to_cusp = findVelocitySignChange(transformed_plan); + const double dist_to_cusp = findVelocitySignChange(transformed_plan); // if the lookahead distance is further than the cusp, use the cusp distance instead if (dist_to_cusp < lookahead_dist) { @@ -308,6 +169,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity } } + // Get the particular point on the path at the lookahead distance auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan); carrot_pub_->publish(createCarrotMsg(carrot_pose)); @@ -327,11 +189,11 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // Setting the velocity direction double sign = 1.0; - if (allow_reversing_) { + if (params_->allow_reversing) { sign = carrot_pose.pose.position.x >= 0.0 ? 1.0 : -1.0; } - linear_vel = desired_linear_vel_; + linear_vel = params_->desired_linear_vel; // Make sure we're in compliance with basic constraints double angle_to_heading; @@ -343,7 +205,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity } else { applyConstraints( curvature, speed, - costAtPose(pose.pose.position.x, pose.pose.position.y), transformed_plan, + collision_checker_->costAtPose(pose.pose.position.x, pose.pose.position.y), transformed_plan, linear_vel, sign); // Apply curvature to angular velocity after constraining linear velocity @@ -352,7 +214,9 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // Collision checking on this velocity heading const double & carrot_dist = hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y); - if (use_collision_detection_ && isCollisionImminent(pose, linear_vel, angular_vel, carrot_dist)) { + if (params_->use_collision_detection && + collision_checker_->isCollisionImminent(pose, linear_vel, angular_vel, carrot_dist)) + { throw nav2_core::NoValidControl("RegulatedPurePursuitController detected collision ahead!"); } @@ -369,7 +233,8 @@ bool RegulatedPurePursuitController::shouldRotateToPath( { // Whether we should rotate robot to rough path heading angle_to_path = atan2(carrot_pose.pose.position.y, carrot_pose.pose.position.x); - return use_rotate_to_heading_ && fabs(angle_to_path) > rotate_to_heading_min_angle_; + return params_->use_rotate_to_heading && + fabs(angle_to_path) > params_->rotate_to_heading_min_angle; } bool RegulatedPurePursuitController::shouldRotateToGoalHeading( @@ -377,7 +242,7 @@ bool RegulatedPurePursuitController::shouldRotateToGoalHeading( { // Whether we should rotate robot to goal heading double dist_to_goal = std::hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y); - return use_rotate_to_heading_ && dist_to_goal < goal_dist_tol_; + return params_->use_rotate_to_heading && dist_to_goal < goal_dist_tol_; } void RegulatedPurePursuitController::rotateToHeading( @@ -387,11 +252,11 @@ void RegulatedPurePursuitController::rotateToHeading( // Rotate in place using max angular velocity / acceleration possible linear_vel = 0.0; const double sign = angle_to_path > 0.0 ? 1.0 : -1.0; - angular_vel = sign * rotate_to_heading_angular_vel_; + angular_vel = sign * params_->rotate_to_heading_angular_vel; const double & dt = control_duration_; - const double min_feasible_angular_speed = curr_speed.angular.z - max_angular_accel_ * dt; - const double max_feasible_angular_speed = curr_speed.angular.z + max_angular_accel_ * dt; + const double min_feasible_angular_speed = curr_speed.angular.z - params_->max_angular_accel * dt; + const double max_feasible_angular_speed = curr_speed.angular.z + params_->max_angular_accel * dt; angular_vel = std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed); } @@ -443,7 +308,7 @@ geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoin // If the no pose is not far enough, take the last pose if (goal_pose_it == transformed_plan.poses.end()) { goal_pose_it = std::prev(transformed_plan.poses.end()); - } else if (use_interpolation_ && goal_pose_it != transformed_plan.poses.begin()) { + } else if (params_->use_interpolation && goal_pose_it != transformed_plan.poses.begin()) { // Find the point on the line segment between the two poses // that is exactly the lookahead distance away from the robot pose (the origin) // This can be found with a closed form for the intersection of a segment and a circle @@ -463,300 +328,62 @@ geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoin return *goal_pose_it; } -bool RegulatedPurePursuitController::isCollisionImminent( - const geometry_msgs::msg::PoseStamped & robot_pose, - const double & linear_vel, const double & angular_vel, - const double & carrot_dist) -{ - // Note(stevemacenski): This may be a bit unusual, but the robot_pose is in - // odom frame and the carrot_pose is in robot base frame. - - // check current point is OK - if (inCollision( - robot_pose.pose.position.x, robot_pose.pose.position.y, - tf2::getYaw(robot_pose.pose.orientation))) - { - return true; - } - - // visualization messages - nav_msgs::msg::Path arc_pts_msg; - arc_pts_msg.header.frame_id = costmap_ros_->getGlobalFrameID(); - arc_pts_msg.header.stamp = robot_pose.header.stamp; - geometry_msgs::msg::PoseStamped pose_msg; - pose_msg.header.frame_id = arc_pts_msg.header.frame_id; - pose_msg.header.stamp = arc_pts_msg.header.stamp; - - double projection_time = 0.0; - if (fabs(linear_vel) < 0.01 && fabs(angular_vel) > 0.01) { - // rotating to heading at goal or toward path - // Equation finds the angular distance required for the largest - // part of the robot radius to move to another costmap cell: - // theta_min = 2.0 * sin ((res/2) / r_max) - // via isosceles triangle r_max-r_max-resolution, - // dividing by angular_velocity gives us a timestep. - double max_radius = costmap_ros_->getLayeredCostmap()->getCircumscribedRadius(); - projection_time = - 2.0 * sin((costmap_->getResolution() / 2) / max_radius) / fabs(angular_vel); - } else { - // Normal path tracking - projection_time = costmap_->getResolution() / fabs(linear_vel); - } - - const geometry_msgs::msg::Point & robot_xy = robot_pose.pose.position; - geometry_msgs::msg::Pose2D curr_pose; - curr_pose.x = robot_pose.pose.position.x; - curr_pose.y = robot_pose.pose.position.y; - curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation); - - // only forward simulate within time requested - int i = 1; - while (i * projection_time < max_allowed_time_to_collision_up_to_carrot_) { - i++; - - // apply velocity at curr_pose over distance - curr_pose.x += projection_time * (linear_vel * cos(curr_pose.theta)); - curr_pose.y += projection_time * (linear_vel * sin(curr_pose.theta)); - curr_pose.theta += projection_time * angular_vel; - - // check if past carrot pose, where no longer a thoughtfully valid command - if (hypot(curr_pose.x - robot_xy.x, curr_pose.y - robot_xy.y) > carrot_dist) { - break; - } - - // store it for visualization - pose_msg.pose.position.x = curr_pose.x; - pose_msg.pose.position.y = curr_pose.y; - pose_msg.pose.position.z = 0.01; - arc_pts_msg.poses.push_back(pose_msg); - - // check for collision at the projected pose - if (inCollision(curr_pose.x, curr_pose.y, curr_pose.theta)) { - carrot_arc_pub_->publish(arc_pts_msg); - return true; - } - } - - carrot_arc_pub_->publish(arc_pts_msg); - - return false; -} - -bool RegulatedPurePursuitController::inCollision( - const double & x, - const double & y, - const double & theta) -{ - unsigned int mx, my; - - if (!costmap_->worldToMap(x, y, mx, my)) { - RCLCPP_WARN_THROTTLE( - logger_, *(clock_), 30000, - "The dimensions of the costmap is too small to successfully check for " - "collisions as far ahead as requested. Proceed at your own risk, slow the robot, or " - "increase your costmap size."); - return false; - } - - double footprint_cost = collision_checker_->footprintCostAtPose( - x, y, theta, costmap_ros_->getRobotFootprint()); - if (footprint_cost == static_cast(NO_INFORMATION) && - costmap_ros_->getLayeredCostmap()->isTrackingUnknown()) - { - return false; - } - - // if occupied or unknown and not to traverse unknown space - return footprint_cost >= static_cast(LETHAL_OBSTACLE); -} - -double RegulatedPurePursuitController::costAtPose(const double & x, const double & y) -{ - unsigned int mx, my; - - if (!costmap_->worldToMap(x, y, mx, my)) { - RCLCPP_FATAL( - logger_, - "The dimensions of the costmap is too small to fully include your robot's footprint, " - "thusly the robot cannot proceed further"); - throw nav2_core::ControllerException( - "RegulatedPurePursuitController: Dimensions of the costmap are too small " - "to encapsulate the robot footprint at current speeds!"); - } - - unsigned char cost = costmap_->getCost(mx, my); - return static_cast(cost); -} - -double RegulatedPurePursuitController::approachVelocityScalingFactor( - const nav_msgs::msg::Path & transformed_path -) const -{ - // Waiting to apply the threshold based on integrated distance ensures we don't - // erroneously apply approach scaling on curvy paths that are contained in a large local costmap. - double remaining_distance = nav2_util::geometry_utils::calculate_path_length(transformed_path); - if (remaining_distance < approach_velocity_scaling_dist_) { - auto & last = transformed_path.poses.back(); - // Here we will use a regular euclidean distance from the robot frame (origin) - // to get smooth scaling, regardless of path density. - double distance_to_last_pose = std::hypot(last.pose.position.x, last.pose.position.y); - return distance_to_last_pose / approach_velocity_scaling_dist_; - } else { - return 1.0; - } -} - -void RegulatedPurePursuitController::applyApproachVelocityScaling( - const nav_msgs::msg::Path & path, - double & linear_vel -) const -{ - double approach_vel = linear_vel; - double velocity_scaling = approachVelocityScalingFactor(path); - double unbounded_vel = approach_vel * velocity_scaling; - if (unbounded_vel < min_approach_linear_velocity_) { - approach_vel = min_approach_linear_velocity_; - } else { - approach_vel *= velocity_scaling; - } - - // Use the lowest velocity between approach and other constraints, if all overlapping - linear_vel = std::min(linear_vel, approach_vel); -} - void RegulatedPurePursuitController::applyConstraints( const double & curvature, const geometry_msgs::msg::Twist & /*curr_speed*/, const double & pose_cost, const nav_msgs::msg::Path & path, double & linear_vel, double & sign) { - double curvature_vel = linear_vel; - double cost_vel = linear_vel; + double curvature_vel = linear_vel, cost_vel = linear_vel; // limit the linear velocity by curvature - const double radius = fabs(1.0 / curvature); - const double & min_rad = regulated_linear_scaling_min_radius_; - if (use_regulated_linear_velocity_scaling_ && radius < min_rad) { - curvature_vel *= 1.0 - (fabs(radius - min_rad) / min_rad); + if (params_->use_regulated_linear_velocity_scaling) { + curvature_vel = heuristics::curvatureConstraint( + linear_vel, curvature, params_->regulated_linear_scaling_min_radius); } // limit the linear velocity by proximity to obstacles - if (use_cost_regulated_linear_velocity_scaling_ && - pose_cost != static_cast(NO_INFORMATION) && - pose_cost != static_cast(FREE_SPACE)) - { - const double inscribed_radius = costmap_ros_->getLayeredCostmap()->getInscribedRadius(); - const double min_distance_to_obstacle = (-1.0 / inflation_cost_scaling_factor_) * - std::log(pose_cost / (INSCRIBED_INFLATED_OBSTACLE - 1)) + inscribed_radius; - - if (min_distance_to_obstacle < cost_scaling_dist_) { - cost_vel *= cost_scaling_gain_ * min_distance_to_obstacle / cost_scaling_dist_; - } + if (params_->use_cost_regulated_linear_velocity_scaling) { + cost_vel = heuristics::costConstraint(linear_vel, pose_cost, costmap_ros_, params_); } - // Use the lowest of the 2 constraint heuristics, but above the minimum translational speed + // Use the lowest of the 2 constraints, but above the minimum translational speed linear_vel = std::min(cost_vel, curvature_vel); - linear_vel = std::max(linear_vel, regulated_linear_scaling_min_speed_); + linear_vel = std::max(linear_vel, params_->regulated_linear_scaling_min_speed); - applyApproachVelocityScaling(path, linear_vel); + // Apply constraint to reduce speed on approach to the final goal pose + linear_vel = heuristics::approachVelocityConstraint( + linear_vel, path, params_->min_approach_linear_velocity, + params_->approach_velocity_scaling_dist); // Limit linear velocities to be valid - linear_vel = std::clamp(fabs(linear_vel), 0.0, desired_linear_vel_); + linear_vel = std::clamp(fabs(linear_vel), 0.0, params_->desired_linear_vel); linear_vel = sign * linear_vel; } void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path) { - global_plan_ = path; + path_handler_->setPlan(path); } void RegulatedPurePursuitController::setSpeedLimit( const double & speed_limit, const bool & percentage) { + std::lock_guard lock_reinit(param_handler_->getMutex()); + if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) { // Restore default value - desired_linear_vel_ = base_desired_linear_vel_; + params_->desired_linear_vel = params_->base_desired_linear_vel; } else { if (percentage) { // Speed limit is expressed in % from maximum speed of robot - desired_linear_vel_ = base_desired_linear_vel_ * speed_limit / 100.0; + params_->desired_linear_vel = params_->base_desired_linear_vel * speed_limit / 100.0; } else { // Speed limit is expressed in absolute value - desired_linear_vel_ = speed_limit; + params_->desired_linear_vel = speed_limit; } } } -nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( - const geometry_msgs::msg::PoseStamped & pose) -{ - if (global_plan_.poses.empty()) { - throw nav2_core::InvalidPath("Received plan with zero length"); - } - - // let's get the pose of the robot in the frame of the plan - geometry_msgs::msg::PoseStamped robot_pose; - if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) { - throw nav2_core::ControllerTFError("Unable to transform robot pose into global plan's frame"); - } - - // We'll discard points on the plan that are outside the local costmap - double max_costmap_extent = getCostmapMaxExtent(); - - auto closest_pose_upper_bound = - nav2_util::geometry_utils::first_after_integrated_distance( - global_plan_.poses.begin(), global_plan_.poses.end(), max_robot_pose_search_dist_); - - // First find the closest pose on the path to the robot - // bounded by when the path turns around (if it does) so we don't get a pose from a later - // portion of the path - auto transformation_begin = - nav2_util::geometry_utils::min_by( - global_plan_.poses.begin(), closest_pose_upper_bound, - [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) { - return euclidean_distance(robot_pose, ps); - }); - - // Find points up to max_transform_dist so we only transform them. - auto transformation_end = std::find_if( - transformation_begin, global_plan_.poses.end(), - [&](const auto & pose) { - return euclidean_distance(pose, robot_pose) > max_costmap_extent; - }); - - // Lambda to transform a PoseStamped from global frame to local - auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { - geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; - stamped_pose.header.frame_id = global_plan_.header.frame_id; - stamped_pose.header.stamp = robot_pose.header.stamp; - stamped_pose.pose = global_plan_pose.pose; - if (!transformPose(costmap_ros_->getBaseFrameID(), stamped_pose, transformed_pose)) { - throw nav2_core::ControllerTFError("Unable to transform plan pose into local frame"); - } - transformed_pose.pose.position.z = 0.0; - return transformed_pose; - }; - - // Transform the near part of the global plan into the robot's frame of reference. - nav_msgs::msg::Path transformed_plan; - std::transform( - transformation_begin, transformation_end, - std::back_inserter(transformed_plan.poses), - transformGlobalPoseToLocal); - transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); - transformed_plan.header.stamp = robot_pose.header.stamp; - - // Remove the portion of the global plan that we've already passed so we don't - // process it on the next iteration (this is called path pruning) - global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin); - global_path_pub_->publish(transformed_plan); - - if (transformed_plan.poses.empty()) { - throw nav2_core::InvalidPath("Resulting plan has 0 poses in it."); - } - - return transformed_plan; -} - double RegulatedPurePursuitController::findVelocitySignChange( const nav_msgs::msg::Path & transformed_plan) { @@ -786,119 +413,6 @@ double RegulatedPurePursuitController::findVelocitySignChange( return std::numeric_limits::max(); } - -bool RegulatedPurePursuitController::transformPose( - const std::string frame, - const geometry_msgs::msg::PoseStamped & in_pose, - geometry_msgs::msg::PoseStamped & out_pose) const -{ - if (in_pose.header.frame_id == frame) { - out_pose = in_pose; - return true; - } - - try { - tf_->transform(in_pose, out_pose, frame, transform_tolerance_); - out_pose.header.frame_id = frame; - return true; - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR(logger_, "Exception in transformPose: %s", ex.what()); - } - return false; -} - -double RegulatedPurePursuitController::getCostmapMaxExtent() const -{ - const double max_costmap_dim_meters = std::max( - costmap_->getSizeInMetersX(), costmap_->getSizeInMetersY()); - return max_costmap_dim_meters / 2.0; -} - - -rcl_interfaces::msg::SetParametersResult -RegulatedPurePursuitController::dynamicParametersCallback( - std::vector parameters) -{ - rcl_interfaces::msg::SetParametersResult result; - std::lock_guard lock_reinit(mutex_); - - for (auto parameter : parameters) { - const auto & type = parameter.get_type(); - const auto & name = parameter.get_name(); - - if (type == ParameterType::PARAMETER_DOUBLE) { - if (name == plugin_name_ + ".inflation_cost_scaling_factor") { - if (parameter.as_double() <= 0.0) { - RCLCPP_WARN( - logger_, "The value inflation_cost_scaling_factor is incorrectly set, " - "it should be >0. Ignoring parameter update."); - continue; - } - inflation_cost_scaling_factor_ = parameter.as_double(); - } else if (name == plugin_name_ + ".desired_linear_vel") { - desired_linear_vel_ = parameter.as_double(); - base_desired_linear_vel_ = parameter.as_double(); - } else if (name == plugin_name_ + ".lookahead_dist") { - lookahead_dist_ = parameter.as_double(); - } else if (name == plugin_name_ + ".max_lookahead_dist") { - max_lookahead_dist_ = parameter.as_double(); - } else if (name == plugin_name_ + ".min_lookahead_dist") { - min_lookahead_dist_ = parameter.as_double(); - } else if (name == plugin_name_ + ".lookahead_time") { - lookahead_time_ = parameter.as_double(); - } else if (name == plugin_name_ + ".rotate_to_heading_angular_vel") { - rotate_to_heading_angular_vel_ = parameter.as_double(); - } else if (name == plugin_name_ + ".min_approach_linear_velocity") { - min_approach_linear_velocity_ = parameter.as_double(); - } else if (name == plugin_name_ + ".max_allowed_time_to_collision_up_to_carrot") { - max_allowed_time_to_collision_up_to_carrot_ = parameter.as_double(); - } else if (name == plugin_name_ + ".cost_scaling_dist") { - cost_scaling_dist_ = parameter.as_double(); - } else if (name == plugin_name_ + ".cost_scaling_gain") { - cost_scaling_gain_ = parameter.as_double(); - } else if (name == plugin_name_ + ".regulated_linear_scaling_min_radius") { - regulated_linear_scaling_min_radius_ = parameter.as_double(); - } else if (name == plugin_name_ + ".transform_tolerance") { - double transform_tolerance = parameter.as_double(); - transform_tolerance_ = tf2::durationFromSec(transform_tolerance); - } else if (name == plugin_name_ + ".regulated_linear_scaling_min_speed") { - regulated_linear_scaling_min_speed_ = parameter.as_double(); - } else if (name == plugin_name_ + ".max_angular_accel") { - max_angular_accel_ = parameter.as_double(); - } else if (name == plugin_name_ + ".rotate_to_heading_min_angle") { - rotate_to_heading_min_angle_ = parameter.as_double(); - } - } else if (type == ParameterType::PARAMETER_BOOL) { - if (name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") { - use_velocity_scaled_lookahead_dist_ = parameter.as_bool(); - } else if (name == plugin_name_ + ".use_regulated_linear_velocity_scaling") { - use_regulated_linear_velocity_scaling_ = parameter.as_bool(); - } else if (name == plugin_name_ + ".use_cost_regulated_linear_velocity_scaling") { - use_cost_regulated_linear_velocity_scaling_ = parameter.as_bool(); - } else if (name == plugin_name_ + ".use_rotate_to_heading") { - if (parameter.as_bool() && allow_reversing_) { - RCLCPP_WARN( - logger_, "Both use_rotate_to_heading and allow_reversing " - "parameter cannot be set to true. Rejecting parameter update."); - continue; - } - use_rotate_to_heading_ = parameter.as_bool(); - } else if (name == plugin_name_ + ".allow_reversing") { - if (use_rotate_to_heading_ && parameter.as_bool()) { - RCLCPP_WARN( - logger_, "Both use_rotate_to_heading and allow_reversing " - "parameter cannot be set to true. Rejecting parameter update."); - continue; - } - allow_reversing_ = parameter.as_bool(); - } - } - } - - result.successful = true; - return result; -} - } // namespace nav2_regulated_pure_pursuit_controller // Register this controller as a nav2_core plugin diff --git a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp index 51bb277fed..09892755d9 100644 --- a/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp +++ b/nav2_regulated_pure_pursuit_controller/test/test_regulated_pp.cpp @@ -41,9 +41,9 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure BasicAPIRPP() : nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController() {} - nav_msgs::msg::Path getPlan() {return global_plan_;} + nav_msgs::msg::Path getPlan() {return path_handler_->getPlan();} - double getSpeed() {return desired_linear_vel_;} + double getSpeed() {return params_->desired_linear_vel;} std::unique_ptr createCarrotMsgWrapper( const geometry_msgs::msg::PoseStamped & carrot_pose) @@ -51,9 +51,9 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure return createCarrotMsg(carrot_pose); } - void setVelocityScaledLookAhead() {use_velocity_scaled_lookahead_dist_ = true;} - void setCostRegulationScaling() {use_cost_regulated_linear_velocity_scaling_ = true;} - void resetVelocityRegulationScaling() {use_regulated_linear_velocity_scaling_ = false;} + void setVelocityScaledLookAhead() {params_->use_velocity_scaled_lookahead_dist = true;} + void setCostRegulationScaling() {params_->use_cost_regulated_linear_velocity_scaling = true;} + void resetVelocityRegulationScaling() {params_->use_regulated_linear_velocity_scaling = false;} double getLookAheadDistanceWrapper(const geometry_msgs::msg::Twist & twist) { @@ -110,7 +110,7 @@ class BasicAPIRPP : public nav2_regulated_pure_pursuit_controller::RegulatedPure nav_msgs::msg::Path transformGlobalPlanWrapper( const geometry_msgs::msg::PoseStamped & pose) { - return transformGlobalPlan(pose); + return path_handler_->transformGlobalPlan(pose, params_->max_robot_pose_search_dist); } }; @@ -168,8 +168,16 @@ TEST(RegulatedPurePursuitTest, createCarrotMsg) TEST(RegulatedPurePursuitTest, findVelocitySignChange) { - auto ctrl = std::make_shared(); auto node = std::make_shared("testRPPfindVelocitySignChange"); + auto ctrl = std::make_shared(); + + std::string name = "PathFollower"; + auto tf = std::make_shared(node->get_clock()); + auto costmap = std::make_shared("fake_costmap"); + rclcpp_lifecycle::State state; + costmap->on_configure(state); + ctrl->configure(node, name, tf, costmap); + geometry_msgs::msg::PoseStamped pose; pose.header.frame_id = "smb"; auto time = node->get_clock()->now(); @@ -604,6 +612,7 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter) rclcpp::Parameter("test.use_velocity_scaled_lookahead_dist", false), rclcpp::Parameter("test.use_regulated_linear_velocity_scaling", false), rclcpp::Parameter("test.use_cost_regulated_linear_velocity_scaling", false), + rclcpp::Parameter("test.inflation_cost_scaling_factor", 1.0), rclcpp::Parameter("test.allow_reversing", false), rclcpp::Parameter("test.use_rotate_to_heading", false)}); @@ -630,11 +639,36 @@ TEST(RegulatedPurePursuitTest, testDynamicParameter) EXPECT_EQ(node->get_parameter("test.regulated_linear_scaling_min_speed").as_double(), 4.0); EXPECT_EQ(node->get_parameter("test.use_velocity_scaled_lookahead_dist").as_bool(), false); EXPECT_EQ(node->get_parameter("test.use_regulated_linear_velocity_scaling").as_bool(), false); + EXPECT_EQ(node->get_parameter("test.inflation_cost_scaling_factor").as_double(), 1.0); EXPECT_EQ( node->get_parameter( "test.use_cost_regulated_linear_velocity_scaling").as_bool(), false); EXPECT_EQ(node->get_parameter("test.allow_reversing").as_bool(), false); EXPECT_EQ(node->get_parameter("test.use_rotate_to_heading").as_bool(), false); + + // Should fail + auto results2 = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.inflation_cost_scaling_factor", -1.0)}); + + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + results2); + + auto results3 = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.use_rotate_to_heading", true)}); + + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + results3); + + auto results4 = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.allow_reversing", false), + rclcpp::Parameter("test.use_rotate_to_heading", true), + rclcpp::Parameter("test.allow_reversing", true)}); + + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + results4); } class TransformGlobalPlanTest : public ::testing::Test @@ -644,8 +678,8 @@ class TransformGlobalPlanTest : public ::testing::Test { ctrl_ = std::make_shared(); node_ = std::make_shared("testRPP"); - tf_buffer_ = std::make_shared(node_->get_clock()); costmap_ = std::make_shared("fake_costmap"); + tf_buffer_ = std::make_shared(node_->get_clock()); } void configure_costmap(uint16_t width, double resolution) @@ -736,6 +770,7 @@ TEST_F(TransformGlobalPlanTest, no_pruning_on_large_costmap) robot_pose.pose.position.z = 0.0; // A really big costmap // the max_costmap_extent should be 50m + configure_costmap(100u, 0.1); configure_controller(5.0); setup_transforms(robot_pose.pose.position); From c3b5ccef31764ddd25de9e779acd05d457af3216 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 3 Nov 2022 11:14:05 -0400 Subject: [PATCH 20/43] exceptions for compute path through poses (#3248) * exceptions for compute path through poses * lint fix * code review * code review Co-authored-by: Joshua Wallace --- .../compute_path_through_poses_action.hpp | 8 ++- .../action/compute_path_to_pose_action.hpp | 12 ++-- nav2_behavior_tree/nav2_tree_nodes.xml | 2 + .../compute_path_through_poses_action.cpp | 5 ++ .../action/compute_path_to_pose_action.cpp | 8 +-- .../include/nav2_core/planner_exceptions.hpp | 7 ++ .../action/ComputePathThroughPoses.action | 14 ++++ .../include/nav2_planner/planner_server.hpp | 2 + nav2_planner/src/planner_server.cpp | 71 +++++++++++++------ 9 files changed, 96 insertions(+), 33 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp index f7610696f8..ad5954b6a4 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp @@ -32,6 +32,10 @@ namespace nav2_behavior_tree class ComputePathThroughPosesAction : public BtActionNode { + using Action = nav2_msgs::action::ComputePathThroughPoses; + using ActionResult = Action::Result; + using ActionGoal = Action::Goal; + public: /** * @brief A constructor for nav2_behavior_tree::ComputePathThroughPosesAction @@ -72,7 +76,6 @@ class ComputePathThroughPosesAction { return providedBasicPorts( { - BT::OutputPort("path", "Path created by ComputePathThroughPoses node"), BT::InputPort>( "goals", "Destinations to plan through"), @@ -81,6 +84,9 @@ class ComputePathThroughPosesAction BT::InputPort( "planner_id", "", "Mapped name to the planner plugin type to use"), + BT::OutputPort("path", "Path created by ComputePathThroughPoses node"), + BT::OutputPort( + "compute_path_through_poses_error_code", "The compute path through poses error code"), }); } }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp index e980012cc4..1c7f0526df 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp @@ -29,6 +29,10 @@ namespace nav2_behavior_tree */ class ComputePathToPoseAction : public BtActionNode { + using Action = nav2_msgs::action::ComputePathToPose; + using ActionResult = Action::Result; + using ActionGoal = Action::Goal; + public: /** * @brief A constructor for nav2_behavior_tree::ComputePathToPoseAction @@ -57,7 +61,7 @@ class ComputePathToPoseAction : public BtActionNode("path", "Path created by ComputePathToPose node"), - BT::OutputPort( - "compute_path_to_pose_error_code", "The compute path to pose error code"), BT::InputPort("goal", "Destination to plan to"), BT::InputPort( "start", "Start pose of the path if overriding current robot pose"), BT::InputPort( "planner_id", "", "Mapped name to the planner plugin type to use"), + BT::OutputPort("path", "Path created by ComputePathToPose node"), + BT::OutputPort( + "compute_path_to_pose_error_code", "The compute path to pose error code"), }); } }; diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 425875c24c..95e5a0917d 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -87,6 +87,8 @@ Server timeout Mapped name to the planner plugin type to use Path created by ComputePathToPose node + "The compute path through poses + pose error code" diff --git a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp index c7421d5599..ffcbd328ae 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp @@ -41,6 +41,8 @@ void ComputePathThroughPosesAction::on_tick() BT::NodeStatus ComputePathThroughPosesAction::on_success() { setOutput("path", result_.result->path); + // Set empty error code, action was successful + setOutput("compute_path_through_poses_error_code", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } @@ -48,6 +50,7 @@ BT::NodeStatus ComputePathThroughPosesAction::on_aborted() { nav_msgs::msg::Path empty_path; setOutput("path", empty_path); + setOutput("compute_path_through_poses_error_code", result_.result->error_code); return BT::NodeStatus::FAILURE; } @@ -55,6 +58,8 @@ BT::NodeStatus ComputePathThroughPosesAction::on_cancelled() { nav_msgs::msg::Path empty_path; setOutput("path", empty_path); + // Set empty error code, action was cancelled + setOutput("compute_path_through_poses_error_code", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp index b5e0e9084c..68a909b52c 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp @@ -24,7 +24,7 @@ ComputePathToPoseAction::ComputePathToPoseAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf) +: BtActionNode(xml_tag_name, action_name, conf) { } @@ -41,8 +41,7 @@ BT::NodeStatus ComputePathToPoseAction::on_success() { setOutput("path", result_.result->path); // Set empty error code, action was successful - result_.result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NONE; - setOutput("compute_path_to_pose_error_code", result_.result->error_code); + setOutput("compute_path_to_pose_error_code", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } @@ -59,8 +58,7 @@ BT::NodeStatus ComputePathToPoseAction::on_cancelled() nav_msgs::msg::Path empty_path; setOutput("path", empty_path); // Set empty error code, action was cancelled - result_.result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NONE; - setOutput("compute_path_to_pose_error_code", result_.result->error_code); + setOutput("compute_path_to_pose_error_code", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_core/include/nav2_core/planner_exceptions.hpp b/nav2_core/include/nav2_core/planner_exceptions.hpp index 622ee0f0db..e607d98ccc 100644 --- a/nav2_core/include/nav2_core/planner_exceptions.hpp +++ b/nav2_core/include/nav2_core/planner_exceptions.hpp @@ -99,6 +99,13 @@ class PlannerTFError : public PlannerException : PlannerException(description) {} }; +class NoViapointsGiven : public PlannerException +{ +public: + explicit NoViapointsGiven(const std::string & description) + : PlannerException(description) {} +}; + } // namespace nav2_core #endif // NAV2_CORE__PLANNER_EXCEPTIONS_HPP_ diff --git a/nav2_msgs/action/ComputePathThroughPoses.action b/nav2_msgs/action/ComputePathThroughPoses.action index a1f609a9e0..c37297c69e 100644 --- a/nav2_msgs/action/ComputePathThroughPoses.action +++ b/nav2_msgs/action/ComputePathThroughPoses.action @@ -1,3 +1,16 @@ +# Error codes +# Note: The expected priority order of the errors should match the message order +int16 NONE=0 +int16 UNKNOWN=1 +int16 TF_ERROR=2 +int16 START_OUTSIDE_MAP=3 +int16 GOAL_OUTSIDE_MAP=4 +int16 START_OCCUPIED=5 +int16 GOAL_OCCUPIED=6 +int16 TIMEOUT=7 +int16 NO_VALID_PATH=8 +int16 NO_VIAPOINTS_GIVEN=9 + #goal definition geometry_msgs/PoseStamped[] goals geometry_msgs/PoseStamped start @@ -7,5 +20,6 @@ bool use_start # If false, use current robot pose as path start, if true, use st #result definition nav_msgs/Path path builtin_interfaces/Duration planning_time +int16 error_code --- #feedback definition diff --git a/nav2_planner/include/nav2_planner/planner_server.hpp b/nav2_planner/include/nav2_planner/planner_server.hpp index ad4f71c3b1..fee711f974 100644 --- a/nav2_planner/include/nav2_planner/planner_server.hpp +++ b/nav2_planner/include/nav2_planner/planner_server.hpp @@ -107,7 +107,9 @@ class PlannerServer : public nav2_util::LifecycleNode nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; using ActionToPose = nav2_msgs::action::ComputePathToPose; + using ActionToPoseGoal = ActionToPose::Goal; using ActionThroughPoses = nav2_msgs::action::ComputePathThroughPoses; + using ActionThroughPosesGoal = ActionThroughPoses::Goal; using ActionServerToPose = nav2_util::SimpleActionServer; using ActionServerThroughPoses = nav2_util::SimpleActionServer; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index c7a90bcbbe..e597522430 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -336,11 +336,13 @@ void PlannerServer::computePlanThroughPoses() auto start_time = steady_clock_.now(); - // Initialize the ComputePathToPose goal and result + // Initialize the ComputePathThroughPoses goal and result auto goal = action_server_poses_->get_current_goal(); auto result = std::make_shared(); nav_msgs::msg::Path concat_path; + geometry_msgs::msg::PoseStamped curr_start, curr_goal; + try { if (isServerInactive(action_server_poses_) || isCancelRequested(action_server_poses_)) { return; @@ -350,11 +352,8 @@ void PlannerServer::computePlanThroughPoses() getPreemptedGoalIfRequested(action_server_poses_, goal); - if (goal->goals.size() == 0) { - RCLCPP_WARN( - get_logger(), - "Compute path through poses requested a plan with no viapoint poses, returning."); - action_server_poses_->terminate_current(); + if (goal->goals.empty()) { + throw nav2_core::NoViapointsGiven("No viapoints given"); } // Use start pose if provided otherwise use current robot pose @@ -364,8 +363,6 @@ void PlannerServer::computePlanThroughPoses() } // Get consecutive paths through these points - std::vector::iterator goal_iter; - geometry_msgs::msg::PoseStamped curr_start, curr_goal; for (unsigned int i = 0; i != goal->goals.size(); i++) { // Get starting point if (i == 0) { @@ -408,13 +405,42 @@ void PlannerServer::computePlanThroughPoses() } action_server_poses_->succeeded_current(result); + } catch (nav2_core::StartOccupied & ex) { + exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); + result->error_code = ActionThroughPosesGoal::START_OCCUPIED; + action_server_poses_->terminate_current(result); + } catch (nav2_core::GoalOccupied & ex) { + exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); + result->error_code = ActionThroughPosesGoal::GOAL_OCCUPIED; + action_server_poses_->terminate_current(result); + } catch (nav2_core::NoValidPathCouldBeFound & ex) { + exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); + result->error_code = ActionThroughPosesGoal::NO_VALID_PATH; + action_server_poses_->terminate_current(result); + } catch (nav2_core::PlannerTimedOut & ex) { + exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); + result->error_code = ActionThroughPosesGoal::TIMEOUT; + action_server_poses_->terminate_current(result); + } catch (nav2_core::StartOutsideMapBounds & ex) { + exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); + result->error_code = ActionThroughPosesGoal::START_OUTSIDE_MAP; + action_server_poses_->terminate_current(result); + } catch (nav2_core::GoalOutsideMapBounds & ex) { + exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); + result->error_code = ActionThroughPosesGoal::GOAL_OUTSIDE_MAP; + action_server_poses_->terminate_current(result); + } catch (nav2_core::PlannerTFError & ex) { + exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); + result->error_code = ActionThroughPosesGoal::TF_ERROR; + action_server_poses_->terminate_current(result); + } catch (nav2_core::NoViapointsGiven & ex) { + exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); + result->error_code = ActionThroughPosesGoal::NO_VIAPOINTS_GIVEN; + action_server_poses_->terminate_current(result); } catch (std::exception & ex) { - RCLCPP_WARN( - get_logger(), - "%s plugin failed to plan through %zu points with final goal (%.2f, %.2f): \"%s\"", - goal->planner_id.c_str(), goal->goals.size(), goal->goals.back().pose.position.x, - goal->goals.back().pose.position.y, ex.what()); - action_server_poses_->terminate_current(); + exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); + result->error_code = ActionThroughPosesGoal::UNKNOWN; + action_server_poses_->terminate_current(result); } } @@ -469,39 +495,38 @@ PlannerServer::computePlan() "Planner loop missed its desired rate of %.4f Hz. Current loop rate is %.4f Hz", 1 / max_planner_duration_, 1 / cycle_duration.seconds()); } - action_server_pose_->succeeded_current(result); } catch (nav2_core::StartOccupied & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_OCCUPIED; + result->error_code = ActionToPoseGoal::START_OCCUPIED; action_server_pose_->terminate_current(result); } catch (nav2_core::GoalOccupied & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = nav2_msgs::action::ComputePathToPose::Goal::GOAL_OCCUPIED; + result->error_code = ActionToPoseGoal::GOAL_OCCUPIED; action_server_pose_->terminate_current(result); } catch (nav2_core::NoValidPathCouldBeFound & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = nav2_msgs::action::ComputePathToPose::Goal::NO_VALID_PATH; + result->error_code = ActionToPoseGoal::NO_VALID_PATH; action_server_pose_->terminate_current(result); } catch (nav2_core::PlannerTimedOut & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TIMEOUT; + result->error_code = ActionToPoseGoal::TIMEOUT; action_server_pose_->terminate_current(result); } catch (nav2_core::StartOutsideMapBounds & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = nav2_msgs::action::ComputePathToPose::Goal::START_OUTSIDE_MAP; + result->error_code = ActionToPoseGoal::START_OUTSIDE_MAP; action_server_pose_->terminate_current(result); } catch (nav2_core::GoalOutsideMapBounds & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = nav2_msgs::action::ComputePathToPose::Goal::GOAL_OUTSIDE_MAP; + result->error_code = ActionToPoseGoal::GOAL_OUTSIDE_MAP; action_server_pose_->terminate_current(result); } catch (nav2_core::PlannerTFError & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = nav2_msgs::action::ComputePathToPose::Goal::TF_ERROR; + result->error_code = ActionToPoseGoal::TF_ERROR; action_server_pose_->terminate_current(result); } catch (std::exception & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); - result->error_code = nav2_msgs::action::ComputePathToPose::Goal::UNKNOWN; + result->error_code = ActionToPoseGoal::UNKNOWN; action_server_pose_->terminate_current(result); } } From d04278601e4953d4cda02119b92bceb885e501cc Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 3 Nov 2022 08:33:54 -0700 Subject: [PATCH 21/43] =?UTF-8?q?Reclaim=20Our=20CI=20Coverage=20from=20th?= =?UTF-8?q?e=20Lords=20of=20Painful=20Subtle=20Regressions=20=E2=9A=94?= =?UTF-8?q?=EF=B8=8F=E2=9A=94=EF=B8=8F=E2=9A=94=EF=B8=8F=20(#3266)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * test waypoint follower with composition off for logging * adding no composition to all system tests --- .../assisted_teleop/test_assisted_teleop_behavior_launch.py | 1 + .../src/behaviors/backup/test_backup_behavior_launch.py | 1 + .../drive_on_heading/test_drive_on_heading_behavior_launch.py | 1 + .../src/behaviors/spin/test_spin_behavior_launch.py | 1 + .../src/behaviors/wait/test_wait_behavior_launch.py | 1 + nav2_system_tests/src/costmap_filters/test_keepout_launch.py | 1 + nav2_system_tests/src/costmap_filters/test_speed_launch.py | 1 + nav2_system_tests/src/system/test_multi_robot_launch.py | 1 + nav2_system_tests/src/system/test_system_launch.py | 1 + nav2_system_tests/src/system/test_wrong_init_pose_launch.py | 1 + .../src/system_failure/test_system_failure_launch.py | 1 + nav2_system_tests/src/updown/test_updown_launch.py | 1 + nav2_system_tests/src/waypoint_follower/test_case_py.launch | 1 + 13 files changed, 13 insertions(+) diff --git a/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py index 976eb02c2f..57f2b65625 100755 --- a/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/assisted_teleop/test_assisted_teleop_behavior_launch.py @@ -78,6 +78,7 @@ def generate_launch_description(): 'use_sim_time': 'True', 'params_file': configured_params, 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', 'autostart': 'True'}.items()), ]) diff --git a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py index bf7f4f04f1..6091ffb40c 100755 --- a/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py @@ -78,6 +78,7 @@ def generate_launch_description(): 'use_sim_time': 'True', 'params_file': configured_params, 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', 'autostart': 'True'}.items()), ]) diff --git a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py index 29530dd475..49959ec86d 100755 --- a/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/drive_on_heading/test_drive_on_heading_behavior_launch.py @@ -78,6 +78,7 @@ def generate_launch_description(): 'use_sim_time': 'True', 'params_file': configured_params, 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', 'autostart': 'True'}.items()), ]) diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py index d26795834b..154f9ed533 100755 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py @@ -78,6 +78,7 @@ def generate_launch_description(): 'use_sim_time': 'True', 'params_file': configured_params, 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', 'autostart': 'True'}.items()), ]) diff --git a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py index 37f2ebd34c..ee89e416de 100755 --- a/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py +++ b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py @@ -78,6 +78,7 @@ def generate_launch_description(): 'use_sim_time': 'True', 'params_file': configured_params, 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', 'autostart': 'True'}.items()), ]) diff --git a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py index 796667c65d..5e21a7241a 100755 --- a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py @@ -118,6 +118,7 @@ def generate_launch_description(): 'use_sim_time': 'True', 'params_file': new_yaml, 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', 'autostart': 'True'}.items()), Node( diff --git a/nav2_system_tests/src/costmap_filters/test_speed_launch.py b/nav2_system_tests/src/costmap_filters/test_speed_launch.py index 5b0c61ed9d..433dbc184e 100755 --- a/nav2_system_tests/src/costmap_filters/test_speed_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_speed_launch.py @@ -116,6 +116,7 @@ def generate_launch_description(): 'use_sim_time': 'True', 'params_file': new_yaml, 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', 'autostart': 'True'}.items()), ]) diff --git a/nav2_system_tests/src/system/test_multi_robot_launch.py b/nav2_system_tests/src/system/test_multi_robot_launch.py index 4c22101eca..d323f97567 100755 --- a/nav2_system_tests/src/system/test_multi_robot_launch.py +++ b/nav2_system_tests/src/system/test_multi_robot_launch.py @@ -106,6 +106,7 @@ def generate_launch_description(): 'params_file': params_file, 'bt_xml_file': bt_xml_file, 'autostart': 'True', + 'use_composition': 'False', 'use_remappings': 'True'}.items()) ]) nav_instances_cmds.append(group) diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index 6b067deea4..3678b242ac 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -96,6 +96,7 @@ def generate_launch_description(): 'use_sim_time': 'True', 'params_file': new_yaml, 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', 'autostart': 'True'}.items()), ]) diff --git a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py index 740c267fa2..2c10898a22 100755 --- a/nav2_system_tests/src/system/test_wrong_init_pose_launch.py +++ b/nav2_system_tests/src/system/test_wrong_init_pose_launch.py @@ -96,6 +96,7 @@ def generate_launch_description(): 'use_sim_time': 'True', 'params_file': new_yaml, 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', 'autostart': 'True', 'use_composition': 'False'}.items()), ]) diff --git a/nav2_system_tests/src/system_failure/test_system_failure_launch.py b/nav2_system_tests/src/system_failure/test_system_failure_launch.py index 56d4d44fdb..5bc8022766 100755 --- a/nav2_system_tests/src/system_failure/test_system_failure_launch.py +++ b/nav2_system_tests/src/system_failure/test_system_failure_launch.py @@ -86,6 +86,7 @@ def generate_launch_description(): 'use_sim_time': 'True', 'params_file': new_yaml, 'bt_xml_file': bt_navigator_xml, + 'use_composition': 'False', 'autostart': 'True'}.items()), ]) diff --git a/nav2_system_tests/src/updown/test_updown_launch.py b/nav2_system_tests/src/updown/test_updown_launch.py index aac4c6e2de..28b20bd4c3 100755 --- a/nav2_system_tests/src/updown/test_updown_launch.py +++ b/nav2_system_tests/src/updown/test_updown_launch.py @@ -61,6 +61,7 @@ def generate_launch_description(): os.path.join(launch_dir, 'bringup_launch.py')), launch_arguments={'map': map_yaml_file, 'use_sim_time': 'True', + 'use_composition': 'False', 'autostart': 'False'}.items()) start_test = launch.actions.ExecuteProcess( diff --git a/nav2_system_tests/src/waypoint_follower/test_case_py.launch b/nav2_system_tests/src/waypoint_follower/test_case_py.launch index 98819e6e63..adeb839c48 100755 --- a/nav2_system_tests/src/waypoint_follower/test_case_py.launch +++ b/nav2_system_tests/src/waypoint_follower/test_case_py.launch @@ -79,6 +79,7 @@ def generate_launch_description(): launch_arguments={ 'map': map_yaml_file, 'use_sim_time': 'True', + 'use_composition': 'False', 'params_file': new_yaml, 'bt_xml_file': bt_navigator_xml, 'autostart': 'True'}.items()), From 6d66c9dc5444ede067b92156c2edba38754238cb Mon Sep 17 00:00:00 2001 From: Afif Swaidan <53655365+afifswaidan@users.noreply.github.com> Date: Mon, 7 Nov 2022 21:06:49 +0100 Subject: [PATCH 22/43] Added Line Iterator (#3197) * Added Line Iterator * Updated Line Iterator to a new iteration method * Added the resolution as a parameter/ fixed linting * Added the resolution as a parameter/ fixed linting * Added unittests for the line iterator * Added unittests based on "unittest" package * Fixed __init__.py and rephrased some docstrings * Fixed linting errors * Fixed Linting Errors * Added some unittests and removed some methods * Dummy commit for CircleCI Issue Co-authored-by: Afif Swaidan --- .../nav2_simple_commander/line_iterator.py | 177 ++++++++++++++++++ nav2_simple_commander/pytest.ini | 2 + .../test/test_line_iterator.py | 107 +++++++++++ 3 files changed, 286 insertions(+) create mode 100644 nav2_simple_commander/nav2_simple_commander/line_iterator.py create mode 100644 nav2_simple_commander/pytest.ini create mode 100644 nav2_simple_commander/test/test_line_iterator.py diff --git a/nav2_simple_commander/nav2_simple_commander/line_iterator.py b/nav2_simple_commander/nav2_simple_commander/line_iterator.py new file mode 100644 index 0000000000..db19d4180c --- /dev/null +++ b/nav2_simple_commander/nav2_simple_commander/line_iterator.py @@ -0,0 +1,177 @@ +#! /usr/bin/env python3 +# Copyright 2021 Samsung Research America +# Copyright 2022 Afif Swaidan +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +""" +This is a Python3 API for a line iterator. + +It provides the ability to iterate +through the points of a line. +""" + +from cmath import sqrt + + +class LineIterator(): + """ + LineIterator. + + LineIterator Python3 API for iterating along the points of a given line + """ + + def __init__(self, x0, y0, x1, y1, step_size=1.0): + """ + Initialize the LineIterator. + + Args + ---- + x0 (float): Abscissa of the initial point + y0 (float): Ordinate of the initial point + x1 (float): Abscissa of the final point + y1 (float): Ordinate of the final point + step_size (float): Optional, Increments' resolution, defaults to 1 + + Raises + ------ + TypeError: When one (or more) of the inputs is not a number + ValueError: When step_size is not a positive number + + """ + if type(x0) not in [int, float]: + raise TypeError("x0 must be a number (int or float)") + + if type(y0) not in [int, float]: + raise TypeError("y0 must be a number (int or float)") + + if type(x1) not in [int, float]: + raise TypeError("x1 must be a number (int or float)") + + if type(y1) not in [int, float]: + raise TypeError("y1 must be a number (int or float)") + + if type(step_size) not in [int, float]: + raise TypeError("step_size must be a number (int or float)") + + if step_size <= 0: + raise ValueError("step_size must be a positive number") + + self.x0_ = x0 + self.y0_ = y0 + self.x1_ = x1 + self.y1_ = y1 + self.x_ = x0 + self.y_ = y0 + self.step_size_ = step_size + + if x1 != x0 and y1 != y0: + self.valid_ = True + self.m_ = (y1-y0)/(x1-x0) + self.b_ = y1 - (self.m_*x1) + elif x1 == x0 and y1 != y0: + self.valid_ = True + elif y1 == y1 and x1 != x0: + self.valid_ = True + self.m_ = (y1-y0)/(x1-x0) + self.b_ = y1 - (self.m_*x1) + else: + self.valid_ = False + raise ValueError( + "Line has zero length (All 4 points have same coordinates)") + + def isValid(self): + """Check if line is valid.""" + return self.valid_ + + def advance(self): + """Advance to the next point in the line.""" + if self.x1_ > self.x0_: + if self.x_ < self.x1_: + self.x_ = round(self.clamp( + self.x_ + self.step_size_, self.x0_, self.x1_), 5) + self.y_ = round(self.m_ * self.x_ + self.b_, 5) + else: + self.valid_ = False + elif self.x1_ < self.x0_: + if self.x_ > self.x1_: + self.x_ = round(self.clamp( + self.x_ - self.step_size_, self.x1_, self.x0_), 5) + self.y_ = round(self.m_ * self.x_ + self.b_, 5) + else: + self.valid_ = False + else: + if self.y1_ > self.y0_: + if self.y_ < self.y1_: + self.y_ = round(self.clamp( + self.y_ + self.step_size_, self.y0_, self.y1_), 5) + else: + self.valid_ = False + elif self.y1_ < self.y0_: + if self.y_ > self.y1_: + self.y_ = round(self.clamp( + self.y_ - self.step_size_, self.y1_, self.y0_), 5) + else: + self.valid_ = False + else: + self.valid_ = False + + def getX(self): + """Get the abscissa of the current point.""" + return self.x_ + + def getY(self): + """Get the ordinate of the current point.""" + return self.y_ + + def getX0(self): + """Get the abscissa of the initial point.""" + return self.x0_ + + def getY0(self): + """Get the ordinate of the intial point.""" + return self.y0_ + + def getX1(self): + """Get the abscissa of the final point.""" + return self.x1_ + + def getY1(self): + """Get the ordinate of the final point.""" + return self.y1_ + + def get_line_length(self): + """Get the length of the line.""" + return sqrt(pow(self.x1_ - self.x0_, 2) + pow(self.y1_ - self.y0_, 2)) + + def clamp(self, n, min_n, max_n): + """ + Clamp n to be between min_n and max_n. + + Args + ---- + n (float): input value + min_n (float): minimum value + max_n (float): maximum value + + Returns + ------- + n (float): input value clamped between given min and max + + """ + if n < min_n: + return min_n + elif n > max_n: + return max_n + else: + return n diff --git a/nav2_simple_commander/pytest.ini b/nav2_simple_commander/pytest.ini new file mode 100644 index 0000000000..50d6d01257 --- /dev/null +++ b/nav2_simple_commander/pytest.ini @@ -0,0 +1,2 @@ +[pytest] +junit_family=xunit2 \ No newline at end of file diff --git a/nav2_simple_commander/test/test_line_iterator.py b/nav2_simple_commander/test/test_line_iterator.py new file mode 100644 index 0000000000..4c5e97420a --- /dev/null +++ b/nav2_simple_commander/test/test_line_iterator.py @@ -0,0 +1,107 @@ +# Copyright 2022 Afif Swaidan +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import unittest +from cmath import sqrt +from nav2_simple_commander.line_iterator import LineIterator + + +class TestLineIterator(unittest.TestCase): + + def test_type_error(self): + # Test if a type error raised when passing invalid arguements types + self.assertRaises(TypeError, LineIterator, 0, 0, '10', 10, '1') + + def test_value_error(self): + # Test if a value error raised when passing negative or zero step_size + self.assertRaises(ValueError, LineIterator, 0, 0, 10, 10, -2) + # Test if a value error raised when passing zero length line + self.assertRaises(ValueError, LineIterator, 2, 2, 2, 2, 1) + + def test_get_xy(self): + # Test if the initial and final coordinates are returned correctly + lt = LineIterator(0, 0, 5, 5, 1) + self.assertEqual(lt.getX0(), 0) + self.assertEqual(lt.getY0(), 0) + self.assertEqual(lt.getX1(), 5) + self.assertEqual(lt.getY1(), 5) + + def test_line_length(self): + # Test if the line length is calculated correctly + lt = LineIterator(0, 0, 5, 5, 1) + self.assertEqual(lt.get_line_length(), sqrt(pow(5, 2) + pow(5, 2))) + + def test_straight_line(self): + # Test if the calculations are correct for y = x + lt = LineIterator(0, 0, 5, 5, 1) + i = 0 + while lt.isValid(): + self.assertEqual(lt.getX(), lt.getX0() + i) + self.assertEqual(lt.getY(), lt.getY0() + i) + lt.advance() + i += 1 + + # Test if the calculations are correct for y = 2x (positive slope) + lt = LineIterator(0, 0, 5, 10, 1) + i = 0 + while lt.isValid(): + self.assertEqual(lt.getX(), lt.getX0() + i) + self.assertEqual(lt.getY(), lt.getY0() + (i*2)) + lt.advance() + i += 1 + + # Test if the calculations are correct for y = -2x (negative slope) + lt = LineIterator(0, 0, 5, -10, 1) + i = 0 + while lt.isValid(): + self.assertEqual(lt.getX(), lt.getX0() + i) + self.assertEqual(lt.getY(), lt.getY0() + (-i*2)) + lt.advance() + i += 1 + + def test_hor_line(self): + # Test if the calculations are correct for y = 0x+b (horizontal line) + lt = LineIterator(0, 10, 5, 10, 1) + i = 0 + while lt.isValid(): + self.assertEqual(lt.getX(), lt.getX0() + i) + self.assertEqual(lt.getY(), lt.getY0()) + lt.advance() + i += 1 + + def test_ver_line(self): + # Test if the calculations are correct for x = n (vertical line) + lt = LineIterator(5, 0, 5, 10, 1) + i = 0 + while lt.isValid(): + self.assertEqual(lt.getX(), lt.getX0()) + self.assertEqual(lt.getY(), lt.getY0() + i) + lt.advance() + i += 1 + + def test_clamp(self): + # Test if the increments are clamped to avoid crossing the final points + # when step_size is large with respect to line length + lt = LineIterator(0, 0, 5, 5, 10) + self.assertEqual(lt.getX(), 0) + self.assertEqual(lt.getY(), 0) + lt.advance() + while lt.isValid(): + self.assertEqual(lt.getX(), 5) + self.assertEqual(lt.getY(), 5) + lt.advance() + + +if __name__ == '__main__': + unittest.main() From ee7e5058607f8499b88b03ea388e831b8555142c Mon Sep 17 00:00:00 2001 From: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Date: Mon, 7 Nov 2022 21:53:59 +0100 Subject: [PATCH 23/43] Use SetParameter Launch API to set the yaml filename for map_server (#3174) * implement launch priority for the mapserver parameter yaml_filename minor fix fix commit reincluded rewritten function comment remaining lines for yaml_filename removed default_value issue with composable node alternative soltion to the condition param not working in composable node remove unused import remove comments and reorder composablenode execution fixing commit fixing format fixing lint Update nav2_bringup/params/nav2_params.yaml Co-authored-by: Steve Macenski * state new ros-rolling release changes and deprecation Co-authored-by: Steve Macenski --- nav2_bringup/launch/bringup_launch.py | 7 +-- nav2_bringup/launch/localization_launch.py | 56 +++++++++++++++++++--- nav2_bringup/params/nav2_params.yaml | 11 +++-- 3 files changed, 57 insertions(+), 17 deletions(-) diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index 065337adb8..8fddcc3253 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -53,14 +53,10 @@ def generate_launch_description(): remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] - # Create our own temporary YAML files that include substitutions - param_substitutions = { - 'yaml_filename': map_yaml_file} - configured_params = RewrittenYaml( source_file=params_file, root_key=namespace, - param_rewrites=param_substitutions, + param_rewrites={}, convert_types=True) stdout_linebuf_envvar = SetEnvironmentVariable( @@ -83,6 +79,7 @@ def generate_launch_description(): declare_map_yaml_cmd = DeclareLaunchArgument( 'map', + default_value='', description='Full path to map yaml file to load') declare_use_sim_time_cmd = DeclareLaunchArgument( diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py index dca5bf23e4..f2469197bd 100644 --- a/nav2_bringup/launch/localization_launch.py +++ b/nav2_bringup/launch/localization_launch.py @@ -17,8 +17,11 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable +from launch.actions import DeclareLaunchArgument, GroupAction +from launch.actions import SetEnvironmentVariable from launch.conditions import IfCondition +from launch.conditions import LaunchConfigurationEquals +from launch.conditions import LaunchConfigurationNotEquals from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import LoadComposableNodes, SetParameter from launch_ros.actions import Node @@ -51,14 +54,10 @@ def generate_launch_description(): remappings = [('/tf', 'tf'), ('/tf_static', 'tf_static')] - # Create our own temporary YAML files that include substitutions - param_substitutions = { - 'yaml_filename': map_yaml_file} - configured_params = RewrittenYaml( source_file=params_file, root_key=namespace, - param_rewrites=param_substitutions, + param_rewrites={}, convert_types=True) stdout_linebuf_envvar = SetEnvironmentVariable( @@ -71,6 +70,7 @@ def generate_launch_description(): declare_map_yaml_cmd = DeclareLaunchArgument( 'map', + default_value='', description='Full path to map yaml file to load') declare_use_sim_time_cmd = DeclareLaunchArgument( @@ -108,6 +108,7 @@ def generate_launch_description(): actions=[ SetParameter("use_sim_time", use_sim_time), Node( + condition=LaunchConfigurationEquals('map', ''), package='nav2_map_server', executable='map_server', name='map_server', @@ -117,6 +118,18 @@ def generate_launch_description(): parameters=[configured_params], arguments=['--ros-args', '--log-level', log_level], remappings=remappings), + Node( + condition=LaunchConfigurationNotEquals('map', ''), + package='nav2_map_server', + executable='map_server', + name='map_server', + output='screen', + respawn=use_respawn, + respawn_delay=2.0, + parameters=[configured_params, + {'yaml_filename': map_yaml_file}], + arguments=['--ros-args', '--log-level', log_level], + remappings=remappings), Node( package='nav2_amcl', executable='amcl', @@ -137,13 +150,24 @@ def generate_launch_description(): {'node_names': lifecycle_nodes}]) ] ) - + # LoadComposableNode for map server twice depending if we should use the + # value of map from a CLI or launch default or user defined value in the + # yaml configuration file. They are separated since the conditions + # currently only work on the LoadComposableNodes commands and not on the + # ComposableNode node function itself + # EqualsSubstitution and NotEqualsSubstitution susbsitutions was recently + # added to solve this problem but it has not been ported yet to + # ros-rolling. See https://github.com/ros2/launch_ros/issues/328. + # LaunchConfigurationEquals and LaunchConfigurationNotEquals are scheduled + # for deprecation once a Rolling sync is conducted. Switching to this new + # would be required for both ComposableNode and normal nodes. load_composable_nodes = GroupAction( condition=IfCondition(use_composition), actions=[ SetParameter("use_sim_time", use_sim_time), LoadComposableNodes( target_container=container_name, + condition=LaunchConfigurationEquals('map', ''), composable_node_descriptions=[ ComposableNode( package='nav2_map_server', @@ -151,6 +175,24 @@ def generate_launch_description(): name='map_server', parameters=[configured_params], remappings=remappings), + ], + ), + LoadComposableNodes( + target_container=container_name, + condition=LaunchConfigurationNotEquals('map', ''), + composable_node_descriptions=[ + ComposableNode( + package='nav2_map_server', + plugin='nav2_map_server::MapServer', + name='map_server', + parameters=[configured_params, + {'yaml_filename': map_yaml_file}], + remappings=remappings), + ], + ), + LoadComposableNodes( + target_container=container_name, + composable_node_descriptions=[ ComposableNode( package='nav2_amcl', plugin='nav2_amcl::AmclNode', diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 0bd4f4e45b..4577f49550 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -243,11 +243,12 @@ global_costmap: inflation_radius: 0.55 always_send_full_costmap: True -map_server: - ros__parameters: - # Overridden in launch by the "map" launch configuration or provided default value. - # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. - yaml_filename: "" +# The yaml_filename does not need to be specified since it going to be set by defaults in launch. +# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py +# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used. +# map_server: +# ros__parameters: +# yaml_filename: "" map_saver: ros__parameters: From 52a31c61c0437517320b7a06f684385bfb2a06da Mon Sep 17 00:00:00 2001 From: Pradheep Krishna Date: Thu, 10 Nov 2022 03:32:16 +0530 Subject: [PATCH 24/43] adding reconfigure test to thetastar (#3275) --- .../test/test_theta_star.cpp | 44 +++++++++++++++++++ 1 file changed, 44 insertions(+) diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp index 264f737de4..44b37f4ea7 100644 --- a/nav2_theta_star_planner/test/test_theta_star.cpp +++ b/nav2_theta_star_planner/test/test_theta_star.cpp @@ -185,3 +185,47 @@ TEST(ThetaStarPlanner, test_theta_star_planner) { life_node.reset(); costmap_ros.reset(); } + +TEST(ThetaStarPlanner, test_theta_star_reconfigure) +{ + rclcpp_lifecycle::LifecycleNode::SharedPtr life_node = + std::make_shared("ThetaStarPlannerTest"); + + std::shared_ptr costmap_ros = + std::make_shared("global_costmap"); + costmap_ros->on_configure(rclcpp_lifecycle::State()); + + auto planner = std::make_unique(); + try { + // Expect to throw due to invalid prims file in param + planner->configure(life_node, "test", nullptr, costmap_ros); + } catch (...) { + } + planner->activate(); + + auto rec_param = std::make_shared( + life_node->get_node_base_interface(), life_node->get_node_topics_interface(), + life_node->get_node_graph_interface(), + life_node->get_node_services_interface()); + + auto results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.how_many_corners", 8), + rclcpp::Parameter("test.w_euc_cost", 1.0), + rclcpp::Parameter("test.w_traversal_cost", 2.0), + rclcpp::Parameter("test.use_final_approach_orientation", false)}); + + rclcpp::spin_until_future_complete( + life_node->get_node_base_interface(), + results); + + EXPECT_EQ(life_node->get_parameter("test.how_many_corners").as_int(), 8); + EXPECT_EQ( + life_node->get_parameter("test.w_euc_cost").as_double(), + 1.0); + EXPECT_EQ(life_node->get_parameter("test.w_traversal_cost").as_double(), 2.0); + EXPECT_EQ(life_node->get_parameter("test.use_final_approach_orientation").as_bool(), false); + + rclcpp::spin_until_future_complete( + life_node->get_node_base_interface(), + results); +} From e528ccc3b1f308c4eea701323a3ce8404642e1d9 Mon Sep 17 00:00:00 2001 From: Hao-Xuan Song <44140526+Cryst4L9527@users.noreply.github.com> Date: Fri, 11 Nov 2022 04:59:21 +0800 Subject: [PATCH 25/43] Check for range_max of laserscan in updateFilter to avoid a implicit overflow crash. (#3276) * Update amcl_node.cpp * fit the code style * fit code style --- nav2_amcl/src/amcl_node.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index 2e9730ae56..303a753b73 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -808,6 +808,15 @@ bool AmclNode::updateFilter( get_logger(), "Laser %d angles in base frame: min: %.3f inc: %.3f", laser_index, angle_min, angle_increment); + // Check the validity of range_max, must > 0.0 + if (laser_scan->range_max <= 0.0) { + RCLCPP_WARN( + get_logger(), "wrong range_max of laser_scan data: %f. The message could be malformed." + " Ignore this message and stop updating.", + laser_scan->range_max); + return false; + } + // Apply range min/max thresholds, if the user supplied them if (laser_max_range_ > 0.0) { ldata.range_max = std::min(laser_scan->range_max, static_cast(laser_max_range_)); From 3f8011f7ea9dcddd0aa016b660884cc01bb1d687 Mon Sep 17 00:00:00 2001 From: Erwin Lejeune Date: Fri, 11 Nov 2022 19:11:11 +0100 Subject: [PATCH 26/43] BT Service Node to throw if service was not available in time (#3256) * throw if service server wasn't available in time mimic the behavior of the bt action node constructor * throw if action unavailable in bt cancel action * use chrono literals namespace * fix linting errors * fix code style divergence --- .../nav2_behavior_tree/bt_cancel_action_node.hpp | 9 ++++++++- .../include/nav2_behavior_tree/bt_service_node.hpp | 13 ++++++++++++- 2 files changed, 20 insertions(+), 2 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp index b48478ecc4..504df9157a 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_cancel_action_node.hpp @@ -27,6 +27,8 @@ namespace nav2_behavior_tree { +using namespace std::chrono_literals; // NOLINT + /** * @brief Abstract class representing an action for cancelling BT node * @tparam ActionT Type of action @@ -87,7 +89,12 @@ class BtCancelActionNode : public BT::ActionNodeBase // Make sure the server is actually there before continuing RCLCPP_DEBUG(node_->get_logger(), "Waiting for \"%s\" action server", action_name.c_str()); - action_client_->wait_for_action_server(); + if (!action_client_->wait_for_action_server(1s)) { + RCLCPP_ERROR( + node_->get_logger(), "\"%s\" action server not available after waiting for 1 s", + action_name.c_str()); + throw std::runtime_error(std::string("Action server %s not available", action_name.c_str())); + } } /** diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp index b99e5b74d4..91b0c0cd9d 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp @@ -17,6 +17,7 @@ #include #include +#include #include "behaviortree_cpp_v3/action_node.h" #include "nav2_util/node_utils.hpp" @@ -26,6 +27,8 @@ namespace nav2_behavior_tree { +using namespace std::chrono_literals; // NOLINT + /** * @brief Abstract class representing a service based BT node * @tparam ServiceT Type of service @@ -71,7 +74,15 @@ class BtServiceNode : public BT::ActionNodeBase RCLCPP_DEBUG( node_->get_logger(), "Waiting for \"%s\" service", service_name_.c_str()); - service_client_->wait_for_service(); + if (!service_client_->wait_for_service(1s)) { + RCLCPP_ERROR( + node_->get_logger(), "\"%s\" service server not available after waiting for 1 s", + service_node_name.c_str()); + throw std::runtime_error( + std::string( + "Service server %s not available", + service_node_name.c_str())); + } RCLCPP_DEBUG( node_->get_logger(), "\"%s\" BtServiceNode initialized", From ead132a9dac580545fa5992dae4ff838fea033d2 Mon Sep 17 00:00:00 2001 From: Adam Aposhian Date: Fri, 11 Nov 2022 12:43:15 -0700 Subject: [PATCH 27/43] remove exec_depend on behaviortree_cpp_v3 (#3279) --- nav2_behavior_tree/package.xml | 1 - nav2_bt_navigator/package.xml | 1 - 2 files changed, 2 deletions(-) diff --git a/nav2_behavior_tree/package.xml b/nav2_behavior_tree/package.xml index 81e24a8cb3..30e1dda75d 100644 --- a/nav2_behavior_tree/package.xml +++ b/nav2_behavior_tree/package.xml @@ -36,7 +36,6 @@ rclcpp_action rclcpp_lifecycle std_msgs - behaviortree_cpp_v3 builtin_interfaces geometry_msgs sensor_msgs diff --git a/nav2_bt_navigator/package.xml b/nav2_bt_navigator/package.xml index e3287b118d..6f86318a79 100644 --- a/nav2_bt_navigator/package.xml +++ b/nav2_bt_navigator/package.xml @@ -24,7 +24,6 @@ nav2_util nav2_core - behaviortree_cpp_v3 rclcpp rclcpp_action rclcpp_lifecycle From 7335d6b8c9391015e3723ef14ca6898bb2ac843e Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 14 Nov 2022 17:53:13 -0800 Subject: [PATCH 28/43] add parameterized refinement recursion numbers in Smac Planner Smoother and Simple Smoother (#3284) * add parameterized refinement recursion numbers * fix tests --- nav2_smac_planner/include/nav2_smac_planner/smoother.hpp | 2 +- nav2_smac_planner/include/nav2_smac_planner/types.hpp | 4 ++++ nav2_smac_planner/src/smoother.cpp | 5 +++-- nav2_smoother/include/nav2_smoother/simple_smoother.hpp | 2 +- nav2_smoother/src/simple_smoother.cpp | 7 +++++-- nav2_smoother/test/test_simple_smoother.cpp | 6 +++--- 6 files changed, 17 insertions(+), 9 deletions(-) diff --git a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp index 8c2adc5d29..d896cd90b5 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/smoother.hpp @@ -232,7 +232,7 @@ class Smoother bool & reversing_segment); double min_turning_rad_, tolerance_, data_w_, smooth_w_; - int max_its_, refinement_ctr_; + int max_its_, refinement_ctr_, refinement_num_; bool is_holonomic_, do_refinement_; MotionModel motion_model_; ompl::base::StateSpacePtr state_space_; diff --git a/nav2_smac_planner/include/nav2_smac_planner/types.hpp b/nav2_smac_planner/include/nav2_smac_planner/types.hpp index f2cd73d33a..1df34851af 100644 --- a/nav2_smac_planner/include/nav2_smac_planner/types.hpp +++ b/nav2_smac_planner/include/nav2_smac_planner/types.hpp @@ -87,6 +87,9 @@ struct SmootherParams nav2_util::declare_parameter_if_not_declared( node, local_name + "do_refinement", rclcpp::ParameterValue(true)); node->get_parameter(local_name + "do_refinement", do_refinement_); + nav2_util::declare_parameter_if_not_declared( + node, local_name + "refinement_num", rclcpp::ParameterValue(2)); + node->get_parameter(local_name + "refinement_num", refinement_num_); } double tolerance_; @@ -95,6 +98,7 @@ struct SmootherParams double w_smooth_; bool holonomic_; bool do_refinement_; + int refinement_num_; }; /** diff --git a/nav2_smac_planner/src/smoother.cpp b/nav2_smac_planner/src/smoother.cpp index fd0ac34be2..a69e14011f 100644 --- a/nav2_smac_planner/src/smoother.cpp +++ b/nav2_smac_planner/src/smoother.cpp @@ -31,6 +31,7 @@ Smoother::Smoother(const SmootherParams & params) smooth_w_ = params.w_smooth_; is_holonomic_ = params.holonomic_; do_refinement_ = params.do_refinement_; + refinement_num_ = params.refinement_num_; } void Smoother::initialize(const double & min_turning_radius) @@ -49,7 +50,6 @@ bool Smoother::smooth( return false; } - refinement_ctr_ = 0; steady_clock::time_point start = steady_clock::now(); double time_remaining = max_time; bool success = true, reversing_segment; @@ -69,6 +69,7 @@ bool Smoother::smooth( // Make sure we're still able to smooth with time remaining steady_clock::time_point now = steady_clock::now(); time_remaining = max_time - duration_cast>(now - start).count(); + refinement_ctr_ = 0; // Smooth path segment naively const geometry_msgs::msg::Pose start_pose = curr_path_segment.poses.front().pose; @@ -178,7 +179,7 @@ bool Smoother::smoothImpl( // Lets do additional refinement, it shouldn't take more than a couple milliseconds // but really puts the path quality over the top. - if (do_refinement_ && refinement_ctr_ < 4) { + if (do_refinement_ && refinement_ctr_ < refinement_num_) { refinement_ctr_++; smoothImpl(new_path, reversing_segment, costmap, max_time); } diff --git a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp index 95f19dc54c..2e8fea4a94 100644 --- a/nav2_smoother/include/nav2_smoother/simple_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/simple_smoother.hpp @@ -121,7 +121,7 @@ class SimpleSmoother : public nav2_core::Smoother const double & value); double tolerance_, data_w_, smooth_w_; - int max_its_, refinement_ctr_; + int max_its_, refinement_ctr_, refinement_num_; bool do_refinement_; std::shared_ptr costmap_sub_; rclcpp::Logger logger_{rclcpp::get_logger("SimpleSmoother")}; diff --git a/nav2_smoother/src/simple_smoother.cpp b/nav2_smoother/src/simple_smoother.cpp index d04d88bc61..2db6ce3c92 100644 --- a/nav2_smoother/src/simple_smoother.cpp +++ b/nav2_smoother/src/simple_smoother.cpp @@ -44,12 +44,15 @@ void SimpleSmoother::configure( node, name + ".w_smooth", rclcpp::ParameterValue(0.3)); declare_parameter_if_not_declared( node, name + ".do_refinement", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, name + ".refinement_num", rclcpp::ParameterValue(2)); node->get_parameter(name + ".tolerance", tolerance_); node->get_parameter(name + ".max_its", max_its_); node->get_parameter(name + ".w_data", data_w_); node->get_parameter(name + ".w_smooth", smooth_w_); node->get_parameter(name + ".do_refinement", do_refinement_); + node->get_parameter(name + ".refinement_num", refinement_num_); } bool SimpleSmoother::smooth( @@ -58,7 +61,6 @@ bool SimpleSmoother::smooth( { auto costmap = costmap_sub_->getCostmap(); - refinement_ctr_ = 0; steady_clock::time_point start = steady_clock::now(); double time_remaining = max_time.seconds(); @@ -80,6 +82,7 @@ bool SimpleSmoother::smooth( // Make sure we're still able to smooth with time remaining steady_clock::time_point now = steady_clock::now(); time_remaining = max_time.seconds() - duration_cast>(now - start).count(); + refinement_ctr_ = 0; // Smooth path segment naively success = success && smoothImpl( @@ -180,7 +183,7 @@ bool SimpleSmoother::smoothImpl( // Lets do additional refinement, it shouldn't take more than a couple milliseconds // but really puts the path quality over the top. - if (do_refinement_ && refinement_ctr_ < 4) { + if (do_refinement_ && refinement_ctr_ < refinement_num_) { refinement_ctr_++; smoothImpl(new_path, reversing_segment, costmap, max_time); } diff --git a/nav2_smoother/test/test_simple_smoother.cpp b/nav2_smoother/test/test_simple_smoother.cpp index 7d9b56d457..fc3dceec97 100644 --- a/nav2_smoother/test/test_simple_smoother.cpp +++ b/nav2_smoother/test/test_simple_smoother.cpp @@ -128,7 +128,7 @@ TEST(SmootherTest, test_simple_smoother) EXPECT_LT( fabs( straight_irregular_path.poses[i].pose.position.y - - straight_irregular_path.poses[i + 1].pose.position.y), 0.38); + straight_irregular_path.poses[i + 1].pose.position.y), 0.50); } // Test regular path, should see no effective change @@ -181,8 +181,8 @@ TEST(SmootherTest, test_simple_smoother) straight_regular_path.poses[10].pose.position.x = 0.95; straight_regular_path.poses[10].pose.position.y = 0.5; EXPECT_TRUE(smoother->smooth(straight_regular_path, max_time)); - EXPECT_NEAR(straight_regular_path.poses[5].pose.position.x, 0.637, 0.01); - EXPECT_NEAR(straight_regular_path.poses[5].pose.position.y, 0.353, 0.01); + EXPECT_NEAR(straight_regular_path.poses[5].pose.position.x, 0.607, 0.01); + EXPECT_NEAR(straight_regular_path.poses[5].pose.position.y, 0.387, 0.01); // Test that collisions are rejected nav_msgs::msg::Path collision_path; From 65a9e4f271999b1d40a3d7b7ad2fc46ed3cda2cf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pedro=20Alejandro=20Gonz=C3=A1lez?= <71234974+pepisg@users.noreply.github.com> Date: Wed, 16 Nov 2022 16:43:40 -0500 Subject: [PATCH 29/43] Add allow_unknown parameter to theta star planner (#3286) * Add allow unknown parameter to theta star planner * Add allow unknown parameter to tests * missing comma * Change cost of unknown tiles * Uncrustify --- .../include/nav2_theta_star_planner/theta_star.hpp | 13 +++++++++++-- nav2_theta_star_planner/src/theta_star.cpp | 1 + nav2_theta_star_planner/src/theta_star_planner.cpp | 6 ++++++ nav2_theta_star_planner/test/test_theta_star.cpp | 4 +++- 4 files changed, 21 insertions(+), 3 deletions(-) diff --git a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp index dacd8a57bc..d1ddf7354c 100644 --- a/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp +++ b/nav2_theta_star_planner/include/nav2_theta_star_planner/theta_star.hpp @@ -24,6 +24,8 @@ #include "nav2_costmap_2d/costmap_2d_ros.hpp" const double INF_COST = DBL_MAX; +const int UNKNOWN_COST = 255; +const int OBS_COST = 254; const int LETHAL_COST = 252; struct coordsM @@ -70,6 +72,8 @@ class ThetaStar double w_heuristic_cost_; /// parameter to set the number of adjacent nodes to be searched on int how_many_corners_; + /// parameter to set weather the planner can plan through unknown space + bool allow_unknown_; /// the x-directional and y-directional lengths of the map respectively int size_x_, size_y_; @@ -91,7 +95,9 @@ class ThetaStar */ inline bool isSafe(const int & cx, const int & cy) const { - return costmap_->getCost(cx, cy) < LETHAL_COST; + return (costmap_->getCost( + cx, + cy) == UNKNOWN_COST && allow_unknown_) || costmap_->getCost(cx, cy) < LETHAL_COST; } /** @@ -185,7 +191,10 @@ class ThetaStar bool isSafe(const int & cx, const int & cy, double & cost) const { double curr_cost = getCost(cx, cy); - if (curr_cost < LETHAL_COST) { + if ((costmap_->getCost(cx, cy) == UNKNOWN_COST && allow_unknown_) || curr_cost < LETHAL_COST) { + if (costmap_->getCost(cx, cy) == UNKNOWN_COST) { + curr_cost = OBS_COST - 1; + } cost += w_traversal_cost_ * curr_cost * curr_cost / LETHAL_COST / LETHAL_COST; return true; } else { diff --git a/nav2_theta_star_planner/src/theta_star.cpp b/nav2_theta_star_planner/src/theta_star.cpp index e0267b34c0..baddc754d7 100644 --- a/nav2_theta_star_planner/src/theta_star.cpp +++ b/nav2_theta_star_planner/src/theta_star.cpp @@ -23,6 +23,7 @@ ThetaStar::ThetaStar() w_euc_cost_(2.0), w_heuristic_cost_(1.0), how_many_corners_(8), + allow_unknown_(true), size_x_(0), size_y_(0), index_generated_(0) diff --git a/nav2_theta_star_planner/src/theta_star_planner.cpp b/nav2_theta_star_planner/src/theta_star_planner.cpp index 9f4930da72..c509afb065 100644 --- a/nav2_theta_star_planner/src/theta_star_planner.cpp +++ b/nav2_theta_star_planner/src/theta_star_planner.cpp @@ -45,6 +45,10 @@ void ThetaStarPlanner::configure( RCLCPP_WARN(logger_, "Your value for - .how_many_corners was overridden, and is now set to 8"); } + nav2_util::declare_parameter_if_not_declared( + node, name_ + ".allow_unknown", rclcpp::ParameterValue(true)); + node->get_parameter(name_ + ".allow_unknown", planner_->allow_unknown_); + nav2_util::declare_parameter_if_not_declared( node, name_ + ".w_euc_cost", rclcpp::ParameterValue(1.0)); node->get_parameter(name_ + ".w_euc_cost", planner_->w_euc_cost_); @@ -237,6 +241,8 @@ ThetaStarPlanner::dynamicParametersCallback(std::vector param } else if (type == ParameterType::PARAMETER_BOOL) { if (name == name_ + ".use_final_approach_orientation") { use_final_approach_orientation_ = parameter.as_bool(); + } else if (name == name_ + ".allow_unknown") { + planner_->allow_unknown_ = parameter.as_bool(); } } } diff --git a/nav2_theta_star_planner/test/test_theta_star.cpp b/nav2_theta_star_planner/test/test_theta_star.cpp index 44b37f4ea7..c2360d9f87 100644 --- a/nav2_theta_star_planner/test/test_theta_star.cpp +++ b/nav2_theta_star_planner/test/test_theta_star.cpp @@ -212,7 +212,8 @@ TEST(ThetaStarPlanner, test_theta_star_reconfigure) {rclcpp::Parameter("test.how_many_corners", 8), rclcpp::Parameter("test.w_euc_cost", 1.0), rclcpp::Parameter("test.w_traversal_cost", 2.0), - rclcpp::Parameter("test.use_final_approach_orientation", false)}); + rclcpp::Parameter("test.use_final_approach_orientation", false), + rclcpp::Parameter("test.allow_unknown", false)}); rclcpp::spin_until_future_complete( life_node->get_node_base_interface(), @@ -224,6 +225,7 @@ TEST(ThetaStarPlanner, test_theta_star_reconfigure) 1.0); EXPECT_EQ(life_node->get_parameter("test.w_traversal_cost").as_double(), 2.0); EXPECT_EQ(life_node->get_parameter("test.use_final_approach_orientation").as_bool(), false); + EXPECT_EQ(life_node->get_parameter("test.allow_unknown").as_bool(), false); rclcpp::spin_until_future_complete( life_node->get_node_base_interface(), From eb2c77961d435221ac8912fc3df08cbea3873cb6 Mon Sep 17 00:00:00 2001 From: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Date: Thu, 17 Nov 2022 00:11:30 +0100 Subject: [PATCH 30/43] Include test cases waypoint follwer (#3288) * WIP * included missed_waypoing check * finished inclding test * fix format * return default sleep value --- .../src/waypoint_follower/tester.py | 55 ++++++++++++++++--- 1 file changed, 47 insertions(+), 8 deletions(-) diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index e6d299caa5..2579513797 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -20,10 +20,12 @@ from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped from nav2_msgs.action import FollowWaypoints from nav2_msgs.srv import ManageLifecycleNodes +from rcl_interfaces.srv import SetParameters import rclpy from rclpy.action import ActionClient from rclpy.node import Node +from rclpy.parameter import Parameter from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy from rclpy.qos import QoSProfile @@ -38,6 +40,7 @@ def __init__(self): 'initialpose', 10) self.initial_pose_received = False self.goal_handle = None + self.action_result = None pose_qos = QoSProfile( durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, @@ -47,6 +50,8 @@ def __init__(self): self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped, 'amcl_pose', self.poseCallback, pose_qos) + self.param_cli = self.create_client(SetParameters, + '/waypoint_follower/set_parameters') def setInitialPose(self, pose): self.init_pose = PoseWithCovarianceStamped() @@ -70,10 +75,10 @@ def setWaypoints(self, waypoints): msg.pose.orientation.w = 1.0 self.waypoints.append(msg) - def run(self, block): - if not self.waypoints: - rclpy.error_msg('Did not set valid waypoints before running test!') - return False + def run(self, block, cancel): + # if not self.waypoints: + # rclpy.error_msg('Did not set valid waypoints before running test!') + # return False while not self.action_client.wait_for_server(timeout_sec=1.0): self.info_msg("'follow_waypoints' action server not available, waiting...") @@ -98,12 +103,16 @@ def run(self, block): return True get_result_future = self.goal_handle.get_result_async() + if cancel: + time.sleep(2) + self.cancel_goal() self.info_msg("Waiting for 'follow_waypoints' action to complete") try: rclpy.spin_until_future_complete(self, get_result_future) status = get_result_future.result().status result = get_result_future.result().result + self.action_result = result except Exception as e: # noqa: B902 self.error_msg(f'Service call failed {e!r}') @@ -121,6 +130,13 @@ def run(self, block): def publishInitialPose(self): self.initial_pose_pub.publish(self.init_pose) + def setStopFailureParam(self, value): + req = SetParameters.Request() + req.parameters = [Parameter('stop_on_failure', + Parameter.Type.BOOL, value).to_parameter_msg()] + future = self.param_cli.call_async(req) + rclpy.spin_until_future_complete(self, future) + def shutdown(self): self.info_msg('Shutting down') @@ -194,15 +210,15 @@ def main(argv=sys.argv[1:]): test.info_msg('Waiting for amcl_pose to be received') rclpy.spin_once(test, timeout_sec=1.0) # wait for poseCallback - result = test.run(True) + result = test.run(True, False) assert result # preempt with new point test.setWaypoints([starting_pose]) - result = test.run(False) + result = test.run(False, False) time.sleep(2) test.setWaypoints([wps[1]]) - result = test.run(False) + result = test.run(False, False) # cancel time.sleep(2) @@ -211,7 +227,30 @@ def main(argv=sys.argv[1:]): # a failure case time.sleep(2) test.setWaypoints([[100.0, 100.0]]) - result = test.run(True) + result = test.run(True, False) + assert not result + result = not result + + # stop on failure test with bogous waypoint + test.setStopFailureParam(True) + bwps = [[-0.52, -0.54], [100.0, 100.0], [0.58, 0.52]] + starting_pose = [-2.0, -0.5] + test.setWaypoints(bwps) + test.setInitialPose(starting_pose) + result = test.run(True, False) + assert not result + result = not result + mwps = test.action_result.missed_waypoints + result = (len(mwps) == 1) & (mwps[0] == 1) + test.setStopFailureParam(False) + + # Zero goal test + test.setWaypoints([]) + result = test.run(True, False) + + # Cancel test + test.setWaypoints(wps) + result = test.run(True, True) assert not result result = not result From 06cac219397f1fb7815bb7c831c14784d76a0188 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Date: Thu, 17 Nov 2022 22:31:59 +0300 Subject: [PATCH 31/43] Dynamically changing polygons support (#3245) * Add Collision Monitor polygon topics subscription * Add the support of polygons published in different frame * Internal review * Fix working with polygons visualization * Update nav2_collision_monitor/README.md Co-authored-by: Steve Macenski * Move getTransform to nav2_util * Fix misprint * Meet remaining review items: * Update polygon params handling logic * Warn if polygon shape was not set * Publish with ownership movement * Correct polygons_test.cpp parameters handling logic * Adjust README for dynamic polygons logic update Co-authored-by: Steve Macenski --- nav2_collision_monitor/README.md | 8 +- .../include/nav2_collision_monitor/circle.hpp | 11 +- .../nav2_collision_monitor/polygon.hpp | 37 ++- .../include/nav2_collision_monitor/source.hpp | 15 -- nav2_collision_monitor/src/circle.cpp | 8 +- .../src/collision_monitor_node.cpp | 8 + nav2_collision_monitor/src/pointcloud.cpp | 8 +- nav2_collision_monitor/src/polygon.cpp | 191 +++++++++++----- nav2_collision_monitor/src/range.cpp | 8 +- nav2_collision_monitor/src/scan.cpp | 9 +- nav2_collision_monitor/src/source.cpp | 32 --- nav2_collision_monitor/test/polygons_test.cpp | 213 +++++++++++++++--- nav2_util/include/nav2_util/robot_utils.hpp | 53 +++++ nav2_util/src/robot_utils.cpp | 67 ++++++ 14 files changed, 526 insertions(+), 142 deletions(-) diff --git a/nav2_collision_monitor/README.md b/nav2_collision_monitor/README.md index 54841675e9..788a77ebd4 100644 --- a/nav2_collision_monitor/README.md +++ b/nav2_collision_monitor/README.md @@ -27,9 +27,9 @@ The following models of safety behaviors are employed by Collision Monitor: The zones around the robot can take the following shapes: -* Arbitrary user-defined polygon relative to the robot base frame. -* Circle: is made for the best performance and could be used in the cases where the zone or robot could be approximated by round shape. -* Robot footprint polygon, which is used in the approach behavior model only. Will use the footprint topic to allow it to be dynamically adjusted over time. +* Arbitrary user-defined polygon relative to the robot base frame, which can be static in a configuration file or dynamically changing via a topic interface. +* Robot footprint polygon, which is used in the approach behavior model only. Will use the static user-defined polygon or the footprint topic to allow it to be dynamically adjusted over time. +* Circle: is made for the best performance and could be used in the cases where the zone or robot footprint could be approximated by round shape. The data may be obtained from different data sources: @@ -43,7 +43,7 @@ The Collision Monitor is designed to operate below Nav2 as an independent safety This acts as a filter on the `cmd_vel` topic coming out of the Controller Server. If no such zone is triggered, then the Controller's `cmd_vel` is used. Else, it is scaled or set to stop as appropriate. The following diagram is showing the high-level design of Collision Monitor module. All shapes (Polygons and Circles) are derived from base `Polygon` class, so without loss of generality we can call them as polygons. Subscribed footprint is also having the same properties as other polygons, but it is being obtained a footprint topic for the Approach Model. -![HDL.png](doc/HLD.png) +![HLD.png](doc/HLD.png) ## Configuration diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp index 807d4b3dc7..9cab7485be 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/circle.hpp @@ -66,15 +66,24 @@ class Circle : public Polygon */ int getPointsInside(const std::vector & points) const override; + /** + * @brief Specifies that the shape is always set for a circle object + */ + bool isShapeSet() override {return true;} + protected: /** * @brief Supporting routine obtaining polygon-specific ROS-parameters + * @brief polygon_sub_topic Input name of polygon subscription topic * @param polygon_pub_topic Output name of polygon publishing topic * @param footprint_topic Output name of footprint topic. For Circle returns empty string, * there is no footprint subscription in this class. * @return True if all parameters were obtained or false in failure case */ - bool getParameters(std::string & polygon_pub_topic, std::string & footprint_topic) override; + bool getParameters( + std::string & polygon_sub_topic, + std::string & polygon_pub_topic, + std::string & footprint_topic) override; // ----- Variables ----- diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp index 97423dc411..e1354e434f 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/polygon.hpp @@ -21,7 +21,6 @@ #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/polygon_stamped.hpp" -#include "geometry_msgs/msg/polygon.hpp" #include "tf2/time.h" #include "tf2_ros/buffer.h" @@ -110,6 +109,12 @@ class Polygon */ virtual void getPolygon(std::vector & poly) const; + /** + * @brief Returns true if polygon points were set. + * Othewise, prints a warning and returns false. + */ + virtual bool isShapeSet(); + /** * @brief Updates polygon from footprint subscriber (if any) */ @@ -138,7 +143,7 @@ class Polygon /** * @brief Publishes polygon message into a its own topic */ - void publish() const; + void publish(); protected: /** @@ -150,11 +155,29 @@ class Polygon /** * @brief Supporting routine obtaining polygon-specific ROS-parameters + * @brief polygon_sub_topic Output name of polygon subscription topic. + * Empty, if no polygon subscription. * @param polygon_pub_topic Output name of polygon publishing topic - * @param footprint_topic Output name of footprint topic. Empty, if no footprint subscription + * @param footprint_topic Output name of footprint topic. + * Empty, if no footprint subscription. * @return True if all parameters were obtained or false in failure case */ - virtual bool getParameters(std::string & polygon_pub_topic, std::string & footprint_topic); + virtual bool getParameters( + std::string & polygon_sub_topic, + std::string & polygon_pub_topic, + std::string & footprint_topic); + + /** + * @brief Updates polygon from geometry_msgs::msg::PolygonStamped message + * @param msg Message to update polygon from + */ + void updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg); + + /** + * @brief Dynamic polygon callback + * @param msg Shared pointer to the polygon message + */ + void polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg); /** * @brief Checks if point is inside polygon @@ -183,6 +206,8 @@ class Polygon double time_before_collision_; /// @brief Time step for robot movement simulation double simulation_time_step_; + /// @brief Polygon subscription + rclcpp::Subscription::SharedPtr polygon_sub_; /// @brief Footprint subscriber std::unique_ptr footprint_sub_; @@ -197,8 +222,8 @@ class Polygon // Visualization /// @brief Whether to publish the polygon bool visualize_; - /// @brief Polygon points stored for later publishing - geometry_msgs::msg::Polygon polygon_; + /// @brief Polygon stored for later publishing + geometry_msgs::msg::PolygonStamped polygon_; /// @brief Polygon publisher for visualization purposes rclcpp_lifecycle::LifecyclePublisher::SharedPtr polygon_pub_; diff --git a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp index a24859bb4a..5fd95de7ee 100644 --- a/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp +++ b/nav2_collision_monitor/include/nav2_collision_monitor/source.hpp @@ -88,21 +88,6 @@ class Source const rclcpp::Time & source_time, const rclcpp::Time & curr_time) const; - /** - * @brief Obtains a transform from source_frame_id at source_time -> - * to base_frame_id_ at curr_time time - * @param source_frame_id Source frame ID to convert data from - * @param source_time Source timestamp to convert data from - * @param curr_time Current node time to interpolate data to - * @param tf_transform Output source->base transform - * @return True if got correct transform, otherwise false - */ - bool getTransform( - const std::string & source_frame_id, - const rclcpp::Time & source_time, - const rclcpp::Time & curr_time, - tf2::Transform & tf_transform) const; - // ----- Variables ----- /// @brief Collision Monitor node diff --git a/nav2_collision_monitor/src/circle.cpp b/nav2_collision_monitor/src/circle.cpp index 7dc9708967..fa0fc33a4f 100644 --- a/nav2_collision_monitor/src/circle.cpp +++ b/nav2_collision_monitor/src/circle.cpp @@ -70,7 +70,10 @@ int Circle::getPointsInside(const std::vector & points) const return num; } -bool Circle::getParameters(std::string & polygon_pub_topic, std::string & footprint_topic) +bool Circle::getParameters( + std::string & polygon_sub_topic, + std::string & polygon_pub_topic, + std::string & footprint_topic) { auto node = node_.lock(); if (!node) { @@ -81,7 +84,8 @@ bool Circle::getParameters(std::string & polygon_pub_topic, std::string & footpr return false; } - // There is no footprint subscription for the Circle. Thus, set string as empty. + // There is no polygon or footprint subscription for the Circle. Thus, set strings as empty. + polygon_sub_topic.clear(); footprint_topic.clear(); try { diff --git a/nav2_collision_monitor/src/collision_monitor_node.cpp b/nav2_collision_monitor/src/collision_monitor_node.cpp index 313d71fb0a..d706ba0cd9 100644 --- a/nav2_collision_monitor/src/collision_monitor_node.cpp +++ b/nav2_collision_monitor/src/collision_monitor_node.cpp @@ -389,6 +389,10 @@ bool CollisionMonitor::processStopSlowdown( const Velocity & velocity, Action & robot_action) const { + if (!polygon->isShapeSet()) { + return false; + } + if (polygon->getPointsInside(collision_points) > polygon->getMaxPoints()) { if (polygon->getActionType() == STOP) { // Setting up zero velocity for STOP model @@ -420,6 +424,10 @@ bool CollisionMonitor::processApproach( { polygon->updatePolygon(); + if (!polygon->isShapeSet()) { + return false; + } + // Obtain time before a collision const double collision_time = polygon->getCollisionTime(collision_points, velocity); if (collision_time >= 0.0) { diff --git a/nav2_collision_monitor/src/pointcloud.cpp b/nav2_collision_monitor/src/pointcloud.cpp index df4e86b63e..282dec5463 100644 --- a/nav2_collision_monitor/src/pointcloud.cpp +++ b/nav2_collision_monitor/src/pointcloud.cpp @@ -19,6 +19,7 @@ #include "sensor_msgs/point_cloud2_iterator.hpp" #include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" namespace nav2_collision_monitor { @@ -78,7 +79,12 @@ void PointCloud::getData( // Obtaining the transform to get data from source frame and time where it was received // to the base frame and current time tf2::Transform tf_transform; - if (!getTransform(data_->header.frame_id, data_->header.stamp, curr_time, tf_transform)) { + if ( + !nav2_util::getTransform( + data_->header.frame_id, data_->header.stamp, + base_frame_id_, curr_time, global_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { return; } diff --git a/nav2_collision_monitor/src/polygon.cpp b/nav2_collision_monitor/src/polygon.cpp index d17c34b46a..824ee75f67 100644 --- a/nav2_collision_monitor/src/polygon.cpp +++ b/nav2_collision_monitor/src/polygon.cpp @@ -21,6 +21,7 @@ #include "geometry_msgs/msg/point32.hpp" #include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" #include "nav2_collision_monitor/kinematics.hpp" @@ -43,6 +44,8 @@ Polygon::Polygon( Polygon::~Polygon() { RCLCPP_INFO(logger_, "[%s]: Destroying Polygon", polygon_name_.c_str()); + polygon_sub_.reset(); + polygon_pub_.reset(); poly_.clear(); } @@ -53,20 +56,36 @@ bool Polygon::configure() throw std::runtime_error{"Failed to lock node"}; } - std::string polygon_pub_topic, footprint_topic; + std::string polygon_sub_topic, polygon_pub_topic, footprint_topic; - if (!getParameters(polygon_pub_topic, footprint_topic)) { + if (!getParameters(polygon_sub_topic, polygon_pub_topic, footprint_topic)) { return false; } + if (!polygon_sub_topic.empty()) { + RCLCPP_INFO( + logger_, + "[%s]: Subscribing on %s topic for polygon", + polygon_name_.c_str(), polygon_sub_topic.c_str()); + rclcpp::QoS polygon_qos = rclcpp::SystemDefaultsQoS(); // set to default + polygon_sub_ = node->create_subscription( + polygon_sub_topic, polygon_qos, + std::bind(&Polygon::polygonCallback, this, std::placeholders::_1)); + } + if (!footprint_topic.empty()) { + RCLCPP_INFO( + logger_, + "[%s]: Making footprint subscriber on %s topic", + polygon_name_.c_str(), footprint_topic.c_str()); footprint_sub_ = std::make_unique( node, footprint_topic, *tf_buffer_, base_frame_id_, tf2::durationToSec(transform_tolerance_)); } if (visualize_) { - // Fill polygon_ points for future usage + // Fill polygon_ for future usage + polygon_.header.frame_id = base_frame_id_; std::vector poly; getPolygon(poly); for (const Point & p : poly) { @@ -74,7 +93,7 @@ bool Polygon::configure() p_s.x = p.x; p_s.y = p.y; // p_s.z will remain 0.0 - polygon_.points.push_back(p_s); + polygon_.polygon.points.push_back(p_s); } rclcpp::QoS polygon_qos = rclcpp::SystemDefaultsQoS(); // set to default @@ -129,6 +148,15 @@ void Polygon::getPolygon(std::vector & poly) const poly = poly_; } +bool Polygon::isShapeSet() +{ + if (poly_.empty()) { + RCLCPP_WARN(logger_, "[%s]: Polygon shape is not set yet", polygon_name_.c_str()); + return false; + } + return true; +} + void Polygon::updatePolygon() { if (footprint_sub_ != nullptr) { @@ -139,14 +167,15 @@ void Polygon::updatePolygon() std::size_t new_size = footprint_vec.size(); poly_.resize(new_size); - polygon_.points.resize(new_size); + polygon_.header.frame_id = base_frame_id_; + polygon_.polygon.points.resize(new_size); geometry_msgs::msg::Point32 p_s; for (std::size_t i = 0; i < new_size; i++) { poly_[i] = {footprint_vec[i].x, footprint_vec[i].y}; p_s.x = footprint_vec[i].x; p_s.y = footprint_vec[i].y; - polygon_.points[i] = p_s; + polygon_.polygon.points[i] = p_s; } } } @@ -192,7 +221,7 @@ double Polygon::getCollisionTime( return -1.0; } -void Polygon::publish() const +void Polygon::publish() { if (!visualize_) { return; @@ -203,15 +232,10 @@ void Polygon::publish() const throw std::runtime_error{"Failed to lock node"}; } - // Fill PolygonStamped struct - std::unique_ptr poly_s = - std::make_unique(); - poly_s->header.stamp = node->now(); - poly_s->header.frame_id = base_frame_id_; - poly_s->polygon = polygon_; - - // Publish polygon - polygon_pub_->publish(std::move(poly_s)); + // Actualize the time to current and publish the polygon + polygon_.header.stamp = node->now(); + auto msg = std::make_unique(polygon_); + polygon_pub_->publish(std::move(msg)); } bool Polygon::getCommonParameters(std::string & polygon_pub_topic) @@ -280,7 +304,10 @@ bool Polygon::getCommonParameters(std::string & polygon_pub_topic) return true; } -bool Polygon::getParameters(std::string & polygon_pub_topic, std::string & footprint_topic) +bool Polygon::getParameters( + std::string & polygon_sub_topic, + std::string & polygon_pub_topic, + std::string & footprint_topic) { auto node = node_.lock(); if (!node) { @@ -291,48 +318,62 @@ bool Polygon::getParameters(std::string & polygon_pub_topic, std::string & footp return false; } + // Clear the subscription topics. They will be set later, if necessary. + polygon_sub_topic.clear(); + footprint_topic.clear(); + try { - if (action_type_ == APPROACH) { - // Obtain the footprint topic to make a footprint subscription for approach polygon + try { + // Leave it uninitialized: it will throw an inner exception if the parameter is not set nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + ".footprint_topic", - rclcpp::ParameterValue("local_costmap/published_footprint")); - footprint_topic = - node->get_parameter(polygon_name_ + ".footprint_topic").as_string(); + node, polygon_name_ + ".points", rclcpp::PARAMETER_DOUBLE_ARRAY); + std::vector poly_row = + node->get_parameter(polygon_name_ + ".points").as_double_array(); + // Check for points format correctness + if (poly_row.size() <= 6 || poly_row.size() % 2 != 0) { + RCLCPP_ERROR( + logger_, + "[%s]: Polygon has incorrect points description", + polygon_name_.c_str()); + return false; + } - // This is robot footprint: do not need to get polygon points from ROS parameters. - // It will be set dynamically later. - return true; - } else { - // Make it empty otherwise - footprint_topic.clear(); - } + // Obtain polygon vertices + Point point; + bool first = true; + for (double val : poly_row) { + if (first) { + point.x = val; + } else { + point.y = val; + poly_.push_back(point); + } + first = !first; + } - // Leave it not initialized: the will cause an error if it will not set - nav2_util::declare_parameter_if_not_declared( - node, polygon_name_ + ".points", rclcpp::PARAMETER_DOUBLE_ARRAY); - std::vector poly_row = - node->get_parameter(polygon_name_ + ".points").as_double_array(); - // Check for points format correctness - if (poly_row.size() <= 6 || poly_row.size() % 2 != 0) { - RCLCPP_ERROR( + // Do not need to proceed further, if "points" parameter is defined. + // Static polygon will be used. + return true; + } catch (const rclcpp::exceptions::ParameterUninitializedException &) { + RCLCPP_INFO( logger_, - "[%s]: Polygon has incorrect points description", + "[%s]: Polygon points are not defined. Using dynamic subscription instead.", polygon_name_.c_str()); - return false; } - // Obtain polygon vertices - Point point; - bool first = true; - for (double val : poly_row) { - if (first) { - point.x = val; - } else { - point.y = val; - poly_.push_back(point); - } - first = !first; + if (action_type_ == STOP || action_type_ == SLOWDOWN) { + // Dynamic polygon will be used + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".polygon_sub_topic", rclcpp::PARAMETER_STRING); + polygon_sub_topic = + node->get_parameter(polygon_name_ + ".polygon_sub_topic").as_string(); + } else if (action_type_ == APPROACH) { + // Obtain the footprint topic to make a footprint subscription for approach polygon + nav2_util::declare_parameter_if_not_declared( + node, polygon_name_ + ".footprint_topic", + rclcpp::ParameterValue("local_costmap/published_footprint")); + footprint_topic = + node->get_parameter(polygon_name_ + ".footprint_topic").as_string(); } } catch (const std::exception & ex) { RCLCPP_ERROR( @@ -345,6 +386,54 @@ bool Polygon::getParameters(std::string & polygon_pub_topic, std::string & footp return true; } +void Polygon::updatePolygon(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg) +{ + std::size_t new_size = msg->polygon.points.size(); + + if (new_size < 3) { + RCLCPP_ERROR( + logger_, + "[%s]: Polygon should have at least 3 points", + polygon_name_.c_str()); + return; + } + + // Get the transform from PolygonStamped frame to base_frame_id_ + tf2::Transform tf_transform; + if ( + !nav2_util::getTransform( + msg->header.frame_id, base_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { + return; + } + + // Set main polygon vertices + poly_.resize(new_size); + for (std::size_t i = 0; i < new_size; i++) { + // Transform point coordinates from PolygonStamped frame -> to base frame + tf2::Vector3 p_v3_s(msg->polygon.points[i].x, msg->polygon.points[i].y, 0.0); + tf2::Vector3 p_v3_b = tf_transform * p_v3_s; + + // Fill poly_ array + poly_[i] = {p_v3_b.x(), p_v3_b.y()}; + } + + if (visualize_) { + // Store polygon_ for visualization + polygon_ = *msg; + } +} + +void Polygon::polygonCallback(geometry_msgs::msg::PolygonStamped::ConstSharedPtr msg) +{ + RCLCPP_INFO( + logger_, + "[%s]: Polygon shape update has been arrived", + polygon_name_.c_str()); + updatePolygon(msg); +} + inline bool Polygon::isPointInside(const Point & point) const { // Adaptation of Shimrat, Moshe. "Algorithm 112: position of point relative to polygon." diff --git a/nav2_collision_monitor/src/range.cpp b/nav2_collision_monitor/src/range.cpp index 3ef51e2b69..0ba4be75aa 100644 --- a/nav2_collision_monitor/src/range.cpp +++ b/nav2_collision_monitor/src/range.cpp @@ -19,6 +19,7 @@ #include #include "nav2_util/node_utils.hpp" +#include "nav2_util/robot_utils.hpp" namespace nav2_collision_monitor { @@ -87,7 +88,12 @@ void Range::getData( // Obtaining the transform to get data from source frame and time where it was received // to the base frame and current time tf2::Transform tf_transform; - if (!getTransform(data_->header.frame_id, data_->header.stamp, curr_time, tf_transform)) { + if ( + !nav2_util::getTransform( + data_->header.frame_id, data_->header.stamp, + base_frame_id_, curr_time, global_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { return; } diff --git a/nav2_collision_monitor/src/scan.cpp b/nav2_collision_monitor/src/scan.cpp index d1f52b31f1..63ea1d6a76 100644 --- a/nav2_collision_monitor/src/scan.cpp +++ b/nav2_collision_monitor/src/scan.cpp @@ -17,6 +17,8 @@ #include #include +#include "nav2_util/robot_utils.hpp" + namespace nav2_collision_monitor { @@ -76,7 +78,12 @@ void Scan::getData( // Obtaining the transform to get data from source frame and time where it was received // to the base frame and current time tf2::Transform tf_transform; - if (!getTransform(data_->header.frame_id, data_->header.stamp, curr_time, tf_transform)) { + if ( + !nav2_util::getTransform( + data_->header.frame_id, data_->header.stamp, + base_frame_id_, curr_time, global_frame_id_, + transform_tolerance_, tf_buffer_, tf_transform)) + { return; } diff --git a/nav2_collision_monitor/src/source.cpp b/nav2_collision_monitor/src/source.cpp index bd1028518c..a7de3bbe4e 100644 --- a/nav2_collision_monitor/src/source.cpp +++ b/nav2_collision_monitor/src/source.cpp @@ -18,9 +18,6 @@ #include "geometry_msgs/msg/transform_stamped.hpp" -#include "tf2/convert.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" - #include "nav2_util/node_utils.hpp" namespace nav2_collision_monitor @@ -76,33 +73,4 @@ bool Source::sourceValid( return true; } -bool Source::getTransform( - const std::string & source_frame_id, - const rclcpp::Time & source_time, - const rclcpp::Time & curr_time, - tf2::Transform & tf2_transform) const -{ - geometry_msgs::msg::TransformStamped transform; - tf2_transform.setIdentity(); // initialize by identical transform - - try { - // Obtaining the transform to get data from source to base frame. - // This also considers the time shift between source and base. - transform = tf_buffer_->lookupTransform( - base_frame_id_, curr_time, - source_frame_id, source_time, - global_frame_id_, transform_tolerance_); - } catch (tf2::TransformException & e) { - RCLCPP_ERROR( - logger_, - "[%s]: Failed to get \"%s\"->\"%s\" frame transform: %s", - source_name_.c_str(), source_frame_id.c_str(), base_frame_id_.c_str(), e.what()); - return false; - } - - // Convert TransformStamped to TF2 transform - tf2::fromMsg(transform.transform, tf2_transform); - return true; -} - } // namespace nav2_collision_monitor diff --git a/nav2_collision_monitor/test/polygons_test.cpp b/nav2_collision_monitor/test/polygons_test.cpp index 87576ddda9..c8c6b46601 100644 --- a/nav2_collision_monitor/test/polygons_test.cpp +++ b/nav2_collision_monitor/test/polygons_test.cpp @@ -40,8 +40,10 @@ using namespace std::chrono_literals; static constexpr double EPSILON = std::numeric_limits::epsilon(); static const char BASE_FRAME_ID[]{"base_link"}; +static const char BASE2_FRAME_ID[]{"base2_link"}; static const char FOOTPRINT_TOPIC[]{"footprint"}; -static const char POLYGON_PUB_TOPIC[]{"polygon"}; +static const char POLYGON_SUB_TOPIC[]{"polygon_sub"}; +static const char POLYGON_PUB_TOPIC[]{"polygon_pub"}; static const char POLYGON_NAME[]{"TestPolygon"}; static const char CIRCLE_NAME[]{"TestCircle"}; static const std::vector SQUARE_POLYGON { @@ -68,9 +70,38 @@ class TestNode : public nav2_util::LifecycleNode ~TestNode() { + polygon_pub_.reset(); footprint_pub_.reset(); } + void publishPolygon(const std::string & frame_id, const bool is_correct) + { + polygon_pub_ = this->create_publisher( + POLYGON_SUB_TOPIC, rclcpp::SystemDefaultsQoS()); + + std::unique_ptr msg = + std::make_unique(); + + unsigned int polygon_size; + if (is_correct) { + polygon_size = SQUARE_POLYGON.size(); + } else { + polygon_size = 2; + } + + msg->header.frame_id = frame_id; + msg->header.stamp = this->now(); + + geometry_msgs::msg::Point32 p; + for (unsigned int i = 0; i < polygon_size; i = i + 2) { + p.x = SQUARE_POLYGON[i]; + p.y = SQUARE_POLYGON[i + 1]; + msg->polygon.points.push_back(p); + } + + polygon_pub_->publish(std::move(msg)); + } + void publishFootprint() { footprint_pub_ = this->create_publisher( @@ -112,6 +143,7 @@ class TestNode : public nav2_util::LifecycleNode } private: + rclcpp::Publisher::SharedPtr polygon_pub_; rclcpp::Publisher::SharedPtr footprint_pub_; rclcpp::Subscription::SharedPtr polygon_sub_; @@ -177,13 +209,22 @@ class Tester : public ::testing::Test protected: // Working with parameters void setCommonParameters(const std::string & polygon_name, const std::string & action_type); - void setPolygonParameters(const std::vector & points); + void setPolygonParameters( + const std::vector & points, + const bool is_static); void setCircleParameters(const double radius); bool checkUndeclaredParameter(const std::string & polygon_name, const std::string & param); // Creating routines - void createPolygon(const std::string & action_type); + void createPolygon(const std::string & action_type, const bool is_static); void createCircle(const std::string & action_type); + void sendTransforms(); + + // Wait until polygon will be received + bool waitPolygon( + const std::chrono::nanoseconds & timeout, + std::vector & poly); + // Wait until footprint will be received bool waitFootprint( const std::chrono::nanoseconds & timeout, @@ -257,17 +298,25 @@ void Tester::setCommonParameters(const std::string & polygon_name, const std::st rclcpp::Parameter(polygon_name + ".polygon_pub_topic", POLYGON_PUB_TOPIC)); } -void Tester::setPolygonParameters(const std::vector & points) +void Tester::setPolygonParameters( + const std::vector & points, const bool is_static) { - test_node_->declare_parameter( - std::string(POLYGON_NAME) + ".footprint_topic", rclcpp::ParameterValue(FOOTPRINT_TOPIC)); - test_node_->set_parameter( - rclcpp::Parameter(std::string(POLYGON_NAME) + ".footprint_topic", FOOTPRINT_TOPIC)); - - test_node_->declare_parameter( - std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(points)); - test_node_->set_parameter( - rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", points)); + if (is_static) { + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".points", rclcpp::ParameterValue(points)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".points", points)); + } else { + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".polygon_sub_topic", rclcpp::ParameterValue(POLYGON_SUB_TOPIC)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".polygon_sub_topic", POLYGON_SUB_TOPIC)); + + test_node_->declare_parameter( + std::string(POLYGON_NAME) + ".footprint_topic", rclcpp::ParameterValue(FOOTPRINT_TOPIC)); + test_node_->set_parameter( + rclcpp::Parameter(std::string(POLYGON_NAME) + ".footprint_topic", FOOTPRINT_TOPIC)); + } } void Tester::setCircleParameters(const double radius) @@ -296,10 +345,10 @@ bool Tester::checkUndeclaredParameter(const std::string & polygon_name, const st return ret; } -void Tester::createPolygon(const std::string & action_type) +void Tester::createPolygon(const std::string & action_type, const bool is_static) { setCommonParameters(POLYGON_NAME, action_type); - setPolygonParameters(SQUARE_POLYGON); + setPolygonParameters(SQUARE_POLYGON, is_static); polygon_ = std::make_shared( test_node_, POLYGON_NAME, @@ -320,6 +369,45 @@ void Tester::createCircle(const std::string & action_type) circle_->activate(); } +void Tester::sendTransforms() +{ + std::shared_ptr tf_broadcaster = + std::make_shared(test_node_); + + geometry_msgs::msg::TransformStamped transform; + + // base_frame -> base2_frame transform + transform.header.frame_id = BASE_FRAME_ID; + transform.child_frame_id = BASE2_FRAME_ID; + + transform.header.stamp = test_node_->now(); + transform.transform.translation.x = 0.1; + transform.transform.translation.y = 0.1; + transform.transform.translation.z = 0.0; + transform.transform.rotation.x = 0.0; + transform.transform.rotation.y = 0.0; + transform.transform.rotation.z = 0.0; + transform.transform.rotation.w = 1.0; + + tf_broadcaster->sendTransform(transform); +} + +bool Tester::waitPolygon( + const std::chrono::nanoseconds & timeout, + std::vector & poly) +{ + rclcpp::Time start_time = test_node_->now(); + while (rclcpp::ok() && test_node_->now() - start_time <= rclcpp::Duration(timeout)) { + polygon_->getPolygon(poly); + if (poly.size() > 0) { + return true; + } + rclcpp::spin_some(test_node_->get_node_base_interface()); + std::this_thread::sleep_for(10ms); + } + return false; +} + bool Tester::waitFootprint( const std::chrono::nanoseconds & timeout, std::vector & footprint) @@ -339,7 +427,7 @@ bool Tester::waitFootprint( TEST_F(Tester, testPolygonGetStopParameters) { - createPolygon("stop"); + createPolygon("stop", true); // Check that common parameters set correctly EXPECT_EQ(polygon_->getName(), POLYGON_NAME); @@ -363,7 +451,7 @@ TEST_F(Tester, testPolygonGetStopParameters) TEST_F(Tester, testPolygonGetSlowdownParameters) { - createPolygon("slowdown"); + createPolygon("slowdown", true); // Check that common parameters set correctly EXPECT_EQ(polygon_->getName(), POLYGON_NAME); @@ -376,7 +464,7 @@ TEST_F(Tester, testPolygonGetSlowdownParameters) TEST_F(Tester, testPolygonGetApproachParameters) { - createPolygon("approach"); + createPolygon("approach", true); // Check that common parameters set correctly EXPECT_EQ(polygon_->getName(), POLYGON_NAME); @@ -415,7 +503,7 @@ TEST_F(Tester, testPolygonUndeclaredActionType) TEST_F(Tester, testPolygonUndeclaredPoints) { - // "points" parameter is not initialized + // "points" and "polygon_sub_topic" parameters are not initialized test_node_->declare_parameter( std::string(POLYGON_NAME) + ".action_type", rclcpp::ParameterValue("stop")); test_node_->set_parameter( @@ -424,14 +512,15 @@ TEST_F(Tester, testPolygonUndeclaredPoints) test_node_, POLYGON_NAME, tf_buffer_, BASE_FRAME_ID, TRANSFORM_TOLERANCE); ASSERT_FALSE(polygon_->configure()); - // Check that "points" parameter is not set after configuring + // Check that "points" and "polygon_sub_topic" parameters are not set after configuring ASSERT_TRUE(checkUndeclaredParameter(POLYGON_NAME, "points")); + ASSERT_TRUE(checkUndeclaredParameter(POLYGON_NAME, "polygon_sub_topic")); } TEST_F(Tester, testPolygonIncorrectActionType) { setCommonParameters(POLYGON_NAME, "incorrect_action_type"); - setPolygonParameters(SQUARE_POLYGON); + setPolygonParameters(SQUARE_POLYGON, true); polygon_ = std::make_shared( test_node_, POLYGON_NAME, @@ -486,12 +575,79 @@ TEST_F(Tester, testCircleUndeclaredRadius) ASSERT_TRUE(checkUndeclaredParameter(CIRCLE_NAME, "radius")); } -TEST_F(Tester, testPolygonUpdate) +TEST_F(Tester, testPolygonTopicUpdate) +{ + createPolygon("stop", false); + + std::vector poly; + polygon_->getPolygon(poly); + ASSERT_FALSE(polygon_->isShapeSet()); + ASSERT_EQ(poly.size(), 0u); + + // Publish incorrect shape and check that polygon was not updated + test_node_->publishPolygon(BASE_FRAME_ID, false); + ASSERT_FALSE(waitPolygon(100ms, poly)); + ASSERT_FALSE(polygon_->isShapeSet()); + + // Publush correct polygon and make shure that it was set correctly + test_node_->publishPolygon(BASE_FRAME_ID, true); + ASSERT_TRUE(waitPolygon(500ms, poly)); + ASSERT_TRUE(polygon_->isShapeSet()); + ASSERT_EQ(poly.size(), 4u); + EXPECT_NEAR(poly[0].x, SQUARE_POLYGON[0], EPSILON); + EXPECT_NEAR(poly[0].y, SQUARE_POLYGON[1], EPSILON); + EXPECT_NEAR(poly[1].x, SQUARE_POLYGON[2], EPSILON); + EXPECT_NEAR(poly[1].y, SQUARE_POLYGON[3], EPSILON); + EXPECT_NEAR(poly[2].x, SQUARE_POLYGON[4], EPSILON); + EXPECT_NEAR(poly[2].y, SQUARE_POLYGON[5], EPSILON); + EXPECT_NEAR(poly[3].x, SQUARE_POLYGON[6], EPSILON); + EXPECT_NEAR(poly[3].y, SQUARE_POLYGON[7], EPSILON); +} + +TEST_F(Tester, testPolygonTopicUpdateDifferentFrame) +{ + createPolygon("stop", false); + sendTransforms(); + + std::vector poly; + polygon_->getPolygon(poly); + ASSERT_EQ(poly.size(), 0u); + + // Publush polygon in different frame and make shure that it was set correctly + test_node_->publishPolygon(BASE2_FRAME_ID, true); + ASSERT_TRUE(waitPolygon(500ms, poly)); + ASSERT_EQ(poly.size(), 4u); + EXPECT_NEAR(poly[0].x, SQUARE_POLYGON[0] + 0.1, EPSILON); + EXPECT_NEAR(poly[0].y, SQUARE_POLYGON[1] + 0.1, EPSILON); + EXPECT_NEAR(poly[1].x, SQUARE_POLYGON[2] + 0.1, EPSILON); + EXPECT_NEAR(poly[1].y, SQUARE_POLYGON[3] + 0.1, EPSILON); + EXPECT_NEAR(poly[2].x, SQUARE_POLYGON[4] + 0.1, EPSILON); + EXPECT_NEAR(poly[2].y, SQUARE_POLYGON[5] + 0.1, EPSILON); + EXPECT_NEAR(poly[3].x, SQUARE_POLYGON[6] + 0.1, EPSILON); + EXPECT_NEAR(poly[3].y, SQUARE_POLYGON[7] + 0.1, EPSILON); +} + +TEST_F(Tester, testPolygonTopicUpdateIncorrectFrame) +{ + createPolygon("stop", false); + sendTransforms(); + + std::vector poly; + polygon_->getPolygon(poly); + ASSERT_EQ(poly.size(), 0u); + + // Publush polygon in incorrect frame and check that polygon was not updated + test_node_->publishPolygon("incorrect_frame", true); + ASSERT_FALSE(waitPolygon(100ms, poly)); +} + +TEST_F(Tester, testPolygonFootprintUpdate) { - createPolygon("approach"); + createPolygon("approach", false); std::vector poly; polygon_->getPolygon(poly); + ASSERT_FALSE(polygon_->isShapeSet()); ASSERT_EQ(poly.size(), 0u); test_node_->publishFootprint(); @@ -499,6 +655,7 @@ TEST_F(Tester, testPolygonUpdate) std::vector footprint; ASSERT_TRUE(waitFootprint(500ms, footprint)); + ASSERT_TRUE(polygon_->isShapeSet()); ASSERT_EQ(footprint.size(), 4u); EXPECT_NEAR(footprint[0].x, SQUARE_POLYGON[0], EPSILON); EXPECT_NEAR(footprint[0].y, SQUARE_POLYGON[1], EPSILON); @@ -512,7 +669,7 @@ TEST_F(Tester, testPolygonUpdate) TEST_F(Tester, testPolygonGetPointsInside) { - createPolygon("stop"); + createPolygon("stop", true); std::vector points; @@ -533,7 +690,7 @@ TEST_F(Tester, testPolygonGetPointsInsideEdge) // Test for checking edge cases in raytracing algorithm. // All points are lie on the edge lines parallel to OX, where the raytracing takes place. setCommonParameters(POLYGON_NAME, "stop"); - setPolygonParameters(ARBITRARY_POLYGON); + setPolygonParameters(ARBITRARY_POLYGON, true); polygon_ = std::make_shared( test_node_, POLYGON_NAME, @@ -572,7 +729,7 @@ TEST_F(Tester, testCircleGetPointsInside) TEST_F(Tester, testPolygonGetCollisionTime) { - createPolygon("approach"); + createPolygon("approach", false); // Set footprint for Polygon test_node_->publishFootprint(); @@ -640,7 +797,7 @@ TEST_F(Tester, testPolygonGetCollisionTime) TEST_F(Tester, testPolygonPublish) { - createPolygon("stop"); + createPolygon("stop", true); polygon_->publish(); geometry_msgs::msg::PolygonStamped::SharedPtr polygon_received = test_node_->waitPolygonReceived(500ms); @@ -666,7 +823,7 @@ TEST_F(Tester, testPolygonDefaultVisualize) std::string(POLYGON_NAME) + ".action_type", rclcpp::ParameterValue("stop")); test_node_->set_parameter( rclcpp::Parameter(std::string(POLYGON_NAME) + ".action_type", "stop")); - setPolygonParameters(SQUARE_POLYGON); + setPolygonParameters(SQUARE_POLYGON, true); // Create new polygon polygon_ = std::make_shared( diff --git a/nav2_util/include/nav2_util/robot_utils.hpp b/nav2_util/include/nav2_util/robot_utils.hpp index b9712044b6..22306cb545 100644 --- a/nav2_util/include/nav2_util/robot_utils.hpp +++ b/nav2_util/include/nav2_util/robot_utils.hpp @@ -18,8 +18,10 @@ #define NAV2_UTIL__ROBOT_UTILS_HPP_ #include +#include #include "geometry_msgs/msg/pose_stamped.hpp" +#include "tf2/time.h" #include "tf2_ros/buffer.h" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "rclcpp/rclcpp.hpp" @@ -56,6 +58,57 @@ bool transformPoseInTargetFrame( tf2_ros::Buffer & tf_buffer, const std::string target_frame, const double transform_timeout = 0.1); +/** + * @brief Obtains a transform from source_frame_id at source_time -> + * to target_frame_id at target_time time + * @param source_frame_id Source frame ID to convert from + * @param source_time Source timestamp to convert from + * @param target_frame_id Target frame ID to convert to + * @param target_time Target time to interpolate to + * @param transform_tolerance Transform tolerance + * @param tf_transform Output source->target transform + * @return True if got correct transform, otherwise false + */ + +/** + * @brief Obtains a transform from source_frame_id -> to target_frame_id + * @param source_frame_id Source frame ID to convert from + * @param target_frame_id Target frame ID to convert to + * @param transform_tolerance Transform tolerance + * @param tf_buffer TF buffer to use for the transformation + * @param tf_transform Output source->target transform + * @return True if got correct transform, otherwise false + */ +bool getTransform( + const std::string & source_frame_id, + const std::string & target_frame_id, + const tf2::Duration & transform_tolerance, + const std::shared_ptr tf_buffer, + tf2::Transform & tf2_transform); + +/** + * @brief Obtains a transform from source_frame_id at source_time -> + * to target_frame_id at target_time time + * @param source_frame_id Source frame ID to convert from + * @param source_time Source timestamp to convert from + * @param target_frame_id Target frame ID to convert to + * @param target_time Current node time to interpolate to + * @param fixed_frame_id The frame in which to assume the transform is constant in time + * @param transform_tolerance Transform tolerance + * @param tf_buffer TF buffer to use for the transformation + * @param tf_transform Output source->target transform + * @return True if got correct transform, otherwise false + */ +bool getTransform( + const std::string & source_frame_id, + const rclcpp::Time & source_time, + const std::string & target_frame_id, + const rclcpp::Time & target_time, + const std::string & fixed_frame_id, + const tf2::Duration & transform_tolerance, + const std::shared_ptr tf_buffer, + tf2::Transform & tf2_transform); + } // end namespace nav2_util #endif // NAV2_UTIL__ROBOT_UTILS_HPP_ diff --git a/nav2_util/src/robot_utils.cpp b/nav2_util/src/robot_utils.cpp index e99d9e3334..8714f05a99 100644 --- a/nav2_util/src/robot_utils.cpp +++ b/nav2_util/src/robot_utils.cpp @@ -17,6 +17,7 @@ #include #include +#include "tf2/convert.h" #include "nav2_util/robot_utils.hpp" #include "rclcpp/logger.hpp" @@ -75,4 +76,70 @@ bool transformPoseInTargetFrame( return false; } +bool getTransform( + const std::string & source_frame_id, + const std::string & target_frame_id, + const tf2::Duration & transform_tolerance, + const std::shared_ptr tf_buffer, + tf2::Transform & tf2_transform) +{ + geometry_msgs::msg::TransformStamped transform; + tf2_transform.setIdentity(); // initialize by identical transform + + if (source_frame_id == target_frame_id) { + // We are already in required frame + return true; + } + + try { + // Obtaining the transform to get data from source to target frame + transform = tf_buffer->lookupTransform( + target_frame_id, source_frame_id, + tf2::TimePointZero, transform_tolerance); + } catch (tf2::TransformException & e) { + RCLCPP_ERROR( + rclcpp::get_logger("getTransform"), + "Failed to get \"%s\"->\"%s\" frame transform: %s", + source_frame_id.c_str(), target_frame_id.c_str(), e.what()); + return false; + } + + // Convert TransformStamped to TF2 transform + tf2::fromMsg(transform.transform, tf2_transform); + return true; +} + +bool getTransform( + const std::string & source_frame_id, + const rclcpp::Time & source_time, + const std::string & target_frame_id, + const rclcpp::Time & target_time, + const std::string & fixed_frame_id, + const tf2::Duration & transform_tolerance, + const std::shared_ptr tf_buffer, + tf2::Transform & tf2_transform) +{ + geometry_msgs::msg::TransformStamped transform; + tf2_transform.setIdentity(); // initialize by identical transform + + try { + // Obtaining the transform to get data from source to target frame. + // This also considers the time shift between source and target. + transform = tf_buffer->lookupTransform( + target_frame_id, target_time, + source_frame_id, source_time, + fixed_frame_id, transform_tolerance); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + rclcpp::get_logger("getTransform"), + "Failed to get \"%s\"->\"%s\" frame transform: %s", + source_frame_id.c_str(), target_frame_id.c_str(), ex.what()); + return false; + } + + // Convert TransformStamped to TF2 transform + tf2::fromMsg(transform.transform, tf2_transform); + return true; +} + } // end namespace nav2_util From bc950388f9fb2bf9798386ede09b999ebc07a6ac Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 17 Nov 2022 15:03:20 -0800 Subject: [PATCH 32/43] adding getCostScalingFactor (#3290) --- nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp | 5 +++++ nav2_costmap_2d/plugins/inflation_layer.cpp | 2 +- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp index 5f68dcaed1..c330cd4ef6 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/inflation_layer.hpp @@ -172,6 +172,11 @@ class InflationLayer : public Layer return access_; } + double getCostScalingFactor() + { + return cost_scaling_factor_; + } + protected: /** * @brief Process updates on footprint changes to the inflation layer diff --git a/nav2_costmap_2d/plugins/inflation_layer.cpp b/nav2_costmap_2d/plugins/inflation_layer.cpp index 796c2fd62f..067877e548 100644 --- a/nav2_costmap_2d/plugins/inflation_layer.cpp +++ b/nav2_costmap_2d/plugins/inflation_layer.cpp @@ -442,7 +442,7 @@ InflationLayer::dynamicParametersCallback( need_reinflation_ = true; need_cache_recompute = true; } else if (param_name == name_ + "." + "cost_scaling_factor" && // NOLINT - cost_scaling_factor_ != parameter.as_double()) + getCostScalingFactor() != parameter.as_double()) { cost_scaling_factor_ = parameter.as_double(); need_reinflation_ = true; From 6b221b74c816f0c19701a6a187ab117b08834a0a Mon Sep 17 00:00:00 2001 From: Owen Hooper <17ofh@queensu.ca> Date: Fri, 18 Nov 2022 13:40:52 -0500 Subject: [PATCH 33/43] Implemented smoother selector bt node (#3283) * Implemented smoother selector bt node Signed-off-by: Owen Hooper <17ofh@queensu.ca> * updated copyright in modified file Signed-off-by: Owen Hooper <17ofh@queensu.ca> Signed-off-by: Owen Hooper <17ofh@queensu.ca> --- nav2_behavior_tree/CMakeLists.txt | 3 + .../plugins/action/smoother_selector_node.hpp | 99 ++++++++++ nav2_behavior_tree/nav2_tree_nodes.xml | 6 + .../plugins/action/smoother_selector_node.cpp | 92 +++++++++ .../test/plugins/action/CMakeLists.txt | 4 + .../action/test_smoother_selector_node.cpp | 187 ++++++++++++++++++ 6 files changed, 391 insertions(+) create mode 100644 nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp create mode 100644 nav2_behavior_tree/plugins/action/smoother_selector_node.cpp create mode 100644 nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp diff --git a/nav2_behavior_tree/CMakeLists.txt b/nav2_behavior_tree/CMakeLists.txt index 224e8498e1..1767bdcf32 100644 --- a/nav2_behavior_tree/CMakeLists.txt +++ b/nav2_behavior_tree/CMakeLists.txt @@ -183,6 +183,9 @@ list(APPEND plugin_libs nav2_planner_selector_bt_node) add_library(nav2_controller_selector_bt_node SHARED plugins/action/controller_selector_node.cpp) list(APPEND plugin_libs nav2_controller_selector_bt_node) +add_library(nav2_smoother_selector_bt_node SHARED plugins/action/smoother_selector_node.cpp) +list(APPEND plugin_libs nav2_smoother_selector_bt_node) + add_library(nav2_goal_checker_selector_bt_node SHARED plugins/action/goal_checker_selector_node.cpp) list(APPEND plugin_libs nav2_goal_checker_selector_bt_node) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp new file mode 100644 index 0000000000..f9bb293d26 --- /dev/null +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/smoother_selector_node.hpp @@ -0,0 +1,99 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Pablo Iñigo Blasco +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SMOOTHER_SELECTOR_NODE_HPP_ +#define NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SMOOTHER_SELECTOR_NODE_HPP_ + +#include +#include + +#include "std_msgs/msg/string.hpp" + +#include "behaviortree_cpp_v3/action_node.h" + +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +/** + * @brief The SmootherSelector behavior is used to switch the smoother + * that will be used by the smoother server. It subscribes to a topic "smoother_selector" + * to get the decision about what smoother must be used. It is usually used before of + * the FollowPath. The selected_smoother output port is passed to smoother_id + * input port of the FollowPath + */ +class SmootherSelector : public BT::SyncActionNode +{ +public: + /** + * @brief A constructor for nav2_behavior_tree::SmootherSelector + * + * @param xml_tag_name Name for the XML tag for this node + * @param conf BT node configuration + */ + SmootherSelector( + const std::string & xml_tag_name, + const BT::NodeConfiguration & conf); + + /** + * @brief Creates list of BT ports + * @return BT::PortsList Containing basic ports along with node-specific ports + */ + static BT::PortsList providedPorts() + { + return { + BT::InputPort( + "default_smoother", + "the default smoother to use if there is not any external topic message received."), + + BT::InputPort( + "topic_name", + "smoother_selector", + "the input topic name to select the smoother"), + + BT::OutputPort( + "selected_smoother", + "Selected smoother by subscription") + }; + } + +private: + /** + * @brief Function to perform some user-defined operation on tick + */ + BT::NodeStatus tick() override; + + /** + * @brief callback function for the smoother_selector topic + * + * @param msg the message with the id of the smoother_selector + */ + void callbackSmootherSelect(const std_msgs::msg::String::SharedPtr msg); + + rclcpp::Subscription::SharedPtr smoother_selector_sub_; + + std::string last_selected_smoother_; + + rclcpp::Node::SharedPtr node_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::executors::SingleThreadedExecutor callback_group_executor_; + + std::string topic_name_; +}; + +} // namespace nav2_behavior_tree + +#endif // NAV2_BEHAVIOR_TREE__PLUGINS__ACTION__SMOOTHER_SELECTOR_NODE_HPP_ diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 95e5a0917d..cf3ff5f903 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -166,6 +166,12 @@ Name of the selected controller received from the topic subcription + + Name of the topic to receive smoother selection commands + Default smoother of the smoother selector + Name of the selected smoother received from the topic subcription + + Name of the topic to receive goal checker selection commands Default goal checker of the controller selector diff --git a/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp b/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp new file mode 100644 index 0000000000..0a84062e08 --- /dev/null +++ b/nav2_behavior_tree/plugins/action/smoother_selector_node.cpp @@ -0,0 +1,92 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Pablo Iñigo Blasco +// Copyright (c) 2022 Owen Hooper +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "std_msgs/msg/string.hpp" + +#include "nav2_behavior_tree/plugins/action/smoother_selector_node.hpp" + +#include "rclcpp/rclcpp.hpp" + +namespace nav2_behavior_tree +{ + +using std::placeholders::_1; + +SmootherSelector::SmootherSelector( + const std::string & name, + const BT::NodeConfiguration & conf) +: BT::SyncActionNode(name, conf) +{ + node_ = config().blackboard->get("node"); + callback_group_ = node_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, + false); + callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); + + getInput("topic_name", topic_name_); + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + rclcpp::SubscriptionOptions sub_option; + sub_option.callback_group = callback_group_; + smoother_selector_sub_ = node_->create_subscription( + topic_name_, + qos, + std::bind(&SmootherSelector::callbackSmootherSelect, this, _1), + sub_option); +} + +BT::NodeStatus SmootherSelector::tick() +{ + callback_group_executor_.spin_some(); + + // This behavior always use the last selected smoother received from the topic input. + // When no input is specified it uses the default smoother. + // If the default smoother is not specified then we work in "required smoother mode": + // In this mode, the behavior returns failure if the smoother selection is not received from + // the topic input. + if (last_selected_smoother_.empty()) { + std::string default_smoother; + getInput("default_smoother", default_smoother); + if (default_smoother.empty()) { + return BT::NodeStatus::FAILURE; + } else { + last_selected_smoother_ = default_smoother; + } + } + + setOutput("selected_smoother", last_selected_smoother_); + + return BT::NodeStatus::SUCCESS; +} + +void +SmootherSelector::callbackSmootherSelect(const std_msgs::msg::String::SharedPtr msg) +{ + last_selected_smoother_ = msg->data; +} + +} // namespace nav2_behavior_tree + +#include "behaviortree_cpp_v3/bt_factory.h" +BT_REGISTER_NODES(factory) +{ + factory.registerNodeType("SmootherSelector"); +} diff --git a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt index 0b7cae1a6c..c640e82424 100644 --- a/nav2_behavior_tree/test/plugins/action/CMakeLists.txt +++ b/nav2_behavior_tree/test/plugins/action/CMakeLists.txt @@ -100,6 +100,10 @@ ament_add_gtest(test_controller_selector_node test_controller_selector_node.cpp) target_link_libraries(test_controller_selector_node nav2_controller_selector_bt_node) ament_target_dependencies(test_controller_selector_node ${dependencies}) +ament_add_gtest(test_smoother_selector_node test_smoother_selector_node.cpp) +target_link_libraries(test_smoother_selector_node nav2_smoother_selector_bt_node) +ament_target_dependencies(test_smoother_selector_node ${dependencies}) + ament_add_gtest(test_goal_checker_selector_node test_goal_checker_selector_node.cpp) target_link_libraries(test_goal_checker_selector_node nav2_goal_checker_selector_bt_node) ament_target_dependencies(test_goal_checker_selector_node ${dependencies}) diff --git a/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp new file mode 100644 index 0000000000..c59cac1b2c --- /dev/null +++ b/nav2_behavior_tree/test/plugins/action/test_smoother_selector_node.cpp @@ -0,0 +1,187 @@ +// Copyright (c) 2018 Intel Corporation +// Copyright (c) 2020 Pablo Iñigo Blasco +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include + +#include "../../test_action_server.hpp" +#include "behaviortree_cpp_v3/bt_factory.h" +#include "nav2_behavior_tree/plugins/action/smoother_selector_node.hpp" +#include "nav_msgs/msg/path.hpp" +#include "std_msgs/msg/string.hpp" + +class SmootherSelectorTestFixture : public ::testing::Test +{ +public: + static void SetUpTestCase() + { + node_ = std::make_shared("smoother_selector_test_fixture"); + factory_ = std::make_shared(); + + config_ = new BT::NodeConfiguration(); + + // Create the blackboard that will be shared by all of the nodes in the tree + config_->blackboard = BT::Blackboard::create(); + // Put items on the blackboard + config_->blackboard->set("node", node_); + + BT::NodeBuilder builder = [](const std::string & name, const BT::NodeConfiguration & config) { + return std::make_unique(name, config); + }; + + factory_->registerBuilder( + "SmootherSelector", + builder); + } + + static void TearDownTestCase() + { + delete config_; + config_ = nullptr; + node_.reset(); + factory_.reset(); + } + + void TearDown() override + { + tree_.reset(); + } + +protected: + static rclcpp::Node::SharedPtr node_; + static BT::NodeConfiguration * config_; + static std::shared_ptr factory_; + static std::shared_ptr tree_; +}; + +rclcpp::Node::SharedPtr SmootherSelectorTestFixture::node_ = nullptr; + +BT::NodeConfiguration * SmootherSelectorTestFixture::config_ = nullptr; +std::shared_ptr SmootherSelectorTestFixture::factory_ = nullptr; +std::shared_ptr SmootherSelectorTestFixture::tree_ = nullptr; + +TEST_F(SmootherSelectorTestFixture, test_custom_topic) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // check default value + std::string selected_smoother_result; + config_->blackboard->get("selected_smoother", selected_smoother_result); + + EXPECT_EQ(selected_smoother_result, "DWB"); + + std_msgs::msg::String selected_smoother_cmd; + + selected_smoother_cmd.data = "DWC"; + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + auto smoother_selector_pub = + node_->create_publisher("smoother_selector_custom_topic_name", qos); + + // publish a few updates of the selected_smoother + auto start = node_->now(); + while ((node_->now() - start).seconds() < 0.5) { + tree_->rootNode()->executeTick(); + smoother_selector_pub->publish(selected_smoother_cmd); + + rclcpp::spin_some(node_); + } + + // check smoother updated + config_->blackboard->get("selected_smoother", selected_smoother_result); + EXPECT_EQ("DWC", selected_smoother_result); +} + +TEST_F(SmootherSelectorTestFixture, test_default_topic) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // check default value + std::string selected_smoother_result; + config_->blackboard->get("selected_smoother", selected_smoother_result); + + EXPECT_EQ(selected_smoother_result, "GridBased"); + + std_msgs::msg::String selected_smoother_cmd; + + selected_smoother_cmd.data = "RRT"; + + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local().reliable(); + + auto smoother_selector_pub = + node_->create_publisher("smoother_selector", qos); + + // publish a few updates of the selected_smoother + auto start = node_->now(); + while ((node_->now() - start).seconds() < 0.5) { + tree_->rootNode()->executeTick(); + smoother_selector_pub->publish(selected_smoother_cmd); + + rclcpp::spin_some(node_); + } + + // check smoother updated + config_->blackboard->get("selected_smoother", selected_smoother_result); + EXPECT_EQ("RRT", selected_smoother_result); +} + +int main(int argc, char ** argv) +{ + ::testing::InitGoogleTest(&argc, argv); + + // initialize ROS + rclcpp::init(argc, argv); + + int all_successful = RUN_ALL_TESTS(); + + // shutdown ROS + rclcpp::shutdown(); + + return all_successful; +} From ebf079e0843f83ec0af8a0711a64047e412fa943 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 18 Nov 2022 14:26:27 -0500 Subject: [PATCH 34/43] Pipe error codes (#3251) * issue with finding key * passed up codes to bt_navigator * lint fix * updates * adding error_code_id back in * error codes names in params * bump error codes * lint * spelling * test fix * update behavior trees * cleanup * Update bt_action_server_impl.hpp * code review * lint * code review * log fix * error code for waypoint follower * clean up * remove waypoint error test, too flaky on CI * lint and code review * rough imp for waypoint changes * lint * code review * build fix * clean up * revert * space * remove * try to make github happ * stop gap * loading in param file * working tests :) * lint * fixed cmake * lint * lint * trigger build * added invalid plugin error * added test for piping up error codes * clean up * test waypoint follower * only launch what is needed * waypoint test * revert lines for robot navigator * fix test * waypoint test * switched to uint16 * clean up * code review * todo to note * lint * remove comment * Update nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp Co-authored-by: Steve Macenski * rename error_codes * error code for navigate to pose * error codes for navigate through poses. * error codes for navigate through poses * message update for waypoint follower * rename to error code * update node xml Co-authored-by: Joshua Wallace Co-authored-by: Steve Macenski --- .../nav2_behavior_tree/bt_action_server.hpp | 10 + .../bt_action_server_impl.hpp | 49 +++++ .../compute_path_through_poses_action.hpp | 2 +- .../action/compute_path_to_pose_action.hpp | 2 +- .../plugins/action/follow_path_action.hpp | 2 +- .../action/navigate_through_poses_action.hpp | 21 +++ .../action/navigate_to_pose_action.hpp | 21 +++ nav2_behavior_tree/nav2_tree_nodes.xml | 9 +- .../compute_path_through_poses_action.cpp | 6 +- .../action/compute_path_to_pose_action.cpp | 6 +- .../plugins/action/follow_path_action.cpp | 6 +- .../action/navigate_through_poses_action.cpp | 20 ++ .../action/navigate_to_pose_action.cpp | 24 ++- nav2_bringup/params/nav2_params.yaml | 96 +++++----- .../behavior_trees/follow_point.xml | 4 +- ...replanning_and_if_path_becomes_invalid.xml | 4 +- ...hrough_poses_w_replanning_and_recovery.xml | 6 +- ...gate_to_pose_w_replanning_and_recovery.xml | 4 +- ..._replanning_goal_patience_and_recovery.xml | 8 +- ...eplanning_only_if_path_becomes_invalid.xml | 4 +- .../navigate_w_replanning_distance.xml | 4 +- ...e_w_replanning_only_if_goal_is_updated.xml | 4 +- ...eplanning_only_if_path_becomes_invalid.xml | 8 +- .../navigate_w_replanning_speed.xml | 4 +- .../navigate_w_replanning_time.xml | 4 +- nav2_bt_navigator/src/bt_navigator.cpp | 2 - .../src/navigators/navigate_to_pose.cpp | 1 - nav2_controller/src/controller_server.cpp | 9 +- .../nav2_core/controller_exceptions.hpp | 7 + .../include/nav2_core/planner_exceptions.hpp | 7 + nav2_msgs/CMakeLists.txt | 1 + .../action/ComputePathThroughPoses.action | 23 +-- nav2_msgs/action/ComputePathToPose.action | 21 ++- nav2_msgs/action/FollowPath.action | 17 +- nav2_msgs/action/FollowWaypoints.action | 8 +- nav2_msgs/action/NavigateThroughPoses.action | 5 + nav2_msgs/action/NavigateToPose.action | 5 + nav2_msgs/msg/MissedWaypoint.msg | 3 + nav2_planner/src/planner_server.cpp | 13 +- .../nav2_simple_commander/robot_navigator.py | 28 ++- nav2_system_tests/CMakeLists.txt | 37 ++++ nav2_system_tests/package.xml | 2 + .../src/error_codes/CMakeLists.txt | 12 ++ .../controller/controller_error_plugins.cpp | 25 +++ .../controller/controller_error_plugins.hpp | 114 ++++++++++++ .../src/error_codes/controller_plugins.xml | 26 +++ .../src/error_codes/error_code_param.yaml | 175 ++++++++++++++++++ .../planner/planner_error_plugin.cpp | 26 +++ .../planner/planner_error_plugin.hpp | 144 ++++++++++++++ .../src/error_codes/planner_plugins.xml | 42 +++++ .../src/error_codes/test_error_codes.py | 130 +++++++++++++ .../error_codes/test_error_codes_launch.py | 94 ++++++++++ .../src/planning/test_planner_plugins.cpp | 9 +- .../src/waypoint_follower/tester.py | 10 +- .../waypoint_follower.hpp | 11 +- .../src/waypoint_follower.cpp | 46 +++-- 56 files changed, 1221 insertions(+), 160 deletions(-) create mode 100644 nav2_msgs/msg/MissedWaypoint.msg create mode 100644 nav2_system_tests/src/error_codes/CMakeLists.txt create mode 100644 nav2_system_tests/src/error_codes/controller/controller_error_plugins.cpp create mode 100644 nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp create mode 100644 nav2_system_tests/src/error_codes/controller_plugins.xml create mode 100644 nav2_system_tests/src/error_codes/error_code_param.yaml create mode 100644 nav2_system_tests/src/error_codes/planner/planner_error_plugin.cpp create mode 100644 nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp create mode 100644 nav2_system_tests/src/error_codes/planner_plugins.xml create mode 100755 nav2_system_tests/src/error_codes/test_error_codes.py create mode 100755 nav2_system_tests/src/error_codes/test_error_codes_launch.py diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp index 7558080347..79b1df5bb4 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server.hpp @@ -189,6 +189,13 @@ class BtActionServer */ void executeCallback(); + /** + * @brief updates the action server result to the highest priority error code posted on the + * blackboard + * @param result the action server result to be updated + */ + void populateErrorCode(typename std::shared_ptr result); + // Action name std::string action_name_; @@ -211,6 +218,9 @@ class BtActionServer // Libraries to pull plugins (BT Nodes) from std::vector plugin_lib_names_; + // Error code id names + std::vector error_code_names_; + // A regular, non-spinning ROS node that we can use for calls to the action client rclcpp::Node::SharedPtr client_node_; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index db619e1a70..c3f9244f17 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav2_behavior_tree/bt_action_server.hpp" @@ -59,6 +60,24 @@ BtActionServer::BtActionServer( if (!node->has_parameter("default_server_timeout")) { node->declare_parameter("default_server_timeout", 20); } + + std::vector error_code_names = { + "follow_path_error_code", + "compute_path_error_code" + }; + + if (!node->has_parameter("error_code_names")) { + std::string error_codes_str; + for (const auto & error_code : error_code_names) { + error_codes_str += error_code + "\n"; + } + RCLCPP_WARN_STREAM( + logger_, "Error_code parameters were not set. Using default values of: " + << error_codes_str + << "Make sure these match your BT and there are not other sources of error codes you want " + "reported to your application"); + node->declare_parameter("error_code_names", error_code_names); + } } template @@ -104,6 +123,9 @@ bool BtActionServer::on_configure() node->get_parameter("default_server_timeout", timeout); default_server_timeout_ = std::chrono::milliseconds(timeout); + // Get error code id names to grab off of the blackboard + error_code_names_ = node->get_parameter("error_code_names").as_string_array(); + // Create the class that registers our custom nodes and executes the BT bt_ = std::make_unique(plugin_lib_names_); @@ -226,6 +248,9 @@ void BtActionServer::executeCallback() // Give server an opportunity to populate the result message or simple give // an indication that the action is complete. auto result = std::make_shared(); + + populateErrorCode(result); + on_completion_callback_(result, rc); switch (rc) { @@ -246,6 +271,30 @@ void BtActionServer::executeCallback() } } +template +void BtActionServer::populateErrorCode( + typename std::shared_ptr result) +{ + int highest_priority_error_code = std::numeric_limits::max(); + for (const auto & error_code : error_code_names_) { + try { + int current_error_code = blackboard_->get(error_code); + if (current_error_code != 0 && current_error_code < highest_priority_error_code) { + highest_priority_error_code = current_error_code; + } + } catch (...) { + RCLCPP_ERROR( + logger_, + "Failed to get error code: %s from blackboard", + error_code.c_str()); + } + } + + if (highest_priority_error_code != std::numeric_limits::max()) { + result->error_code = highest_priority_error_code; + } +} + } // namespace nav2_behavior_tree #endif // NAV2_BEHAVIOR_TREE__BT_ACTION_SERVER_IMPL_HPP_ diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp index ad5954b6a4..0b5f3b0972 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.hpp @@ -86,7 +86,7 @@ class ComputePathThroughPosesAction "Mapped name to the planner plugin type to use"), BT::OutputPort("path", "Path created by ComputePathThroughPoses node"), BT::OutputPort( - "compute_path_through_poses_error_code", "The compute path through poses error code"), + "error_code_id", "The compute path through poses error code"), }); } }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp index 1c7f0526df..829fd04428 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp @@ -86,7 +86,7 @@ class ComputePathToPoseAction : public BtActionNode("path", "Path created by ComputePathToPose node"), BT::OutputPort( - "compute_path_to_pose_error_code", "The compute path to pose error code"), + "error_code_id", "The compute path to pose error code"), }); } }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp index 369e7ab0f9..4460b0e9a4 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/follow_path_action.hpp @@ -85,7 +85,7 @@ class FollowPathAction : public BtActionNode BT::InputPort("controller_id", ""), BT::InputPort("goal_checker_id", ""), BT::OutputPort( - "follow_path_error_code", "The follow path error code"), + "error_code_id", "The follow path error code"), }); } }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp index 7e7f3d5c30..45d0adff10 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_through_poses_action.hpp @@ -30,6 +30,10 @@ namespace nav2_behavior_tree */ class NavigateThroughPosesAction : public BtActionNode { + using Action = nav2_msgs::action::NavigateThroughPoses; + using ActionResult = Action::Result; + using ActionGoal = Action::Goal; + public: /** * @brief A constructor for nav2_behavior_tree::NavigateThroughPosesAction @@ -47,6 +51,21 @@ class NavigateThroughPosesAction : public BtActionNode("goals", "Destinations to plan through"), BT::InputPort("behavior_tree", "Behavior tree to run"), + BT::OutputPort( + "error_code_id", "The navigate through poses error code"), }); } }; diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp index 3708017f16..51c5974390 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/navigate_to_pose_action.hpp @@ -30,6 +30,10 @@ namespace nav2_behavior_tree */ class NavigateToPoseAction : public BtActionNode { + using Action = nav2_msgs::action::NavigateToPose; + using ActionResult = Action::Result; + using ActionGoal = Action::Goal; + public: /** * @brief A constructor for nav2_behavior_tree::NavigateToPoseAction @@ -47,6 +51,21 @@ class NavigateToPoseAction : public BtActionNode("goal", "Destination to plan to"), BT::InputPort("behavior_tree", "Behavior tree to run"), + BT::OutputPort( + "error_code_id", "Navigate to pose error code"), }); } }; diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index cf3ff5f903..6b6ac08eec 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -77,7 +77,7 @@ Service name Server timeout Path created by ComputePathToPose node - "The compute path to pose error code" + "Compute path to pose error code" @@ -87,8 +87,7 @@ Server timeout Mapped name to the planner plugin type to use Path created by ComputePathToPose node - "The compute path through poses - pose error code" + "Compute path through poses error code" @@ -115,7 +114,7 @@ Goal checker Service name Server timeout - Follow Path error code + Follow Path error code @@ -123,6 +122,7 @@ Service name Server timeout Behavior tree to run + Navigate to pose error code @@ -130,6 +130,7 @@ Service name Server timeout Behavior tree to run + Navigate through poses error code diff --git a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp index ffcbd328ae..5fede84f2c 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_through_poses_action.cpp @@ -42,7 +42,7 @@ BT::NodeStatus ComputePathThroughPosesAction::on_success() { setOutput("path", result_.result->path); // Set empty error code, action was successful - setOutput("compute_path_through_poses_error_code", ActionGoal::NONE); + setOutput("error_code_id", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } @@ -50,7 +50,7 @@ BT::NodeStatus ComputePathThroughPosesAction::on_aborted() { nav_msgs::msg::Path empty_path; setOutput("path", empty_path); - setOutput("compute_path_through_poses_error_code", result_.result->error_code); + setOutput("error_code_id", result_.result->error_code); return BT::NodeStatus::FAILURE; } @@ -59,7 +59,7 @@ BT::NodeStatus ComputePathThroughPosesAction::on_cancelled() nav_msgs::msg::Path empty_path; setOutput("path", empty_path); // Set empty error code, action was cancelled - setOutput("compute_path_through_poses_error_code", ActionGoal::NONE); + setOutput("error_code_id", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp index 68a909b52c..0619a41ee0 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp @@ -41,7 +41,7 @@ BT::NodeStatus ComputePathToPoseAction::on_success() { setOutput("path", result_.result->path); // Set empty error code, action was successful - setOutput("compute_path_to_pose_error_code", ActionGoal::NONE); + setOutput("error_code_id", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } @@ -49,7 +49,7 @@ BT::NodeStatus ComputePathToPoseAction::on_aborted() { nav_msgs::msg::Path empty_path; setOutput("path", empty_path); - setOutput("compute_path_to_pose_error_code", result_.result->error_code); + setOutput("error_code_id", result_.result->error_code); return BT::NodeStatus::FAILURE; } @@ -58,7 +58,7 @@ BT::NodeStatus ComputePathToPoseAction::on_cancelled() nav_msgs::msg::Path empty_path; setOutput("path", empty_path); // Set empty error code, action was cancelled - setOutput("compute_path_to_pose_error_code", ActionGoal::NONE); + setOutput("error_code_id", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/action/follow_path_action.cpp b/nav2_behavior_tree/plugins/action/follow_path_action.cpp index c90d8fb65a..3649fad8a1 100644 --- a/nav2_behavior_tree/plugins/action/follow_path_action.cpp +++ b/nav2_behavior_tree/plugins/action/follow_path_action.cpp @@ -37,20 +37,20 @@ void FollowPathAction::on_tick() BT::NodeStatus FollowPathAction::on_success() { - setOutput("follow_path_error_code", ActionGoal::NONE); + setOutput("error_code_id", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } BT::NodeStatus FollowPathAction::on_aborted() { - setOutput("follow_path_error_code", result_.result->error_code); + setOutput("error_code_id", result_.result->error_code); return BT::NodeStatus::FAILURE; } BT::NodeStatus FollowPathAction::on_cancelled() { // Set empty error code, action was cancelled - setOutput("compute_path_to_pose_error_code", ActionGoal::NONE); + setOutput("error_code_id", ActionGoal::NONE); return BT::NodeStatus::SUCCESS; } diff --git a/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp index 693fbfa146..5e3e65f424 100644 --- a/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_through_poses_action.cpp @@ -39,6 +39,26 @@ void NavigateThroughPosesAction::on_tick() getInput("behavior_tree", goal_.behavior_tree); } +BT::NodeStatus NavigateThroughPosesAction::on_success() +{ + setOutput("error_code_id", ActionGoal::NONE); + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus NavigateThroughPosesAction::on_aborted() +{ + setOutput("error_code_id", result_.result->error_code); + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus NavigateThroughPosesAction::on_cancelled() +{ + // Set empty error code, action was cancelled + setOutput("error_code_id", ActionGoal::NONE); + return BT::NodeStatus::SUCCESS; +} + + } // namespace nav2_behavior_tree #include "behaviortree_cpp_v3/bt_factory.h" diff --git a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp index 82cdab44f8..dabc576fcc 100644 --- a/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/navigate_to_pose_action.cpp @@ -24,9 +24,8 @@ NavigateToPoseAction::NavigateToPoseAction( const std::string & xml_tag_name, const std::string & action_name, const BT::NodeConfiguration & conf) -: BtActionNode(xml_tag_name, action_name, conf) -{ -} +: BtActionNode(xml_tag_name, action_name, conf) +{} void NavigateToPoseAction::on_tick() { @@ -39,6 +38,25 @@ void NavigateToPoseAction::on_tick() getInput("behavior_tree", goal_.behavior_tree); } +BT::NodeStatus NavigateToPoseAction::on_success() +{ + setOutput("error_code_id", ActionGoal::NONE); + return BT::NodeStatus::SUCCESS; +} + +BT::NodeStatus NavigateToPoseAction::on_aborted() +{ + setOutput("error_code_id", result_.result->error_code); + return BT::NodeStatus::FAILURE; +} + +BT::NodeStatus NavigateToPoseAction::on_cancelled() +{ + // Set empty error code, action was cancelled + setOutput("error_code_id", ActionGoal::NONE); + return BT::NodeStatus::SUCCESS; +} + } // namespace nav2_behavior_tree #include "behaviortree_cpp_v3/bt_factory.h" diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 4577f49550..69a1f2507e 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -50,52 +50,56 @@ bt_navigator: # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. plugin_lib_names: - - nav2_compute_path_to_pose_action_bt_node - - nav2_compute_path_through_poses_action_bt_node - - nav2_smooth_path_action_bt_node - - nav2_follow_path_action_bt_node - - nav2_spin_action_bt_node - - nav2_wait_action_bt_node - - nav2_assisted_teleop_action_bt_node - - nav2_back_up_action_bt_node - - nav2_drive_on_heading_bt_node - - nav2_clear_costmap_service_bt_node - - nav2_is_stuck_condition_bt_node - - nav2_goal_reached_condition_bt_node - - nav2_goal_updated_condition_bt_node - - nav2_globally_updated_goal_condition_bt_node - - nav2_is_path_valid_condition_bt_node - - nav2_initial_pose_received_condition_bt_node - - nav2_reinitialize_global_localization_service_bt_node - - nav2_rate_controller_bt_node - - nav2_distance_controller_bt_node - - nav2_speed_controller_bt_node - - nav2_truncate_path_action_bt_node - - nav2_truncate_path_local_action_bt_node - - nav2_goal_updater_node_bt_node - - nav2_recovery_node_bt_node - - nav2_pipeline_sequence_bt_node - - nav2_round_robin_node_bt_node - - nav2_transform_available_condition_bt_node - - nav2_time_expired_condition_bt_node - - nav2_path_expiring_timer_condition - - nav2_distance_traveled_condition_bt_node - - nav2_single_trigger_bt_node - - nav2_goal_updated_controller_bt_node - - nav2_is_battery_low_condition_bt_node - - nav2_navigate_through_poses_action_bt_node - - nav2_navigate_to_pose_action_bt_node - - nav2_remove_passed_goals_action_bt_node - - nav2_planner_selector_bt_node - - nav2_controller_selector_bt_node - - nav2_goal_checker_selector_bt_node - - nav2_controller_cancel_bt_node - - nav2_path_longer_on_approach_bt_node - - nav2_wait_cancel_bt_node - - nav2_spin_cancel_bt_node - - nav2_back_up_cancel_bt_node - - nav2_assisted_teleop_cancel_bt_node - - nav2_drive_on_heading_cancel_bt_node + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + error_code_names: + - compute_path_error_code + - follow_path_error_code + controller_server: ros__parameters: diff --git a/nav2_bt_navigator/behavior_trees/follow_point.xml b/nav2_bt_navigator/behavior_trees/follow_point.xml index ffae8d9b2a..930a2691fb 100644 --- a/nav2_bt_navigator/behavior_trees/follow_point.xml +++ b/nav2_bt_navigator/behavior_trees/follow_point.xml @@ -8,13 +8,13 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml index f4ed518444..0a00a60a4f 100644 --- a/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/nav_to_pose_with_consistent_replanning_and_if_path_becomes_invalid.xml @@ -19,13 +19,13 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml index 1378863cdf..3dc5a76503 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml @@ -11,13 +11,13 @@ - + - + ` - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml index 2396b844ae..d50bd7fe5d 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml @@ -10,12 +10,12 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml index 6b5883bfe7..e2bbeb12c7 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_to_pose_w_replanning_goal_patience_and_recovery.xml @@ -1,8 +1,8 @@ @@ -11,7 +11,7 @@ - + @@ -25,7 +25,7 @@ - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml index d21efa3b4b..bd4e1c9aca 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml @@ -17,13 +17,13 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml index 8ca5dbbc5a..9345d8f66d 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_distance.xml @@ -6,9 +6,9 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml index 3e889440a8..4ba2f67cc9 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_goal_is_updated.xml @@ -6,9 +6,9 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml index e921db5557..753de5d780 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_only_if_path_becomes_invalid.xml @@ -5,17 +5,17 @@ - + - + - + - \ No newline at end of file + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml index 049e78794b..97b70b7a62 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_speed.xml @@ -6,9 +6,9 @@ - + - + diff --git a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml index 33c2c411ad..db0e733db1 100644 --- a/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml +++ b/nav2_bt_navigator/behavior_trees/navigate_w_replanning_time.xml @@ -6,9 +6,9 @@ - + - + diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 7c83ff9d9a..3c6286ba01 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -17,8 +17,6 @@ #include #include #include -#include -#include #include #include "nav2_util/geometry_utils.hpp" diff --git a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp index 82727af5e2..226a673512 100644 --- a/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp +++ b/nav2_bt_navigator/src/navigators/navigate_to_pose.cpp @@ -14,7 +14,6 @@ #include #include -#include #include #include #include "nav2_bt_navigator/navigators/navigate_to_pose.hpp" diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index c527e41b14..9d341394a6 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -353,7 +353,7 @@ void ControllerServer::computeControl() if (findControllerId(c_name, current_controller)) { current_controller_ = current_controller; } else { - throw nav2_core::ControllerException("Failed to find controller name: " + c_name); + throw nav2_core::InvalidController("Failed to find controller name: " + c_name); } std::string gc_name = action_server_->get_current_goal()->goal_checker_id; @@ -403,6 +403,13 @@ void ControllerServer::computeControl() controller_frequency_); } } + } catch (nav2_core::InvalidController & e) { + RCLCPP_ERROR(this->get_logger(), "%s", e.what()); + publishZeroVelocity(); + std::shared_ptr result = std::make_shared(); + result->error_code = Action::Goal::INVALID_CONTROLLER; + action_server_->terminate_current(result); + return; } catch (nav2_core::ControllerTFError & e) { RCLCPP_ERROR(this->get_logger(), "%s", e.what()); publishZeroVelocity(); diff --git a/nav2_core/include/nav2_core/controller_exceptions.hpp b/nav2_core/include/nav2_core/controller_exceptions.hpp index bf9c22f4e6..5f87c4c287 100644 --- a/nav2_core/include/nav2_core/controller_exceptions.hpp +++ b/nav2_core/include/nav2_core/controller_exceptions.hpp @@ -27,6 +27,13 @@ class ControllerException : public std::runtime_error : std::runtime_error(description) {} }; +class InvalidController : public ControllerException +{ +public: + explicit InvalidController(const std::string & description) + : ControllerException(description) {} +}; + class ControllerTFError : public ControllerException { public: diff --git a/nav2_core/include/nav2_core/planner_exceptions.hpp b/nav2_core/include/nav2_core/planner_exceptions.hpp index e607d98ccc..2680b3c669 100644 --- a/nav2_core/include/nav2_core/planner_exceptions.hpp +++ b/nav2_core/include/nav2_core/planner_exceptions.hpp @@ -50,6 +50,13 @@ class PlannerException : public std::runtime_error : std::runtime_error(description) {} }; +class InvalidPlanner : public PlannerException +{ +public: + explicit InvalidPlanner(const std::string & description) + : PlannerException(description) {} +}; + class StartOccupied : public PlannerException { public: diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 2be81db0a7..09fbc08226 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -22,6 +22,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/BehaviorTreeLog.msg" "msg/Particle.msg" "msg/ParticleCloud.msg" + "msg/MissedWaypoint.msg" "srv/GetCostmap.srv" "srv/IsPathValid.srv" "srv/ClearCostmapExceptRegion.srv" diff --git a/nav2_msgs/action/ComputePathThroughPoses.action b/nav2_msgs/action/ComputePathThroughPoses.action index c37297c69e..38c7f70a5d 100644 --- a/nav2_msgs/action/ComputePathThroughPoses.action +++ b/nav2_msgs/action/ComputePathThroughPoses.action @@ -1,15 +1,16 @@ # Error codes # Note: The expected priority order of the errors should match the message order -int16 NONE=0 -int16 UNKNOWN=1 -int16 TF_ERROR=2 -int16 START_OUTSIDE_MAP=3 -int16 GOAL_OUTSIDE_MAP=4 -int16 START_OCCUPIED=5 -int16 GOAL_OCCUPIED=6 -int16 TIMEOUT=7 -int16 NO_VALID_PATH=8 -int16 NO_VIAPOINTS_GIVEN=9 +uint16 NONE=0 +uint16 UNKNOWN=300 +uint16 INVALID_PLANNER=301 +uint16 TF_ERROR=3002 +uint16 START_OUTSIDE_MAP=303 +uint16 GOAL_OUTSIDE_MAP=304 +uint16 START_OCCUPIED=305 +uint16 GOAL_OCCUPIED=306 +uint16 TIMEOUT=3007 +uint16 NO_VALID_PATH=308 +uint16 NO_VIAPOINTS_GIVEN=309 #goal definition geometry_msgs/PoseStamped[] goals @@ -20,6 +21,6 @@ bool use_start # If false, use current robot pose as path start, if true, use st #result definition nav_msgs/Path path builtin_interfaces/Duration planning_time -int16 error_code +uint16 error_code --- #feedback definition diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index 7e727088a1..5abf2e6826 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -1,14 +1,15 @@ # Error codes # Note: The expected priority order of the errors should match the message order -int16 NONE=0 -int16 UNKNOWN=1 -int16 TF_ERROR=2 -int16 START_OUTSIDE_MAP=3 -int16 GOAL_OUTSIDE_MAP=4 -int16 START_OCCUPIED=5 -int16 GOAL_OCCUPIED=6 -int16 TIMEOUT=7 -int16 NO_VALID_PATH=8 +uint16 NONE=0 +uint16 UNKNOWN=200 +uint16 INVALID_PLANNER=201 +uint16 TF_ERROR=2002 +uint16 START_OUTSIDE_MAP=203 +uint16 GOAL_OUTSIDE_MAP=204 +uint16 START_OCCUPIED=205 +uint16 GOAL_OCCUPIED=206 +uint16 TIMEOUT=207 +uint16 NO_VALID_PATH=208 #goal definition geometry_msgs/PoseStamped goal @@ -19,6 +20,6 @@ bool use_start # If false, use current robot pose as path start, if true, use st #result definition nav_msgs/Path path builtin_interfaces/Duration planning_time -int16 error_code +uint16 error_code --- #feedback definition diff --git a/nav2_msgs/action/FollowPath.action b/nav2_msgs/action/FollowPath.action index 9ab82ff4d7..c06b918c54 100644 --- a/nav2_msgs/action/FollowPath.action +++ b/nav2_msgs/action/FollowPath.action @@ -1,12 +1,13 @@ # Error codes # Note: The expected priority order of the errors should match the message order -int16 NONE=0 -int16 UNKNOWN=1 -int16 TF_ERROR=2 -int16 INVALID_PATH=3 -int16 PATIENCE_EXCEEDED=4 -int16 FAILED_TO_MAKE_PROGRESS=5 -int16 NO_VALID_CONTROL=6 +uint16 NONE=0 +uint16 UNKNOWN=100 +uint16 INVALID_CONTROLLER=101 +uint16 TF_ERROR=102 +uint16 INVALID_PATH=103 +uint16 PATIENCE_EXCEEDED=104 +uint16 FAILED_TO_MAKE_PROGRESS=105 +uint16 NO_VALID_CONTROL=106 #goal definition nav_msgs/Path path @@ -15,7 +16,7 @@ string goal_checker_id --- #result definition std_msgs/Empty result -int16 error_code +uint16 error_code --- #feedback definition float32 distance_to_goal diff --git a/nav2_msgs/action/FollowWaypoints.action b/nav2_msgs/action/FollowWaypoints.action index c4b4764821..12f9a39760 100644 --- a/nav2_msgs/action/FollowWaypoints.action +++ b/nav2_msgs/action/FollowWaypoints.action @@ -1,8 +1,14 @@ +# Error codes +# Note: The expected priority order of the errors should match the message order +uint16 NONE=0 +uint16 UNKNOWN=600 +uint16 TASK_EXECUTOR_FAILED=601 + #goal definition geometry_msgs/PoseStamped[] poses --- #result definition -int32[] missed_waypoints +MissedWaypoint[] missed_waypoints --- #feedback definition uint32 current_waypoint diff --git a/nav2_msgs/action/NavigateThroughPoses.action b/nav2_msgs/action/NavigateThroughPoses.action index e2a57f25a9..ed1f8ca5f1 100644 --- a/nav2_msgs/action/NavigateThroughPoses.action +++ b/nav2_msgs/action/NavigateThroughPoses.action @@ -1,9 +1,14 @@ +# Error codes +# Note: The expected priority order of the errors should match the message order +uint16 NONE=0 + #goal definition geometry_msgs/PoseStamped[] poses string behavior_tree --- #result definition std_msgs/Empty result +uint16 error_code --- #feedback definition geometry_msgs/PoseStamped current_pose diff --git a/nav2_msgs/action/NavigateToPose.action b/nav2_msgs/action/NavigateToPose.action index b707792241..b38aa0002c 100644 --- a/nav2_msgs/action/NavigateToPose.action +++ b/nav2_msgs/action/NavigateToPose.action @@ -1,9 +1,14 @@ +# Error codes +# Note: The expected priority order of the errors should match the message order +uint16 NONE=0 + #goal definition geometry_msgs/PoseStamped pose string behavior_tree --- #result definition std_msgs/Empty result +uint16 error_code --- #feedback definition geometry_msgs/PoseStamped current_pose diff --git a/nav2_msgs/msg/MissedWaypoint.msg b/nav2_msgs/msg/MissedWaypoint.msg new file mode 100644 index 0000000000..44b2dc3b9c --- /dev/null +++ b/nav2_msgs/msg/MissedWaypoint.msg @@ -0,0 +1,3 @@ +uint32 index +geometry_msgs/PoseStamped goal +uint16 error_code diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index e597522430..f58bdac31f 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -381,7 +381,7 @@ void PlannerServer::computePlanThroughPoses() nav_msgs::msg::Path curr_path = getPlan(curr_start, curr_goal, goal->planner_id); if (!validatePath(curr_goal, curr_path, goal->planner_id)) { - throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + "generated a empty path"); + throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + " generated a empty path"); } // Concatenate paths together @@ -405,6 +405,10 @@ void PlannerServer::computePlanThroughPoses() } action_server_poses_->succeeded_current(result); + } catch (nav2_core::InvalidPlanner & ex) { + exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); + result->error_code = ActionToPoseGoal::INVALID_PLANNER; + action_server_poses_->terminate_current(result); } catch (nav2_core::StartOccupied & ex) { exceptionWarning(curr_start, curr_goal, goal->planner_id, ex); result->error_code = ActionThroughPosesGoal::START_OCCUPIED; @@ -480,7 +484,7 @@ PlannerServer::computePlan() result->path = getPlan(start, goal_pose, goal->planner_id); if (!validatePath(goal_pose, result->path, goal->planner_id)) { - throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + "generated a empty path"); + throw nav2_core::NoValidPathCouldBeFound(goal->planner_id + " generated a empty path"); } // Publish the plan for visualization purposes @@ -496,6 +500,10 @@ PlannerServer::computePlan() 1 / max_planner_duration_, 1 / cycle_duration.seconds()); } action_server_pose_->succeeded_current(result); + } catch (nav2_core::InvalidPlanner & ex) { + exceptionWarning(start, goal->goal, goal->planner_id, ex); + result->error_code = ActionToPoseGoal::INVALID_PLANNER; + action_server_pose_->terminate_current(result); } catch (nav2_core::StartOccupied & ex) { exceptionWarning(start, goal->goal, goal->planner_id, ex); result->error_code = ActionToPoseGoal::START_OCCUPIED; @@ -556,6 +564,7 @@ PlannerServer::getPlan( get_logger(), "planner %s is not a valid planner. " "Planner names are: %s", planner_id.c_str(), planner_ids_concat_.c_str()); + throw nav2_core::InvalidPlanner("Planner id " + planner_id + " is invalid"); } } diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index a844498ab8..5b00c6a49e 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -345,22 +345,28 @@ def _getPathImpl(self, start, goal, planner_id='', use_start=False): self.result_future = self.goal_handle.get_result_async() rclpy.spin_until_future_complete(self, self.result_future) self.status = self.result_future.result().status - if self.status != GoalStatus.STATUS_SUCCEEDED: - self.warn(f'Getting path failed with status code: {self.status}') - return None return self.result_future.result().result def getPath(self, start, goal, planner_id='', use_start=False): """Send a `ComputePathToPose` action request.""" rtn = self._getPathImpl(start, goal, planner_id='', use_start=False) + + if self.status != GoalStatus.STATUS_SUCCEEDED: + self.warn(f'Getting path failed with status code: {self.status}') + return None + if not rtn: return None else: return rtn.path - def getPathThroughPoses(self, start, goals, planner_id='', use_start=False): - """Send a `ComputePathThroughPoses` action request.""" + def _getPathThroughPosesImpl(self, start, goals, planner_id='', use_start=False): + """ + Send a `ComputePathThroughPoses` action request. + + Internal implementation to get the full result, not just the path. + """ self.debug("Waiting for 'ComputePathThroughPoses' action server") while not self.compute_path_through_poses_client.wait_for_server(timeout_sec=1.0): self.info("'ComputePathThroughPoses' action server not available, waiting...") @@ -383,11 +389,21 @@ def getPathThroughPoses(self, start, goals, planner_id='', use_start=False): self.result_future = self.goal_handle.get_result_async() rclpy.spin_until_future_complete(self, self.result_future) self.status = self.result_future.result().status + + return self.result_future.result().result + + def getPathThroughPoses(self, start, goals, planner_id='', use_start=False): + """Send a `ComputePathThroughPoses` action request.""" + rtn = self.__getPathThroughPosesImpl(start, goals, planner_id='', use_start=False) + if self.status != GoalStatus.STATUS_SUCCEEDED: self.warn(f'Getting path failed with status code: {self.status}') return None - return self.result_future.result().result.path + if not rtn: + return None + else: + return rtn.path def _smoothPathImpl(self, path, smoother_id='', max_duration=2.0, check_for_collision=False): """ diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 6d56df1ef7..558ae809bd 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -47,6 +47,40 @@ set(dependencies behaviortree_cpp_v3 ) +set(local_controller_plugin_lib local_controller) + +add_library(${local_controller_plugin_lib} SHARED src/error_codes/controller/controller_error_plugins.cpp) +ament_target_dependencies(${local_controller_plugin_lib} ${dependencies}) +target_compile_definitions(${local_controller_plugin_lib} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +pluginlib_export_plugin_description_file(nav2_core src/error_codes/controller_plugins.xml) + +install(TARGETS ${local_controller_plugin_lib} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(FILES src/error_codes/controller_plugins.xml + DESTINATION share/${PROJECT_NAME} +) + +set(global_planner_plugin_lib global_planner) + +add_library(${global_planner_plugin_lib} SHARED src/error_codes/planner/planner_error_plugin.cpp) +ament_target_dependencies(${global_planner_plugin_lib} ${dependencies}) +target_compile_definitions(${global_planner_plugin_lib} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +pluginlib_export_plugin_description_file(nav2_core src/error_codes/planner_plugins.xml) + +install(TARGETS ${global_planner_plugin_lib} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +install(FILES src/error_codes/planner_plugins.xml + DESTINATION share/${PROJECT_NAME} +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() @@ -67,8 +101,11 @@ if(BUILD_TESTING) add_subdirectory(src/behaviors/drive_on_heading) add_subdirectory(src/behaviors/assisted_teleop) add_subdirectory(src/costmap_filters) + add_subdirectory(src/error_codes) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) endif() +ament_export_libraries(${local_controller_plugin_lib}) +ament_export_libraries(${global_planner_plugin_lib}) ament_package() diff --git a/nav2_system_tests/package.xml b/nav2_system_tests/package.xml index 0940173e04..12f269a2d9 100644 --- a/nav2_system_tests/package.xml +++ b/nav2_system_tests/package.xml @@ -65,5 +65,7 @@ ament_cmake + + diff --git a/nav2_system_tests/src/error_codes/CMakeLists.txt b/nav2_system_tests/src/error_codes/CMakeLists.txt new file mode 100644 index 0000000000..be7052be16 --- /dev/null +++ b/nav2_system_tests/src/error_codes/CMakeLists.txt @@ -0,0 +1,12 @@ +ament_add_test(test_error_codes + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_error_codes_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} + TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml +) diff --git a/nav2_system_tests/src/error_codes/controller/controller_error_plugins.cpp b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.cpp new file mode 100644 index 0000000000..efcdac9c82 --- /dev/null +++ b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.cpp @@ -0,0 +1,25 @@ +// Copyright (c) 2022 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "controller_error_plugins.hpp" + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::UnknownErrorController, nav2_core::Controller) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::TFErrorController, nav2_core::Controller) +PLUGINLIB_EXPORT_CLASS( + nav2_system_tests::FailedToMakeProgressErrorController, + nav2_core::Controller) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::PatienceExceededErrorController, nav2_core::Controller) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::InvalidPathErrorController, nav2_core::Controller) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::NoValidControlErrorController, nav2_core::Controller) diff --git a/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp new file mode 100644 index 0000000000..ba194b195d --- /dev/null +++ b/nav2_system_tests/src/error_codes/controller/controller_error_plugins.hpp @@ -0,0 +1,114 @@ +// Copyright (c) 2022 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ERROR_CODES__CONTROLLER__CONTROLLER_ERROR_PLUGINS_HPP_ +#define ERROR_CODES__CONTROLLER__CONTROLLER_ERROR_PLUGINS_HPP_ + +#include +#include + +#include "nav2_core/controller.hpp" +#include "nav2_core/controller_exceptions.hpp" + +namespace nav2_system_tests +{ + +class UnknownErrorController : public nav2_core::Controller +{ +public: + UnknownErrorController() = default; + ~UnknownErrorController() = default; + + void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr &, + std::string, std::shared_ptr, + std::shared_ptr) override {} + + void cleanup() {} + + void activate() {} + + void deactivate() {} + + void setPlan(const nav_msgs::msg::Path &) {} + + virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::Twist &, + nav2_core::GoalChecker *) + { + throw nav2_core::ControllerException("Unknown Error"); + } + + void setSpeedLimit(const double &, const bool &) {} +}; + +class TFErrorController : public UnknownErrorController +{ + virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::Twist &, + nav2_core::GoalChecker *) + { + throw nav2_core::ControllerTFError("TF error"); + } +}; + +class FailedToMakeProgressErrorController : public UnknownErrorController +{ + virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::Twist &, + nav2_core::GoalChecker *) + { + throw nav2_core::FailedToMakeProgress("Failed to make progress"); + } +}; + +class PatienceExceededErrorController : public UnknownErrorController +{ + virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::Twist &, + nav2_core::GoalChecker *) + { + throw nav2_core::PatienceExceeded("Patience exceeded"); + } +}; + +class InvalidPathErrorController : public UnknownErrorController +{ + virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::Twist &, + nav2_core::GoalChecker *) + { + throw nav2_core::InvalidPath("Invalid path"); + } +}; + +class NoValidControlErrorController : public UnknownErrorController +{ + virtual geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::Twist &, + nav2_core::GoalChecker *) + { + throw nav2_core::NoValidControl("No valid control"); + } +}; + +} // namespace nav2_system_tests + +#endif // ERROR_CODES__CONTROLLER__CONTROLLER_ERROR_PLUGINS_HPP_ diff --git a/nav2_system_tests/src/error_codes/controller_plugins.xml b/nav2_system_tests/src/error_codes/controller_plugins.xml new file mode 100644 index 0000000000..c20fe0ab5e --- /dev/null +++ b/nav2_system_tests/src/error_codes/controller_plugins.xml @@ -0,0 +1,26 @@ + + + This local planner produces the general exception. + + + This local planner produces a tf exception. + + + This local planner produces a failed to make progress exception. + + + This local planner produces a start patience exceeded exception. + + + This local planner produces an invalid path exception. + + + This local planner produces a no valid control exception. + + diff --git a/nav2_system_tests/src/error_codes/error_code_param.yaml b/nav2_system_tests/src/error_codes/error_code_param.yaml new file mode 100644 index 0000000000..4c3e839520 --- /dev/null +++ b/nav2_system_tests/src/error_codes/error_code_param.yaml @@ -0,0 +1,175 @@ +controller_server: + ros__parameters: + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.2 + failure_tolerance: -0.1 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: [ "unknown", "tf_error", "invalid_path", "patience_exceeded","failed_to_make_progress", "no_valid_control"] + unknown: + plugin: "nav2_system_tests::UnknownErrorController" + tf_error: + plugin: "nav2_system_tests::TFErrorController" + invalid_path: + plugin: "nav2_system_tests::InvalidPathErrorController" + patience_exceeded: + plugin: "nav2_system_tests::PatienceExceededErrorController" + failed_to_make_progress: + plugin: "nav2_system_tests::FailedToMakeProgressErrorController" + no_valid_control: + plugin: "nav2_system_tests::NoValidControlErrorController" + + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["obstacle_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + planner_plugins: [ "unknown", "tf_error", "start_outside_map", "goal_outside_map", + "start_occupied", "goal_occupied", "timeout","no_valid_path", + "no_viapoints_given" ] + unknown: + plugin: "nav2_system_tests::UnknownErrorPlanner" + tf_error: + plugin: "nav2_system_tests::TFErrorPlanner" + start_outside_map: + plugin: "nav2_system_tests::StartOutsideMapErrorPlanner" + goal_outside_map: + plugin: "nav2_system_tests::GoalOutsideMapErrorPlanner" + start_occupied: + plugin: "nav2_system_tests::StartOccupiedErrorPlanner" + goal_occupied: + plugin: "nav2_system_tests::GoalOccupiedErrorPlanner" + timeout: + plugin: "nav2_system_tests::TimedOutErrorPlanner" + no_valid_path: + plugin: "nav2_system_tests::NoValidPathErrorPlanner" + no_viapoints_given: + plugin: "nav2_system_tests::NoViapointsGivenErrorPlanner" diff --git a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.cpp b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.cpp new file mode 100644 index 0000000000..90b18c5b11 --- /dev/null +++ b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.cpp @@ -0,0 +1,26 @@ +// Copyright (c) 2022 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "planner_error_plugin.hpp" + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::UnknownErrorPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::StartOccupiedErrorPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::GoalOccupiedErrorPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::StartOutsideMapErrorPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::GoalOutsideMapErrorPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::NoValidPathErrorPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::TimedOutErrorPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::TFErrorPlanner, nav2_core::GlobalPlanner) +PLUGINLIB_EXPORT_CLASS(nav2_system_tests::NoViapointsGivenErrorPlanner, nav2_core::GlobalPlanner) diff --git a/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp new file mode 100644 index 0000000000..2b6ae7971d --- /dev/null +++ b/nav2_system_tests/src/error_codes/planner/planner_error_plugin.hpp @@ -0,0 +1,144 @@ +// Copyright (c) 2022 Joshua Wallace +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ERROR_CODES__PLANNER__PLANNER_ERROR_PLUGIN_HPP_ +#define ERROR_CODES__PLANNER__PLANNER_ERROR_PLUGIN_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include "nav2_core/global_planner.hpp" +#include "nav_msgs/msg/path.hpp" +#include "nav2_util/robot_utils.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_core/planner_exceptions.hpp" + +namespace nav2_system_tests +{ + +class UnknownErrorPlanner : public nav2_core::GlobalPlanner +{ +public: + UnknownErrorPlanner() = default; + ~UnknownErrorPlanner() = default; + + void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr &, + std::string, std::shared_ptr, + std::shared_ptr) override {} + + void cleanup() override {} + + void activate() override {} + + void deactivate() override {} + + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &) override + { + throw nav2_core::PlannerException("Unknown Error"); + } +}; + +class StartOccupiedErrorPlanner : public UnknownErrorPlanner +{ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &) override + { + throw nav2_core::StartOccupied("Start Occupied"); + } +}; + +class GoalOccupiedErrorPlanner : public UnknownErrorPlanner +{ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &) override + { + throw nav2_core::GoalOccupied("Goal occupied"); + } +}; + +class StartOutsideMapErrorPlanner : public UnknownErrorPlanner +{ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &) override + { + throw nav2_core::StartOutsideMapBounds("Start OutsideMapBounds"); + } +}; + +class GoalOutsideMapErrorPlanner : public UnknownErrorPlanner +{ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &) override + { + throw nav2_core::GoalOutsideMapBounds("Goal outside map bounds"); + } +}; + +class NoValidPathErrorPlanner : public UnknownErrorPlanner +{ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &) override + { + return nav_msgs::msg::Path(); + } +}; + + +class TimedOutErrorPlanner : public UnknownErrorPlanner +{ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &) override + { + throw nav2_core::PlannerTimedOut("Planner Timed Out"); + } +}; + +class TFErrorPlanner : public UnknownErrorPlanner +{ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &) override + { + throw nav2_core::PlannerTFError("TF Error"); + } +}; + +class NoViapointsGivenErrorPlanner : public UnknownErrorPlanner +{ + nav_msgs::msg::Path createPlan( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &) override + { + throw nav2_core::NoViapointsGiven("No Via points given"); + } +}; + +} // namespace nav2_system_tests + + +#endif // ERROR_CODES__PLANNER__PLANNER_ERROR_PLUGIN_HPP_ diff --git a/nav2_system_tests/src/error_codes/planner_plugins.xml b/nav2_system_tests/src/error_codes/planner_plugins.xml new file mode 100644 index 0000000000..9d6f7f0545 --- /dev/null +++ b/nav2_system_tests/src/error_codes/planner_plugins.xml @@ -0,0 +1,42 @@ + + + This global planner produces a goal a timeout exception. + + + This global planner produces a start occupied exception. + + + This global planner produces a goal occupied exception. + + + This global planner produces a start outside map bounds exception. + + + This global planner produces a goal outside map bounds exception. + + + This global planner produces a no valid path exception. + + + This global planner produces a no via points given exception. + + + This global planner produces a timed out exception. + + + This global planner produces a planner tf error exception. + + + This global planner produces a no via points exception. + + diff --git a/nav2_system_tests/src/error_codes/test_error_codes.py b/nav2_system_tests/src/error_codes/test_error_codes.py new file mode 100755 index 0000000000..3079e3a3fc --- /dev/null +++ b/nav2_system_tests/src/error_codes/test_error_codes.py @@ -0,0 +1,130 @@ +#! /usr/bin/env python3 +# Copyright 2021 Samsung Research America +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import sys +import time + +from geometry_msgs.msg import PoseStamped +from nav_msgs.msg import Path +from nav2_simple_commander.robot_navigator import BasicNavigator + +import rclpy +from nav2_msgs.action import FollowPath, ComputePathToPose, ComputePathThroughPoses + + +def main(argv=sys.argv[1:]): + rclpy.init() + + navigator = BasicNavigator() + + # Set our demo's initial pose + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = navigator.get_clock().now().to_msg() + + # Initial pose does not respect the orientation... not sure why + initial_pose.pose.position.x = -2.00 + initial_pose.pose.position.y = -0.50 + initial_pose.pose.orientation.z = 0.0 + initial_pose.pose.orientation.w = 1.0 + + # Follow path error codes + path = Path() + + goal_pose = initial_pose + goal_pose.pose.position.x += 0.25 + + goal_pose1 = goal_pose + goal_pose1.pose.position.x += 0.25 + + path.poses.append(initial_pose) + path.poses.append(goal_pose) + path.poses.append(goal_pose1) + + follow_path = { + 'unknown': FollowPath.Goal().UNKNOWN, + 'invalid_controller': FollowPath.Goal().INVALID_CONTROLLER, + 'tf_error': FollowPath.Goal().TF_ERROR, + 'invalid_path': FollowPath.Goal().INVALID_PATH, + 'patience_exceeded': FollowPath.Goal().PATIENCE_EXCEEDED, + 'failed_to_make_progress': FollowPath.Goal().FAILED_TO_MAKE_PROGRESS, + 'no_valid_control': FollowPath.Goal().NO_VALID_CONTROL + } + + for controller, error_code in follow_path.items(): + success = navigator.followPath(path, controller) + + if success: + while not navigator.isTaskComplete(): + time.sleep(0.5) + + assert navigator.result_future.result().result.error_code == error_code, \ + "Follow path error code does not match" + + else: + assert False, "Follow path was rejected" + + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'map' + goal_pose.header.stamp = navigator.get_clock().now().to_msg() + + # Check compute path to pose error codes + goal_pose.pose.position.x = -1.00 + goal_pose.pose.position.y = -0.50 + goal_pose.pose.orientation.z = 0.0 + goal_pose.pose.orientation.w = 1.0 + + compute_path_to_pose = { + 'unknown': ComputePathToPose.Goal().UNKNOWN, + 'invalid_planner': ComputePathToPose.Goal().INVALID_PLANNER, + 'tf_error': ComputePathToPose.Goal().TF_ERROR, + 'start_outside_map': ComputePathToPose.Goal().START_OUTSIDE_MAP, + 'goal_outside_map': ComputePathToPose.Goal().GOAL_OUTSIDE_MAP, + 'start_occupied': ComputePathToPose.Goal().START_OCCUPIED, + 'goal_occupied': ComputePathToPose.Goal().GOAL_OCCUPIED, + 'timeout': ComputePathToPose.Goal().TIMEOUT, + 'no_valid_path': ComputePathToPose.Goal().NO_VALID_PATH} + + for planner, error_code in compute_path_to_pose.items(): + result = navigator._getPathImpl(initial_pose, goal_pose, planner) + assert result.error_code == error_code, "Compute path to pose error does not match" + + # Check compute path through error codes + goal_pose1 = goal_pose + goal_poses = [goal_pose, goal_pose1] + + compute_path_through_poses = { + 'unknown': ComputePathThroughPoses.Goal().UNKNOWN, + 'invalid_planner': ComputePathToPose.Goal().INVALID_PLANNER, + 'tf_error': ComputePathThroughPoses.Goal().TF_ERROR, + 'start_outside_map': ComputePathThroughPoses.Goal().START_OUTSIDE_MAP, + 'goal_outside_map': ComputePathThroughPoses.Goal().GOAL_OUTSIDE_MAP, + 'start_occupied': ComputePathThroughPoses.Goal().START_OCCUPIED, + 'goal_occupied': ComputePathThroughPoses.Goal().GOAL_OCCUPIED, + 'timeout': ComputePathThroughPoses.Goal().TIMEOUT, + 'no_valid_path': ComputePathThroughPoses.Goal().NO_VALID_PATH, + 'no_viapoints_given': ComputePathThroughPoses.Goal().NO_VIAPOINTS_GIVEN} + + for planner, error_code in compute_path_through_poses.items(): + result = navigator._getPathThroughPosesImpl(initial_pose, goal_poses, planner) + assert result.error_code == error_code, "Compute path through pose error does not match" + + navigator.lifecycleShutdown() + rclpy.shutdown() + exit(0) + + +if __name__ == '__main__': + main() diff --git a/nav2_system_tests/src/error_codes/test_error_codes_launch.py b/nav2_system_tests/src/error_codes/test_error_codes_launch.py new file mode 100755 index 0000000000..165e1806ce --- /dev/null +++ b/nav2_system_tests/src/error_codes/test_error_codes_launch.py @@ -0,0 +1,94 @@ +#! /usr/bin/env python3 +# Copyright (c) 2019 Samsung Research America +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import os +import sys + +from launch import LaunchDescription +from launch import LaunchService +from launch.actions import ExecuteProcess +from launch_testing.legacy import LaunchTestService +from launch_ros.actions import Node +from launch.actions import GroupAction + + +def generate_launch_description(): + params_file = os.path.join(os.getenv('TEST_DIR'), 'error_code_param.yaml') + + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + lifecycle_nodes = ['controller_server', + 'planner_server'] + + load_nodes = GroupAction( + actions=[ + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'map', 'odom']), + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'odom', 'base_link']), + Node( + package='nav2_controller', + executable='controller_server', + output='screen', + respawn_delay=2.0, + parameters=[params_file], + remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]), + Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + respawn_delay=2.0, + parameters=[params_file], + remappings=remappings), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_navigation', + output='screen', + parameters=[{'autostart': True}, + {'node_names': lifecycle_nodes}]) + ]) + + ld = LaunchDescription() + ld.add_action(load_nodes) + + return ld + + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + test_error_codes_action = ExecuteProcess( + cmd=[os.path.join(os.getenv('TEST_DIR'), 'test_error_codes.py')], + name='test_error_codes', + output='screen') + + lts = LaunchTestService() + lts.add_test_action(ld, test_error_codes_action) + ls = LaunchService(argv=argv) + ls.include_launch_description(ld) + return lts.run(ls) + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/nav2_system_tests/src/planning/test_planner_plugins.cpp b/nav2_system_tests/src/planning/test_planner_plugins.cpp index a56a7216b1..906e8f44d3 100644 --- a/nav2_system_tests/src/planning/test_planner_plugins.cpp +++ b/nav2_system_tests/src/planning/test_planner_plugins.cpp @@ -21,6 +21,7 @@ #include "planner_tester.hpp" #include "nav2_util/lifecycle_utils.hpp" #include "nav2_util/geometry_utils.hpp" +#include "nav2_core/planner_exceptions.hpp" using namespace std::chrono_literals; @@ -129,8 +130,12 @@ TEST(testPluginMap, Failures) auto path = obj->getPlan(start, goal, plugin_none); EXPECT_EQ(path.header.frame_id, std::string("map")); - path = obj->getPlan(start, goal, plugin_fake); - EXPECT_EQ(path.poses.size(), 0ul); + try { + path = obj->getPlan(start, goal, plugin_fake); + FAIL() << "Failed to throw invalid planner id exception"; + } catch (const nav2_core::InvalidPlanner & ex) { + EXPECT_EQ(ex.what(), std::string("Planner id fake is invalid")); + } obj->onCleanup(state); } diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index 2579513797..2e88a63a7a 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -18,7 +18,7 @@ from action_msgs.msg import GoalStatus from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped -from nav2_msgs.action import FollowWaypoints +from nav2_msgs.action import FollowWaypoints, ComputePathToPose from nav2_msgs.srv import ManageLifecycleNodes from rcl_interfaces.srv import SetParameters @@ -119,9 +119,9 @@ def run(self, block, cancel): if status != GoalStatus.STATUS_SUCCEEDED: self.info_msg(f'Goal failed with status code: {status}') return False - if len(result.missed_waypoints) > 0: + if len(self.action_result.missed_waypoints) > 0: self.info_msg('Goal failed to process all waypoints,' - ' missed {0} wps.'.format(len(result.missed_waypoints))) + ' missed {0} wps.'.format(len(self.action_result.missed_waypoints))) return False self.info_msg('Goal succeeded!') @@ -224,12 +224,14 @@ def main(argv=sys.argv[1:]): time.sleep(2) test.cancel_goal() - # a failure case + # set waypoint outside of map time.sleep(2) test.setWaypoints([[100.0, 100.0]]) result = test.run(True, False) assert not result result = not result + assert test.action_result.missed_waypoints[0].error_code == \ + ComputePathToPose.Goal().GOAL_OUTSIDE_MAP # stop on failure test with bogous waypoint test.setStopFailureParam(True) diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index b7cd82d684..e5c59aa20b 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -23,6 +23,7 @@ #include "nav2_util/lifecycle_node.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav2_msgs/action/follow_waypoints.hpp" +#include "nav2_msgs/msg/missed_waypoint.hpp" #include "nav_msgs/msg/path.hpp" #include "nav2_util/simple_action_server.hpp" #include "rclcpp_action/rclcpp_action.hpp" @@ -43,6 +44,12 @@ enum class ActionStatus SUCCEEDED = 3 }; +struct GoalStatus +{ + ActionStatus status; + int error_code; +}; + /** * @class nav2_waypoint_follower::WaypointFollower * @brief An action server that uses behavior tree for navigating a robot to its @@ -133,10 +140,10 @@ class WaypointFollower : public nav2_util::LifecycleNode rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_group_executor_; std::shared_future::SharedPtr> future_goal_handle_; + bool stop_on_failure_; - ActionStatus current_goal_status_; int loop_rate_; - std::vector failed_ids_; + GoalStatus current_goal_status_; // Task Execution At Waypoint Plugin pluginlib::ClassLoader diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index a04c404c77..2bb95fea7d 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -205,30 +205,33 @@ WaypointFollower::followWaypoints() std::bind(&WaypointFollower::goalResponseCallback, this, std::placeholders::_1); future_goal_handle_ = nav_to_pose_client_->async_send_goal(client_goal, send_goal_options); - current_goal_status_ = ActionStatus::PROCESSING; + current_goal_status_.status = ActionStatus::PROCESSING; } feedback->current_waypoint = goal_index; action_server_->publish_feedback(feedback); - if (current_goal_status_ == ActionStatus::FAILED) { - failed_ids_.push_back(goal_index); + if (current_goal_status_.status == ActionStatus::FAILED) { + nav2_msgs::msg::MissedWaypoint missedWaypoint; + missedWaypoint.index = goal_index; + missedWaypoint.goal = goal->poses[goal_index]; + missedWaypoint.error_code = current_goal_status_.error_code; + result->missed_waypoints.push_back(missedWaypoint); if (stop_on_failure_) { RCLCPP_WARN( get_logger(), "Failed to process waypoint %i in waypoint " "list and stop on failure is enabled." " Terminating action.", goal_index); - result->missed_waypoints = failed_ids_; action_server_->terminate_current(result); - failed_ids_.clear(); + current_goal_status_.error_code = 0; return; } else { RCLCPP_INFO( get_logger(), "Failed to process waypoint %i," " moving to next.", goal_index); } - } else if (current_goal_status_ == ActionStatus::SUCCEEDED) { + } else if (current_goal_status_.status == ActionStatus::SUCCEEDED) { RCLCPP_INFO( get_logger(), "Succeeded processing waypoint %i, processing waypoint task execution", goal_index); @@ -237,16 +240,23 @@ WaypointFollower::followWaypoints() RCLCPP_INFO( get_logger(), "Task execution at waypoint %i %s", goal_index, is_task_executed ? "succeeded" : "failed!"); + + if (!is_task_executed) { + nav2_msgs::msg::MissedWaypoint missedWaypoint; + missedWaypoint.index = goal_index; + missedWaypoint.goal = goal->poses[goal_index]; + missedWaypoint.error_code = nav2_msgs::action::FollowWaypoints::Goal::TASK_EXECUTOR_FAILED; + result->missed_waypoints.push_back(missedWaypoint); + } // if task execution was failed and stop_on_failure_ is on , terminate action if (!is_task_executed && stop_on_failure_) { - failed_ids_.push_back(goal_index); RCLCPP_WARN( get_logger(), "Failed to execute task at waypoint %i " " stop on failure is enabled." " Terminating action.", goal_index); - result->missed_waypoints = failed_ids_; + action_server_->terminate_current(result); - failed_ids_.clear(); + current_goal_status_.error_code = 0; return; } else { RCLCPP_INFO( @@ -255,8 +265,8 @@ WaypointFollower::followWaypoints() } } - if (current_goal_status_ != ActionStatus::PROCESSING && - current_goal_status_ != ActionStatus::UNKNOWN) + if (current_goal_status_.status != ActionStatus::PROCESSING && + current_goal_status_.status != ActionStatus::UNKNOWN) { // Update server state goal_index++; @@ -265,9 +275,8 @@ WaypointFollower::followWaypoints() RCLCPP_INFO( get_logger(), "Completed all %zu waypoints requested.", goal->poses.size()); - result->missed_waypoints = failed_ids_; action_server_->succeeded_current(result); - failed_ids_.clear(); + current_goal_status_.error_code = 0; return; } } else { @@ -296,16 +305,17 @@ WaypointFollower::resultCallback( switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: - current_goal_status_ = ActionStatus::SUCCEEDED; + current_goal_status_.status = ActionStatus::SUCCEEDED; return; case rclcpp_action::ResultCode::ABORTED: - current_goal_status_ = ActionStatus::FAILED; + current_goal_status_.status = ActionStatus::FAILED; + current_goal_status_.error_code = result.result->error_code; return; case rclcpp_action::ResultCode::CANCELED: - current_goal_status_ = ActionStatus::FAILED; + current_goal_status_.status = ActionStatus::FAILED; return; default: - current_goal_status_ = ActionStatus::UNKNOWN; + current_goal_status_.status = ActionStatus::UNKNOWN; return; } } @@ -318,7 +328,7 @@ WaypointFollower::goalResponseCallback( RCLCPP_ERROR( get_logger(), "navigate_to_pose action client failed to send goal to server."); - current_goal_status_ = ActionStatus::FAILED; + current_goal_status_.status = ActionStatus::FAILED; } } From ba3d647e9276cbd96332ebe1c2c6cbebedd124bf Mon Sep 17 00:00:00 2001 From: MartiBolet <43337758+MartiBolet@users.noreply.github.com> Date: Sun, 20 Nov 2022 09:05:06 +0100 Subject: [PATCH 35/43] Solve bug when CostmapInfoServer is reactivated (#3292) * Solve bug when CostmapInfoServer is reactivated --- .../costmap_filter_info_server.hpp | 2 +- .../costmap_filter_info_server.cpp | 17 +++++----- .../unit/test_costmap_filter_info_server.cpp | 31 +++++++++++++++++++ 3 files changed, 41 insertions(+), 9 deletions(-) diff --git a/nav2_map_server/include/nav2_map_server/costmap_filter_info_server.hpp b/nav2_map_server/include/nav2_map_server/costmap_filter_info_server.hpp index 99311deff7..14a69ad229 100644 --- a/nav2_map_server/include/nav2_map_server/costmap_filter_info_server.hpp +++ b/nav2_map_server/include/nav2_map_server/costmap_filter_info_server.hpp @@ -71,7 +71,7 @@ class CostmapFilterInfoServer : public nav2_util::LifecycleNode private: rclcpp_lifecycle::LifecyclePublisher::SharedPtr publisher_; - std::unique_ptr msg_; + nav2_msgs::msg::CostmapFilterInfo msg_; }; // CostmapFilterInfoServer } // namespace nav2_map_server diff --git a/nav2_map_server/src/costmap_filter_info/costmap_filter_info_server.cpp b/nav2_map_server/src/costmap_filter_info/costmap_filter_info_server.cpp index 7b1c618315..f6671b0291 100644 --- a/nav2_map_server/src/costmap_filter_info/costmap_filter_info_server.cpp +++ b/nav2_map_server/src/costmap_filter_info/costmap_filter_info_server.cpp @@ -48,13 +48,13 @@ CostmapFilterInfoServer::on_configure(const rclcpp_lifecycle::State & /*state*/) publisher_ = this->create_publisher( filter_info_topic, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); - msg_ = std::make_unique(); - msg_->header.frame_id = ""; - msg_->header.stamp = now(); - msg_->type = get_parameter("type").as_int(); - msg_->filter_mask_topic = get_parameter("mask_topic").as_string(); - msg_->base = static_cast(get_parameter("base").as_double()); - msg_->multiplier = static_cast(get_parameter("multiplier").as_double()); + msg_ = nav2_msgs::msg::CostmapFilterInfo(); + msg_.header.frame_id = ""; + msg_.header.stamp = now(); + msg_.type = get_parameter("type").as_int(); + msg_.filter_mask_topic = get_parameter("mask_topic").as_string(); + msg_.base = static_cast(get_parameter("base").as_double()); + msg_.multiplier = static_cast(get_parameter("multiplier").as_double()); return nav2_util::CallbackReturn::SUCCESS; } @@ -65,7 +65,8 @@ CostmapFilterInfoServer::on_activate(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Activating"); publisher_->on_activate(); - publisher_->publish(std::move(msg_)); + auto costmap_filter_info = std::make_unique(msg_); + publisher_->publish(std::move(costmap_filter_info)); // create bond connection createBond(); diff --git a/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp b/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp index 168fbc9f7b..072aaf8d1b 100644 --- a/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp +++ b/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp @@ -59,6 +59,16 @@ class InfoServerWrapper : public nav2_map_server::CostmapFilterInfoServer on_cleanup(get_current_state()); on_shutdown(get_current_state()); } + + void deactivate() + { + on_deactivate(get_current_state()); + } + + void activate() + { + on_activate(get_current_state()); + } }; class InfoServerTester : public ::testing::Test @@ -144,3 +154,24 @@ TEST_F(InfoServerTester, testCostmapFilterInfoPublish) EXPECT_NEAR(info_->base, BASE, EPSILON); EXPECT_NEAR(info_->multiplier, MULTIPLIER, EPSILON); } + +TEST_F(InfoServerTester, testCostmapFilterInfoDeactivateActivate) +{ + info_server_->deactivate(); + info_ = nullptr; + info_server_->activate(); + + rclcpp::Time start_time = info_server_->now(); + while (!isReceived()) { + rclcpp::spin_some(info_server_->get_node_base_interface()); + std::this_thread::sleep_for(100ms); + // Waiting no more than 5 seconds + ASSERT_TRUE((info_server_->now() - start_time) <= rclcpp::Duration(5000ms)); + } + + // Checking received CostmapFilterInfo for consistency + EXPECT_EQ(info_->type, TYPE); + EXPECT_EQ(info_->filter_mask_topic, MASK_TOPIC); + EXPECT_NEAR(info_->base, BASE, EPSILON); + EXPECT_NEAR(info_->multiplier, MULTIPLIER, EPSILON); +} From 76314ac485376e4669324de53169d3fb55bf06a0 Mon Sep 17 00:00:00 2001 From: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com> Date: Sun, 20 Nov 2022 21:59:00 +0300 Subject: [PATCH 36/43] Smoothness metrics update (#3294) * Update metrics for path smoothness * Support Savitzky-Golay smoother --- tools/smoother_benchmarking/README.md | 4 +++- tools/smoother_benchmarking/metrics.py | 2 +- tools/smoother_benchmarking/process_data.py | 2 +- 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/tools/smoother_benchmarking/README.md b/tools/smoother_benchmarking/README.md index 65ea8a4c9d..aa9a5c83fe 100644 --- a/tools/smoother_benchmarking/README.md +++ b/tools/smoother_benchmarking/README.md @@ -30,12 +30,14 @@ planner_server: ``` smoother_server: ros__parameters: - smoother_plugins: ["simple_smoother", "constrained_smoother"] + smoother_plugins: ["simple_smoother", "constrained_smoother", "sg_smoother"] simple_smoother: plugin: "nav2_smoother::SimpleSmoother" constrained_smoother: plugin: "nav2_constrained_smoother/ConstrainedSmoother" w_smooth: 100000.0 # tuned + sg_smoother: + plugin: "nav2_smoother::SavitzkyGolaySmoother" ``` Set global costmap, path planner and smoothers parameters to those desired in `nav2_params.yaml`. diff --git a/tools/smoother_benchmarking/metrics.py b/tools/smoother_benchmarking/metrics.py index 6b576b08a9..9636eee11f 100644 --- a/tools/smoother_benchmarking/metrics.py +++ b/tools/smoother_benchmarking/metrics.py @@ -111,7 +111,7 @@ def main(): costmap.resize(costmap_msg.metadata.size_y, costmap_msg.metadata.size_x) planner = 'SmacHybrid' - smoothers = ['simple_smoother', 'constrained_smoother'] + smoothers = ['simple_smoother', 'constrained_smoother', 'sg_smoother'] max_cost = 210 side_buffer = 10 time_stamp = navigator.get_clock().now().to_msg() diff --git a/tools/smoother_benchmarking/process_data.py b/tools/smoother_benchmarking/process_data.py index edc0dad811..43392833bd 100644 --- a/tools/smoother_benchmarking/process_data.py +++ b/tools/smoother_benchmarking/process_data.py @@ -82,7 +82,7 @@ def getSmoothness(pt_prev, pt, pt_next): d1 = pt - pt_prev d2 = pt_next - pt delta = d2 - d1 - return np.dot(delta, delta) + return np.linalg.norm(delta) def getPathSmoothnesses(paths): smoothnesses = [] From a7a77fc1d1962fbaea37f75272b94abeb5f99c70 Mon Sep 17 00:00:00 2001 From: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com> Date: Tue, 29 Nov 2022 20:31:48 +0100 Subject: [PATCH 37/43] preempt/cancel test for time behavior, spin pluguin (#3301) * include preempt/cancel test for time behavior, spin pluguin * linting * fix bug in code --- .../behaviors/spin/spin_behavior_tester.cpp | 15 ++++++-- .../behaviors/spin/spin_behavior_tester.hpp | 4 ++- .../spin/test_spin_behavior_node.cpp | 34 +++++++++++++++++++ 3 files changed, 50 insertions(+), 3 deletions(-) diff --git a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp index 557f5b7959..27b1f5e800 100644 --- a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp +++ b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp @@ -123,7 +123,9 @@ void SpinBehaviorTester::deactivate() bool SpinBehaviorTester::defaultSpinBehaviorTest( const float target_yaw, - const double tolerance) + const double tolerance, + const bool nonblocking_action, + const bool cancel_action) { if (!is_active_) { RCLCPP_ERROR(node_->get_logger(), "Not activated"); @@ -181,6 +183,16 @@ bool SpinBehaviorTester::defaultSpinBehaviorTest( return false; } + if (!nonblocking_action) { + return true; + } + if (cancel_action) { + sleep(2); + // cancel the goal + auto cancel_response = client_ptr_->async_cancel_goal(goal_handle_future.get()); + rclcpp::spin_until_future_complete(node_, cancel_response); + } + // Wait for the server to be done with the goal auto result_future = client_ptr_->async_get_result(goal_handle); @@ -347,5 +359,4 @@ void SpinBehaviorTester::amclPoseCallback( { initial_pose_received_ = true; } - } // namespace nav2_system_tests diff --git a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp index 745cfe4212..c7fb6f719d 100644 --- a/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp +++ b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp @@ -57,7 +57,9 @@ class SpinBehaviorTester // Runs a single test with given target yaw bool defaultSpinBehaviorTest( float target_yaw, - double tolerance = 0.1); + double tolerance = 0.1, + bool nonblocking_action = true, + bool cancel_action = false); void activate(); diff --git a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp index 150a6599fa..a1efed8cd9 100644 --- a/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp @@ -80,6 +80,40 @@ TEST_P(SpinBehaviorTestFixture, testSpinRecovery) } } +TEST_F(SpinBehaviorTestFixture, testSpinPreemption) +{ + // Goal + float target_yaw = 3.0 * M_PIf32; + float tolerance = 0.1; + bool nonblocking_action = false; + bool success = false; + + // Send the first goal + success = spin_recovery_tester->defaultSpinBehaviorTest( + target_yaw, tolerance, + nonblocking_action); + EXPECT_EQ(true, success); + // Preempt goal + sleep(2); + success = false; + float prempt_target_yaw = 4.0 * M_PIf32; + float preempt_tolerance = 0.1; + success = spin_recovery_tester->defaultSpinBehaviorTest(prempt_target_yaw, preempt_tolerance); + EXPECT_EQ(false, success); +} + +TEST_F(SpinBehaviorTestFixture, testSpinCancel) +{ + // Goal + float target_yaw = 4.0 * M_PIf32; + float tolerance = 0.1; + bool nonblocking_action = true, cancel_action = true, success = false; + success = spin_recovery_tester->defaultSpinBehaviorTest( + target_yaw, tolerance, + nonblocking_action, cancel_action); + EXPECT_EQ(false, success); +} + INSTANTIATE_TEST_SUITE_P( SpinRecoveryTests, SpinBehaviorTestFixture, From 31522144c24d0222a0e9c78bf9f5e1ecc7c88c13 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 24 Oct 2022 14:23:45 -0400 Subject: [PATCH 38/43] lint fix --- nav2_costmap_2d/src/costmap_2d_ros.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index d5e8eef093..232cad8ae7 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -219,7 +219,8 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) std::make_unique( shared_from_this(), costmap_layer.get(), global_frame_, - layer->getName(), always_send_full_costmap_)); + layer->getName(), always_send_full_costmap_) + ); } } From 83376aac34c103fd6b182bc0d0829effc3728360 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 2 Dec 2022 13:45:09 -0500 Subject: [PATCH 39/43] clean up test --- nav2_costmap_2d/src/costmap_2d_ros.cpp | 13 +++---- .../integration/test_costmap_2d_publisher.cpp | 35 ++++--------------- 2 files changed, 14 insertions(+), 34 deletions(-) diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 232cad8ae7..efeb0e86e7 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -320,6 +320,13 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); + costmap_publisher_.reset(); + clear_costmap_service_.reset(); + + for (auto & layer_pub : layer_publishers_) { + layer_pub.reset(); + } + layered_costmap_.reset(); tf_listener_.reset(); @@ -328,12 +335,6 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) footprint_sub_.reset(); footprint_pub_.reset(); - costmap_publisher_.reset(); - clear_costmap_service_.reset(); - - for (auto & layer_pub : layer_publishers_) { - layer_pub.reset(); - } executor_thread_.reset(); return nav2_util::CallbackReturn::SUCCESS; diff --git a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp index 8cb7e66240..ecbd56315a 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp @@ -17,7 +17,6 @@ #include #include "nav2_costmap_2d/costmap_2d.hpp" -#include "nav2_costmap_2d/costmap_2d_publisher.hpp" #include "nav2_costmap_2d/costmap_subscriber.hpp" #include "nav2_costmap_2d/cost_values.hpp" #include "tf2_ros/transform_listener.h" @@ -34,33 +33,22 @@ RclCppFixture g_rclcppfixture; class TestCostmap2dPublisher : public nav2_util::LifecycleNode { public: - explicit TestCostmap2dPublisher(std::string name) + explicit TestCostmap2dPublisher(const std::string& name) : LifecycleNode(name) { RCLCPP_INFO(get_logger(), "Constructing"); } - ~TestCostmap2dPublisher() {} + ~TestCostmap2dPublisher() override = default; nav2_util::CallbackReturn - on_configure(const rclcpp_lifecycle::State &) + on_configure(const rclcpp_lifecycle::State &) override { RCLCPP_INFO(get_logger(), "Configuring"); callback_group_ = create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); - costmap_ = std::make_shared( - 100, 100, 0.05, 0, 0, - nav2_costmap_2d::LETHAL_OBSTACLE); - - costmap_pub_ = std::make_shared( - shared_from_this(), - costmap_.get(), - "map", - "dummy_costmap", - true); - rclcpp::SubscriptionOptions sub_option; sub_option.callback_group = callback_group_; @@ -88,16 +76,15 @@ class TestCostmap2dPublisher : public nav2_util::LifecycleNode } nav2_util::CallbackReturn - on_activate(const rclcpp_lifecycle::State &) + on_activate(const rclcpp_lifecycle::State &) override { RCLCPP_INFO(get_logger(), "Activating"); - costmap_pub_->on_activate(); costmap_ros_->activate(); return nav2_util::CallbackReturn::SUCCESS; } nav2_util::CallbackReturn - on_deactivate(const rclcpp_lifecycle::State &) + on_deactivate(const rclcpp_lifecycle::State &) override { RCLCPP_INFO(get_logger(), "Deactivating"); costmap_ros_->deactivate(); @@ -105,7 +92,7 @@ class TestCostmap2dPublisher : public nav2_util::LifecycleNode } nav2_util::CallbackReturn - on_cleanup(const rclcpp_lifecycle::State &) + on_cleanup(const rclcpp_lifecycle::State &) override { executor_thread_.reset(); costmap_thread_.reset(); @@ -113,19 +100,12 @@ class TestCostmap2dPublisher : public nav2_util::LifecycleNode return nav2_util::CallbackReturn::SUCCESS; } - void publishCostmap() - { - costmap_pub_->publishCostmap(); - } - void costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr costmap) { promise_.set_value(costmap); } - std::shared_ptr costmap_pub_; rclcpp::Subscription::SharedPtr costmap_sub_; - std::shared_ptr costmap_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; @@ -146,7 +126,7 @@ class TestNode : public ::testing::Test costmap_publisher_->on_activate(costmap_publisher_->get_current_state()); } - ~TestNode() + ~TestNode() override { costmap_publisher_->on_deactivate(costmap_publisher_->get_current_state()); costmap_publisher_->on_cleanup(costmap_publisher_->get_current_state()); @@ -158,7 +138,6 @@ class TestNode : public ::testing::Test TEST_F(TestNode, costmap_pub_test) { - costmap_publisher_->publishCostmap(); auto future = costmap_publisher_->promise_.get_future(); auto status = future.wait_for(std::chrono::seconds(5)); EXPECT_TRUE(status == std::future_status::ready); From cdaa851300db5371eaa1e8c9875a4d2cb75d88bb Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Fri, 2 Dec 2022 13:46:09 -0500 Subject: [PATCH 40/43] lint --- nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp index ecbd56315a..46842b41c6 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp @@ -33,7 +33,7 @@ RclCppFixture g_rclcppfixture; class TestCostmap2dPublisher : public nav2_util::LifecycleNode { public: - explicit TestCostmap2dPublisher(const std::string& name) + explicit TestCostmap2dPublisher(const std::string & name) : LifecycleNode(name) { RCLCPP_INFO(get_logger(), "Constructing"); From 0cad73115c3115ce3b1cef34c9c8a6f8be4bb242 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 7 Dec 2022 16:46:24 -0500 Subject: [PATCH 41/43] cleaned up test --- nav2_costmap_2d/test/CMakeLists.txt | 4 +- .../test/integration/CMakeLists.txt | 228 +++++++++--------- .../integration/test_costmap_2d_publisher.cpp | 131 ++++++---- 3 files changed, 195 insertions(+), 168 deletions(-) diff --git a/nav2_costmap_2d/test/CMakeLists.txt b/nav2_costmap_2d/test/CMakeLists.txt index ad7905ad1f..76481e883b 100644 --- a/nav2_costmap_2d/test/CMakeLists.txt +++ b/nav2_costmap_2d/test/CMakeLists.txt @@ -1,6 +1,6 @@ set(TEST_MAP_DIR ${CMAKE_CURRENT_SOURCE_DIR}/map) set(TEST_LAUNCH_DIR ${CMAKE_CURRENT_SOURCE_DIR}/test_launch_files) -add_subdirectory(unit) +#add_subdirectory(unit) add_subdirectory(integration) -add_subdirectory(regression) +#add_subdirectory(regression) diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index 0768972a0b..1070064533 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -1,68 +1,68 @@ -ament_add_gtest_executable(footprint_tests_exec - footprint_tests.cpp -) -ament_target_dependencies(footprint_tests_exec - ${dependencies} -) -target_link_libraries(footprint_tests_exec - nav2_costmap_2d_core - layers -) - -ament_add_gtest_executable(test_collision_checker_exec - test_costmap_topic_collision_checker.cpp -) -ament_target_dependencies(test_collision_checker_exec - ${dependencies} -) -target_link_libraries(test_collision_checker_exec - nav2_costmap_2d_core - nav2_costmap_2d_client - layers -) - -ament_add_gtest_executable(inflation_tests_exec - inflation_tests.cpp -) -ament_target_dependencies(inflation_tests_exec - ${dependencies} -) -target_link_libraries(inflation_tests_exec - nav2_costmap_2d_core - layers -) - -ament_add_gtest_executable(obstacle_tests_exec - obstacle_tests.cpp -) -ament_target_dependencies(obstacle_tests_exec - ${dependencies} -) -target_link_libraries(obstacle_tests_exec - nav2_costmap_2d_core - layers -) - -ament_add_gtest_executable(range_tests_exec - range_tests.cpp -) -ament_target_dependencies(range_tests_exec - ${dependencies} -) -target_link_libraries(range_tests_exec - nav2_costmap_2d_core - layers -) - -ament_add_gtest(dyn_params_tests - dyn_params_tests.cpp -) -ament_target_dependencies(dyn_params_tests - ${dependencies} -) -target_link_libraries(dyn_params_tests - nav2_costmap_2d_core -) +#ament_add_gtest_executable(footprint_tests_exec +# footprint_tests.cpp +#) +#ament_target_dependencies(footprint_tests_exec +# ${dependencies} +#) +#target_link_libraries(footprint_tests_exec +# nav2_costmap_2d_core +# layers +#) +# +#ament_add_gtest_executable(test_collision_checker_exec +# test_costmap_topic_collision_checker.cpp +#) +#ament_target_dependencies(test_collision_checker_exec +# ${dependencies} +#) +#target_link_libraries(test_collision_checker_exec +# nav2_costmap_2d_core +# nav2_costmap_2d_client +# layers +#) +# +#ament_add_gtest_executable(inflation_tests_exec +# inflation_tests.cpp +#) +#ament_target_dependencies(inflation_tests_exec +# ${dependencies} +#) +#target_link_libraries(inflation_tests_exec +# nav2_costmap_2d_core +# layers +#) +# +#ament_add_gtest_executable(obstacle_tests_exec +# obstacle_tests.cpp +#) +#ament_target_dependencies(obstacle_tests_exec +# ${dependencies} +#) +#target_link_libraries(obstacle_tests_exec +# nav2_costmap_2d_core +# layers +#) +# +#ament_add_gtest_executable(range_tests_exec +# range_tests.cpp +#) +#ament_target_dependencies(range_tests_exec +# ${dependencies} +#) +#target_link_libraries(range_tests_exec +# nav2_costmap_2d_core +# layers +#) +# +#ament_add_gtest(dyn_params_tests +# dyn_params_tests.cpp +#) +#ament_target_dependencies(dyn_params_tests +# ${dependencies} +#) +#target_link_libraries(dyn_params_tests +# nav2_costmap_2d_core +#) ament_add_gtest_executable(test_costmap_publisher_exec test_costmap_2d_publisher.cpp @@ -76,55 +76,55 @@ target_link_libraries(test_costmap_publisher_exec layers ) -ament_add_test(test_collision_checker - GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - ENV - TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml - TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} - TEST_EXECUTABLE=$ -) - -ament_add_test(footprint_tests - GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - ENV - TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml - TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} - TEST_EXECUTABLE=$ -) - -ament_add_test(inflation_tests - GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - ENV - TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml - TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} - TEST_EXECUTABLE=$ -) - -ament_add_test(obstacle_tests - GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - ENV - TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml - TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} - TEST_EXECUTABLE=$ -) - -ament_add_test(range_tests - GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - ENV - TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml - TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} - TEST_EXECUTABLE=$ -) +#ament_add_test(test_collision_checker +# GENERATE_RESULT_FOR_RETURN_CODE_ZERO +# COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" +# WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" +# ENV +# TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml +# TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} +# TEST_EXECUTABLE=$ +#) +# +#ament_add_test(footprint_tests +# GENERATE_RESULT_FOR_RETURN_CODE_ZERO +# COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" +# WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" +# ENV +# TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml +# TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} +# TEST_EXECUTABLE=$ +#) +# +#ament_add_test(inflation_tests +# GENERATE_RESULT_FOR_RETURN_CODE_ZERO +# COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" +# WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" +# ENV +# TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml +# TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} +# TEST_EXECUTABLE=$ +#) +# +#ament_add_test(obstacle_tests +# GENERATE_RESULT_FOR_RETURN_CODE_ZERO +# COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" +# WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" +# ENV +# TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml +# TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} +# TEST_EXECUTABLE=$ +#) +# +#ament_add_test(range_tests +# GENERATE_RESULT_FOR_RETURN_CODE_ZERO +# COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" +# WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" +# ENV +# TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml +# TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} +# TEST_EXECUTABLE=$ +#) ament_add_test(test_costmap_publisher_exec GENERATE_RESULT_FOR_RETURN_CODE_ZERO diff --git a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp index 46842b41c6..1e678d67c7 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp @@ -30,43 +30,22 @@ class RclCppFixture }; RclCppFixture g_rclcppfixture; -class TestCostmap2dPublisher : public nav2_util::LifecycleNode +class CostmapRosLifecycleNode : public nav2_util::LifecycleNode { public: - explicit TestCostmap2dPublisher(const std::string & name) - : LifecycleNode(name) - { - RCLCPP_INFO(get_logger(), "Constructing"); - } + explicit CostmapRosLifecycleNode(const std::string & name) + : LifecycleNode(name), + name_(name) {} - ~TestCostmap2dPublisher() override = default; + ~CostmapRosLifecycleNode() override = default; nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State &) override { - RCLCPP_INFO(get_logger(), "Configuring"); - - callback_group_ = create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive, false); - - rclcpp::SubscriptionOptions sub_option; - sub_option.callback_group = callback_group_; - - std::string topic_name = "/dummy_costmap/static_layer_raw"; - costmap_sub_ = this->create_subscription( - topic_name, - rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), - std::bind(&TestCostmap2dPublisher::costmapCallback, this, std::placeholders::_1), - sub_option); - - executor_ = std::make_shared(); - executor_->add_callback_group(callback_group_, get_node_base_interface()); - executor_thread_ = std::make_unique(executor_); - costmap_ros_ = std::make_shared( - "dummy_costmap", + name_, std::string{get_namespace()}, - "dummy_costmap", + name_, get_parameter("use_sim_time").as_bool()); costmap_thread_ = std::make_unique(costmap_ros_); @@ -78,7 +57,6 @@ class TestCostmap2dPublisher : public nav2_util::LifecycleNode nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State &) override { - RCLCPP_INFO(get_logger(), "Activating"); costmap_ros_->activate(); return nav2_util::CallbackReturn::SUCCESS; } @@ -86,7 +64,6 @@ class TestCostmap2dPublisher : public nav2_util::LifecycleNode nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override { - RCLCPP_INFO(get_logger(), "Deactivating"); costmap_ros_->deactivate(); return nav2_util::CallbackReturn::SUCCESS; } @@ -94,58 +71,108 @@ class TestCostmap2dPublisher : public nav2_util::LifecycleNode nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override { - executor_thread_.reset(); costmap_thread_.reset(); costmap_ros_->deactivate(); return nav2_util::CallbackReturn::SUCCESS; } - void costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr costmap) +protected: + std::shared_ptr costmap_ros_; + std::unique_ptr costmap_thread_; + const std::string name_; +}; + +class LayerSubscriber +{ +public: + explicit LayerSubscriber(const nav2_util::LifecycleNode::WeakPtr & parent) { - promise_.set_value(costmap); + auto node = parent.lock(); + + callback_group_ = node->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + + rclcpp::SubscriptionOptions sub_option; + sub_option.callback_group = callback_group_; + + std::string topic_name = "/fake_costmap/static_layer_raw"; + layer_sub_ = node->create_subscription( + topic_name, + rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + std::bind(&LayerSubscriber::layerCallback, this, std::placeholders::_1), + sub_option); + + executor_ = std::make_shared(); + executor_->add_callback_group(callback_group_, node->get_node_base_interface()); + executor_thread_ = std::make_unique(executor_); + } + + ~LayerSubscriber() + { + executor_thread_.reset(); } - rclcpp::Subscription::SharedPtr costmap_sub_; + std::promise layer_promise_; + +protected: + void layerCallback(const nav2_msgs::msg::Costmap::SharedPtr layer) + { + if (!callback_hit_) { + layer_promise_.set_value(layer); + callback_hit_ = true; + } + } + rclcpp::Subscription::SharedPtr layer_sub_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; std::unique_ptr executor_thread_; - std::promise promise_; - - std::shared_ptr costmap_ros_; - std::unique_ptr costmap_thread_; + bool callback_hit_{false}; }; -class TestNode : public ::testing::Test +class CostmapRosTestFixture : public ::testing::Test { public: - TestNode() + CostmapRosTestFixture() { - costmap_publisher_ = std::make_shared("test_costmap_publisher"); - costmap_publisher_->on_configure(costmap_publisher_->get_current_state()); - costmap_publisher_->on_activate(costmap_publisher_->get_current_state()); + costmap_lifecycle_node_ = std::make_shared("fake_costmap"); + layer_subscriber_ = std::make_shared( + costmap_lifecycle_node_->shared_from_this()); + costmap_lifecycle_node_->on_configure(costmap_lifecycle_node_->get_current_state()); + costmap_lifecycle_node_->on_activate(costmap_lifecycle_node_->get_current_state()); } - ~TestNode() override + ~CostmapRosTestFixture() override { - costmap_publisher_->on_deactivate(costmap_publisher_->get_current_state()); - costmap_publisher_->on_cleanup(costmap_publisher_->get_current_state()); + costmap_lifecycle_node_->on_deactivate(costmap_lifecycle_node_->get_current_state()); + costmap_lifecycle_node_->on_cleanup(costmap_lifecycle_node_->get_current_state()); } protected: - std::shared_ptr costmap_publisher_; + std::shared_ptr costmap_lifecycle_node_; + std::shared_ptr layer_subscriber_; }; -TEST_F(TestNode, costmap_pub_test) +TEST_F(CostmapRosTestFixture, costmap_pub_test) { - auto future = costmap_publisher_->promise_.get_future(); + auto future = layer_subscriber_->layer_promise_.get_future(); auto status = future.wait_for(std::chrono::seconds(5)); EXPECT_TRUE(status == std::future_status::ready); auto costmap_raw = future.get(); - // Check that the first row is free space - for (int i = 0; i < 10; ++i) { - EXPECT_EQ(costmap_raw->data[0], nav2_costmap_2d::FREE_SPACE); + // Check first 20 cells of the 10by10 map + unsigned int i = 0; + for (; i < 7; ++i) { + EXPECT_EQ(costmap_raw->data[i], nav2_costmap_2d::FREE_SPACE); + } + for (; i < 10; ++i) { + EXPECT_EQ(costmap_raw->data[i++], nav2_costmap_2d::LETHAL_OBSTACLE); + } + for (; i < 17; ++i) { + EXPECT_EQ(costmap_raw->data[i], nav2_costmap_2d::FREE_SPACE); + } + for (; i < 20; ++i) { + EXPECT_EQ(costmap_raw->data[i++], nav2_costmap_2d::LETHAL_OBSTACLE); } } From 539db41d9675b416b8d95b066b611e2b7fb13cbe Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Wed, 7 Dec 2022 17:20:09 -0500 Subject: [PATCH 42/43] update --- nav2_costmap_2d/src/costmap_2d_ros.cpp | 28 ++++++++++++++------------ 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 6ee2ed8f7f..aa7fd7f12b 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -476,22 +476,24 @@ Costmap2DROS::mapUpdateLoop(double frequency) layered_costmap_->getBounds(&x0, &xn, &y0, &yn); costmap_publisher_->updateBounds(x0, xn, y0, yn); - for (auto & layer_pub : layer_publishers_) { - layer_pub->updateBounds(x0, xn, y0, yn); - } + for (auto &layer_pub: layer_publishers_) { + layer_pub->updateBounds(x0, xn, y0, yn); + } - auto current_time = now(); - if ((last_publish_ + publish_cycle_ < current_time) || // publish_cycle_ is due - (current_time < last_publish_)) // time has moved backwards, probably due to a switch to sim_time // NOLINT - { - RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str()); - costmap_publisher_->publishCostmap(); + auto current_time = now(); + if ((last_publish_ + publish_cycle_ < current_time) || // publish_cycle_ is due + (current_time < + last_publish_)) // time has moved backwards, probably due to a switch to sim_time // NOLINT + { + RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str()); + costmap_publisher_->publishCostmap(); - for (auto & layer_pub : layer_publishers_) { - layer_pub->publishCostmap(); - } + for (auto &layer_pub: layer_publishers_) { + layer_pub->publishCostmap(); + } - last_publish_ = current_time; + last_publish_ = current_time; + } } } From 1a05397d8ebd64b90787a54286c8c902c88fb3bb Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Thu, 8 Dec 2022 09:34:35 -0500 Subject: [PATCH 43/43] revert Cmake --- nav2_costmap_2d/test/CMakeLists.txt | 4 +- .../test/integration/CMakeLists.txt | 228 +++++++++--------- 2 files changed, 116 insertions(+), 116 deletions(-) diff --git a/nav2_costmap_2d/test/CMakeLists.txt b/nav2_costmap_2d/test/CMakeLists.txt index 76481e883b..ad7905ad1f 100644 --- a/nav2_costmap_2d/test/CMakeLists.txt +++ b/nav2_costmap_2d/test/CMakeLists.txt @@ -1,6 +1,6 @@ set(TEST_MAP_DIR ${CMAKE_CURRENT_SOURCE_DIR}/map) set(TEST_LAUNCH_DIR ${CMAKE_CURRENT_SOURCE_DIR}/test_launch_files) -#add_subdirectory(unit) +add_subdirectory(unit) add_subdirectory(integration) -#add_subdirectory(regression) +add_subdirectory(regression) diff --git a/nav2_costmap_2d/test/integration/CMakeLists.txt b/nav2_costmap_2d/test/integration/CMakeLists.txt index 1070064533..0768972a0b 100644 --- a/nav2_costmap_2d/test/integration/CMakeLists.txt +++ b/nav2_costmap_2d/test/integration/CMakeLists.txt @@ -1,68 +1,68 @@ -#ament_add_gtest_executable(footprint_tests_exec -# footprint_tests.cpp -#) -#ament_target_dependencies(footprint_tests_exec -# ${dependencies} -#) -#target_link_libraries(footprint_tests_exec -# nav2_costmap_2d_core -# layers -#) -# -#ament_add_gtest_executable(test_collision_checker_exec -# test_costmap_topic_collision_checker.cpp -#) -#ament_target_dependencies(test_collision_checker_exec -# ${dependencies} -#) -#target_link_libraries(test_collision_checker_exec -# nav2_costmap_2d_core -# nav2_costmap_2d_client -# layers -#) -# -#ament_add_gtest_executable(inflation_tests_exec -# inflation_tests.cpp -#) -#ament_target_dependencies(inflation_tests_exec -# ${dependencies} -#) -#target_link_libraries(inflation_tests_exec -# nav2_costmap_2d_core -# layers -#) -# -#ament_add_gtest_executable(obstacle_tests_exec -# obstacle_tests.cpp -#) -#ament_target_dependencies(obstacle_tests_exec -# ${dependencies} -#) -#target_link_libraries(obstacle_tests_exec -# nav2_costmap_2d_core -# layers -#) -# -#ament_add_gtest_executable(range_tests_exec -# range_tests.cpp -#) -#ament_target_dependencies(range_tests_exec -# ${dependencies} -#) -#target_link_libraries(range_tests_exec -# nav2_costmap_2d_core -# layers -#) -# -#ament_add_gtest(dyn_params_tests -# dyn_params_tests.cpp -#) -#ament_target_dependencies(dyn_params_tests -# ${dependencies} -#) -#target_link_libraries(dyn_params_tests -# nav2_costmap_2d_core -#) +ament_add_gtest_executable(footprint_tests_exec + footprint_tests.cpp +) +ament_target_dependencies(footprint_tests_exec + ${dependencies} +) +target_link_libraries(footprint_tests_exec + nav2_costmap_2d_core + layers +) + +ament_add_gtest_executable(test_collision_checker_exec + test_costmap_topic_collision_checker.cpp +) +ament_target_dependencies(test_collision_checker_exec + ${dependencies} +) +target_link_libraries(test_collision_checker_exec + nav2_costmap_2d_core + nav2_costmap_2d_client + layers +) + +ament_add_gtest_executable(inflation_tests_exec + inflation_tests.cpp +) +ament_target_dependencies(inflation_tests_exec + ${dependencies} +) +target_link_libraries(inflation_tests_exec + nav2_costmap_2d_core + layers +) + +ament_add_gtest_executable(obstacle_tests_exec + obstacle_tests.cpp +) +ament_target_dependencies(obstacle_tests_exec + ${dependencies} +) +target_link_libraries(obstacle_tests_exec + nav2_costmap_2d_core + layers +) + +ament_add_gtest_executable(range_tests_exec + range_tests.cpp +) +ament_target_dependencies(range_tests_exec + ${dependencies} +) +target_link_libraries(range_tests_exec + nav2_costmap_2d_core + layers +) + +ament_add_gtest(dyn_params_tests + dyn_params_tests.cpp +) +ament_target_dependencies(dyn_params_tests + ${dependencies} +) +target_link_libraries(dyn_params_tests + nav2_costmap_2d_core +) ament_add_gtest_executable(test_costmap_publisher_exec test_costmap_2d_publisher.cpp @@ -76,55 +76,55 @@ target_link_libraries(test_costmap_publisher_exec layers ) -#ament_add_test(test_collision_checker -# GENERATE_RESULT_FOR_RETURN_CODE_ZERO -# COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" -# WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" -# ENV -# TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml -# TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} -# TEST_EXECUTABLE=$ -#) -# -#ament_add_test(footprint_tests -# GENERATE_RESULT_FOR_RETURN_CODE_ZERO -# COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" -# WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" -# ENV -# TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml -# TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} -# TEST_EXECUTABLE=$ -#) -# -#ament_add_test(inflation_tests -# GENERATE_RESULT_FOR_RETURN_CODE_ZERO -# COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" -# WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" -# ENV -# TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml -# TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} -# TEST_EXECUTABLE=$ -#) -# -#ament_add_test(obstacle_tests -# GENERATE_RESULT_FOR_RETURN_CODE_ZERO -# COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" -# WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" -# ENV -# TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml -# TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} -# TEST_EXECUTABLE=$ -#) -# -#ament_add_test(range_tests -# GENERATE_RESULT_FOR_RETURN_CODE_ZERO -# COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" -# WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" -# ENV -# TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml -# TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} -# TEST_EXECUTABLE=$ -#) +ament_add_test(test_collision_checker + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + ENV + TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml + TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} + TEST_EXECUTABLE=$ +) + +ament_add_test(footprint_tests + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + ENV + TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml + TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} + TEST_EXECUTABLE=$ +) + +ament_add_test(inflation_tests + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + ENV + TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml + TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} + TEST_EXECUTABLE=$ +) + +ament_add_test(obstacle_tests + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + ENV + TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml + TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} + TEST_EXECUTABLE=$ +) + +ament_add_test(range_tests + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/costmap_tests_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + ENV + TEST_MAP=${TEST_MAP_DIR}/TenByTen.yaml + TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR} + TEST_EXECUTABLE=$ +) ament_add_test(test_costmap_publisher_exec GENERATE_RESULT_FOR_RETURN_CODE_ZERO