@@ -18,63 +18,63 @@ def __init__(self):
1818 self .current_yaw = 0.0
1919 self .start_yaw = None
2020 self .turning = False
21- self .turn_angle = math .radians (90 ) # 角度调这里
22- self .angular_speed = - 0.2 # 方向和速度调这里
21+ self .turn_angle = math .radians (90 ) # angle
22+ self .angular_speed = - 0.2 # dirention and speed
2323 self .rate = rospy .Rate (10 ) # 10Hz
2424
2525 def odom_callback (self , msg ):
26- # 从四元数获取偏航角
26+ # Get the yaw angle from a quaternion.
2727 orientation = msg .pose .pose .orientation
2828 quaternion = [orientation .x , orientation .y , orientation .z , orientation .w ]
2929 _ , _ , yaw = euler_from_quaternion (quaternion )
3030
3131 self .current_yaw = yaw
3232
33- # 初始化起始偏航角
33+ # initialize the start yaw
3434 if self .start_yaw is None and not self .turning :
3535 self .start_yaw = yaw
36- rospy .loginfo (f"起始偏航角 : { math .degrees (self .start_yaw ):.2f} 度 " )
36+ rospy .loginfo (f"start yaw : { math .degrees (self .start_yaw ):.2f} )
3737
3838 def execute_turn (self ):
3939 if self .start_yaw is None :
40- rospy .loginfo ("等待初始位姿数据 ..." )
40+ rospy .loginfo ("wait for init yaw state ..." )
4141 return False
4242
4343 if not self .turning :
4444 self .turning = True
45- rospy .loginfo ("开始原地转弯90度 " )
45+ rospy .loginfo ("start to turn " )
4646
47- # 计算已转过的角度
47+ # compute turned angle
4848 current_angle = self .current_yaw - self .start_yaw
4949
50- # 处理角度超过π或小于-π的情况（角度归一化）
50+ # normalize the angle
5151 if current_angle > math .pi :
5252 current_angle -= 2 * math .pi
5353 elif current_angle < - math .pi :
5454 current_angle += 2 * math .pi
5555
56- # 计算还需转过的角度
56+ # compute remaining angle
5757 remaining_angle = self .turn_angle - abs (current_angle )
5858
59- # 创建 Twist 消息
59+ # create Twist msg
6060 twist = Twist ()
6161
62- # 如果还没达到目标角度，继续旋转
63- if remaining_angle > 0.05 : # 留一点余量（约2.86度 ）
62+ # if not reach the goal angle, keep turning
63+ if remaining_angle > 0.05 : # allow some diff (about 2.86 degree ）
6464 twist .angular .z = self .angular_speed * min (1.0 , remaining_angle * 6 )
6565 print (f"twist.angular.z { twist .angular .z } { remaining_angle } )
6666 self .cmd_vel_pub .publish (twist )
6767 return False
6868 else :
6969 twist .angular .z = 0.0
7070 self .cmd_vel_pub .publish (twist )
71- rospy .loginfo (f"完成转弯，最终偏航角 : { math .degrees (self .current_yaw ):.2f} 度 " )
71+ rospy .loginfo (f"finish turn, final yaw : { math .degrees (self .current_yaw ):.2f} )
7272 return True
7373
7474 def run (self ):
7575 while not rospy .is_shutdown ():
7676 if self .execute_turn ():
77- rospy .loginfo ("任务完成，退出节点 " )
77+ rospy .loginfo ("Task finished " )
7878 break
7979 self .rate .sleep ()
8080
@@ -112,7 +112,7 @@ def move_forward(self, distance=0.25):
112112 rospy .loginfo ("Move forward command executed." )
113113
114114 def turn_left (self , angle = 15 , speed = 0.2 ):
115- self .turn_angle = math .radians (angle ) # 角度调这里
115+ self .turn_angle = math .radians (angle ) # update angle
116116 self .angular_speed = speed # Set positive angular speed for left turn
117117 self .start_yaw = None # Reset start yaw to current position
118118 self .turning = False # Reset turning flag
@@ -121,7 +121,7 @@ def turn_left(self, angle=15, speed=0.2):
121121 rospy .loginfo ("Turn left command executed." )
122122
123123 def turn_right (self , angle = 15 , speed = - 0.2 ):
124- self .turn_angle = math .radians (angle ) # 角度调这里
124+ self .turn_angle = math .radians (angle ) # update angle
125125 self .angular_speed = speed # Set positive angular speed for left turn
126126 self .start_yaw = None # Reset start yaw to current position
127127 self .turning = False # Reset turning flag
0 commit comments