Verbessert

ExoMy_Software-master/launch/exomy.launch

@@ -7,20 +7,7 @@
         <param name="coalesce_interval" value="0.05"/>
     </node>
 
-    <node pkg="web_video_server" type="web_video_server"  name="web_video_server" respawn="false" output="screen">
+    <param name="controller" value="logitech-F710"/>
-        <param name="default_transport" value="compressed"/>
-        <param name="quality" value="50"/>
-    </node>
-
-    <node pkg="usb_cam" type="usb_cam_node" name="pi_cam" respawn="false" output="screen">
-        <param name="framerate" value="10"/>
-        <param name="video_device" value="/dev/video0"/>
-        <param name="image_width" value="640"/>
-        <param name="image_height" value="480"/>
-        <param name="pixel_format" value="yuyv"/>
-        <param name="camera_frame_id" value="pi_cam"/>
-        <param name="io_method" value="mmap"/>
-    </node>
 
     <include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch"/>
     <rosparam file="$(find exomy)/config/exomy.yaml"/> 

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)
+    y = apply_deadzone(
-    x = apply_deadzone(data.axes[0], AXIS_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
 

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
+        # x = Achsabstand, y = Spurbreite
-        self.wheel_y = 20.0
+        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
+                return steering_angles
-            else:
+
-                r = self.ackermann_r_max - \
+            radius = self.ackermann_r_max - \
                     abs(math.cos(math.radians(steering_command))) * \
                     ((self.ackermann_r_max-self.ackermann_r_min))
 
-            # No steering
+            rear_inner_angle = int(math.degrees(
-            if r == self.ackermann_r_max:
+                math.atan(self.wheel_rx / (abs(radius) - (self.wheel_ry / 2)))))
-                return steering_angles
+            rear_outer_angle = int(math.degrees(
-
+                math.atan(self.wheel_rx / (abs(radius) + (self.wheel_ry / 2)))))
-            inner_angle = int(math.degrees(
+            front_inner_angle = int(math.degrees(
-                math.atan(self.wheel_x/(abs(r)-self.wheel_y))))
+                math.atan(self.wheel_fx / (abs(radius) - (self.wheel_fy / 2)))))
-            outer_angle = int(math.degrees(
+            front_outer_angle = int(math.degrees(
-                math.atan(self.wheel_x/(abs(r)+self.wheel_y))))
+                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.FL] = front_outer_angle
-                steering_angles[self.FR] = inner_angle
+                steering_angles[self.FR] = front_inner_angle
-                steering_angles[self.RL] = -outer_angle
+                steering_angles[self.RL] = -rear_outer_angle
-                steering_angles[self.RR] = -inner_angle
+                steering_angles[self.RR] = -rear_inner_angle
             else:
                 # Steering to the left
-                steering_angles[self.FL] = -inner_angle
+                steering_angles[self.FL] = -front_inner_angle
-                steering_angles[self.FR] = -outer_angle
+                steering_angles[self.FR] = -front_outer_angle
-                steering_angles[self.RL] = inner_angle
+                steering_angles[self.RL] = rear_inner_angle
-                steering_angles[self.RR] = outer_angle
+                steering_angles[self.RR] = rear_outer_angle
 
             return steering_angles
 
         if(self.locomotion_mode == LocomotionMode.POINT_TURN.value):
-            steering_angles[self.FL] = 45
+            point_turn_angle = int(math.degrees(
-            steering_angles[self.FR] = -45
+                math.atan((self.wheel_rx + self.wheel_fx) / self.wheel_ry)))
-            steering_angles[self.RL] = -45
+            point_turn_angle_center = int(math.degrees(
-            steering_angles[self.RR] = 45
+                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)
+                reference_radius = max(r1, r2, r3, r4, r5, r6)
-                b = math.pow(abs(radius) + self.wheel_x, 2)
-                c = math.pow(abs(radius) - self.wheel_x, 2)
-                rmax_float = float(rmax)
 
-                r1 = math.sqrt(a+b)
+                v1 = int(v * r1 / reference_radius)
-                r2 = rmax_float
+                v2 = int(v * r2 / reference_radius)
-                r3 = r1
+                v3 = int(v * r3 / reference_radius)
-                r4 = math.sqrt(a+c)
+                v4 = int(v * r4 / reference_radius)
-                r5 = abs(radius) - self.wheel_x
+                v5 = int(v * r5 / reference_radius)
-                r6 = r4
+                v6 = int(v * r6 / reference_radius)
-
-                v1 = int(v)
-                v2 = int(v*r2/r1)
-                v3 = v1
-                v4 = int(v*r4/r1)
-                v5 = int(v*r5/r1)
-                v6 = v4
 
                 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.FL] = -v_outer
-                    motor_speeds[self.FR] = 50
+                    motor_speeds[self.FR] = v_outer
-                    motor_speeds[self.CL] = -50
+                    motor_speeds[self.CL] = -v_inner
-                    motor_speeds[self.CR] = 50
+                    motor_speeds[self.CR] = v_inner
-                    motor_speeds[self.RL] = -50
+                    motor_speeds[self.RL] = -v_outer
-                    motor_speeds[self.RR] = 50
+                    motor_speeds[self.RR] = v_outer
                 # Right turn
                 elif(deg > 95 or deg < -95):
-                    motor_speeds[self.FL] = 50
+                    motor_speeds[self.FL] = v_outer
-                    motor_speeds[self.FR] = -50
+                    motor_speeds[self.FR] = -v_outer
-                    motor_speeds[self.CL] = 50
+                    motor_speeds[self.CL] = v_inner
-                    motor_speeds[self.CR] = -50
+                    motor_speeds[self.CR] = -v_inner
-                    motor_speeds[self.RL] = 50
+                    motor_speeds[self.RL] = v_outer
-                    motor_speeds[self.RR] = -50
+                    motor_speeds[self.RR] = -v_outer
             else:
                 # Stop
                 motor_speeds[self.FL] = 0