def prepare_rotation_msg(self, robot_heading):
msg = ActionAtTime()
msg.angle = angle_to_180(self.get_turning_angle(robot_heading))
msg.time = self.get_rotation_time(msg.angle) # TODO time!!!
msg.movement = False
msg.rotation = True
return msg
def prepare_rotation_msg(self, robot_turning_angle):
msg = ActionAtTime()
msg.movement = False
msg.rotation = True
robot_turning_angle = angle_to_180(robot_turning_angle)
if robot_turning_angle < -90:
robot_turning_angle = 180 + robot_turning_angle
elif robot_turning_angle > 90:
robot_turning_angle = robot_turning_angle - 180
msg.angle = robot_turning_angle
msg.time = self.get_rotation_time(robot_turning_angle)
return msg
def get_turning_angle(self):
robot_heading_slope = self.get_robot_heading_slope()
robot_heading_slope = self.turn_robot_slope_to_sector_slope(
robot_heading_slope)
sector_slope = self.get_sector_slope()
robot_heading_slope = robot_heading_slope if robot_heading_slope > 0 else robot_heading_slope + 180
sector_slope = sector_slope if sector_slope > 0 else sector_slope + 180
turning_angle = sector_slope - robot_heading_slope
turning_angle = angle_to_180(turning_angle)
if turning_angle < -90:
turning_angle = 180 + turning_angle
elif turning_angle > 90:
turning_angle = turning_angle - 180
return turning_angle
def prepare_move_msg(self, dst_point):
moving_angle = degrees(
get_angle_by_3_points(self.robot.direction, dst_point,
self.robot.center))
angle_sign = get_angle_sign_pt_to_line(dst_point, self.robot.center,
self.robot.direction)
moving_angle *= angle_sign
msg = ActionAtTime()
msg.angle = self.angle_to_sector(angle_to_180(moving_angle))
msg.movement = True
msg.rotation = False
msg.time = self.get_moving_time(
get_distance_between_points(self.robot.center, dst_point))
return msg
def prepare_msg(self):
robot_turning_angle = self.get_turning_angle()
robot_turning_angle = angle_to_180(robot_turning_angle)
if abs(robot_turning_angle) > self.angle_eps:
msg = self.prepare_rotation_msg(robot_turning_angle)
else:
robot_projection_on_section = self.get_robots_projection_on_section_line(
)
dist_to_proj = get_distance_between_points(
self.robot.center, robot_projection_on_section)
dst_point = self.path_section[
-1] if dist_to_proj <= self.distance_eps else robot_projection_on_section
if not self.robot_on_point(dst_point):
msg = self.prepare_movement_msg(dst_point)
else:
msg = self.prepare_stop_msg()
return msg
def get_turning_angle(self, robot_heading):
turning_angle = self.dst_heading - robot_heading
return angle_to_180(turning_angle)
def get_robot_heading(self):
return angle_to_180(
self.point_to_angle(self.robot.center, self.robot.direction))