From 7a852a17c09f60c4decf791f55f532324996ec24 Mon Sep 17 00:00:00 2001 From: Irina Date: Wed, 11 Nov 2026 13:45:39 -0300 Subject: [PATCH 1/2] Publish outdoor robots' poses in UTM frames Add RobotSession.publish_utm_pose, which sends a pose in a utm- frame on ros/loc/pose (as the agent does, with the 32-bit float offset split), so InOrbit creates the robot's outdoor map and draws it with its heading, with no spatial transformation or map configured. publish_geo_pose does the same from a GPS position and heading. inorbit_edge.geo holds the WGS84 to UTM projection and MapDatum, to convert poses in a local map frame anchored to a datum. --- inorbit_edge/geo.py | 184 ++++++++++++++++++ inorbit_edge/robot.py | 101 ++++++++-- inorbit_edge/tests/test_geo.py | 112 +++++++++++ .../tests/test_robot_session_utm_pose.py | 82 ++++++++ 4 files changed, 466 insertions(+), 13 deletions(-) create mode 100644 inorbit_edge/geo.py create mode 100644 inorbit_edge/tests/test_geo.py create mode 100644 inorbit_edge/tests/test_robot_session_utm_pose.py diff --git a/inorbit_edge/geo.py b/inorbit_edge/geo.py new file mode 100644 index 0000000..164fea8 --- /dev/null +++ b/inorbit_edge/geo.py @@ -0,0 +1,184 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""Geographic helpers for outdoor robots: WGS84 to UTM. + +InOrbit shows outdoor robots on UTM maps, one per zone, with frame ids ``utm-`` +(``utm--s`` south of the equator), and creates a robot's UTM map by itself the +first time a pose in such a frame arrives (``RobotSession.publish_utm_pose``). These +helpers give connectors that pose: :func:`latlon_to_utm_pose` from a GPS position and +heading, and :class:`MapDatum` from a pose in a local map frame anchored to a datum. + +The projection is the Krüger series (3 terms, sub-millimeter within a zone), the same +one InOrbit's simulations use; no external dependency. +""" + +import math +from typing import Tuple + +# WGS84 +_A = 6378137.0 +_F = 1 / 298.257223563 +_E2 = _F * (2 - _F) +_E = math.sqrt(_E2) +_N = _F / (2 - _F) +_RECTIFYING_RADIUS = _A / (1 + _N) * (1 + _N**2 / 4 + _N**4 / 64) +_ALPHA = ( + _N / 2 - 2 * _N**2 / 3 + 5 * _N**3 / 16, + 13 * _N**2 / 48 - 3 * _N**3 / 5, + 61 * _N**3 / 240, +) +_K0 = 0.9996 +_FALSE_EASTING = 500000.0 +_FALSE_NORTHING_SOUTH = 10000000.0 + +# Distance used to measure the local scale and rotation of the projection at the datum +_DIFF_STEP_M = 100.0 + + +def utm_zone(longitude: float) -> int: + """The UTM zone (1..60) of a longitude in degrees.""" + return min(int((longitude + 180) / 6) + 1, 60) + + +def utm_frame_id(latitude: float, longitude: float) -> str: + """InOrbit's frame id for the UTM zone of a position: ``utm-``, and + ``utm--s`` south of the equator.""" + zone = utm_zone(longitude) + return f"utm-{zone}-s" if latitude < 0 else f"utm-{zone}" + + +def latlon_to_utm( + latitude: float, longitude: float, zone: int = None +) -> Tuple[float, float]: + """Projects a WGS84 position to UTM. + + Args: + latitude (float): degrees, -80..84. + longitude (float): degrees. + zone (int, optional): the zone to project to. Defaults to the position's own + zone. + + Returns: + Tuple[float, float]: easting and northing in meters (northing with the 10000 km + false northing south of the equator). + """ + if not -80.0 <= latitude <= 84.0: + raise ValueError(f"UTM covers latitudes -80..84, not {latitude}") + zone = zone or utm_zone(longitude) + phi = math.radians(latitude) + lam = math.radians(longitude - (6 * zone - 183)) + t = math.sinh(math.atanh(math.sin(phi)) - _E * math.atanh(_E * math.sin(phi))) + xi = math.atan2(t, math.cos(lam)) + eta = math.atanh(math.sin(lam) / math.sqrt(1 + t * t)) + easting, northing = eta, xi + for j, alpha in enumerate(_ALPHA, start=1): + easting += alpha * math.cos(2 * j * xi) * math.sinh(2 * j * eta) + northing += alpha * math.sin(2 * j * xi) * math.cosh(2 * j * eta) + easting = _FALSE_EASTING + _K0 * _RECTIFYING_RADIUS * easting + northing = _K0 * _RECTIFYING_RADIUS * northing + if latitude < 0: + northing += _FALSE_NORTHING_SOUTH + return easting, northing + + +class MapDatum: + """A robot's local map frame anchored to the Earth: its origin is at (latitude, + longitude), x points east and y north, rotated by ``yaw`` (radians, + counterclockwise from east), as robot_localization's navsat_transform datum. + + It converts map poses to the UTM frame InOrbit draws outdoor robots in, through the + affine map -> UTM transformation at the datum: exact at the origin, within + centimeters over a few kilometers. + """ + + def __init__(self, latitude: float, longitude: float, yaw: float = 0.0): + """ + Args: + latitude (float): latitude of the map origin, degrees. + longitude (float): longitude of the map origin, degrees. + yaw (float, optional): angle of the map's x axis from east, radians. + Defaults to 0 (x east, y north). + """ + self.latitude = latitude + self.longitude = longitude + self.yaw = yaw + self.frame_id = utm_frame_id(latitude, longitude) + self._zone = utm_zone(longitude) + + # Local east and north derivatives of the projection, by central differences + # of _DIFF_STEP_M: they hold the grid convergence and the scale factor here + phi = math.radians(latitude) + w = math.sqrt(1 - _E2 * math.sin(phi) ** 2) + dlat = math.degrees(_DIFF_STEP_M / (_A * (1 - _E2) / w**3)) + dlon = math.degrees(_DIFF_STEP_M / (_A / w * math.cos(phi))) + e_east1, n_east1 = self._utm(latitude, longitude + dlon) + e_east0, n_east0 = self._utm(latitude, longitude - dlon) + e_north1, n_north1 = self._utm(latitude + dlat, longitude) + e_north0, n_north0 = self._utm(latitude - dlat, longitude) + step = 2 * _DIFF_STEP_M + east = ((e_east1 - e_east0) / step, (n_east1 - n_east0) / step) + north = ((e_north1 - e_north0) / step, (n_north1 - n_north0) / step) + + # map -> local east/north (the datum yaw), then east/north -> UTM + cos_yaw, sin_yaw = math.cos(yaw), math.sin(yaw) + self._a00 = east[0] * cos_yaw + north[0] * sin_yaw + self._a01 = -east[0] * sin_yaw + north[0] * cos_yaw + self._a10 = east[1] * cos_yaw + north[1] * sin_yaw + self._a11 = -east[1] * sin_yaw + north[1] * cos_yaw + self._origin = self._utm(latitude, longitude) + # The map's x axis direction in the UTM grid + self._yaw_offset = math.atan2(self._a10, self._a00) + + def _utm(self, latitude, longitude): + # Always the datum's zone, so a robot near a zone border stays in one frame + return latlon_to_utm(latitude, longitude, self._zone) + + @property + def transformation(self): + """The map -> UTM transformation, as the 3x3 matrix of an InOrbit + SpatialTransformation.""" + return [ + [self._a00, self._a01, self._origin[0]], + [self._a10, self._a11, self._origin[1]], + [0.0, 0.0, 1.0], + ] + + def map_to_utm(self, x: float, y: float, yaw: float) -> Tuple[float, float, float]: + """Converts a pose in the robot's map frame to the datum's UTM frame. + + Args: + x (float): meters. + y (float): meters. + yaw (float): radians. + + Returns: + Tuple[float, float, float]: easting, northing (meters) and yaw in the UTM + grid (radians, counterclockwise from grid east, wrapped to -pi..pi). + """ + easting = self._origin[0] + self._a00 * x + self._a01 * y + northing = self._origin[1] + self._a10 * x + self._a11 * y + utm_yaw = math.atan2( + math.sin(yaw + self._yaw_offset), math.cos(yaw + self._yaw_offset) + ) + return easting, northing, utm_yaw + + +def latlon_to_utm_pose( + latitude: float, longitude: float, yaw: float +) -> Tuple[str, float, float, float]: + """Converts a GPS position and heading to a pose in the UTM frame of its zone. + + Args: + latitude (float): degrees, -80..84. + longitude (float): degrees. + yaw (float): radians, counterclockwise from east (ENU, as ROS). + + Returns: + Tuple[str, float, float, float]: frame id, easting, northing (meters) and yaw in + the UTM grid (radians, counterclockwise from grid east, wrapped to -pi..pi). + """ + # A datum at the position itself: exact there, and it turns the heading by the grid + # convergence + datum = MapDatum(latitude, longitude) + easting, northing, utm_yaw = datum.map_to_utm(0.0, 0.0, yaw) + return datum.frame_id, easting, northing, utm_yaw diff --git a/inorbit_edge/robot.py b/inorbit_edge/robot.py index 723a5cb..1ce5dd8 100644 --- a/inorbit_edge/robot.py +++ b/inorbit_edge/robot.py @@ -45,9 +45,12 @@ OdometryDataMessage, PathDataMessage, PathPoint, + PoseMessage, + PoseMessageData, RobotPath, SystemStatsMessage, ) +from inorbit_edge.geo import latlon_to_utm_pose from inorbit_edge.metrics import ( attrs_from_self, publish_camera_frame_counter, @@ -73,6 +76,13 @@ INORBIT_DEFAULT_API_URL = "https://api.inorbit.ai" MQTT_SUBTOPIC_POSE = "ros/loc/data2" +# Poses in UTM frames (outdoor robots). InOrbit creates the robot's UTM map the first +# time one arrives on this topic. +MQTT_SUBTOPIC_UTM_POSE = "ros/loc/pose" +# Protobuf positions are 32-bit floats: UTM coordinates are split into a multiple of +# this (offset) and the rest, as the agent does, so they keep millimeters instead of +# steps of ~25 cm +POSITION_OFFSET_STEP = 10000.0 MQTT_SUBTOPIC_PATH = "ros/loc/path" MQTT_SUBTOPIC_LASER_CONFIG_BASE = "ros/loc/config/" MQTT_SUBTOPIC_ODOMETRY = "ros/odometry/data" @@ -1440,6 +1450,83 @@ def publish_pose(self, x, y, yaw, frame_id="map", ts=None): ts (int, optional): Pose timestamp. Defaults to int(time() * 1000). """ + pose_ts = self._track_pose(x, y, yaw, frame_id, ts) + + if not self._should_publish_message(method="publish_pose"): + return None + + msg = LocationAndPoseMessage() + msg.ts = pose_ts + msg.pos_x = x + msg.pos_y = y + msg.yaw = yaw + msg.frame_id = frame_id + + self.publish_protobuf(MQTT_SUBTOPIC_POSE, msg) + + def publish_utm_pose(self, easting, northing, yaw, frame_id, ts=None): + """Publish the pose of an outdoor robot in a UTM frame. + + InOrbit shows outdoor robots on UTM maps, one per zone, and creates the robot's + UTM map the first time a pose in its frame arrives: no spatial transformation or + map has to be configured for the robot. Robots that localize in their own map + frame can convert their poses with ``inorbit_edge.geo.MapDatum``. + + It shares ``publish_pose``'s throttling, and its pose is the one used for + distances and waypoints. + + Args: + easting (float): meters. + northing (float): meters (with the 10000 km false northing south of the + equator). + yaw (float): radians, counterclockwise from grid east. + frame_id (str): ``utm-``, or ``utm--s`` south of the equator + (see ``inorbit_edge.geo.utm_frame_id``). + ts (int, optional): Pose timestamp. Defaults to int(time() * 1000). + """ + pose_ts = self._track_pose(easting, northing, yaw, frame_id, ts) + + if not self._should_publish_message(method="publish_pose"): + return None + + data = PoseMessageData() + data.ts = pose_ts + data.pos_x, data.offset_x = self._split_position(easting) + data.pos_y, data.offset_y = self._split_position(northing) + data.yaw = yaw + data.frame_id = frame_id + msg = PoseMessage() + msg.poses.append(data) + + self.publish_protobuf(MQTT_SUBTOPIC_UTM_POSE, msg) + + def publish_geo_pose(self, latitude, longitude, yaw, ts=None): + """Publish the pose of an outdoor robot from its GPS position and heading. + + The position is projected to the UTM frame of its zone and sent with + ``publish_utm_pose``. A GPS position alone has no heading: send the robot's + orientation (from its IMU, INS or localization), or its icon won't turn. + + Args: + latitude (float): degrees, -80..84. + longitude (float): degrees. + yaw (float): radians, counterclockwise from east (ENU, as ROS). + ts (int, optional): Pose timestamp. Defaults to int(time() * 1000). + """ + frame_id, easting, northing, utm_yaw = latlon_to_utm_pose( + latitude, longitude, yaw + ) + self.publish_utm_pose(easting, northing, utm_yaw, frame_id, ts) + + @staticmethod + def _split_position(value): + """(position, offset) for a coordinate, see POSITION_OFFSET_STEP.""" + offset = round(value / POSITION_OFFSET_STEP) * POSITION_OFFSET_STEP + return value - offset, offset + + def _track_pose(self, x, y, yaw, frame_id, ts): + """Stores the robot's current pose and accumulates its distance, before any + throttling. Returns the pose timestamp.""" pose = Pose(frame_id=frame_id, x=x, y=y, theta=yaw) pose_ts = ts if ts else int(time.time() * 1000) @@ -1457,21 +1544,9 @@ def publish_pose(self, x, y, yaw, frame_id="map", ts=None): ) self._distance_accumulator.accumulate(pose, discard_delta=discard_delta) - # Store the pose before checking for throttling self._last_pose = pose self._last_pose_ts = pose_ts - - if not self._should_publish_message(method="publish_pose"): - return None - - msg = LocationAndPoseMessage() - msg.ts = pose_ts - msg.pos_x = x - msg.pos_y = y - msg.yaw = yaw - msg.frame_id = frame_id - - self.publish_protobuf(MQTT_SUBTOPIC_POSE, msg) + return pose_ts def reached_waypoint(self, waypoint: Pose, tolerance: SpatialTolerance): if self._last_pose is None: diff --git a/inorbit_edge/tests/test_geo.py b/inorbit_edge/tests/test_geo.py new file mode 100644 index 0000000..bde79a9 --- /dev/null +++ b/inorbit_edge/tests/test_geo.py @@ -0,0 +1,112 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""Tests for inorbit_edge.geo: UTM projection, GPS poses and map datums.""" + +import math + +import pytest + +from inorbit_edge.geo import ( + MapDatum, + latlon_to_utm, + latlon_to_utm_pose, + utm_frame_id, + utm_zone, +) + +# Reference values: InOrbit ingest's GPS tests (north and south of the equator) and the +# Flatland/Gazebo outdoor sims' datum, whose map -> utm-10 transformation those sims +# apply +GOLDEN_GATE = (37.811072, -122.477603, 545982.64, 4184981.92, "utm-10") +CORDOBA = (-31.315845, -64.323323, 374080.68, 6534637.77, "utm-20-s") +SIM_DATUM = (37.39276686732228, -122.07953949815608) +SIM_TRANSFORMATION = [ + [0.999634192, -0.009753199, 581476.705], + [0.009753199, 0.999634192, 4138842.183], +] + + +@pytest.mark.parametrize( + "lat, lon, easting, northing, frame_id", [GOLDEN_GATE, CORDOBA] +) +def test_latlon_to_utm(lat, lon, easting, northing, frame_id): + e, n = latlon_to_utm(lat, lon) + assert e == pytest.approx(easting, abs=0.01) + assert n == pytest.approx(northing, abs=0.01) + assert utm_frame_id(lat, lon) == frame_id + + +def test_utm_zone_edges(): + assert utm_zone(-180) == 1 + assert utm_zone(179.999) == 60 + assert utm_zone(180) == 60 + + +def test_latlon_to_utm_rejects_polar_latitudes(): + with pytest.raises(ValueError): + latlon_to_utm(85, 0) + + +def test_map_datum_transformation_matches_the_sims(): + datum = MapDatum(*SIM_DATUM) + assert datum.frame_id == "utm-10" + for row, expected in zip(datum.transformation, SIM_TRANSFORMATION): + assert row == pytest.approx(expected, abs=0.001) + assert datum.transformation[2] == [0.0, 0.0, 1.0] + + +def test_map_datum_origin_and_heading(): + datum = MapDatum(*SIM_DATUM) + e0, n0 = latlon_to_utm(*SIM_DATUM) + easting, northing, yaw = datum.map_to_utm(0, 0, 0) + assert (easting, northing) == pytest.approx((e0, n0), abs=0.001) + # Facing east in the map is facing east rotated by the grid convergence in UTM + assert yaw == pytest.approx(math.atan2(0.009753199, 0.999634192), abs=1e-6) + + +def test_map_datum_100_m_east_lands_100_m_east(): + datum = MapDatum(*SIM_DATUM) + # The point 100 m east of the datum, projected directly + phi = math.radians(SIM_DATUM[0]) + dlon = math.degrees( + 100 / (6378137 * math.cos(phi) / math.sqrt(1 - 0.00669438 * math.sin(phi) ** 2)) + ) + expected = latlon_to_utm(SIM_DATUM[0], SIM_DATUM[1] + dlon) + easting, northing, _ = datum.map_to_utm(100, 0, 0) + assert (easting, northing) == pytest.approx(expected, abs=0.01) + + +def test_map_datum_yaw_rotates_the_map(): + # A map whose x axis points north: map +x is UTM (about) +northing + datum = MapDatum(*SIM_DATUM, yaw=math.pi / 2) + e0, n0, _ = datum.map_to_utm(0, 0, 0) + e1, n1, yaw = datum.map_to_utm(10, 0, 0) + assert n1 - n0 == pytest.approx(10, abs=0.01) + assert abs(e1 - e0) < 0.2 + assert yaw == pytest.approx( + math.pi / 2 + math.atan2(0.009753199, 0.999634192), abs=1e-6 + ) + + +def test_map_datum_wraps_yaw(): + datum = MapDatum(*SIM_DATUM) + _, _, yaw = datum.map_to_utm(0, 0, math.pi) + assert -math.pi <= yaw <= math.pi + + +def test_latlon_to_utm_pose_matches_the_sims(): + frame_id, easting, northing, yaw = latlon_to_utm_pose(*SIM_DATUM, 0.0) + assert frame_id == "utm-10" + assert easting == pytest.approx(SIM_TRANSFORMATION[0][2], abs=0.01) + assert northing == pytest.approx(SIM_TRANSFORMATION[1][2], abs=0.01) + # East in the world is turned by the grid convergence in the UTM grid + convergence = math.atan2(SIM_TRANSFORMATION[1][0], SIM_TRANSFORMATION[0][0]) + assert yaw == pytest.approx(convergence, abs=1e-6) + + +def test_latlon_to_utm_pose_south(): + lat, lon, easting, northing, frame_id = CORDOBA + got_frame_id, e, n, _ = latlon_to_utm_pose(lat, lon, 1.0) + assert got_frame_id == frame_id + assert e == pytest.approx(easting, abs=0.01) + assert n == pytest.approx(northing, abs=0.01) diff --git a/inorbit_edge/tests/test_robot_session_utm_pose.py b/inorbit_edge/tests/test_robot_session_utm_pose.py new file mode 100644 index 0000000..6c2ff68 --- /dev/null +++ b/inorbit_edge/tests/test_robot_session_utm_pose.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +"""Tests for RobotSession.publish_utm_pose and publish_geo_pose.""" + +from unittest.mock import MagicMock + +import pytest + +from inorbit_edge.geo import latlon_to_utm_pose +from inorbit_edge.inorbit_pb2 import LocationAndPoseMessage, PoseMessage +from inorbit_edge.robot import ( + MQTT_SUBTOPIC_POSE, + MQTT_SUBTOPIC_UTM_POSE, + RobotSession, +) + +SIM_DATUM = (37.39276686732228, -122.07953949815608) +SIM_UTM = (581476.705, 4138842.183) + + +@pytest.fixture +def robot_session(mock_sleep): + session = RobotSession( + robot_id="id_123", robot_name="name_123", api_key="apikey_123" + ) + session.publish_protobuf = MagicMock() + return session + + +def published(session): + """(topic, message) of the only publish_protobuf call.""" + session.publish_protobuf.assert_called_once() + topic, msg = session.publish_protobuf.call_args.args[:2] + return topic, msg + + +def test_publish_pose_stays_in_the_map_frame(robot_session): + robot_session.publish_pose(1.0, 2.0, 0.5) + topic, msg = published(robot_session) + assert topic == MQTT_SUBTOPIC_POSE + assert isinstance(msg, LocationAndPoseMessage) + assert msg.frame_id == "map" + + +def test_publish_utm_pose(robot_session): + robot_session.publish_utm_pose(*SIM_UTM, 0.5, "utm-10", ts=1234) + topic, msg = published(robot_session) + assert topic == MQTT_SUBTOPIC_UTM_POSE + assert isinstance(msg, PoseMessage) + (pose,) = msg.poses + assert pose.frame_id == "utm-10" + assert pose.ts == 1234 + # Split into offset + position, so 32-bit floats keep millimeters + assert pose.offset_x % 10000 == 0 and pose.offset_y % 10000 == 0 + assert abs(pose.pos_x) <= 5000 and abs(pose.pos_y) <= 5000 + assert pose.offset_x + pose.pos_x == pytest.approx(SIM_UTM[0], abs=0.001) + assert pose.offset_y + pose.pos_y == pytest.approx(SIM_UTM[1], abs=0.001) + assert pose.yaw == pytest.approx(0.5, abs=1e-6) + + +def test_publish_utm_pose_tracks_the_pose(robot_session): + robot_session.publish_utm_pose(*SIM_UTM, 0.5, "utm-10") + assert robot_session._last_pose.frame_id == "utm-10" + assert (robot_session._last_pose.x, robot_session._last_pose.y) == SIM_UTM + + +def test_publish_utm_pose_shares_publish_pose_throttling(robot_session): + robot_session.publish_pose(1.0, 2.0, 0.5) + robot_session.publish_utm_pose(*SIM_UTM, 0.5, "utm-10") + robot_session.publish_protobuf.assert_called_once() + + +def test_publish_geo_pose(robot_session): + robot_session.publish_geo_pose(*SIM_DATUM, 0.5, ts=1234) + topic, msg = published(robot_session) + frame_id, easting, northing, yaw = latlon_to_utm_pose(*SIM_DATUM, 0.5) + assert topic == MQTT_SUBTOPIC_UTM_POSE + (pose,) = msg.poses + assert pose.frame_id == frame_id == "utm-10" + assert pose.offset_x + pose.pos_x == pytest.approx(easting, abs=0.001) + assert pose.offset_y + pose.pos_y == pytest.approx(northing, abs=0.001) + assert pose.yaw == pytest.approx(yaw, abs=1e-6) From d74cef98f63e23cd60bade7d78d2c826812ca033 Mon Sep 17 00:00:00 2001 From: Irina Date: Thu, 12 Nov 2026 09:10:22 -0300 Subject: [PATCH 2/2] Add publish_gps_fix for raw GPS fixes Raw GPS fixes go on ros/loc/gps/fix as the agent's GPSAgentlet sends them, from their own method: the pose methods never send them. --- inorbit_edge/robot.py | 51 +++++++++++++++++++ .../tests/test_robot_session_utm_pose.py | 33 +++++++++++- 2 files changed, 82 insertions(+), 2 deletions(-) diff --git a/inorbit_edge/robot.py b/inorbit_edge/robot.py index 1ce5dd8..1958514 100644 --- a/inorbit_edge/robot.py +++ b/inorbit_edge/robot.py @@ -39,6 +39,7 @@ Echo, KeyValueCustomElement, LaserMessage, + GpsFixMessage, LocationAndPoseMessage, MapMessage, MapRequest, @@ -83,6 +84,8 @@ # this (offset) and the rest, as the agent does, so they keep millimeters instead of # steps of ~25 cm POSITION_OFFSET_STEP = 10000.0 +# Raw GPS fixes (latitude, longitude), as the agent's GPSAgentlet sends them +MQTT_SUBTOPIC_GPS_FIX = "ros/loc/gps/fix" MQTT_SUBTOPIC_PATH = "ros/loc/path" MQTT_SUBTOPIC_LASER_CONFIG_BASE = "ros/loc/config/" MQTT_SUBTOPIC_ODOMETRY = "ros/odometry/data" @@ -485,6 +488,10 @@ def __init__(self, robot_id, robot_name, api_key=None, **kwargs) -> None: "min_time_between_calls": 1, # seconds }, "publish_key_values": {}, # this supports key-level throttling + "publish_gps_fix": { + "last_ts": 0, + "min_time_between_calls": 1, # seconds + }, "publish_odometry": { "last_ts": 0, "min_time_between_calls": 1, # seconds @@ -1518,6 +1525,50 @@ def publish_geo_pose(self, latitude, longitude, yaw, ts=None): ) self.publish_utm_pose(easting, northing, utm_yaw, frame_id, ts) + def publish_gps_fix( + self, + latitude, + longitude, + altitude=0.0, + accuracy_meters=None, + bearing=None, + ts=None, + ): + """Publish a raw GPS fix, as the agent's GPSAgentlet does. + + The fix is stored as the robot's GPS data (position, accuracy). It is not a + pose: to show the robot on the outdoor map with its heading, use + ``publish_utm_pose`` or ``publish_geo_pose``. Note that InOrbit also draws + each fix as a pose in the UTM frame of its zone, with the bearing (or no + heading) as its orientation, so a robot sending both may jump between them. + + Args: + latitude (float): degrees. + longitude (float): degrees. + altitude (float, optional): meters. Defaults to 0. + accuracy_meters (float, optional): horizontal accuracy, meters. Defaults to + None (unknown). + bearing (float, optional): direction of travel, degrees clockwise from + north (as gps_msgs/GPSFix track). Defaults to None (unknown). + ts (int, optional): Fix timestamp. Defaults to int(time() * 1000). + """ + if not self._should_publish_message(method="publish_gps_fix"): + return None + + msg = GpsFixMessage() + msg.ts = ts if ts else int(time.time() * 1000) + msg.latitude = latitude + msg.longitude = longitude + msg.altitude = altitude + if accuracy_meters is not None: + msg.has_accuracy = True + msg.accuracy_meters = accuracy_meters + if bearing is not None: + msg.has_bearing = True + msg.bearing = bearing + + self.publish_protobuf(MQTT_SUBTOPIC_GPS_FIX, msg) + @staticmethod def _split_position(value): """(position, offset) for a coordinate, see POSITION_OFFSET_STEP.""" diff --git a/inorbit_edge/tests/test_robot_session_utm_pose.py b/inorbit_edge/tests/test_robot_session_utm_pose.py index 6c2ff68..7661163 100644 --- a/inorbit_edge/tests/test_robot_session_utm_pose.py +++ b/inorbit_edge/tests/test_robot_session_utm_pose.py @@ -1,14 +1,19 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- -"""Tests for RobotSession.publish_utm_pose and publish_geo_pose.""" +"""Tests for RobotSession's outdoor methods: UTM and geo poses, GPS fixes.""" from unittest.mock import MagicMock import pytest from inorbit_edge.geo import latlon_to_utm_pose -from inorbit_edge.inorbit_pb2 import LocationAndPoseMessage, PoseMessage +from inorbit_edge.inorbit_pb2 import ( + GpsFixMessage, + LocationAndPoseMessage, + PoseMessage, +) from inorbit_edge.robot import ( + MQTT_SUBTOPIC_GPS_FIX, MQTT_SUBTOPIC_POSE, MQTT_SUBTOPIC_UTM_POSE, RobotSession, @@ -80,3 +85,27 @@ def test_publish_geo_pose(robot_session): assert pose.offset_x + pose.pos_x == pytest.approx(easting, abs=0.001) assert pose.offset_y + pose.pos_y == pytest.approx(northing, abs=0.001) assert pose.yaw == pytest.approx(yaw, abs=1e-6) + + +def test_publish_gps_fix(robot_session): + robot_session.publish_gps_fix(*SIM_DATUM, altitude=10.0, ts=1234) + topic, msg = published(robot_session) + assert topic == MQTT_SUBTOPIC_GPS_FIX + assert isinstance(msg, GpsFixMessage) + assert (msg.latitude, msg.longitude) == pytest.approx(SIM_DATUM) + assert msg.altitude == pytest.approx(10.0) + assert msg.ts == 1234 + assert not msg.has_accuracy and not msg.has_bearing + + +def test_publish_gps_fix_with_accuracy_and_bearing(robot_session): + robot_session.publish_gps_fix(*SIM_DATUM, accuracy_meters=0.02, bearing=90.0) + _, msg = published(robot_session) + assert msg.has_accuracy and msg.accuracy_meters == pytest.approx(0.02) + assert msg.has_bearing and msg.bearing == pytest.approx(90.0) + + +def test_poses_send_no_gps_fix(robot_session): + robot_session.publish_geo_pose(*SIM_DATUM, 0.5) + topic, _ = published(robot_session) + assert topic == MQTT_SUBTOPIC_UTM_POSE