Havoc420mac/mi-task
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

#3

Browse Source
This commit is contained in:
havocrao 2025-08-17 23:33:09 +08:00
parent 85cfa8f23b
commit f9c59291fd
1 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
base-move-demo.py
View File

@ -83,6 +83,9 @@ class Robot_Ctrl(object):
# 新增: 基础消息
self.base_msg = BaseMsg(self, self.cmd_msg)
self.timestamp = 0
self.rec_msg_last_mode_id = 0
def run(self):
self.lc_r.subscribe("robot_control_response", self.msg_handler)
self.lc_o.subscribe("global_to_robot", self.msg_handler_o)
@ -98,7 +101,11 @@ class Robot_Ctrl(object):
self.mode_ok = self.rec_msg.mode
else:
self.mode_ok = 0
print("[rec_msg]", self.rec_msg.mode, self.rec_msg.gait_id, self.rec_msg.contact, self.rec_msg.order_process_bar, self.rec_msg.switch_status, self.rec_msg.ori_error, self.rec_msg.footpos_error, self.rec_msg.motor_error)
if self.timestamp % 100 == 0 or self.rec_msg_last_mode_id != self.rec_msg.mode_id:
print("[rec_msg]", self.rec_msg.mode, self.rec_msg.gait_id, self.rec_msg.contact, self.rec_msg.order_process_bar, self.rec_msg.switch_status, self.rec_msg.ori_error, self.rec_msg.footpos_error, self.rec_msg.motor_error)
self.timestamp = 0
self.rec_msg_last_mode_id = self.rec_msg.mode_id
self.timestamp += 1
def msg_handler_o(self, channel, data):
self.odo_msg = localization_lcmt().decode(data)
@ -156,6 +163,7 @@ class Robot_Ctrl(object):
self.cmd_msg = msg
self.cmd_msg.life_count %= 127
self.send_lock.release()
self.send_publish()
def place_marker(self, x, y, z, color, observe=False):
return None