GPS eingebaut
This commit is contained in:
@@ -2,7 +2,7 @@
|
||||
set -eo pipefail
|
||||
|
||||
cleanup() {
|
||||
for pid_var in HTTP_PID ROSMASTER_PID ROSBRIDGE_PID ROSAPI_PID ROBOT_PID MOTOR_PID JOYSTICK_PID JOY_PID DELAY_PID; do
|
||||
for pid_var in HTTP_PID ROSMASTER_PID ROSBRIDGE_PID ROSAPI_PID ROBOT_PID MOTOR_PID JOYSTICK_PID JOY_PID DELAY_PID GPS_PID; do
|
||||
if [[ -n "${!pid_var:-}" ]]; then
|
||||
kill "${!pid_var}" 2>/dev/null || true
|
||||
fi
|
||||
@@ -53,8 +53,10 @@ then
|
||||
MOTOR_PID=$!
|
||||
python /root/exomy_ws/src/exomy/src/joystick_parser_node.py > /tmp/joystick_parser.log 2>&1 &
|
||||
JOYSTICK_PID=$!
|
||||
python /root/exomy_ws/src/exomy/src/gps_node.py > /tmp/gps_node.log 2>&1 &
|
||||
GPS_PID=$!
|
||||
|
||||
wait -n "$HTTP_PID" "$ROSMASTER_PID" "$ROSBRIDGE_PID" "$ROSAPI_PID" "$ROBOT_PID" "$MOTOR_PID" "$JOYSTICK_PID" "$JOY_PID" "$DELAY_PID"
|
||||
wait -n "$HTTP_PID" "$ROSMASTER_PID" "$ROSBRIDGE_PID" "$ROSAPI_PID" "$ROBOT_PID" "$MOTOR_PID" "$JOYSTICK_PID" "$JOY_PID" "$DELAY_PID" "$GPS_PID"
|
||||
exit 1
|
||||
elif [[ $1 == "devel" ]]
|
||||
then
|
||||
|
||||
@@ -64,8 +64,10 @@
|
||||
CTRL: <span id="input-state">Joystick</span>
|
||||
</div>
|
||||
<div class="c-bl">
|
||||
X: <span id="cam-x">+0.00</span><br>
|
||||
Y: <span id="cam-y">+0.00</span>
|
||||
GPS: <span id="gps-state">Offline</span><br>
|
||||
SAT: <span id="gps-sats">--</span><br>
|
||||
LAT: <span id="gps-lat">--</span><br>
|
||||
LON: <span id="gps-lon">--</span>
|
||||
</div>
|
||||
<div class="c-br">
|
||||
HOST: <span id="cam-host">--</span><br>
|
||||
@@ -280,6 +282,8 @@ var ros = null;
|
||||
var joyListener = null;
|
||||
var roverCommandListener = null;
|
||||
var motorCommandsListener = null;
|
||||
var gpsFixListener = null;
|
||||
var gpsStatusListener = null;
|
||||
var publishTimer = null;
|
||||
var statusTimer = null;
|
||||
var axes = [0, 0, 0, 0, 0, 0];
|
||||
@@ -584,8 +588,28 @@ function updateAxesDisplay(x, y) {
|
||||
setText("jy", formatAxis(y));
|
||||
setText("dpx", formatAxis(x));
|
||||
setText("dpy", formatAxis(y));
|
||||
setText("cam-x", formatAxis(x));
|
||||
setText("cam-y", formatAxis(y));
|
||||
}
|
||||
|
||||
function updateGpsOverlayFromFix(message) {
|
||||
if (!message) return;
|
||||
|
||||
var hasFix = message.status && message.status.status >= 0;
|
||||
if (!hasFix) {
|
||||
setText("gps-lat", "--");
|
||||
setText("gps-lon", "--");
|
||||
return;
|
||||
}
|
||||
|
||||
setText("gps-lat", formatNumber(Number(message.latitude || 0), 5));
|
||||
setText("gps-lon", formatNumber(Number(message.longitude || 0), 5));
|
||||
}
|
||||
|
||||
function updateGpsOverlayFromStatus(text) {
|
||||
var value = String(text || "Offline");
|
||||
setText("gps-state", value);
|
||||
|
||||
var satMatch = value.match(/(\d+)\s*Sat/i);
|
||||
setText("gps-sats", satMatch ? satMatch[1] : "--");
|
||||
}
|
||||
|
||||
function updateModeDisplay() {
|
||||
@@ -1028,6 +1052,22 @@ window.addEventListener("load", function () {
|
||||
renderTrajectoryOverlay();
|
||||
});
|
||||
|
||||
gpsFixListener = new ROSLIB.Topic({
|
||||
ros: ros,
|
||||
name: "/fix",
|
||||
messageType: "sensor_msgs/NavSatFix"
|
||||
});
|
||||
gpsFixListener.subscribe(updateGpsOverlayFromFix);
|
||||
|
||||
gpsStatusListener = new ROSLIB.Topic({
|
||||
ros: ros,
|
||||
name: "/gps/status_text",
|
||||
messageType: "std_msgs/String"
|
||||
});
|
||||
gpsStatusListener.subscribe(function (message) {
|
||||
updateGpsOverlayFromStatus(message.data);
|
||||
});
|
||||
|
||||
var joySubscriber = new ROSLIB.Topic({
|
||||
ros: ros,
|
||||
name: "/joy",
|
||||
|
||||
@@ -2,6 +2,11 @@
|
||||
<node name="robot" pkg="exomy" type="robot_node.py" respawn="true" output="screen"/>
|
||||
<node name="motors" pkg="exomy" type="motor_node.py" respawn="true" output="screen" />
|
||||
<node name="joystick" pkg="exomy" type="joystick_parser_node.py" respawn="true" output="screen" />
|
||||
<node name="gps" pkg="exomy" type="gps_node.py" respawn="true" output="screen">
|
||||
<param name="port" value="/dev/ttyUSB0"/>
|
||||
<param name="baud" value="4800"/>
|
||||
<param name="frame_id" value="gps"/>
|
||||
</node>
|
||||
|
||||
<node respawn="true" pkg="joy" type="joy_node" name="joy_node">
|
||||
<param name="coalesce_interval" value="0.05"/>
|
||||
|
||||
@@ -0,0 +1,327 @@
|
||||
#!/usr/bin/env python
|
||||
import errno
|
||||
import os
|
||||
import select
|
||||
import termios
|
||||
from datetime import datetime
|
||||
|
||||
import rospy
|
||||
from sensor_msgs.msg import NavSatFix, NavSatStatus
|
||||
from std_msgs.msg import String
|
||||
|
||||
|
||||
PORT = '/dev/ttyUSB0'
|
||||
BAUD = 4800
|
||||
FRAME_ID = 'gps'
|
||||
READ_SIZE = 512
|
||||
|
||||
BAUD_MAP = {
|
||||
4800: termios.B4800,
|
||||
9600: termios.B9600,
|
||||
19200: termios.B19200,
|
||||
38400: termios.B38400,
|
||||
57600: termios.B57600,
|
||||
115200: termios.B115200,
|
||||
}
|
||||
|
||||
|
||||
def nmea_to_decimal(raw_value, hemisphere):
|
||||
if not raw_value or not hemisphere:
|
||||
return None
|
||||
|
||||
try:
|
||||
value = float(raw_value)
|
||||
except ValueError:
|
||||
return None
|
||||
|
||||
degrees = int(value / 100)
|
||||
minutes = value - degrees * 100
|
||||
decimal = degrees + minutes / 60.0
|
||||
|
||||
if hemisphere in ('S', 'W'):
|
||||
decimal *= -1.0
|
||||
|
||||
return decimal
|
||||
|
||||
|
||||
def verify_checksum(sentence):
|
||||
if not sentence.startswith('$'):
|
||||
return False
|
||||
|
||||
if '*' not in sentence:
|
||||
return True
|
||||
|
||||
payload, checksum_text = sentence[1:].split('*', 1)
|
||||
checksum_text = checksum_text.strip()
|
||||
checksum = 0
|
||||
for char in payload:
|
||||
checksum ^= ord(char)
|
||||
|
||||
try:
|
||||
expected = int(checksum_text[:2], 16)
|
||||
except ValueError:
|
||||
return False
|
||||
|
||||
return checksum == expected
|
||||
|
||||
|
||||
class GpsState(object):
|
||||
def __init__(self):
|
||||
self.connected = False
|
||||
self.latitude = None
|
||||
self.longitude = None
|
||||
self.altitude = None
|
||||
self.hdop = None
|
||||
self.speed_mps = None
|
||||
self.track_deg = None
|
||||
self.fix_quality = 0
|
||||
self.satellites = 0
|
||||
self.valid = False
|
||||
self.last_sentence_time = None
|
||||
self.last_fix_time = None
|
||||
|
||||
def summary_text(self):
|
||||
if not self.connected:
|
||||
return 'Offline'
|
||||
if self.valid and self.latitude is not None and self.longitude is not None:
|
||||
speed_kmh = (self.speed_mps or 0.0) * 3.6
|
||||
return 'Fix | {0} Sat | {1:.1f} km/h'.format(max(0, int(self.satellites or 0)), speed_kmh)
|
||||
if self.last_sentence_time is not None:
|
||||
return 'Suche Satelliten'
|
||||
return 'Initialisiere'
|
||||
|
||||
|
||||
def configure_serial(fd, baud):
|
||||
attrs = termios.tcgetattr(fd)
|
||||
attrs[0] = 0
|
||||
attrs[1] = 0
|
||||
attrs[2] = termios.CS8 | termios.CREAD | termios.CLOCAL
|
||||
attrs[3] = 0
|
||||
attrs[4] = baud
|
||||
attrs[5] = baud
|
||||
attrs[6][termios.VMIN] = 0
|
||||
attrs[6][termios.VTIME] = 0
|
||||
termios.tcsetattr(fd, termios.TCSANOW, attrs)
|
||||
termios.tcflush(fd, termios.TCIFLUSH)
|
||||
|
||||
|
||||
def open_device(port, baud):
|
||||
while not rospy.is_shutdown():
|
||||
try:
|
||||
fd = os.open(port, os.O_RDONLY | os.O_NOCTTY | os.O_NONBLOCK)
|
||||
configure_serial(fd, baud)
|
||||
rospy.loginfo('GPS verbunden: %s @ %s Baud', port, baud_rate)
|
||||
return fd
|
||||
except OSError as exc:
|
||||
if exc.errno != errno.ENOENT:
|
||||
rospy.logwarn('GPS kann nicht geoeffnet werden: %s', exc)
|
||||
rospy.sleep(1.0)
|
||||
except termios.error as exc:
|
||||
rospy.logwarn('GPS-Port kann nicht konfiguriert 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
|
||||
|
||||
|
||||
def parse_gga(fields, state):
|
||||
if len(fields) < 10:
|
||||
return False
|
||||
|
||||
latitude = nmea_to_decimal(fields[2], fields[3])
|
||||
longitude = nmea_to_decimal(fields[4], fields[5])
|
||||
|
||||
try:
|
||||
fix_quality = int(fields[6] or 0)
|
||||
except ValueError:
|
||||
fix_quality = 0
|
||||
|
||||
try:
|
||||
satellites = int(fields[7] or 0)
|
||||
except ValueError:
|
||||
satellites = 0
|
||||
|
||||
try:
|
||||
hdop = float(fields[8]) if fields[8] else None
|
||||
except ValueError:
|
||||
hdop = None
|
||||
|
||||
try:
|
||||
altitude = float(fields[9]) if fields[9] else None
|
||||
except ValueError:
|
||||
altitude = None
|
||||
|
||||
if latitude is not None:
|
||||
state.latitude = latitude
|
||||
if longitude is not None:
|
||||
state.longitude = longitude
|
||||
state.fix_quality = fix_quality
|
||||
state.satellites = satellites
|
||||
state.hdop = hdop
|
||||
state.altitude = altitude
|
||||
state.valid = fix_quality > 0 and latitude is not None and longitude is not None
|
||||
if state.valid:
|
||||
state.last_fix_time = rospy.Time.now()
|
||||
return True
|
||||
|
||||
|
||||
def parse_rmc(fields, state):
|
||||
if len(fields) < 10:
|
||||
return False
|
||||
|
||||
status = fields[2] if len(fields) > 2 else ''
|
||||
latitude = nmea_to_decimal(fields[3], fields[4])
|
||||
longitude = nmea_to_decimal(fields[5], fields[6])
|
||||
|
||||
try:
|
||||
speed_knots = float(fields[7]) if fields[7] else 0.0
|
||||
except ValueError:
|
||||
speed_knots = 0.0
|
||||
|
||||
try:
|
||||
track_deg = float(fields[8]) if fields[8] else 0.0
|
||||
except ValueError:
|
||||
track_deg = 0.0
|
||||
|
||||
if latitude is not None:
|
||||
state.latitude = latitude
|
||||
if longitude is not None:
|
||||
state.longitude = longitude
|
||||
state.speed_mps = speed_knots * 0.514444
|
||||
state.track_deg = track_deg
|
||||
if status == 'A' and state.latitude is not None and state.longitude is not None:
|
||||
state.valid = True
|
||||
state.last_fix_time = rospy.Time.now()
|
||||
|
||||
# Optional date/time parse for debugging consistency.
|
||||
if fields[1] and fields[9]:
|
||||
try:
|
||||
datetime.strptime(fields[9] + fields[1].split('.')[0], '%d%m%y%H%M%S')
|
||||
except ValueError:
|
||||
pass
|
||||
return True
|
||||
|
||||
|
||||
def parse_sentence(sentence, state):
|
||||
if not verify_checksum(sentence):
|
||||
return False
|
||||
|
||||
fields = sentence.split('*', 1)[0].split(',')
|
||||
if not fields:
|
||||
return False
|
||||
|
||||
sentence_type = fields[0]
|
||||
state.last_sentence_time = rospy.Time.now()
|
||||
|
||||
if sentence_type.endswith('GGA'):
|
||||
return parse_gga(fields, state)
|
||||
if sentence_type.endswith('RMC'):
|
||||
return parse_rmc(fields, state)
|
||||
return False
|
||||
|
||||
|
||||
def publish_fix(fix_publisher, overlay_publisher, state, frame_id):
|
||||
message = NavSatFix()
|
||||
message.header.stamp = rospy.Time.now()
|
||||
message.header.frame_id = frame_id
|
||||
message.status.service = NavSatStatus.SERVICE_GPS
|
||||
|
||||
if state.valid and state.latitude is not None and state.longitude is not None:
|
||||
message.status.status = NavSatStatus.STATUS_FIX
|
||||
message.latitude = state.latitude
|
||||
message.longitude = state.longitude
|
||||
message.altitude = state.altitude if state.altitude is not None else 0.0
|
||||
if state.hdop is not None:
|
||||
horizontal_variance = max(0.25, state.hdop * state.hdop)
|
||||
vertical_variance = max(1.0, (state.hdop * 2.0) * (state.hdop * 2.0))
|
||||
message.position_covariance = [
|
||||
horizontal_variance, 0.0, 0.0,
|
||||
0.0, horizontal_variance, 0.0,
|
||||
0.0, 0.0, vertical_variance
|
||||
]
|
||||
message.position_covariance_type = NavSatFix.COVARIANCE_TYPE_APPROXIMATED
|
||||
else:
|
||||
message.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
|
||||
else:
|
||||
message.status.status = NavSatStatus.STATUS_NO_FIX
|
||||
message.latitude = 0.0
|
||||
message.longitude = 0.0
|
||||
message.altitude = 0.0
|
||||
message.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
|
||||
|
||||
fix_publisher.publish(message)
|
||||
overlay_publisher.publish(String(data=state.summary_text()))
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
rospy.init_node('gps_node')
|
||||
|
||||
port = rospy.get_param('~port', PORT)
|
||||
baud_rate = int(rospy.get_param('~baud', BAUD))
|
||||
frame_id = rospy.get_param('~frame_id', FRAME_ID)
|
||||
baud = BAUD_MAP.get(baud_rate, termios.B4800)
|
||||
|
||||
fix_publisher = rospy.Publisher('/fix', NavSatFix, queue_size=5)
|
||||
overlay_publisher = rospy.Publisher('/gps/status_text', String, queue_size=5)
|
||||
|
||||
state = GpsState()
|
||||
serial_fd = None
|
||||
buffer = bytearray()
|
||||
|
||||
while not rospy.is_shutdown():
|
||||
if serial_fd is None:
|
||||
serial_fd = open_device(port, baud)
|
||||
buffer = bytearray()
|
||||
state.connected = serial_fd is not None
|
||||
publish_fix(fix_publisher, overlay_publisher, state, frame_id)
|
||||
if serial_fd is None:
|
||||
break
|
||||
|
||||
try:
|
||||
readable, _, _ = select.select([serial_fd], [], [], 0.5)
|
||||
if serial_fd not in readable:
|
||||
publish_fix(fix_publisher, overlay_publisher, state, frame_id)
|
||||
continue
|
||||
|
||||
chunk = os.read(serial_fd, READ_SIZE)
|
||||
if not chunk:
|
||||
raise OSError(errno.ENODEV, 'GPS getrennt')
|
||||
|
||||
buffer.extend(chunk)
|
||||
|
||||
while b'\n' in buffer:
|
||||
raw_line, _, buffer = buffer.partition(b'\n')
|
||||
line = raw_line.decode('ascii', 'ignore').strip()
|
||||
if not line or not line.startswith('$'):
|
||||
continue
|
||||
if parse_sentence(line, state):
|
||||
publish_fix(fix_publisher, overlay_publisher, state, frame_id)
|
||||
|
||||
except OSError as exc:
|
||||
if exc.errno not in (errno.EAGAIN, errno.EWOULDBLOCK):
|
||||
rospy.logwarn('GPS-Lesefehler: %s', exc)
|
||||
close_device(serial_fd)
|
||||
serial_fd = None
|
||||
state.connected = False
|
||||
state.valid = False
|
||||
state.last_sentence_time = None
|
||||
state.satellites = 0
|
||||
rospy.sleep(1.0)
|
||||
except termios.error as exc:
|
||||
rospy.logwarn('GPS-Fehler: %s', exc)
|
||||
close_device(serial_fd)
|
||||
serial_fd = None
|
||||
state.connected = False
|
||||
state.valid = False
|
||||
state.last_sentence_time = None
|
||||
state.satellites = 0
|
||||
rospy.sleep(1.0)
|
||||
|
||||
close_device(serial_fd)
|
||||
Reference in New Issue
Block a user