重构(main.py): 移除任务4并引入任务3
- 注释掉MarkerRunner相关代码 - 替换任务4为任务3的调用 - 在place_marker函数中添加返回None以避免错误
This commit is contained in:
parent
3de4bb8485
commit
5ab480d507
14
main.py
14
main.py
@ -17,12 +17,12 @@ from utils.robot_control_cmd_lcmt import robot_control_cmd_lcmt
|
||||
from utils.robot_control_response_lcmt import robot_control_response_lcmt
|
||||
from utils.localization_lcmt import localization_lcmt
|
||||
from utils.image_raw import ImageProcessor
|
||||
from utils.marker_client import MarkerRunner
|
||||
# from utils.marker_client import MarkerRunner
|
||||
from utils.base_msg import BaseMsg
|
||||
|
||||
from task_1.task_1 import run_task_1
|
||||
from task_2_5.task_2_5 import run_task_2_5
|
||||
from task_4.task_4 import run_task_4
|
||||
from task_3.task_3 import run_task_3
|
||||
from task_test.task_left_line import run_task_test
|
||||
|
||||
pass_marker = True
|
||||
@ -43,9 +43,10 @@ def main():
|
||||
|
||||
# run_task_2_5(Ctrl, msg)
|
||||
|
||||
# run_task_4(Ctrl, msg)
|
||||
# 执行任务3
|
||||
run_task_3(Ctrl, msg)
|
||||
|
||||
run_task_test(Ctrl, msg)
|
||||
# run_task_test(Ctrl, msg)
|
||||
|
||||
# time.sleep(100)
|
||||
|
||||
@ -74,7 +75,7 @@ class Robot_Ctrl(object):
|
||||
self.odo_msg = localization_lcmt()
|
||||
self.image_processor = ImageProcessor()
|
||||
# DEBUG
|
||||
self.marker_runner = MarkerRunner(pass_flag=pass_marker)
|
||||
# self.marker_runner = MarkerRunner(pass_flag=pass_marker)
|
||||
self.send_lock = Lock()
|
||||
self.delay_cnt = 0
|
||||
self.mode_ok = 0
|
||||
@ -94,7 +95,7 @@ class Robot_Ctrl(object):
|
||||
self.rec_thread.start()
|
||||
self.odo_thread.start()
|
||||
self.image_processor.run()
|
||||
self.marker_runner.run()
|
||||
# self.marker_runner.run()
|
||||
|
||||
def msg_handler(self, channel, data):
|
||||
self.rec_msg = robot_control_response_lcmt().decode(data)
|
||||
@ -160,6 +161,7 @@ class Robot_Ctrl(object):
|
||||
self.send_lock.release()
|
||||
|
||||
def place_marker(self, x, y, z, color, observe=False):
|
||||
return None
|
||||
"""调用 MarkerRunner 放置标志物"""
|
||||
if self.marker_runner is None or self.marker_runner.marker_client is None:
|
||||
print("MarkerRunner 未初始化，无法放置标志物")
|
||||
|
||||
138
task_3/Gait_Def_moonwalk.toml
Executable file
138
task_3/Gait_Def_moonwalk.toml
Executable file
@ -0,0 +1,138 @@
|
||||
# Gait Def for moonwalk
|
||||
|
||||
# 1
|
||||
[[section]]
|
||||
contact = [1, 1, 1, 1]
|
||||
duration = 8
|
||||
|
||||
[[section]]
|
||||
contact = [0, 0, 0, 0]
|
||||
duration = 2
|
||||
|
||||
[[section]]
|
||||
contact = [0, 1, 1, 0]
|
||||
duration = 6
|
||||
|
||||
[[section]]
|
||||
contact = [0, 0, 0, 0]
|
||||
duration = 2
|
||||
|
||||
[[section]]
|
||||
contact = [1, 1, 1, 1]
|
||||
duration = 8
|
||||
|
||||
[[section]]
|
||||
contact = [0, 0, 0, 0]
|
||||
duration = 2
|
||||
|
||||
[[section]]
|
||||
contact = [1, 0, 0, 1]
|
||||
duration = 6
|
||||
|
||||
[[section]]
|
||||
contact = [0, 0, 0, 0]
|
||||
duration = 2
|
||||
|
||||
# 2
|
||||
[[section]]
|
||||
contact = [1, 1, 1, 1]
|
||||
duration = 8
|
||||
|
||||
[[section]]
|
||||
contact = [0, 0, 0, 0]
|
||||
duration = 2
|
||||
|
||||
[[section]]
|
||||
contact = [0, 1, 1, 0]
|
||||
duration = 6
|
||||
|
||||
[[section]]
|
||||
contact = [0, 0, 0, 0]
|
||||
duration = 2
|
||||
|
||||
[[section]]
|
||||
contact = [1, 1, 1, 1]
|
||||
duration = 8
|
||||
|
||||
[[section]]
|
||||
contact = [0, 0, 0, 0]
|
||||
duration = 2
|
||||
|
||||
[[section]]
|
||||
contact = [1, 0, 0, 1]
|
||||
duration = 6
|
||||
|
||||
[[section]]
|
||||
contact = [0, 0, 0, 0]
|
||||
duration = 2
|
||||
|
||||
# 3
|
||||
[[section]]
|
||||
contact = [1, 1, 1, 1]
|
||||
duration = 8
|
||||
|
||||
[[section]]
|
||||
contact = [0, 0, 0, 0]
|
||||
duration = 2
|
||||
|
||||
[[section]]
|
||||
contact = [0, 1, 1, 0]
|
||||
duration = 6
|
||||
|
||||
[[section]]
|
||||
contact = [0, 0, 0, 0]
|
||||
duration = 2
|
||||
|
||||
[[section]]
|
||||
contact = [1, 1, 1, 1]
|
||||
duration = 8
|
||||
|
||||
[[section]]
|
||||
contact = [0, 0, 0, 0]
|
||||
duration = 2
|
||||
|
||||
[[section]]
|
||||
contact = [1, 0, 0, 1]
|
||||
duration = 6
|
||||
|
||||
[[section]]
|
||||
contact = [0, 0, 0, 0]
|
||||
duration = 2
|
||||
|
||||
# 4
|
||||
[[section]]
|
||||
contact = [1, 1, 1, 1]
|
||||
duration = 8
|
||||
|
||||
[[section]]
|
||||
contact = [0, 0, 0, 0]
|
||||
duration = 2
|
||||
|
||||
[[section]]
|
||||
contact = [0, 1, 1, 0]
|
||||
duration = 6
|
||||
|
||||
[[section]]
|
||||
contact = [0, 0, 0, 0]
|
||||
duration = 2
|
||||
|
||||
[[section]]
|
||||
contact = [1, 1, 1, 1]
|
||||
duration = 8
|
||||
|
||||
[[section]]
|
||||
contact = [0, 0, 0, 0]
|
||||
duration = 2
|
||||
|
||||
[[section]]
|
||||
contact = [1, 0, 0, 1]
|
||||
duration = 6
|
||||
|
||||
[[section]]
|
||||
contact = [0, 0, 0, 0]
|
||||
duration = 2
|
||||
|
||||
# 5
|
||||
[[section]]
|
||||
contact = [1, 1, 1, 1]
|
||||
duration = 8
|
||||
5782
task_3/Gait_Def_up.toml
Normal file
5782
task_3/Gait_Def_up.toml
Normal file
File diff suppressed because it is too large
Load Diff
8738
task_3/Gait_Params_up.toml
Normal file
8738
task_3/Gait_Params_up.toml
Normal file
File diff suppressed because it is too large
Load Diff
4981
task_3/Gait_Params_up_full.toml
Normal file
4981
task_3/Gait_Params_up_full.toml
Normal file
File diff suppressed because it is too large
Load Diff
44
task_3/Usergait_List.toml
Executable file
44
task_3/Usergait_List.toml
Executable file
@ -0,0 +1,44 @@
|
||||
[[step]]
|
||||
acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
contact = 0
|
||||
ctrl_point = [0.0, 0.0, 0.0]
|
||||
duration = 0 # Expected execution time of Position interpolation control, For recovery stand need > 5.0S
|
||||
foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
gait_id = 0
|
||||
life_count = 0 #Fake value
|
||||
mode = 12
|
||||
pos_des = [0.0, 0.0, 0.0]
|
||||
rpy_des = [0.0, 0.0, 0.0]
|
||||
step_height = [0.0, 0.0]
|
||||
value = 0
|
||||
vel_des = [0.0, 0.0, 0.0]
|
||||
|
||||
[[step]]
|
||||
acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
contact = 15
|
||||
ctrl_point = [0.0, 0.0, 0.0]
|
||||
duration = 0
|
||||
foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
gait_id = 110
|
||||
life_count = 0
|
||||
mode = 62 # User define gait
|
||||
pos_des = [0.0, 0.0, 0.0]
|
||||
rpy_des = [0.0, 0.0, 0.0]
|
||||
step_height = [0.0, 0.0]
|
||||
value = 0
|
||||
vel_des = [0.0, 0.0, 0.0] # velocity of x y yaw
|
||||
|
||||
# [[step]]
|
||||
# acc_des = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
# contact = 0
|
||||
# ctrl_point = [0.0, 0.0, 0.0]
|
||||
# duration = 1000
|
||||
# foot_pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
# gait_id = 1 #采用受控趴下
|
||||
# life_count = 0
|
||||
# mode = 7 #Puredamper
|
||||
# pos_des = [0.0, 0.0, 0.0]
|
||||
# rpy_des = [0.0, 0.0, 0.0]
|
||||
# step_height = [0.0, 0.0]
|
||||
# value = 0
|
||||
# vel_des = [0.0, 0.0, 0.0]
|
||||
67
task_3/file_send_lcmt.py
Normal file
67
task_3/file_send_lcmt.py
Normal file
@ -0,0 +1,67 @@
|
||||
"""LCM type definitions This file automatically generated by lcm.
|
||||
struct file_send_lcmt {
|
||||
string data;
|
||||
}
|
||||
"""
|
||||
try:
|
||||
import cStringIO.StringIO as BytesIO
|
||||
except ImportError:
|
||||
from io import BytesIO
|
||||
import struct
|
||||
|
||||
class file_send_lcmt(object):
|
||||
__slots__ = ["data"]
|
||||
|
||||
__typenames__ = ["string"]
|
||||
|
||||
__dimensions__ = [None]
|
||||
|
||||
def __init__(self):
|
||||
self.data = ""
|
||||
|
||||
def encode(self):
|
||||
buf = BytesIO()
|
||||
buf.write(file_send_lcmt._get_packed_fingerprint())
|
||||
self._encode_one(buf)
|
||||
return buf.getvalue()
|
||||
|
||||
def _encode_one(self, buf):
|
||||
__data_encoded = self.data.encode('utf-8')
|
||||
buf.write(struct.pack('>I', len(__data_encoded)+1))
|
||||
buf.write(__data_encoded)
|
||||
buf.write(b"\0")
|
||||
|
||||
def decode(data):
|
||||
if hasattr(data, 'read'):
|
||||
buf = data
|
||||
else:
|
||||
buf = BytesIO(data)
|
||||
if buf.read(8) != file_send_lcmt._get_packed_fingerprint():
|
||||
raise ValueError("Decode error")
|
||||
return file_send_lcmt._decode_one(buf)
|
||||
decode = staticmethod(decode)
|
||||
|
||||
def _decode_one(buf):
|
||||
self = file_send_lcmt()
|
||||
__data_len = struct.unpack('>I', buf.read(4))[0]
|
||||
self.data = buf.read(__data_len)[:-1].decode('utf-8', 'replace')
|
||||
return self
|
||||
_decode_one = staticmethod(_decode_one)
|
||||
|
||||
def _get_hash_recursive(parents):
|
||||
if file_send_lcmt in parents: return 0
|
||||
tmphash = (0x90df9b84cdceaf0a) & 0xffffffffffffffff
|
||||
tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff
|
||||
return tmphash
|
||||
_get_hash_recursive = staticmethod(_get_hash_recursive)
|
||||
_packed_fingerprint = None
|
||||
|
||||
def _get_packed_fingerprint():
|
||||
if file_send_lcmt._packed_fingerprint is None:
|
||||
file_send_lcmt._packed_fingerprint = struct.pack(">Q", file_send_lcmt._get_hash_recursive([]))
|
||||
return file_send_lcmt._packed_fingerprint
|
||||
_get_packed_fingerprint = staticmethod(_get_packed_fingerprint)
|
||||
|
||||
def get_hash(self):
|
||||
"""Get the LCM hash of the struct"""
|
||||
return struct.unpack(">Q", file_send_lcmt._get_packed_fingerprint())[0]
|
||||
113
task_3/main.py
Normal file
113
task_3/main.py
Normal file
@ -0,0 +1,113 @@
|
||||
'''
|
||||
This demo show the communication interface of MR813 motion control board based on Lcm
|
||||
- robot_control_cmd_lcmt.py
|
||||
- file_send_lcmt.py
|
||||
- Gait_Def_moonwalk.toml
|
||||
- Gait_Params_moonwalk.toml
|
||||
- Usergait_List.toml
|
||||
'''
|
||||
import lcm
|
||||
import sys
|
||||
import time
|
||||
import toml
|
||||
import copy
|
||||
import math
|
||||
from robot_control_cmd_lcmt import robot_control_cmd_lcmt
|
||||
from file_send_lcmt import file_send_lcmt
|
||||
|
||||
robot_cmd = {
|
||||
'mode':0, 'gait_id':0, 'contact':0, 'life_count':0,
|
||||
'vel_des':[0.0, 0.0, 0.0],
|
||||
'rpy_des':[0.0, 0.0, 0.0],
|
||||
'pos_des':[0.0, 0.0, 0.0],
|
||||
'acc_des':[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
'ctrl_point':[0.0, 0.0, 0.0],
|
||||
'foot_pose':[0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
|
||||
'step_height':[0.0, 0.0],
|
||||
'value':0, 'duration':0
|
||||
}
|
||||
|
||||
def main():
|
||||
lcm_cmd = lcm.LCM("udpm://239.255.76.67:7671?ttl=255")
|
||||
lcm_usergait = lcm.LCM("udpm://239.255.76.67:7671?ttl=255")
|
||||
usergait_msg = file_send_lcmt()
|
||||
cmd_msg = robot_control_cmd_lcmt()
|
||||
try:
|
||||
steps = toml.load("Gait_Params_up.toml")
|
||||
full_steps = {'step':[robot_cmd]}
|
||||
k =0
|
||||
for i in steps['step']:
|
||||
cmd = copy.deepcopy(robot_cmd)
|
||||
cmd['duration'] = i['duration']
|
||||
if i['type'] == 'usergait':
|
||||
cmd['mode'] = 11 # LOCOMOTION
|
||||
cmd['gait_id'] = 110 # USERGAIT
|
||||
cmd['vel_des'] = i['body_vel_des']
|
||||
cmd['rpy_des'] = i['body_pos_des'][0:3]
|
||||
cmd['pos_des'] = i['body_pos_des'][3:6]
|
||||
cmd['foot_pose'][0:2] = i['landing_pos_des'][0:2]
|
||||
cmd['foot_pose'][2:4] = i['landing_pos_des'][3:5]
|
||||
cmd['foot_pose'][4:6] = i['landing_pos_des'][6:8]
|
||||
cmd['ctrl_point'][0:2] = i['landing_pos_des'][9:11]
|
||||
cmd['step_height'][0] = math.ceil(i['step_height'][0] * 1e3) + math.ceil(i['step_height'][1] * 1e3) * 1e3
|
||||
cmd['step_height'][1] = math.ceil(i['step_height'][2] * 1e3) + math.ceil(i['step_height'][3] * 1e3) * 1e3
|
||||
cmd['acc_des'] = i['weight']
|
||||
cmd['value'] = i['use_mpc_traj']
|
||||
cmd['contact'] = math.floor(i['landing_gain'] * 1e1)
|
||||
cmd['ctrl_point'][2] = i['mu']
|
||||
if k == 0:
|
||||
full_steps['step'] = [cmd]
|
||||
else:
|
||||
full_steps['step'].append(cmd)
|
||||
k=k+1
|
||||
f = open("Gait_Params_up_full.toml", 'w')
|
||||
f.write("# Gait Params\n")
|
||||
f.writelines(toml.dumps(full_steps))
|
||||
f.close()
|
||||
|
||||
file_obj_gait_def = open("Gait_Def_up.toml",'r')
|
||||
file_obj_gait_params = open("Gait_Params_up_full.toml",'r')
|
||||
usergait_msg.data = file_obj_gait_def.read()
|
||||
lcm_usergait.publish("user_gait_file",usergait_msg.encode())
|
||||
time.sleep(0.5)
|
||||
usergait_msg.data = file_obj_gait_params.read()
|
||||
lcm_usergait.publish("user_gait_file",usergait_msg.encode())
|
||||
time.sleep(0.1)
|
||||
file_obj_gait_def.close()
|
||||
file_obj_gait_params.close()
|
||||
|
||||
user_gait_list = open("Usergait_List.toml",'r')
|
||||
steps = toml.load(user_gait_list)
|
||||
for step in steps['step']:
|
||||
cmd_msg.mode = step['mode']
|
||||
cmd_msg.value = step['value']
|
||||
cmd_msg.contact = step['contact']
|
||||
cmd_msg.gait_id = step['gait_id']
|
||||
cmd_msg.duration = step['duration']
|
||||
cmd_msg.life_count += 1
|
||||
for i in range(3):
|
||||
cmd_msg.vel_des[i] = step['vel_des'][i]
|
||||
cmd_msg.rpy_des[i] = step['rpy_des'][i]
|
||||
cmd_msg.pos_des[i] = step['pos_des'][i]
|
||||
cmd_msg.acc_des[i] = step['acc_des'][i]
|
||||
cmd_msg.acc_des[i+3] = step['acc_des'][i+3]
|
||||
cmd_msg.foot_pose[i] = step['foot_pose'][i]
|
||||
cmd_msg.ctrl_point[i] = step['ctrl_point'][i]
|
||||
for i in range(2):
|
||||
cmd_msg.step_height[i] = step['step_height'][i]
|
||||
lcm_cmd.publish("robot_control_cmd",cmd_msg.encode())
|
||||
time.sleep( 0.1 )
|
||||
for i in range(325): #15s Heat beat It is used to maintain the heartbeat when life count is not updated
|
||||
lcm_cmd.publish("robot_control_cmd",cmd_msg.encode())
|
||||
time.sleep( 0.2 )
|
||||
except KeyboardInterrupt:
|
||||
cmd_msg.mode = 7 #PureDamper before KeyboardInterrupt:
|
||||
cmd_msg.gait_id = 0
|
||||
cmd_msg.duration = 0
|
||||
cmd_msg.life_count += 1
|
||||
lcm_cmd.publish("robot_control_cmd",cmd_msg.encode())
|
||||
pass
|
||||
sys.exit()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
149
task_3/robot_control_cmd_lcmt.py
Normal file
149
task_3/robot_control_cmd_lcmt.py
Normal file
@ -0,0 +1,149 @@
|
||||
# LCM type definitions This file automatically generated by lcm.
|
||||
try:
|
||||
import cStringIO.StringIO as BytesIO
|
||||
except ImportError:
|
||||
from io import BytesIO
|
||||
import struct
|
||||
|
||||
class robot_control_cmd_lcmt(object):
|
||||
__slots__ = ["mode", "gait_id", "contact", "life_count", "vel_des", "rpy_des", "pos_des", "acc_des", "ctrl_point", "foot_pose", "step_height", "value", "duration"]
|
||||
|
||||
__typenames__ = ["int8_t", "int8_t", "int8_t", "int8_t", "float", "float", "float", "float", "float", "float", "float", "int32_t", "int32_t"]
|
||||
|
||||
__dimensions__ = [None, None, None, None, [3], [3], [3], [6], [3], [6], [2], None, None]
|
||||
|
||||
def __init__(self):
|
||||
self.mode = 0
|
||||
self.gait_id = 0
|
||||
self.contact = 0
|
||||
self.life_count = 0
|
||||
self.vel_des = [ 0.0 for dim0 in range(3) ]
|
||||
self.rpy_des = [ 0.0 for dim0 in range(3) ]
|
||||
self.pos_des = [ 0.0 for dim0 in range(3) ]
|
||||
self.acc_des = [ 0.0 for dim0 in range(6) ]
|
||||
self.ctrl_point = [ 0.0 for dim0 in range(3) ]
|
||||
self.foot_pose = [ 0.0 for dim0 in range(6) ]
|
||||
self.step_height = [ 0.0 for dim0 in range(2) ]
|
||||
self.value = 0
|
||||
self.duration = 0
|
||||
|
||||
def encode(self):
|
||||
buf = BytesIO()
|
||||
buf.write(robot_control_cmd_lcmt._get_packed_fingerprint())
|
||||
self._encode_one(buf)
|
||||
return buf.getvalue()
|
||||
|
||||
def _encode_one(self, buf):
|
||||
buf.write(struct.pack(">bbbb", self.mode, self.gait_id, self.contact, self.life_count))
|
||||
buf.write(struct.pack('>3f', *self.vel_des[:3]))
|
||||
buf.write(struct.pack('>3f', *self.rpy_des[:3]))
|
||||
buf.write(struct.pack('>3f', *self.pos_des[:3]))
|
||||
buf.write(struct.pack('>6f', *self.acc_des[:6]))
|
||||
buf.write(struct.pack('>3f', *self.ctrl_point[:3]))
|
||||
buf.write(struct.pack('>6f', *self.foot_pose[:6]))
|
||||
buf.write(struct.pack('>2f', *self.step_height[:2]))
|
||||
buf.write(struct.pack(">ii", self.value, self.duration))
|
||||
|
||||
def decode(data):
|
||||
if hasattr(data, 'read'):
|
||||
buf = data
|
||||
else:
|
||||
buf = BytesIO(data)
|
||||
if buf.read(8) != robot_control_cmd_lcmt._get_packed_fingerprint():
|
||||
raise ValueError("Decode error")
|
||||
return robot_control_cmd_lcmt._decode_one(buf)
|
||||
decode = staticmethod(decode)
|
||||
|
||||
def _decode_one(buf):
|
||||
self = robot_control_cmd_lcmt()
|
||||
self.mode, self.gait_id, self.contact, self.life_count = struct.unpack(">bbbb", buf.read(4))
|
||||
self.vel_des = struct.unpack('>3f', buf.read(12))
|
||||
self.rpy_des = struct.unpack('>3f', buf.read(12))
|
||||
self.pos_des = struct.unpack('>3f', buf.read(12))
|
||||
self.acc_des = struct.unpack('>6f', buf.read(24))
|
||||
self.ctrl_point = struct.unpack('>3f', buf.read(12))
|
||||
self.foot_pose = struct.unpack('>6f', buf.read(24))
|
||||
self.step_height = struct.unpack('>2f', buf.read(8))
|
||||
self.value, self.duration = struct.unpack(">ii", buf.read(8))
|
||||
return self
|
||||
_decode_one = staticmethod(_decode_one)
|
||||
|
||||
def _get_hash_recursive(parents):
|
||||
if robot_control_cmd_lcmt in parents: return 0
|
||||
tmphash = (0x476b61e296af96f5) & 0xffffffffffffffff
|
||||
tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff
|
||||
return tmphash
|
||||
_get_hash_recursive = staticmethod(_get_hash_recursive)
|
||||
_packed_fingerprint = None
|
||||
|
||||
def _get_packed_fingerprint():
|
||||
if robot_control_cmd_lcmt._packed_fingerprint is None:
|
||||
robot_control_cmd_lcmt._packed_fingerprint = struct.pack(">Q", robot_control_cmd_lcmt._get_hash_recursive([]))
|
||||
return robot_control_cmd_lcmt._packed_fingerprint
|
||||
_get_packed_fingerprint = staticmethod(_get_packed_fingerprint)
|
||||
|
||||
def get_hash(self):
|
||||
"""Get the LCM hash of the struct"""
|
||||
return struct.unpack(">Q", robot_control_cmd_lcmt._get_packed_fingerprint())[0]
|
||||
|
||||
class robot_control_response_lcmt(object):
|
||||
__slots__ = ["mode", "gait_id", "contact", "order_process_bar", "switch_status", "ori_error", "footpos_error", "motor_error"]
|
||||
|
||||
__typenames__ = ["int8_t", "int8_t", "int8_t", "int8_t", "int8_t", "int8_t", "int16_t", "int32_t"]
|
||||
|
||||
__dimensions__ = [None, None, None, None, None, None, None, [12]]
|
||||
|
||||
def __init__(self):
|
||||
self.mode = 0
|
||||
self.gait_id = 0
|
||||
self.contact = 0
|
||||
self.order_process_bar = 0
|
||||
self.switch_status = 0
|
||||
self.ori_error = 0
|
||||
self.footpos_error = 0
|
||||
self.motor_error = [ 0 for dim0 in range(12) ]
|
||||
|
||||
def encode(self):
|
||||
buf = BytesIO()
|
||||
buf.write(robot_control_response_lcmt._get_packed_fingerprint())
|
||||
self._encode_one(buf)
|
||||
return buf.getvalue()
|
||||
|
||||
def _encode_one(self, buf):
|
||||
buf.write(struct.pack(">bbbbbbh", self.mode, self.gait_id, self.contact, self.order_process_bar, self.switch_status, self.ori_error, self.footpos_error))
|
||||
buf.write(struct.pack('>12i', *self.motor_error[:12]))
|
||||
|
||||
def decode(data):
|
||||
if hasattr(data, 'read'):
|
||||
buf = data
|
||||
else:
|
||||
buf = BytesIO(data)
|
||||
if buf.read(8) != robot_control_response_lcmt._get_packed_fingerprint():
|
||||
raise ValueError("Decode error")
|
||||
return robot_control_response_lcmt._decode_one(buf)
|
||||
decode = staticmethod(decode)
|
||||
|
||||
def _decode_one(buf):
|
||||
self = robot_control_response_lcmt()
|
||||
self.mode, self.gait_id, self.contact, self.order_process_bar, self.switch_status, self.ori_error, self.footpos_error = struct.unpack(">bbbbbbh", buf.read(8))
|
||||
self.motor_error = struct.unpack('>12i', buf.read(48))
|
||||
return self
|
||||
_decode_one = staticmethod(_decode_one)
|
||||
|
||||
def _get_hash_recursive(parents):
|
||||
if robot_control_response_lcmt in parents: return 0
|
||||
tmphash = (0x485da98216eda8c7) & 0xffffffffffffffff
|
||||
tmphash = (((tmphash<<1)&0xffffffffffffffff) + (tmphash>>63)) & 0xffffffffffffffff
|
||||
return tmphash
|
||||
_get_hash_recursive = staticmethod(_get_hash_recursive)
|
||||
_packed_fingerprint = None
|
||||
|
||||
def _get_packed_fingerprint():
|
||||
if robot_control_response_lcmt._packed_fingerprint is None:
|
||||
robot_control_response_lcmt._packed_fingerprint = struct.pack(">Q", robot_control_response_lcmt._get_hash_recursive([]))
|
||||
return robot_control_response_lcmt._packed_fingerprint
|
||||
_get_packed_fingerprint = staticmethod(_get_packed_fingerprint)
|
||||
|
||||
def get_hash(self):
|
||||
"""Get the LCM hash of the struct"""
|
||||
return struct.unpack(">Q", robot_control_response_lcmt._get_packed_fingerprint())[0]
|
||||
105
task_3/task_3.py
Normal file
105
task_3/task_3.py
Normal file
@ -0,0 +1,105 @@
|
||||
import time
|
||||
import sys
|
||||
import os
|
||||
import toml
|
||||
|
||||
# 添加父目录到路径，以便能够导入utils
|
||||
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||||
# 添加当前目录到路径，确保可以找到local文件
|
||||
sys.path.append(os.path.dirname(os.path.abspath(__file__)))
|
||||
|
||||
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
|
||||
from base_move.turn_degree import turn_degree
|
||||
from base_move.go_straight import go_straight
|
||||
from file_send_lcmt import file_send_lcmt
|
||||
|
||||
# 创建本模块特定的日志记录器
|
||||
logger = get_logger("任务3")
|
||||
|
||||
observe = True
|
||||
|
||||
def load_gait_file(file_path):
|
||||
"""加载步态文件"""
|
||||
try:
|
||||
with open(file_path, 'r') as f:
|
||||
return f.read()
|
||||
except Exception as e:
|
||||
error(f"加载步态文件失败: {e}", "文件")
|
||||
return None
|
||||
|
||||
def run_task_3(ctrl, msg):
|
||||
section('任务3：步态切换', "启动")
|
||||
info('开始执行任务3...', "启动")
|
||||
|
||||
# 创建一个LCM通信实例用于发送文件
|
||||
lc_file = ctrl.lc_s # 复用已有的LCM发送通道
|
||||
|
||||
section('任务3-1：切换到直立步态', "步态")
|
||||
# 加载直立步态文件
|
||||
gait_up_def = load_gait_file(os.path.join(os.path.dirname(__file__), "Gait_Def_up.toml"))
|
||||
gait_up_params = load_gait_file(os.path.join(os.path.dirname(__file__), "Gait_Params_up.toml"))
|
||||
|
||||
if gait_up_def and gait_up_params:
|
||||
# 发送步态定义文件
|
||||
file_msg = file_send_lcmt()
|
||||
file_msg.data = gait_up_def
|
||||
lc_file.publish("Gait_Def", file_msg.encode())
|
||||
time.sleep(0.5) # 等待处理
|
||||
|
||||
# 发送步态参数文件
|
||||
file_msg.data = gait_up_params
|
||||
lc_file.publish("Gait_Params", file_msg.encode())
|
||||
time.sleep(0.5) # 等待处理
|
||||
|
||||
# 执行步态切换
|
||||
msg.mode = 62 # 用户自定义步态模式
|
||||
msg.gait_id = 110 # 根据Usergait_List.toml定义
|
||||
ctrl.Send_cmd(msg)
|
||||
|
||||
# 等待步态切换完成
|
||||
success("直立步态切换完成", "步态")
|
||||
time.sleep(2) # 给机器人一些时间适应新步态
|
||||
|
||||
# 使用新步态进行简单移动
|
||||
go_straight(ctrl, msg, distance=0.5, speed=0.3, observe=observe)
|
||||
else:
|
||||
error("直立步态文件加载失败，无法执行任务3-1", "错误")
|
||||
return
|
||||
|
||||
section('任务3-2：切换到后退步态', "步态")
|
||||
# 加载后退步态文件
|
||||
gait_moonwalk_def = load_gait_file(os.path.join(os.path.dirname(__file__), "Gait_Def_moonwalk.toml"))
|
||||
gait_moonwalk_params = load_gait_file(os.path.join(os.path.dirname(__file__), "Gait_Params_moonwalk.toml"))
|
||||
|
||||
if gait_moonwalk_def and gait_moonwalk_params:
|
||||
# 发送步态定义文件
|
||||
file_msg = file_send_lcmt()
|
||||
file_msg.data = gait_moonwalk_def
|
||||
lc_file.publish("Gait_Def", file_msg.encode())
|
||||
time.sleep(0.5) # 等待处理
|
||||
|
||||
# 发送步态参数文件
|
||||
file_msg.data = gait_moonwalk_params
|
||||
lc_file.publish("Gait_Params", file_msg.encode())
|
||||
time.sleep(0.5) # 等待处理
|
||||
|
||||
# 执行步态切换
|
||||
msg.mode = 62 # 用户自定义步态模式
|
||||
msg.gait_id = 110 # 根据Usergait_List.toml定义
|
||||
ctrl.Send_cmd(msg)
|
||||
|
||||
# 等待步态切换完成
|
||||
success("后退步态切换完成", "步态")
|
||||
time.sleep(2) # 给机器人一些时间适应新步态
|
||||
|
||||
# 使用新步态进行简单移动
|
||||
go_straight(ctrl, msg, distance=-0.5, speed=0.3, observe=observe)
|
||||
else:
|
||||
error("后退步态文件加载失败，无法执行任务3-2", "错误")
|
||||
return
|
||||
|
||||
# 回到默认步态
|
||||
section('任务3-3：恢复默认步态', "步态")
|
||||
ctrl.base_msg.stand_up() # 使用基础消息模块恢复到默认站立姿态
|
||||
|
||||
success("任务3完成", "完成")
|
||||
@ -1,28 +0,0 @@
|
||||
import time
|
||||
import sys
|
||||
import os
|
||||
|
||||
# 添加父目录到路径，以便能够导入utils
|
||||
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||||
|
||||
from base_move.turn_degree import turn_degree
|
||||
from base_move.go_straight import go_straight
|
||||
from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
|
||||
|
||||
# 创建本模块特定的日志记录器
|
||||
logger = get_logger("任务5")
|
||||
|
||||
def run_task_4(ctrl, msg):
|
||||
"""
|
||||
参数:
|
||||
ctrl: Robot_Ctrl对象
|
||||
msg: 控制消息对象
|
||||
image_processor: 可选的图像处理器实例
|
||||
"""
|
||||
|
||||
turn_degree(ctrl, msg, 90, absolute=90)
|
||||
|
||||
time.sleep(100)
|
||||
|
||||
# go_straight(ctrl, msg, distance=6)
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user