diff --git a/nav2_rotation_shim_controller/CMakeLists.txt b/nav2_rotation_shim_controller/CMakeLists.txt index 68b64fdc0a8..b966865fa47 100644 --- a/nav2_rotation_shim_controller/CMakeLists.txt +++ b/nav2_rotation_shim_controller/CMakeLists.txt @@ -24,7 +24,8 @@ nav2_package() set(library_name nav2_rotation_shim_controller) add_library(${library_name} SHARED - src/nav2_rotation_shim_controller.cpp) + src/nav2_rotation_shim_controller.cpp + src/parameter_handler.cpp) target_include_directories(${library_name} PUBLIC "$" diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp index 5f12c6dda68..8a556dac40d 100644 --- a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/nav2_rotation_shim_controller.hpp @@ -29,6 +29,7 @@ #include "nav2_core/controller_exceptions.hpp" #include "nav2_costmap_2d/footprint_collision_checker.hpp" #include "nav2_controller/plugins/position_goal_checker.hpp" +#include "nav2_rotation_shim_controller/parameter_handler.hpp" namespace nav2_rotation_shim_controller { @@ -162,13 +163,6 @@ class RotationShimController : public nav2_core::Controller */ bool isGoalChanged(const nav_msgs::msg::Path & path); - /** - * @brief Callback executed when a parameter change is detected - * @param event ParameterEvent message - */ - rcl_interfaces::msg::SetParametersResult - dynamicParametersCallback(std::vector parameters); - nav2::LifecycleNode::WeakPtr node_; std::shared_ptr tf_; std::string plugin_name_; @@ -182,18 +176,11 @@ class RotationShimController : public nav2_core::Controller nav2_core::Controller::Ptr primary_controller_; bool path_updated_; nav_msgs::msg::Path current_path_; - double forward_sampling_distance_, angular_dist_threshold_, angular_disengage_threshold_; - double rotate_to_heading_angular_vel_, max_angular_accel_; - double control_duration_, simulate_ahead_time_; - bool rotate_to_goal_heading_, in_rotation_, rotate_to_heading_once_; - bool closed_loop_; - bool use_path_orientations_; + Parameters * params_; + bool in_rotation_; double last_angular_vel_ = std::numeric_limits::max(); - - // Dynamic parameters handler - std::mutex mutex_; - rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; std::unique_ptr position_goal_checker_; + std::unique_ptr param_handler_; }; } // namespace nav2_rotation_shim_controller diff --git a/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/parameter_handler.hpp b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/parameter_handler.hpp new file mode 100644 index 00000000000..b7f8f1cb333 --- /dev/null +++ b/nav2_rotation_shim_controller/include/nav2_rotation_shim_controller/parameter_handler.hpp @@ -0,0 +1,117 @@ +// 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_ROTATION_SHIM_CONTROLLER__PARAMETER_HANDLER_HPP_ +#define NAV2_ROTATION_SHIM_CONTROLLER__PARAMETER_HANDLER_HPP_ + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "nav2_ros_common/lifecycle_node.hpp" +#include "nav2_util/odometry_utils.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav2_ros_common/node_utils.hpp" + +namespace nav2_rotation_shim_controller +{ + +struct Parameters +{ + double forward_sampling_distance; + double angular_dist_threshold; + double angular_disengage_threshold; + double rotate_to_heading_angular_vel; + double max_angular_accel; + double simulate_ahead_time; + double control_duration; + bool rotate_to_goal_heading; + bool rotate_to_heading_once; + bool closed_loop; + bool use_path_orientations; + std::string primary_controller; +}; + +/** + * @class nav2_rotation_shim_controller::ParameterHandler + * @brief Handles parameters and dynamic parameters for Rotation Shim + */ +class ParameterHandler +{ +public: + /** + * @brief Constructor for nav2_rotation_shim_controller::ParameterHandler + */ + ParameterHandler( + const nav2::LifecycleNode::SharedPtr & node, + std::string & plugin_name, + rclcpp::Logger & logger); + + /** + * @brief Destrructor for nav2_rotation_shim_controller::ParameterHandler + */ + ~ParameterHandler(); + + std::mutex & getMutex() {return mutex_;} + + Parameters * getParams() {return ¶ms_;} + + /** + * @brief Registers callbacks for dynamic parameter handling. + */ + void activate(); + + /** + * @brief Resets callbacks for dynamic parameter handling. + */ + void deactivate(); + +protected: + nav2::LifecycleNode::WeakPtr node_; + + /** + * @brief Apply parameter updates after validation + * This callback is executed when parameters have been successfully updated. + * It updates the internal configuration of the node with the new parameter values. + * @param parameters List of parameters that have been updated. + */ + void + updateParametersCallback(const std::vector & parameters); + + /** + * @brief Validate incoming parameter updates before applying them. + * This callback is triggered when one or more parameters are about to be updated. + * It checks the validity of parameter values and rejects updates that would lead + * to invalid or inconsistent configurations + * @param parameters List of parameters that are being updated. + * @return rcl_interfaces::msg::SetParametersResult Result indicating whether the update is accepted. + */ + rcl_interfaces::msg::SetParametersResult + validateParameterUpdatesCallback(const std::vector & parameters); + + // Dynamic parameters handler + std::mutex mutex_; + 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("RotationShimController")}; +}; + +} // namespace nav2_rotation_shim_controller + +#endif // NAV2_ROTATION_SHIM_CONTROLLER__PARAMETER_HANDLER_HPP_ diff --git a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp index 5be66f548ac..210e63c6679 100644 --- a/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp +++ b/nav2_rotation_shim_controller/src/nav2_rotation_shim_controller.cpp @@ -54,54 +54,17 @@ void RotationShimController::configure( logger_ = node->get_logger(); clock_ = node->get_clock(); - std::string primary_controller; - double control_frequency; - nav2::declare_parameter_if_not_declared( - node, plugin_name_ + ".angular_dist_threshold", rclcpp::ParameterValue(0.785)); // 45 deg - nav2::declare_parameter_if_not_declared( - node, plugin_name_ + ".angular_disengage_threshold", rclcpp::ParameterValue(0.785 / 2.0)); - nav2::declare_parameter_if_not_declared( - node, plugin_name_ + ".forward_sampling_distance", rclcpp::ParameterValue(0.5)); - nav2::declare_parameter_if_not_declared( - node, plugin_name_ + ".rotate_to_heading_angular_vel", rclcpp::ParameterValue(1.8)); - nav2::declare_parameter_if_not_declared( - node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); - nav2::declare_parameter_if_not_declared( - node, plugin_name_ + ".simulate_ahead_time", rclcpp::ParameterValue(1.0)); - nav2::declare_parameter_if_not_declared( - node, plugin_name_ + ".primary_controller", rclcpp::PARAMETER_STRING); - nav2::declare_parameter_if_not_declared( - node, plugin_name_ + ".rotate_to_goal_heading", rclcpp::ParameterValue(false)); - nav2::declare_parameter_if_not_declared( - node, plugin_name_ + ".rotate_to_heading_once", rclcpp::ParameterValue(false)); - nav2::declare_parameter_if_not_declared( - node, plugin_name_ + ".closed_loop", rclcpp::ParameterValue(true)); - nav2::declare_parameter_if_not_declared( - node, plugin_name_ + ".use_path_orientations", rclcpp::ParameterValue(false)); - - node->get_parameter(plugin_name_ + ".angular_dist_threshold", angular_dist_threshold_); - node->get_parameter(plugin_name_ + ".angular_disengage_threshold", angular_disengage_threshold_); - node->get_parameter(plugin_name_ + ".forward_sampling_distance", forward_sampling_distance_); - node->get_parameter( - plugin_name_ + ".rotate_to_heading_angular_vel", - rotate_to_heading_angular_vel_); - node->get_parameter(plugin_name_ + ".max_angular_accel", max_angular_accel_); - node->get_parameter(plugin_name_ + ".simulate_ahead_time", simulate_ahead_time_); - - primary_controller = node->get_parameter(plugin_name_ + ".primary_controller").as_string(); - node->get_parameter("controller_frequency", control_frequency); - control_duration_ = 1.0 / control_frequency; - - node->get_parameter(plugin_name_ + ".rotate_to_goal_heading", rotate_to_goal_heading_); - node->get_parameter(plugin_name_ + ".rotate_to_heading_once", rotate_to_heading_once_); - node->get_parameter(plugin_name_ + ".closed_loop", closed_loop_); - node->get_parameter(plugin_name_ + ".use_path_orientations", use_path_orientations_); + // Handles storage and dynamic configuration of parameters. + // Returns pointer to data current param settings. + param_handler_ = std::make_unique( + node, plugin_name_, logger_); + params_ = param_handler_->getParams(); try { - primary_controller_ = lp_loader_.createUniqueInstance(primary_controller); + primary_controller_ = lp_loader_.createUniqueInstance(params_->primary_controller); RCLCPP_INFO( logger_, "Created internal controller for rotation shimming: %s of type %s", - plugin_name_.c_str(), primary_controller.c_str()); + plugin_name_.c_str(), params_->primary_controller.c_str()); } catch (const pluginlib::PluginlibException & ex) { RCLCPP_FATAL( logger_, @@ -109,7 +72,7 @@ void RotationShimController::configure( return; } - primary_controller_->configure(parent, name, tf, costmap_ros); + primary_controller_->configure(parent, name + ".primary_controller", tf, costmap_ros); // initialize collision checker and set costmap collision_checker_ = std::make_uniqueactivate(); in_rotation_ = false; last_angular_vel_ = std::numeric_limits::max(); - - auto node = node_.lock(); - dyn_params_handler_ = node->add_on_set_parameters_callback( - std::bind( - &RotationShimController::dynamicParametersCallback, - this, std::placeholders::_1)); position_goal_checker_->reset(); + param_handler_->activate(); } void RotationShimController::deactivate() @@ -145,11 +103,7 @@ void RotationShimController::deactivate() plugin_name_.c_str()); primary_controller_->deactivate(); - - if (auto node = node_.lock()) { - node->remove_on_set_parameters_callback(dyn_params_handler_.get()); - } - dyn_params_handler_.reset(); + param_handler_->deactivate(); } void RotationShimController::cleanup() @@ -171,8 +125,8 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands nav2_core::GoalChecker * goal_checker) { // Rotate to goal heading when in goal xy tolerance - if (rotate_to_goal_heading_) { - std::lock_guard lock_reinit(mutex_); + if (params_->rotate_to_goal_heading) { + std::lock_guard lock_reinit(param_handler_->getMutex()); try { geometry_msgs::msg::PoseStamped sampled_pt_goal = getSampledPathGoal(); @@ -212,11 +166,11 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); std::unique_lock lock(*(costmap->getMutex())); - std::lock_guard lock_reinit(mutex_); + std::lock_guard lock_reinit(param_handler_->getMutex()); try { auto sampled_pt = getSampledPathPt(); double angular_distance_to_heading; - if (use_path_orientations_) { + if (params_->use_path_orientations) { angular_distance_to_heading = angles::shortest_angular_distance( tf2::getYaw(pose.pose.orientation), tf2::getYaw(sampled_pt.pose.orientation)); @@ -228,7 +182,7 @@ geometry_msgs::msg::TwistStamped RotationShimController::computeVelocityCommands } double angular_thresh = - in_rotation_ ? angular_disengage_threshold_ : angular_dist_threshold_; + in_rotation_ ? params_->angular_disengage_threshold : params_->angular_dist_threshold; if (abs(angular_distance_to_heading) > angular_thresh) { RCLCPP_DEBUG( logger_, @@ -274,7 +228,7 @@ geometry_msgs::msg::PoseStamped RotationShimController::getSampledPathPt() for (unsigned int i = 1; i != current_path_.poses.size(); i++) { dx = current_path_.poses[i].pose.position.x - start.position.x; dy = current_path_.poses[i].pose.position.y - start.position.y; - if (hypot(dx, dy) >= forward_sampling_distance_) { + if (hypot(dx, dy) >= params_->forward_sampling_distance) { current_path_.poses[i].header.frame_id = current_path_.header.frame_id; current_path_.poses[i].header.stamp = clock_->now(); // Get current time transformation return current_path_.poses[i]; @@ -315,7 +269,7 @@ RotationShimController::computeRotateToHeadingCommand( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & velocity) { - auto current = closed_loop_ ? velocity.angular.z : last_angular_vel_; + auto current = params_->closed_loop ? velocity.angular.z : last_angular_vel_; if (current == std::numeric_limits::max()) { current = 0.0; } @@ -323,15 +277,16 @@ RotationShimController::computeRotateToHeadingCommand( geometry_msgs::msg::TwistStamped cmd_vel; cmd_vel.header = pose.header; const double sign = angular_distance_to_heading > 0.0 ? 1.0 : -1.0; - const double angular_vel = sign * rotate_to_heading_angular_vel_; - const double & dt = control_duration_; - const double min_feasible_angular_speed = current - max_angular_accel_ * dt; - const double max_feasible_angular_speed = current + max_angular_accel_ * dt; + const double angular_vel = sign * params_->rotate_to_heading_angular_vel; + const double & dt = params_->control_duration; + const double min_feasible_angular_speed = current - params_->max_angular_accel * dt; + const double max_feasible_angular_speed = current + params_->max_angular_accel * dt; cmd_vel.twist.angular.z = std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed); // Check if we need to slow down to avoid overshooting - double max_vel_to_stop = std::sqrt(2 * max_angular_accel_ * fabs(angular_distance_to_heading)); + double max_vel_to_stop = std::sqrt(2 * params_->max_angular_accel * + fabs(angular_distance_to_heading)); if (fabs(cmd_vel.twist.angular.z) > max_vel_to_stop) { cmd_vel.twist.angular.z = sign * max_vel_to_stop; } @@ -351,10 +306,10 @@ void RotationShimController::isCollisionFree( double yaw = 0.0; double footprint_cost = 0.0; double remaining_rotation_before_thresh = - fabs(angular_distance_to_heading) - angular_dist_threshold_; + fabs(angular_distance_to_heading) - params_->angular_dist_threshold; - while (simulated_time < simulate_ahead_time_) { - simulated_time += control_duration_; + while (simulated_time < params_->simulate_ahead_time) { + simulated_time += params_->control_duration; yaw = initial_yaw + cmd_vel.twist.angular.z * simulated_time; // Stop simulating past the point it would be passed onto the primary controller @@ -393,7 +348,7 @@ bool RotationShimController::isGoalChanged(const nav_msgs::msg::Path & path) void RotationShimController::setPlan(const nav_msgs::msg::Path & path) { - path_updated_ = rotate_to_heading_once_ ? isGoalChanged(path) : true; + path_updated_ = params_->rotate_to_heading_once ? isGoalChanged(path) : true; current_path_ = path; primary_controller_->setPlan(path); position_goal_checker_->reset(); @@ -411,48 +366,6 @@ void RotationShimController::reset() position_goal_checker_->reset(); } -rcl_interfaces::msg::SetParametersResult -RotationShimController::dynamicParametersCallback(std::vector parameters) -{ - rcl_interfaces::msg::SetParametersResult result; - std::lock_guard lock_reinit(mutex_); - - for (auto parameter : parameters) { - const auto & param_type = parameter.get_type(); - const auto & param_name = parameter.get_name(); - if (param_name.find(plugin_name_ + ".") != 0) { - continue; - } - - if (param_type == ParameterType::PARAMETER_DOUBLE) { - if (param_name == plugin_name_ + ".angular_dist_threshold") { - angular_dist_threshold_ = parameter.as_double(); - } else if (param_name == plugin_name_ + ".forward_sampling_distance") { - forward_sampling_distance_ = parameter.as_double(); - } else if (param_name == plugin_name_ + ".rotate_to_heading_angular_vel") { - rotate_to_heading_angular_vel_ = parameter.as_double(); - } else if (param_name == plugin_name_ + ".max_angular_accel") { - max_angular_accel_ = parameter.as_double(); - } else if (param_name == plugin_name_ + ".simulate_ahead_time") { - simulate_ahead_time_ = parameter.as_double(); - } - } else if (param_type == ParameterType::PARAMETER_BOOL) { - if (param_name == plugin_name_ + ".rotate_to_goal_heading") { - rotate_to_goal_heading_ = parameter.as_bool(); - } else if (param_name == plugin_name_ + ".rotate_to_heading_once") { - rotate_to_heading_once_ = parameter.as_bool(); - } else if (param_name == plugin_name_ + ".closed_loop") { - closed_loop_ = parameter.as_bool(); - } else if (param_name == plugin_name_ + ".use_path_orientations") { - use_path_orientations_ = parameter.as_bool(); - } - } - } - - result.successful = true; - return result; -} - } // namespace nav2_rotation_shim_controller // Register this controller as a nav2_core plugin diff --git a/nav2_rotation_shim_controller/src/parameter_handler.cpp b/nav2_rotation_shim_controller/src/parameter_handler.cpp new file mode 100644 index 00000000000..cc1c8feed6a --- /dev/null +++ b/nav2_rotation_shim_controller/src/parameter_handler.cpp @@ -0,0 +1,176 @@ +// 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_rotation_shim_controller/parameter_handler.hpp" +#include "nav2_core/controller_exceptions.hpp" + +namespace nav2_rotation_shim_controller +{ + +using nav2::declare_parameter_if_not_declared; +using rcl_interfaces::msg::ParameterType; + +ParameterHandler::ParameterHandler( + const nav2::LifecycleNode::SharedPtr & node, + std::string & plugin_name, rclcpp::Logger & logger) +{ + node_ = node; + plugin_name_ = plugin_name; + logger_ = logger; + + params_.angular_dist_threshold = node->declare_or_get_parameter(plugin_name_ + + ".angular_dist_threshold", 0.785); // 45 deg + params_.angular_disengage_threshold = node->declare_or_get_parameter(plugin_name_ + + ".angular_disengage_threshold", 0.785 / 2.0); + params_.forward_sampling_distance = node->declare_or_get_parameter(plugin_name_ + + ".forward_sampling_distance", 0.5); + params_.rotate_to_heading_angular_vel = node->declare_or_get_parameter(plugin_name_ + + ".rotate_to_heading_angular_vel", 1.8); + params_.max_angular_accel = node->declare_or_get_parameter(plugin_name_ + ".max_angular_accel", + 3.2); + params_.simulate_ahead_time = node->declare_or_get_parameter(plugin_name_ + + ".simulate_ahead_time", 1.0); + try { + params_.primary_controller = node->declare_or_get_parameter(plugin_name_ + + ".primary_controller.plugin"); + } catch (const rclcpp::exceptions::InvalidParameterValueException & e) { + RCLCPP_WARN(logger_, "the primary controller must be defined in a namespace:\n" + " primary_controller:\n" + " plugin: \n" + " ... other parameters ...\n" + "Please update your YAML configuration. " + "See the migration guide: " + // NOLINTNEXTLINE + "https://docs.nav2.org/migration/Kilted.html#namespace-added-for-primary-controller-parameters-in-rotation-shim-controller" + ); + throw nav2_core::ControllerException( + "Failed to get 'primary_controller.plugin' parameter"); + } + params_.rotate_to_goal_heading = node->declare_or_get_parameter(plugin_name_ + + ".rotate_to_goal_heading", false); + params_.rotate_to_heading_once = node->declare_or_get_parameter(plugin_name_ + + ".rotate_to_heading_once", false); + params_.closed_loop = node->declare_or_get_parameter(plugin_name_ + ".closed_loop", true); + params_.use_path_orientations = node->declare_or_get_parameter(plugin_name_ + + ".use_path_orientations", false); + double control_frequency = 20.0; + node->get_parameter("controller_frequency", control_frequency); + params_.control_duration = 1.0 / control_frequency; +} + +void ParameterHandler::activate() +{ + auto node = node_.lock(); + post_set_params_handler_ = node->add_post_set_parameters_callback( + std::bind( + &ParameterHandler::updateParametersCallback, + this, std::placeholders::_1)); + on_set_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &ParameterHandler::validateParameterUpdatesCallback, + this, std::placeholders::_1)); +} + +void ParameterHandler::deactivate() +{ + auto node = node_.lock(); + if (post_set_params_handler_ && node) { + node->remove_post_set_parameters_callback(post_set_params_handler_.get()); + } + post_set_params_handler_.reset(); + if (on_set_params_handler_ && node) { + node->remove_on_set_parameters_callback(on_set_params_handler_.get()); + } + on_set_params_handler_.reset(); +} + +ParameterHandler::~ParameterHandler() +{ +} +rcl_interfaces::msg::SetParametersResult ParameterHandler::validateParameterUpdatesCallback( + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const auto & parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find(plugin_name_ + ".") != 0) { + continue; + } + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == plugin_name_ + ".simulate_ahead_time" && + parameter.as_double() < 0.0) + { + RCLCPP_WARN( + logger_, "The value of simulate_ahead_time is incorrectly set, " + "it should be >=0. Ignoring parameter update."); + result.successful = false; + } else if (parameter.as_double() <= 0.0) { + RCLCPP_WARN( + logger_, "The value of parameter '%s' is incorrectly set to %f, " + "it should be >0. Ignoring parameter update.", + param_name.c_str(), parameter.as_double()); + result.successful = false; + } + } + } + return result; +} +void +ParameterHandler::updateParametersCallback( + const std::vector & parameters) +{ + std::lock_guard lock_reinit(mutex_); + + for (const auto & parameter : parameters) { + const auto & param_type = parameter.get_type(); + const auto & param_name = parameter.get_name(); + if (param_name.find(plugin_name_ + ".") != 0) { + continue; + } + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == plugin_name_ + ".angular_dist_threshold") { + params_.angular_dist_threshold = parameter.as_double(); + } else if (param_name == plugin_name_ + ".forward_sampling_distance") { + params_.forward_sampling_distance = parameter.as_double(); + } else if (param_name == plugin_name_ + ".rotate_to_heading_angular_vel") { + params_.rotate_to_heading_angular_vel = parameter.as_double(); + } else if (param_name == plugin_name_ + ".max_angular_accel") { + params_.max_angular_accel = parameter.as_double(); + } else if (param_name == plugin_name_ + ".simulate_ahead_time") { + params_.simulate_ahead_time = parameter.as_double(); + } + } else if (param_type == ParameterType::PARAMETER_BOOL) { + if (param_name == plugin_name_ + ".rotate_to_goal_heading") { + params_.rotate_to_goal_heading = parameter.as_bool(); + } else if (param_name == plugin_name_ + ".rotate_to_heading_once") { + params_.rotate_to_heading_once = parameter.as_bool(); + } else if (param_name == plugin_name_ + ".closed_loop") { + params_.closed_loop = parameter.as_bool(); + } else if (param_name == plugin_name_ + ".use_path_orientations") { + params_.use_path_orientations = parameter.as_bool(); + } + } + } +} + +} // namespace nav2_rotation_shim_controller diff --git a/nav2_rotation_shim_controller/test/test_shim_controller.cpp b/nav2_rotation_shim_controller/test/test_shim_controller.cpp index ee87908d827..f3dbcbe9a95 100644 --- a/nav2_rotation_shim_controller/test/test_shim_controller.cpp +++ b/nav2_rotation_shim_controller/test/test_shim_controller.cpp @@ -95,7 +95,7 @@ TEST(RotationShimControllerTest, lifecycleTransitions) node->get_node_services_interface()); auto results = rec_param->set_parameters_atomically( {rclcpp::Parameter( - "PathFollower.primary_controller", + "PathFollower.primary_controller.plugin", std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"))}); rclcpp::spin_until_future_complete( node->get_node_base_interface(), @@ -124,7 +124,7 @@ TEST(RotationShimControllerTest, setPlanAndSampledPointsTests) // set a valid primary controller so we can do lifecycle node->declare_parameter( - "PathFollower.primary_controller", + "PathFollower.primary_controller.plugin", std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); auto controller = std::make_shared(); @@ -173,7 +173,7 @@ TEST(RotationShimControllerTest, rotationAndTransformTests) // set a valid primary controller so we can do lifecycle node->declare_parameter( - "PathFollower.primary_controller", + "PathFollower.primary_controller.plugin", std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); node->declare_parameter("controller_frequency", 1.0); @@ -246,7 +246,7 @@ TEST(RotationShimControllerTest, computeVelocityTests) // set a valid primary controller so we can do lifecycle node->declare_parameter( - "PathFollower.primary_controller", + "PathFollower.primary_controller.plugin", std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); node->declare_parameter("controller_frequency", 1.0); @@ -331,7 +331,7 @@ TEST(RotationShimControllerTest, openLoopRotationTests) { // set a valid primary controller so we can do lifecycle node->declare_parameter( - "PathFollower.primary_controller", + "PathFollower.primary_controller.plugin", std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); node->declare_parameter( "controller_frequency", @@ -409,11 +409,13 @@ TEST(RotationShimControllerTest, computeVelocityGoalRotationTests) { // set a valid primary controller so we can do lifecycle node->declare_parameter( - "PathFollower.primary_controller", + "PathFollower.primary_controller.plugin", std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); node->declare_parameter( "PathFollower.rotate_to_goal_heading", true); + node->declare_parameter("controller_frequency", 1.0); + node->declare_parameter("PathFollower.primary_controller.use_collision_detection", false); auto controller = std::make_shared(); controller->configure(node, name, tf, costmap); @@ -482,7 +484,7 @@ TEST(RotationShimControllerTest, accelerationTests) { // set a valid primary controller so we can do lifecycle node->declare_parameter( - "PathFollower.primary_controller", + "PathFollower.primary_controller.plugin", std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); node->declare_parameter( "controller_frequency", @@ -561,7 +563,7 @@ TEST(RotationShimControllerTest, isGoalChangedTest) // set a valid primary controller so we can do lifecycle node->declare_parameter( - "PathFollower.primary_controller", + "PathFollower.primary_controller.plugin", std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); node->declare_parameter( "PathFollower.rotate_to_heading_once", @@ -600,7 +602,7 @@ TEST(RotationShimControllerTest, testDynamicParameter) // set a valid primary controller so we can do lifecycle node->declare_parameter( - "test.primary_controller", + "test.primary_controller.plugin", std::string("nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController")); auto controller = std::make_shared(); @@ -618,7 +620,7 @@ TEST(RotationShimControllerTest, testDynamicParameter) rclcpp::Parameter("test.rotate_to_heading_angular_vel", 7.0), rclcpp::Parameter("test.max_angular_accel", 7.0), rclcpp::Parameter("test.simulate_ahead_time", 7.0), - rclcpp::Parameter("test.primary_controller", std::string("HI")), + rclcpp::Parameter("test.primary_controller.plugin", std::string("HI")), rclcpp::Parameter("test.rotate_to_goal_heading", true), rclcpp::Parameter("test.rotate_to_heading_once", true), rclcpp::Parameter("test.closed_loop", false), @@ -637,6 +639,26 @@ TEST(RotationShimControllerTest, testDynamicParameter) EXPECT_EQ(node->get_parameter("test.rotate_to_heading_once").as_bool(), true); EXPECT_EQ(node->get_parameter("test.closed_loop").as_bool(), false); EXPECT_EQ(node->get_parameter("test.use_path_orientations").as_bool(), true); + + results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.angular_dist_threshold", -1.0)} + ); + + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + results); + + EXPECT_EQ(node->get_parameter("test.angular_dist_threshold").as_double(), 7.0); + + results = rec_param->set_parameters_atomically( + {rclcpp::Parameter("test.simulate_ahead_time", -0.1)} + ); + + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + results); + + EXPECT_EQ(node->get_parameter("test.simulate_ahead_time").as_double(), 7.0); } int main(int argc, char **argv)