EskimueandClaude Sonnet 4.6
ff58356c23
rosparam /auto_rotate_active pausiert joystick_parser_node komplett. Modus-Wiederherstellung erst nach Erreichen der Zielposition. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
120 lines
3.0 KiB
Python
120 lines
3.0 KiB
Python
#!/usr/bin/env python
|
|
# -*- coding: utf-8 -*-
|
|
"""
|
|
Laeuft INNERHALB des Docker-Containers mit nativem rospy.
|
|
Dreht den Rover auf der Stelle zu einem Ziel-Heading und beendet sich.
|
|
|
|
Aufruf:
|
|
python3 auto_rotate_ros.py <target_deg> <restore_mode> <heading_offset_deg>
|
|
|
|
Exit-Codes:
|
|
0 = Ziel erreicht
|
|
1 = Fehler
|
|
"""
|
|
|
|
import math
|
|
import signal
|
|
import sys
|
|
import time
|
|
|
|
import rospy
|
|
from exomy.msg import RoverCommand
|
|
from sensor_msgs.msg import Imu
|
|
|
|
_POINT_TURN = 2
|
|
_DONE_DEG = float(sys.argv[4]) if len(sys.argv) > 4 else 1.0
|
|
_VEL = int(sys.argv[5]) if len(sys.argv) > 5 else 20
|
|
|
|
target_deg = float(sys.argv[1]) % 360
|
|
restore_mode = int(sys.argv[2])
|
|
heading_offset = float(sys.argv[3])
|
|
|
|
_running = True
|
|
_publisher = None
|
|
_heading = None
|
|
|
|
|
|
def _normalize(deg):
|
|
return (float(deg) % 360 + 360) % 360
|
|
|
|
|
|
def _shortest_error(current, target):
|
|
return (target - current + 540) % 360 - 180
|
|
|
|
|
|
def _on_imu(msg):
|
|
global _heading
|
|
o = msg.orientation
|
|
x, y, z, w = o.x, o.y, o.z, o.w
|
|
siny = 2.0 * (w * z + x * y)
|
|
cosy = 1.0 - 2.0 * (y * y + z * z)
|
|
yaw_deg = math.degrees(math.atan2(siny, cosy))
|
|
_heading = _normalize(yaw_deg + heading_offset)
|
|
|
|
|
|
def _publish(locomotion_mode, vel, steering=0):
|
|
if _publisher is None:
|
|
return
|
|
cmd = RoverCommand()
|
|
cmd.connected = True
|
|
cmd.motors_enabled = True
|
|
cmd.locomotion_mode = locomotion_mode
|
|
cmd.vel = vel
|
|
cmd.steering = steering
|
|
_publisher.publish(cmd)
|
|
|
|
|
|
def _shutdown(signum, frame):
|
|
global _running
|
|
_running = False
|
|
|
|
|
|
def main():
|
|
global _publisher, _running
|
|
|
|
signal.signal(signal.SIGTERM, _shutdown)
|
|
signal.signal(signal.SIGINT, _shutdown)
|
|
|
|
rospy.init_node('auto_rotate', anonymous=True, disable_signals=True)
|
|
|
|
_publisher = rospy.Publisher('/rover_command', RoverCommand, queue_size=1)
|
|
rospy.Subscriber('/imu/data', Imu, _on_imu, queue_size=1)
|
|
rospy.set_param('/auto_rotate_active', True)
|
|
|
|
# Kurz warten bis IMU-Daten ankommen
|
|
deadline = time.time() + 5.0
|
|
while _heading is None and time.time() < deadline and _running:
|
|
time.sleep(0.05)
|
|
|
|
if _heading is None:
|
|
rospy.logerr('auto_rotate: keine IMU-Daten')
|
|
rospy.set_param('/auto_rotate_active', False)
|
|
_publish(restore_mode, vel=0)
|
|
sys.exit(1)
|
|
|
|
rate = rospy.Rate(50) # 50 Hz, passend zum PWM-Chip
|
|
|
|
while _running and not rospy.is_shutdown():
|
|
error = _shortest_error(_heading, target_deg)
|
|
|
|
if abs(error) <= _DONE_DEG:
|
|
rospy.set_param('/auto_rotate_active', False)
|
|
_publish(restore_mode, vel=0)
|
|
sys.exit(0)
|
|
|
|
# Richtung wie Joystick: immer positiver vel, steering ±180 fuer Richtung
|
|
if error > 0:
|
|
_publish(_POINT_TURN, vel=_VEL, steering=0)
|
|
else:
|
|
_publish(_POINT_TURN, vel=_VEL, steering=180)
|
|
rate.sleep()
|
|
|
|
# Abbruch per Signal
|
|
rospy.set_param('/auto_rotate_active', False)
|
|
_publish(restore_mode, vel=0)
|
|
sys.exit(0)
|
|
|
|
|
|
if __name__ == '__main__':
|
|
main()
|