Turn on Point kann jetzt schon im Admin aufgerufen werden um den Rover in eine Richtug zu drehen
This commit is contained in:
@@ -227,6 +227,7 @@ def _connect_imu_ros():
|
||||
global IMU_ROS_CLIENT, IMU_ROS_TOPICS
|
||||
|
||||
import roslibpy
|
||||
import imu_rotate
|
||||
|
||||
client = roslibpy.Ros(host=ROSBRIDGE_HOST, port=ROSBRIDGE_PORT)
|
||||
client.run()
|
||||
@@ -239,13 +240,29 @@ def _connect_imu_ros():
|
||||
imu_topic = roslibpy.Topic(client, '/imu/data', 'sensor_msgs/Imu')
|
||||
mag_topic = roslibpy.Topic(client, '/imu/mag', 'sensor_msgs/MagneticField')
|
||||
status_topic = roslibpy.Topic(client, '/imu/status', 'std_msgs/String')
|
||||
rover_cmd_topic = roslibpy.Topic(client, '/rover_command', 'exomy/RoverCommand')
|
||||
imu_topic.subscribe(_on_imu_data_message)
|
||||
mag_topic.subscribe(_on_imu_mag_message)
|
||||
status_topic.subscribe(_on_imu_status_message)
|
||||
rover_cmd_topic.advertise()
|
||||
IMU_ROS_CLIENT = client
|
||||
IMU_ROS_TOPICS = [imu_topic, mag_topic, status_topic]
|
||||
IMU_ROS_TOPICS = [imu_topic, mag_topic, status_topic, rover_cmd_topic]
|
||||
_imu_ros_cache_update(status='ROS verbunden', error='')
|
||||
|
||||
def _publish_rover_cmd(locomotion_mode, vel, steering):
|
||||
try:
|
||||
rover_cmd_topic.publish(roslibpy.Message({
|
||||
'connected': True,
|
||||
'motors_enabled': True,
|
||||
'locomotion_mode': int(locomotion_mode),
|
||||
'vel': int(vel),
|
||||
'steering': int(steering),
|
||||
}))
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
imu_rotate.set_publish_fn(_publish_rover_cmd)
|
||||
|
||||
|
||||
def _imu_ros_worker():
|
||||
global IMU_ROS_CLIENT, IMU_ROS_TOPICS
|
||||
@@ -1957,6 +1974,10 @@ class Handler(http.server.BaseHTTPRequestHandler):
|
||||
self.end_headers()
|
||||
|
||||
def do_GET(self):
|
||||
if self.path == '/api/imu/rotate-status':
|
||||
import imu_rotate
|
||||
self.write_json(200, {'rotate': imu_rotate.get_rotate_status()})
|
||||
return
|
||||
if self.path == '/api/delay':
|
||||
self.write_json(200, {'delay_seconds': get_delay()})
|
||||
return
|
||||
@@ -1999,7 +2020,8 @@ class Handler(http.server.BaseHTTPRequestHandler):
|
||||
|
||||
def do_POST(self):
|
||||
raw_body = b'{}'
|
||||
if self.path in ('/api/routes/save', '/api/routes/delete', '/api/imu/heading-offset'):
|
||||
if self.path in ('/api/routes/save', '/api/routes/delete', '/api/imu/heading-offset',
|
||||
'/api/imu/rotate-to'):
|
||||
try:
|
||||
content_length = int(self.headers.get('Content-Length', '0'))
|
||||
except ValueError:
|
||||
@@ -2290,6 +2312,39 @@ class Handler(http.server.BaseHTTPRequestHandler):
|
||||
self.write_json(status_code, response)
|
||||
return
|
||||
|
||||
if self.path == '/api/imu/rotate-to':
|
||||
try:
|
||||
payload = json.loads(raw_body.decode('utf-8'))
|
||||
target_deg = float(payload.get('target_deg', 0))
|
||||
except (UnicodeDecodeError, json.JSONDecodeError, TypeError, ValueError):
|
||||
self.write_json(400, {'status': 'Ungültige Anfrage'})
|
||||
return
|
||||
with IMU_ROS_LOCK:
|
||||
heading = IMU_ROS_CACHE.get('heading_deg')
|
||||
if heading is None:
|
||||
self.write_json(409, {'status': 'Noch keine frischen IMU-Daten'})
|
||||
return
|
||||
import imu_rotate
|
||||
heading_offset = read_imu_heading_calibration()['offset_deg']
|
||||
|
||||
def _get_corrected_heading():
|
||||
with IMU_ROS_LOCK:
|
||||
raw = IMU_ROS_CACHE.get('heading_deg')
|
||||
return get_corrected_heading_deg(raw, heading_offset) if raw is not None else None
|
||||
|
||||
status = imu_rotate.start_rotate(
|
||||
target_deg=target_deg,
|
||||
get_heading_fn=_get_corrected_heading,
|
||||
)
|
||||
self.write_json(200, {'status': 'Drehung gestartet', 'rotate': status})
|
||||
return
|
||||
|
||||
if self.path == '/api/imu/rotate-stop':
|
||||
import imu_rotate
|
||||
imu_rotate.stop_rotate()
|
||||
self.write_json(200, {'status': 'Drehung gestoppt', 'rotate': imu_rotate.get_rotate_status()})
|
||||
return
|
||||
|
||||
actions = {
|
||||
'/api/cold-start-gps': {
|
||||
'handler': cold_start_gps_receiver,
|
||||
|
||||
@@ -0,0 +1,133 @@
|
||||
"""
|
||||
Dreht den Rover auf der Stelle zu einem Ziel-Heading.
|
||||
|
||||
Öffentliche API:
|
||||
set_publish_fn(fn) wird von exomy_admin_api nach ROS-Connect gesetzt
|
||||
start_rotate(target_deg, get_heading_fn) -> dict
|
||||
stop_rotate()
|
||||
get_rotate_status() -> dict
|
||||
"""
|
||||
|
||||
import threading
|
||||
import time
|
||||
|
||||
_POINT_TURN = 2 # LocomotionMode.POINT_TURN
|
||||
_ACKERMANN = 1 # LocomotionMode.ACKERMANN (Standard-Rückkehrmodus)
|
||||
_DONE_DEG = 3.0 # Toleranz in Grad
|
||||
_VEL = 20 # konstante Drehgeschwindigkeit (0–100), direkte Motoransteuerung
|
||||
_INTERVAL = 0.02 # Sekunden zwischen Regelschritten (50 Hz)
|
||||
|
||||
_lock = threading.Lock()
|
||||
_thread = None
|
||||
_stop_event = threading.Event()
|
||||
_publish_fn = None
|
||||
|
||||
_status = {
|
||||
'running': False,
|
||||
'target_deg': None,
|
||||
'current_deg': None,
|
||||
'error_deg': None,
|
||||
'result': 'idle',
|
||||
'message': '',
|
||||
}
|
||||
|
||||
|
||||
def set_publish_fn(fn):
|
||||
global _publish_fn
|
||||
_publish_fn = fn
|
||||
|
||||
|
||||
def _publish(locomotion_mode, vel, steering):
|
||||
fn = _publish_fn
|
||||
if fn is not None:
|
||||
fn(locomotion_mode=locomotion_mode, vel=vel, steering=steering)
|
||||
|
||||
|
||||
def _shortest_error(current, target):
|
||||
"""Vorzeichen: positiv = rechtsdrehung nötig, negativ = linksdrehung."""
|
||||
return (target - current + 540) % 360 - 180
|
||||
|
||||
|
||||
def _restore(restore_mode):
|
||||
_publish(restore_mode, vel=0, steering=0)
|
||||
|
||||
|
||||
def _control_loop(target_deg, get_heading_fn, restore_mode):
|
||||
global _status
|
||||
|
||||
try:
|
||||
while not _stop_event.is_set():
|
||||
heading = get_heading_fn()
|
||||
if heading is None:
|
||||
time.sleep(0.05)
|
||||
continue
|
||||
|
||||
error = _shortest_error(heading, target_deg)
|
||||
|
||||
with _lock:
|
||||
_status['current_deg'] = round(heading, 1)
|
||||
_status['error_deg'] = round(error, 1)
|
||||
|
||||
if abs(error) <= _DONE_DEG:
|
||||
_restore(restore_mode)
|
||||
with _lock:
|
||||
_status['running'] = False
|
||||
_status['result'] = 'done'
|
||||
_status['message'] = 'Ziel erreicht'
|
||||
return
|
||||
|
||||
vel = _VEL if error > 0 else -_VEL
|
||||
_publish(_POINT_TURN, vel=vel, steering=0)
|
||||
time.sleep(_INTERVAL)
|
||||
|
||||
_restore(restore_mode)
|
||||
with _lock:
|
||||
_status['running'] = False
|
||||
_status['result'] = 'aborted'
|
||||
_status['message'] = 'Abgebrochen'
|
||||
|
||||
except Exception as exc:
|
||||
_restore(restore_mode)
|
||||
with _lock:
|
||||
_status['running'] = False
|
||||
_status['result'] = 'error'
|
||||
_status['message'] = str(exc)
|
||||
|
||||
|
||||
def start_rotate(target_deg, get_heading_fn):
|
||||
global _thread, _status
|
||||
|
||||
target_deg = float(target_deg) % 360
|
||||
stop_rotate()
|
||||
|
||||
_stop_event.clear()
|
||||
with _lock:
|
||||
_status = {
|
||||
'running': True,
|
||||
'target_deg': round(target_deg, 1),
|
||||
'current_deg': None,
|
||||
'error_deg': None,
|
||||
'result': 'running',
|
||||
'message': 'Drehe zu {0:.0f}°'.format(target_deg),
|
||||
}
|
||||
|
||||
_thread = threading.Thread(
|
||||
target=_control_loop,
|
||||
args=(target_deg, get_heading_fn),
|
||||
daemon=True,
|
||||
)
|
||||
_thread.start()
|
||||
return get_rotate_status()
|
||||
|
||||
|
||||
def stop_rotate():
|
||||
global _thread
|
||||
if _thread is not None and _thread.is_alive():
|
||||
_stop_event.set()
|
||||
_thread.join(timeout=2.0)
|
||||
_thread = None
|
||||
|
||||
|
||||
def get_rotate_status():
|
||||
with _lock:
|
||||
return dict(_status)
|
||||
Reference in New Issue
Block a user