增加配置文件和launch文件

config/ctrlgui.yaml

@@ -0,0 +1,33 @@
+ctrlgui_node:
+  ros__parameters:
+    collection_enabled: false
+    data_base_dir: "/home/demo/hivecore_robot_os1/data"
+    log_base_dir: "/home/demo/hivecore_robot_os1/logs"
+    queue_size: 10
+    action_server_wait_timeout_sec: 1.0
+    work_info_log_throttle_sec: 1.0
+    joint_state_log_throttle_sec: 1.0
+    work_info_log_every_n: 100
+    bt_xml_log_max_len: 100
+    ui_update_period_sec: 0.5
+    host: "0.0.0.0"
+    port: 8080
+
+    topic_led_cmd: "/led_cmd"
+    service_rebuild_now: "/cerebrum/rebuild_now"
+    topic_robot_work_info: "/robot_work_info"
+    action_execute_bt: "/execute_bt_action"
+    topic_joint_states: "/joint_states"
+    action_gripper0_cmd: "/gripper_cmd0"
+    action_gripper1_cmd: "/gripper_cmd1"
+    topic_gripper0_status: "/gripper0/status"
+    topic_gripper1_status: "/gripper1/status"
+    topic_arm_errors: "/arm_errors"
+    service_clear_arm_error: "clear_arm_error"
+
+    topic_top_color: "/camera/color/image_raw"
+    topic_top_depth: "/camera/depth/image_raw"
+    topic_left_color: "/camera1/camera1/color/image_raw"
+    topic_left_depth: "/camera1/camera1/depth/image_rect_raw"
+    topic_right_color: "/camera2/camera2/color/image_raw"
+    topic_right_depth: "/camera2/camera2/depth/image_rect_raw"

ctrlgui/ctrlgui_node.py

@@ -36,55 +36,122 @@ from interfaces.msg import GripperStatus
 class CtrlGuiNode(Node):
     """ROS2 node that publishes String messages to /led_cmd."""
 
+    def _declare_parameters(self) -> None:
+        self.declare_parameter('collection_enabled', False)
+        self.declare_parameter('data_base_dir', '/home/demo/hivecore_robot_os1/data')
+        self.declare_parameter('log_base_dir', '/home/demo/hivecore_robot_os1/logs')
+        self.declare_parameter('queue_size', 10)
+        self.declare_parameter('action_server_wait_timeout_sec', 1.0)
+        self.declare_parameter('work_info_log_throttle_sec', 1.0)
+        self.declare_parameter('joint_state_log_throttle_sec', 1.0)
+        self.declare_parameter('work_info_log_every_n', 100)
+        self.declare_parameter('bt_xml_log_max_len', 100)
+        self.declare_parameter('ui_update_period_sec', 0.5)
+        self.declare_parameter('host', '0.0.0.0')
+        self.declare_parameter('port', 8080)
+
+        # Topics/services/actions
+        self.declare_parameter('topic_led_cmd', '/led_cmd')
+        self.declare_parameter('service_rebuild_now', '/cerebrum/rebuild_now')
+        self.declare_parameter('topic_robot_work_info', '/robot_work_info')
+        self.declare_parameter('action_execute_bt', '/execute_bt_action')
+        self.declare_parameter('topic_joint_states', '/joint_states')
+        self.declare_parameter('action_gripper0_cmd', '/gripper_cmd0')
+        self.declare_parameter('action_gripper1_cmd', '/gripper_cmd1')
+        self.declare_parameter('topic_gripper0_status', '/gripper0/status')
+        self.declare_parameter('topic_gripper1_status', '/gripper1/status')
+        self.declare_parameter('topic_arm_errors', '/arm_errors')
+        self.declare_parameter('service_clear_arm_error', 'clear_arm_error')
+        self.declare_parameter('topic_top_color', '/camera/color/image_raw')
+        self.declare_parameter('topic_top_depth', '/camera/depth/image_raw')
+        self.declare_parameter('topic_left_color', '/camera1/camera1/color/image_raw')
+        self.declare_parameter('topic_left_depth', '/camera1/camera1/depth/image_rect_raw')
+        self.declare_parameter('topic_right_color', '/camera2/camera2/color/image_raw')
+        self.declare_parameter('topic_right_depth', '/camera2/camera2/depth/image_rect_raw')
+
+    def _load_parameters(self) -> None:
+        self.param_collection_enabled = bool(self.get_parameter('collection_enabled').value)
+        self.data_base_dir = str(self.get_parameter('data_base_dir').value)
+        self.log_base_dir = str(self.get_parameter('log_base_dir').value)
+        self.param_queue_size = int(self.get_parameter('queue_size').value)
+        self.param_action_server_wait_timeout_sec = float(self.get_parameter('action_server_wait_timeout_sec').value)
+        self.param_work_info_log_throttle_sec = float(self.get_parameter('work_info_log_throttle_sec').value)
+        self.param_joint_state_log_throttle_sec = float(self.get_parameter('joint_state_log_throttle_sec').value)
+        self.param_work_info_log_every_n = int(self.get_parameter('work_info_log_every_n').value)
+        self.param_bt_xml_log_max_len = int(self.get_parameter('bt_xml_log_max_len').value)
+        self.param_ui_update_period_sec = float(self.get_parameter('ui_update_period_sec').value)
+        self.param_host = str(self.get_parameter('host').value)
+        self.param_port = int(self.get_parameter('port').value)
+
+        self.topic_led_cmd = str(self.get_parameter('topic_led_cmd').value)
+        self.service_rebuild_now = str(self.get_parameter('service_rebuild_now').value)
+        self.topic_robot_work_info = str(self.get_parameter('topic_robot_work_info').value)
+        self.action_execute_bt = str(self.get_parameter('action_execute_bt').value)
+        self.topic_joint_states = str(self.get_parameter('topic_joint_states').value)
+        self.action_gripper0_cmd = str(self.get_parameter('action_gripper0_cmd').value)
+        self.action_gripper1_cmd = str(self.get_parameter('action_gripper1_cmd').value)
+        self.topic_gripper0_status = str(self.get_parameter('topic_gripper0_status').value)
+        self.topic_gripper1_status = str(self.get_parameter('topic_gripper1_status').value)
+        self.topic_arm_errors = str(self.get_parameter('topic_arm_errors').value)
+        self.service_clear_arm_error = str(self.get_parameter('service_clear_arm_error').value)
+        self.topic_top_color = str(self.get_parameter('topic_top_color').value)
+        self.topic_top_depth = str(self.get_parameter('topic_top_depth').value)
+        self.topic_left_color = str(self.get_parameter('topic_left_color').value)
+        self.topic_left_depth = str(self.get_parameter('topic_left_depth').value)
+        self.topic_right_color = str(self.get_parameter('topic_right_color').value)
+        self.topic_right_depth = str(self.get_parameter('topic_right_depth').value)
+
     def __init__(self, node_name: str = 'ctrlgui_node'):
         super().__init__(node_name)
+        self._declare_parameters()
+        self._load_parameters()
         # Initialize counters for monitoring
         self.messages_sent = 0
         self.messages_received = 0
         self.rebuild_requests = 0
         self.start_time = datetime.datetime.now()
         # Data collection enable flag (performance toggle)
-        self.collection_enabled = True
+        self.collection_enabled = self.param_collection_enabled
         
         # Setup logging to file
         self._setup_file_logging()
         
         # publisher on /led_cmd
-        self.publisher_ = self.create_publisher(String, '/led_cmd', 10)
-        self.run_trigger_ = self.create_client(BtRebuild, '/cerebrum/rebuild_now')
+        self.publisher_ = self.create_publisher(String, self.topic_led_cmd, self.param_queue_size)
+        self.run_trigger_ = self.create_client(BtRebuild, self.service_rebuild_now)
         # subscribe to /robot_work_info to display incoming messages in the UI
         self.last_work_info: RobotWorkInfo = RobotWorkInfo()
         self._sub_work_info = self.create_subscription(
-            RobotWorkInfo, '/robot_work_info', self._on_work_info, 10
+            RobotWorkInfo, self.topic_robot_work_info, self._on_work_info, self.param_queue_size
         )
         # bt action client
-        self.exe_bt_action_client_ = ActionClient(self, ExecuteBtAction, '/execute_bt_action')
+        self.exe_bt_action_client_ = ActionClient(self, ExecuteBtAction, self.action_execute_bt)
         self.exe_bt_action_epoch = 0
 
         self.joint_state_sub_ = self.create_subscription(
-            JointState, '/joint_states', self._on_joint_state, 10
+            JointState, self.topic_joint_states, self._on_joint_state, self.param_queue_size
         )
 
-        self.gripper0_action_client_ = ActionClient(self, GripperCmd, '/gripper_cmd0')
-        self.gripper1_action_client_ = ActionClient(self, GripperCmd, '/gripper_cmd1')
+        self.gripper0_action_client_ = ActionClient(self, GripperCmd, self.action_gripper0_cmd)
+        self.gripper1_action_client_ = ActionClient(self, GripperCmd, self.action_gripper1_cmd)
 
         # gripper status subscription
         self.last_gripper_status = {0: GripperStatus(), 1: GripperStatus()}
         self._sub_gripper0_status = self.create_subscription(
-            GripperStatus, '/gripper0/status', lambda msg: self._on_gripper_status(msg, 0), 10
+            GripperStatus, self.topic_gripper0_status, lambda msg: self._on_gripper_status(msg, 0), self.param_queue_size
         )
         self._sub_gripper1_status = self.create_subscription(
-            GripperStatus, '/gripper1/status', lambda msg: self._on_gripper_status(msg, 1), 10
+            GripperStatus, self.topic_gripper1_status, lambda msg: self._on_gripper_status(msg, 1), self.param_queue_size
         )
 
         # arm error subscription
         self.last_arm_error = {0: None, 1: None}
         self._sub_arm_error = self.create_subscription(
-            ArmError, '/arm_errors', self._on_arm_error, 10
+            ArmError, self.topic_arm_errors, self._on_arm_error, self.param_queue_size
         )
         
         # clear arm error service client
-        self.clear_arm_err_client_ = self.create_client(ClearArmError, 'clear_arm_error')
+        self.clear_arm_err_client_ = self.create_client(ClearArmError, self.service_clear_arm_error)
 
         # Top Camera subscription
         self.bridge = CvBridge()
@@ -122,6 +189,8 @@ class CtrlGuiNode(Node):
     async def _wait_for_action_server(self, client: ActionClient, timeout_sec: float = 1.0) -> bool:
         """Asynchronously wait for an action server to be available."""
         import asyncio
+        if timeout_sec is None:
+            timeout_sec = self.param_action_server_wait_timeout_sec
         start = time.time()
         while time.time() - start < timeout_sec:
             if client.server_is_ready():
@@ -141,7 +210,7 @@ class CtrlGuiNode(Node):
         self.file_logger.info(f"goal_msg: {goal_msg}")
         
         # Non-blocking wait for server
-        if not await self._wait_for_action_server(self.exe_bt_action_client_, 1.0):
+        if not await self._wait_for_action_server(self.exe_bt_action_client_, None):
              self.file_logger.error('BT Action Server not available')
              ui.notify('BT Action Server not available', color='negative')
              return
@@ -193,7 +262,7 @@ class CtrlGuiNode(Node):
         
         client = self.gripper0_action_client_ if id == 0 else self.gripper1_action_client_
         
-        if not await self._wait_for_action_server(client, 1.0):
+        if not await self._wait_for_action_server(client, None):
              ui.notify(f'Gripper {id} server not available', color='negative')
              return
 
@@ -255,8 +324,7 @@ class CtrlGuiNode(Node):
             date_str = now.strftime("%Y%m%d")
             time_str = now.strftime("%H%M%S")
             
-            data_base_dir = "/home/demo/hivecore_robot_os1/data"
-            self.data_collection_dir = os.path.join(data_base_dir, date_str, time_str)
+            self.data_collection_dir = os.path.join(self.data_base_dir, date_str, time_str)
             try:
                 os.makedirs(self.data_collection_dir, exist_ok=True)
                 os.makedirs(os.path.join(self.data_collection_dir, 'images'), exist_ok=True)
@@ -302,27 +370,27 @@ class CtrlGuiNode(Node):
     def _create_camera_subscriptions(self) -> None:
         if self._sub_top_image is None:
             self._sub_top_image = self.create_subscription(
-                Image, '/camera/color/image_raw', self._on_top_image, 10
+                Image, self.topic_top_color, self._on_top_image, self.param_queue_size
             )
         if self._sub_top_depth is None:
             self._sub_top_depth = self.create_subscription(
-                Image, '/camera/depth/image_raw', self._on_top_depth, 10
+                Image, self.topic_top_depth, self._on_top_depth, self.param_queue_size
             )
         if self._sub_left_image is None:
             self._sub_left_image = self.create_subscription(
-                Image, '/camera1/camera1/color/image_raw', self._on_left_image, 10
+                Image, self.topic_left_color, self._on_left_image, self.param_queue_size
             )
         if self._sub_left_depth is None:
             self._sub_left_depth = self.create_subscription(
-                Image, '/camera1/camera1/depth/image_rect_raw', self._on_left_depth, 10
+                Image, self.topic_left_depth, self._on_left_depth, self.param_queue_size
             )
         if self._sub_right_image is None:
             self._sub_right_image = self.create_subscription(
-                Image, '/camera2/camera2/color/image_raw', self._on_right_image, 10
+                Image, self.topic_right_color, self._on_right_image, self.param_queue_size
             )
         if self._sub_right_depth is None:
             self._sub_right_depth = self.create_subscription(
-                Image, '/camera2/camera2/depth/image_rect_raw', self._on_right_depth, 10
+                Image, self.topic_right_depth, self._on_right_depth, self.param_queue_size
             )
 
     def _destroy_camera_subscriptions(self) -> None:
@@ -375,7 +443,7 @@ class CtrlGuiNode(Node):
     def _setup_file_logging(self):
         """Setup logging to a file with timestamp in filename."""
         # Base logs directory
-        base_log_dir = "/home/demo/hivecore_robot_os1/logs"
+        base_log_dir = self.log_base_dir
         now = datetime.datetime.now()
         today_str = now.strftime("%Y%m%d")
         today_dir = os.path.join(base_log_dir, today_str)
@@ -429,7 +497,7 @@ class CtrlGuiNode(Node):
         
         current_time = time.time()
         # Throttle logging to 1Hz
-        if current_time - self.last_work_info_log_time >= 1.0:
+        if current_time - self.last_work_info_log_time >= self.param_work_info_log_throttle_sec:
             # Logging content for diagnostic purposes
             action_strs = []
             for action in msg.action:
@@ -437,7 +505,11 @@ class CtrlGuiNode(Node):
                 action_strs.append(f"{{skill: {action.skill}, action_name: {action.action_name}, params: {action.params}, execution: {action.execution}, state: {action.state}, stamp: {ts}}}")
             
             # Truncate bt_xml in periodic logs to save CPU and log size
-            bt_xml_log = (msg.bt_xml[:100] + "...") if msg.bt_xml and len(msg.bt_xml) > 100 else str(msg.bt_xml)
+            bt_xml_log = (
+                (msg.bt_xml[:self.param_bt_xml_log_max_len] + "...")
+                if msg.bt_xml and len(msg.bt_xml) > self.param_bt_xml_log_max_len
+                else str(msg.bt_xml)
+            )
 
             log_msg = (
                 f"Message content - msg_id: {str(msg.msg_id)}, working_state: {str(msg.working_state)}, "
@@ -477,7 +549,7 @@ class CtrlGuiNode(Node):
         self.last_bt_status = current_bt_status
 
         # Log summary periodically
-        if self.messages_received % 100 == 0:
+        if self.messages_received % self.param_work_info_log_every_n == 0:
             self.file_logger.info(f'Processed {self.messages_received} messages from /robot_work_info')
 
     def _log_session_summary(self, msg: RobotWorkInfo, end_status: str):
@@ -672,7 +744,7 @@ class CtrlGuiNode(Node):
 
         # Throttle logging to 1Hz
         current_time = time.time()
-        if current_time - self.last_joint_state_log_time >= 1.0:
+        if current_time - self.last_joint_state_log_time >= self.param_joint_state_log_throttle_sec:
             self.file_logger.info(f"/joint_states positions: {str(self.last_joint_state.position)}")
             self.last_joint_state_log_time = current_time
 
@@ -810,7 +882,10 @@ def build_ui(node: CtrlGuiNode) -> None:
             else:
                 ui.notify('已启用数据采集与图像保存流程', color='positive')
 
-        collection_enable_btn = ui.button('禁用采集流程', on_click=toggle_collection_enabled)
+        collection_enable_btn = ui.button(
+            '禁用采集流程' if node.collection_enabled else '启用采集流程',
+            on_click=toggle_collection_enabled
+        )
 
     # confirm dialog function
     def show_confirm_dialog(message: str, confirm_callback) -> None:
@@ -1319,7 +1394,7 @@ def build_ui(node: CtrlGuiNode) -> None:
                 _set_label_if_changed(l_torque, f'gripper_{gid}_torque', 'Torque: N/A')
                 _set_label_if_changed(l_state, f'gripper_{gid}_state', 'State: N/A')
 
-    ui.timer(0.5, update_work_info)
+    ui.timer(node.param_ui_update_period_sec, update_work_info)
 
 def main() -> None:
     """Start rclpy, the ROS2 node in a background thread, and run the NiceGUI app.
@@ -1390,11 +1465,8 @@ def main() -> None:
     atexit.register(shutdown)
 
     # Start NiceGUI honoring HOST/PORT environment variables (works with ros2 launch env)
-    host = os.getenv('HOST', '0.0.0.0')
-    try:
-        port = int(os.getenv('PORT', '8080'))
-    except ValueError:
-        port = 8080
+    host = node.param_host
+    port = node.param_port
     print(f'Starting NiceGUI on {host}:{port}...')
     ui.run(title='HiveCore Robot', host=host, port=port, reload=False, uvicorn_logging_level='info')
 

launch/ctrlgui.launch.py

@@ -2,6 +2,8 @@ from launch import LaunchDescription
 from launch.actions import DeclareLaunchArgument
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+import os
 
 
 def generate_launch_description() -> LaunchDescription:
@@ -15,8 +17,12 @@ def generate_launch_description() -> LaunchDescription:
     port_arg = DeclareLaunchArgument(
         'port', default_value='8080',
         description='TCP port for NiceGUI')
+    params_file_arg = DeclareLaunchArgument(
+        'params_file',
+        default_value=os.path.join(get_package_share_directory('ctrlgui'), 'config', 'ctrlgui.yaml'),
+        description='Path to the ctrlgui parameter file')
 
-    ld = LaunchDescription([node_name_arg, host_arg, port_arg])
+    ld = LaunchDescription([node_name_arg, host_arg, port_arg, params_file_arg])
 
     ctrlgui_node = Node(
         package='ctrlgui',
@@ -24,11 +30,13 @@ def generate_launch_description() -> LaunchDescription:
         name=LaunchConfiguration('node_name'),
         output='screen',
         emulate_tty=True,
-        parameters=[],
-        env={
-            'HOST': LaunchConfiguration('host'),
-            'PORT': LaunchConfiguration('port'),
-        },
+        parameters=[
+            LaunchConfiguration('params_file'),
+            {
+                'host': LaunchConfiguration('host'),
+                'port': LaunchConfiguration('port'),
+            },
+        ],
     )
 
     ld.add_action(ctrlgui_node)

setup.py

@@ -17,6 +17,7 @@ setup(
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
         ('share/' + package_name + '/launch', ['launch/ctrlgui.launch.py']),
+        ('share/' + package_name + '/config', ['config/ctrlgui.yaml']),
         # Install top-level static assets into share for runtime serving
         ('share/' + package_name + '/static', [
             *glob(os.path.join('static', '*'))