Verbessert
This commit is contained in:
@@ -1,8 +1,6 @@
|
||||
#!/usr/bin/env python
|
||||
import rospy
|
||||
import time
|
||||
import math
|
||||
import enum
|
||||
from locomotion_modes import LocomotionMode
|
||||
import numpy as np
|
||||
|
||||
@@ -21,14 +19,19 @@ class Rover():
|
||||
def __init__(self):
|
||||
self.locomotion_mode = LocomotionMode.FAKE_ACKERMANN
|
||||
|
||||
self.wheel_x = 12.0
|
||||
self.wheel_y = 20.0
|
||||
# x = Achsabstand, y = Spurbreite
|
||||
self.wheel_rx = 14.0
|
||||
self.wheel_ry = 20.3
|
||||
self.wheel_fx = 16.0
|
||||
self.wheel_fy = 20.3
|
||||
|
||||
max_steering_angle = 45
|
||||
self.ackermann_r_min = abs(
|
||||
self.wheel_y) / math.tan(max_steering_angle * math.pi / 180.0) + self.wheel_x
|
||||
|
||||
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 setLocomotionMode(self, locomotion_mode_command):
|
||||
'''
|
||||
@@ -114,46 +117,52 @@ class Rover():
|
||||
if(self.locomotion_mode == LocomotionMode.ACKERMANN.value):
|
||||
|
||||
# No steering if robot is not driving
|
||||
if(driving_command is 0):
|
||||
if(driving_command == 0):
|
||||
return steering_angles
|
||||
|
||||
# Scale between min and max Ackermann radius
|
||||
if math.cos(math.radians(steering_command)) == 0:
|
||||
r = self.ackermann_r_max
|
||||
else:
|
||||
r = self.ackermann_r_max - \
|
||||
return steering_angles
|
||||
|
||||
radius = self.ackermann_r_max - \
|
||||
abs(math.cos(math.radians(steering_command))) * \
|
||||
((self.ackermann_r_max-self.ackermann_r_min))
|
||||
|
||||
# No steering
|
||||
if r == self.ackermann_r_max:
|
||||
return steering_angles
|
||||
|
||||
inner_angle = int(math.degrees(
|
||||
math.atan(self.wheel_x/(abs(r)-self.wheel_y))))
|
||||
outer_angle = int(math.degrees(
|
||||
math.atan(self.wheel_x/(abs(r)+self.wheel_y))))
|
||||
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] = outer_angle
|
||||
steering_angles[self.FR] = inner_angle
|
||||
steering_angles[self.RL] = -outer_angle
|
||||
steering_angles[self.RR] = -inner_angle
|
||||
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] = -inner_angle
|
||||
steering_angles[self.FR] = -outer_angle
|
||||
steering_angles[self.RL] = inner_angle
|
||||
steering_angles[self.RR] = outer_angle
|
||||
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):
|
||||
steering_angles[self.FL] = 45
|
||||
steering_angles[self.FR] = -45
|
||||
steering_angles[self.RL] = -45
|
||||
steering_angles[self.RR] = 45
|
||||
point_turn_angle = int(math.degrees(
|
||||
math.atan((self.wheel_rx + self.wheel_fx) / self.wheel_ry)))
|
||||
point_turn_angle_center = int(math.degrees(
|
||||
math.atan((((self.wheel_rx + self.wheel_fx) / 2) - self.wheel_fx) / (self.wheel_ry / 2))))
|
||||
|
||||
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):
|
||||
@@ -220,53 +229,62 @@ class Rover():
|
||||
if (radius == self.ackermann_r_max):
|
||||
return [v] * 6
|
||||
else:
|
||||
rmax = radius + self.wheel_x
|
||||
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))))
|
||||
|
||||
a = math.pow(self.wheel_y, 2)
|
||||
b = math.pow(abs(radius) + self.wheel_x, 2)
|
||||
c = math.pow(abs(radius) - self.wheel_x, 2)
|
||||
rmax_float = float(rmax)
|
||||
reference_radius = max(r1, r2, r3, r4, r5, r6)
|
||||
|
||||
r1 = math.sqrt(a+b)
|
||||
r2 = rmax_float
|
||||
r3 = r1
|
||||
r4 = math.sqrt(a+c)
|
||||
r5 = abs(radius) - self.wheel_x
|
||||
r6 = r4
|
||||
|
||||
v1 = int(v)
|
||||
v2 = int(v*r2/r1)
|
||||
v3 = v1
|
||||
v4 = int(v*r4/r1)
|
||||
v5 = int(v*r5/r1)
|
||||
v6 = v4
|
||||
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 = [v1, v2, v3, v4, v5, v6]
|
||||
motor_speeds = [v2, v1, v4, v3, v6, v5]
|
||||
else:
|
||||
motor_speeds = [v6, v5, v4, v3, v2, v1]
|
||||
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 is not 0):
|
||||
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] = -50
|
||||
motor_speeds[self.FR] = 50
|
||||
motor_speeds[self.CL] = -50
|
||||
motor_speeds[self.CR] = 50
|
||||
motor_speeds[self.RL] = -50
|
||||
motor_speeds[self.RR] = 50
|
||||
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] = 50
|
||||
motor_speeds[self.FR] = -50
|
||||
motor_speeds[self.CL] = 50
|
||||
motor_speeds[self.CR] = -50
|
||||
motor_speeds[self.RL] = 50
|
||||
motor_speeds[self.RR] = -50
|
||||
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
|
||||
|
||||
Reference in New Issue
Block a user