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

Alles verbessert

Browse Source
This commit is contained in:
Eskimue
2026-05-21 19:30:33 +02:00
parent 5dccc8985e
commit 33b71416ff
19 changed files with 855 additions and 229 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ExoMy_Software-master/src/f710_joy_node.py
+99
View File
@@ -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)
ExoMy_Software-master/src/joystick_parser_node.py
+17 -1
View File
@@ -14,6 +14,8 @@ locomotion_mode = LocomotionMode.ACKERMANN.value
motors_enabled = True
AXIS_DEADZONE = 0.1
last_start_button_pressed = False
last_webgui_time = None
WEBGUI_PRIORITY_TIMEOUT = 2.0
VELOCITY_EXPO = 2.0
@@ -23,6 +25,7 @@ CONTROLLER_FUNCTION_MAPS = {
"x_axis": 0,
"y_axis": 1,
"invert_x_axis": True,
"invert_y_axis": False,
"X_button": 0,
"Y_button": 3,
"A_button": 1,
@@ -34,7 +37,8 @@ CONTROLLER_FUNCTION_MAPS = {
"logitech-F710": {
"x_axis": 0,
"y_axis": 1,
"invert_x_axis": False,
"invert_x_axis": True,
"invert_y_axis": True,
"X_button": 0,
"Y_button": 3,
"A_button": 1,
@@ -47,6 +51,7 @@ CONTROLLER_FUNCTION_MAPS = {
"x_axis": 0,
"y_axis": 1,
"invert_x_axis": False,
"invert_y_axis": False,
"X_button": 2,
"Y_button": 3,
"A_button": 0,
@@ -94,6 +99,15 @@ def callback(data):
global locomotion_mode
global motors_enabled
global last_start_button_pressed
global last_webgui_time
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()
@@ -118,6 +132,8 @@ def callback(data):
if controller_function_map["invert_x_axis"]:
x *= -1
if controller_function_map["invert_y_axis"]:
y *= -1
# Reading out button data to set locomotion mode
# X Button
ExoMy_Software-master/src/motors.py
+10 -7
View File
@@ -53,7 +53,7 @@ class Motors():
self.pins['steer'][self.RR] = rospy.get_param("pin_steer_rr")
# PWM characteristics
self.pwm = Adafruit_PCA9685.PCA9685()
self.pwm = Adafruit_PCA9685.PCA9685(busnum=1)
self.pwm.set_pwm_freq(50) # Hz
self.steering_pwm_neutral = [None] * 6
@@ -67,7 +67,13 @@ class Motors():
self.steering_pwm_range = rospy.get_param("steer_pwm_range")
self.driving_pwm_low_limit = 100
self.driving_pwm_neutral = rospy.get_param("drive_pwm_neutral")
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")
@@ -111,14 +117,11 @@ class Motors():
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 +
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):
# Set driving wheels to neutral position to stop them
duty_cycle = int(self.driving_pwm_neutral)
for wheel_name, motor_pin in self.pins['drive'].items():
self.pwm.set_pwm(motor_pin, 0, duty_cycle)
self.pwm.set_pwm(motor_pin, 0, int(self.driving_pwm_neutral[wheel_name]))
ExoMy_Software-master/src/rover.py
+9 -4
View File
@@ -24,6 +24,7 @@ class Rover():
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
@@ -152,10 +153,14 @@ class Rover():
return steering_angles
if(self.locomotion_mode == LocomotionMode.POINT_TURN.value):
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))))
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