HiveCoreRD/hivecore_robot_ctrl_gui
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

增加数据采集

Browse Source
This commit is contained in:
NuoDaJia02
2026-01-27 17:37:58 +08:00
parent 695af8196d
commit 1cd009fd6b
1 changed files with 216 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

234
ctrlgui/ctrlgui_node.py
View File

@@ -13,7 +13,9 @@ from rclpy.node import Node
from rclpy.action.client import ActionClient
from std_msgs.msg import String
from std_srvs.srv import Trigger
from sensor_msgs.msg import JointState
from sensor_msgs.msg import JointState, Image
from cv_bridge import CvBridge
import cv2
from nicegui import ui
from interfaces.msg import RobotWorkInfo
from starlette.staticfiles import StaticFiles
@@ -28,6 +30,7 @@ from interfaces.srv import ClearArmError
from interfaces.action import GripperCmd
from interfaces.msg import GripperStatus
import time
import json
class CtrlGuiNode(Node):
@@ -81,6 +84,30 @@ class CtrlGuiNode(Node):
# clear arm error service client
self.clear_arm_err_client_ = self.create_client(ClearArmError, 'clear_arm_error')
# Top Camera subscription
self.bridge = CvBridge()
self.last_top_image = None
self.last_left_image = None
self.last_right_image = None
self._sub_top_image = self.create_subscription(
Image, '/camera/color/image_raw', self._on_top_image, 10
)
# Left Camera subscription
self._sub_left_image = self.create_subscription(
Image, '/camera1/camera1/color/image_raw', self._on_left_image, 10
)
# Right Camera subscription
self._sub_right_image = self.create_subscription(
Image, '/camera2/camera2/color/image_raw', self._on_right_image, 10
)
# Data collection state
self.is_collecting = False
self.data_collection_dir = None
self.last_action_name = ""
self.last_bt_status = ""
self.file_logger.info('CtrlGuiNode initialized')
self.file_logger.info(f'Node started at: {self.start_time}')
@@ -240,13 +267,36 @@ class CtrlGuiNode(Node):
def _setup_file_logging(self):
"""Setup logging to a file with timestamp in filename."""
# Create logs directory if it doesn't exist
log_dir = os.path.join(os.path.dirname(__file__), '..', 'logs')
# Base logs directory
base_log_dir = "/home/demo/hivecore_robot_os1/logs"
now = datetime.datetime.now()
today_str = now.strftime("%Y%m%d")
today_dir = os.path.join(base_log_dir, today_str)
log_dir = None
try:
# 1. 检查当天的目录是否存在
if os.path.exists(today_dir):
# 2. 查找当天目录中最新的 "robot_logs_..." 文件夹
dirs = [os.path.join(today_dir, d) for d in os.listdir(today_dir)
if os.path.isdir(os.path.join(today_dir, d)) and d.startswith("robot_logs_")]
if dirs:
log_dir = max(dirs, key=os.path.getmtime)
# 3. 如果当天目录不存在，或者里面没有 robot_logs 文件夹，则创建新的
if not log_dir:
time_str = now.strftime("%H%M%S")
log_dir = os.path.join(today_dir, f"robot_logs_{today_str}_{time_str}")
except Exception:
# 兜底方案
log_dir = os.path.join(today_dir, f"robot_logs_{today_str}_{now.strftime('%H%M%S')}")
os.makedirs(log_dir, exist_ok=True)
# Generate filename with timestamp
timestamp = datetime.datetime.now().strftime("%Y%m%d_%H%M%S")
timestamp = now.strftime("%Y%m%d_%H%M%S")
log_filename = os.path.join(log_dir, f"ctrlgui_{timestamp}.log")
print(f"Logging to file: {log_filename}")
# Create file handler
file_handler = logging.FileHandler(log_filename)
@@ -274,7 +324,7 @@ class CtrlGuiNode(Node):
# Log to file with message content
self.file_logger.info(f"Received message #{self.messages_received} from /robot_work_info")
self.file_logger.info(f"Message content - msg_id: {str(msg.msg_id)}, working_state: {str(msg.working_state)}, battery_capacity: {str(msg.battery_capacity)}, working_time: {str(msg.working_time)}, nav_state: {str(msg.nav_state)}, comm_quality: {str(msg.comm_quality)}, expt_completion_time: {str(msg.expt_completion_time)}, work_log: {str(msg.work_log)}, task: {str(list(msg.task))}, skill: {str(msg.skill)}, action_name: {str(msg.action_name)}, instance_params: {str(msg.instance_params)}, bt_node_status: {str(msg.bt_node_status)}, smacc_state: {str(msg.smacc_state)}")
self.file_logger.info(f"Message content - msg_id: {str(msg.msg_id)}, timestamp: {str(msg.stamp.sec)}.{str(msg.stamp.nanosec)}, working_state: {str(msg.working_state)}, battery_capacity: {str(msg.battery_capacity)}, working_time: {str(msg.working_time)}, nav_state: {str(msg.nav_state)}, comm_quality: {str(msg.comm_quality)}, expt_completion_time: {str(msg.expt_completion_time)}, work_log: {str(msg.work_log)}, task: {str(list(msg.task))}, skill: {str(msg.skill)}, action_name: {str(msg.action_name)}, instance_params: {str(msg.instance_params)}, bt_node_status: {str(msg.bt_node_status)}, smacc_state: {str(msg.smacc_state)}")
# Log periodically to avoid spamming the logs
if self.messages_received % 10 == 0:
@@ -301,6 +351,27 @@ class CtrlGuiNode(Node):
self.file_logger.info(f"/joint_states positions: {str(self.last_joint_state.position)}")
def _on_top_image(self, msg: Image) -> None:
"""Callback for /camera/color/image_raw subscription: store latest top image."""
try:
self.last_top_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
except Exception as e:
self.file_logger.error(f"Error converting top image: {str(e)}")
def _on_left_image(self, msg: Image) -> None:
"""Callback for /camera1/camera1/color/image_raw subscription: store latest left image."""
try:
self.last_left_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
except Exception as e:
self.file_logger.error(f"Error converting left image: {str(e)}")
def _on_right_image(self, msg: Image) -> None:
"""Callback for /camera2/camera1/color/image_raw subscription: store latest right image."""
try:
self.last_right_image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
except Exception as e:
self.file_logger.error(f"Error converting right image: {str(e)}")
def publish_info(self, text: str) -> None:
msg = String()
msg.data = text
@@ -389,7 +460,37 @@ def build_ui(node: CtrlGuiNode) -> None:
ui.label('HiveCore Robot').classes('text-2xl font-bold').style('color: orange').classes('self-end')
# ui.separator()
ui.label('Control Panel').classes('text-xl')
with ui.row().classes('items-center'):
ui.label('Control Panel').classes('text-xl')
def toggle_collection():
if not node.is_collecting:
# Start collecting
timestamp_s = int(time.time())
data_base_dir = "/home/demo/hivecore_robot_os1/data"
node.data_collection_dir = os.path.join(data_base_dir, str(timestamp_s))
try:
os.makedirs(node.data_collection_dir, exist_ok=True)
skill_info_path = os.path.join(node.data_collection_dir, 'skill_info.txt')
# Create empty file
with open(skill_info_path, 'w') as f:
pass
node.is_collecting = True
node.last_action_name = "" # Reset to ensure we capture the first state
collect_btn.set_text('停止采集')
collect_btn.props('color=red')
ui.notify(f'开始采集数据: {node.data_collection_dir}')
except Exception as e:
ui.notify(f'创建采集目录失败: {str(e)}', color='negative')
else:
# Stop collecting
node.is_collecting = False
collect_btn.set_text('采集数据')
collect_btn.props('color=primary')
ui.notify('停止采集数据')
collect_btn = ui.button('采集数据', on_click=toggle_collection)
# confirm dialog function
def show_confirm_dialog(message: str, confirm_callback) -> None:
"""显示确认对话框
@@ -734,6 +835,7 @@ def build_ui(node: CtrlGuiNode) -> None:
ui.label('Skill Info').classes('text-lg font-bold')
with ui.grid(columns=2).classes('w-full'):
timestamp_label = ui.label()
skill_label = ui.label()
action_name_label = ui.label()
instance_params_label = ui.label().classes('col-span-2')
@@ -789,18 +891,113 @@ def build_ui(node: CtrlGuiNode) -> None:
# Update the labels periodically from the UI thread (safe cross-thread update)
def update_work_info():
msg = node.last_work_info
# Update statistics
# stats = node.get_stats()
# stats_text = f"""
# - System Time: {str(datetime.datetime.now())}
# - Start Time: {str(stats['start_time'])}
# - Uptime: {str(stats['uptime'])}
# - Messages Sent: {str(stats['messages_sent'])}
# - Messages Received: {str(stats['messages_received'])}
# - Rebuild Requests: {str(stats['rebuild_requests'])}
# """
# stats_label.set_text(stats_text)
# Data collection logic
if node.is_collecting:
current_action_name = str(msg.action_name)
current_bt_status = str(msg.bt_node_status)
skill_info_path = os.path.join(node.data_collection_dir, 'skill_info.txt')
# 记录逻辑：
# 1. 如果 action_name 变了，新增一行数据
if current_action_name != node.last_action_name:
try:
# 解析 instance_params
try:
# 先尝试直接作为 JSON 解析
instance_params_data = json.loads(msg.instance_params)
except (json.JSONDecodeError, TypeError):
# 如果不是 JSON，尝试解析 "key: value\n" 格式
instance_params_data = {}
if isinstance(msg.instance_params, str) and msg.instance_params.strip():
lines = msg.instance_params.strip().split('\n')
for line in lines:
if ':' in line:
key, val = line.split(':', 1)
key = key.strip()
val = val.strip()
# 尝试将值转换为 JSON (处理数字、列表等)
try:
# 兼容单引号
cleaned_val = val.replace("'", '"')
instance_params_data[key] = json.loads(cleaned_val)
except:
instance_params_data[key] = val
elif line.strip():
instance_params_data[f"param_{len(instance_params_data)}"] = line.strip()
# 如果没有任何键值对，回退为原始字符串
if not instance_params_data and msg.instance_params:
instance_params_data = str(msg.instance_params)
# Save images if available
timestamp = time.time()
image_filename_top = ""
image_filename_left = ""
image_filename_right = ""
if node.last_top_image is not None:
try:
image_filename_top = f"top_cam_{timestamp:.6f}.png"
cv2.imwrite(os.path.join(node.data_collection_dir, image_filename_top), node.last_top_image)
except Exception as e:
node.file_logger.error(f"Failed to save top image: {str(e)}")
if node.last_left_image is not None:
try:
image_filename_left = f"left_cam_{timestamp:.6f}.png"
cv2.imwrite(os.path.join(node.data_collection_dir, image_filename_left), node.last_left_image)
except Exception as e:
node.file_logger.error(f"Failed to save left image: {str(e)}")
if node.last_right_image is not None:
try:
image_filename_right = f"right_cam_{timestamp:.6f}.png"
cv2.imwrite(os.path.join(node.data_collection_dir, image_filename_right), node.last_right_image)
except Exception as e:
node.file_logger.error(f"Failed to save right image: {str(e)}")
data = {
"action_name": current_action_name,
"skill": str(msg.skill),
"stamp": f"{msg.stamp.sec}.{msg.stamp.nanosec:09d}",
"instance_params": instance_params_data,
"bt_node_status": current_bt_status,
"image_path_top": image_filename_top,
"image_path_left": image_filename_left,
"image_path_right": image_filename_right
}
with open(skill_info_path, 'a') as f:
f.write(json.dumps(data, ensure_ascii=False) + '\n')
node.last_action_name = current_action_name
node.last_bt_status = current_bt_status
except Exception as e:
node.file_logger.error(f'Failed to log new action: {str(e)}')
# 2. 如果 action_name 没变，但 bt_node_status 变了，更新当前最后一行的内容
elif current_bt_status != node.last_bt_status:
try:
if os.path.exists(skill_info_path):
with open(skill_info_path, 'r') as f:
lines = f.readlines()
if lines:
# 解析最后一行
last_entry = json.loads(lines[-1].strip())
# 只有当 action_name 匹配时才更新状态
if last_entry.get("action_name") == current_action_name:
last_entry["bt_node_status"] = current_bt_status
lines[-1] = json.dumps(last_entry, ensure_ascii=False) + '\n'
with open(skill_info_path, 'w') as f:
f.writelines(lines)
else:
# 如果不匹配（理论上不应发生），可以视为新动作或记录异常
node.file_logger.warning(f'Action name mismatch during status update: {last_entry.get("action_name")} vs {current_action_name}')
node.last_bt_status = current_bt_status
except Exception as e:
node.file_logger.error(f'Failed to update bt_node_status: {str(e)}')
# Basic Information
msg_id_label.set_text(f'Message ID: {str(msg.msg_id)}')
@@ -840,6 +1037,7 @@ def build_ui(node: CtrlGuiNode) -> None:
right_arm_joint_label.set_text('Right Arm Joint: N/A')
# Skill Information
timestamp_label.set_text(f'Timestamp: {str(msg.stamp.sec)}.{str(msg.stamp.nanosec)}')
skill_label.set_text(f'Skill: {str(msg.skill)}')
action_name_label.set_text(f'Action Name: {str(msg.action_name)}')
instance_params_label.set_text(f'Instance Parameters: {str(msg.instance_params)}')