From d7ad3991e109b525f0ef8979bd16b1d99ccaa78c Mon Sep 17 00:00:00 2001 From: NuoDaJia02 Date: Sun, 1 Feb 2026 11:09:49 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E9=85=8D=E7=BD=AE=E6=96=87?= =?UTF-8?q?=E4=BB=B6=E5=92=8Claunch=E6=96=87=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- config/ctrlgui.yaml | 33 +++++++++ ctrlgui/ctrlgui_node.py | 140 +++++++++++++++++++++++++++++---------- launch/ctrlgui.launch.py | 20 ++++-- setup.py | 1 + 4 files changed, 154 insertions(+), 40 deletions(-) create mode 100644 config/ctrlgui.yaml diff --git a/config/ctrlgui.yaml b/config/ctrlgui.yaml new file mode 100644 index 0000000..af717de --- /dev/null +++ b/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" diff --git a/ctrlgui/ctrlgui_node.py b/ctrlgui/ctrlgui_node.py index 0442933..85c8bf7 100644 --- a/ctrlgui/ctrlgui_node.py +++ b/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') diff --git a/launch/ctrlgui.launch.py b/launch/ctrlgui.launch.py index 5991bd5..27a1c0a 100644 --- a/launch/ctrlgui.launch.py +++ b/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) diff --git a/setup.py b/setup.py index 336fc77..510be0a 100644 --- a/setup.py +++ b/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', '*'))