chore: 增加配置文件启动的launch方法

saturn_controller/CMakeLists.txt

@@ -70,4 +70,9 @@ install(DIRECTORY
   launch
   DESTINATION share/${PROJECT_NAME}/)
 
+install(DIRECTORY
+  config
+  DESTINATION share/${PROJECT_NAME}/
+)
+
 ament_package()

saturn_controller/config/saturn_controller.yaml

@@ -0,0 +1,8 @@
+nav/saturn_controller:
+  ros__parameters:
+    # 基本类型参数
+    motion_computer_ip_address: '127.0.0.1'
+    main_timer_frequency: 50.0
+    imu_timer_frequency: 30.0
+    legged_odom_timer_frequency: 100.0
+    joints_state_timer_frequency: 5.0

saturn_controller/launch/saturn_ros2.launch.py

@@ -2,35 +2,77 @@ import os
 from ament_index_python.packages import get_package_share_directory
 import launch
 import launch_ros.actions
+from launch.substitutions import EnvironmentVariable
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
 
 def generate_launch_description():
+    config = os.path.join(
+        get_package_share_directory('saturn_controller'),
+        'config',
+        'saturn_controller.yaml'
+        )
+    print(f"config:{config}" )
 
-    robot_name = os.getenv('ROBOT_NAME', 'default_robot')
-    robot_type = os.getenv('ROBOT_TYPE', 'default_type')
-
-    print("Robot name: " + robot_name)
-    print("Robot type: " + robot_type)
-
-    component_manager_node = launch_ros.actions.Node(
+    # 创建组件容器
+    container = ComposableNodeContainer(
+        name='rclcpp_components',
+        namespace='nav',
         package='rclcpp_components',
         executable='component_container',
-        name='component_container',
-        output='screen'
-    )
-
-    composable_node = launch_ros.actions.LoadComposableNodes(
-        target_container='/component_container',
         composable_node_descriptions=[
-            launch_ros.descriptions.ComposableNode(
+            # 添加你的组件
+            ComposableNode(
                 package='saturn_controller',
                 plugin='saturn_ros2::SaturnController',
-                namespace = robot_name,
-                name='saturn_controller'
-            ),
-        ]
+                name='saturn_controller',
+                namespace='nav',
+                parameters=[config]
+                # remappings=[
+                #     ('input_topic', 'new_input_topic'),
+                #     ('output_topic', 'new_output_topic')
+                # ]
+            )
+        ],
+        output='screen'
     )
 
-    return launch.LaunchDescription([
-        component_manager_node,
-        composable_node
-    ])
+    return launch.LaunchDescription([container])
+
+# def generate_launch_description():
+#
+#     robot_name = EnvironmentVariable('ROBOT_NAME', default_value='')
+#
+#     # print("Robot name: " + robot_name)
+#
+#     config = os.path.join(
+#         get_package_share_directory('saturn_controller'),
+#         'config',
+#         'saturn_controller.yaml'
+#         )
+#     print(R"config:{config}" )
+#
+#     component_manager_node = launch_ros.actions.Node(
+#         package='rclcpp_components',
+#         executable='component_container',
+#         name='component_container',
+#         namespace = robot_name,
+#         output='screen'
+#     )
+#
+#     composable_node = launch_ros.actions.LoadComposableNodes(
+#         target_container='/component_container',
+#         composable_node_descriptions=[
+#             launch_ros.descriptions.ComposableNode(
+#                 package='saturn_controller',
+#                 plugin='saturn_ros2::SaturnController',
+#                 parameters=[config],
+#                 name='saturn_controller'
+#             ),
+#         ]
+#     )
+#
+#     return launch.LaunchDescription([
+#         component_manager_node,
+#         composable_node
+#     ])

saturn_controller/src/config_handler.cpp

@@ -11,13 +11,39 @@ ConfigHandler::ConfigHandler(std::shared_ptr<rclcpp::Node> node)
     loadConfig();
 }
 
+template <typename T>
+T declareOrGetParam(rclcpp::Node::SharedPtr node_, const std::string &param_name, T default_value)
+{
+    T output_value;
+    try
+    {
+        if (node_->has_parameter(param_name))
+            node_->get_parameter<T>(param_name, output_value);
+        else
+            output_value = node_->declare_parameter<T>(param_name, default_value);
+    }
+    catch (const rclcpp::exceptions::InvalidParameterTypeException &e)
+    {
+        RCLCPP_WARN_STREAM(node_->get_logger(), "InvalidParameterTypeException(" << param_name << "): " << e.what());
+        RCLCPP_ERROR_STREAM(node_->get_logger(), "Error getting parameter \'" << param_name << "\', check parameter type in YAML file");
+        throw e;
+    }
+    RCLCPP_INFO_STREAM(node_->get_logger(), "Found parameter - " << param_name << ": " << output_value);
+    return output_value;
+}
+
+
 void ConfigHandler::loadConfig() {
     // 声明参数
-    motion_computer_ip_address_ = node_->declare_parameter("motion_computer_ip_address", "127.0.0.1");
-
-    main_timer_frequency_ = node_->declare_parameter("main_timer_frequency", 50.0);
-    imu_timer_frequency_ = node_->declare_parameter("imu_timer_frequency", 300.0);
-    legged_odom_timer_frequency_ = node_->declare_parameter("legged_odom_timer_frequency", 100.0);
-    joints_state_timer_frequency_ = node_->declare_parameter("joints_state_timer_frequency", 5.0);
-}
+    motion_computer_ip_address_ = declareOrGetParam<std::string>(
+        node_, "motion_computer_ip_address", "192.168.144.103");
+    main_timer_frequency_ =
+        declareOrGetParam<float>(node_, "main_timer_frequency", 50.0);
+    imu_timer_frequency_ =
+        declareOrGetParam<float>(node_, "imu_timer_frequency", 30.0);
+    legged_odom_timer_frequency_ =
+        declareOrGetParam<float>(node_, "legged_odom_timer_frequency", 100.0);
+    joints_state_timer_frequency_ =
+        declareOrGetParam<float>(node_, "joints_state_timer_frequency", 5.0);
 }
+} // namespace saturn_ros2