Auto-Rotate blockiert Joystick-Parser während der Drehung
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>
This commit is contained in:
@@ -79,6 +79,7 @@ def main():
|
||||
|
||||
_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
|
||||
@@ -87,6 +88,7 @@ def main():
|
||||
|
||||
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)
|
||||
|
||||
@@ -96,12 +98,11 @@ def main():
|
||||
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 für Richtung
|
||||
# steering=0 → Linksdrehung (deg < 85)
|
||||
# steering=180 → Rechtsdrehung (deg >= 85)
|
||||
# Richtung wie Joystick: immer positiver vel, steering ±180 fuer Richtung
|
||||
if error > 0:
|
||||
_publish(_POINT_TURN, vel=_VEL, steering=0)
|
||||
else:
|
||||
@@ -109,6 +110,7 @@ def main():
|
||||
rate.sleep()
|
||||
|
||||
# Abbruch per Signal
|
||||
rospy.set_param('/auto_rotate_active', False)
|
||||
_publish(restore_mode, vel=0)
|
||||
sys.exit(0)
|
||||
|
||||
|
||||
Reference in New Issue
Block a user