diff --git a/ExoMy_Software-master/launch/exomy.launch b/ExoMy_Software-master/launch/exomy.launch
index 3dca58d..d10428f 100644
--- a/ExoMy_Software-master/launch/exomy.launch
+++ b/ExoMy_Software-master/launch/exomy.launch
@@ -7,20 +7,7 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
diff --git a/ExoMy_Software-master/src/joystick_parser_node.py b/ExoMy_Software-master/src/joystick_parser_node.py
index 6ad141c..3520feb 100644
--- a/ExoMy_Software-master/src/joystick_parser_node.py
+++ b/ExoMy_Software-master/src/joystick_parser_node.py
@@ -8,13 +8,55 @@ import math
# Define locomotion modes
global locomotion_mode
global motors_enabled
+global last_start_button_pressed
locomotion_mode = LocomotionMode.ACKERMANN.value
motors_enabled = True
-
AXIS_DEADZONE = 0.1
+last_start_button_pressed = False
+
VELOCITY_EXPO = 2.0
+DEFAULT_CONTROLLER = "logitech-F710"
+CONTROLLER_FUNCTION_MAPS = {
+ "webgui": {
+ "x_axis": 0,
+ "y_axis": 1,
+ "invert_x_axis": False,
+ "X_button": 0,
+ "Y_button": 3,
+ "A_button": 1,
+ "B_button": 2,
+ "start_button": 9,
+ "select_button": 8,
+ "sensitivity": 0.15,
+ },
+ "logitech-F710": {
+ "x_axis": 0,
+ "y_axis": 1,
+ "invert_x_axis": False,
+ "X_button": 0,
+ "Y_button": 3,
+ "A_button": 1,
+ "B_button": 2,
+ "start_button": 9,
+ "select_button": 8,
+ "sensitivity": 0.10,
+ },
+ "xbox-one": {
+ "x_axis": 0,
+ "y_axis": 1,
+ "invert_x_axis": False,
+ "X_button": 2,
+ "Y_button": 3,
+ "A_button": 0,
+ "B_button": 1,
+ "start_button": 7,
+ "select_button": 6,
+ "sensitivity": 0.11,
+ },
+}
+
def apply_deadzone(value, deadzone):
if abs(value) <= deadzone:
@@ -24,10 +66,34 @@ def apply_deadzone(value, deadzone):
return math.copysign(scaled, value)
+def get_controller_function_map(data):
+ if data.header.frame_id == "webgui":
+ return CONTROLLER_FUNCTION_MAPS["webgui"]
+
+ controller_name = rospy.get_param("controller", DEFAULT_CONTROLLER)
+ if controller_name in CONTROLLER_FUNCTION_MAPS:
+ return CONTROLLER_FUNCTION_MAPS[controller_name]
+
+ rospy.logwarn("Unbekannter Controller '%s', nutze Fallback '%s'.",
+ controller_name, DEFAULT_CONTROLLER)
+ return CONTROLLER_FUNCTION_MAPS[DEFAULT_CONTROLLER]
+
+
+def get_axis_value(axes, axis_index):
+ if axis_index < len(axes):
+ return axes[axis_index]
+ return 0.0
+
+
+def is_button_pressed(buttons, button_index):
+ return button_index < len(buttons) and buttons[button_index] == 1
+
+
def callback(data):
global locomotion_mode
global motors_enabled
+ global last_start_button_pressed
rover_cmd = RoverCommand()
@@ -40,29 +106,40 @@ def callback(data):
# Left Stick | Control speed and direction
# START Button | Enable and disable motors
+ controller_function_map = get_controller_function_map(data)
+
# Reading out joystick data
- y = apply_deadzone(data.axes[1], AXIS_DEADZONE)
- x = apply_deadzone(data.axes[0], AXIS_DEADZONE)
+ y = apply_deadzone(
+ get_axis_value(data.axes, controller_function_map["y_axis"]),
+ controller_function_map["sensitivity"])
+ x = apply_deadzone(
+ get_axis_value(data.axes, controller_function_map["x_axis"]),
+ controller_function_map["sensitivity"])
+
+ if controller_function_map["invert_x_axis"]:
+ x *= -1
# Reading out button data to set locomotion mode
# X Button
- if (data.buttons[0] == 1):
+ if is_button_pressed(data.buttons, controller_function_map["X_button"]):
locomotion_mode = LocomotionMode.POINT_TURN.value
# A Button
- if (data.buttons[1] == 1):
+ if is_button_pressed(data.buttons, controller_function_map["A_button"]):
locomotion_mode = LocomotionMode.ACKERMANN.value
# B Button
- if (data.buttons[2] == 1):
+ if is_button_pressed(data.buttons, controller_function_map["B_button"]):
pass
# Y Button
- if (data.buttons[3] == 1):
+ if is_button_pressed(data.buttons, controller_function_map["Y_button"]):
locomotion_mode = LocomotionMode.CRABBING.value
rover_cmd.locomotion_mode = locomotion_mode
# Enable and disable motors
# START Button
- if (data.buttons[9] == 1):
+ start_button_pressed = is_button_pressed(
+ data.buttons, controller_function_map["start_button"])
+ if start_button_pressed and not last_start_button_pressed:
if motors_enabled is True:
motors_enabled = False
rospy.loginfo("Motors disabled!")
@@ -73,12 +150,13 @@ def callback(data):
rospy.logerr(
"Exceptional value for [motors_enabled] = {}".format(motors_enabled))
motors_enabled = False
+ last_start_button_pressed = start_button_pressed
rover_cmd.motors_enabled = motors_enabled
# The velocity is decoded as value between 0...100
stick_length = min(math.sqrt(x*x + y*y), 1.0)
- rover_cmd.vel = 100 * math.pow(stick_length, VELOCITY_EXPO)
+ rover_cmd.vel = int(100 * math.pow(stick_length, VELOCITY_EXPO))
# The steering is described as an angle between -180...180
# Which describe the joystick position as follows:
@@ -86,7 +164,7 @@ def callback(data):
# 0 +-180
# -90
#
- rover_cmd.steering = math.atan2(y, x)*180.0/math.pi
+ rover_cmd.steering = int(math.atan2(y, x)*180.0/math.pi)
rover_cmd.connected = True
diff --git a/ExoMy_Software-master/src/rover.py b/ExoMy_Software-master/src/rover.py
index 3c72d63..ed759b6 100644
--- a/ExoMy_Software-master/src/rover.py
+++ b/ExoMy_Software-master/src/rover.py
@@ -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