#!/usr/bin/env python import rospy import math from locomotion_modes import LocomotionMode import numpy as np class Rover(): ''' Rover class contains all the math and motor control algorithms to move the rover ''' # Defining wheel names FL, FR, CL, CR, RL, RR = range(0, 6) # Defining locomotion modes FAKE_ACKERMANN, ACKERMANN, POINT_TURN, CRABBING = range(0, 4) def __init__(self): self.locomotion_mode = LocomotionMode.FAKE_ACKERMANN self.ackermann_straight_tolerance_deg = 5 # x = Achsabstand, y = Spurbreite self.wheel_rx = 14.0 self.wheel_ry = 20.3 self.wheel_fx = 16.0 self.wheel_fy = 20.3 self.point_turn_max_angle = 45 max_steering_angle = 45 self.ackermann_r_max = 250 self.ackermann_rr_min = abs( self.wheel_rx) / math.tan(max_steering_angle * math.pi / 180.0) + (self.wheel_ry / 2) self.ackermann_fr_min = abs( self.wheel_fx) / math.tan(max_steering_angle * math.pi / 180.0) + (self.wheel_fy / 2) self.ackermann_r_min = max(self.ackermann_fr_min, self.ackermann_rr_min) def is_ackermann_straight(self, steering_command): return abs(abs(steering_command) - 90) <= self.ackermann_straight_tolerance_deg def setLocomotionMode(self, locomotion_mode_command): ''' Sets the locomotion mode ''' if(self.locomotion_mode != locomotion_mode_command): self.locomotion_mode = locomotion_mode_command rospy.loginfo('Set locomotion mode to: %s', LocomotionMode(locomotion_mode_command).name) def joystickToSteeringAngle(self, driving_command, steering_command): ''' Converts the steering command [angle of joystick] to angles for the different motors :param int driving_command: Drive speed command range from -100 to 100 :param int stering_command: Turning radius command with the values 0(left) +90(forward) -90(backward) +-180(right) ''' steering_angles = [0]*6 deg = steering_command if(self.locomotion_mode == LocomotionMode.FAKE_ACKERMANN.value): if (driving_command == 0): # Stop steering_angles[self.FL] = 0 steering_angles[self.FR] = 0 steering_angles[self.CR] = 0 steering_angles[self.CL] = 0 steering_angles[self.RL] = 0 steering_angles[self.RR] = 0 return steering_angles if(80 < deg < 100): # Drive straight forward steering_angles[self.FL] = 0 steering_angles[self.FR] = 0 steering_angles[self.CR] = 0 steering_angles[self.CL] = 0 steering_angles[self.RL] = 0 steering_angles[self.RR] = 0 elif(-80 < deg < -100): # Drive straight backwards steering_angles[self.FL] = 0 steering_angles[self.FR] = 0 steering_angles[self.CR] = 0 steering_angles[self.CL] = 0 steering_angles[self.RL] = 0 steering_angles[self.RR] = 0 elif(100 < deg <= 180): # Drive right forwards steering_angles[self.FL] = 45 steering_angles[self.FR] = 45 steering_angles[self.CR] = 0 steering_angles[self.CL] = 0 steering_angles[self.RL] = -45 steering_angles[self.RR] = -45 elif(-100 > deg >= -180): # Drive right backwards steering_angles[self.FL] = 45 steering_angles[self.FR] = 45 steering_angles[self.CR] = 0 steering_angles[self.CL] = 0 steering_angles[self.RL] = -45 steering_angles[self.RR] = -45 elif(80 > deg >= 0): # Drive left forwards steering_angles[self.FL] = -45 steering_angles[self.FR] = -45 steering_angles[self.CR] = 0 steering_angles[self.CL] = 0 steering_angles[self.RL] = 45 steering_angles[self.RR] = 45 elif(0 > deg > -80): # Drive left backwards steering_angles[self.FL] = -45 steering_angles[self.FR] = -45 steering_angles[self.CR] = 0 steering_angles[self.CL] = 0 steering_angles[self.RL] = 45 steering_angles[self.RR] = 45 return steering_angles if(self.locomotion_mode == LocomotionMode.ACKERMANN.value): # No steering if robot is not driving if(driving_command == 0): return steering_angles if self.is_ackermann_straight(steering_command): return steering_angles radius = self.ackermann_r_max - \ abs(math.cos(math.radians(steering_command))) * \ ((self.ackermann_r_max-self.ackermann_r_min)) rear_inner_angle = int(math.degrees( math.atan(self.wheel_rx / (abs(radius) - (self.wheel_ry / 2))))) rear_outer_angle = int(math.degrees( math.atan(self.wheel_rx / (abs(radius) + (self.wheel_ry / 2))))) front_inner_angle = int(math.degrees( math.atan(self.wheel_fx / (abs(radius) - (self.wheel_fy / 2))))) front_outer_angle = int(math.degrees( math.atan(self.wheel_fx / (abs(radius) + (self.wheel_fy / 2))))) if steering_command > 90 or steering_command < -90: # Steering to the right steering_angles[self.FL] = front_outer_angle steering_angles[self.FR] = front_inner_angle steering_angles[self.RL] = -rear_outer_angle steering_angles[self.RR] = -rear_inner_angle else: # Steering to the left steering_angles[self.FL] = -front_inner_angle steering_angles[self.FR] = -front_outer_angle steering_angles[self.RL] = rear_inner_angle steering_angles[self.RR] = rear_outer_angle return steering_angles if(self.locomotion_mode == LocomotionMode.POINT_TURN.value): raw_point_turn_angle = math.degrees( math.atan((self.wheel_rx + self.wheel_fx) / self.wheel_ry)) raw_point_turn_angle_center = math.degrees( math.atan((((self.wheel_rx + self.wheel_fx) / 2) - self.wheel_fx) / (self.wheel_ry / 2))) point_turn_angle = int(min(self.point_turn_max_angle, raw_point_turn_angle)) center_scale = 0.0 if raw_point_turn_angle == 0 else abs(raw_point_turn_angle_center / raw_point_turn_angle) point_turn_angle_center = int(point_turn_angle * center_scale) steering_angles[self.FL] = point_turn_angle steering_angles[self.FR] = -point_turn_angle steering_angles[self.CL] = point_turn_angle_center steering_angles[self.CR] = -point_turn_angle_center steering_angles[self.RL] = -point_turn_angle steering_angles[self.RR] = point_turn_angle return steering_angles if(self.locomotion_mode == LocomotionMode.CRABBING.value): if(driving_command != 0): wheel_direction = 0 if(steering_command >= 0): wheel_direction = steering_command - 90 elif(steering_command < 0): wheel_direction = steering_command + 90 wheel_direction = np.clip(wheel_direction, -75, 75) steering_angles[self.FL] = wheel_direction steering_angles[self.FR] = wheel_direction steering_angles[self.CL] = wheel_direction steering_angles[self.CR] = wheel_direction steering_angles[self.RL] = wheel_direction steering_angles[self.RR] = wheel_direction return steering_angles def joystickToVelocity(self, driving_command, steering_command): ''' Converts the steering and drive command to the speeds of the individual motors :param int driving_command: Drive speed command range from -100 to 100 :param int stering_command: Turning radius command with the values 0(left) +90(forward) -90(backward) +-180(right) ''' motor_speeds = [0]*6 if (self.locomotion_mode == LocomotionMode.FAKE_ACKERMANN.value): if(driving_command > 0 and steering_command >= 0): motor_speeds[self.FL] = 50 motor_speeds[self.FR] = 50 motor_speeds[self.CR] = 50 motor_speeds[self.CL] = 50 motor_speeds[self.RL] = 50 motor_speeds[self.RR] = 50 elif(driving_command > 0 and steering_command <= 0): motor_speeds[self.FL] = -50 motor_speeds[self.FR] = -50 motor_speeds[self.CR] = -50 motor_speeds[self.CL] = -50 motor_speeds[self.RL] = -50 motor_speeds[self.RR] = -50 return motor_speeds if (self.locomotion_mode == LocomotionMode.ACKERMANN.value): v = driving_command if(steering_command < 0): v *= -1 # Scale between min and max Ackermann radius radius = self.ackermann_r_max - \ abs(math.cos(math.radians(steering_command))) * \ ((self.ackermann_r_max-self.ackermann_r_min)) if (v == 0): return motor_speeds if self.is_ackermann_straight(steering_command): return [v] * 6 if (radius == self.ackermann_r_max): return [v] * 6 else: r1 = (radius - (self.wheel_fy / 2)) / math.cos( math.atan(self.wheel_fx / (abs(radius) - (self.wheel_fy / 2)))) r2 = (radius + (self.wheel_fy / 2)) / math.cos( math.atan(self.wheel_fx / (abs(radius) + (self.wheel_fy / 2)))) r3 = radius - (self.wheel_fy / 2) r4 = radius + (self.wheel_fy / 2) r5 = (radius - (self.wheel_ry / 2)) / math.cos( math.atan(self.wheel_rx / (abs(radius) - (self.wheel_ry / 2)))) r6 = (radius + (self.wheel_ry / 2)) / math.cos( math.atan(self.wheel_rx / (abs(radius) + (self.wheel_ry / 2)))) reference_radius = max(r1, r2, r3, r4, r5, r6) v1 = int(v * r1 / reference_radius) v2 = int(v * r2 / reference_radius) v3 = int(v * r3 / reference_radius) v4 = int(v * r4 / reference_radius) v5 = int(v * r5 / reference_radius) v6 = int(v * r6 / reference_radius) if (steering_command > 90 or steering_command < -90): motor_speeds = [v2, v1, v4, v3, v6, v5] else: motor_speeds = [v1, v2, v3, v4, v5, v6] return motor_speeds if (self.locomotion_mode == LocomotionMode.POINT_TURN.value): outer_turning_radius = math.sqrt( math.pow(self.wheel_rx + self.wheel_fx, 2) + math.pow(self.wheel_ry, 2)) / 2 inner_turning_radius = math.sqrt( math.pow(((self.wheel_rx + self.wheel_fx) / 2) - self.wheel_rx, 2) + math.pow((self.wheel_ry / 2), 2)) deg = steering_command if(driving_command != 0): v = int(driving_command) v_outer = v v_inner = int(v * inner_turning_radius / outer_turning_radius) # Left turn if(deg < 85 and deg > -85): motor_speeds[self.FL] = -v_outer motor_speeds[self.FR] = v_outer motor_speeds[self.CL] = -v_inner motor_speeds[self.CR] = v_inner motor_speeds[self.RL] = -v_outer motor_speeds[self.RR] = v_outer # Right turn elif(deg > 95 or deg < -95): motor_speeds[self.FL] = v_outer motor_speeds[self.FR] = -v_outer motor_speeds[self.CL] = v_inner motor_speeds[self.CR] = -v_inner motor_speeds[self.RL] = v_outer motor_speeds[self.RR] = -v_outer else: # Stop motor_speeds[self.FL] = 0 motor_speeds[self.FR] = 0 motor_speeds[self.CL] = 0 motor_speeds[self.CR] = 0 motor_speeds[self.RL] = 0 motor_speeds[self.RR] = 0 return motor_speeds if(self.locomotion_mode == LocomotionMode.CRABBING.value): v = driving_command if(steering_command < 0): v *= -1 motor_speeds[self.FL] = v motor_speeds[self.FR] = v motor_speeds[self.CL] = v motor_speeds[self.CR] = v motor_speeds[self.RL] = v motor_speeds[self.RR] = v return motor_speeds