Aufgeräumt

src/delay_node.py

@@ -0,0 +1,37 @@
+#!/usr/bin/env python
+import collections
+import rospy
+from sensor_msgs.msg import Joy
+
+queue = collections.deque()
+pub = None
+
+
+def callback(msg):
+    delay = rospy.get_param('/delay_seconds', 0.0)
+    if delay <= 0:
+        pub.publish(msg)
+        return
+    queue.append((rospy.Time.now().to_sec(), msg))
+
+
+def spin():
+    rate = rospy.Rate(50)
+    while not rospy.is_shutdown():
+        delay = rospy.get_param('/delay_seconds', 0.0)
+        now = rospy.Time.now().to_sec()
+        if delay <= 0:
+            while queue:
+                pub.publish(queue.popleft()[1])
+        else:
+            while queue and (now - queue[0][0]) >= delay:
+                pub.publish(queue.popleft()[1])
+        rate.sleep()
+
+
+if __name__ == '__main__':
+    rospy.init_node('delay_node')
+    rospy.loginfo('delay_node gestartet')
+    pub = rospy.Publisher('/joy_delayed', Joy, queue_size=50)
+    rospy.Subscriber('/joy', Joy, callback, queue_size=50)
+    spin()

src/f710_joy_node.py

@@ -0,0 +1,99 @@
+#!/usr/bin/env python
+import errno
+import os
+import select
+import struct
+import time
+
+import rospy
+from sensor_msgs.msg import Joy
+
+
+DEVICE_PATH = '/dev/input/js0'
+AXIS_COUNT = 8
+BUTTON_COUNT = 12
+PUBLISH_RATE_HZ = 20.0
+
+JS_EVENT_BUTTON = 0x01
+JS_EVENT_AXIS = 0x02
+JS_EVENT_INIT = 0x80
+EVENT_SIZE = struct.calcsize('IhBB')
+
+
+def open_device():
+    while not rospy.is_shutdown():
+        try:
+            fd = os.open(DEVICE_PATH, os.O_RDONLY | os.O_NONBLOCK)
+            rospy.loginfo('Joystick verbunden: %s', DEVICE_PATH)
+            return fd
+        except OSError as exc:
+            if exc.errno != errno.ENOENT:
+                rospy.logwarn('Joystick kann nicht geoeffnet werden: %s', exc)
+            rospy.sleep(1.0)
+    return None
+
+
+def close_device(fd):
+    if fd is None:
+        return
+    try:
+        os.close(fd)
+    except OSError:
+        pass
+
+
+if __name__ == '__main__':
+    rospy.init_node('f710_joy_node')
+
+    publisher = rospy.Publisher('/joy', Joy, queue_size=5)
+    rate = rospy.Rate(PUBLISH_RATE_HZ)
+
+    axes = [0.0] * AXIS_COUNT
+    buttons = [0] * BUTTON_COUNT
+    joystick_fd = None
+
+    while not rospy.is_shutdown():
+        if joystick_fd is None:
+            joystick_fd = open_device()
+            axes = [0.0] * AXIS_COUNT
+            buttons = [0] * BUTTON_COUNT
+            if joystick_fd is None:
+                break
+
+        try:
+            readable, _, _ = select.select([joystick_fd], [], [], 0.0)
+            if readable:
+                event = os.read(joystick_fd, EVENT_SIZE)
+                while event and len(event) == EVENT_SIZE:
+                    _, value, event_type, number = struct.unpack('IhBB', event)
+                    event_type &= ~JS_EVENT_INIT
+
+                    if event_type == JS_EVENT_AXIS and number < len(axes):
+                        axes[number] = max(-1.0, min(1.0, value / 32767.0))
+                    elif event_type == JS_EVENT_BUTTON and number < len(buttons):
+                        buttons[number] = 1 if value else 0
+
+                    try:
+                        event = os.read(joystick_fd, EVENT_SIZE)
+                    except OSError as exc:
+                        if exc.errno in (errno.EAGAIN, errno.EWOULDBLOCK):
+                            break
+                        raise
+
+            message = Joy()
+            message.header.stamp = rospy.Time.now()
+            message.axes = list(axes)
+            message.buttons = list(buttons)
+            publisher.publish(message)
+            rate.sleep()
+
+        except OSError as exc:
+            if exc.errno in (errno.ENODEV, errno.EIO, errno.ENXIO, errno.EBADF):
+                rospy.logwarn('Joystick getrennt, warte auf Neuverbindung.')
+            else:
+                rospy.logwarn('Joystick-Lesefehler: %s', exc)
+            close_device(joystick_fd)
+            joystick_fd = None
+            time.sleep(1.0)
+
+    close_device(joystick_fd)

src/gps_node.py

@@ -0,0 +1,531 @@
+#!/usr/bin/env python
+import errno
+import glob
+import json
+import os
+import select
+import termios
+from collections import deque
+from datetime import datetime
+
+import rospy
+from sensor_msgs.msg import NavSatFix, NavSatStatus
+from std_msgs.msg import String
+
+
+PORT = 'auto'
+BAUD = 4800
+FRAME_ID = 'gps'
+READ_SIZE = 512
+
+BAUD_MAP = {
+    4800: termios.B4800,
+    9600: termios.B9600,
+    19200: termios.B19200,
+    38400: termios.B38400,
+    57600: termios.B57600,
+    115200: termios.B115200,
+}
+
+
+def nmea_to_decimal(raw_value, hemisphere):
+    if not raw_value or not hemisphere:
+        return None
+
+    try:
+        value = float(raw_value)
+    except ValueError:
+        return None
+
+    degrees = int(value / 100)
+    minutes = value - degrees * 100
+    decimal = degrees + minutes / 60.0
+
+    if hemisphere in ('S', 'W'):
+        decimal *= -1.0
+
+    return decimal
+
+
+def verify_checksum(sentence):
+    if not sentence.startswith('$'):
+        return False
+
+    if '*' not in sentence:
+        return True
+
+    payload, checksum_text = sentence[1:].split('*', 1)
+    checksum_text = checksum_text.strip()
+    checksum = 0
+    for char in payload:
+        checksum ^= ord(char)
+
+    try:
+        expected = int(checksum_text[:2], 16)
+    except ValueError:
+        return False
+
+    return checksum == expected
+
+
+class GpsState(object):
+    def __init__(self):
+        self.connected = False
+        self.latitude = None
+        self.longitude = None
+        self.altitude = None
+        self.hdop = None
+        self.speed_mps = None
+        self.track_deg = None
+        self.fix_quality = 0
+        self.satellites = 0
+        self.pdop = None
+        self.vdop = None
+        self.valid = False
+        self.last_sentence_time = None
+        self.last_fix_time = None
+        self.utc_datetime = None
+        self.visible_satellites = {}
+        self.visible_satellite_count = 0
+        self._gsv_groups = {}
+        self.raw_sentences = deque(maxlen=200)
+
+    def summary_text(self):
+        if not self.connected:
+            return 'Offline'
+        if self.valid and self.latitude is not None and self.longitude is not None:
+            speed_kmh = (self.speed_mps or 0.0) * 3.6
+            return 'Fix | {0} Sat | {1:.1f} km/h'.format(max(0, int(self.satellites or 0)), speed_kmh)
+        if self.last_sentence_time is not None:
+            return 'Suche Satelliten'
+        return 'Initialisiere'
+
+
+def configure_serial(fd, baud):
+    attrs = termios.tcgetattr(fd)
+    attrs[0] = 0
+    attrs[1] = 0
+    attrs[2] = termios.CS8 | termios.CREAD | termios.CLOCAL
+    attrs[3] = 0
+    attrs[4] = baud
+    attrs[5] = baud
+    attrs[6][termios.VMIN] = 0
+    attrs[6][termios.VTIME] = 0
+    termios.tcsetattr(fd, termios.TCSANOW, attrs)
+    termios.tcflush(fd, termios.TCIFLUSH)
+
+
+def resolve_port_candidates(port_config):
+    if port_config is None:
+        port_config = PORT
+
+    config_text = str(port_config).strip()
+    if not config_text or config_text.lower() == 'auto':
+        raw_candidates = [
+            '/dev/serial/by-id/*',
+            '/dev/serial/by-path/*',
+            '/dev/ttyUSB*',
+            '/dev/ttyACM*',
+        ]
+    else:
+        raw_candidates = [item.strip() for item in config_text.split(',') if item.strip()]
+
+    candidates = []
+    seen = set()
+    for candidate in raw_candidates:
+        matches = sorted(glob.glob(candidate)) if any(char in candidate for char in '*?[') else [candidate]
+        for match in matches:
+            if match in seen:
+                continue
+            seen.add(match)
+            candidates.append(match)
+    return candidates
+
+
+def open_device(port_config, baud, baud_rate):
+    last_signature = None
+    while not rospy.is_shutdown():
+        candidates = resolve_port_candidates(port_config)
+        signature = tuple(candidates)
+        if not candidates:
+            if signature != last_signature:
+                rospy.loginfo('GPS wartet auf Geraet. Suchmuster: %s', port_config)
+                last_signature = signature
+            rospy.sleep(1.0)
+            continue
+
+        try:
+            for port in candidates:
+                try:
+                    fd = os.open(port, os.O_RDONLY | os.O_NOCTTY | os.O_NONBLOCK)
+                    configure_serial(fd, baud)
+                    rospy.loginfo('GPS verbunden: %s @ %s Baud', port, baud_rate)
+                    return fd
+                except OSError as exc:
+                    if exc.errno != errno.ENOENT:
+                        rospy.logwarn('GPS kann nicht geoeffnet werden (%s): %s', port, exc)
+                except termios.error as exc:
+                    rospy.logwarn('GPS-Port kann nicht konfiguriert werden (%s): %s', port, exc)
+        finally:
+            last_signature = signature
+        rospy.sleep(1.0)
+    return None
+
+
+def close_device(fd):
+    if fd is None:
+        return
+    try:
+        os.close(fd)
+    except OSError:
+        pass
+
+
+def parse_gga(fields, state):
+    if len(fields) < 10:
+        return False
+
+    latitude = nmea_to_decimal(fields[2], fields[3])
+    longitude = nmea_to_decimal(fields[4], fields[5])
+
+    try:
+        fix_quality = int(fields[6] or 0)
+    except ValueError:
+        fix_quality = 0
+
+    try:
+        satellites = int(fields[7] or 0)
+    except ValueError:
+        satellites = 0
+
+    try:
+        hdop = float(fields[8]) if fields[8] else None
+    except ValueError:
+        hdop = None
+
+    try:
+        altitude = float(fields[9]) if fields[9] else None
+    except ValueError:
+        altitude = None
+
+    if latitude is not None:
+        state.latitude = latitude
+    if longitude is not None:
+        state.longitude = longitude
+    state.fix_quality = fix_quality
+    state.satellites = satellites
+    state.hdop = hdop
+    state.altitude = altitude
+    state.valid = fix_quality > 0 and latitude is not None and longitude is not None
+    if state.valid:
+        state.last_fix_time = rospy.Time.now()
+    return True
+
+
+def parse_rmc(fields, state):
+    if len(fields) < 10:
+        return False
+
+    status = fields[2] if len(fields) > 2 else ''
+    latitude = nmea_to_decimal(fields[3], fields[4])
+    longitude = nmea_to_decimal(fields[5], fields[6])
+
+    try:
+        speed_knots = float(fields[7]) if fields[7] else 0.0
+    except ValueError:
+        speed_knots = 0.0
+
+    try:
+        track_deg = float(fields[8]) if fields[8] else 0.0
+    except ValueError:
+        track_deg = 0.0
+
+    if latitude is not None:
+        state.latitude = latitude
+    if longitude is not None:
+        state.longitude = longitude
+    state.speed_mps = speed_knots * 0.514444
+    state.track_deg = track_deg
+    if status == 'A' and state.latitude is not None and state.longitude is not None:
+        state.valid = True
+        state.last_fix_time = rospy.Time.now()
+
+    if fields[1] and fields[9]:
+        try:
+            state.utc_datetime = datetime.strptime(fields[9] + fields[1].split('.')[0], '%d%m%y%H%M%S')
+        except ValueError:
+            pass
+    return True
+
+
+def parse_gsa(fields, state):
+    if len(fields) < 18:
+        return False
+
+    try:
+        state.pdop = float(fields[15]) if fields[15] else None
+    except ValueError:
+        state.pdop = None
+
+    try:
+        state.hdop = float(fields[16]) if fields[16] else state.hdop
+    except ValueError:
+        pass
+
+    try:
+        state.vdop = float(fields[17]) if fields[17] else None
+    except ValueError:
+        state.vdop = None
+    return True
+
+
+def merge_visible_satellites(state):
+    now = rospy.Time.now()
+    merged = {}
+
+    stale_talkers = []
+    for talker, group in state._gsv_groups.items():
+        completed_at = group.get('completed_at')
+        if completed_at is None:
+            continue
+        try:
+            if (now - completed_at).to_sec() > 8.0:
+                stale_talkers.append(talker)
+                continue
+        except rospy.ROSException:
+            continue
+
+        constellation = group.get('constellation', talker)
+        for sat_key, satellite in group.get('satellites', {}).items():
+            merged[sat_key] = dict(satellite, constellation=constellation)
+
+    for talker in stale_talkers:
+        state._gsv_groups.pop(talker, None)
+
+    state.visible_satellites = dict(sorted(merged.items()))
+    state.visible_satellite_count = len(state.visible_satellites)
+
+
+def parse_gsv(fields, state, sentence_type):
+    if len(fields) < 4:
+        return False
+
+    try:
+        total_sentences = int(fields[1] or 0)
+        sentence_index = int(fields[2] or 0)
+        total_satellites = int(fields[3] or 0)
+    except ValueError:
+        return False
+
+    if sentence_index <= 0:
+        return False
+
+    talker = sentence_type[1:3] if len(sentence_type) >= 3 else 'GN'
+    group = state._gsv_groups.get(talker)
+    if group is None:
+        group = {
+            'constellation': talker,
+            'total_sentences': 0,
+            'working_set': {},
+            'satellites': {},
+            'completed_at': None,
+            'total_satellites_reported': 0,
+        }
+        state._gsv_groups[talker] = group
+
+    if sentence_index == 1 or total_sentences != group.get('total_sentences'):
+        group['working_set'] = {}
+
+    group['total_sentences'] = total_sentences
+    group['total_satellites_reported'] = total_satellites
+
+    for index in range(4, len(fields), 4):
+        if index + 3 >= len(fields):
+            break
+        prn = fields[index].strip()
+        if not prn:
+            continue
+        try:
+            elevation = int(fields[index + 1]) if fields[index + 1] else None
+        except ValueError:
+            elevation = None
+        try:
+            azimuth = int(fields[index + 2]) if fields[index + 2] else None
+        except ValueError:
+            azimuth = None
+        try:
+            snr = int(fields[index + 3]) if fields[index + 3] else None
+        except ValueError:
+            snr = None
+
+        satellite_key = '{0}:{1}'.format(talker, prn)
+        group['working_set'][satellite_key] = {
+            'prn': '{0}-{1}'.format(talker, prn),
+            'elevation': elevation,
+            'azimuth': azimuth,
+            'snr': snr
+        }
+
+    if sentence_index >= total_sentences:
+        group['satellites'] = dict(group['working_set'])
+        group['completed_at'] = rospy.Time.now()
+        merge_visible_satellites(state)
+    return True
+
+
+def parse_sentence(sentence, state):
+    if not verify_checksum(sentence):
+        return False
+
+    state.raw_sentences.append(sentence)
+
+    fields = sentence.split('*', 1)[0].split(',')
+    if not fields:
+        return False
+
+    sentence_type = fields[0]
+    state.last_sentence_time = rospy.Time.now()
+
+    if sentence_type.endswith('GGA'):
+        return parse_gga(fields, state)
+    if sentence_type.endswith('RMC'):
+        return parse_rmc(fields, state)
+    if sentence_type.endswith('GSA'):
+        return parse_gsa(fields, state)
+    if sentence_type.endswith('GSV'):
+        return parse_gsv(fields, state, sentence_type)
+    return False
+
+
+def build_diagnostics_payload(state, frame_id):
+    last_fix_age = None
+    if state.last_fix_time is not None:
+        try:
+            last_fix_age = max(0.0, (rospy.Time.now() - state.last_fix_time).to_sec())
+        except rospy.ROSException:
+            last_fix_age = None
+
+    return {
+        'connected': state.connected,
+        'frame_id': frame_id,
+        'valid': state.valid,
+        'fix_quality': state.fix_quality,
+        'satellites_used': max(0, int(state.satellites or 0)),
+        'satellites_visible': max(0, int(state.visible_satellite_count or 0)),
+        'latitude': state.latitude,
+        'longitude': state.longitude,
+        'altitude_m': state.altitude,
+        'hdop': state.hdop,
+        'pdop': state.pdop,
+        'vdop': state.vdop,
+        'speed_mps': state.speed_mps,
+        'speed_kmh': (state.speed_mps or 0.0) * 3.6 if state.speed_mps is not None else None,
+        'track_deg': state.track_deg,
+        'utc_time': state.utc_datetime.strftime('%H:%M:%S') if state.utc_datetime else None,
+        'utc_date': state.utc_datetime.strftime('%Y-%m-%d') if state.utc_datetime else None,
+        'last_fix_age_s': last_fix_age,
+        'satellites': list(state.visible_satellites.values()),
+        'raw_sentences': list(state.raw_sentences)
+    }
+
+
+def publish_fix(fix_publisher, overlay_publisher, diagnostics_publisher, state, frame_id):
+    message = NavSatFix()
+    message.header.stamp = rospy.Time.now()
+    message.header.frame_id = frame_id
+    message.status.service = NavSatStatus.SERVICE_GPS
+
+    if state.valid and state.latitude is not None and state.longitude is not None:
+        message.status.status = NavSatStatus.STATUS_FIX
+        message.latitude = state.latitude
+        message.longitude = state.longitude
+        message.altitude = state.altitude if state.altitude is not None else 0.0
+        if state.hdop is not None:
+            horizontal_variance = max(0.25, state.hdop * state.hdop)
+            vertical_variance = max(1.0, (state.hdop * 2.0) * (state.hdop * 2.0))
+            message.position_covariance = [
+                horizontal_variance, 0.0, 0.0,
+                0.0, horizontal_variance, 0.0,
+                0.0, 0.0, vertical_variance
+            ]
+            message.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
+        else:
+            message.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
+    else:
+        message.status.status = NavSatStatus.STATUS_NO_FIX
+        message.latitude = 0.0
+        message.longitude = 0.0
+        message.altitude = 0.0
+        message.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
+
+    fix_publisher.publish(message)
+    overlay_publisher.publish(String(data=state.summary_text()))
+    diagnostics_publisher.publish(String(data=json.dumps(build_diagnostics_payload(state, frame_id), sort_keys=True)))
+
+
+if __name__ == '__main__':
+    rospy.init_node('gps_node')
+
+    port = rospy.get_param('~port', PORT)
+    baud_rate = int(rospy.get_param('~baud', BAUD))
+    frame_id = rospy.get_param('~frame_id', FRAME_ID)
+    baud = BAUD_MAP.get(baud_rate, termios.B4800)
+
+    fix_publisher = rospy.Publisher('/fix', NavSatFix, queue_size=5)
+    overlay_publisher = rospy.Publisher('/gps/status_text', String, queue_size=5)
+    diagnostics_publisher = rospy.Publisher('/gps/diagnostics_json', String, queue_size=5)
+
+    state = GpsState()
+    serial_fd = None
+    buffer = bytearray()
+
+    while not rospy.is_shutdown():
+        if serial_fd is None:
+            serial_fd = open_device(port, baud, baud_rate)
+            buffer = bytearray()
+            state.connected = serial_fd is not None
+            publish_fix(fix_publisher, overlay_publisher, diagnostics_publisher, state, frame_id)
+            if serial_fd is None:
+                break
+
+        try:
+            readable, _, _ = select.select([serial_fd], [], [], 0.5)
+            if serial_fd not in readable:
+                publish_fix(fix_publisher, overlay_publisher, diagnostics_publisher, state, frame_id)
+                continue
+
+            chunk = os.read(serial_fd, READ_SIZE)
+            if not chunk:
+                raise OSError(errno.ENODEV, 'GPS getrennt')
+
+            buffer.extend(chunk)
+
+            while b'\n' in buffer:
+                raw_line, _, buffer = buffer.partition(b'\n')
+                line = raw_line.decode('ascii', 'ignore').strip()
+                if not line or not line.startswith('$'):
+                    continue
+                if parse_sentence(line, state):
+                    publish_fix(fix_publisher, overlay_publisher, diagnostics_publisher, state, frame_id)
+
+        except OSError as exc:
+            if exc.errno not in (errno.EAGAIN, errno.EWOULDBLOCK):
+                rospy.logwarn('GPS-Lesefehler: %s', exc)
+                close_device(serial_fd)
+                serial_fd = None
+                state.connected = False
+                state.valid = False
+                state.last_sentence_time = None
+                state.satellites = 0
+                rospy.sleep(1.0)
+        except termios.error as exc:
+            rospy.logwarn('GPS-Fehler: %s', exc)
+            close_device(serial_fd)
+            serial_fd = None
+            state.connected = False
+            state.valid = False
+            state.last_sentence_time = None
+            state.satellites = 0
+            rospy.sleep(1.0)
+
+    close_device(serial_fd)

src/joystick_parser_node.py

@@ -0,0 +1,289 @@
+#!/usr/bin/env python
+import ctypes
+import fcntl
+import glob
+import math
+import os
+import struct
+import threading
+import time
+
+import rospy
+from sensor_msgs.msg import Joy
+from exomy.msg import RoverCommand
+from locomotion_modes import LocomotionMode
+
+# Define locomotion modes
+global locomotion_mode
+global motors_enabled
+global last_start_button_pressed
+global last_locomotion_mode
+
+locomotion_mode = LocomotionMode.ACKERMANN.value
+last_locomotion_mode = locomotion_mode
+motors_enabled = True
+_speed_limit = 100
+_speed_limit_last_read = 0.0
+AXIS_DEADZONE = 0.1
+last_start_button_pressed = False
+last_webgui_time = None
+WEBGUI_PRIORITY_TIMEOUT = 2.0
+
+VELOCITY_EXPO = 2.0
+CARDINAL_SNAP_THRESHOLD = 0.12
+
+DEFAULT_CONTROLLER = "logitech-F710"
+CONTROLLER_FUNCTION_MAPS = {
+    "webgui": {
+        "x_axis": 0,
+        "y_axis": 1,
+        "invert_x_axis": True,
+        "invert_y_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": True,
+        "invert_y_axis": True,
+        "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,
+        "invert_y_axis": False,
+        "X_button": 2,
+        "Y_button": 3,
+        "A_button": 0,
+        "B_button": 1,
+        "start_button": 7,
+        "select_button": 6,
+        "sensitivity": 0.11,
+    },
+}
+
+
+def get_speed_limit():
+    global _speed_limit, _speed_limit_last_read
+    now = time.time()
+    if now - _speed_limit_last_read > 2.0:
+        try:
+            _speed_limit = int(rospy.get_param('/speed_limit_percent', 100))
+        except Exception:
+            pass
+        _speed_limit_last_read = now
+    return _speed_limit
+
+
+def _rumble_thread(duration_ms):
+    EV_FF = 0x15
+    FF_RUMBLE = 0x50
+    EVIOCSFF = (0x40000000 | (48 << 16) | (ord('E') << 8) | 0x80)
+
+    device_path = None
+    for path in sorted(glob.glob('/dev/input/event*')):
+        try:
+            sys_name = '/sys/class/input/{0}/device/name'.format(os.path.basename(path))
+            with open(sys_name) as f:
+                name = f.read().strip()
+            if any(k in name for k in ('Gamepad', 'F710', 'GAMEPAD')):
+                device_path = path
+                break
+        except OSError:
+            pass
+
+    if not device_path:
+        return
+
+    try:
+        fd = os.open(device_path, os.O_RDWR)
+        try:
+            buf = ctypes.create_string_buffer(48)
+            struct.pack_into('<H', buf,  0, FF_RUMBLE)
+            struct.pack_into('<h', buf,  2, -1)
+            struct.pack_into('<H', buf, 10, duration_ms)
+            struct.pack_into('<H', buf, 16, 0xFFFF)
+            struct.pack_into('<H', buf, 18, 0x8000)
+            fcntl.ioctl(fd, EVIOCSFF, buf)
+            effect_id = struct.unpack_from('<h', buf, 2)[0]
+            import time as _t
+            os.write(fd, struct.pack('<QQHHi', 0, 0, EV_FF, effect_id, 1))
+            _t.sleep(duration_ms / 1000.0 + 0.05)
+            os.write(fd, struct.pack('<QQHHi', 0, 0, EV_FF, effect_id, 0))
+        finally:
+            os.close(fd)
+    except Exception as exc:
+        rospy.logwarn('rumble fehler: %s', exc)
+
+
+def trigger_rumble(duration_ms=150):
+    threading.Thread(target=_rumble_thread, args=(duration_ms,)).start()
+
+
+def apply_deadzone(value, deadzone):
+    if abs(value) <= deadzone:
+        return 0.0
+
+    scaled = (abs(value) - deadzone) / (1.0 - 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 snap_axes_to_cardinal(x, y, threshold):
+    if abs(x) <= threshold and abs(y) > 0.0:
+        return 0.0, math.copysign(abs(y), y)
+    if abs(y) <= threshold and abs(x) > 0.0:
+        return math.copysign(abs(x), x), 0.0
+    return x, y
+
+
+def callback(data):
+
+    global locomotion_mode
+    global motors_enabled
+    global last_start_button_pressed
+    global last_webgui_time
+    global last_locomotion_mode
+
+    is_webgui = data.header.frame_id == "webgui"
+    now = rospy.Time.now()
+
+    if is_webgui:
+        last_webgui_time = now
+    elif last_webgui_time is not None and (now - last_webgui_time).to_sec() < WEBGUI_PRIORITY_TIMEOUT:
+        return
+
+    rover_cmd = RoverCommand()
+
+    # Function map for the Logitech F710 joystick
+    # Button on pad | function
+    # --------------|----------------------
+    # A             | Ackermann mode
+    # X             | Point turn mode
+    # Y             | Crabbing mode
+    # 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(
+        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
+    if controller_function_map["invert_y_axis"]:
+        y *= -1
+
+    # Kleine Queranteile sollen beim Geradeausfahren nicht zu einem
+    # unbeabsichtigten Lenkwinkel fuehren.
+    snap_threshold = max(
+        controller_function_map["sensitivity"],
+        CARDINAL_SNAP_THRESHOLD)
+    x, y = snap_axes_to_cardinal(x, y, snap_threshold)
+
+    # Reading out button data to set locomotion mode
+    # X Button
+    if is_button_pressed(data.buttons, controller_function_map["X_button"]):
+        locomotion_mode = LocomotionMode.POINT_TURN.value
+    # A Button
+    if is_button_pressed(data.buttons, controller_function_map["A_button"]):
+        locomotion_mode = LocomotionMode.ACKERMANN.value
+    # B Button
+    if is_button_pressed(data.buttons, controller_function_map["B_button"]):
+        pass
+    # Y Button
+    if is_button_pressed(data.buttons, controller_function_map["Y_button"]):
+        locomotion_mode = LocomotionMode.CRABBING.value
+
+    if not is_webgui and locomotion_mode != last_locomotion_mode:
+        trigger_rumble(150)
+    last_locomotion_mode = locomotion_mode
+
+    rover_cmd.locomotion_mode = locomotion_mode
+
+    # Enable and disable motors
+    # START Button
+    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!")
+        elif motors_enabled is False:
+            motors_enabled = True
+            rospy.loginfo("Motors enabled!")
+        else:
+            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, capped by speed limit
+    stick_length = min(math.sqrt(x*x + y*y), 1.0)
+    rover_cmd.vel = int(100.0 * math.pow(get_speed_limit() / 100.0 * stick_length, VELOCITY_EXPO))
+
+    # The steering is described as an angle between -180...180
+    # Which describe the joystick position as follows:
+    #   +90
+    # 0      +-180
+    #   -90
+    #
+    rover_cmd.steering = int(math.atan2(y, x)*180.0/math.pi)
+
+    rover_cmd.connected = True
+
+    pub.publish(rover_cmd)
+
+
+if __name__ == '__main__':
+    global pub
+
+    rospy.init_node('joystick_parser_node')
+    rospy.loginfo('joystick_parser_node started')
+
+    sub = rospy.Subscriber("/joy_delayed", Joy, callback, queue_size=1)
+    pub = rospy.Publisher('/rover_command', RoverCommand, queue_size=1)
+
+    rospy.spin()

src/locomotion_modes.py

@@ -0,0 +1,10 @@
+#!/usr/bin/env python
+
+import enum
+
+
+class LocomotionMode(enum.Enum):
+    FAKE_ACKERMANN = 0
+    ACKERMANN = 1
+    POINT_TURN = 2
+    CRABBING = 3

src/motor_node.py

@@ -0,0 +1,46 @@
+#!/usr/bin/env python
+import time
+import rospy
+
+from exomy.msg import MotorCommands
+from motors import Motors
+
+motors = Motors()
+global watchdog_timer
+
+
+def callback(cmds):
+    motors.setSteering(cmds.motor_angles)
+    motors.setDriving(cmds.motor_speeds)
+
+    global watchdog_timer
+    watchdog_timer.shutdown()
+    # If this timer runs longer than the duration specified,
+    # then watchdog() is called stopping the driving motors.
+    watchdog_timer = rospy.Timer(rospy.Duration(5.0), watchdog, oneshot=True)
+
+
+def shutdown():
+    motors.stopMotors()
+
+
+def watchdog(event):
+    rospy.loginfo("Watchdog fired. Stopping driving motors.")
+    motors.stopMotors()
+
+
+if __name__ == "__main__":
+    # This node waits for commands from the robot and sets the motors accordingly
+    rospy.init_node("motors")
+    rospy.loginfo("Starting the motors node")
+    rospy.on_shutdown(shutdown)
+
+    global watchdog_timer
+    watchdog_timer = rospy.Timer(rospy.Duration(1.0), watchdog, oneshot=True)
+
+    sub = rospy.Subscriber(
+        "/motor_commands", MotorCommands, callback, queue_size=1)
+
+    rate = rospy.Rate(10)
+
+    rospy.spin()

src/motors.py

@@ -0,0 +1,127 @@
+#!/usr/bin/env python
+import rospy
+from std_msgs.msg import String
+
+import time
+import numpy as np
+
+import Adafruit_PCA9685
+
+
+class Motors():
+    '''
+    Motors class contains all functions to control the steering and driving
+    '''
+
+    # Define wheel names
+    FL, FR, CL, CR, RL, RR = range(0, 6)
+
+    # Motor commands are assuming positiv=driving_forward, negative=driving_backwards.
+    # The driving direction of the left side has to be inverted for this to apply to all wheels.
+    wheel_directions = [-1, 1, -1, 1, -1, 1]
+
+    # 1 fl-||-fr 2
+    #      ||
+    # 3 cl-||-cr 4
+    # 5 rl====rr 6
+
+    def __init__(self):
+
+        # Dictionary containing the pins of all motors
+        self.pins = {
+            'drive': {},
+            'steer': {}
+        }
+
+        # Set variables for the GPIO motor pins
+        self.pins['drive'][self.FL] = rospy.get_param("pin_drive_fl")
+        self.pins['steer'][self.FL] = rospy.get_param("pin_steer_fl")
+
+        self.pins['drive'][self.FR] = rospy.get_param("pin_drive_fr")
+        self.pins['steer'][self.FR] = rospy.get_param("pin_steer_fr")
+
+        self.pins['drive'][self.CL] = rospy.get_param("pin_drive_cl")
+        self.pins['steer'][self.CL] = rospy.get_param("pin_steer_cl")
+
+        self.pins['drive'][self.CR] = rospy.get_param("pin_drive_cr")
+        self.pins['steer'][self.CR] = rospy.get_param("pin_steer_cr")
+
+        self.pins['drive'][self.RL] = rospy.get_param("pin_drive_rl")
+        self.pins['steer'][self.RL] = rospy.get_param("pin_steer_rl")
+
+        self.pins['drive'][self.RR] = rospy.get_param("pin_drive_rr")
+        self.pins['steer'][self.RR] = rospy.get_param("pin_steer_rr")
+
+        # PWM characteristics
+        self.pwm = Adafruit_PCA9685.PCA9685(busnum=1)
+        self.pwm.set_pwm_freq(50)  # Hz
+
+        self.steering_pwm_neutral = [None] * 6
+
+        self.steering_pwm_neutral[self.FL] = rospy.get_param("steer_pwm_neutral_fl")
+        self.steering_pwm_neutral[self.FR] = rospy.get_param("steer_pwm_neutral_fr")
+        self.steering_pwm_neutral[self.CL] = rospy.get_param("steer_pwm_neutral_cl")
+        self.steering_pwm_neutral[self.CR] = rospy.get_param("steer_pwm_neutral_cr")
+        self.steering_pwm_neutral[self.RL] = rospy.get_param("steer_pwm_neutral_rl")
+        self.steering_pwm_neutral[self.RR] = rospy.get_param("steer_pwm_neutral_rr")
+        self.steering_pwm_range = rospy.get_param("steer_pwm_range")
+
+        self.driving_pwm_low_limit = 100
+        self.driving_pwm_neutral = [None] * 6
+        self.driving_pwm_neutral[self.FL] = rospy.get_param("drive_pwm_neutral_fl")
+        self.driving_pwm_neutral[self.FR] = rospy.get_param("drive_pwm_neutral_fr")
+        self.driving_pwm_neutral[self.CL] = rospy.get_param("drive_pwm_neutral_cl")
+        self.driving_pwm_neutral[self.CR] = rospy.get_param("drive_pwm_neutral_cr")
+        self.driving_pwm_neutral[self.RL] = rospy.get_param("drive_pwm_neutral_rl")
+        self.driving_pwm_neutral[self.RR] = rospy.get_param("drive_pwm_neutral_rr")
+        self.driving_pwm_upper_limit = 500
+        self.driving_pwm_range = rospy.get_param("drive_pwm_range")
+
+        # Set steering motors to neutral values (straight)
+        for wheel_name, motor_pin in self.pins['steer'].items():
+            self.pwm.set_pwm(motor_pin, 0,
+                             self.steering_pwm_neutral[wheel_name])
+            time.sleep(0.1)
+
+        self.wiggle()
+
+    def wiggle(self):
+        time.sleep(0.1)
+        self.pwm.set_pwm(self.pins['steer'][self.FL], 0,
+                         int(self.steering_pwm_neutral[self.FL] + self.steering_pwm_range * 0.3))
+        time.sleep(0.1)
+        self.pwm.set_pwm(self.pins['steer'][self.FR], 0,
+                         int(self.steering_pwm_neutral[self.FR] + self.steering_pwm_range * 0.3))
+        time.sleep(0.3)
+        self.pwm.set_pwm(self.pins['steer'][self.FL], 0,
+                         int(self.steering_pwm_neutral[self.FL] - self.steering_pwm_range * 0.3))
+        time.sleep(0.1)
+        self.pwm.set_pwm(self.pins['steer'][self.FR], 0,
+                         int(self.steering_pwm_neutral[self.FR] - self.steering_pwm_range * 0.3))
+        time.sleep(0.3)
+        self.pwm.set_pwm(self.pins['steer'][self.FL], 0,
+                         int(self.steering_pwm_neutral[self.FL]))
+        time.sleep(0.1)
+        self.pwm.set_pwm(self.pins['steer'][self.FR], 0,
+                         int(self.steering_pwm_neutral[self.FR]))
+        time.sleep(0.3)
+
+    def setSteering(self, steering_command):
+        # Loop through pin dictionary. The items key is the wheel_name and the value the pin.
+        for wheel_name, motor_pin in self.pins['steer'].items():
+            duty_cycle = int(
+                self.steering_pwm_neutral[wheel_name] + steering_command[wheel_name]/90.0 * self.steering_pwm_range)
+
+            self.pwm.set_pwm(motor_pin, 0, duty_cycle)
+
+    def setDriving(self, driving_command):
+        # Loop through pin dictionary. The items key is the wheel_name and the value the pin.
+        for wheel_name, motor_pin in self.pins['drive'].items():
+            duty_cycle = int(self.driving_pwm_neutral[wheel_name] +
+                             driving_command[wheel_name]/100.0 * self.driving_pwm_range * self.wheel_directions[wheel_name])
+
+            self.pwm.set_pwm(motor_pin, 0, duty_cycle)
+
+    def stopMotors(self):
+        for wheel_name, motor_pin in self.pins['drive'].items():
+            self.pwm.set_pwm(motor_pin, 0, int(self.driving_pwm_neutral[wheel_name]))

src/robot_node.py

@@ -0,0 +1,41 @@
+#!/usr/bin/env python
+import time
+from exomy.msg import RoverCommand, MotorCommands, Screen
+import rospy
+from rover import Rover
+import message_filters
+
+
+global exomy
+exomy = Rover()
+
+
+def joy_callback(message):
+    cmds = MotorCommands()
+
+    if message.motors_enabled is True:
+        exomy.setLocomotionMode(message.locomotion_mode)
+
+        cmds.motor_angles = exomy.joystickToSteeringAngle(
+            message.vel, message.steering)
+        cmds.motor_speeds = exomy.joystickToVelocity(
+            message.vel, message.steering)
+    else:
+        cmds.motor_angles = exomy.joystickToSteeringAngle(0, 0)
+        cmds.motor_speeds = exomy.joystickToVelocity(0, 0)
+
+    robot_pub.publish(cmds)
+
+
+if __name__ == '__main__':
+    rospy.init_node('robot_node')
+    rospy.loginfo("Starting the robot node")
+    global robot_pub
+    joy_sub = rospy.Subscriber(
+        "/rover_command", RoverCommand, joy_callback, queue_size=1)
+
+    rate = rospy.Rate(10)
+
+    robot_pub = rospy.Publisher("/motor_commands", MotorCommands, queue_size=1)
+
+    rospy.spin()

src/rover.py

@@ -0,0 +1,322 @@
+#!/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