Add DynamicParameterPatterns to pose_pogress_checker plugin

Author: Nils-ChristianIseke

nav2_controller/CMakeLists.txt

@@ -85,7 +85,10 @@ target_link_libraries(simple_progress_checker PRIVATE
   pluginlib::pluginlib
 )
 
-add_library(pose_progress_checker SHARED plugins/pose_progress_checker.cpp)
+add_library(pose_progress_checker SHARED
+  plugins/pose_progress_checker.cpp
+  plugins/parameter_handler_pose_pogress_checker.cpp
+)
 target_include_directories(pose_progress_checker
   PUBLIC
     "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
@@ -97,6 +100,7 @@ target_link_libraries(pose_progress_checker PUBLIC
   rclcpp_lifecycle::rclcpp_lifecycle
   ${rcl_interfaces_TARGETS}
   simple_progress_checker
+
 )
 target_link_libraries(pose_progress_checker PRIVATE
   angles::angles

nav2_controller/include/nav2_controller/plugins/parameter_handler_pose_progress_checker.hpp

@@ -0,0 +1,65 @@
+// Copyright (c) 2023 Dexory
+//
+// 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_CONTROLLER__PLUGINS__PARAMETER_HANDLER_POSE_PROGRESS_CHECKER_HPP_
+#define NAV2_CONTROLLER__PLUGINS__PARAMETER_HANDLER_POSE_PROGRESS_CHECKER_HPP_
+
+#include <string>
+#include <vector>
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_controller/plugins/simple_progress_checker.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "nav2_util/node_utils.hpp"
+
+namespace nav2_controller
+{
+struct Parameters
+{
+  double required_movement_angle;
+};
+
+/**
+ * @class nav2_controller::ParameterHandler
+ * @brief This class handls parameters and dynamic parameter updates for the nav2_controller.
+ */
+class ParameterHandler
+{
+public:
+  ParameterHandler(
+    rclcpp_lifecycle::LifecycleNode::SharedPtr node,
+    std::string & plugin_name, rclcpp::Logger & logger);
+  ~ParameterHandler();
+  Parameters * getParams() {return &params_;}
+
+protected:
+  rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
+
+  void
+  updateParametersCallback(
+    std::vector<rclcpp::Parameter> parameters);
+
+  rcl_interfaces::msg::SetParametersResult
+  validateParameterUpdatesCallback(
+    std::vector<rclcpp::Parameter> parameters);
+  rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr post_set_params_handler_;
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_params_handler_;
+  Parameters params_;
+  std::string plugin_name_;
+  rclcpp::Logger logger_ {
+    rclcpp::get_logger("nav2_controller::PoseProgressChecker")};
+};
+
+}  // namespace nav2_controller
+
+#endif  // NAV2_CONTROLLER__PLUGINS__PARAMETER_HANDLER_POSE_PROGRESS_CHECKER_HPP_

nav2_controller/include/nav2_controller/plugins/pose_progress_checker.hpp

@@ -17,9 +17,11 @@
 
 #include <string>
 #include <vector>
+#include <memory>
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_controller/plugins/simple_progress_checker.hpp"
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "nav2_controller/plugins/parameter_handler_pose_progress_checker.hpp"
 
 namespace nav2_controller
 {
@@ -50,18 +52,15 @@ class PoseProgressChecker : public SimpleProgressChecker
     const geometry_msgs::msg::Pose2D &);
 
   double required_movement_angle_;
-
-  // Dynamic parameters handler
-  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_;
+  Parameters * params_;
   std::string plugin_name_;
 
-  /**
-   * @brief Callback executed when a parameter change is detected
-   * @param parameters list of changed parameters
-   */
-  rcl_interfaces::msg::SetParametersResult
-  dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters);
+  rclcpp::Logger logger_ {rclcpp::get_logger("PoseProgressChecker")};
+  rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
+  std::unique_ptr<nav2_controller::ParameterHandler> param_handler_;
 };
+
+
 }  // namespace nav2_controller
 
 #endif  // NAV2_CONTROLLER__PLUGINS__POSE_PROGRESS_CHECKER_HPP_

nav2_controller/plugins/parameter_handler_pose_pogress_checker.cpp

@@ -0,0 +1,97 @@
+// Copyright (c) 2023 Dexory
+//
+// 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 <cmath>
+#include <string>
+#include <memory>
+#include <vector>
+#include "angles/angles.h"
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose2_d.hpp"
+#include "pluginlib/class_list_macros.hpp"
+
+#include "nav2_util/node_utils.hpp"
+#include "nav2_controller/plugins/parameter_handler_pose_progress_checker.hpp"
+#include "nav2_controller/plugins/pose_progress_checker.hpp"
+
+namespace nav2_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)
+{
+  node_ = node;
+  plugin_name_ = plugin_name;
+  logger_ = logger;
+
+  declare_parameter_if_not_declared(node, plugin_name + ".required_movement_angle",
+    rclcpp::ParameterValue(0.5));
+  node->get_parameter(plugin_name + ".required_movement_angle",
+    params_.required_movement_angle);
+  on_set_params_handler_ = node->add_on_set_parameters_callback(
+    [this](const auto & params) {
+      return this->validateParameterUpdatesCallback(params);
+    });
+  post_set_params_handler_ = node->add_post_set_parameters_callback(
+    [this](const auto & params) {
+      return this->updateParametersCallback(params);
+      });
+}
+ParameterHandler::~ParameterHandler() = default;
+
+void ParameterHandler::updateParametersCallback(
+  std::vector<rclcpp::Parameter> parameters)
+{
+  for (const auto & param : parameters) {
+    const auto & name = param.get_name();
+    const auto & type = param.get_type();
+    if (name == plugin_name_ + ".required_movement_angle" &&
+      type == ParameterType::PARAMETER_DOUBLE)
+    {
+      params_.required_movement_angle = param.as_double();
+    }
+  }
+}
+rcl_interfaces::msg::SetParametersResult
+ParameterHandler::validateParameterUpdatesCallback(std::vector<rclcpp::Parameter> parameters)
+{
+  rcl_interfaces::msg::SetParametersResult result;
+  for (auto parameter : parameters) {
+    const auto & type = parameter.get_type();
+    const auto & name = parameter.get_name();
+    {
+      if (name.find(plugin_name_ + ".") != 0) {
+        continue;
+      }
+      if (name == plugin_name_ + ".required_movement_angle" &&
+        type == ParameterType::PARAMETER_DOUBLE &&
+        (parameter.as_double() <= 0.0 || parameter.as_double() >= 2 * M_PI))
+      {
+        result.successful = false;
+        result.reason = "The value required_movement_angle is incorrectly set, "
+          "it should be 0 < required_movement_angle < 2PI. Ignoring parameter update.";
+        return result;
+      }
+    }
+  }
+  result.successful = true;
+  return result;
+}
+
+}  // namespace nav2_controller

nav2_controller/plugins/pose_progress_checker.cpp

@@ -12,39 +12,44 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "nav2_controller/plugins/pose_progress_checker.hpp"
 #include <cmath>
 #include <string>
 #include <memory>
 #include <vector>
+
 #include "angles/angles.h"
 #include "nav_2d_utils/conversions.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "geometry_msgs/msg/pose2_d.hpp"
 #include "nav2_util/node_utils.hpp"
 #include "pluginlib/class_list_macros.hpp"
+#include "nav2_controller/plugins/pose_progress_checker.hpp"
+#include "nav2_controller/plugins/parameter_handler_pose_progress_checker.hpp"
+#include "nav2_core/controller_exceptions.hpp"
 
 using rcl_interfaces::msg::ParameterType;
 using std::placeholders::_1;
 
 namespace nav2_controller
 {
-
+using nav2_util::declare_parameter_if_not_declared;
+using rcl_interfaces::msg::ParameterType;
 void PoseProgressChecker::initialize(
   const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
   const std::string & plugin_name)
 {
-  plugin_name_ = plugin_name;
-  SimpleProgressChecker::initialize(parent, plugin_name);
   auto node = parent.lock();
+  if (!node) {
+    throw nav2_core::ControllerException("Unable to lock node!");
+  }
+  node_ = parent;
 
-  nav2_util::declare_parameter_if_not_declared(
-    node, plugin_name + ".required_movement_angle", rclcpp::ParameterValue(0.5));
-  node->get_parameter_or(plugin_name + ".required_movement_angle", required_movement_angle_, 0.5);
 
-  // Add callback for dynamic parameters
-  dyn_params_handler_ = node->add_on_set_parameters_callback(
-    std::bind(&PoseProgressChecker::dynamicParametersCallback, this, _1));
+  plugin_name_ = plugin_name;
+  logger_ = node->get_logger();
+  param_handler_ = std::make_unique<ParameterHandler>(node, plugin_name_, logger_);
+  params_ = param_handler_->getParams();
+  SimpleProgressChecker::initialize(parent, plugin_name);
 }
 
 bool PoseProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose)
@@ -64,7 +69,7 @@ bool PoseProgressChecker::check(geometry_msgs::msg::PoseStamped & current_pose)
 bool PoseProgressChecker::isRobotMovedEnough(const geometry_msgs::msg::Pose2D & pose)
 {
   return pose_distance(pose, baseline_pose_) > radius_ ||
-         poseAngleDistance(pose, baseline_pose_) > required_movement_angle_;
+         poseAngleDistance(pose, baseline_pose_) > params_->required_movement_angle;
 }
 
 double PoseProgressChecker::poseAngleDistance(
@@ -74,23 +79,6 @@ double PoseProgressChecker::poseAngleDistance(
   return abs(angles::shortest_angular_distance(pose1.theta, pose2.theta));
 }
 
-rcl_interfaces::msg::SetParametersResult
-PoseProgressChecker::dynamicParametersCallback(std::vector<rclcpp::Parameter> parameters)
-{
-  rcl_interfaces::msg::SetParametersResult result;
-  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_ + ".required_movement_angle") {
-        required_movement_angle_ = parameter.as_double();
-      }
-    }
-  }
-  result.successful = true;
-  return result;
-}
 
 }  // namespace nav2_controller