给导航添加了动态调参的节点

src/navigation/obstacle_nav2/include/obstacle_nav2/nav2_profile_loader.hpp

@@ -0,0 +1,31 @@
+#ifndef OBSTACLE_NAV2__NAV2_PROFILE_LOADER_HPP_
+#define OBSTACLE_NAV2__NAV2_PROFILE_LOADER_HPP_
+
+#include <string>
+#include <set>
+#include <vector>
+
+#include "rclcpp/parameter.hpp"
+
+namespace obstacle_nav2
+{
+
+struct NodeParameterSet
+{
+  std::string node_name;
+  std::vector<rclcpp::Parameter> parameters;
+};
+
+struct Nav2Profile
+{
+  std::vector<NodeParameterSet> nodes;
+};
+
+Nav2Profile loadNav2Profile(const std::string & path);
+std::vector<rclcpp::Parameter> filterDeclaredParameters(
+  const std::vector<rclcpp::Parameter> & parameters,
+  const std::set<std::string> & declared_names);
+
+}  // namespace obstacle_nav2
+
+#endif  // OBSTACLE_NAV2__NAV2_PROFILE_LOADER_HPP_

src/navigation/obstacle_nav2/include/obstacle_nav2/trajectory_guard.hpp

@@ -0,0 +1,82 @@
+#ifndef OBSTACLE_NAV2__TRAJECTORY_GUARD_HPP_
+#define OBSTACLE_NAV2__TRAJECTORY_GUARD_HPP_
+
+#include <cstddef>
+#include <optional>
+#include <string>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+#include "nav_msgs/msg/path.hpp"
+
+namespace obstacle_nav2
+{
+
+struct CollisionCheckResult
+{
+  bool blocked{false};
+  std::size_t path_index{0};
+  std::string reason{"clear"};
+};
+
+struct GuardSettings
+{
+  double lookahead_distance{2.0};
+  double rejoin_min_distance{1.0};
+  double rejoin_max_distance{4.0};
+  int occupied_threshold{50};
+  bool treat_unknown_as_occupied{true};
+  double footprint_half_length{0.14};
+  double footprint_half_width{0.085};
+  double footprint_padding{0.04};
+  double footprint_sample_step{0.05};
+};
+
+double yawFromPose(const geometry_msgs::msg::PoseStamped & pose);
+
+double distance2d(
+  const geometry_msgs::msg::PoseStamped & a,
+  const geometry_msgs::msg::PoseStamped & b);
+
+std::size_t nearestPathIndex(
+  const nav_msgs::msg::Path & path,
+  const geometry_msgs::msg::PoseStamped & pose);
+
+std::size_t advanceByDistance(
+  const nav_msgs::msg::Path & path,
+  std::size_t start_index,
+  double distance_m);
+
+CollisionCheckResult checkPathAhead(
+  const nav_msgs::msg::Path & path,
+  std::size_t start_index,
+  const nav_msgs::msg::OccupancyGrid & costmap,
+  const GuardSettings & settings);
+
+std::optional<std::size_t> findClearRejoinIndex(
+  const nav_msgs::msg::Path & path,
+  std::size_t start_index,
+  const nav_msgs::msg::OccupancyGrid & costmap,
+  const GuardSettings & settings);
+
+bool shouldRetryBlockedRepair(
+  const std::optional<std::size_t> & last_repair_nearest_index,
+  std::size_t nearest_index,
+  int min_repair_progress_indices,
+  double seconds_since_last_repair,
+  double blocked_retry_wait_sec,
+  bool costmap_updated_since_repair);
+
+nav_msgs::msg::Path slicePath(
+  const nav_msgs::msg::Path & path,
+  std::size_t start_index,
+  std::size_t end_index_inclusive);
+
+nav_msgs::msg::Path stitchPaths(
+  const nav_msgs::msg::Path & bypass,
+  const nav_msgs::msg::Path & original,
+  std::size_t rejoin_index);
+
+}  // namespace obstacle_nav2
+
+#endif  // OBSTACLE_NAV2__TRAJECTORY_GUARD_HPP_

src/navigation/obstacle_nav2/launch/nav2_profile_tuner.launch.py

@@ -0,0 +1,48 @@
+#!/usr/bin/env python3
+
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    pkg_dir = get_package_share_directory('obstacle_nav2')
+    profile_10_path = os.path.join(pkg_dir, 'config', 'nav2_profile_10.yaml')
+    profile_11_path = os.path.join(pkg_dir, 'config', 'nav2_profile_11.yaml')
+
+    profile_10_yaml = LaunchConfiguration('profile_10_yaml')
+    profile_11_yaml = LaunchConfiguration('profile_11_yaml')
+    profile_trigger_topic = LaunchConfiguration('profile_trigger_topic')
+
+    nav2_profile_tuner = Node(
+        package='obstacle_nav2',
+        executable='nav2_profile_tuner',
+        name='nav2_profile_tuner',
+        output='screen',
+        parameters=[{
+            'profile_10_yaml': profile_10_yaml,
+            'profile_11_yaml': profile_11_yaml,
+            'trigger_topic': profile_trigger_topic,
+        }],
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument(
+            'profile_10_yaml',
+            default_value=profile_10_path,
+            description='YAML profile applied by default and when sign4return=10'),
+        DeclareLaunchArgument(
+            'profile_11_yaml',
+            default_value=profile_11_path,
+            description='YAML profile applied when sign4return=11'),
+        DeclareLaunchArgument(
+            'profile_trigger_topic',
+            default_value='/sign4return',
+            description='Int32 topic used to switch Nav2 runtime profiles'),
+
+        nav2_profile_tuner,
+    ])

src/navigation/obstacle_nav2/src/nav2_profile_loader.cpp

@@ -0,0 +1,238 @@
+#include "obstacle_nav2/nav2_profile_loader.hpp"
+
+#include <algorithm>
+#include <cctype>
+#include <cstdint>
+#include <regex>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#include "yaml-cpp/yaml.h"
+
+namespace obstacle_nav2
+{
+namespace
+{
+
+std::string lowerCopy(std::string value)
+{
+  std::transform(
+    value.begin(), value.end(), value.begin(),
+    [](unsigned char c) {return static_cast<char>(std::tolower(c));});
+  return value;
+}
+
+bool isBoolScalar(const std::string & value)
+{
+  const auto lower = lowerCopy(value);
+  return lower == "true" || lower == "false";
+}
+
+bool isIntegerScalar(const std::string & value)
+{
+  static const std::regex pattern(R"(^[-+]?[0-9]+$)");
+  return std::regex_match(value, pattern);
+}
+
+bool isDoubleScalar(const std::string & value)
+{
+  static const std::regex pattern(
+    R"(^[-+]?(([0-9]+\.?[0-9]*)|(\.[0-9]+))([eE][-+]?[0-9]+)?$)");
+  return std::regex_match(value, pattern);
+}
+
+rclcpp::Parameter scalarToParameter(const std::string & name, const YAML::Node & node)
+{
+  const auto value = node.Scalar();
+  if (isBoolScalar(value)) {
+    return rclcpp::Parameter(name, node.as<bool>());
+  }
+  if (isIntegerScalar(value)) {
+    return rclcpp::Parameter(name, static_cast<int64_t>(node.as<int64_t>()));
+  }
+  if (isDoubleScalar(value)) {
+    return rclcpp::Parameter(name, node.as<double>());
+  }
+  return rclcpp::Parameter(name, value);
+}
+
+rclcpp::Parameter sequenceToParameter(const std::string & name, const YAML::Node & node)
+{
+  std::vector<std::string> values;
+  values.reserve(node.size());
+  for (const auto & item : node) {
+    if (!item.IsScalar()) {
+      throw std::runtime_error("Parameter array '" + name + "' contains a non-scalar value");
+    }
+    values.push_back(item.Scalar());
+  }
+
+  const auto all_bool = std::all_of(values.begin(), values.end(), isBoolScalar);
+  if (all_bool) {
+    std::vector<bool> parsed;
+    parsed.reserve(values.size());
+    for (const auto & value : values) {
+      parsed.push_back(YAML::Load(value).as<bool>());
+    }
+    return rclcpp::Parameter(name, parsed);
+  }
+
+  const auto all_integer = std::all_of(values.begin(), values.end(), isIntegerScalar);
+  if (all_integer) {
+    std::vector<int64_t> parsed;
+    parsed.reserve(values.size());
+    for (const auto & value : values) {
+      parsed.push_back(YAML::Load(value).as<int64_t>());
+    }
+    return rclcpp::Parameter(name, parsed);
+  }
+
+  const auto all_double = std::all_of(values.begin(), values.end(), isDoubleScalar);
+  if (all_double) {
+    std::vector<double> parsed;
+    parsed.reserve(values.size());
+    for (const auto & value : values) {
+      parsed.push_back(YAML::Load(value).as<double>());
+    }
+    return rclcpp::Parameter(name, parsed);
+  }
+
+  return rclcpp::Parameter(name, values);
+}
+
+rclcpp::Parameter yamlToParameter(const std::string & name, const YAML::Node & node)
+{
+  if (node.IsScalar()) {
+    return scalarToParameter(name, node);
+  }
+  if (node.IsSequence()) {
+    return sequenceToParameter(name, node);
+  }
+  throw std::runtime_error("Parameter '" + name + "' must be a scalar or sequence");
+}
+
+void appendFlattenedParameters(
+  const YAML::Node & parameters,
+  const std::string & prefix,
+  std::vector<rclcpp::Parameter> & output)
+{
+  if (!parameters.IsMap()) {
+    throw std::runtime_error("ros__parameters must contain a parameter map");
+  }
+
+  for (const auto & parameter_entry : parameters) {
+    const auto key = parameter_entry.first.as<std::string>();
+    if (key.empty()) {
+      throw std::runtime_error("ros__parameters contains an empty parameter name");
+    }
+
+    const auto parameter_name = prefix.empty() ? key : prefix + "." + key;
+    const auto value = parameter_entry.second;
+    if (value.IsMap()) {
+      appendFlattenedParameters(value, parameter_name, output);
+    } else {
+      output.push_back(yamlToParameter(parameter_name, value));
+    }
+  }
+}
+
+NodeParameterSet loadNodeParameterSet(
+  const std::string & node_name,
+  const YAML::Node & parameters)
+{
+  if (node_name.empty()) {
+    throw std::runtime_error("Nav2 profile contains an empty node name");
+  }
+  if (!parameters.IsMap()) {
+    throw std::runtime_error("Node '" + node_name + "' must contain a parameter map");
+  }
+
+  NodeParameterSet node_set;
+  node_set.node_name = node_name;
+  for (const auto & parameter_entry : parameters) {
+    const auto parameter_name = parameter_entry.first.as<std::string>();
+    if (parameter_name.empty()) {
+      throw std::runtime_error("Node '" + node_name + "' contains an empty parameter name");
+    }
+    node_set.parameters.push_back(yamlToParameter(parameter_name, parameter_entry.second));
+  }
+  return node_set;
+}
+
+bool isNativeRuntimeProfileNode(const std::string & path)
+{
+  return path == "controller_server" || path == "velocity_smoother";
+}
+
+void collectRosParameterNodes(
+  const YAML::Node & yaml_node,
+  const std::string & path,
+  Nav2Profile & profile)
+{
+  if (!yaml_node.IsMap()) {
+    return;
+  }
+
+  const auto ros_parameters = yaml_node["ros__parameters"];
+  if (ros_parameters) {
+    if (path.empty()) {
+      throw std::runtime_error("ros__parameters must be nested under a node name");
+    }
+
+    NodeParameterSet node_set;
+    node_set.node_name = path.front() == '/' ? path : "/" + path;
+    appendFlattenedParameters(ros_parameters, "", node_set.parameters);
+    if (isNativeRuntimeProfileNode(path)) {
+      profile.nodes.push_back(node_set);
+    }
+    return;
+  }
+
+  for (const auto & child_entry : yaml_node) {
+    const auto key = child_entry.first.as<std::string>();
+    const auto child_path = path.empty() ? key : path + "/" + key;
+    collectRosParameterNodes(child_entry.second, child_path, profile);
+  }
+}
+
+}  // namespace
+
+Nav2Profile loadNav2Profile(const std::string & path)
+{
+  const auto root = YAML::LoadFile(path);
+  const auto nodes = root["nodes"];
+
+  Nav2Profile profile;
+  if (nodes && nodes.IsMap()) {
+    for (const auto & node_entry : nodes) {
+      const auto node_name = node_entry.first.as<std::string>();
+      profile.nodes.push_back(loadNodeParameterSet(node_name, node_entry.second));
+    }
+    return profile;
+  }
+
+  collectRosParameterNodes(root, "", profile);
+  if (profile.nodes.empty()) {
+    throw std::runtime_error(
+      "Nav2 profile '" + path + "' must contain a nodes map or ros__parameters blocks");
+  }
+
+  return profile;
+}
+
+std::vector<rclcpp::Parameter> filterDeclaredParameters(
+  const std::vector<rclcpp::Parameter> & parameters,
+  const std::set<std::string> & declared_names)
+{
+  std::vector<rclcpp::Parameter> filtered;
+  filtered.reserve(parameters.size());
+  for (const auto & parameter : parameters) {
+    if (declared_names.count(parameter.get_name()) > 0) {
+      filtered.push_back(parameter);
+    }
+  }
+  return filtered;
+}
+
+}  // namespace obstacle_nav2

src/navigation/obstacle_nav2/src/nav2_profile_tuner.cpp

@@ -0,0 +1,210 @@
+#include <chrono>
+#include <map>
+#include <memory>
+#include <set>
+#include <string>
+
+#include "obstacle_nav2/nav2_profile_loader.hpp"
+#include "rcl_interfaces/srv/list_parameters.hpp"
+#include "rcl_interfaces/srv/set_parameters.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/int32.hpp"
+
+namespace obstacle_nav2
+{
+namespace
+{
+
+std::string setParametersServiceName(const std::string & node_name)
+{
+  if (node_name.empty()) {
+    return "/set_parameters";
+  }
+  if (node_name.front() == '/') {
+    return node_name + "/set_parameters";
+  }
+  return "/" + node_name + "/set_parameters";
+}
+
+std::string listParametersServiceName(const std::string & node_name)
+{
+  if (node_name.empty()) {
+    return "/list_parameters";
+  }
+  if (node_name.front() == '/') {
+    return node_name + "/list_parameters";
+  }
+  return "/" + node_name + "/list_parameters";
+}
+
+}  // namespace
+
+class Nav2ProfileTuner : public rclcpp::Node
+{
+public:
+  Nav2ProfileTuner()
+  : Node("nav2_profile_tuner")
+  {
+    const auto profile_10_path = declare_parameter<std::string>("profile_10_yaml", "");
+    const auto profile_11_path = declare_parameter<std::string>("profile_11_yaml", "");
+    const auto trigger_topic = declare_parameter<std::string>("trigger_topic", "/sign4return");
+
+    if (profile_10_path.empty() || profile_11_path.empty()) {
+      throw std::runtime_error("profile_10_yaml and profile_11_yaml must both be set");
+    }
+
+    profiles_[10] = loadNav2Profile(profile_10_path);
+    profiles_[11] = loadNav2Profile(profile_11_path);
+
+    trigger_sub_ = create_subscription<std_msgs::msg::Int32>(
+      trigger_topic, 10,
+      [this](const std_msgs::msg::Int32::SharedPtr msg) {
+        handleTrigger(msg->data);
+      });
+
+    retry_timer_ = create_wall_timer(
+      std::chrono::milliseconds(500),
+      [this]() {
+        tryApplyPendingProfile();
+      });
+
+    requestProfile(10);
+  }
+
+private:
+  using SetParameters = rcl_interfaces::srv::SetParameters;
+  using ListParameters = rcl_interfaces::srv::ListParameters;
+
+  void handleTrigger(const int32_t command)
+  {
+    if (command == 10 || command == 11) {
+      requestProfile(command);
+      return;
+    }
+    RCLCPP_DEBUG(get_logger(), "Ignoring sign4return=%d", command);
+  }
+
+  void requestProfile(const int command)
+  {
+    if (active_profile_ == command && !has_pending_profile_) {
+      return;
+    }
+    pending_profile_ = command;
+    has_pending_profile_ = true;
+    RCLCPP_INFO(get_logger(), "Queued Nav2 profile %d", command);
+    tryApplyPendingProfile();
+  }
+
+  void tryApplyPendingProfile()
+  {
+    if (!has_pending_profile_) {
+      return;
+    }
+
+    const auto profile_it = profiles_.find(pending_profile_);
+    if (profile_it == profiles_.end()) {
+      RCLCPP_ERROR(get_logger(), "No Nav2 profile loaded for command %d", pending_profile_);
+      has_pending_profile_ = false;
+      return;
+    }
+
+    for (const auto & node_set : profile_it->second.nodes) {
+      const auto set_service_name = setParametersServiceName(node_set.node_name);
+      auto & set_client = set_clients_[set_service_name];
+      if (!set_client) {
+        set_client = create_client<SetParameters>(set_service_name);
+      }
+      if (!set_client->wait_for_service(std::chrono::milliseconds(0))) {
+        RCLCPP_WARN_THROTTLE(
+          get_logger(), *get_clock(), 3000,
+          "Waiting for parameter service %s", set_service_name.c_str());
+        return;
+      }
+
+      const auto list_service_name = listParametersServiceName(node_set.node_name);
+      auto & list_client = list_clients_[list_service_name];
+      if (!list_client) {
+        list_client = create_client<ListParameters>(list_service_name);
+      }
+      if (!list_client->wait_for_service(std::chrono::milliseconds(0))) {
+        RCLCPP_WARN_THROTTLE(
+          get_logger(), *get_clock(), 3000,
+          "Waiting for parameter service %s", list_service_name.c_str());
+        return;
+      }
+    }
+
+    const auto command = pending_profile_;
+    for (const auto & node_set : profile_it->second.nodes) {
+      const auto list_service_name = listParametersServiceName(node_set.node_name);
+      auto request = std::make_shared<ListParameters::Request>();
+      request->depth = ListParameters::Request::DEPTH_RECURSIVE;
+
+      list_clients_.at(list_service_name)->async_send_request(
+        request,
+        [this, node_set](rclcpp::Client<ListParameters>::SharedFuture future) {
+          const auto response = future.get();
+          const std::set<std::string> declared_names(
+            response->result.names.begin(), response->result.names.end());
+          const auto parameters = filterDeclaredParameters(node_set.parameters, declared_names);
+
+          if (parameters.empty()) {
+            RCLCPP_WARN(
+              get_logger(), "No declared parameters from profile for %s",
+              node_set.node_name.c_str());
+            return;
+          }
+
+          auto set_request = std::make_shared<SetParameters::Request>();
+          set_request->parameters.reserve(parameters.size());
+          for (const auto & parameter : parameters) {
+            set_request->parameters.push_back(parameter.to_parameter_msg());
+          }
+
+          const auto set_service_name = setParametersServiceName(node_set.node_name);
+          set_clients_.at(set_service_name)->async_send_request(
+            set_request,
+            [this, node_name = node_set.node_name](
+              rclcpp::Client<SetParameters>::SharedFuture set_future) {
+              const auto set_response = set_future.get();
+              for (const auto & result : set_response->results) {
+                if (!result.successful) {
+                  RCLCPP_WARN(
+                    get_logger(), "Parameter set failed on %s: %s",
+                    node_name.c_str(), result.reason.c_str());
+                }
+              }
+            });
+        });
+    }
+
+    active_profile_ = command;
+    has_pending_profile_ = false;
+    RCLCPP_INFO(get_logger(), "Applied Nav2 profile %d", active_profile_);
+  }
+
+  std::map<int, Nav2Profile> profiles_;
+  std::map<std::string, rclcpp::Client<SetParameters>::SharedPtr> set_clients_;
+  std::map<std::string, rclcpp::Client<ListParameters>::SharedPtr> list_clients_;
+  rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr trigger_sub_;
+  rclcpp::TimerBase::SharedPtr retry_timer_;
+  int active_profile_{0};
+  int pending_profile_{0};
+  bool has_pending_profile_{false};
+};
+
+}  // namespace obstacle_nav2
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  try {
+    rclcpp::spin(std::make_shared<obstacle_nav2::Nav2ProfileTuner>());
+  } catch (const std::exception & e) {
+    RCLCPP_FATAL(rclcpp::get_logger("nav2_profile_tuner"), "%s", e.what());
+    rclcpp::shutdown();
+    return 1;
+  }
+  rclcpp::shutdown();
+  return 0;
+}

src/navigation/obstacle_nav2/test/test_nav2_profile_loader.cpp

@@ -0,0 +1,127 @@
+#include <gtest/gtest.h>
+
+#include <fstream>
+#include <set>
+#include <string>
+
+#include "obstacle_nav2/nav2_profile_loader.hpp"
+
+namespace
+{
+
+std::string writeTempProfile(const std::string & yaml)
+{
+  const auto path = "/tmp/nav2_profile_loader_test.yaml";
+  std::ofstream out(path);
+  out << yaml;
+  out.close();
+  return path;
+}
+
+}  // namespace
+
+TEST(Nav2ProfileLoaderTest, LoadsScalarAndArrayParameters)
+{
+  const auto path = writeTempProfile(R"(
+nodes:
+  /controller_server:
+    FollowPath.vx_max: 0.35
+    FollowPath.vx_min: -0.10
+    FollowPath.wz_max: 1.2
+    FollowPath.AckermannConstraints.min_turning_r: 0.75
+  /velocity_smoother:
+    max_velocity: [0.35, 0.0, 1.2]
+    min_velocity: [-0.10, 0.0, -1.2]
+)");
+
+  const auto profile = obstacle_nav2::loadNav2Profile(path);
+
+  ASSERT_EQ(profile.nodes.size(), 2u);
+  EXPECT_EQ(profile.nodes[0].node_name, "/controller_server");
+  ASSERT_EQ(profile.nodes[0].parameters.size(), 4u);
+  EXPECT_EQ(profile.nodes[0].parameters[0].get_name(), "FollowPath.vx_max");
+  EXPECT_DOUBLE_EQ(profile.nodes[0].parameters[0].as_double(), 0.35);
+  EXPECT_EQ(
+    profile.nodes[0].parameters[3].get_name(),
+    "FollowPath.AckermannConstraints.min_turning_r");
+  EXPECT_DOUBLE_EQ(profile.nodes[0].parameters[3].as_double(), 0.75);
+
+  EXPECT_EQ(profile.nodes[1].node_name, "/velocity_smoother");
+  ASSERT_EQ(profile.nodes[1].parameters.size(), 2u);
+  const auto max_velocity = profile.nodes[1].parameters[0].as_double_array();
+  ASSERT_EQ(max_velocity.size(), 3u);
+  EXPECT_DOUBLE_EQ(max_velocity[0], 0.35);
+  EXPECT_DOUBLE_EQ(max_velocity[2], 1.2);
+}
+
+TEST(Nav2ProfileLoaderTest, LoadsNativeNav2RosParametersFile)
+{
+  const auto path = writeTempProfile(R"(
+bt_navigator:
+  ros__parameters:
+    default_server_timeout: 20
+controller_server:
+  ros__parameters:
+    controller_frequency: 20.0
+    FollowPath:
+      vx_max: 0.35
+      wz_max: 1.2
+      motion_model: "Ackermann"
+      critics: ["ConstraintCritic", "CostCritic"]
+      AckermannConstraints:
+        min_turning_r: 0.75
+velocity_smoother:
+  ros__parameters:
+    max_velocity: [0.35, 0.0, 1.2]
+    min_velocity: [-0.10, 0.0, -1.2]
+)");
+
+  const auto profile = obstacle_nav2::loadNav2Profile(path);
+
+  ASSERT_EQ(profile.nodes.size(), 2u);
+  EXPECT_EQ(profile.nodes[0].node_name, "/controller_server");
+  ASSERT_EQ(profile.nodes[0].parameters.size(), 6u);
+  EXPECT_EQ(profile.nodes[0].parameters[1].get_name(), "FollowPath.vx_max");
+  EXPECT_DOUBLE_EQ(profile.nodes[0].parameters[1].as_double(), 0.35);
+  EXPECT_EQ(
+    profile.nodes[0].parameters[5].get_name(),
+    "FollowPath.AckermannConstraints.min_turning_r");
+  EXPECT_DOUBLE_EQ(profile.nodes[0].parameters[5].as_double(), 0.75);
+
+  EXPECT_EQ(profile.nodes[1].node_name, "/velocity_smoother");
+  ASSERT_EQ(profile.nodes[1].parameters.size(), 2u);
+  const auto max_velocity = profile.nodes[1].parameters[0].as_double_array();
+  ASSERT_EQ(max_velocity.size(), 3u);
+  EXPECT_DOUBLE_EQ(max_velocity[2], 1.2);
+}
+
+TEST(Nav2ProfileLoaderTest, RejectsProfileWithoutNodesMap)
+{
+  const auto path = writeTempProfile(R"(
+controller_server:
+  FollowPath.vx_max: 0.35
+)");
+
+  EXPECT_THROW(
+    obstacle_nav2::loadNav2Profile(path),
+    std::runtime_error);
+}
+
+TEST(Nav2ProfileLoaderTest, FiltersParametersByDeclaredNames)
+{
+  const std::vector<rclcpp::Parameter> parameters{
+    rclcpp::Parameter("FollowPath.vx_max", 0.35),
+    rclcpp::Parameter("FollowPath.CostCritic.trajectory_point_step", 2),
+    rclcpp::Parameter("max_velocity", std::vector<double>{0.35, 0.0, 1.2}),
+  };
+  const std::set<std::string> declared_names{
+    "FollowPath.vx_max",
+    "max_velocity",
+  };
+
+  const auto filtered = obstacle_nav2::filterDeclaredParameters(parameters, declared_names);
+
+  ASSERT_EQ(filtered.size(), 2u);
+  EXPECT_EQ(filtered[0].get_name(), "FollowPath.vx_max");
+  EXPECT_EQ(filtered[1].get_name(), "max_velocity");
+}