add launch files

2026-01-04 16:40:30 +08:00
parent c9fc59119e
commit 4ec5ff7a05
10 changed files with 335 additions and 42 deletions
--- a/src/brain/CMakeLists.txt
+++ b/src/brain/CMakeLists.txt
@@ -80,6 +80,9 @@ install(TARGETS brain_node
 install(DIRECTORY config/
  DESTINATION share/${PROJECT_NAME}/config)

+install(DIRECTORY launch/
+  DESTINATION share/${PROJECT_NAME}/launch)
+
 set (BUILD_TESTING OFF)

 if(BUILD_TESTING)
--- a/src/brain/config/brain_node_params.yaml
+++ b/src/brain/config/brain_node_params.yaml
@@ -0,0 +1,74 @@
+# Configuration file for brain node parameters
+# This file contains configurable parameters for cerebrum and cerebellum nodes
+
+cerebrum_node:
+  ros__parameters:
+    # Timing parameters
+    tick_rate_hz: 10.0
+    rebuild_interval_ms: 5000
+    stats_publish_interval_ms: 10000
+    
+    # Configuration paths
+    robot_config_path: ""
+    skill_file_path: ""
+    
+    # Behavior tree parameters
+    default_bt_timeout: 300.0
+    enable_sequence_rebuild: true
+    
+    # Original cerebrum parameters
+    node_timeout_sec: 60.0
+    allow_bt_rebuild_during_execution: false
+    allowed_action_skills: ""
+    fixed_skill_sequence: ""
+    random_skill_pick_count: 5
+    
+    # Dynamic skill parameters (examples - these would be set per skill)
+    # cerebrum:
+    #   bt:
+    #     VisionObjectRecognition:
+    #       topic: "vision_object_recognition"
+    #       payload_yaml: ""
+    #       timeout_ms: "10000"
+    #     Arm:
+    #       topic: "arm_control"
+    #       payload_yaml: ""
+    #       timeout_ms: "30000"
+    #     # Example instance-specific overrides
+    #     Arm:
+    #       s1_arm_move_to_object:
+    #         topic: "arm_control_specific"
+    #         payload_yaml: "{target_pose: {x: 0.1, y: 0.2, z: 0.3}}"
+    #         timeout_ms: "45000"
+
+cerebellum_node:
+  ros__parameters:
+    # Action execution parameters
+    abort_on_failure: true
+    default_action_timeout_sec: 45.0
+    vision_grasp_object_timeout_sec: 120.0
+    
+    # Map parameters
+    map_save_url: "/tmp/humanoid_map"
+    map_save_image_format: "pgm"
+    map_save_free_thresh: 0.25
+    map_save_occupied_thresh: 0.65
+    map_load_url: ""
+    nav_goal_yaw_offset: 0.0
+    nav_goal_distance_tolerance: 0.5
+    slam_enable_mapping_default: true
+    
+    # Configuration paths
+    robot_config_path: ""
+    robot_skill_file_path: ""
+    
+    # Statistics parameters
+    enable_stats_publisher: true
+    stats_publish_interval_ms: 5000
+    
+    # Skill-specific timeouts (format: "SkillA=30,SkillB=10")
+    skill_timeouts: ""
+    
+    # Arm-specific parameters
+    arm_target_frame: "base_link"
+    arm_transform_timeout_sec: 2.0
--- a/src/brain/config/example_skill_config.yaml
+++ b/src/brain/config/example_skill_config.yaml
@@ -0,0 +1,55 @@
+# Example configuration file showing how to configure skill-specific parameters
+# This can be used as an additional parameters file in addition to the main brain_node_params.yaml
+
+cerebrum_node:
+  ros__parameters:
+    # Skill-specific parameters
+    cerebrum:
+      bt:
+        VisionObjectRecognition:
+          topic: "vision_object_recognition"
+          payload_yaml: ""
+          timeout_ms: "10000"
+        Arm:
+          topic: "arm_control"
+          payload_yaml: ""
+          timeout_ms: "30000"
+        CameraTakePhoto:
+          topic: "camera_take_photo"
+          payload_yaml: ""
+          timeout_ms: "5000"
+        HandControl:
+          topic: "hand_control"
+          payload_yaml: ""
+          timeout_ms: "5000"
+        MoveWaist:
+          topic: "move_waist"
+          payload_yaml: ""
+          timeout_ms: "10000"
+        MoveLeg:
+          topic: "move_leg"
+          payload_yaml: ""
+          timeout_ms: "15000"
+        MoveWheel:
+          topic: "move_wheel"
+          payload_yaml: ""
+          timeout_ms: "20000"
+        MoveHome:
+          topic: "move_home"
+          payload_yaml: ""
+          timeout_ms: "30000"
+        GripperCmd:
+          topic: "gripper_cmd0"
+          payload_yaml: ""
+          timeout_ms: "5000"
+        
+        # Example instance-specific overrides
+        Arm:
+          s1_arm_move_to_object:
+            topic: "arm_control_specific"
+            payload_yaml: "{target_pose: {x: 0.1, y: 0.2, z: 0.3}}"
+            timeout_ms: "45000"
+          s2_arm_grasp_object:
+            topic: "arm_control_grasp"
+            payload_yaml: "{target_pose: {x: 0.15, y: 0.25, z: 0.35}, grip_force: 10.0}"
+            timeout_ms: "50000"
--- a/src/brain/include/brain/cerebellum_node.hpp
+++ b/src/brain/include/brain/cerebellum_node.hpp
@@ -80,18 +80,18 @@ private:
  // SMACC2 execution context
  std::unique_ptr<smacc2::SmExecution> sm_exec_;

-  // Parameters: abort policy, default action timeout, specific grasp timeout
-  bool abort_on_failure_ {true};
-  double default_action_timeout_sec_ {45.0};
-  double vision_grasp_object_timeout_sec_ {120.0};
-  std::string map_save_url_ {"/tmp/humanoid_map"};
-  std::string map_save_image_format_ {"pgm"};
-  double map_save_free_thresh_ {0.25};
-  double map_save_occupied_thresh_ {0.65};
-  std::string map_load_url_ {};
-  double nav_goal_yaw_offset_ {0.0};
-  double nav_goal_distance_tolerance_ {0.5};
-  bool slam_enable_mapping_default_ {true};
+  // ROS parameters (replacing hardcoded defaults)
+  bool abort_on_failure_{true};
+  double default_action_timeout_sec_{45.0};
+  double vision_grasp_object_timeout_sec_{120.0};
+  std::string map_save_url_{"/tmp/humanoid_map"};
+  std::string map_save_image_format_{"pgm"};
+  double map_save_free_thresh_{0.25};
+  double map_save_occupied_thresh_{0.65};
+  std::string map_load_url_{};
+  double nav_goal_yaw_offset_{0.0};
+  double nav_goal_distance_tolerance_{0.5};
+  bool slam_enable_mapping_default_{true};

  // Optional per-skill override timeout (skill_name -> seconds)
  std::unordered_map<std::string, double> skill_timeouts_;
@@ -99,8 +99,14 @@ private:
  // Statistics: per-skill success/failure counters & sequence totals
  std::unordered_map<std::string, size_t> skill_success_counts_;
  std::unordered_map<std::string, size_t> skill_failure_counts_;
-  size_t total_sequence_success_ {0};
-  size_t total_sequence_failure_ {0};
+  size_t total_sequence_success_{0};
+  size_t total_sequence_failure_{0};
+
+  // ROS parameters (new additions)
+  bool enable_stats_publisher_{true};
+  int stats_publish_interval_ms_{5000};
+  std::string robot_config_path_{""};
+
  rclcpp::TimerBase::SharedPtr stats_timer_;
  double stats_log_interval_sec_ {30.0};
  std::mutex stats_mutex_;
@@ -324,7 +330,7 @@ private:
  * @param detail [out] Detail / diagnostics.
  * @param start_progress Sequence progress prior to executing the service.
  * @param goal_handle Goal handle (used for feedback publishing).
-   * @return true if successful.
+  * @return true if successful.
   */
  bool ExecuteServiceSkill(
    const std::string & skill,
--- a/src/brain/include/brain/cerebrum_node.hpp
+++ b/src/brain/include/brain/cerebrum_node.hpp
@@ -265,6 +265,16 @@ private:
  std::vector<std::string> allowed_action_skills_;
  // Fixed skill sequence (if set, random selection is skipped entirely).
  std::optional<std::vector<std::string>> fixed_skill_sequence_;
+  
+  // ROS parameters
+  double tick_rate_hz_{10.0};
+  int rebuild_interval_ms_{5000};
+  int stats_publish_interval_ms_{10000};
+  std::string robot_config_path_{""};
+  std::string skill_file_path_{""};
+  double default_bt_timeout_{300.0};
+  bool enable_sequence_rebuild_{true};
+
  /**
   * @brief Cancel the currently active ExecuteBtAction goal (if any).
   * @thread_safety Calls into ActionClientRegistry; assumed thread-safe for cancellation.
--- a/src/brain/launch/brain_launch.py
+++ b/src/brain/launch/brain_launch.py
@@ -0,0 +1,42 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Declare launch arguments
+    declare_params_file_cmd = DeclareLaunchArgument(
+        'params_file',
+        default_value=PathJoinSubstitution([
+            FindPackageShare('brain'),
+            'config',
+            'brain_node_params.yaml'
+        ]),
+        description='Full path to params file to load'
+    )
+
+    # Get the params file path
+    params_file = LaunchConfiguration('params_file')
+
+    # Create the brain node that runs both cerebrum and cerebellum
+    brain_node = Node(
+        package='brain',
+        executable='brain_node',
+        # name='brain_node',
+        parameters=[params_file],
+        output='screen',
+        emulate_tty=True
+    )
+
+    # Create the launch description
+    ld = LaunchDescription()
+
+    # Declare launch arguments
+    ld.add_action(declare_params_file_cmd)
+
+    # Add nodes
+    ld.add_action(brain_node)
+
+    return ld
--- a/src/brain/launch/brain_with_params_launch.py
+++ b/src/brain/launch/brain_with_params_launch.py
@@ -0,0 +1,56 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from launch.conditions import IfCondition
+import os
+
+
+def launch_setup(context, *args, **kwargs):
+    # Get the params file path
+    params_file = LaunchConfiguration('params_file').perform(context)
+    
+    # Create the brain node that runs both cerebrum and cerebellum
+    brain_node = Node(
+        package='brain',
+        executable='brain_node',
+        name='brain_node',
+        parameters=[params_file],
+        output='screen',
+        emulate_tty=True,
+        # Override specific parameters if needed
+        arguments=[
+            # Example of overriding individual parameters via command line
+            # Uncomment and modify as needed
+            # "--ros-args", 
+            # "-p", "tick_rate_hz:=20.0",
+            # "-p", "default_action_timeout_sec:=60.0"
+        ]
+    )
+
+    return [brain_node]
+
+
+def generate_launch_description():
+    # Declare launch arguments
+    declare_params_file_cmd = DeclareLaunchArgument(
+        'params_file',
+        default_value=PathJoinSubstitution([
+            FindPackageShare('brain'),
+            'config',
+            'brain_node_params.yaml'
+        ]),
+        description='Full path to params file to load'
+    )
+
+    # Create the launch description
+    ld = LaunchDescription()
+
+    # Declare launch arguments
+    ld.add_action(declare_params_file_cmd)
+
+    # Add the opaque function to set up the nodes
+    ld.add_action(OpaqueFunction(function=launch_setup))
+
+    return ld
--- a/src/brain/src/brain_node.cpp
+++ b/src/brain/src/brain_node.cpp
@@ -25,8 +25,11 @@ int main(int argc, char ** argv)
  rclcpp::Logger logger = rclcpp::get_logger("brain_node");
  RCLCPP_INFO(logger, "Starting brain node...");

-  auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+  // Create node options with parameter overrides if provided
  rclcpp::NodeOptions options;
+  // If command line arguments are provided, they will be handled by rclcpp::init
+  
+  auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();

  auto cerebellum_node = std::make_shared<brain::CerebellumNode>(options);
  auto cerebrum_node = std::make_shared<brain::CerebrumNode>(options);
--- a/src/brain/src/cerebellum_node.cpp
+++ b/src/brain/src/cerebellum_node.cpp
@@ -75,13 +75,23 @@ CerebellumNode::CerebellumNode(const rclcpp::NodeOptions & options)
 {
  RCLCPP_INFO(this->get_logger(), "Cerebellum node started");

-  robot_config_ = std::make_unique<brain::robot_config::RobotConfig>();
+  // Use parameter-based path if available, otherwise default to package share directory
+  std::string config_path = robot_config_path_.empty() 
+    ? share_directory_ + "/config/robot_config.yaml" 
+    : robot_config_path_;
+  
+  robot_config_ = std::make_unique<brain::robot_config::RobotConfig>(config_path);
  auto skill_file = robot_config_->SkillFile("brain");
  if (skill_file == std::nullopt) {
    RCLCPP_ERROR(this->get_logger(), "No skill_file entry found for 'brain' in robot_config.yaml");
    return;
  }
-  robot_skill_file_path_ = share_directory_ + *skill_file;
+  
+  // Use parameter-based skill file path if available
+  robot_skill_file_path_ = robot_skill_file_path_.empty() 
+    ? share_directory_ + *skill_file 
+    : robot_skill_file_path_;
+    
  RCLCPP_WARN(this->get_logger(), "skill file %s", robot_skill_file_path_.c_str());

  tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
@@ -125,40 +135,46 @@ CerebellumNode::CerebellumNode(const rclcpp::NodeOptions & options)
 void CerebellumNode::DeclareAndLoadParameters()
 {
  // Core behavior parameters
-  this->declare_parameter<std::string>("skills_file", "");
+  this->declare_parameter<std::string>("robot_config_path", "");
+  this->declare_parameter<std::string>("robot_skill_file_path", "");
  this->declare_parameter<bool>("abort_on_failure", abort_on_failure_);
  this->declare_parameter<double>("default_action_timeout_sec", default_action_timeout_sec_);
  this->declare_parameter<double>(
    "vision_grasp_object_timeout_sec",
    vision_grasp_object_timeout_sec_);
  this->declare_parameter<std::string>("skill_timeouts", ""); // format: SkillA=30,SkillB=10
-  this->declare_parameter<double>("stats_log_interval_sec", stats_log_interval_sec_);
-  // this->declare_parameter<std::string>("skill_topic_prefix", "humanoid");
+  this->declare_parameter<bool>("enable_stats_publisher", enable_stats_publisher_);
+  this->declare_parameter<int>("stats_publish_interval_ms", stats_publish_interval_ms_);
  this->declare_parameter<std::string>("map_save_url", map_save_url_);
  this->declare_parameter<std::string>("map_save_image_format", map_save_image_format_);
  this->declare_parameter<double>("map_save_free_thresh", map_save_free_thresh_);
  this->declare_parameter<double>("map_save_occupied_thresh", map_save_occupied_thresh_);
  this->declare_parameter<std::string>("map_load_url", map_load_url_);
-  this->declare_parameter<double>("navigate_to_pose.yaw_offset", nav_goal_yaw_offset_);
-  this->declare_parameter<double>("navigate_to_pose.distance_tolerance", nav_goal_distance_tolerance_);
-  this->declare_parameter<bool>("slam_mode.enable_mapping_default", slam_enable_mapping_default_);
-  this->declare_parameter<std::string>("arm.target_pose_target_frame", arm_target_frame_);
-  this->declare_parameter<double>("arm.target_pose_transform_timeout_sec", arm_transform_timeout_sec_);
+  this->declare_parameter<double>("nav_goal_yaw_offset", nav_goal_yaw_offset_);
+  this->declare_parameter<double>("nav_goal_distance_tolerance", nav_goal_distance_tolerance_);
+  this->declare_parameter<bool>("slam_enable_mapping_default", slam_enable_mapping_default_);
+  this->declare_parameter<std::string>("arm_target_frame", arm_target_frame_);
+  this->declare_parameter<double>("arm_transform_timeout_sec", arm_transform_timeout_sec_);
+  
  // Read back
+  this->get_parameter("robot_config_path", robot_config_path_);
+  this->get_parameter("robot_skill_file_path", robot_skill_file_path_);
  this->get_parameter("abort_on_failure", abort_on_failure_);
  this->get_parameter("default_action_timeout_sec", default_action_timeout_sec_);
  this->get_parameter("vision_grasp_object_timeout_sec", vision_grasp_object_timeout_sec_);
-  this->get_parameter("stats_log_interval_sec", stats_log_interval_sec_);
+  this->get_parameter("enable_stats_publisher", enable_stats_publisher_);
+  this->get_parameter("stats_publish_interval_ms", stats_publish_interval_ms_);
  this->get_parameter("map_save_url", map_save_url_);
  this->get_parameter("map_save_image_format", map_save_image_format_);
  this->get_parameter("map_save_free_thresh", map_save_free_thresh_);
  this->get_parameter("map_save_occupied_thresh", map_save_occupied_thresh_);
  this->get_parameter("map_load_url", map_load_url_);
-  this->get_parameter("navigate_to_pose.yaw_offset", nav_goal_yaw_offset_);
-  this->get_parameter("navigate_to_pose.distance_tolerance", nav_goal_distance_tolerance_);
-  this->get_parameter("slam_mode.enable_mapping_default", slam_enable_mapping_default_);
-  this->get_parameter("arm.target_pose_target_frame", arm_target_frame_);
-  this->get_parameter("arm.target_pose_transform_timeout_sec", arm_transform_timeout_sec_);
+  this->get_parameter("nav_goal_yaw_offset", nav_goal_yaw_offset_);
+  this->get_parameter("nav_goal_distance_tolerance", nav_goal_distance_tolerance_);
+  this->get_parameter("slam_enable_mapping_default", slam_enable_mapping_default_);
+  this->get_parameter("arm_target_frame", arm_target_frame_);
+  this->get_parameter("arm_transform_timeout_sec", arm_transform_timeout_sec_);
+  
  arm_transform_timeout_sec_ = std::max(0.01, arm_transform_timeout_sec_);
  map_save_free_thresh_ = std::clamp(map_save_free_thresh_, 0.0, 1.0);
  map_save_occupied_thresh_ = std::clamp(map_save_occupied_thresh_, 0.0, 1.0);
--- a/src/brain/src/cerebrum_node.cpp
+++ b/src/brain/src/cerebrum_node.cpp
@@ -1034,14 +1034,21 @@ CerebrumNode::ExecResultCode CerebrumNode::ParseResultDetail(const std::string &
 }

 /**
- * @brief Declare all ROS 2 parameters consumed by this node.
+ * @brief Declare node parameters with default values.
 *
- * Separation of declaration and retrieval enables later logic (like default
- * skill file resolution) to consult already-declared parameters without
+ * Declares all runtime-modifiable parameters with their default values.
+ * Intended to be called once during construction to avoid triggering
 * triggering implicit declarations. Not intended for repeated runtime calls.
 */
 void CerebrumNode::DeclareParameters()
 {
+  this->declare_parameter<double>("tick_rate_hz", tick_rate_hz_);
+  this->declare_parameter<int>("rebuild_interval_ms", rebuild_interval_ms_);
+  this->declare_parameter<int>("stats_publish_interval_ms", stats_publish_interval_ms_);
+  this->declare_parameter<std::string>("robot_config_path", "");
+  this->declare_parameter<std::string>("skill_file_path", "");
+  this->declare_parameter<double>("default_bt_timeout", default_bt_timeout_);
+  this->declare_parameter<bool>("enable_sequence_rebuild", enable_sequence_rebuild_);
  this->declare_parameter<double>("node_timeout_sec", node_timeout_sec_);
  this->declare_parameter<bool>("allow_bt_rebuild_during_execution", allow_bt_rebuild_during_execution_);
  this->declare_parameter<std::string>("allowed_action_skills", "");
@@ -1053,14 +1060,22 @@ void CerebrumNode::DeclareParameters()
 * @brief Retrieve parameter values and apply normalization / parsing.
 *
 * Tasks performed:
- *  - Clamp random_skill_pick_count_ to at least 1.
- *  - Parse allowed_action_skills_ delimited list into unique tokens.
- *  - Parse fixed_skill_sequence_ if provided.
+ *  - Clamp random_skill_pick_count to at least 1.
+ *  - Parse allowed_action_skills delimited list into unique tokens.
+ *  - Parse fixed_skill_sequence if provided.
 *  - Log summary for transparency.
 * Safe to call only after DeclareParameters().
 */
 void CerebrumNode::LoadParameters()
 {
+  this->get_parameter("tick_rate_hz", tick_rate_hz_);
+  this->get_parameter("rebuild_interval_ms", rebuild_interval_ms_);
+  this->get_parameter("stats_publish_interval_ms", stats_publish_interval_ms_);
+  this->get_parameter("robot_config_path", robot_config_path_);
+  this->get_parameter("skill_file_path", skill_file_path_);
+  this->get_parameter("default_bt_timeout", default_bt_timeout_);
+  this->get_parameter("enable_sequence_rebuild", enable_sequence_rebuild_);
+  
  this->get_parameter("node_timeout_sec", node_timeout_sec_);
  this->get_parameter("allow_bt_rebuild_during_execution", allow_bt_rebuild_during_execution_);
  int random_pick_param = static_cast<int>(random_skill_pick_count_);
@@ -1257,20 +1272,33 @@ std::string CerebrumNode::ExtractFilenameWithoutExtension(const std::string & pa
 */
 bool CerebrumNode::LoadRobotConfiguration()
 {
-  robot_config_ = std::make_unique<brain::robot_config::RobotConfig>();
+  // Use parameter-based path if available, otherwise default to package share directory
+  std::string config_path = robot_config_path_.empty() 
+    ? share_directory_ + "/config/robot_config.yaml" 
+    : robot_config_path_;
+  
+  robot_config_ = std::make_unique<brain::robot_config::RobotConfig>(config_path);
  auto skill_file = robot_config_->SkillFile("brain");
  if (skill_file == std::nullopt) {
    RCLCPP_ERROR(this->get_logger(), "No skill_file entry found for 'brain' in robot_config.yaml");
    return false;
  }
-  robot_skill_file_path_ = share_directory_ + *skill_file;
+  
+  // Use parameter-based skill file path if available
+  robot_skill_file_path_ = skill_file_path_.empty() 
+    ? share_directory_ + *skill_file 
+    : skill_file_path_;
+    
  RCLCPP_WARN(this->get_logger(), "skill file %s", robot_skill_file_path_.c_str());
  auto path = robot_config_->ConfigParamsPath("cerebrum_node");
  if (path != std::nullopt) {
    for (const auto & p : *path) {
      if (!p.config.empty() && !p.param.empty()) {
-        bt_config_params_paths_.push_back({share_directory_ + p.config, share_directory_ + p.param});
-        RCLCPP_WARN(this->get_logger(), "bt config param %s %s", p.config.c_str(), p.param.c_str());
+        // If paths are relative, resolve them relative to the share directory
+        std::string config_path = p.config[0] == '/' ? p.config : share_directory_ + p.config;
+        std::string param_path = p.param[0] == '/' ? p.param : share_directory_ + p.param;
+        bt_config_params_paths_.push_back({config_path, param_path});
+        RCLCPP_WARN(this->get_logger(), "bt config param %s %s", config_path.c_str(), param_path.c_str());
      }
    }
  }