diff --git a/base_move/turn_degree.py b/base_move/turn_degree.py index 78c9772..02f7740 100644 --- a/base_move/turn_degree.py +++ b/base_move/turn_degree.py @@ -1,7 +1,7 @@ import math import time -def turn_degree(ctrl, msg, degree=90): +def turn_degree(ctrl, msg, degree=90, relative=True): """ 结合里程计实现精确稳定的旋转指定角度 @@ -9,7 +9,8 @@ def turn_degree(ctrl, msg, degree=90): ctrl: Robot_Ctrl 对象,包含里程计信息 msg: robot_control_cmd_lcmt 对象,用于发送命令 degree: 要旋转的角度,正值表示逆时针,负值表示顺时针,默认为90度 - + relative: 是否相对于当前角度,默认为True + 返回: Bool: 是否成功旋转到指定角度 """ @@ -20,7 +21,10 @@ def turn_degree(ctrl, msg, degree=90): current_yaw = ctrl.odo_msg.rpy[2] # 计算目标朝向 - target_yaw = current_yaw + target_rad + if relative: + target_yaw = current_yaw + target_rad + else: + target_yaw = target_rad # 标准化目标角度到 [-pi, pi] 范围 if target_yaw > math.pi: diff --git a/main.py b/main.py index 31e4f84..87ae312 100644 --- a/main.py +++ b/main.py @@ -37,7 +37,7 @@ def main(): # run_task_1(Ctrl, msg, image_processor) - # run_task_5(Ctrl, msg) + run_task_5(Ctrl, msg) # time.sleep(100)