Refactor main.py and task_4: Updated turning logic in main.py to use turn_degree_v2 with a fixed degree of -90. Adjusted Gait_Params_stoop_full.toml and Gait_Params_stoop.toml to reduce body velocity from 0.2 to 0.15 for smoother movement. Enhanced pass_bar.py to streamline gait file publishing and added step_num variable for better control during the pass_bar function. Updated run_task_4_back to improve task flow and added a pause for standing up after passing the bar.
This commit is contained in:
parent
5c3e858029
commit
2bb7558fb1
9
main.py
9
main.py
@ -56,6 +56,7 @@ def main():
|
||||
# print(f"arrow_direction: {arrow_direction}")
|
||||
# run_task_2_5(Ctrl, msg, direction=arrow_direction)
|
||||
|
||||
turn_degree_v2(Ctrl, msg, degree=-90, absolute=True)
|
||||
arrow_direction = 'right' # TEST
|
||||
|
||||
# if arrow_direction == 'left':
|
||||
@ -63,11 +64,9 @@ def main():
|
||||
# else:
|
||||
# run_task_3(Ctrl, msg)
|
||||
|
||||
turn_degree_v2(Ctrl, msg, degree=90, absolute=True)
|
||||
# turn_degree_v2(Ctrl, msg, degree=90, absolute=True)
|
||||
|
||||
run_task_5(Ctrl, msg, direction=arrow_direction)
|
||||
|
||||
return
|
||||
# run_task_5(Ctrl, msg, direction=arrow_direction)
|
||||
|
||||
if arrow_direction == 'left':
|
||||
# run_task_3_back(Ctrl, msg)
|
||||
@ -75,6 +74,8 @@ def main():
|
||||
else:
|
||||
run_task_4_back(Ctrl, msg)
|
||||
|
||||
return
|
||||
|
||||
run_task_5(Ctrl, msg)
|
||||
|
||||
run_task_2_5_back(Ctrl, msg, direction=arrow_direction)
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
@ -37,9 +37,11 @@ def pass_bar(ctrl, msg):
|
||||
"""
|
||||
俯身通过一个栅栏
|
||||
"""
|
||||
step_num = 8
|
||||
|
||||
# 切换步态
|
||||
usergait_msg = file_send_lcmt()
|
||||
lcm_usergait = lcm.LCM("udpm://239.255.76.67:7671?ttl=255")
|
||||
# lcm_usergait = lcm.LCM("udpm://239.255.76.67:7671?ttl=255")
|
||||
|
||||
try:
|
||||
steps = toml.load("./task_4/Gait_Params_stoop.toml")
|
||||
@ -78,20 +80,14 @@ def pass_bar(ctrl, msg):
|
||||
file_obj_gait_def = open("./task_4/Gait_Def_stoop.toml",'r')
|
||||
file_obj_gait_params = open("./task_4/Gait_Params_stoop_full.toml",'r')
|
||||
usergait_msg.data = file_obj_gait_def.read()
|
||||
lcm_usergait.publish("user_gait_file",usergait_msg.encode())
|
||||
ctrl.lc_s.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())
|
||||
ctrl.lc_s.publish("user_gait_file", usergait_msg.encode())
|
||||
time.sleep(0.1)
|
||||
file_obj_gait_def.close()
|
||||
file_obj_gait_params.close()
|
||||
|
||||
file_obj_gait_params = open("./task_4/Gait_Params_stoop_full.toml",'r')
|
||||
usergait_msg.data = file_obj_gait_params.read()
|
||||
lcm_usergait.publish("user_gait_file", usergait_msg.encode())
|
||||
time.sleep(0.5)
|
||||
file_obj_gait_params.close()
|
||||
|
||||
msg.mode = 62
|
||||
msg.value = 0
|
||||
msg.contact = 15
|
||||
@ -99,9 +95,9 @@ def pass_bar(ctrl, msg):
|
||||
msg.duration = 1000
|
||||
msg.life_count += 1
|
||||
|
||||
for i in range(50):
|
||||
for i in range(step_num):
|
||||
ctrl.Send_cmd(msg)
|
||||
time.sleep(0.2)
|
||||
time.sleep(1)
|
||||
|
||||
except KeyboardInterrupt:
|
||||
msg.mode = 7 #PureDamper before KeyboardInterrupt:
|
||||
|
||||
@ -45,11 +45,17 @@ def run_task_4_back(ctrl, msg):
|
||||
msg: 控制消息对象
|
||||
image_processor: 可选的图像处理器实例
|
||||
"""
|
||||
section('任务4-1：移动直到灰色天空比例小于阈值', "天空检测")
|
||||
go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.2)
|
||||
section('任务4-1：移动直到灰色天空比例低于阈值', "天空检测")
|
||||
go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.30)
|
||||
|
||||
section('任务4-2：通过栏杆', "移动")
|
||||
pass_bar(ctrl, msg)
|
||||
|
||||
go_straight(ctrl, msg, distance=0.5, speed=0.35)
|
||||
time.sleep(5)
|
||||
ctrl.base_msg.stand_up()
|
||||
|
||||
return
|
||||
|
||||
section('任务4-3：前进直到遇到黄线 - 石板路', "移动")
|
||||
# 使用新创建的函数，直走直到遇到黄线并停在距离黄线0.5米处
|
||||
@ -66,20 +72,25 @@ def run_task_4_back(ctrl, msg):
|
||||
)
|
||||
|
||||
|
||||
def go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.2, step_distance=0.5, max_distance=10, speed=0.3):
|
||||
def go_straight_until_sky_ratio_below(ctrl, msg,
|
||||
sky_ratio_threshold=0.2,
|
||||
step_distance=0.5,
|
||||
max_distance=2,
|
||||
speed=0.3
|
||||
):
|
||||
"""
|
||||
控制机器人沿直线行走，直到灰色天空比例小于指定阈值
|
||||
控制机器人沿直线行走，直到灰色天空比例低于指定阈值
|
||||
|
||||
参数:
|
||||
ctrl: Robot_Ctrl对象
|
||||
msg: 控制命令消息对象
|
||||
sky_ratio_threshold: 灰色天空比例阈值，当检测到的比例小于此值时停止
|
||||
sky_ratio_threshold: 灰色天空比例阈值，当检测到的比例低于此值时停止
|
||||
step_distance: 每次移动的步长(米)
|
||||
max_distance: 最大移动距离(米)，防止无限前进
|
||||
speed: 移动速度(米/秒)
|
||||
|
||||
返回:
|
||||
bool: 是否成功找到天空比例小于阈值的位置
|
||||
bool: 是否成功找到天空比例低于阈值的位置
|
||||
"""
|
||||
total_distance = 0
|
||||
success_flag = False
|
||||
@ -106,9 +117,9 @@ def go_straight_until_sky_ratio_below(ctrl, msg, sky_ratio_threshold=0.2, step_d
|
||||
sky_ratio = analyze_gray_sky_ratio(temp_image_path)
|
||||
info(f"当前灰色天空比例: {sky_ratio:.2%}", "分析")
|
||||
|
||||
# 如果天空比例小于阈值，停止移动
|
||||
# 如果天空比例高于阈值，停止移动
|
||||
if sky_ratio < sky_ratio_threshold:
|
||||
success(f"检测到灰色天空比例({sky_ratio:.2%})小于阈值({sky_ratio_threshold:.2%})，停止移动", "完成")
|
||||
success(f"检测到灰色天空比例({sky_ratio:.2%})低于阈值({sky_ratio_threshold:.2%})，停止移动", "完成")
|
||||
success_flag = True
|
||||
break
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user