From 9f8e6650a41f9c753598816b03624d7bedd347ce Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Thu, 11 Feb 2021 18:13:58 +0100 Subject: [PATCH 1/4] make the 3 params changeable at runtime --- .../include/nav2_navfn_planner/navfn_planner.hpp | 2 ++ nav2_navfn_planner/src/navfn_planner.cpp | 9 ++++++++- 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp index 5f2a861325..c2a40059d4 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp @@ -140,6 +140,8 @@ class NavfnPlanner : public nav2_core::GlobalPlanner // Whether to use the astar planner or default dijkstras bool use_astar_; + + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; }; } // namespace nav2_navfn_planner diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index f741bf799c..c66edb17b6 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -67,7 +67,8 @@ NavfnPlanner::configure( costmap_ = costmap_ros->getCostmap(); global_frame_ = costmap_ros->getGlobalFrameID(); - auto node = parent.lock(); + node_ = parent; + auto node = node_.lock(); clock_ = node->get_clock(); logger_ = node->get_logger(); @@ -123,6 +124,12 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( steady_clock::time_point a = steady_clock::now(); #endif + // Refresh parameter + auto node = node_.lock(); + node->get_parameter(name_ + ".tolerance", tolerance_); + node->get_parameter(name_ + ".use_astar", use_astar_); + node->get_parameter(name_ + ".allow_unknown", allow_unknown_); + // Update planner based on the new costmap size if (isPlannerOutOfDate()) { planner_->setNavArr( From 661a186a3ece66beede22bcb3a34a231051ce9e6 Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Mon, 15 Feb 2021 18:42:24 +0100 Subject: [PATCH 2/4] use parameter events callbacks --- .../nav2_navfn_planner/navfn_planner.hpp | 5 +- nav2_navfn_planner/src/navfn_planner.cpp | 47 +++++++++++++++---- 2 files changed, 42 insertions(+), 10 deletions(-) diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp index c2a40059d4..24f9b47af2 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp @@ -141,7 +141,10 @@ class NavfnPlanner : public nav2_core::GlobalPlanner // Whether to use the astar planner or default dijkstras bool use_astar_; - rclcpp_lifecycle::LifecycleNode::WeakPtr node_; + // Subscription for parameter change + rclcpp::AsyncParametersClient::SharedPtr parameters_client_; + rclcpp::Subscription::SharedPtr parameter_event_sub_; + void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); }; } // namespace nav2_navfn_planner diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index c66edb17b6..f23454b65f 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -40,6 +40,8 @@ using namespace std::chrono_literals; using nav2_util::declare_parameter_if_not_declared; +using rcl_interfaces::msg::ParameterType; +using std::placeholders::_1; namespace nav2_navfn_planner { @@ -67,8 +69,7 @@ NavfnPlanner::configure( costmap_ = costmap_ros->getCostmap(); global_frame_ = costmap_ros->getGlobalFrameID(); - node_ = parent; - auto node = node_.lock(); + auto node = parent.lock(); clock_ = node->get_clock(); logger_ = node->get_logger(); @@ -89,6 +90,16 @@ NavfnPlanner::configure( planner_ = std::make_unique( costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()); + + // Setup callback for changes to parameters. + parameters_client_ = std::make_shared( + node->get_node_base_interface(), + node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + + parameter_event_sub_ = parameters_client_->on_parameter_event( + std::bind(&NavfnPlanner::on_parameter_event_callback, this, _1)); } void @@ -124,12 +135,6 @@ nav_msgs::msg::Path NavfnPlanner::createPlan( steady_clock::time_point a = steady_clock::now(); #endif - // Refresh parameter - auto node = node_.lock(); - node->get_parameter(name_ + ".tolerance", tolerance_); - node->get_parameter(name_ + ".use_astar", use_astar_); - node->get_parameter(name_ + ".allow_unknown", allow_unknown_); - // Update planner based on the new costmap size if (isPlannerOutOfDate()) { planner_->setNavArr( @@ -466,7 +471,31 @@ NavfnPlanner::clearRobotCell(unsigned int mx, unsigned int my) costmap_->setCost(mx, my, nav2_costmap_2d::FREE_SPACE); } -} // namespace nav2_navfn_planner +void +NavfnPlanner::on_parameter_event_callback( + const rcl_interfaces::msg::ParameterEvent::SharedPtr event) +{ + for (auto & changed_parameter : event->changed_parameters) { + const auto & type = changed_parameter.value.type; + const auto & name = changed_parameter.name; + const auto & value = changed_parameter.value; + + if (type == ParameterType::PARAMETER_DOUBLE) { + if (name == name_ + ".tolerance") { + tolerance_ = value.double_value; + } + } else if (type == ParameterType::PARAMETER_BOOL) { + if (name == name_ + ".use_astar") { + use_astar_ = value.bool_value; + } else if (name == name_ + ".allow_unknown") { + allow_unknown_ = value.bool_value; + } + } + } +} + +} +// namespace nav2_navfn_planner #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(nav2_navfn_planner::NavfnPlanner, nav2_core::GlobalPlanner) From 3851b21e83075255ddf632716f3230762885422d Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Thu, 25 Feb 2021 13:08:48 +0100 Subject: [PATCH 3/4] doxygen --- .../include/nav2_navfn_planner/navfn_planner.hpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp index ed192897c1..98180cdd17 100644 --- a/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp +++ b/nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp @@ -217,6 +217,11 @@ class NavfnPlanner : public nav2_core::GlobalPlanner // Subscription for parameter change rclcpp::AsyncParametersClient::SharedPtr parameters_client_; rclcpp::Subscription::SharedPtr parameter_event_sub_; + + /** + * @brief Callback executed when a paramter change is detected + * @param event ParameterEvent message + */ void on_parameter_event_callback(const rcl_interfaces::msg::ParameterEvent::SharedPtr event); }; From 5fd20ed9fb5c410add4fdf2a35d463165dafc96d Mon Sep 17 00:00:00 2001 From: Guillaume Doisy Date: Thu, 25 Feb 2021 13:13:07 +0100 Subject: [PATCH 4/4] lint --- nav2_navfn_planner/src/navfn_planner.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index f23454b65f..4cdaab3b73 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -494,8 +494,7 @@ NavfnPlanner::on_parameter_event_callback( } } -} -// namespace nav2_navfn_planner +} // namespace nav2_navfn_planner #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(nav2_navfn_planner::NavfnPlanner, nav2_core::GlobalPlanner)