Eskimue/ExoMy_Cuno
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Verbessert

Browse Source
This commit is contained in:
Eskimue
2026-05-20 20:34:45 +02:00
parent 4970646365
commit 9b05b5b677
3 changed files with 172 additions and 89 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ExoMy_Software-master/launch/exomy.launch
+1 -14
View File
@@ -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="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>
<param name="controller" value="logitech-F710"/>
<include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch"/>
<rosparam file="$(find exomy)/config/exomy.yaml"/>
ExoMy_Software-master/src/joystick_parser_node.py
+88 -10
View File
@@ -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
ExoMy_Software-master/src/rover.py
+83 -65
View File
@@ -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