def rotational_speed(self, speed_constraints, dt):
diff = center_radians(self._rotation_goal - self.current_pose.theta)
if diff < 0:
acceleration = -ROTATION_ACCELERATION_MAX
else:
acceleration = ROTATION_ACCELERATION_MAX
t_rotation_stop = abs(self.current_speed.vtheta / acceleration)
mul = 1
if abs(diff) <= 3 * ADMITTED_ANGLE_ERROR:
mul = 0.2
omega = self.current_speed.vtheta + dt * acceleration * mul
planned_stop_angle = center_radians(self.current_pose.theta + 2 * (
self.current_speed.vtheta * t_rotation_stop - 1 / 2 * acceleration * t_rotation_stop ** 2))
self.robot.ivy.highlight_robot_angle(0, self._rotation_goal)
self.robot.ivy.highlight_robot_angle(1, planned_stop_angle)
planned_angular_error = center_radians(self._rotation_goal - planned_stop_angle)
if abs(planned_angular_error) <= ADMITTED_ANGLE_ERROR or diff * planned_angular_error < 0:
speed_diff = acceleration * dt
if abs(speed_diff) > abs(self.current_speed.vtheta):
omega = 0
else:
omega = self.current_speed.vtheta - speed_diff
elif abs(planned_angular_error) <= 3 * ADMITTED_ANGLE_ERROR and abs(self.current_speed.vtheta) > 0.3:
omega = self.current_speed.vtheta
omega = min(speed_constraints.max_vtheta, max(speed_constraints.min_vtheta, omega))
# print(omega)
return Speed(0, 0, omega)
def compute_speed(self, delta_time, speed_constraints):
if self.state == self.eState.IDLE:
return Speed(0, 0, 0)
elif self.state == self.eState.STRAIGHT:
if abs(self._relative_straight_goal) <= ADMITTED_POSITION_ERROR:
self.state = self.eState.IDLE
return Speed(0, 0, 0)
else:
dist_remaining = abs(self._relative_straight_goal) - self._start_position.lin_distance_to_point(self.current_pose)
if self._relative_straight_goal <0:
dist_remaining = -dist_remaining
if abs(dist_remaining) <= ADMITTED_POSITION_ERROR:
self.state = self.eState.IDLE
return Speed(0, 0, 0)
else:
return self.linear_speed(speed_constraints, dist_remaining, delta_time)
elif self.state == self.eState.ROTATE:
if abs(center_radians(self.theta - self._rotation_goal)) <= ADMITTED_ANGLE_ERROR:
self.state = self.eState.IDLE
return Speed(0, 0, 0)
else:
return self.rotational_speed(speed_constraints, delta_time)
def new_rotate_goal(self, relative_goal):
if abs(relative_goal) <= ADMITTED_ANGLE_ERROR:
return
self._rotation_goal = center_radians(self.current_pose.theta + relative_goal)
self.state = self.eState.ROTATE
def __init__(self, behavior):
super().__init__(behavior)
self.has_turned = False
self.robot.locomotion.turn(
center_radians(math.pi / 2 - self.robot.locomotion.theta))