From bf56489a8499855f6d970adf4b3e5511c209af15 Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Tue, 11 Jul 2023 22:55:49 -0400 Subject: [PATCH 01/81] read messages from base station --- config/zed_f9p.yaml | 5 +++++ launch/ublox_device.launch | 13 +++++++++++++ src/localization/base_station_driver.py | 18 ++++++++++++++++++ 3 files changed, 36 insertions(+) create mode 100644 config/zed_f9p.yaml create mode 100644 launch/ublox_device.launch create mode 100644 src/localization/base_station_driver.py diff --git a/config/zed_f9p.yaml b/config/zed_f9p.yaml new file mode 100644 index 000000000..5d293a5b9 --- /dev/null +++ b/config/zed_f9p.yaml @@ -0,0 +1,5 @@ +device: /dev/ttyACM0 +frame_id: gps +uart1: + baudrate: 115200 +config_on_startup: false diff --git a/launch/ublox_device.launch b/launch/ublox_device.launch new file mode 100644 index 000000000..78cb95a14 --- /dev/null +++ b/launch/ublox_device.launch @@ -0,0 +1,13 @@ + + + + + + + + diff --git a/src/localization/base_station_driver.py b/src/localization/base_station_driver.py new file mode 100644 index 000000000..5c2804aef --- /dev/null +++ b/src/localization/base_station_driver.py @@ -0,0 +1,18 @@ +import rospy +from serial import Serial +from pyubx2 import UBXReader, UBX_PROTOCOL, RTCM3_PROTOCOL, protocol + +def main(): + with Serial('/dev/ttyACM0', 115200, timeout=1) as ser: + reader = UBXReader(ser, protfilter=(UBX_PROTOCOL | RTCM3_PROTOCOL)) + while True: + if ser.in_waiting: + (raw_data, parsed_data) = reader.read() + if parsed_data: + print(parsed_data.identity) + if protocol(raw_data) == RTCM3_PROTOCOL: + print("rtcm3") + + +if __name__ == '__main__': + main() \ No newline at end of file From 1a6884652b2b0c90658e1fe1eff3e6a022dddf27 Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Wed, 12 Jul 2023 21:22:46 -0400 Subject: [PATCH 02/81] sending RTCMs over ros --- config/esw.yaml | 5 +++ src/localization/base_station_driver.py | 18 -------- src/localization/basestation_gps_driver.py | 48 ++++++++++++++++++++++ 3 files changed, 53 insertions(+), 18 deletions(-) delete mode 100644 src/localization/base_station_driver.py create mode 100755 src/localization/basestation_gps_driver.py diff --git a/config/esw.yaml b/config/esw.yaml index 2af45fa24..cb732b45d 100644 --- a/config/esw.yaml +++ b/config/esw.yaml @@ -8,6 +8,11 @@ gps_driver: useRMC: false # get covariance instead of velocity, see wiki for more info frame_id: "base_link" +basestation_gps_driver: + port: "/dev/ttyACM0" + baud: 115200 + frame_id: "base_link" + imu_driver: port: "/dev/imu" baud: 115200 diff --git a/src/localization/base_station_driver.py b/src/localization/base_station_driver.py deleted file mode 100644 index 5c2804aef..000000000 --- a/src/localization/base_station_driver.py +++ /dev/null @@ -1,18 +0,0 @@ -import rospy -from serial import Serial -from pyubx2 import UBXReader, UBX_PROTOCOL, RTCM3_PROTOCOL, protocol - -def main(): - with Serial('/dev/ttyACM0', 115200, timeout=1) as ser: - reader = UBXReader(ser, protfilter=(UBX_PROTOCOL | RTCM3_PROTOCOL)) - while True: - if ser.in_waiting: - (raw_data, parsed_data) = reader.read() - if parsed_data: - print(parsed_data.identity) - if protocol(raw_data) == RTCM3_PROTOCOL: - print("rtcm3") - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/src/localization/basestation_gps_driver.py b/src/localization/basestation_gps_driver.py new file mode 100755 index 000000000..f2a764023 --- /dev/null +++ b/src/localization/basestation_gps_driver.py @@ -0,0 +1,48 @@ +#!/usr/bin/env python3 +import rospy +from serial import Serial +from pyubx2 import UBXReader, UBX_PROTOCOL, RTCM3_PROTOCOL, protocol +from rtcm_msgs.msg import Message + +def main(): + # TODO: change queue size + rtcm_pub = rospy.Publisher('rtcm', Message, queue_size=1) + rospy.init_node('basestation_driver') + port = rospy.get_param("basestation_gps_driver/port") + baud = rospy.get_param("basestation_gps_driver/baud") + svin_started = False + svin_complete = False + + # connect to GPS over serial, only read UBX and RTCM messages + with Serial(port, baud, timeout=1) as ser: + reader = UBXReader(ser, protfilter=(UBX_PROTOCOL | RTCM3_PROTOCOL)) + while not rospy.is_shutdown(): + if ser.in_waiting: + (raw_msg, msg) = reader.read() + + # skip if message could not be parsed + if not msg: + continue + + # publish RTCM messages + if protocol(raw_msg) == RTCM3_PROTOCOL: + rtcm_pub.publish(Message(message=raw_msg)) + + # print survey-in status + elif msg.identity == "NAV-SVIN": + if not svin_started and msg.active: + svin_started = True + rospy.loginfo("basestation survey-in started") + if not svin_complete and msg.valid: + svin_complete = True + rospy.loginfo(f"basestation survey-in complete, accuracy = {msg.meanAcc}") + if svin_started and not svin_complete: + print(f"current accuracy: {msg.meanAcc}") + + # fix quality information + elif msg.identity == "NAV-PVT": + print(f"{'valid' if msg.gnssFixOk else 'invalid'} fix, {msg.numSV} satellites used") + + +if __name__ == '__main__': + main() \ No newline at end of file From 8da08458b523326f2fa137c63baa348c81dc4d7f Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Wed, 12 Jul 2023 21:55:03 -0400 Subject: [PATCH 03/81] launch file with both GPSs, RTCMs successfully transmitted --- config/zed_f9p.yaml | 5 +++-- launch/rtk.launch | 10 ++++++++++ launch/ublox_device.launch | 11 ++++------- src/localization/basestation_gps_driver.py | 1 - 4 files changed, 17 insertions(+), 10 deletions(-) create mode 100644 launch/rtk.launch diff --git a/config/zed_f9p.yaml b/config/zed_f9p.yaml index 5d293a5b9..800b36dfe 100644 --- a/config/zed_f9p.yaml +++ b/config/zed_f9p.yaml @@ -1,5 +1,6 @@ -device: /dev/ttyACM0 -frame_id: gps +device: /dev/ttyACM1 +frame_id: base_link uart1: baudrate: 115200 config_on_startup: false +publish/rxm/all: true diff --git a/launch/rtk.launch b/launch/rtk.launch new file mode 100644 index 000000000..e59c32374 --- /dev/null +++ b/launch/rtk.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/launch/ublox_device.launch b/launch/ublox_device.launch index 78cb95a14..eddcc0e7c 100644 --- a/launch/ublox_device.launch +++ b/launch/ublox_device.launch @@ -2,12 +2,9 @@ - - + + + + diff --git a/src/localization/basestation_gps_driver.py b/src/localization/basestation_gps_driver.py index f2a764023..37646dab3 100755 --- a/src/localization/basestation_gps_driver.py +++ b/src/localization/basestation_gps_driver.py @@ -5,7 +5,6 @@ from rtcm_msgs.msg import Message def main(): - # TODO: change queue size rtcm_pub = rospy.Publisher('rtcm', Message, queue_size=1) rospy.init_node('basestation_driver') port = rospy.get_param("basestation_gps_driver/port") From a251f1c1f3189dcc277961d2b3f2014257169142 Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Fri, 14 Jul 2023 19:46:03 -0400 Subject: [PATCH 04/81] port change --- config/esw.yaml | 2 +- config/zed_f9p.yaml | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/config/esw.yaml b/config/esw.yaml index cb732b45d..767831ba9 100644 --- a/config/esw.yaml +++ b/config/esw.yaml @@ -9,7 +9,7 @@ gps_driver: frame_id: "base_link" basestation_gps_driver: - port: "/dev/ttyACM0" + port: "/dev/ttyACM1" baud: 115200 frame_id: "base_link" diff --git a/config/zed_f9p.yaml b/config/zed_f9p.yaml index 800b36dfe..1e70a463a 100644 --- a/config/zed_f9p.yaml +++ b/config/zed_f9p.yaml @@ -1,6 +1,7 @@ -device: /dev/ttyACM1 +device: /dev/ttyACM0 frame_id: base_link uart1: baudrate: 115200 config_on_startup: false publish/rxm/all: true +publish/all: true From deb37244f6a348273f2afd4b1d46058ed9fa3548 Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Sun, 30 Jul 2023 12:06:36 -0400 Subject: [PATCH 05/81] output screen --- launch/rtk.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/rtk.launch b/launch/rtk.launch index e59c32374..d9dfe8e64 100644 --- a/launch/rtk.launch +++ b/launch/rtk.launch @@ -3,7 +3,7 @@ - + From 68f9730404a46bba54a5b616acbd34edc0ee624c Mon Sep 17 00:00:00 2001 From: nehakankanala Date: Sun, 22 Oct 2023 17:47:21 +0000 Subject: [PATCH 06/81] added gps driver for rover gps and merged master into rtk --- src/localization/gps_driver.py | 61 ++++++++++++++++++++++++++++++++++ 1 file changed, 61 insertions(+) create mode 100644 src/localization/gps_driver.py diff --git a/src/localization/gps_driver.py b/src/localization/gps_driver.py new file mode 100644 index 000000000..3d992813d --- /dev/null +++ b/src/localization/gps_driver.py @@ -0,0 +1,61 @@ +''' +Reads and parses NMEA messages from the onboard GPS to provide +location data to the rover over LCM (/gps). Subscribes to +/rtcm and passes RTCM messages to the onboard gps to +acquire an RTK fix. +''' +import serial +import asyncio +import rospy +import threading +import numpy as np +from os import getenv +from rover_msgs import GPS, RTCM + + +class GPS_Driver(): + + def __init__(self, port, baudrate): + self.port = port + self.baudrate = baudrate + self.base_station_sub = rospy.Subscriber('/rtcm', RTCM, process_rtcm) + self.gps_pub = rospy.Publisher('/gps', NavSatFix, queue_size=1) + self.lock = threading.Lock() + + def connect(self): + #open connection to rover GPS + self.ser = serial.Serial(self.port, self.baudrate) + return self + + def exit(self): + #close connection + self.ser.close() + + def process_rtcm(self, data): + self.lock.acquire() + rtcm_data = RTCM.decode(data) + self.ser.write(rtcm_data) + self.lock.release() + + def parse_rover_gps_data(self, rover_gps_data): + pass + + def gps_data_thread(self): + while not rospy.is_shutdown(): + self.lock.acquire() + rover_gps_data = self.ser.readline() + parsed_gps_data = self.parse_rover_gps_data(rover_gps_data) + self.gps_pub.publish(parsed_gps_data) + self.lock.release() + + +def main(): + #change values + rtk_manager = GPS_Driver("port", baudrate=1500) + rtk_manager.connect() + rover_gps_thread = threading.Thread(rtk_manager.gps_data_thread) + rover_gps_thread.start() + + +if __name__ == "__main__": + main() \ No newline at end of file From de9c1146cc5fa6462ce09eb89c2982f08eb62f17 Mon Sep 17 00:00:00 2001 From: nehakankanala Date: Thu, 26 Oct 2023 21:44:45 +0000 Subject: [PATCH 07/81] edited rtk launch and esw.yaml --- config/esw.yaml | 5 +++++ launch/rtk.launch | 1 + src/localization/gps_driver.py | 5 +++-- 3 files changed, 9 insertions(+), 2 deletions(-) diff --git a/config/esw.yaml b/config/esw.yaml index 767831ba9..b30ff9649 100644 --- a/config/esw.yaml +++ b/config/esw.yaml @@ -8,6 +8,11 @@ gps_driver: useRMC: false # get covariance instead of velocity, see wiki for more info frame_id: "base_link" +rover_gps_driver: + port: "TODO" + baud: 115200 + frame_id: "base_link" + basestation_gps_driver: port: "/dev/ttyACM1" baud: 115200 diff --git a/launch/rtk.launch b/launch/rtk.launch index d9dfe8e64..30ddef06c 100644 --- a/launch/rtk.launch +++ b/launch/rtk.launch @@ -4,6 +4,7 @@ + diff --git a/src/localization/gps_driver.py b/src/localization/gps_driver.py index 3d992813d..5485aa3f1 100644 --- a/src/localization/gps_driver.py +++ b/src/localization/gps_driver.py @@ -16,8 +16,9 @@ class GPS_Driver(): def __init__(self, port, baudrate): - self.port = port - self.baudrate = baudrate + rospy.init_node('gps_driver') + self.port = rospy.get_param("rover_gps_driver/port") + self.baud = rospy.get_param("rover_gps_driver/baud") self.base_station_sub = rospy.Subscriber('/rtcm', RTCM, process_rtcm) self.gps_pub = rospy.Publisher('/gps', NavSatFix, queue_size=1) self.lock = threading.Lock() From 21f27c9321ceae26e8277801b3a70090f5009431 Mon Sep 17 00:00:00 2001 From: nehakankanala Date: Thu, 26 Oct 2023 21:48:48 +0000 Subject: [PATCH 08/81] used pyubx reader instead of serial getline() --- src/localization/gps_driver.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/localization/gps_driver.py b/src/localization/gps_driver.py index 5485aa3f1..a625484b4 100644 --- a/src/localization/gps_driver.py +++ b/src/localization/gps_driver.py @@ -11,6 +11,8 @@ import numpy as np from os import getenv from rover_msgs import GPS, RTCM +from pyubx2 import UBXReader, UBX_PROTOCOL, RTCM3_PROTOCOL, protocol +from rtcm_msgs.msg import Message class GPS_Driver(): @@ -44,7 +46,9 @@ def parse_rover_gps_data(self, rover_gps_data): def gps_data_thread(self): while not rospy.is_shutdown(): self.lock.acquire() - rover_gps_data = self.ser.readline() + reader = UBXReader(ser, protfilter=(UBX_PROTOCOL | RTCM3_PROTOCOL)) + # rover_gps_data = self.ser.readline() + raw, rover_gps_data = reader.read() parsed_gps_data = self.parse_rover_gps_data(rover_gps_data) self.gps_pub.publish(parsed_gps_data) self.lock.release() From 118ada5b94a2a288a4e2fd4c9f1dbb03cc473261 Mon Sep 17 00:00:00 2001 From: Pearl Lin Date: Fri, 27 Oct 2023 00:00:39 +0000 Subject: [PATCH 09/81] added preliminary dual gnss file --- src/localization/dual_gnss.py | 40 +++++++++++++++++++++++++++++++++++ 1 file changed, 40 insertions(+) create mode 100644 src/localization/dual_gnss.py diff --git a/src/localization/dual_gnss.py b/src/localization/dual_gnss.py new file mode 100644 index 000000000..8cbfaf9b1 --- /dev/null +++ b/src/localization/dual_gnss.py @@ -0,0 +1,40 @@ +import numpy as np +# 2 antennas on the rover a fixed distance apart and pointing in the same direction +# antennas receive GNSS data (latitude, longitude) + +# "mobile" RTK system in that the base station is also moving + +# heading measurement is derived from differencing the 2 GNSS measurements +# at a single point in time + +# vector connecting the two points (in latitude, longitude) can give accurate heading + +# 2 antennas are close to each other, so RTK error subtracting can be applied + +# bearing can be defined as the angle between the north-south line of the earth +# and the vector connecting the 2 GNSS antenna points + + +# bearing formula +# beta = atan2(X, Y) +# X = cos (latitude B) * sin(change in longitude) +# Y = cos (latitude A) * sin(latitude B) - sin(latitude A) * cos(latitude B) * cos(change in longitude) + + + +def get_heading(antenna_point_A, antenna_point_B): + latitude_A = np.radians(antenna_point_A[0]) + latitude_B = np.radians(antenna_point_B[0]) + longitude_A = np.radians(antenna_point_A[1]) + longitude_B = np.radians(antenna_point_B[1]) + + x = np.cos(latitude_B) * np.sin(longitude_B - longitude_A) + y = np.cos(latitude_A) * np.sin(latitude_B) - np.sin(latitude_A) * np.cos(latitude_B) * np.cos(longitude_B - longitude_A) + + bearing = np.arctan2(x, y) + return bearing + + + +test_bearing = get_heading(np.array([39.099912, -94.581213]), np.array([38.627089, -90.200203])) +print(test_bearing) From e7be851122017cd9d469074251fa42d4522d671f Mon Sep 17 00:00:00 2001 From: dllliu Date: Thu, 26 Oct 2023 20:03:40 -0400 Subject: [PATCH 10/81] add notes --- src/localization/dual_gnss.py | 21 ++++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) diff --git a/src/localization/dual_gnss.py b/src/localization/dual_gnss.py index 8cbfaf9b1..06f0c16b8 100644 --- a/src/localization/dual_gnss.py +++ b/src/localization/dual_gnss.py @@ -1,4 +1,5 @@ import numpy as np + # 2 antennas on the rover a fixed distance apart and pointing in the same direction # antennas receive GNSS data (latitude, longitude) @@ -20,6 +21,19 @@ # X = cos (latitude B) * sin(change in longitude) # Y = cos (latitude A) * sin(latitude B) - sin(latitude A) * cos(latitude B) * cos(change in longitude) +# rospy.init_node('gps_driver') + +# there are already imu and gps callback functions that publishes data to a linearized pose + +# A GNSS Compass/INS also contains an additional feature that utilizes the two separate +# onboard GNSS receivers to accurately estimate the system's heading: the powerful technique known as GNSS compassing. + +# moving baseline rtk (determine heading of rover based off of relative position of two attennas) + +# Bearing can be defined as direction or an angle, between the north-south line of earth or meridian +# and the line connecting the target and the reference point. + +# While Heading is an angle or direction where you are currently navigating in. def get_heading(antenna_point_A, antenna_point_B): @@ -27,14 +41,15 @@ def get_heading(antenna_point_A, antenna_point_B): latitude_B = np.radians(antenna_point_B[0]) longitude_A = np.radians(antenna_point_A[1]) longitude_B = np.radians(antenna_point_B[1]) - + x = np.cos(latitude_B) * np.sin(longitude_B - longitude_A) - y = np.cos(latitude_A) * np.sin(latitude_B) - np.sin(latitude_A) * np.cos(latitude_B) * np.cos(longitude_B - longitude_A) + y = np.cos(latitude_A) * np.sin(latitude_B) - np.sin(latitude_A) * np.cos(latitude_B) * np.cos( + longitude_B - longitude_A + ) bearing = np.arctan2(x, y) return bearing - test_bearing = get_heading(np.array([39.099912, -94.581213]), np.array([38.627089, -90.200203])) print(test_bearing) From 6087729d571e738af9dbc832a223e57b916f1165 Mon Sep 17 00:00:00 2001 From: dllliu Date: Sun, 29 Oct 2023 15:47:54 -0400 Subject: [PATCH 11/81] graph vectors for heading calculations --- src/localization/dual_gnss.py | 69 ++++++++++++++++------------------- 1 file changed, 32 insertions(+), 37 deletions(-) diff --git a/src/localization/dual_gnss.py b/src/localization/dual_gnss.py index 06f0c16b8..e6dbc57d7 100644 --- a/src/localization/dual_gnss.py +++ b/src/localization/dual_gnss.py @@ -1,39 +1,5 @@ import numpy as np - -# 2 antennas on the rover a fixed distance apart and pointing in the same direction -# antennas receive GNSS data (latitude, longitude) - -# "mobile" RTK system in that the base station is also moving - -# heading measurement is derived from differencing the 2 GNSS measurements -# at a single point in time - -# vector connecting the two points (in latitude, longitude) can give accurate heading - -# 2 antennas are close to each other, so RTK error subtracting can be applied - -# bearing can be defined as the angle between the north-south line of the earth -# and the vector connecting the 2 GNSS antenna points - - -# bearing formula -# beta = atan2(X, Y) -# X = cos (latitude B) * sin(change in longitude) -# Y = cos (latitude A) * sin(latitude B) - sin(latitude A) * cos(latitude B) * cos(change in longitude) - -# rospy.init_node('gps_driver') - -# there are already imu and gps callback functions that publishes data to a linearized pose - -# A GNSS Compass/INS also contains an additional feature that utilizes the two separate -# onboard GNSS receivers to accurately estimate the system's heading: the powerful technique known as GNSS compassing. - -# moving baseline rtk (determine heading of rover based off of relative position of two attennas) - -# Bearing can be defined as direction or an angle, between the north-south line of earth or meridian -# and the line connecting the target and the reference point. - -# While Heading is an angle or direction where you are currently navigating in. +import matplotlib.pyplot as plt def get_heading(antenna_point_A, antenna_point_B): @@ -51,5 +17,34 @@ def get_heading(antenna_point_A, antenna_point_B): return bearing -test_bearing = get_heading(np.array([39.099912, -94.581213]), np.array([38.627089, -90.200203])) -print(test_bearing) +# test_bearing = get_heading(np.array([39.099912, -94.581213]), np.array([38.627089, -90.200203])) +# print(test_bearing) + + +def spherical_to_cartesian(spherical_coord: np.ndarray, reference_coord: np.ndarray) -> np.ndarray: + r = 6371000 + x = r * (np.radians(spherical_coord[1]) - np.radians(reference_coord[1])) * np.cos(np.radians(reference_coord[0])) + y = r * (np.radians(spherical_coord[0]) - np.radians(reference_coord[0])) + z = 0 + return np.array([x, y, z]) + + +P_1 = spherical_to_cartesian(np.array([39.099912, -94.581213]), np.array([42.293195, -83.7096706])) + +P_2 = spherical_to_cartesian(np.array([38.627089, -90.200203]), np.array([42.293195, -83.7096706])) + +result = P_1 - P_2 + +plt.scatter([P_1[0], P_2[0], result[0]], [P_1[1], P_2[1], result[1]]) +plt.plot( + [P_1[0], P_2[0]], + [ + P_1[1], + P_2[1], + ], +) + +plt.xlabel("X-axis") +plt.ylabel("Y-axis") +plt.grid(True) +plt.show() From 9602c61f3c9a50e005d47df4a8b7d936a9dcd7cc Mon Sep 17 00:00:00 2001 From: dllliu Date: Mon, 30 Oct 2023 21:50:08 -0400 Subject: [PATCH 12/81] calc heading and bearing --- src/localization/dual_gnss.py | 59 ++++++++++++++++++++++------------- 1 file changed, 38 insertions(+), 21 deletions(-) diff --git a/src/localization/dual_gnss.py b/src/localization/dual_gnss.py index e6dbc57d7..b2759a3d2 100644 --- a/src/localization/dual_gnss.py +++ b/src/localization/dual_gnss.py @@ -1,6 +1,6 @@ import numpy as np import matplotlib.pyplot as plt - +import math def get_heading(antenna_point_A, antenna_point_B): latitude_A = np.radians(antenna_point_A[0]) @@ -16,11 +16,10 @@ def get_heading(antenna_point_A, antenna_point_B): bearing = np.arctan2(x, y) return bearing +test_bearing = get_heading(np.array([39.099912, -94.581213]), np.array([38.627089, -90.200203])) +print(np.degrees(test_bearing)) -# test_bearing = get_heading(np.array([39.099912, -94.581213]), np.array([38.627089, -90.200203])) -# print(test_bearing) - - +#reference point should be close def spherical_to_cartesian(spherical_coord: np.ndarray, reference_coord: np.ndarray) -> np.ndarray: r = 6371000 x = r * (np.radians(spherical_coord[1]) - np.radians(reference_coord[1])) * np.cos(np.radians(reference_coord[0])) @@ -28,23 +27,41 @@ def spherical_to_cartesian(spherical_coord: np.ndarray, reference_coord: np.ndar z = 0 return np.array([x, y, z]) +def plot_vectors(P_1, P_2): + result = P_1 - P_2 + plt.scatter([P_1[0], P_2[0], result[0]], [P_1[1], P_2[1], result[1]]) + plt.plot( + [P_1[0], P_2[0]], + [ + P_1[1], + P_2[1], + ], + ) -P_1 = spherical_to_cartesian(np.array([39.099912, -94.581213]), np.array([42.293195, -83.7096706])) + plt.xlabel("X-axis") + plt.ylabel("Y-axis") + plt.grid(True) + plt.show() +P_1 = spherical_to_cartesian(np.array([39.099912, -94.581213]), np.array([42.293195, -83.7096706])) P_2 = spherical_to_cartesian(np.array([38.627089, -90.200203]), np.array([42.293195, -83.7096706])) +# result = P_1 - P_2 +# plot_vectors(P_1, P_2) + +def calculate_angle_from_true_north(P_1, P_2): + result = P_1 - P_2 + print(result) + true_north = (0, 1, 0) + # Calculate the dot product between the resultant vector and the true north vector + dot_product = result[0] * true_north[0] + result[1] * true_north[1] + + # Calculate the magnitudes of both vectors + magnitude_result = math.sqrt(result[0]**2 + result[1]**2) + magnitude_north = math.sqrt(true_north[0]**2 + true_north[1]**2 + true_north[2]**2) + + angle_radians = math.acos(dot_product / (magnitude_result * magnitude_north)) + angle = math.degrees(angle_radians) + + return angle -result = P_1 - P_2 - -plt.scatter([P_1[0], P_2[0], result[0]], [P_1[1], P_2[1], result[1]]) -plt.plot( - [P_1[0], P_2[0]], - [ - P_1[1], - P_2[1], - ], -) - -plt.xlabel("X-axis") -plt.ylabel("Y-axis") -plt.grid(True) -plt.show() +print(calculate_angle_from_true_north(P_1, P_2)) \ No newline at end of file From 9ca084b2d0d630a9d3ffe33a947d3657b60cd7b5 Mon Sep 17 00:00:00 2001 From: Rahul Date: Tue, 31 Oct 2023 15:48:02 -0400 Subject: [PATCH 13/81] driver testing; not fully working :alien: --- config/esw.yaml | 8 +-- launch/rtk.launch | 5 +- src/localization/basestation_gps_driver.py | 2 +- src/localization/gps_config_plots.py | 8 +++ src/localization/gps_driver.py | 64 ++++++++++++++-------- 5 files changed, 55 insertions(+), 32 deletions(-) create mode 100644 src/localization/gps_config_plots.py mode change 100644 => 100755 src/localization/gps_driver.py diff --git a/config/esw.yaml b/config/esw.yaml index b30ff9649..df0c19ecd 100644 --- a/config/esw.yaml +++ b/config/esw.yaml @@ -9,13 +9,13 @@ gps_driver: frame_id: "base_link" rover_gps_driver: - port: "TODO" - baud: 115200 + port: "/dev/ttyACM1" + baud: 38400 frame_id: "base_link" basestation_gps_driver: - port: "/dev/ttyACM1" - baud: 115200 + port: "/dev/ttyACM0" + baud: 38400 frame_id: "base_link" imu_driver: diff --git a/launch/rtk.launch b/launch/rtk.launch index 30ddef06c..cdbaf69e4 100644 --- a/launch/rtk.launch +++ b/launch/rtk.launch @@ -4,8 +4,5 @@ - - - - + diff --git a/src/localization/basestation_gps_driver.py b/src/localization/basestation_gps_driver.py index 37646dab3..c5414ff38 100755 --- a/src/localization/basestation_gps_driver.py +++ b/src/localization/basestation_gps_driver.py @@ -4,7 +4,7 @@ from pyubx2 import UBXReader, UBX_PROTOCOL, RTCM3_PROTOCOL, protocol from rtcm_msgs.msg import Message -def main(): +def main() -> None: rtcm_pub = rospy.Publisher('rtcm', Message, queue_size=1) rospy.init_node('basestation_driver') port = rospy.get_param("basestation_gps_driver/port") diff --git a/src/localization/gps_config_plots.py b/src/localization/gps_config_plots.py new file mode 100644 index 000000000..83c0731ea --- /dev/null +++ b/src/localization/gps_config_plots.py @@ -0,0 +1,8 @@ +#!/usr/bin/env python3 + +import rosbag + +if __name__ == "__main__": + bag = rosbag.Bag('base.bag') + for (topic,msg,t) in bag.read_messages(): + print (topic,msg,t) diff --git a/src/localization/gps_driver.py b/src/localization/gps_driver.py old mode 100644 new mode 100755 index a625484b4..92c8c4f8e --- a/src/localization/gps_driver.py +++ b/src/localization/gps_driver.py @@ -1,3 +1,4 @@ +#!/usr/bin/env python3 ''' Reads and parses NMEA messages from the onboard GPS to provide location data to the rover over LCM (/gps). Subscribes to @@ -10,7 +11,7 @@ import threading import numpy as np from os import getenv -from rover_msgs import GPS, RTCM +# from rover_msgs import GPS, RTCM from pyubx2 import UBXReader, UBX_PROTOCOL, RTCM3_PROTOCOL, protocol from rtcm_msgs.msg import Message @@ -21,45 +22,62 @@ def __init__(self, port, baudrate): rospy.init_node('gps_driver') self.port = rospy.get_param("rover_gps_driver/port") self.baud = rospy.get_param("rover_gps_driver/baud") - self.base_station_sub = rospy.Subscriber('/rtcm', RTCM, process_rtcm) - self.gps_pub = rospy.Publisher('/gps', NavSatFix, queue_size=1) + self.base_station_sub = rospy.Subscriber('/rtcm', Message, self.process_rtcm) + # self.gps_pub = rospy.Publisher('/gps', NavSatFix, queue_size=1) self.lock = threading.Lock() + def connect(self): #open connection to rover GPS - self.ser = serial.Serial(self.port, self.baudrate) + self.ser = serial.Serial(self.port, self.baud) + self.reader = UBXReader(self.ser, protfilter=(UBX_PROTOCOL | RTCM3_PROTOCOL)) + return self - def exit(self): + def exit(self) -> None: #close connection self.ser.close() - def process_rtcm(self, data): - self.lock.acquire() - rtcm_data = RTCM.decode(data) - self.ser.write(rtcm_data) - self.lock.release() + def process_rtcm(self, data) -> None: + print("hello") + with self.lock: + # rtcm_data = RTCM.decode(data) + self.ser.write(data.message) + + def parse_rover_gps_data(self, rover_gps_data) -> None: + msg = rover_gps_data + # skip if message could not be parsed + if not msg: + return + + print("hello") + + if rover_gps_data.identity == "RXM-RTCM": + rospy.loginfo(vars(rover_gps_data)) + + if rover_gps_data.identity == "NAV-PVT": + rospy.loginfo(vars(rover_gps_data)) - def parse_rover_gps_data(self, rover_gps_data): - pass - def gps_data_thread(self): + def gps_data_thread(self) -> None: + #TODO: add more message checks if needed while not rospy.is_shutdown(): - self.lock.acquire() - reader = UBXReader(ser, protfilter=(UBX_PROTOCOL | RTCM3_PROTOCOL)) - # rover_gps_data = self.ser.readline() - raw, rover_gps_data = reader.read() - parsed_gps_data = self.parse_rover_gps_data(rover_gps_data) - self.gps_pub.publish(parsed_gps_data) - self.lock.release() + print() + # with self.lock: + # if self.ser.in_waiting: + # raw, rover_gps_data = self.reader.read() + # parsed_gps_data = self.parse_rover_gps_data(rover_gps_data) def main(): + rospy.logerr("rover gps driver") #change values - rtk_manager = GPS_Driver("port", baudrate=1500) + rtk_manager = GPS_Driver("port", baudrate=38400) rtk_manager.connect() - rover_gps_thread = threading.Thread(rtk_manager.gps_data_thread) - rover_gps_thread.start() + #rover_gps_thread = threading.Thread(rtk_manager.gps_data_thread) + #rover_gps_thread.start() + rtk_manager.gps_data_thread() + rtk_manager.exit() if __name__ == "__main__": From 285d1b671ef945de98407752650ad4155951cfa6 Mon Sep 17 00:00:00 2001 From: dllliu Date: Tue, 31 Oct 2023 19:47:11 -0400 Subject: [PATCH 14/81] plots for heading calculations --- src/localization/dual_gnss.py | 51 ++++++++++++++++++++++------------- 1 file changed, 33 insertions(+), 18 deletions(-) diff --git a/src/localization/dual_gnss.py b/src/localization/dual_gnss.py index b2759a3d2..44ddfaa2d 100644 --- a/src/localization/dual_gnss.py +++ b/src/localization/dual_gnss.py @@ -2,6 +2,7 @@ import matplotlib.pyplot as plt import math + def get_heading(antenna_point_A, antenna_point_B): latitude_A = np.radians(antenna_point_A[0]) latitude_B = np.radians(antenna_point_B[0]) @@ -16,10 +17,12 @@ def get_heading(antenna_point_A, antenna_point_B): bearing = np.arctan2(x, y) return bearing + test_bearing = get_heading(np.array([39.099912, -94.581213]), np.array([38.627089, -90.200203])) print(np.degrees(test_bearing)) -#reference point should be close + +# reference point should be close, needs to be specified based on location def spherical_to_cartesian(spherical_coord: np.ndarray, reference_coord: np.ndarray) -> np.ndarray: r = 6371000 x = r * (np.radians(spherical_coord[1]) - np.radians(reference_coord[1])) * np.cos(np.radians(reference_coord[0])) @@ -27,41 +30,53 @@ def spherical_to_cartesian(spherical_coord: np.ndarray, reference_coord: np.ndar z = 0 return np.array([x, y, z]) + def plot_vectors(P_1, P_2): result = P_1 - P_2 - plt.scatter([P_1[0], P_2[0], result[0]], [P_1[1], P_2[1], result[1]]) - plt.plot( - [P_1[0], P_2[0]], - [ - P_1[1], - P_2[1], - ], - ) + slope_resultant = (P_2[1] - P_1[1]) / (P_2[0] - P_1[0]) + midpoint = np.array([((P_2[0] + P_1[0]) / 2), ((P_2[1] + P_1[1]) / 2)]) + slope_pd = -1 / slope_resultant + + plt.scatter([P_1[0], P_2[0]], [P_1[1], P_2[1]]) + x_intercept = (-midpoint[1] / slope_pd) + midpoint[0] + x_pd = np.linspace(midpoint[0], x_intercept, 300) + y_pd = slope_pd * (x_pd - midpoint[0]) + midpoint[1] + plt.plot([P_1[0], P_2[0]], [P_1[1], P_2[1]]) + plt.plot(x_pd, y_pd) + + plt.axvline(x=midpoint[0], color="b") + + angle = np.degrees(np.arctan((midpoint[0] - x_intercept) / midpoint[1])) + print(angle) + # plt.plot([0, 1, 0]) plt.xlabel("X-axis") plt.ylabel("Y-axis") plt.grid(True) plt.show() + P_1 = spherical_to_cartesian(np.array([39.099912, -94.581213]), np.array([42.293195, -83.7096706])) P_2 = spherical_to_cartesian(np.array([38.627089, -90.200203]), np.array([42.293195, -83.7096706])) -# result = P_1 - P_2 -# plot_vectors(P_1, P_2) +result = P_1 - P_2 +plot_vectors(P_1, P_2) + def calculate_angle_from_true_north(P_1, P_2): - result = P_1 - P_2 - print(result) - true_north = (0, 1, 0) + result = P_1 - P_2 # this should be changed to be the perpendicular vector + # print(result) + true_north = (0, 1, 0) # not sure if this true north vector works # Calculate the dot product between the resultant vector and the true north vector - dot_product = result[0] * true_north[0] + result[1] * true_north[1] + dot_product = result[0] * true_north[0] + result[1] * true_north[1] # Calculate the magnitudes of both vectors - magnitude_result = math.sqrt(result[0]**2 + result[1]**2) - magnitude_north = math.sqrt(true_north[0]**2 + true_north[1]**2 + true_north[2]**2) + magnitude_result = math.sqrt(result[0] ** 2 + result[1] ** 2) + magnitude_north = math.sqrt(true_north[0] ** 2 + true_north[1] ** 2 + true_north[2] ** 2) angle_radians = math.acos(dot_product / (magnitude_result * magnitude_north)) angle = math.degrees(angle_radians) return angle -print(calculate_angle_from_true_north(P_1, P_2)) \ No newline at end of file + +# print(calculate_angle_from_true_north(P_1, P_2)) From 4b84aed3532822bc2e9d697962c459f0f3e78b68 Mon Sep 17 00:00:00 2001 From: Pearl Lin Date: Thu, 2 Nov 2023 23:04:37 +0000 Subject: [PATCH 15/81] added get_heading_vector function --- src/localization/dual_gnss.py | 44 ++++++++++++++++++++++++----------- 1 file changed, 31 insertions(+), 13 deletions(-) diff --git a/src/localization/dual_gnss.py b/src/localization/dual_gnss.py index 44ddfaa2d..14e44b250 100644 --- a/src/localization/dual_gnss.py +++ b/src/localization/dual_gnss.py @@ -3,23 +3,23 @@ import math -def get_heading(antenna_point_A, antenna_point_B): - latitude_A = np.radians(antenna_point_A[0]) - latitude_B = np.radians(antenna_point_B[0]) - longitude_A = np.radians(antenna_point_A[1]) - longitude_B = np.radians(antenna_point_B[1]) +# def get_heading(antenna_point_A, antenna_point_B): +# latitude_A = np.radians(antenna_point_A[0]) +# latitude_B = np.radians(antenna_point_B[0]) +# longitude_A = np.radians(antenna_point_A[1]) +# longitude_B = np.radians(antenna_point_B[1]) - x = np.cos(latitude_B) * np.sin(longitude_B - longitude_A) - y = np.cos(latitude_A) * np.sin(latitude_B) - np.sin(latitude_A) * np.cos(latitude_B) * np.cos( - longitude_B - longitude_A - ) +# x = np.cos(latitude_B) * np.sin(longitude_B - longitude_A) +# y = np.cos(latitude_A) * np.sin(latitude_B) - np.sin(latitude_A) * np.cos(latitude_B) * np.cos( +# longitude_B - longitude_A +# ) - bearing = np.arctan2(x, y) - return bearing +# bearing = np.arctan2(x, y) +# return bearing -test_bearing = get_heading(np.array([39.099912, -94.581213]), np.array([38.627089, -90.200203])) -print(np.degrees(test_bearing)) +# test_bearing = get_heading(np.array([39.099912, -94.581213]), np.array([38.627089, -90.200203])) +# print(np.degrees(test_bearing)) # reference point should be close, needs to be specified based on location @@ -30,6 +30,20 @@ def spherical_to_cartesian(spherical_coord: np.ndarray, reference_coord: np.ndar z = 0 return np.array([x, y, z]) +def get_heading_vector_from_angle(heading_angle): + x = np.sin(heading_angle) + y = np.cos(heading_angle) + return np.array([x, y]) + +def get_heading_vector(point_L: np.array, point_R: np.array): + vector_connecting = np.array([point_R[0] - point_L[0], point_R[1] - point_L[1], 0]) + vector_perp = np.zeros_like(vector_connecting) + vector_perp[0] = -vector_connecting[1] + vector_perp[1] = vector_connecting[0] + print(vector_perp) + return vector_perp / np.linalg.norm(vector_perp) + + def plot_vectors(P_1, P_2): result = P_1 - P_2 @@ -80,3 +94,7 @@ def calculate_angle_from_true_north(P_1, P_2): # print(calculate_angle_from_true_north(P_1, P_2)) +# print(get_heading_vector_from_angle((3 * np.pi) / 2)) +# print(get_heading_vector_from_angle(np.pi / 4)) + +print(get_heading_vector(np.array([0, 0]), np.array([3, 3]))) From 325792d1a01591d9c1028d4e5aa14cb346fd8f39 Mon Sep 17 00:00:00 2001 From: dllliu Date: Thu, 2 Nov 2023 21:33:06 -0400 Subject: [PATCH 16/81] add tests for vector heading method --- src/localization/dual_gnss.py | 57 +++++++++++++---------------------- 1 file changed, 21 insertions(+), 36 deletions(-) diff --git a/src/localization/dual_gnss.py b/src/localization/dual_gnss.py index 14e44b250..8eb8b7754 100644 --- a/src/localization/dual_gnss.py +++ b/src/localization/dual_gnss.py @@ -1,6 +1,5 @@ import numpy as np import matplotlib.pyplot as plt -import math # def get_heading(antenna_point_A, antenna_point_B): @@ -36,15 +35,13 @@ def get_heading_vector_from_angle(heading_angle): return np.array([x, y]) def get_heading_vector(point_L: np.array, point_R: np.array): - vector_connecting = np.array([point_R[0] - point_L[0], point_R[1] - point_L[1], 0]) + vector_connecting = np.array([point_R[0] - point_L[0], point_R[1] - point_L[1]]) vector_perp = np.zeros_like(vector_connecting) vector_perp[0] = -vector_connecting[1] vector_perp[1] = vector_connecting[0] - print(vector_perp) + #print(vector_perp) return vector_perp / np.linalg.norm(vector_perp) - - def plot_vectors(P_1, P_2): result = P_1 - P_2 slope_resultant = (P_2[1] - P_1[1]) / (P_2[0] - P_1[0]) @@ -62,39 +59,27 @@ def plot_vectors(P_1, P_2): plt.axvline(x=midpoint[0], color="b") angle = np.degrees(np.arctan((midpoint[0] - x_intercept) / midpoint[1])) - print(angle) - # plt.plot([0, 1, 0]) plt.xlabel("X-axis") plt.ylabel("Y-axis") plt.grid(True) plt.show() - -P_1 = spherical_to_cartesian(np.array([39.099912, -94.581213]), np.array([42.293195, -83.7096706])) -P_2 = spherical_to_cartesian(np.array([38.627089, -90.200203]), np.array([42.293195, -83.7096706])) -result = P_1 - P_2 -plot_vectors(P_1, P_2) - - -def calculate_angle_from_true_north(P_1, P_2): - result = P_1 - P_2 # this should be changed to be the perpendicular vector - # print(result) - true_north = (0, 1, 0) # not sure if this true north vector works - # Calculate the dot product between the resultant vector and the true north vector - dot_product = result[0] * true_north[0] + result[1] * true_north[1] - - # Calculate the magnitudes of both vectors - magnitude_result = math.sqrt(result[0] ** 2 + result[1] ** 2) - magnitude_north = math.sqrt(true_north[0] ** 2 + true_north[1] ** 2 + true_north[2] ** 2) - - angle_radians = math.acos(dot_product / (magnitude_result * magnitude_north)) - angle = math.degrees(angle_radians) - - return angle - - -# print(calculate_angle_from_true_north(P_1, P_2)) -# print(get_heading_vector_from_angle((3 * np.pi) / 2)) -# print(get_heading_vector_from_angle(np.pi / 4)) - -print(get_heading_vector(np.array([0, 0]), np.array([3, 3]))) +def offset_lat_long(P_1, P_2,n): + return P_1 + n * 0.001, P_2 + n * 0.001 + +P_1 = np.array([42.3006, -83.71006]) +P_2 = np.array([42.30061, -83.71006]) +ref = np.array([42.293195, -83.7096706]) + +for i in range(4000,4100): + new_p1, new_p2 = offset_lat_long(P_1, P_2, i) + new_p1 = spherical_to_cartesian(new_p1, ref) + new_p2 = spherical_to_cartesian(new_p2, ref) + print("P1") + print(new_p1) + print("P2") + print(new_p2) + print("heading") + print(get_heading_vector(new_p1, new_p2)) + +#print(get_heading_vector(np.array([0, 0]), np.array([3, 3]))) \ No newline at end of file From 42533bad05f85cb3ba1364b03d99d32da6ba7cd4 Mon Sep 17 00:00:00 2001 From: Rahul Date: Thu, 9 Nov 2023 15:52:54 -0500 Subject: [PATCH 17/81] working as intended furing testing --- config/esw.yaml | 4 ++-- src/localization/gps_driver.py | 31 ++++++++++++++++++++++--------- 2 files changed, 24 insertions(+), 11 deletions(-) diff --git a/config/esw.yaml b/config/esw.yaml index df0c19ecd..390d89f2b 100644 --- a/config/esw.yaml +++ b/config/esw.yaml @@ -9,12 +9,12 @@ gps_driver: frame_id: "base_link" rover_gps_driver: - port: "/dev/ttyACM1" + port: "/dev/ttyACM2" baud: 38400 frame_id: "base_link" basestation_gps_driver: - port: "/dev/ttyACM0" + port: "/dev/ttyACM1" baud: 38400 frame_id: "base_link" diff --git a/src/localization/gps_driver.py b/src/localization/gps_driver.py index 92c8c4f8e..4e17323b1 100755 --- a/src/localization/gps_driver.py +++ b/src/localization/gps_driver.py @@ -39,7 +39,7 @@ def exit(self) -> None: self.ser.close() def process_rtcm(self, data) -> None: - print("hello") + print("processing RTCM") with self.lock: # rtcm_data = RTCM.decode(data) self.ser.write(data.message) @@ -50,23 +50,36 @@ def parse_rover_gps_data(self, rover_gps_data) -> None: if not msg: return - print("hello") - - if rover_gps_data.identity == "RXM-RTCM": + try: + print(msg.identity) rospy.loginfo(vars(rover_gps_data)) + + except: + pass + + + print("parsing GPS") + + if rover_gps_data.identity == "RXM-RTCM": + print("RXM") + # rospy.loginfo(vars(rover_gps_data)) if rover_gps_data.identity == "NAV-PVT": + print("PVT") rospy.loginfo(vars(rover_gps_data)) + if rover_gps_data.identity == "NAV-STATUS": + print("NAV STATUS") + + def gps_data_thread(self) -> None: #TODO: add more message checks if needed while not rospy.is_shutdown(): - print() - # with self.lock: - # if self.ser.in_waiting: - # raw, rover_gps_data = self.reader.read() - # parsed_gps_data = self.parse_rover_gps_data(rover_gps_data) + with self.lock: + if self.ser.in_waiting: + raw, rover_gps_data = self.reader.read() + parsed_gps_data = self.parse_rover_gps_data(rover_gps_data) def main(): From 280ef721de5f92bd2007e506482e3fb77dcf5ae7 Mon Sep 17 00:00:00 2001 From: dllliu Date: Thu, 9 Nov 2023 19:11:15 -0500 Subject: [PATCH 18/81] add tests to determine optimal amt of sig figs for gps coordinates --- src/localization/dual_gnss.py | 54 ++++++++++++++++++++++++++--------- 1 file changed, 41 insertions(+), 13 deletions(-) diff --git a/src/localization/dual_gnss.py b/src/localization/dual_gnss.py index 8eb8b7754..596f1e622 100644 --- a/src/localization/dual_gnss.py +++ b/src/localization/dual_gnss.py @@ -29,19 +29,22 @@ def spherical_to_cartesian(spherical_coord: np.ndarray, reference_coord: np.ndar z = 0 return np.array([x, y, z]) + def get_heading_vector_from_angle(heading_angle): x = np.sin(heading_angle) y = np.cos(heading_angle) return np.array([x, y]) + def get_heading_vector(point_L: np.array, point_R: np.array): vector_connecting = np.array([point_R[0] - point_L[0], point_R[1] - point_L[1]]) vector_perp = np.zeros_like(vector_connecting) vector_perp[0] = -vector_connecting[1] vector_perp[1] = vector_connecting[0] - #print(vector_perp) + # print(vector_perp) return vector_perp / np.linalg.norm(vector_perp) + def plot_vectors(P_1, P_2): result = P_1 - P_2 slope_resultant = (P_2[1] - P_1[1]) / (P_2[0] - P_1[0]) @@ -64,22 +67,47 @@ def plot_vectors(P_1, P_2): plt.grid(True) plt.show() -def offset_lat_long(P_1, P_2,n): - return P_1 + n * 0.001, P_2 + n * 0.001 -P_1 = np.array([42.3006, -83.71006]) -P_2 = np.array([42.30061, -83.71006]) +def offset_lat_left(P_1, P_2, n): + return P_1 - n * 0.0000001, P_2 + + +def offset_lat_right(P_1, P_2, n): + return P_1 + n * 0.0000001, P_2 + + +def offset_long_left(P_1, P_2, n): + return P_1, P_2 - n * 0.0000001 + + +def offset_long_right(P_1, P_2, n): + return P_1, P_2 + n * 0.0000001 + + +# [1,0], [-1,0] if switched latitude 5th digit +# P_1 = np.array([42.30061, -83.71006]) +# P_2 = np.array([42.30060, -83.71006]) + +# [0,-1], [0,1] if switched longitude 5th digit +P_1 = np.array([42.300611, -83.710071]) +P_2 = np.array([42.300611, -83.710083]) ref = np.array([42.293195, -83.7096706]) -for i in range(4000,4100): - new_p1, new_p2 = offset_lat_long(P_1, P_2, i) +for i in range(0, 170): # graph changes based on these bounds due to reference coordinate + new_p1, new_p2 = offset_lat_left(P_1, P_2, i) # optimal bound is 0.0000001 (1e^-7) new_p1 = spherical_to_cartesian(new_p1, ref) new_p2 = spherical_to_cartesian(new_p2, ref) - print("P1") - print(new_p1) - print("P2") - print(new_p2) + # print("P1") + # print(new_p1) + # print("P2") + # print(new_p2) print("heading") print(get_heading_vector(new_p1, new_p2)) - -#print(get_heading_vector(np.array([0, 0]), np.array([3, 3]))) \ No newline at end of file + test = get_heading_vector(new_p1, new_p2) + plt.scatter(test[0], test[1]) +plt.xlabel("heading x") +plt.ylabel("heading y") +plt.grid(True) +plt.show() + +# print(get_heading_vector(np.array([0, 0]), np.array([3, 3]))) From 0803e8b3ef2fbe8989f93b1b46bc7351483d71e2 Mon Sep 17 00:00:00 2001 From: Rahul Date: Sun, 12 Nov 2023 14:30:17 -0500 Subject: [PATCH 19/81] added more message parsing --- config/esw.yaml | 2 +- src/localization/gps_driver.py | 42 +++++++++++++++++++++++++++------- 2 files changed, 35 insertions(+), 9 deletions(-) diff --git a/config/esw.yaml b/config/esw.yaml index 390d89f2b..acaa9437b 100644 --- a/config/esw.yaml +++ b/config/esw.yaml @@ -9,7 +9,7 @@ gps_driver: frame_id: "base_link" rover_gps_driver: - port: "/dev/ttyACM2" + port: "/dev/ttyACM0" baud: 38400 frame_id: "base_link" diff --git a/src/localization/gps_driver.py b/src/localization/gps_driver.py index 4e17323b1..eeb32be9d 100755 --- a/src/localization/gps_driver.py +++ b/src/localization/gps_driver.py @@ -12,7 +12,9 @@ import numpy as np from os import getenv # from rover_msgs import GPS, RTCM -from pyubx2 import UBXReader, UBX_PROTOCOL, RTCM3_PROTOCOL, protocol +from pyubx2 import UBXReader, UBX_PROTOCOL, RTCM3_PROTOCOL, protocol, UBXMessage +from std_msgs.msg import Header +from sensor_msgs.msg import NavSatFix from rtcm_msgs.msg import Message @@ -23,7 +25,7 @@ def __init__(self, port, baudrate): self.port = rospy.get_param("rover_gps_driver/port") self.baud = rospy.get_param("rover_gps_driver/baud") self.base_station_sub = rospy.Subscriber('/rtcm', Message, self.process_rtcm) - # self.gps_pub = rospy.Publisher('/gps', NavSatFix, queue_size=1) + self.gps_pub = rospy.Publisher('/gps', NavSatFix, queue_size=1) self.lock = threading.Lock() @@ -52,24 +54,48 @@ def parse_rover_gps_data(self, rover_gps_data) -> None: try: print(msg.identity) - rospy.loginfo(vars(rover_gps_data)) except: pass - - - print("parsing GPS") if rover_gps_data.identity == "RXM-RTCM": print("RXM") + msg_used = msg.msgUsed + + if msg_used == 0: + print("RTCM Usage unknown\n") + elif msg_used == 1: + print("RTCM message not used\n") + elif msg_used == 2: + print("RTCM message successfully used by receiver\n") + + + # rospy.loginfo(vars(rover_gps_data)) if rover_gps_data.identity == "NAV-PVT": print("PVT") - rospy.loginfo(vars(rover_gps_data)) + parsed_latitude = msg.lat + parsed_longitude = msg.lon + parsed_altitude = msg.hMSL + message_header = Header(stamp=rospy.Time.now(), frame_id="base_link") + + self.gps_pub.publish( + NavSatFix(header=message_header, latitude=parsed_latitude, longitude=parsed_longitude, altitude=parsed_altitude) + ) + + if msg.difSoln == 1: + print("Differemtial Corrections Applied") + + if msg.carrSoln == 0: + print("No RTK\n") + elif msg.carrSoln == 1: + print("Floating RTK Fix\n") + elif msg.carrSoln == 2: + print("RTK FIX\n") if rover_gps_data.identity == "NAV-STATUS": - print("NAV STATUS") + pass From ec73cc1f633774500061a042b8261b72cd402b05 Mon Sep 17 00:00:00 2001 From: Pearl Lin Date: Tue, 14 Nov 2023 23:16:30 +0000 Subject: [PATCH 20/81] started adding gps to gazebo --- urdf/rover/rover.urdf.xacro | 12 +++++++++++ urdf/rover/rover_gazebo_plugins.urdf.xacro | 25 +++++++++++++++++++--- 2 files changed, 34 insertions(+), 3 deletions(-) diff --git a/urdf/rover/rover.urdf.xacro b/urdf/rover/rover.urdf.xacro index 71512c19e..b9da576fa 100644 --- a/urdf/rover/rover.urdf.xacro +++ b/urdf/rover/rover.urdf.xacro @@ -36,6 +36,18 @@ + + + + + + + + + + + + diff --git a/urdf/rover/rover_gazebo_plugins.urdf.xacro b/urdf/rover/rover_gazebo_plugins.urdf.xacro index 5bcf12bd6..b926c089c 100644 --- a/urdf/rover/rover_gazebo_plugins.urdf.xacro +++ b/urdf/rover/rover_gazebo_plugins.urdf.xacro @@ -18,10 +18,10 @@ base_link - - + + 100.0 - base_link + rover_gps_left base_link gps/fix gps/fix_velocity @@ -35,8 +35,27 @@ 0.0 0.0 0.0 0.2 0.2 0.2 + + + + 100.0 + rover_gps_right + + gps/fix_velocity + + + + + + + + + + + + From 89b8f75912a0ddaa169e437014ea594fc671dad0 Mon Sep 17 00:00:00 2001 From: Pearl Lin Date: Tue, 14 Nov 2023 23:24:16 +0000 Subject: [PATCH 21/81] finished adding new gps and links --- urdf/rover/rover_gazebo_plugins.urdf.xacro | 22 ++++++++++++---------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/urdf/rover/rover_gazebo_plugins.urdf.xacro b/urdf/rover/rover_gazebo_plugins.urdf.xacro index b926c089c..41903f17a 100644 --- a/urdf/rover/rover_gazebo_plugins.urdf.xacro +++ b/urdf/rover/rover_gazebo_plugins.urdf.xacro @@ -22,8 +22,8 @@ 100.0 rover_gps_left - base_link - gps/fix + + gps/fix_gps_left gps/fix_velocity 42.293195 -83.7096706 @@ -41,15 +41,17 @@ 100.0 rover_gps_right + gps/fix_gps_right gps/fix_velocity - - - - - - - - + 42.293195 + -83.7096706 + 90.0 + 0.0 + 0 0 0 + 0.0 0.0 0.0 + 0.3 0.3 0.3 + 0.0 0.0 0.0 + 0.2 0.2 0.2 From b927d78181f0be93a5cdb99268b27b7a53a678d7 Mon Sep 17 00:00:00 2001 From: Pearl Lin Date: Tue, 14 Nov 2023 23:54:53 +0000 Subject: [PATCH 22/81] edited links --- src/localization/test_gps_sim.py | 5 ++++ urdf/rover/rover.urdf.xacro | 30 +++++++++++++--------- urdf/rover/rover_gazebo_plugins.urdf.xacro | 6 ++--- 3 files changed, 26 insertions(+), 15 deletions(-) create mode 100644 src/localization/test_gps_sim.py diff --git a/src/localization/test_gps_sim.py b/src/localization/test_gps_sim.py new file mode 100644 index 000000000..f721f79f4 --- /dev/null +++ b/src/localization/test_gps_sim.py @@ -0,0 +1,5 @@ +# import rospy + +# class Test_Sim: +# def __init__(self): + \ No newline at end of file diff --git a/urdf/rover/rover.urdf.xacro b/urdf/rover/rover.urdf.xacro index b9da576fa..c486baccb 100644 --- a/urdf/rover/rover.urdf.xacro +++ b/urdf/rover/rover.urdf.xacro @@ -36,18 +36,6 @@ - - - - - - - - - - - - @@ -149,6 +137,24 @@ + + + + + + + + + + + + + + + + + + diff --git a/urdf/rover/rover_gazebo_plugins.urdf.xacro b/urdf/rover/rover_gazebo_plugins.urdf.xacro index 41903f17a..9fc04817a 100644 --- a/urdf/rover/rover_gazebo_plugins.urdf.xacro +++ b/urdf/rover/rover_gazebo_plugins.urdf.xacro @@ -21,7 +21,7 @@ 100.0 - rover_gps_left + rover_gps_left_link gps/fix_gps_left gps/fix_velocity @@ -39,7 +39,7 @@ 100.0 - rover_gps_right + rover_gps_right_link gps/fix_gps_right gps/fix_velocity @@ -48,7 +48,7 @@ 90.0 0.0 0 0 0 - 0.0 0.0 0.0 + 0.0 0.0 0.0 0.3 0.3 0.3 0.0 0.0 0.0 0.2 0.2 0.2 From a271f7fc3163a7e8035a7c355eba4a3eebc873d5 Mon Sep 17 00:00:00 2001 From: Pearl Lin Date: Wed, 15 Nov 2023 00:53:44 +0000 Subject: [PATCH 23/81] added links --- launch/rover_core.launch | 2 +- urdf/rover/rover.urdf.xacro | 10 ++++------ 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/launch/rover_core.launch b/launch/rover_core.launch index 234de4e60..47a65ea80 100644 --- a/launch/rover_core.launch +++ b/launch/rover_core.launch @@ -4,7 +4,7 @@ - + \ No newline at end of file diff --git a/urdf/rover/rover.urdf.xacro b/urdf/rover/rover.urdf.xacro index c486baccb..c9b1f1b6e 100644 --- a/urdf/rover/rover.urdf.xacro +++ b/urdf/rover/rover.urdf.xacro @@ -139,22 +139,20 @@ + - + - - + - + - - From a2b3a7bfa87576a80905012cb997c9f588de44b4 Mon Sep 17 00:00:00 2001 From: dllliu Date: Thu, 16 Nov 2023 20:01:34 -0500 Subject: [PATCH 24/81] try to debug plugins and make test subscriber file --- launch/auton.launch | 39 ++--- launch/auton_sim.launch | 21 +-- src/localization/Test-Subscriber.py | 53 +++++++ urdf/rover/rover.urdf.xacro | 168 +++++++++++---------- urdf/rover/rover_gazebo_plugins.urdf.xacro | 7 +- 5 files changed, 175 insertions(+), 113 deletions(-) create mode 100755 src/localization/Test-Subscriber.py diff --git a/launch/auton.launch b/launch/auton.launch index 763657241..43be78198 100644 --- a/launch/auton.launch +++ b/launch/auton.launch @@ -1,9 +1,9 @@ - - - - + + + + + args="manager" output="screen" /> + pkg="nodelet" type="nodelet" name="tag_detector" respawn="true" + args="load mrover/TagDetectorNodelet perception_nodelet_manager" output="screen" /> - - - + + + - - - + + + - + clear_params="true" launch-prefix="bash -c 'sleep $(arg ekf_start_delay); $0 $@'"> + - + - - + + + + + \ No newline at end of file diff --git a/launch/auton_sim.launch b/launch/auton_sim.launch index 37cfaa6ea..4419a65df 100644 --- a/launch/auton_sim.launch +++ b/launch/auton_sim.launch @@ -2,26 +2,27 @@ This launch file launches everything necessary to run auton code in the simulator. --> - - + + - + + - + - + + args="0 0 0 0 0 0 1 left_camera_link zed2i_left_camera_frame 100" /> - - - + + + - + \ No newline at end of file diff --git a/src/localization/Test-Subscriber.py b/src/localization/Test-Subscriber.py new file mode 100755 index 000000000..a4ef7bb7e --- /dev/null +++ b/src/localization/Test-Subscriber.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python3 + +# python linear algebra library +import numpy as np + +# library for interacting with ROS and TF tree +import rospy +import tf2_ros +import math + +# ROS message types we need to use +from sensor_msgs.msg import NavSatFix, Imu + +# SE3 library for handling poses and TF tree +from util.SE3 import SE3 +from util.SO3 import SO3 + + +class Localization: + pose: SE3 + + def __init__(self): + # create subscribers for GPS and IMU data, linking them to our callback functions + # TODO + rospy.Subscriber("gps/fix_gps_left", NavSatFix, self.gps_left_callback) + rospy.Subscriber("gps/fix_gps_right", NavSatFix, self.gps_right_callback) + + # create a transform broadcaster for publishing to the TF tree + # self.tf_broadcaster = tf2_ros.TransformBroadcaster() + + # initialize pose to all zeros + # self.pose = SE3() + + def gps_left_callback(self, msg: NavSatFix): + print(msg) + + def gps_right_callback(self, msg: NavSatFix): + print(msg) + + +def main(): + # initialize the node + rospy.init_node("localization_test") + + # create and start our localization system + localization = Localization() + + # let the callback functions run asynchronously in the background + rospy.spin() + + +if __name__ == "__main__": + main() diff --git a/urdf/rover/rover.urdf.xacro b/urdf/rover/rover.urdf.xacro index c9b1f1b6e..238b357b3 100644 --- a/urdf/rover/rover.urdf.xacro +++ b/urdf/rover/rover.urdf.xacro @@ -1,210 +1,216 @@ - - - - - - + + + + + + - + - - + + - - + + - - + + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - + - - - - + + + + - - - - + + + + - - - - + + + + - - - + + + - - - + + + - + - - - - - + + + + + - - - - - + + + + + + + + - - - + + + - + - + - + - + - - - + + + - - - + + + - + - + - - - + + + - - - - + + + + \ No newline at end of file diff --git a/urdf/rover/rover_gazebo_plugins.urdf.xacro b/urdf/rover/rover_gazebo_plugins.urdf.xacro index 9fc04817a..e42a42b99 100644 --- a/urdf/rover/rover_gazebo_plugins.urdf.xacro +++ b/urdf/rover/rover_gazebo_plugins.urdf.xacro @@ -21,7 +21,7 @@ 100.0 - rover_gps_left_link + rover_gps_left gps/fix_gps_left gps/fix_velocity @@ -39,7 +39,7 @@ 100.0 - rover_gps_right_link + rover_gps_right gps/fix_gps_right gps/fix_velocity @@ -57,7 +57,6 @@ - @@ -139,4 +138,4 @@ - + \ No newline at end of file From 9965be83ea46c5f4e7c29dd894bccd90ddf97b0f Mon Sep 17 00:00:00 2001 From: Rahul Date: Sun, 26 Nov 2023 13:18:36 -0500 Subject: [PATCH 25/81] added left and right gps using groups --- config/esw.yaml | 8 +++++++- launch/rtk.launch | 12 +++++++++++- src/localization/gps_driver.py | 19 +++++++------------ 3 files changed, 25 insertions(+), 14 deletions(-) diff --git a/config/esw.yaml b/config/esw.yaml index acaa9437b..1a99df0c8 100644 --- a/config/esw.yaml +++ b/config/esw.yaml @@ -8,11 +8,17 @@ gps_driver: useRMC: false # get covariance instead of velocity, see wiki for more info frame_id: "base_link" -rover_gps_driver: +right_gps_driver: port: "/dev/ttyACM0" baud: 38400 frame_id: "base_link" +left_gps_driver: + port: "/dev/ttyACM0" + baud: 38400 + frame_id: "base_link" + + basestation_gps_driver: port: "/dev/ttyACM1" baud: 38400 diff --git a/launch/rtk.launch b/launch/rtk.launch index cdbaf69e4..e7a8a841a 100644 --- a/launch/rtk.launch +++ b/launch/rtk.launch @@ -4,5 +4,15 @@ - + + + + + + + + diff --git a/src/localization/gps_driver.py b/src/localization/gps_driver.py index eeb32be9d..72c43244c 100755 --- a/src/localization/gps_driver.py +++ b/src/localization/gps_driver.py @@ -20,10 +20,10 @@ class GPS_Driver(): - def __init__(self, port, baudrate): + def __init__(self): rospy.init_node('gps_driver') - self.port = rospy.get_param("rover_gps_driver/port") - self.baud = rospy.get_param("rover_gps_driver/baud") + self.port = rospy.get_param("port") + self.baud = rospy.get_param("baud") self.base_station_sub = rospy.Subscriber('/rtcm', Message, self.process_rtcm) self.gps_pub = rospy.Publisher('/gps', NavSatFix, queue_size=1) self.lock = threading.Lock() @@ -51,13 +51,7 @@ def parse_rover_gps_data(self, rover_gps_data) -> None: # skip if message could not be parsed if not msg: return - - try: - print(msg.identity) - - except: - pass - + if rover_gps_data.identity == "RXM-RTCM": print("RXM") msg_used = msg.msgUsed @@ -86,7 +80,9 @@ def parse_rover_gps_data(self, rover_gps_data) -> None: if msg.difSoln == 1: print("Differemtial Corrections Applied") + + # publidh to navsatstatus in navsatfix if msg.carrSoln == 0: print("No RTK\n") elif msg.carrSoln == 1: @@ -109,9 +105,8 @@ def gps_data_thread(self) -> None: def main(): - rospy.logerr("rover gps driver") #change values - rtk_manager = GPS_Driver("port", baudrate=38400) + rtk_manager = GPS_Driver() rtk_manager.connect() #rover_gps_thread = threading.Thread(rtk_manager.gps_data_thread) #rover_gps_thread.start() From 77c8f2bbfdc2f5fa690370eadeb60f0c1514a92f Mon Sep 17 00:00:00 2001 From: dllliu Date: Tue, 28 Nov 2023 18:56:11 -0500 Subject: [PATCH 26/81] gps creation and subscribers --- src/localization/Test-Subscriber.py | 2 ++ src/localization/test_gps_sim.py | 5 ----- urdf/rover/rover.urdf.xacro | 3 ++- urdf/rover/rover_gazebo_plugins.urdf.xacro | 12 ++++++------ 4 files changed, 10 insertions(+), 12 deletions(-) delete mode 100644 src/localization/test_gps_sim.py diff --git a/src/localization/Test-Subscriber.py b/src/localization/Test-Subscriber.py index a4ef7bb7e..1edbc8498 100755 --- a/src/localization/Test-Subscriber.py +++ b/src/localization/Test-Subscriber.py @@ -32,9 +32,11 @@ def __init__(self): # self.pose = SE3() def gps_left_callback(self, msg: NavSatFix): + print("GPS left callback") print(msg) def gps_right_callback(self, msg: NavSatFix): + print("GPS right callback") print(msg) diff --git a/src/localization/test_gps_sim.py b/src/localization/test_gps_sim.py deleted file mode 100644 index f721f79f4..000000000 --- a/src/localization/test_gps_sim.py +++ /dev/null @@ -1,5 +0,0 @@ -# import rospy - -# class Test_Sim: -# def __init__(self): - \ No newline at end of file diff --git a/urdf/rover/rover.urdf.xacro b/urdf/rover/rover.urdf.xacro index 238b357b3..6427bb41a 100644 --- a/urdf/rover/rover.urdf.xacro +++ b/urdf/rover/rover.urdf.xacro @@ -157,7 +157,8 @@ - + + diff --git a/urdf/rover/rover_gazebo_plugins.urdf.xacro b/urdf/rover/rover_gazebo_plugins.urdf.xacro index e42a42b99..8a9031c5c 100644 --- a/urdf/rover/rover_gazebo_plugins.urdf.xacro +++ b/urdf/rover/rover_gazebo_plugins.urdf.xacro @@ -19,10 +19,10 @@ - + 100.0 - rover_gps_left - + base_link + base_link gps/fix_gps_left gps/fix_velocity 42.293195 @@ -37,10 +37,10 @@ - + 100.0 - rover_gps_right - + base_link + base_link gps/fix_gps_right gps/fix_velocity 42.293195 From 1348cd6c3a2d3f392db1b9a37a0248267d3fb539 Mon Sep 17 00:00:00 2001 From: dllliu Date: Tue, 28 Nov 2023 19:57:15 -0500 Subject: [PATCH 27/81] add tests for moving gps and subscribed --- src/localization/Test-Subscriber.py | 14 +++++++++--- src/localization/gps_linearization.py | 4 ++-- urdf/rover/rover_gazebo_plugins.urdf.xacro | 26 ++++++++++++++++++---- 3 files changed, 35 insertions(+), 9 deletions(-) diff --git a/src/localization/Test-Subscriber.py b/src/localization/Test-Subscriber.py index 1edbc8498..a7e7cc14b 100755 --- a/src/localization/Test-Subscriber.py +++ b/src/localization/Test-Subscriber.py @@ -14,13 +14,18 @@ # SE3 library for handling poses and TF tree from util.SE3 import SE3 from util.SO3 import SO3 +from matplotlib import pyplot as plt + + +lat_arr = [] +long_arr = [] class Localization: pose: SE3 def __init__(self): - # create subscribers for GPS and IMU data, linking them to our callback functions + # create subscribers for GPS data, linking them to our callback functions # TODO rospy.Subscriber("gps/fix_gps_left", NavSatFix, self.gps_left_callback) rospy.Subscriber("gps/fix_gps_right", NavSatFix, self.gps_right_callback) @@ -32,12 +37,13 @@ def __init__(self): # self.pose = SE3() def gps_left_callback(self, msg: NavSatFix): - print("GPS left callback") print(msg) + lat_arr.append(msg.latitude) + long_arr.append(msg.longitude) def gps_right_callback(self, msg: NavSatFix): - print("GPS right callback") print(msg) + pass def main(): @@ -53,3 +59,5 @@ def main(): if __name__ == "__main__": main() + plt.scatter(lat_arr, long_arr) + plt.show() diff --git a/src/localization/gps_linearization.py b/src/localization/gps_linearization.py index 983e84156..5d1d991a5 100755 --- a/src/localization/gps_linearization.py +++ b/src/localization/gps_linearization.py @@ -45,8 +45,8 @@ def __init__(self): self.last_gps_msg = None self.last_imu_msg = None - - rospy.Subscriber("gps/fix", NavSatFix, self.gps_callback) + # gps/fix + rospy.Subscriber("gps/fix_gps_left", NavSatFix, self.gps_callback) rospy.Subscriber("imu/data", ImuAndMag, self.imu_callback) self.pose_publisher = rospy.Publisher("linearized_pose", PoseWithCovarianceStamped, queue_size=1) diff --git a/urdf/rover/rover_gazebo_plugins.urdf.xacro b/urdf/rover/rover_gazebo_plugins.urdf.xacro index 8a9031c5c..162dcb9ff 100644 --- a/urdf/rover/rover_gazebo_plugins.urdf.xacro +++ b/urdf/rover/rover_gazebo_plugins.urdf.xacro @@ -18,12 +18,12 @@ base_link - + 100.0 base_link base_link - gps/fix_gps_left + gps/fix gps/fix_velocity 42.293195 -83.7096706 @@ -36,6 +36,24 @@ 0.2 0.2 0.2 + + + 100.0 + base_link + base_link + gps/fix_gps_left + gps/fix_velocity + 42.293195 + -83.7096706 + 90.0 + 0.0 + 0 0.5 0 + 0.0 0.0 0.0 + 0 0 0 + 0.0 0.0 0.0 + 0.2 0.2 0.2 + + 100.0 @@ -47,9 +65,9 @@ -83.7096706 90.0 0.0 - 0 0 0 + 0 -0.5 0 0.0 0.0 0.0 - 0.3 0.3 0.3 + 0 0 0 0.0 0.0 0.0 0.2 0.2 0.2 From 1fa6b18023bb6c55067155bb21fef73b877deaa0 Mon Sep 17 00:00:00 2001 From: Rahul Date: Thu, 30 Nov 2023 18:29:53 -0500 Subject: [PATCH 28/81] started editing gps linearization --- src/localization/gps_driver.py | 2 +- src/localization/gps_linearization.py | 101 +++++++++++++++----------- 2 files changed, 59 insertions(+), 44 deletions(-) diff --git a/src/localization/gps_driver.py b/src/localization/gps_driver.py index 72c43244c..029ac96cb 100755 --- a/src/localization/gps_driver.py +++ b/src/localization/gps_driver.py @@ -25,7 +25,7 @@ def __init__(self): self.port = rospy.get_param("port") self.baud = rospy.get_param("baud") self.base_station_sub = rospy.Subscriber('/rtcm', Message, self.process_rtcm) - self.gps_pub = rospy.Publisher('/gps', NavSatFix, queue_size=1) + self.gps_pub = rospy.Publisher('fix', NavSatFix, queue_size=1) self.lock = threading.Lock() diff --git a/src/localization/gps_linearization.py b/src/localization/gps_linearization.py index 983e84156..a895266f2 100755 --- a/src/localization/gps_linearization.py +++ b/src/localization/gps_linearization.py @@ -10,6 +10,7 @@ from std_msgs.msg import Header from util.SE3 import SE3 from util.np_utils import numpify +import message_filters class GPSLinearization: @@ -46,27 +47,38 @@ def __init__(self): self.last_gps_msg = None self.last_imu_msg = None - rospy.Subscriber("gps/fix", NavSatFix, self.gps_callback) + right_gps_sub = message_filters.Subscriber("right_gps_driver/fix", NavSatFix) + left_gps_sub = message_filters.Subscriber("left_gps_driver/fix", NavSatFix) + + sync_gps_sub = message_filters.ApproximateTimeSynchronizer([right_gps_sub, left_gps_sub], 10, 0.5) + sync_gps_sub.registerCallback(self.gps_callback) + + rospy.Subscriber("imu/data", ImuAndMag, self.imu_callback) self.pose_publisher = rospy.Publisher("linearized_pose", PoseWithCovarianceStamped, queue_size=1) - def gps_callback(self, msg: NavSatFix): + def gps_callback(self, right_gps_msg: NavSatFix, left_gps_msg: NavSatFix): """ Callback function that receives GPS messages, assigns their covariance matrix, and then publishes the linearized pose. :param msg: The NavSatFix message containing GPS data that was just received + TODO: Handle invalid PVTs """ - if np.any(np.isnan([msg.latitude, msg.longitude, msg.altitude])): + if np.any(np.isnan([right_gps_msg.latitude, right_gps_msg.longitude, right_gps_msg.altitude])): + return + if np.any(np.isnan([left_gps_msg.latitude, left_gps_msg.longitude, left_gps_msg.altitude])): return + + ref_coord = np.array([self.ref_lat, self.ref_lon, self.ref_alt]) + + right_cartesian = np.array(geodetic2enu(right_gps_msg.latitude, right_gps_msg.longitude, right_gps_msg.altitude, *ref_coord, deg=True)) + left_cartesian = np.array(geodetic2enu(left_gps_msg.latitude, left_gps_msg.longitude, left_gps_msg.altitude, *ref_coord, deg=True)) - if not self.use_dop_covariance: - msg.position_covariance = self.config_gps_covariance + self.compute_gps_bearing(right_cartesian=right_cartesian, left_cartesian=left_cartesian) - self.last_gps_msg = msg + #todo compute pose and publish to EKF topic - if self.last_imu_msg is not None: - self.publish_pose() def imu_callback(self, msg: ImuAndMag): """ @@ -79,41 +91,41 @@ def imu_callback(self, msg: ImuAndMag): if self.last_gps_msg is not None: self.publish_pose() - @staticmethod - def get_linearized_pose_in_world( - gps_msg: NavSatFix, imu_msg: ImuAndMag, ref_coord: np.ndarray - ) -> Tuple[SE3, np.ndarray]: - """ - Linearizes the GPS geodetic coordinates into ENU cartesian coordinates, - then combines them with the IMU orientation into a pose estimate relative - to the world frame, with corresponding covariance matrix. - - :param gps_msg: Message containing the rover's GPS coordinates and their corresponding - covariance matrix. - :param imu_msg: Message containing the rover's orientation from IMU, with - corresponding covariance matrix. - :param ref_coord: numpy array containing the geodetic coordinate which will be the origin - of the tangent plane, [latitude, longitude, altitude] - :returns: The pose consisting of linearized GPS and IMU orientation, and the corresponding - covariance matrix as a 6x6 numpy array where each row is [x, y, z, roll, pitch, yaw] - """ - # linearize GPS coordinates into cartesian - cartesian = np.array(geodetic2enu(gps_msg.latitude, gps_msg.longitude, gps_msg.altitude, *ref_coord, deg=True)) - - # ignore Z - cartesian[2] = 0 - - imu_quat = numpify(imu_msg.imu.orientation) - - # normalize to avoid rounding errors - imu_quat = imu_quat / np.linalg.norm(imu_quat) - pose = SE3.from_pos_quat(position=cartesian, quaternion=imu_quat) - - covariance = np.zeros((6, 6)) - covariance[:3, :3] = np.array([gps_msg.position_covariance]).reshape(3, 3) - covariance[3:, 3:] = np.array([imu_msg.imu.orientation_covariance]).reshape(3, 3) - - return pose, covariance + # @staticmethod + # def get_linearized_pose_in_world( + # gps_msg: NavSatFix, imu_msg: ImuAndMag, ref_coord: np.ndarray + # ) -> Tuple[SE3, np.ndarray]: + # """ + # Linearizes the GPS geodetic coordinates into ENU cartesian coordinates, + # then combines them with the IMU orientation into a pose estimate relative + # to the world frame, with corresponding covariance matrix. + + # :param gps_msg: Message containing the rover's GPS coordinates and their corresponding + # covariance matrix. + # :param imu_msg: Message containing the rover's orientation from IMU, with + # corresponding covariance matrix. + # :param ref_coord: numpy array containing the geodetic coordinate which will be the origin + # of the tangent plane, [latitude, longitude, altitude] + # :returns: The pose consisting of linearized GPS and IMU orientation, and the corresponding + # covariance matrix as a 6x6 numpy array where each row is [x, y, z, roll, pitch, yaw] + # """ + # # linearize GPS coordinates into cartesian + # cartesian = np.array(geodetic2enu(gps_msg.latitude, gps_msg.longitude, gps_msg.altitude, *ref_coord, deg=True)) + + # # ignore Z + # cartesian[2] = 0 + + # imu_quat = numpify(imu_msg.imu.orientation) + + # # normalize to avoid rounding errors + # imu_quat = imu_quat / np.linalg.norm(imu_quat) + # pose = SE3.from_pos_quat(position=cartesian, quaternion=imu_quat) + + # covariance = np.zeros((6, 6)) + # covariance[:3, :3] = np.array([gps_msg.position_covariance]).reshape(3, 3) + # covariance[3:, 3:] = np.array([imu_msg.imu.orientation_covariance]).reshape(3, 3) + + # return pose, covariance def publish_pose(self): """ @@ -136,6 +148,9 @@ def publish_pose(self): self.pose_publisher.publish(pose_msg) + def compute_gps_bearing(self, right_cartesian, left_cartesian) -> np.ndarray: + ... + def main(): # start the node and spin to wait for messages to be received rospy.init_node("gps_linearization") From 819952fc6a4371d5dbbd89fd19432b4400565bd1 Mon Sep 17 00:00:00 2001 From: Pearl Lin Date: Thu, 30 Nov 2023 23:43:11 +0000 Subject: [PATCH 29/81] gps now put in sim --- launch/auton.launch | 4 +-- urdf/rover/rover.urdf.xacro | 18 +++++------- urdf/rover/rover_gazebo_plugins.urdf.xacro | 34 +++++----------------- 3 files changed, 18 insertions(+), 38 deletions(-) diff --git a/launch/auton.launch b/launch/auton.launch index 43be78198..cdfe87691 100644 --- a/launch/auton.launch +++ b/launch/auton.launch @@ -46,8 +46,8 @@ - + \ No newline at end of file diff --git a/urdf/rover/rover.urdf.xacro b/urdf/rover/rover.urdf.xacro index 6427bb41a..559f54772 100644 --- a/urdf/rover/rover.urdf.xacro +++ b/urdf/rover/rover.urdf.xacro @@ -138,28 +138,26 @@ - + - + - + - + - + + - + - + - - - diff --git a/urdf/rover/rover_gazebo_plugins.urdf.xacro b/urdf/rover/rover_gazebo_plugins.urdf.xacro index 162dcb9ff..f420d3758 100644 --- a/urdf/rover/rover_gazebo_plugins.urdf.xacro +++ b/urdf/rover/rover_gazebo_plugins.urdf.xacro @@ -18,12 +18,12 @@ base_link - + 100.0 - base_link - base_link - gps/fix + rover_gps_left_link + rover_gps_left_link + gps/fix/rover_gps_left gps/fix_velocity 42.293195 -83.7096706 @@ -36,36 +36,18 @@ 0.2 0.2 0.2 - - - 100.0 - base_link - base_link - gps/fix_gps_left - gps/fix_velocity - 42.293195 - -83.7096706 - 90.0 - 0.0 - 0 0.5 0 - 0.0 0.0 0.0 - 0 0 0 - 0.0 0.0 0.0 - 0.2 0.2 0.2 - - 100.0 - base_link - base_link - gps/fix_gps_right + rover_gps_right_link + rover_gps_right_link + gps/fix/rover_gps_right gps/fix_velocity 42.293195 -83.7096706 90.0 0.0 - 0 -0.5 0 + 0 0.5 0 0.0 0.0 0.0 0 0 0 0.0 0.0 0.0 From 473c1dd0200eda18892cf09bc7e0a0337cc759f0 Mon Sep 17 00:00:00 2001 From: Pearl Lin Date: Fri, 1 Dec 2023 00:11:25 +0000 Subject: [PATCH 30/81] edited test subscriber --- launch/auton.launch | 4 ++-- src/localization/Test-Subscriber.py | 4 ++-- src/localization/gps_linearization.py | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/launch/auton.launch b/launch/auton.launch index cdfe87691..5573825ba 100644 --- a/launch/auton.launch +++ b/launch/auton.launch @@ -46,8 +46,8 @@ - - --> + \ No newline at end of file diff --git a/src/localization/Test-Subscriber.py b/src/localization/Test-Subscriber.py index a7e7cc14b..17d8bfb00 100755 --- a/src/localization/Test-Subscriber.py +++ b/src/localization/Test-Subscriber.py @@ -27,8 +27,8 @@ class Localization: def __init__(self): # create subscribers for GPS data, linking them to our callback functions # TODO - rospy.Subscriber("gps/fix_gps_left", NavSatFix, self.gps_left_callback) - rospy.Subscriber("gps/fix_gps_right", NavSatFix, self.gps_right_callback) + rospy.Subscriber("gps/fix/rover_gps_left", NavSatFix, self.gps_left_callback) + rospy.Subscriber("gps/fix/rover_gps_right", NavSatFix, self.gps_right_callback) # create a transform broadcaster for publishing to the TF tree # self.tf_broadcaster = tf2_ros.TransformBroadcaster() diff --git a/src/localization/gps_linearization.py b/src/localization/gps_linearization.py index 5d1d991a5..70dad5c1d 100755 --- a/src/localization/gps_linearization.py +++ b/src/localization/gps_linearization.py @@ -46,7 +46,7 @@ def __init__(self): self.last_gps_msg = None self.last_imu_msg = None # gps/fix - rospy.Subscriber("gps/fix_gps_left", NavSatFix, self.gps_callback) + rospy.Subscriber("gps/fix/rover_gps_left", NavSatFix, self.gps_callback) rospy.Subscriber("imu/data", ImuAndMag, self.imu_callback) self.pose_publisher = rospy.Publisher("linearized_pose", PoseWithCovarianceStamped, queue_size=1) From 5aa6dacd27294cfe4140a05ae4e47300f6eb2749 Mon Sep 17 00:00:00 2001 From: Rahul Date: Thu, 30 Nov 2023 23:15:40 -0500 Subject: [PATCH 31/81] added pose calculation using heading --- src/localization/gps_linearization.py | 60 +++++++++------------------ 1 file changed, 20 insertions(+), 40 deletions(-) diff --git a/src/localization/gps_linearization.py b/src/localization/gps_linearization.py index a895266f2..51bd61218 100755 --- a/src/localization/gps_linearization.py +++ b/src/localization/gps_linearization.py @@ -9,6 +9,7 @@ from sensor_msgs.msg import NavSatFix from std_msgs.msg import Header from util.SE3 import SE3 +from util.SO3 import SO3 from util.np_utils import numpify import message_filters @@ -75,9 +76,11 @@ def gps_callback(self, right_gps_msg: NavSatFix, left_gps_msg: NavSatFix): right_cartesian = np.array(geodetic2enu(right_gps_msg.latitude, right_gps_msg.longitude, right_gps_msg.altitude, *ref_coord, deg=True)) left_cartesian = np.array(geodetic2enu(left_gps_msg.latitude, left_gps_msg.longitude, left_gps_msg.altitude, *ref_coord, deg=True)) - self.compute_gps_bearing(right_cartesian=right_cartesian, left_cartesian=left_cartesian) + pose = GPSLinearization.compute_gps_pose(right_cartesian=right_cartesian, left_cartesian=left_cartesian) + + # TODO: publish to ekf + - #todo compute pose and publish to EKF topic def imu_callback(self, msg: ImuAndMag): @@ -91,42 +94,6 @@ def imu_callback(self, msg: ImuAndMag): if self.last_gps_msg is not None: self.publish_pose() - # @staticmethod - # def get_linearized_pose_in_world( - # gps_msg: NavSatFix, imu_msg: ImuAndMag, ref_coord: np.ndarray - # ) -> Tuple[SE3, np.ndarray]: - # """ - # Linearizes the GPS geodetic coordinates into ENU cartesian coordinates, - # then combines them with the IMU orientation into a pose estimate relative - # to the world frame, with corresponding covariance matrix. - - # :param gps_msg: Message containing the rover's GPS coordinates and their corresponding - # covariance matrix. - # :param imu_msg: Message containing the rover's orientation from IMU, with - # corresponding covariance matrix. - # :param ref_coord: numpy array containing the geodetic coordinate which will be the origin - # of the tangent plane, [latitude, longitude, altitude] - # :returns: The pose consisting of linearized GPS and IMU orientation, and the corresponding - # covariance matrix as a 6x6 numpy array where each row is [x, y, z, roll, pitch, yaw] - # """ - # # linearize GPS coordinates into cartesian - # cartesian = np.array(geodetic2enu(gps_msg.latitude, gps_msg.longitude, gps_msg.altitude, *ref_coord, deg=True)) - - # # ignore Z - # cartesian[2] = 0 - - # imu_quat = numpify(imu_msg.imu.orientation) - - # # normalize to avoid rounding errors - # imu_quat = imu_quat / np.linalg.norm(imu_quat) - # pose = SE3.from_pos_quat(position=cartesian, quaternion=imu_quat) - - # covariance = np.zeros((6, 6)) - # covariance[:3, :3] = np.array([gps_msg.position_covariance]).reshape(3, 3) - # covariance[3:, 3:] = np.array([imu_msg.imu.orientation_covariance]).reshape(3, 3) - - # return pose, covariance - def publish_pose(self): """ Publishes the linearized pose of the rover relative to the map frame, @@ -147,9 +114,22 @@ def publish_pose(self): ) self.pose_publisher.publish(pose_msg) + @staticmethod + def compute_gps_pose(right_cartesian, left_cartesian) -> np.ndarray: + vector_connecting = left_cartesian - right_cartesian + vector_connecting[2] = 0 + + + vector_perp = np.zeros(3,1) + vector_perp[0] = vector_connecting[1] + vector_perp[1] = -vector_connecting[0] + + rotation_matrix = np.hstack((vector_perp, vector_connecting, [0,0,1])) + + pose = SE3(left_cartesian, SO3.from_matrix(rotation_matrix=rotation_matrix)) + + return pose - def compute_gps_bearing(self, right_cartesian, left_cartesian) -> np.ndarray: - ... def main(): # start the node and spin to wait for messages to be received From db998ea9fb4ba23e01bd0669b53dbf6dae5b6a7e Mon Sep 17 00:00:00 2001 From: dllliu Date: Sun, 3 Dec 2023 13:18:12 -0500 Subject: [PATCH 32/81] tests for both gps distance --- src/localization/Test-Subscriber.py | 41 +++++++++++++++++----- urdf/rover/rover_gazebo_plugins.urdf.xacro | 4 +-- 2 files changed, 34 insertions(+), 11 deletions(-) diff --git a/src/localization/Test-Subscriber.py b/src/localization/Test-Subscriber.py index 17d8bfb00..6c4096bff 100755 --- a/src/localization/Test-Subscriber.py +++ b/src/localization/Test-Subscriber.py @@ -5,6 +5,7 @@ # library for interacting with ROS and TF tree import rospy +import csv import tf2_ros import math @@ -12,17 +13,23 @@ from sensor_msgs.msg import NavSatFix, Imu # SE3 library for handling poses and TF tree -from util.SE3 import SE3 -from util.SO3 import SO3 +# from util.SE3 import SE3 +# from util.SO3 import SO3 from matplotlib import pyplot as plt lat_arr = [] long_arr = [] +coord_arr_left_latitude = [] +coord_arr_left_longitude = [] +coord_arr_right_latitude = [] +coord_arr_right_longitude = [] +distances = [] + class Localization: - pose: SE3 + # pose: SE3 def __init__(self): # create subscribers for GPS data, linking them to our callback functions @@ -30,6 +37,9 @@ def __init__(self): rospy.Subscriber("gps/fix/rover_gps_left", NavSatFix, self.gps_left_callback) rospy.Subscriber("gps/fix/rover_gps_right", NavSatFix, self.gps_right_callback) + # csvfile = open("/home/daniel/catkin_ws/src/mrover/src/localization/test_gps_sim.csv", "w") + # csvwriter = csv.writer(csvfile) + # csvwriter.writerow([9, 10]) # create a transform broadcaster for publishing to the TF tree # self.tf_broadcaster = tf2_ros.TransformBroadcaster() @@ -37,13 +47,16 @@ def __init__(self): # self.pose = SE3() def gps_left_callback(self, msg: NavSatFix): - print(msg) - lat_arr.append(msg.latitude) - long_arr.append(msg.longitude) + # print(msg) + coord_arr_left_latitude.append(msg.latitude) + coord_arr_left_longitude.append(msg.longitude) + # lat_arr.append(msg.latitude) + # long_arr.append(msg.longitude) def gps_right_callback(self, msg: NavSatFix): - print(msg) - pass + # print(msg) + coord_arr_right_latitude.append(msg.latitude) + coord_arr_right_longitude.append(msg.longitude) def main(): @@ -59,5 +72,15 @@ def main(): if __name__ == "__main__": main() - plt.scatter(lat_arr, long_arr) + + # array_left = np.array(coord_arr_left) + # array_right = np.array(coord_arr_right) + # for i in range(min(len(coord_arr_left_latitude), len(coord_arr_right_latitude))): + # plt.plot(coord_arr_left_latitude[i], coord_arr_left_longitude[i], color="red") + # plt.plot(coord_arr_right_latitude[i], coord_arr_right_longitude[i], color="blue") + # plt.show() + # print("here") + # distances[i] = math.dist([coord_arr_left_latitude]) + plt.scatter(coord_arr_left_latitude, coord_arr_left_longitude, color="red") + plt.scatter(coord_arr_right_latitude, coord_arr_right_longitude, color="blue") plt.show() diff --git a/urdf/rover/rover_gazebo_plugins.urdf.xacro b/urdf/rover/rover_gazebo_plugins.urdf.xacro index f420d3758..46a054a93 100644 --- a/urdf/rover/rover_gazebo_plugins.urdf.xacro +++ b/urdf/rover/rover_gazebo_plugins.urdf.xacro @@ -47,9 +47,9 @@ -83.7096706 90.0 0.0 - 0 0.5 0 + 0 0 0 0.0 0.0 0.0 - 0 0 0 + 0.3 0.3 0.3 0.0 0.0 0.0 0.2 0.2 0.2 From 670261f0fe859509c3416ace3936a330dd421ac4 Mon Sep 17 00:00:00 2001 From: dllliu Date: Sun, 3 Dec 2023 14:07:47 -0500 Subject: [PATCH 33/81] changes for testing with convert lat long to cartesian to see offset better --- launch/auton.launch | 2 +- src/localization/Test-Subscriber.py | 71 ++++++++++++++++------ urdf/rover/rover_gazebo_plugins.urdf.xacro | 8 +-- 3 files changed, 57 insertions(+), 24 deletions(-) diff --git a/launch/auton.launch b/launch/auton.launch index 5573825ba..43be78198 100644 --- a/launch/auton.launch +++ b/launch/auton.launch @@ -46,7 +46,7 @@ - + diff --git a/src/localization/Test-Subscriber.py b/src/localization/Test-Subscriber.py index 6c4096bff..9c4be58ce 100755 --- a/src/localization/Test-Subscriber.py +++ b/src/localization/Test-Subscriber.py @@ -17,14 +17,18 @@ # from util.SO3 import SO3 from matplotlib import pyplot as plt +# import gps_linearization + lat_arr = [] long_arr = [] -coord_arr_left_latitude = [] -coord_arr_left_longitude = [] -coord_arr_right_latitude = [] -coord_arr_right_longitude = [] +# coord_arr_left_latitude = [] +# coord_arr_left_longitude = [] +# coord_arr_right_latitude = [] +# coord_arr_right_longitude = [] +coord_arr_left = np.array() +coord_arr_right = np.array() distances = [] @@ -46,28 +50,57 @@ def __init__(self): # initialize pose to all zeros # self.pose = SE3() + def spherical_to_cartesian(spherical_coord: np.ndarray, reference_coord: np.ndarray) -> np.ndarray: + """ + This is a utility function that should convert spherical (latitude, longitude) + coordinates into cartesian (x, y, z) coordinates using the specified reference point + as the center of the tangent plane used for approximation. + :param spherical_coord: the spherical coordinate to convert, + given as a numpy array [latitude, longitude] + :param reference_coord: the reference coordinate to use for conversion, + given as a numpy array [latitude, longitude] + :returns: the approximated cartesian coordinates in meters, given as a numpy array [x, y, z] + """ + crc = 6371000 + longDist = ( + crc + * (np.radians(spherical_coord[1]) - np.radians(reference_coord[1])) + * np.cos(np.radians(reference_coord[0])) + ) + latDist = crc * (np.radians(spherical_coord[0]) - np.radians(reference_coord[0])) + z = 0 + return np.array([longDist, latDist]) + def gps_left_callback(self, msg: NavSatFix): # print(msg) - coord_arr_left_latitude.append(msg.latitude) - coord_arr_left_longitude.append(msg.longitude) + # coord_arr_left_latitude.append(gps_linearization.geodetic2enu(msg.latitude, msg.longitude) + # coord_arr_left_longitude.append(msg.longitude) + print(self.spherical_to_cartesian(np.array([msg.latitude, msg.longitude]), np.array([42.293195, -83.7096706]))) + # print(np.array([42.293195, -83.7096706])) + coord_arr_left.append( + self.spherical_to_cartesian(np.array([msg.latitude, msg.longitude]), np.array([42.293195, -83.7096706])) + ) + # lat_arr.append(msg.latitude) # long_arr.append(msg.longitude) def gps_right_callback(self, msg: NavSatFix): - # print(msg) - coord_arr_right_latitude.append(msg.latitude) - coord_arr_right_longitude.append(msg.longitude) - + print(msg) + # coord_arr_right.append( + # self.spherical_to_cartesian(np.array([msg.latitude, msg.longitude]), np.array([42.293195, -83.7096706])) + # ) + # coord_arr_right_latitude.append(msg.latitude) + # coord_arr_right_longitude.append(msg.longitude) -def main(): - # initialize the node - rospy.init_node("localization_test") + def main(): + # initialize the node + rospy.init_node("localization_test") - # create and start our localization system - localization = Localization() + # create and start our localization system + localization = Localization() - # let the callback functions run asynchronously in the background - rospy.spin() + # let the callback functions run asynchronously in the background + rospy.spin() if __name__ == "__main__": @@ -81,6 +114,6 @@ def main(): # plt.show() # print("here") # distances[i] = math.dist([coord_arr_left_latitude]) - plt.scatter(coord_arr_left_latitude, coord_arr_left_longitude, color="red") - plt.scatter(coord_arr_right_latitude, coord_arr_right_longitude, color="blue") + plt.scatter(coord_arr_left, color="red") + plt.scatter(coord_arr_right, color="blue") plt.show() diff --git a/urdf/rover/rover_gazebo_plugins.urdf.xacro b/urdf/rover/rover_gazebo_plugins.urdf.xacro index 46a054a93..5d120f356 100644 --- a/urdf/rover/rover_gazebo_plugins.urdf.xacro +++ b/urdf/rover/rover_gazebo_plugins.urdf.xacro @@ -31,9 +31,9 @@ 0.0 0 0 0 0.0 0.0 0.0 - 0.3 0.3 0.3 + 0 0 0 0.0 0.0 0.0 - 0.2 0.2 0.2 + 0 0 0 @@ -49,9 +49,9 @@ 0.0 0 0 0 0.0 0.0 0.0 - 0.3 0.3 0.3 + 0 0 0 0.0 0.0 0.0 - 0.2 0.2 0.2 + 0 0 0 From 29e0748726ca09539e3cecc99e4f5322d24dd660 Mon Sep 17 00:00:00 2001 From: dllliu Date: Tue, 5 Dec 2023 19:33:55 -0500 Subject: [PATCH 34/81] tests for offset --- src/localization/Test-Subscriber.py | 97 +++++++++++----------- urdf/rover/rover.urdf.xacro | 11 ++- urdf/rover/rover_gazebo_plugins.urdf.xacro | 8 +- 3 files changed, 59 insertions(+), 57 deletions(-) diff --git a/src/localization/Test-Subscriber.py b/src/localization/Test-Subscriber.py index 9c4be58ce..4867516b1 100755 --- a/src/localization/Test-Subscriber.py +++ b/src/localization/Test-Subscriber.py @@ -16,6 +16,8 @@ # from util.SE3 import SE3 # from util.SO3 import SO3 from matplotlib import pyplot as plt +import csv +from decimal import Decimal # import gps_linearization @@ -23,12 +25,12 @@ lat_arr = [] long_arr = [] -# coord_arr_left_latitude = [] -# coord_arr_left_longitude = [] -# coord_arr_right_latitude = [] -# coord_arr_right_longitude = [] -coord_arr_left = np.array() -coord_arr_right = np.array() +coord_arr_left_latitude = [] +coord_arr_left_longitude = [] +coord_arr_right_latitude = [] +coord_arr_right_longitude = [] +# coord_arr_left = [] +# coord_arr_right = [] distances = [] @@ -50,47 +52,24 @@ def __init__(self): # initialize pose to all zeros # self.pose = SE3() - def spherical_to_cartesian(spherical_coord: np.ndarray, reference_coord: np.ndarray) -> np.ndarray: - """ - This is a utility function that should convert spherical (latitude, longitude) - coordinates into cartesian (x, y, z) coordinates using the specified reference point - as the center of the tangent plane used for approximation. - :param spherical_coord: the spherical coordinate to convert, - given as a numpy array [latitude, longitude] - :param reference_coord: the reference coordinate to use for conversion, - given as a numpy array [latitude, longitude] - :returns: the approximated cartesian coordinates in meters, given as a numpy array [x, y, z] - """ - crc = 6371000 - longDist = ( - crc - * (np.radians(spherical_coord[1]) - np.radians(reference_coord[1])) - * np.cos(np.radians(reference_coord[0])) - ) - latDist = crc * (np.radians(spherical_coord[0]) - np.radians(reference_coord[0])) - z = 0 - return np.array([longDist, latDist]) - def gps_left_callback(self, msg: NavSatFix): - # print(msg) + print(msg) # coord_arr_left_latitude.append(gps_linearization.geodetic2enu(msg.latitude, msg.longitude) # coord_arr_left_longitude.append(msg.longitude) - print(self.spherical_to_cartesian(np.array([msg.latitude, msg.longitude]), np.array([42.293195, -83.7096706]))) - # print(np.array([42.293195, -83.7096706])) - coord_arr_left.append( - self.spherical_to_cartesian(np.array([msg.latitude, msg.longitude]), np.array([42.293195, -83.7096706])) - ) + + coord_arr_left_latitude.append(msg.latitude) + coord_arr_left_longitude.append(msg.longitude) + # coord_arr_left.append([msg.latitude, msg.longitude]) # lat_arr.append(msg.latitude) # long_arr.append(msg.longitude) def gps_right_callback(self, msg: NavSatFix): print(msg) - # coord_arr_right.append( - # self.spherical_to_cartesian(np.array([msg.latitude, msg.longitude]), np.array([42.293195, -83.7096706])) - # ) - # coord_arr_right_latitude.append(msg.latitude) - # coord_arr_right_longitude.append(msg.longitude) + # coord_arr_right.append([msg.latitude, msg.longitude]) + coord_arr_right_latitude.append(msg.latitude) + coord_arr_right_longitude.append(msg.longitude) + # coord_arr_right.append([msg.latitude, msg.longitude]) def main(): # initialize the node @@ -104,16 +83,40 @@ def main(): if __name__ == "__main__": - main() + Localization.main() - # array_left = np.array(coord_arr_left) - # array_right = np.array(coord_arr_right) - # for i in range(min(len(coord_arr_left_latitude), len(coord_arr_right_latitude))): # plt.plot(coord_arr_left_latitude[i], coord_arr_left_longitude[i], color="red") # plt.plot(coord_arr_right_latitude[i], coord_arr_right_longitude[i], color="blue") # plt.show() - # print("here") - # distances[i] = math.dist([coord_arr_left_latitude]) - plt.scatter(coord_arr_left, color="red") - plt.scatter(coord_arr_right, color="blue") - plt.show() + + # plt.scatter(coord_arr_left_latitude, coord_arr_left_longitude, color="red") + # plt.scatter(coord_arr_right_latitude, coord_arr_right_longitude, color="blue") + # Set the scale (adjust the limits according to your data) + # plt.xlim(coord_arr_left_latitude[0], coord_arr_left_latitude[4]) + # plt.ylim(coord_arr_left_longitude[0], coord_arr_right_longitude[4]) + + # plt.show() + + for i in range(len(coord_arr_left_latitude)): + print(Decimal(coord_arr_left_latitude[i]) - Decimal(coord_arr_right_latitude[i])) + print(Decimal(coord_arr_left_longitude[i]) - Decimal(coord_arr_right_longitude[i])) + print("\n") + print(coord_arr_right_latitude[:10]) + print(coord_arr_left_latitude[:10]) + print("\n") + print(coord_arr_right_longitude[:10]) + print(coord_arr_left_longitude[:10]) + # print( + # (sum(coord_arr_left_latitude) / len(coord_arr_left_latitude)) + # - (sum(coord_arr_right_latitude) / len(coord_arr_right_latitude)) + # ) + # print( + # (sum(coord_arr_left_longitude) / len(coord_arr_right_longitude)) + # - (sum(coord_arr_right_longitude) / len(coord_arr_right_longitude)) + # ) + # plt.pause(300) + + # print("Click on the plot window and press any key to close.") + # plt.waitforbuttonpress() + # plt.close() + # plt.show() diff --git a/urdf/rover/rover.urdf.xacro b/urdf/rover/rover.urdf.xacro index 559f54772..508eed06f 100644 --- a/urdf/rover/rover.urdf.xacro +++ b/urdf/rover/rover.urdf.xacro @@ -138,22 +138,21 @@ - + - + - + - - + - + diff --git a/urdf/rover/rover_gazebo_plugins.urdf.xacro b/urdf/rover/rover_gazebo_plugins.urdf.xacro index 5d120f356..67da9e2bc 100644 --- a/urdf/rover/rover_gazebo_plugins.urdf.xacro +++ b/urdf/rover/rover_gazebo_plugins.urdf.xacro @@ -25,8 +25,8 @@ rover_gps_left_link gps/fix/rover_gps_left gps/fix_velocity - 42.293195 - -83.7096706 + 42.2931950000000000000000 + -83.80967060000000000000000 90.0 0.0 0 0 0 @@ -43,8 +43,8 @@ rover_gps_right_link gps/fix/rover_gps_right gps/fix_velocity - 42.293195 - -83.7096706 + 42.2931950000000000000000 + -83.80967060000000000000000 90.0 0.0 0 0 0 From 090866c2d42823fcf3d38d72c0a8d28db98109d6 Mon Sep 17 00:00:00 2001 From: dllliu Date: Tue, 5 Dec 2023 20:02:01 -0500 Subject: [PATCH 35/81] offset is 10? --- src/localization/Test-Subscriber.py | 29 ++++++++-------------- urdf/rover/rover.urdf.xacro | 2 +- urdf/rover/rover_gazebo_plugins.urdf.xacro | 2 +- 3 files changed, 12 insertions(+), 21 deletions(-) diff --git a/src/localization/Test-Subscriber.py b/src/localization/Test-Subscriber.py index 4867516b1..e7886bcf0 100755 --- a/src/localization/Test-Subscriber.py +++ b/src/localization/Test-Subscriber.py @@ -16,8 +16,6 @@ # from util.SE3 import SE3 # from util.SO3 import SO3 from matplotlib import pyplot as plt -import csv -from decimal import Decimal # import gps_linearization @@ -91,21 +89,20 @@ def main(): # plt.scatter(coord_arr_left_latitude, coord_arr_left_longitude, color="red") # plt.scatter(coord_arr_right_latitude, coord_arr_right_longitude, color="blue") - # Set the scale (adjust the limits according to your data) - # plt.xlim(coord_arr_left_latitude[0], coord_arr_left_latitude[4]) - # plt.ylim(coord_arr_left_longitude[0], coord_arr_right_longitude[4]) # plt.show() for i in range(len(coord_arr_left_latitude)): - print(Decimal(coord_arr_left_latitude[i]) - Decimal(coord_arr_right_latitude[i])) - print(Decimal(coord_arr_left_longitude[i]) - Decimal(coord_arr_right_longitude[i])) - print("\n") - print(coord_arr_right_latitude[:10]) - print(coord_arr_left_latitude[:10]) - print("\n") - print(coord_arr_right_longitude[:10]) - print(coord_arr_left_longitude[:10]) + print(coord_arr_left_latitude[i] - coord_arr_right_latitude[i]) + print(coord_arr_left_longitude[i] - coord_arr_right_longitude[i]) + + # print("\n") + # print(coord_arr_right_latitude[:10]) + # print(coord_arr_left_latitude[:10]) + + # print("\n") + # print(coord_arr_right_longitude[:10]) + # print(coord_arr_left_longitude[:10]) # print( # (sum(coord_arr_left_latitude) / len(coord_arr_left_latitude)) # - (sum(coord_arr_right_latitude) / len(coord_arr_right_latitude)) @@ -114,9 +111,3 @@ def main(): # (sum(coord_arr_left_longitude) / len(coord_arr_right_longitude)) # - (sum(coord_arr_right_longitude) / len(coord_arr_right_longitude)) # ) - # plt.pause(300) - - # print("Click on the plot window and press any key to close.") - # plt.waitforbuttonpress() - # plt.close() - # plt.show() diff --git a/urdf/rover/rover.urdf.xacro b/urdf/rover/rover.urdf.xacro index 508eed06f..31e18083e 100644 --- a/urdf/rover/rover.urdf.xacro +++ b/urdf/rover/rover.urdf.xacro @@ -152,7 +152,7 @@ - + diff --git a/urdf/rover/rover_gazebo_plugins.urdf.xacro b/urdf/rover/rover_gazebo_plugins.urdf.xacro index 67da9e2bc..05014b2cc 100644 --- a/urdf/rover/rover_gazebo_plugins.urdf.xacro +++ b/urdf/rover/rover_gazebo_plugins.urdf.xacro @@ -47,7 +47,7 @@ -83.80967060000000000000000 90.0 0.0 - 0 0 0 + 0 10 0 0.0 0.0 0.0 0 0 0 0.0 0.0 0.0 From 069a2cf69895f4870b83846ac1dddbe1caa5f265 Mon Sep 17 00:00:00 2001 From: nehakankanala Date: Thu, 7 Dec 2023 19:39:29 +0000 Subject: [PATCH 36/81] chanegd offset to 1 and modified testing code --- src/localization/Test-Subscriber.py | 4 ++-- urdf/rover/rover.urdf.xacro | 2 +- urdf/rover/rover_gazebo_plugins.urdf.xacro | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/localization/Test-Subscriber.py b/src/localization/Test-Subscriber.py index e7886bcf0..e354af24c 100755 --- a/src/localization/Test-Subscriber.py +++ b/src/localization/Test-Subscriber.py @@ -93,8 +93,8 @@ def main(): # plt.show() for i in range(len(coord_arr_left_latitude)): - print(coord_arr_left_latitude[i] - coord_arr_right_latitude[i]) - print(coord_arr_left_longitude[i] - coord_arr_right_longitude[i]) + print("latitude" + str(coord_arr_left_latitude[i] - coord_arr_right_latitude[i])) + print("longitude" + str(coord_arr_left_longitude[i] - coord_arr_right_longitude[i])) # print("\n") # print(coord_arr_right_latitude[:10]) diff --git a/urdf/rover/rover.urdf.xacro b/urdf/rover/rover.urdf.xacro index 31e18083e..508eed06f 100644 --- a/urdf/rover/rover.urdf.xacro +++ b/urdf/rover/rover.urdf.xacro @@ -152,7 +152,7 @@ - + diff --git a/urdf/rover/rover_gazebo_plugins.urdf.xacro b/urdf/rover/rover_gazebo_plugins.urdf.xacro index 05014b2cc..292ee03a2 100644 --- a/urdf/rover/rover_gazebo_plugins.urdf.xacro +++ b/urdf/rover/rover_gazebo_plugins.urdf.xacro @@ -47,7 +47,7 @@ -83.80967060000000000000000 90.0 0.0 - 0 10 0 + 0 1 0 0.0 0.0 0.0 0 0 0 0.0 0.0 0.0 From 10d328dee1bd08c00d733a1cd0647599c28108f6 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Sun, 10 Dec 2023 16:15:03 -0500 Subject: [PATCH 37/81] Add requirements --- ansible/roles/basestation_networks/tasks/main.yml | 4 ++-- package.xml | 2 +- pyproject.toml | 1 + 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/ansible/roles/basestation_networks/tasks/main.yml b/ansible/roles/basestation_networks/tasks/main.yml index dded708bd..a0cfc70b6 100644 --- a/ansible/roles/basestation_networks/tasks/main.yml +++ b/ansible/roles/basestation_networks/tasks/main.yml @@ -4,9 +4,9 @@ conn_name: MRover state: present type: ethernet - ifname: enp5s0 + ifname: enx207bd284f29c autoconnect: yes - ip4: 10.0.0.2/24 + ip4: 10.0.0.2/8 # Share connection to the Internet method4: shared method6: disabled diff --git a/package.xml b/package.xml index 87ca4865b..17826655e 100644 --- a/package.xml +++ b/package.xml @@ -67,7 +67,7 @@ rviz_imu_plugin robot_localization - nmea_navsat_driver + rtcm_msgs smach_ros diff --git a/pyproject.toml b/pyproject.toml index 17c26b5cc..f1bfd9fc3 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -23,6 +23,7 @@ dependencies = [ "moteus==0.3.59", "pymap3d==3.0.1", "aenum==3.1.15", + "pyubx2", ] [project.optional-dependencies] From 2659e84b3b1157319d2827259aad59de9aab6fc8 Mon Sep 17 00:00:00 2001 From: Rahul Date: Sun, 14 Jan 2024 13:46:24 -0500 Subject: [PATCH 38/81] updated linearlization --- src/localization/gps_linearization.py | 23 ++++++++++++++++++++++- 1 file changed, 22 insertions(+), 1 deletion(-) diff --git a/src/localization/gps_linearization.py b/src/localization/gps_linearization.py index 51bd61218..df1c76f8c 100755 --- a/src/localization/gps_linearization.py +++ b/src/localization/gps_linearization.py @@ -43,7 +43,10 @@ def __init__(self): self.ref_alt = rospy.get_param("gps_linearization/reference_point_altitude") self.world_frame = rospy.get_param("world_frame") self.use_dop_covariance = rospy.get_param("global_ekf/use_gps_dop_covariance") + + #config gps and imu convariance matrices self.config_gps_covariance = np.array(rospy.get_param("global_ekf/gps_covariance", None)) + self.config_imu_covariance = np.array(rospy.get_param("global_ekf/imu_orientation_covariance", None)) self.last_gps_msg = None self.last_imu_msg = None @@ -78,7 +81,23 @@ def gps_callback(self, right_gps_msg: NavSatFix, left_gps_msg: NavSatFix): pose = GPSLinearization.compute_gps_pose(right_cartesian=right_cartesian, left_cartesian=left_cartesian) + covariance_matrix = np.zeros(6,6) + covariance_matrix[:3,:3] = self.config_gps_covariance.reshape(3,3) + covariance_matrix[3:, 3:] = self.config_imu_covariance.reshape(3,3) + # TODO: publish to ekf + pose_msg = PoseWithCovarianceStamped( + header=Header(stamp=rospy.Time.now(), frame_id=self.world_frame), + pose=PoseWithCovariance( + pose=Pose( + position=Point(*pose.position), + orientation=Quaternion(*pose.rotation.quaternion), + ), + covariance=covariance_matrix.flatten().tolist(), + ), + ) + + self.pose_publisher.publish(pose_msg) @@ -118,6 +137,8 @@ def publish_pose(self): def compute_gps_pose(right_cartesian, left_cartesian) -> np.ndarray: vector_connecting = left_cartesian - right_cartesian vector_connecting[2] = 0 + magnitude = np.linalg.norm(vector_connecting) + vector_connecting = vector_connecting / magnitude vector_perp = np.zeros(3,1) @@ -139,4 +160,4 @@ def main(): if __name__ == "__main__": - main() + main() \ No newline at end of file From a3eb9e7787bcc0ad644d943af8359ffc6aa8fdd0 Mon Sep 17 00:00:00 2001 From: dllliu Date: Tue, 16 Jan 2024 19:55:41 -0500 Subject: [PATCH 39/81] debug linearization function and try to debug sim --- launch/auton.launch | 13 +++++---- src/localization/gps_linearization.py | 33 +++++++++++----------- urdf/rover/rover_gazebo_plugins.urdf.xacro | 4 +-- 3 files changed, 26 insertions(+), 24 deletions(-) diff --git a/launch/auton.launch b/launch/auton.launch index 43be78198..73a0401db 100644 --- a/launch/auton.launch +++ b/launch/auton.launch @@ -2,7 +2,7 @@ - + - - + --> - + - + - + \ No newline at end of file diff --git a/src/localization/gps_linearization.py b/src/localization/gps_linearization.py index df1c76f8c..bd15d73ef 100755 --- a/src/localization/gps_linearization.py +++ b/src/localization/gps_linearization.py @@ -44,7 +44,7 @@ def __init__(self): self.world_frame = rospy.get_param("world_frame") self.use_dop_covariance = rospy.get_param("global_ekf/use_gps_dop_covariance") - #config gps and imu convariance matrices + # config gps and imu convariance matrices self.config_gps_covariance = np.array(rospy.get_param("global_ekf/gps_covariance", None)) self.config_imu_covariance = np.array(rospy.get_param("global_ekf/imu_orientation_covariance", None)) @@ -57,7 +57,6 @@ def __init__(self): sync_gps_sub = message_filters.ApproximateTimeSynchronizer([right_gps_sub, left_gps_sub], 10, 0.5) sync_gps_sub.registerCallback(self.gps_callback) - rospy.Subscriber("imu/data", ImuAndMag, self.imu_callback) self.pose_publisher = rospy.Publisher("linearized_pose", PoseWithCovarianceStamped, queue_size=1) @@ -73,17 +72,21 @@ def gps_callback(self, right_gps_msg: NavSatFix, left_gps_msg: NavSatFix): return if np.any(np.isnan([left_gps_msg.latitude, left_gps_msg.longitude, left_gps_msg.altitude])): return - + ref_coord = np.array([self.ref_lat, self.ref_lon, self.ref_alt]) - - right_cartesian = np.array(geodetic2enu(right_gps_msg.latitude, right_gps_msg.longitude, right_gps_msg.altitude, *ref_coord, deg=True)) - left_cartesian = np.array(geodetic2enu(left_gps_msg.latitude, left_gps_msg.longitude, left_gps_msg.altitude, *ref_coord, deg=True)) + + right_cartesian = np.array( + geodetic2enu(right_gps_msg.latitude, right_gps_msg.longitude, right_gps_msg.altitude, *ref_coord, deg=True) + ) + left_cartesian = np.array( + geodetic2enu(left_gps_msg.latitude, left_gps_msg.longitude, left_gps_msg.altitude, *ref_coord, deg=True) + ) pose = GPSLinearization.compute_gps_pose(right_cartesian=right_cartesian, left_cartesian=left_cartesian) - covariance_matrix = np.zeros(6,6) - covariance_matrix[:3,:3] = self.config_gps_covariance.reshape(3,3) - covariance_matrix[3:, 3:] = self.config_imu_covariance.reshape(3,3) + covariance_matrix = np.zeros((6, 6)) + covariance_matrix[:3, :3] = self.config_gps_covariance.reshape(3, 3) + covariance_matrix[3:, 3:] = self.config_imu_covariance.reshape(3, 3) # TODO: publish to ekf pose_msg = PoseWithCovarianceStamped( @@ -99,9 +102,6 @@ def gps_callback(self, right_gps_msg: NavSatFix, left_gps_msg: NavSatFix): self.pose_publisher.publish(pose_msg) - - - def imu_callback(self, msg: ImuAndMag): """ Callback function that receives IMU messages and publishes the linearized pose. @@ -140,12 +140,13 @@ def compute_gps_pose(right_cartesian, left_cartesian) -> np.ndarray: magnitude = np.linalg.norm(vector_connecting) vector_connecting = vector_connecting / magnitude - - vector_perp = np.zeros(3,1) + vector_perp = np.zeros(shape=(3, 1)) vector_perp[0] = vector_connecting[1] vector_perp[1] = -vector_connecting[0] - rotation_matrix = np.hstack((vector_perp, vector_connecting, [0,0,1])) + rotation_matrix = np.hstack( + (vector_perp, np.reshape(vector_connecting, (3, 1)), np.array(object=[[0], [0], [1]])) + ) pose = SE3(left_cartesian, SO3.from_matrix(rotation_matrix=rotation_matrix)) @@ -160,4 +161,4 @@ def main(): if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/urdf/rover/rover_gazebo_plugins.urdf.xacro b/urdf/rover/rover_gazebo_plugins.urdf.xacro index 292ee03a2..763e02b08 100644 --- a/urdf/rover/rover_gazebo_plugins.urdf.xacro +++ b/urdf/rover/rover_gazebo_plugins.urdf.xacro @@ -23,7 +23,7 @@ 100.0 rover_gps_left_link rover_gps_left_link - gps/fix/rover_gps_left + left_gps_driver/fix gps/fix_velocity 42.2931950000000000000000 -83.80967060000000000000000 @@ -41,7 +41,7 @@ 100.0 rover_gps_right_link rover_gps_right_link - gps/fix/rover_gps_right + right_gps_driver/fix gps/fix_velocity 42.2931950000000000000000 -83.80967060000000000000000 From 6ac7f8dea94190d844ca2ed45049357395feb237 Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Fri, 19 Jan 2024 16:18:22 -0500 Subject: [PATCH 40/81] fixed reference points and removed z component for better rviz experience --- launch/auton.launch | 2 +- src/localization/gps_linearization.py | 4 +++- urdf/rover/rover.urdf.xacro | 4 ++-- urdf/rover/rover_gazebo_plugins.urdf.xacro | 8 ++++---- 4 files changed, 10 insertions(+), 8 deletions(-) diff --git a/launch/auton.launch b/launch/auton.launch index 73a0401db..2d2dac960 100644 --- a/launch/auton.launch +++ b/launch/auton.launch @@ -47,7 +47,7 @@ - + diff --git a/src/localization/gps_linearization.py b/src/localization/gps_linearization.py index bd15d73ef..0494d480d 100755 --- a/src/localization/gps_linearization.py +++ b/src/localization/gps_linearization.py @@ -135,8 +135,10 @@ def publish_pose(self): @staticmethod def compute_gps_pose(right_cartesian, left_cartesian) -> np.ndarray: + left_cartesian[2] = 0 + right_cartesian[2] = 0 vector_connecting = left_cartesian - right_cartesian - vector_connecting[2] = 0 + # vector_connecting[2] = 0 magnitude = np.linalg.norm(vector_connecting) vector_connecting = vector_connecting / magnitude diff --git a/urdf/rover/rover.urdf.xacro b/urdf/rover/rover.urdf.xacro index 508eed06f..b0d049d3b 100644 --- a/urdf/rover/rover.urdf.xacro +++ b/urdf/rover/rover.urdf.xacro @@ -144,7 +144,7 @@ - + @@ -152,7 +152,7 @@ - + diff --git a/urdf/rover/rover_gazebo_plugins.urdf.xacro b/urdf/rover/rover_gazebo_plugins.urdf.xacro index 763e02b08..d02ed5032 100644 --- a/urdf/rover/rover_gazebo_plugins.urdf.xacro +++ b/urdf/rover/rover_gazebo_plugins.urdf.xacro @@ -25,8 +25,8 @@ rover_gps_left_link left_gps_driver/fix gps/fix_velocity - 42.2931950000000000000000 - -83.80967060000000000000000 + 42.293195 + -83.7096706 90.0 0.0 0 0 0 @@ -43,8 +43,8 @@ rover_gps_right_link right_gps_driver/fix gps/fix_velocity - 42.2931950000000000000000 - -83.80967060000000000000000 + 42.293195 + -83.7096706 90.0 0.0 0 1 0 From 46e4f77feb64f9068c4672891bf626f75ecaeba8 Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Fri, 19 Jan 2024 17:12:20 -0500 Subject: [PATCH 41/81] made a temporary GPS sim --- launch/auton.launch | 2 + src/localization/temp_gps_sim.py | 76 ++++++++++++++++++++++ urdf/rover/rover_gazebo_plugins.urdf.xacro | 5 +- 3 files changed, 80 insertions(+), 3 deletions(-) create mode 100755 src/localization/temp_gps_sim.py diff --git a/launch/auton.launch b/launch/auton.launch index 2d2dac960..59fb0a298 100644 --- a/launch/auton.launch +++ b/launch/auton.launch @@ -46,6 +46,8 @@ + + diff --git a/src/localization/temp_gps_sim.py b/src/localization/temp_gps_sim.py new file mode 100755 index 000000000..b27d4a01d --- /dev/null +++ b/src/localization/temp_gps_sim.py @@ -0,0 +1,76 @@ +#!/usr/bin/env python3 +import numpy as np +import rospy +from nav_msgs.msg import Odometry +from sensor_msgs.msg import NavSatFix +from util.SE3 import SE3 +from pymap3d.enu import enu2geodetic + +class GPSSim: + latest_odom: Odometry = None + ref_point: np.ndarray + right_gps_pub: rospy.Publisher + left_gps_pub: rospy.Publisher + ground_truth_sub: rospy.Subscriber + + def __init__(self): + ref_lat = rospy.get_param("gps_linearization/reference_point_latitude") + ref_lon = rospy.get_param("gps_linearization/reference_point_longitude") + ref_alt = rospy.get_param("gps_linearization/reference_point_altitude") + self.ref_point = np.array([ref_lat, ref_lon, ref_alt]) + + self.right_gps_pub = rospy.Publisher("right_gps_driver/fix", NavSatFix, queue_size=10) + self.left_gps_pub = rospy.Publisher("left_gps_driver/fix", NavSatFix, queue_size=10) + self.ground_truth_sub = rospy.Subscriber("ground_truth", Odometry, self.ground_truth_callback) + + def ground_truth_callback(self, msg: Odometry): + self.latest_odom = msg + + def publish_gps(self): + if self.latest_odom is None: + return + + # position of each GPS antenna relative to base_link, hardcoded for now + left_gps_offset = np.array([0, 1, 0]) + right_gps_offset = np.array([0, -1, 0]) + + # get the homogeneous transform matrix from base_link to map from the latest ground truth reading + pose = self.latest_odom.pose.pose + p = np.array([pose.position.x, pose.position.y, pose.position.z]) + q = np.array([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]) + rover_in_map = SE3.from_pos_quat(p, q) + rover_to_map = rover_in_map.transform_matrix() + + # calculate the position of each GPS antenna in the map frame + left_gps_in_map = rover_to_map @ np.append(left_gps_offset, 1) + right_gps_in_map = rover_to_map @ np.append(right_gps_offset, 1) + + # convert GPS positions to geodetic coordinates + (left_lat, left_lon, left_alt) = enu2geodetic(*left_gps_in_map[:3], *self.ref_point, deg=True) + (right_lat, right_lon, right_alt) = enu2geodetic(*right_gps_in_map[:3], *self.ref_point, deg=True) + + # publish GPS messages + left_msg = NavSatFix() + left_msg.header.stamp = rospy.Time.now() + left_msg.latitude = left_lat + left_msg.longitude = left_lon + left_msg.altitude = left_alt + self.left_gps_pub.publish(left_msg) + + right_msg = NavSatFix() + right_msg.header.stamp = rospy.Time.now() + right_msg.latitude = right_lat + right_msg.longitude = right_lon + right_msg.altitude = right_alt + self.right_gps_pub.publish(right_msg) + + def run(self): + rate = rospy.Rate(10) + while not rospy.is_shutdown(): + self.publish_gps() + rate.sleep() + +if __name__ == "__main__": + rospy.init_node("temp_gps_sim") + node = GPSSim() + node.run() diff --git a/urdf/rover/rover_gazebo_plugins.urdf.xacro b/urdf/rover/rover_gazebo_plugins.urdf.xacro index d02ed5032..4efe1e841 100644 --- a/urdf/rover/rover_gazebo_plugins.urdf.xacro +++ b/urdf/rover/rover_gazebo_plugins.urdf.xacro @@ -19,7 +19,7 @@ - + 100.0 rover_gps_right_link @@ -52,7 +51,7 @@ 0 0 0 0.0 0.0 0.0 0 0 0 - + --> From e7e62ca7ea41315ba391a3da564f4ae82e6d22de Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Fri, 19 Jan 2024 17:43:21 -0500 Subject: [PATCH 42/81] fixed pose publishing offset temporarily --- src/localization/gps_linearization.py | 7 ++++++- src/localization/temp_gps_sim.py | 1 + urdf/rover/rover.urdf.xacro | 12 ++++++------ 3 files changed, 13 insertions(+), 7 deletions(-) diff --git a/src/localization/gps_linearization.py b/src/localization/gps_linearization.py index 0494d480d..8434f01ae 100755 --- a/src/localization/gps_linearization.py +++ b/src/localization/gps_linearization.py @@ -135,6 +135,7 @@ def publish_pose(self): @staticmethod def compute_gps_pose(right_cartesian, left_cartesian) -> np.ndarray: + # TODO: give simulated GPS non zero altitude so we can stop erasing the z component left_cartesian[2] = 0 right_cartesian[2] = 0 vector_connecting = left_cartesian - right_cartesian @@ -149,8 +150,12 @@ def compute_gps_pose(right_cartesian, left_cartesian) -> np.ndarray: rotation_matrix = np.hstack( (vector_perp, np.reshape(vector_connecting, (3, 1)), np.array(object=[[0], [0], [1]])) ) + + # temporary fix, assumes base_link is exactly in the middle of the two GPS antennas + # TODO: use static tf from base_link to left_antenna instead + rover_position = (left_cartesian + right_cartesian) / 2 - pose = SE3(left_cartesian, SO3.from_matrix(rotation_matrix=rotation_matrix)) + pose = SE3(rover_position, SO3.from_matrix(rotation_matrix=rotation_matrix)) return pose diff --git a/src/localization/temp_gps_sim.py b/src/localization/temp_gps_sim.py index b27d4a01d..a3e752ef6 100755 --- a/src/localization/temp_gps_sim.py +++ b/src/localization/temp_gps_sim.py @@ -24,6 +24,7 @@ def __init__(self): self.ground_truth_sub = rospy.Subscriber("ground_truth", Odometry, self.ground_truth_callback) def ground_truth_callback(self, msg: Odometry): + # don't publish directly from here to reduce rate from 1kHz to 10Hz self.latest_odom = msg def publish_gps(self): diff --git a/urdf/rover/rover.urdf.xacro b/urdf/rover/rover.urdf.xacro index b0d049d3b..ed9de4dbb 100644 --- a/urdf/rover/rover.urdf.xacro +++ b/urdf/rover/rover.urdf.xacro @@ -142,19 +142,19 @@ - - + + - + - - + + - + From b7c97f8b72962a983ccc72da49a0ba5daee12d42 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sat, 20 Jan 2024 13:27:16 -0500 Subject: [PATCH 43/81] Add starter work for gps --- .../files/networking_setup_basestation.sh | 4 +- src/simulator/pch.hpp | 1 + src/simulator/simulator.cpp | 5 +- src/simulator/simulator.gui.cpp | 13 ++++- src/simulator/simulator.hpp | 4 +- src/simulator/simulator.sensors.cpp | 50 +++++++++++++------ urdf/rover/rover.urdf.xacro | 21 +++++--- 7 files changed, 69 insertions(+), 29 deletions(-) diff --git a/ansible/roles/basestation_networks/files/networking_setup_basestation.sh b/ansible/roles/basestation_networks/files/networking_setup_basestation.sh index abb850ecf..2f64797da 100644 --- a/ansible/roles/basestation_networks/files/networking_setup_basestation.sh +++ b/ansible/roles/basestation_networks/files/networking_setup_basestation.sh @@ -1,4 +1,4 @@ #!/usr/bin/env sh -export ROS_MASTER_URI=http://10.0.0.2:11311 -export ROS_IP=10.0.0.7 +export ROS_MASTER_URI=http://10.0.0.10:11311 +export ROS_IP=10.0.0.2 diff --git a/src/simulator/pch.hpp b/src/simulator/pch.hpp index ad1cbdf6c..d0de86fac 100644 --- a/src/simulator/pch.hpp +++ b/src/simulator/pch.hpp @@ -24,6 +24,7 @@ #include #include +#include #include #include #include diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp index aa06b0242..d8b70c19d 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -15,7 +15,10 @@ namespace mrover { mTwistSub = mNh.subscribe("/cmd_vel", 1, &SimulatorNodelet::twistCallback, this); mJointPositionsSub = mNh.subscribe("/arm_position_cmd", 1, &SimulatorNodelet::jointPositionsCallback, this); - mPosePub = mNh.advertise("/linearized_pose", 1); + // mLinearizedPosePub = mNh.advertise("/linearized_pose", 1); + + mLeftGpsPub = mNh.advertise("/right_gps_driver/fix", 1); + mRightGpsPub = mNh.advertise("/left_gps_driver/fix", 1); mIkTargetPub = mNh.advertise("/arm_ik", 1); diff --git a/src/simulator/simulator.gui.cpp b/src/simulator/simulator.gui.cpp index ea4a38bb7..99c38e3c4 100644 --- a/src/simulator/simulator.gui.cpp +++ b/src/simulator/simulator.gui.cpp @@ -2,6 +2,10 @@ namespace mrover { + auto r3ToBtVector3(R3 const& r3) -> btVector3 { + return btVector3{static_cast(r3.x()), static_cast(r3.y()), static_cast(r3.z())}; + } + auto SimulatorNodelet::guiUpdate(wgpu::RenderPassEncoder& pass) -> void { if (mSaveTask.shouldUpdate() && mEnablePhysics) { if (auto it = mUrdfs.find("rover"); it != mUrdfs.end()) { @@ -31,7 +35,7 @@ namespace mrover { if (auto it = mUrdfs.find("rover"); it != mUrdfs.end()) { URDF const& rover = it->second; - const auto& [baseTransform, baseVelocity, links] = mSaveHistory.at(mSaveSelection + historySize); + auto const& [baseTransform, baseVelocity, links] = mSaveHistory.at(mSaveSelection + historySize); rover.physics->setBaseWorldTransform(baseTransform); rover.physics->setBaseVel(baseVelocity); for (int link = 0; link < rover.physics->getNumLinks(); ++link) { @@ -109,6 +113,13 @@ namespace mrover { // } } + { + R3 rayStart = mCameraInWorld.position(); + R3 rayEnd = rayStart + mCameraInWorld.rotation().quaternion() * R3{100, 0, 0}; + // btMul + // mDynamicsWorld->rayTest(r3ToBtVector3(rayStart), r3ToBtVector3(rayEnd), [ + } + for (Camera const& camera: mCameras) { float aspect = static_cast(camera.resolution.x()) / static_cast(camera.resolution.y()); ImGui::Image(camera.colorTextureView, {320, 320 / aspect}, {0, 0}, {1, 1}); diff --git a/src/simulator/simulator.hpp b/src/simulator/simulator.hpp index 88bfd8572..74d4c76fe 100644 --- a/src/simulator/simulator.hpp +++ b/src/simulator/simulator.hpp @@ -192,7 +192,9 @@ namespace mrover { ros::Subscriber mTwistSub, mJointPositionsSub; - ros::Publisher mPosePub; + ros::Publisher mLinearizedPosePub; + ros::Publisher mLeftGpsPub; + ros::Publisher mRightGpsPub; tf2_ros::Buffer mTfBuffer; tf2_ros::TransformListener mTfListener{mTfBuffer}; diff --git a/src/simulator/simulator.sensors.cpp b/src/simulator/simulator.sensors.cpp index 066b8202f..0af5f936f 100644 --- a/src/simulator/simulator.sensors.cpp +++ b/src/simulator/simulator.sensors.cpp @@ -91,26 +91,44 @@ namespace mrover { } } + auto computeNavSatFix(SE3 const& gpuInMap) -> sensor_msgs::NavSatFix { + sensor_msgs::NavSatFix gps; + gps.header.stamp = ros::Time::now(); + gps.header.frame_id = "map"; + // TODO(riley): fill in values + return gps; + } + auto SimulatorNodelet::gpsAndImusUpdate() -> void { if (auto lookup = getUrdf("rover"); lookup) { URDF const& rover = *lookup; - SE3 baseLinkInMap = rover.linkInWorld("base_link"); - - geometry_msgs::PoseWithCovarianceStamped pose; - pose.header.stamp = ros::Time::now(); - pose.header.frame_id = "map"; - R3 p = baseLinkInMap.position(); - pose.pose.pose.position.x = p.x(); - pose.pose.pose.position.y = p.y(); - pose.pose.pose.position.z = p.z(); - S3 q = baseLinkInMap.rotation().quaternion(); - pose.pose.pose.orientation.w = q.w(); - pose.pose.pose.orientation.x = q.x(); - pose.pose.pose.orientation.y = q.y(); - pose.pose.pose.orientation.z = q.z(); - // TODO(quintin, riley): fill in covariance - mPosePub.publish(pose); + if (mLinearizedPosePub) { + SE3 baseLinkInMap = rover.linkInWorld("base_link"); + geometry_msgs::PoseWithCovarianceStamped pose; + pose.header.stamp = ros::Time::now(); + pose.header.frame_id = "map"; + R3 p = baseLinkInMap.position(); + pose.pose.pose.position.x = p.x(); + pose.pose.pose.position.y = p.y(); + pose.pose.pose.position.z = p.z(); + S3 q = baseLinkInMap.rotation().quaternion(); + pose.pose.pose.orientation.w = q.w(); + pose.pose.pose.orientation.x = q.x(); + pose.pose.pose.orientation.y = q.y(); + pose.pose.pose.orientation.z = q.z(); + // TODO(quintin, riley): fill in covariance + mLinearizedPosePub.publish(pose); + } + + if (mLeftGpsPub) { + SE3 leftGpsInMap = rover.linkInWorld("left_gps"); + mLeftGpsPub.publish(computeNavSatFix(leftGpsInMap)); + } + if (mRightGpsPub) { + SE3 rightGpsInMap = rover.linkInWorld("right_gps"); + mRightGpsPub.publish(computeNavSatFix(rightGpsInMap)); + } } } diff --git a/urdf/rover/rover.urdf.xacro b/urdf/rover/rover.urdf.xacro index b08c0f46d..8dcffbcc7 100644 --- a/urdf/rover/rover.urdf.xacro +++ b/urdf/rover/rover.urdf.xacro @@ -237,16 +237,21 @@ - + + + + + + + - - - - - - + + + + + - + From aee2d8c9748d9558a7fc8ca002f63efafc77f47b Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Sat, 20 Jan 2024 13:57:09 -0500 Subject: [PATCH 44/81] cartesian to geodetic conversion --- src/simulator/simulator.sensors.cpp | 35 ++++++++++++++++++++++++----- 1 file changed, 29 insertions(+), 6 deletions(-) diff --git a/src/simulator/simulator.sensors.cpp b/src/simulator/simulator.sensors.cpp index 0af5f936f..de13b8260 100644 --- a/src/simulator/simulator.sensors.cpp +++ b/src/simulator/simulator.sensors.cpp @@ -91,12 +91,35 @@ namespace mrover { } } - auto computeNavSatFix(SE3 const& gpuInMap) -> sensor_msgs::NavSatFix { - sensor_msgs::NavSatFix gps; - gps.header.stamp = ros::Time::now(); - gps.header.frame_id = "map"; - // TODO(riley): fill in values - return gps; + Eigen::Vector3d cartesian_to_geodetic(R3 const& cartesian, Eigen::Vector3d const& ref_geodetic, double ref_heading) { + constexpr double equatorial_radius = 6378137.0; + constexpr double flattening = 1.0 / 298.257223563; + constexpr double eccentricity2 = 2 * flattening - flattening * flattening; + using std::sin, std::cos, std::pow, std::sqrt, std::numbers::pi; + + auto lat0 = ref_geodetic(0); + auto lon0 = ref_geodetic(1); + auto h0 = ref_geodetic(2); + double temp = 1.0 / (1.0 - eccentricity2 * sin(lat0 * pi / 180.0) * sin(lat0 * pi / 180.0)); + double prime_vertical_radius = equatorial_radius * sqrt(temp); + double radius_north = prime_vertical_radius * (1 - eccentricity2) * temp; + double radius_east = prime_vertical_radius * cos(lat0 * pi / 180.0); + + double lat = lat0 + (cos(ref_heading) * cartesian.x() + sin(ref_heading) * cartesian.y()) / radius_north * 180.0 / pi; + double lon = lon0 - (-sin(ref_heading) * cartesian.x() + cos(ref_heading) * cartesian.y()) / radius_east * 180.0 / pi; + double alt = h0 + cartesian.z(); + return {lat, lon, alt}; + } + + auto computeNavSatFix(SE3 const& gpuInMap, Eigen::Vector3d const& ref_geodetic, double ref_heading) -> sensor_msgs::NavSatFix { + sensor_msgs::NavSatFix gps_msg; + gps_msg.header.stamp = ros::Time::now(); + gps_msg.header.frame_id = "map"; + auto geodetic = cartesian_to_geodetic(gpuInMap.position(), ref_geodetic, ref_heading); + gps_msg.latitude = geodetic(0); + gps_msg.longitude = geodetic(1); + gps_msg.altitude = geodetic(2); + return gps_msg; } auto SimulatorNodelet::gpsAndImusUpdate() -> void { From 12a9710177748d776d187011f125b7310919f562 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sat, 20 Jan 2024 14:33:46 -0500 Subject: [PATCH 45/81] Finish off gps and add imu --- config/localization.yaml | 1 + launch/new_sim.launch | 2 +- src/simulator/pch.hpp | 2 +- src/simulator/simulator.cpp | 2 + src/simulator/simulator.gui.cpp | 2 +- src/simulator/simulator.hpp | 11 +++- src/simulator/simulator.params.cpp | 37 ++++++++---- src/simulator/simulator.physics.cpp | 2 +- src/simulator/simulator.sensors.cpp | 90 ++++++++++++++++++++--------- src/util/lie/se3.hpp | 2 +- src/util/lie/so3.cpp | 6 +- 11 files changed, 108 insertions(+), 49 deletions(-) diff --git a/config/localization.yaml b/config/localization.yaml index 2d2654e34..000c48ad8 100644 --- a/config/localization.yaml +++ b/config/localization.yaml @@ -7,3 +7,4 @@ gps_linearization: reference_point_latitude: 42.293195 reference_point_longitude: -83.7096706 reference_point_altitude: 234.1 + reference_heading: 90.0 diff --git a/launch/new_sim.launch b/launch/new_sim.launch index 19b3e6d0c..a712a559e 100644 --- a/launch/new_sim.launch +++ b/launch/new_sim.launch @@ -4,7 +4,7 @@ - + #include #include - #include #include #include @@ -72,3 +71,4 @@ #include #include +#include diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp index d8b70c19d..db2a0b22a 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -19,6 +19,8 @@ namespace mrover { mLeftGpsPub = mNh.advertise("/right_gps_driver/fix", 1); mRightGpsPub = mNh.advertise("/left_gps_driver/fix", 1); + mGpsTask = PeriodicTask{mPnh.param("gps_rate", 10)}; + mImuTask = PeriodicTask{mPnh.param("imu_rate", 100)}; mIkTargetPub = mNh.advertise("/arm_ik", 1); diff --git a/src/simulator/simulator.gui.cpp b/src/simulator/simulator.gui.cpp index 99c38e3c4..1574658a8 100644 --- a/src/simulator/simulator.gui.cpp +++ b/src/simulator/simulator.gui.cpp @@ -115,7 +115,7 @@ namespace mrover { { R3 rayStart = mCameraInWorld.position(); - R3 rayEnd = rayStart + mCameraInWorld.rotation().quaternion() * R3{100, 0, 0}; + R3 rayEnd = rayStart + mCameraInWorld.rotation().matrix().col(0); // btMul // mDynamicsWorld->rayTest(r3ToBtVector3(rayStart), r3ToBtVector3(rayEnd), [ } diff --git a/src/simulator/simulator.hpp b/src/simulator/simulator.hpp index 74d4c76fe..dc7904a12 100644 --- a/src/simulator/simulator.hpp +++ b/src/simulator/simulator.hpp @@ -195,6 +195,7 @@ namespace mrover { ros::Publisher mLinearizedPosePub; ros::Publisher mLeftGpsPub; ros::Publisher mRightGpsPub; + ros::Publisher mImuPub; tf2_ros::Buffer mTfBuffer; tf2_ros::TransformListener mTfListener{mTfBuffer}; @@ -203,6 +204,12 @@ namespace mrover { Eigen::Vector3f mIkTarget{1.0, 0.1, 0}; ros::Publisher mIkTargetPub; + R3 mGpsLinerizationReferencePoint{}; + double mGpsLinerizationReferenceHeading{}; + + PeriodicTask mGpsTask; + PeriodicTask mImuTask; + // Rendering GlfwInstance mGlfwInstance; @@ -271,6 +278,8 @@ namespace mrover { return rawPointer; } + R3 mRoverLinearVelocity{}; + // Scene std::unordered_map mUrdfs; @@ -297,7 +306,7 @@ namespace mrover { auto cameraUpdate(Camera& camera, wgpu::CommandEncoder& encoder, wgpu::RenderPassDescriptor const& passDescriptor) -> void; - auto gpsAndImusUpdate() -> void; + auto gpsAndImusUpdate(Clock::duration dt) -> void; auto linksToTfUpdate() -> void; diff --git a/src/simulator/simulator.params.cpp b/src/simulator/simulator.params.cpp index 803846f92..70ddb663b 100644 --- a/src/simulator/simulator.params.cpp +++ b/src/simulator/simulator.params.cpp @@ -3,24 +3,39 @@ namespace mrover { auto SimulatorNodelet::parseParams() -> void { - XmlRpc::XmlRpcValue objects; - mNh.getParam("objects", objects); - if (objects.getType() != XmlRpc::XmlRpcValue::TypeArray) throw std::invalid_argument{"objects must be an array"}; + { + XmlRpc::XmlRpcValue objects; + mNh.getParam("objects", objects); + if (objects.getType() != XmlRpc::XmlRpcValue::TypeArray) throw std::invalid_argument{"URDFs to load must be an array. Did you rosparam load a simulator config file properly?"}; - for (int i = 0; i < objects.size(); ++i) { // NOLINT(*-loop-convert) - XmlRpc::XmlRpcValue const& object = objects[i]; + for (int i = 0; i < objects.size(); ++i) { // NOLINT(*-loop-convert) + XmlRpc::XmlRpcValue const& object = objects[i]; - auto type = xmlRpcValueToTypeOrDefault(object, "type"); - auto name = xmlRpcValueToTypeOrDefault(object, "name", ""); + auto type = xmlRpcValueToTypeOrDefault(object, "type"); + auto name = xmlRpcValueToTypeOrDefault(object, "name", ""); - NODELET_INFO_STREAM(std::format("Loading object: {} of type: {}", name, type)); + NODELET_INFO_STREAM(std::format("Loading object: {} of type: {}", name, type)); - if (type == "urdf") { - if (auto [_, was_added] = mUrdfs.try_emplace(name, *this, object); !was_added) { - throw std::invalid_argument{std::format("Duplicate object name: {}", name)}; + if (type == "urdf") { + if (auto [_, was_added] = mUrdfs.try_emplace(name, *this, object); !was_added) { + throw std::invalid_argument{std::format("Duplicate object name: {}", name)}; + } } } } + { + XmlRpc::XmlRpcValue gpsLinearization; + mNh.getParam("gps_linearization", gpsLinearization); + if (gpsLinearization.getType() != XmlRpc::XmlRpcValue::TypeStruct) throw std::invalid_argument{"GPS lineraization must be a struct. Did you rosparam load a simulator config file properly?"}; + + mGpsLinerizationReferencePoint = { + xmlRpcValueToTypeOrDefault(gpsLinearization, "reference_point_latitude"), + xmlRpcValueToTypeOrDefault(gpsLinearization, "reference_point_longitude"), + xmlRpcValueToTypeOrDefault(gpsLinearization, "reference_point_altitude"), + }; + mGpsLinerizationReferenceHeading = xmlRpcValueToTypeOrDefault(gpsLinearization, "reference_heading"); + ROS_INFO_STREAM("Point:" << mGpsLinerizationReferencePoint); + } } } // namespace mrover diff --git a/src/simulator/simulator.physics.cpp b/src/simulator/simulator.physics.cpp index 9172abadc..fd178f5ac 100644 --- a/src/simulator/simulator.physics.cpp +++ b/src/simulator/simulator.physics.cpp @@ -63,7 +63,7 @@ namespace mrover { linksToTfUpdate(); - gpsAndImusUpdate(); + gpsAndImusUpdate(dt); } auto SimulatorNodelet::linksToTfUpdate() -> void { diff --git a/src/simulator/simulator.sensors.cpp b/src/simulator/simulator.sensors.cpp index de13b8260..2b692664f 100644 --- a/src/simulator/simulator.sensors.cpp +++ b/src/simulator/simulator.sensors.cpp @@ -91,38 +91,64 @@ namespace mrover { } } - Eigen::Vector3d cartesian_to_geodetic(R3 const& cartesian, Eigen::Vector3d const& ref_geodetic, double ref_heading) { - constexpr double equatorial_radius = 6378137.0; + auto cartesianToGeodetic(R3 const& cartesian, Eigen::Vector3d const& referenceGeodetic, double referenceHeadingDegrees) -> Eigen::Vector3d { + constexpr double equatorialRadius = 6378137.0; constexpr double flattening = 1.0 / 298.257223563; constexpr double eccentricity2 = 2 * flattening - flattening * flattening; - using std::sin, std::cos, std::pow, std::sqrt, std::numbers::pi; - - auto lat0 = ref_geodetic(0); - auto lon0 = ref_geodetic(1); - auto h0 = ref_geodetic(2); - double temp = 1.0 / (1.0 - eccentricity2 * sin(lat0 * pi / 180.0) * sin(lat0 * pi / 180.0)); - double prime_vertical_radius = equatorial_radius * sqrt(temp); - double radius_north = prime_vertical_radius * (1 - eccentricity2) * temp; - double radius_east = prime_vertical_radius * cos(lat0 * pi / 180.0); - - double lat = lat0 + (cos(ref_heading) * cartesian.x() + sin(ref_heading) * cartesian.y()) / radius_north * 180.0 / pi; - double lon = lon0 - (-sin(ref_heading) * cartesian.x() + cos(ref_heading) * cartesian.y()) / radius_east * 180.0 / pi; + using std::sin, std::cos, std::pow, std::sqrt; + + double lat0 = referenceGeodetic(0) * DEG_TO_RAD; + double lon0 = referenceGeodetic(1) * DEG_TO_RAD; + double h0 = referenceGeodetic(2); + double temp = 1.0 / (1.0 - eccentricity2 * sin(lat0) * sin(lat0)); + double primeVerticalRadius = equatorialRadius * sqrt(temp); + double radiusNorth = primeVerticalRadius * (1 - eccentricity2) * temp; + double radiusEast = primeVerticalRadius * cos(lat0); + + double referenceHeadingRadians = referenceHeadingDegrees * DEG_TO_RAD; + double lat = lat0 + (cos(referenceHeadingRadians) * cartesian.x() + sin(referenceHeadingRadians) * cartesian.y()) / radiusNorth / DEG_TO_RAD; + double lon = lon0 - (-sin(referenceHeadingRadians) * cartesian.x() + cos(referenceHeadingRadians) * cartesian.y()) / radiusEast / DEG_TO_RAD; double alt = h0 + cartesian.z(); return {lat, lon, alt}; } - auto computeNavSatFix(SE3 const& gpuInMap, Eigen::Vector3d const& ref_geodetic, double ref_heading) -> sensor_msgs::NavSatFix { - sensor_msgs::NavSatFix gps_msg; - gps_msg.header.stamp = ros::Time::now(); - gps_msg.header.frame_id = "map"; - auto geodetic = cartesian_to_geodetic(gpuInMap.position(), ref_geodetic, ref_heading); - gps_msg.latitude = geodetic(0); - gps_msg.longitude = geodetic(1); - gps_msg.altitude = geodetic(2); - return gps_msg; + auto computeNavSatFix(SE3 const& gpuInMap, Eigen::Vector3d const& referenceGeodetic, double referenceHeadingDegrees) -> sensor_msgs::NavSatFix { + sensor_msgs::NavSatFix gpsMessage; + gpsMessage.header.stamp = ros::Time::now(); + gpsMessage.header.frame_id = "map"; + auto geodetic = cartesianToGeodetic(gpuInMap.position(), referenceGeodetic, referenceHeadingDegrees); + gpsMessage.latitude = geodetic(0); + gpsMessage.longitude = geodetic(1); + gpsMessage.altitude = geodetic(2); + return gpsMessage; } - auto SimulatorNodelet::gpsAndImusUpdate() -> void { + auto computeImu(SE3 const& imuInMap, R3 const& imuAngularVelocity, R3 const& linearAcceleration, R3 const& magneticField) -> ImuAndMag { + ImuAndMag imuMessage; + imuMessage.header.stamp = ros::Time::now(); + imuMessage.header.frame_id = "map"; + S3 q = imuInMap.rotation().quaternion(); + imuMessage.imu.orientation.w = q.w(); + imuMessage.imu.orientation.x = q.x(); + imuMessage.imu.orientation.y = q.y(); + imuMessage.imu.orientation.z = q.z(); + imuMessage.imu.angular_velocity.x = imuAngularVelocity.x(); + imuMessage.imu.angular_velocity.y = imuAngularVelocity.y(); + imuMessage.imu.angular_velocity.z = imuAngularVelocity.z(); + imuMessage.imu.linear_acceleration.x = linearAcceleration.x(); + imuMessage.imu.linear_acceleration.y = linearAcceleration.y(); + imuMessage.imu.linear_acceleration.z = linearAcceleration.z(); + imuMessage.mag.magnetic_field.x = magneticField.x(); + imuMessage.mag.magnetic_field.y = magneticField.y(); + imuMessage.mag.magnetic_field.z = magneticField.z(); + return imuMessage; + } + + auto btVector3ToR3(btVector3 const& v) -> R3 { + return {v.x(), v.y(), v.z()}; + } + + auto SimulatorNodelet::gpsAndImusUpdate(Clock::duration dt) -> void { if (auto lookup = getUrdf("rover"); lookup) { URDF const& rover = *lookup; @@ -144,13 +170,21 @@ namespace mrover { mLinearizedPosePub.publish(pose); } - if (mLeftGpsPub) { + if (mLeftGpsPub && mGpsTask.shouldUpdate()) { SE3 leftGpsInMap = rover.linkInWorld("left_gps"); - mLeftGpsPub.publish(computeNavSatFix(leftGpsInMap)); + mLeftGpsPub.publish(computeNavSatFix(leftGpsInMap, mGpsLinerizationReferencePoint, mGpsLinerizationReferenceHeading)); } - if (mRightGpsPub) { + if (mRightGpsPub && mGpsTask.shouldUpdate()) { SE3 rightGpsInMap = rover.linkInWorld("right_gps"); - mRightGpsPub.publish(computeNavSatFix(rightGpsInMap)); + mRightGpsPub.publish(computeNavSatFix(rightGpsInMap, mGpsLinerizationReferencePoint, mGpsLinerizationReferenceHeading)); + } + if (mImuPub && mImuTask.shouldUpdate()) { + R3 imuAngularVelocity = btVector3ToR3(rover.physics->getBaseOmega()); + R3 roverLinearVelocity = btVector3ToR3(rover.physics->getBaseVel()); + R3 imuLinearAcceleration = (roverLinearVelocity - mRoverLinearVelocity) / std::chrono::duration_cast>(dt).count(); + mRoverLinearVelocity = roverLinearVelocity; + SE3 imuInMap = rover.linkInWorld("imu"); + mImuPub.publish(computeImu(imuInMap, imuAngularVelocity, imuLinearAcceleration, imuInMap.rotation().matrix().transpose().col(1))); } } } diff --git a/src/util/lie/se3.hpp b/src/util/lie/se3.hpp index 3babf71f6..923192188 100644 --- a/src/util/lie/se3.hpp +++ b/src/util/lie/se3.hpp @@ -40,7 +40,7 @@ class SO3 { [[nodiscard]] R3 operator*(R3 const& other) const; - [[nodiscard]] Eigen::Matrix4d matrix() const; + [[nodiscard]] Eigen::Matrix3d matrix() const; [[nodiscard]] Eigen::Quaterniond quaternion() const; diff --git a/src/util/lie/so3.cpp b/src/util/lie/so3.cpp index d7b8cd72a..422cd5c42 100644 --- a/src/util/lie/so3.cpp +++ b/src/util/lie/so3.cpp @@ -4,10 +4,8 @@ Eigen::Quaterniond SO3::quaternion() const { return Eigen::Quaterniond{mAngleAxis}; } -Eigen::Matrix4d SO3::matrix() const { - Eigen::Matrix4d matrix = Eigen::Matrix4d::Identity(); - matrix.block<3, 3>(0, 0) = mAngleAxis.toRotationMatrix(); - return matrix; +Eigen::Matrix3d SO3::matrix() const { + return mAngleAxis.toRotationMatrix(); } SO3 SO3::operator*(SO3 const& other) const { From ad25d424b2bddba639fa22ba4fb860c8171bf195 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sat, 20 Jan 2024 15:27:35 -0500 Subject: [PATCH 46/81] Yo workingin the new sim --- launch/new_sim.launch | 13 +- scripts/build_dawn.sh | 2 +- src/simulator/simulator.cpp | 5 +- src/simulator/simulator.params.cpp | 1 - src/simulator/simulator.sensors.cpp | 11 +- urdf/rover/rover.urdf.xacro | 237 +++++++++++++++------------- 6 files changed, 140 insertions(+), 129 deletions(-) diff --git a/launch/new_sim.launch b/launch/new_sim.launch index a712a559e..992a82485 100644 --- a/launch/new_sim.launch +++ b/launch/new_sim.launch @@ -1,14 +1,13 @@ - + - - - + + + args="load mrover/SimulatorNodelet nodelet_manager" output="screen" /> - - + + \ No newline at end of file diff --git a/scripts/build_dawn.sh b/scripts/build_dawn.sh index 420b5704f..6094878b3 100755 --- a/scripts/build_dawn.sh +++ b/scripts/build_dawn.sh @@ -1,5 +1,5 @@ #!/bin/bash pushd deps/dawn || exit -CC=clang CXX=clang++ cmake -B out/Release -G Ninja -D DAWN_FETCH_DEPENDENCIES=ON -D DAWN_ENABLE_DESKTOP_GL=OFF -D DAWN_ENABLE_OPENGLES=OFF -D DAWN_ENABLE_NULL=OFF -D DAWN_BUILD_SAMPLES=OFF -D TINT_BUILD_DOCS=OFF -D TINT_BUILD_TESTS=OFF -D DAWN_ENABLE_INSTALL=ON -D BUILD_SHARED_LIBS=ON +CC=clang CXX=clang++ cmake -B out/Release -G Ninja -D CMAKE_BUILD_TYPE=Release -D DAWN_FETCH_DEPENDENCIES=ON -D DAWN_ENABLE_DESKTOP_GL=OFF -D DAWN_ENABLE_OPENGLES=OFF -D DAWN_ENABLE_NULL=OFF -D DAWN_BUILD_SAMPLES=OFF -D TINT_BUILD_DOCS=OFF -D TINT_BUILD_TESTS=OFF -D DAWN_ENABLE_INSTALL=ON -D BUILD_SHARED_LIBS=ON cmake --build out/Release diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp index db2a0b22a..b5dc271a6 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -17,8 +17,9 @@ namespace mrover { // mLinearizedPosePub = mNh.advertise("/linearized_pose", 1); - mLeftGpsPub = mNh.advertise("/right_gps_driver/fix", 1); - mRightGpsPub = mNh.advertise("/left_gps_driver/fix", 1); + mLeftGpsPub = mNh.advertise("/left_gps_driver/fix", 1); + mRightGpsPub = mNh.advertise("/right_gps_driver/fix", 1); + mImuPub = mNh.advertise("/imu", 1); mGpsTask = PeriodicTask{mPnh.param("gps_rate", 10)}; mImuTask = PeriodicTask{mPnh.param("imu_rate", 100)}; diff --git a/src/simulator/simulator.params.cpp b/src/simulator/simulator.params.cpp index 70ddb663b..48dabcc53 100644 --- a/src/simulator/simulator.params.cpp +++ b/src/simulator/simulator.params.cpp @@ -34,7 +34,6 @@ namespace mrover { xmlRpcValueToTypeOrDefault(gpsLinearization, "reference_point_altitude"), }; mGpsLinerizationReferenceHeading = xmlRpcValueToTypeOrDefault(gpsLinearization, "reference_heading"); - ROS_INFO_STREAM("Point:" << mGpsLinerizationReferencePoint); } } diff --git a/src/simulator/simulator.sensors.cpp b/src/simulator/simulator.sensors.cpp index 2b692664f..802f17cc1 100644 --- a/src/simulator/simulator.sensors.cpp +++ b/src/simulator/simulator.sensors.cpp @@ -106,8 +106,8 @@ namespace mrover { double radiusEast = primeVerticalRadius * cos(lat0); double referenceHeadingRadians = referenceHeadingDegrees * DEG_TO_RAD; - double lat = lat0 + (cos(referenceHeadingRadians) * cartesian.x() + sin(referenceHeadingRadians) * cartesian.y()) / radiusNorth / DEG_TO_RAD; - double lon = lon0 - (-sin(referenceHeadingRadians) * cartesian.x() + cos(referenceHeadingRadians) * cartesian.y()) / radiusEast / DEG_TO_RAD; + double lat = (lat0 + (cos(referenceHeadingRadians) * cartesian.x() + sin(referenceHeadingRadians) * cartesian.y()) / radiusNorth) / DEG_TO_RAD; + double lon = (lon0 - (-sin(referenceHeadingRadians) * cartesian.x() + cos(referenceHeadingRadians) * cartesian.y()) / radiusEast) / DEG_TO_RAD; double alt = h0 + cartesian.z(); return {lat, lon, alt}; } @@ -170,15 +170,14 @@ namespace mrover { mLinearizedPosePub.publish(pose); } - if (mLeftGpsPub && mGpsTask.shouldUpdate()) { + if (mGpsTask.shouldUpdate()) { SE3 leftGpsInMap = rover.linkInWorld("left_gps"); mLeftGpsPub.publish(computeNavSatFix(leftGpsInMap, mGpsLinerizationReferencePoint, mGpsLinerizationReferenceHeading)); - } - if (mRightGpsPub && mGpsTask.shouldUpdate()) { + SE3 rightGpsInMap = rover.linkInWorld("right_gps"); mRightGpsPub.publish(computeNavSatFix(rightGpsInMap, mGpsLinerizationReferencePoint, mGpsLinerizationReferenceHeading)); } - if (mImuPub && mImuTask.shouldUpdate()) { + if (mImuTask.shouldUpdate()) { R3 imuAngularVelocity = btVector3ToR3(rover.physics->getBaseOmega()); R3 roverLinearVelocity = btVector3ToR3(rover.physics->getBaseVel()); R3 imuLinearAcceleration = (roverLinearVelocity - mRoverLinearVelocity) / std::chrono::duration_cast>(dt).count(); diff --git a/urdf/rover/rover.urdf.xacro b/urdf/rover/rover.urdf.xacro index c1fc34b9b..0ae989d82 100644 --- a/urdf/rover/rover.urdf.xacro +++ b/urdf/rover/rover.urdf.xacro @@ -3,43 +3,43 @@ - - - - - - - - + + + + + + + + - + - + + iyy="${m*(3*r*r+h*h)/12}" iyz="0" + izz="${m*r*r/2}"/> - + + iyy="${m / 12.0 * (d*d + h*h)}" iyz="0" + izz="${m / 12.0 * (d*d + w*w)}"/> - + + iyy="${2.0*m*(r*r)/5.0}" iyz="0" + izz="${2.0*m*(r*r)/5.0}"/> @@ -47,16 +47,16 @@ - + - + - + - + @@ -64,202 +64,215 @@ - - - - - + + + + + - + - + - + - - - + + + - + - + - + - + - + - + - + - + - + - + - + - + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - + - + - + - + - + - + - + - - - + + + - - - - - + + + + + - - - - - + + + + + + x="0.034346" y="0" z="0.049024" + lower="${-TAU / 8}" upper="0"/> + x="0.534365" y="0" z="0.009056" + lower="0" upper="${TAU / 2}"/> + x="0.546033" y="0" z="0.088594" + lower="0" upper="0"/> + x="0.044886" y="0" z="0" + lower="0" upper="0"/> - - + + - + + + + + + + + + + + + + + + - - - - - - + + + + + - + - - - + + + - + From 39ec2cd0bbeb6b96fc5867742bf531c68e8fa00f Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sat, 20 Jan 2024 16:15:51 -0500 Subject: [PATCH 47/81] Submodule update for dawn --- scripts/build_dawn.sh | 2 ++ 1 file changed, 2 insertions(+) diff --git a/scripts/build_dawn.sh b/scripts/build_dawn.sh index 6094878b3..b6b1fc7b6 100755 --- a/scripts/build_dawn.sh +++ b/scripts/build_dawn.sh @@ -1,5 +1,7 @@ #!/bin/bash +git submodule update --init deps/dawn + pushd deps/dawn || exit CC=clang CXX=clang++ cmake -B out/Release -G Ninja -D CMAKE_BUILD_TYPE=Release -D DAWN_FETCH_DEPENDENCIES=ON -D DAWN_ENABLE_DESKTOP_GL=OFF -D DAWN_ENABLE_OPENGLES=OFF -D DAWN_ENABLE_NULL=OFF -D DAWN_BUILD_SAMPLES=OFF -D TINT_BUILD_DOCS=OFF -D TINT_BUILD_TESTS=OFF -D DAWN_ENABLE_INSTALL=ON -D BUILD_SHARED_LIBS=ON cmake --build out/Release From 594cffc6a28c08ac70be61750ec69f9d18d666f8 Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Sat, 20 Jan 2024 19:19:14 -0500 Subject: [PATCH 48/81] added rviz to launch file, misc cleanup --- launch/new_sim.launch | 3 + src/localization/gps_linearization.py | 8 +- src/simulator/simulator.sensors.cpp | 4 +- urdf/rover/rover.urdf.xacro | 232 +++++++++++++------------- 4 files changed, 125 insertions(+), 122 deletions(-) diff --git a/launch/new_sim.launch b/launch/new_sim.launch index 992a82485..0b36b3f15 100644 --- a/launch/new_sim.launch +++ b/launch/new_sim.launch @@ -1,4 +1,5 @@ + @@ -6,6 +7,8 @@ + + diff --git a/src/localization/gps_linearization.py b/src/localization/gps_linearization.py index 8434f01ae..0d5364981 100755 --- a/src/localization/gps_linearization.py +++ b/src/localization/gps_linearization.py @@ -136,10 +136,10 @@ def publish_pose(self): @staticmethod def compute_gps_pose(right_cartesian, left_cartesian) -> np.ndarray: # TODO: give simulated GPS non zero altitude so we can stop erasing the z component - left_cartesian[2] = 0 - right_cartesian[2] = 0 + # left_cartesian[2] = 0 + # right_cartesian[2] = 0 vector_connecting = left_cartesian - right_cartesian - # vector_connecting[2] = 0 + vector_connecting[2] = 0 magnitude = np.linalg.norm(vector_connecting) vector_connecting = vector_connecting / magnitude @@ -150,7 +150,7 @@ def compute_gps_pose(right_cartesian, left_cartesian) -> np.ndarray: rotation_matrix = np.hstack( (vector_perp, np.reshape(vector_connecting, (3, 1)), np.array(object=[[0], [0], [1]])) ) - + # temporary fix, assumes base_link is exactly in the middle of the two GPS antennas # TODO: use static tf from base_link to left_antenna instead rover_position = (left_cartesian + right_cartesian) / 2 diff --git a/src/simulator/simulator.sensors.cpp b/src/simulator/simulator.sensors.cpp index 802f17cc1..24eb27fbe 100644 --- a/src/simulator/simulator.sensors.cpp +++ b/src/simulator/simulator.sensors.cpp @@ -112,11 +112,11 @@ namespace mrover { return {lat, lon, alt}; } - auto computeNavSatFix(SE3 const& gpuInMap, Eigen::Vector3d const& referenceGeodetic, double referenceHeadingDegrees) -> sensor_msgs::NavSatFix { + auto computeNavSatFix(SE3 const& gpsInMap, Eigen::Vector3d const& referenceGeodetic, double referenceHeadingDegrees) -> sensor_msgs::NavSatFix { sensor_msgs::NavSatFix gpsMessage; gpsMessage.header.stamp = ros::Time::now(); gpsMessage.header.frame_id = "map"; - auto geodetic = cartesianToGeodetic(gpuInMap.position(), referenceGeodetic, referenceHeadingDegrees); + auto geodetic = cartesianToGeodetic(gpsInMap.position(), referenceGeodetic, referenceHeadingDegrees); gpsMessage.latitude = geodetic(0); gpsMessage.longitude = geodetic(1); gpsMessage.altitude = geodetic(2); diff --git a/urdf/rover/rover.urdf.xacro b/urdf/rover/rover.urdf.xacro index 0ae989d82..5adfa4b2c 100644 --- a/urdf/rover/rover.urdf.xacro +++ b/urdf/rover/rover.urdf.xacro @@ -3,43 +3,43 @@ - - - - - - - - + + + + + + + + - + - + + iyy="${m*(3*r*r+h*h)/12}" iyz="0" + izz="${m*r*r/2}" /> - + + iyy="${m / 12.0 * (d*d + h*h)}" iyz="0" + izz="${m / 12.0 * (d*d + w*w)}" /> - + + iyy="${2.0*m*(r*r)/5.0}" iyz="0" + izz="${2.0*m*(r*r)/5.0}" /> @@ -47,16 +47,16 @@ - + - + - + - + @@ -64,215 +64,215 @@ - - - - - + + + + + - + - + - + - - - + + + - + - + - + - + - + - + - + - + - + - + - + - + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - - - - - + + + + + - + - + - + - + - + - + - + - - - + + + - - - - - + + + + + - - - - - + + + + + + x="0.034346" y="0" z="0.049024" + lower="${-TAU / 8}" upper="0" /> + x="0.534365" y="0" z="0.009056" + lower="0" upper="${TAU / 2}" /> + x="0.546033" y="0" z="0.088594" + lower="0" upper="0" /> + x="0.044886" y="0" z="0" + lower="0" upper="0" /> - - + + - - - + + + - + - - - + + + - + - - - + + + - + - - - + + + - + From 9aabf039704eec4fd204256c7fba47eec73faeaa Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Sat, 20 Jan 2024 22:02:40 -0500 Subject: [PATCH 49/81] refactored utils and tag detector, crashes when it sees a tag --- CMakeLists.txt | 8 ++- src/perception/tag_detector/pch.hpp | 2 + src/perception/tag_detector/tag_detector.hpp | 6 +- .../tag_detector/tag_detector.processing.cpp | 10 +-- src/perception/zed_wrapper/pch.hpp | 5 +- src/perception/zed_wrapper/zed_wrapper.cpp | 10 +-- src/simulator/pch.hpp | 5 +- src/util/se3_ros_utils.cpp | 61 +++++++++++++++++++ src/util/se3_ros_utils.hpp | 33 ++++++++++ 9 files changed, 121 insertions(+), 19 deletions(-) create mode 100644 src/util/se3_ros_utils.cpp create mode 100644 src/util/se3_ros_utils.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index b8607ac5e..2a3d11867 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -81,6 +81,7 @@ endif () find_package(OpenCV REQUIRED) find_package(ZED QUIET) find_package(Eigen3 REQUIRED) +find_package(manif REQUIRED) find_package(PkgConfig REQUIRED) pkg_search_module(NetLink libnl-3.0) pkg_search_module(NetLinkRoute libnl-route-3.0) @@ -130,6 +131,7 @@ catkin_package() ## Libraries mrover_add_library(lie src/util/lie/*.cpp src/util/lie) +mrover_add_header_only_library(manif ${manif_INCLUDE_DIRS}) ## ESW @@ -167,7 +169,7 @@ endif () ## Perception mrover_add_nodelet(tag_detector src/perception/tag_detector/*.cpp src/perception/tag_detector src/perception/tag_detector/pch.hpp) -mrover_nodelet_link_libraries(tag_detector opencv_core opencv_objdetect opencv_aruco opencv_imgproc opencv_highgui lie) +mrover_nodelet_link_libraries(tag_detector opencv_core opencv_objdetect opencv_aruco opencv_imgproc opencv_highgui lie manif) if (CUDA_FOUND) mrover_add_library(streaming src/esw/streaming/*.c* src/esw/streaming) @@ -180,7 +182,7 @@ endif () if (ZED_FOUND) mrover_add_nodelet(zed src/perception/zed_wrapper/*.c* src/perception/zed_wrapper src/perception/zed_wrapper/pch.hpp) mrover_nodelet_include_directories(zed ${ZED_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS}) - mrover_nodelet_link_libraries(zed ${ZED_LIBRARIES} ${SPECIAL_OS_LIBS} lie) + mrover_nodelet_link_libraries(zed ${ZED_LIBRARIES} ${SPECIAL_OS_LIBS} lie manif) mrover_nodelet_defines(zed ALLOW_BUILD_DEBUG # Ignore ZED warnings about Debug mode __CUDA_INCLUDE_COMPILER_INTERNAL_HEADERS__ # Eigen includes some files it should not, ignore @@ -196,7 +198,7 @@ mrover_add_node(arm_controller src/teleoperation/arm_controller/*.cpp) if (MROVER_BUILD_SIM) mrover_add_nodelet(simulator src/simulator/*.cpp src/simulator src/simulator/pch.hpp) mrover_nodelet_include_directories(simulator ${ASSIMP_INCLUDE_DIRS} ${BULLET_INCLUDE_DIRS} ${OPENCV_INCLUDE_DIRS} src/simulator/imgui) - mrover_nodelet_link_libraries(simulator ${ASSIMP_LIBRARIES} ${BULLET_LIBRARIES} glfw3webgpu webgpu glfw opencv_core opencv_imgcodecs opencv_imgproc lie) + mrover_nodelet_link_libraries(simulator ${ASSIMP_LIBRARIES} ${BULLET_LIBRARIES} glfw3webgpu webgpu glfw opencv_core opencv_imgcodecs opencv_imgproc lie manif) mrover_nodelet_defines(simulator BOOST_THREAD_PROVIDES_FUTURE) endif () diff --git a/src/perception/tag_detector/pch.hpp b/src/perception/tag_detector/pch.hpp index 31e6c1eb9..302a16d34 100644 --- a/src/perception/tag_detector/pch.hpp +++ b/src/perception/tag_detector/pch.hpp @@ -32,5 +32,7 @@ #include #include +#include #include #include +#include diff --git a/src/perception/tag_detector/tag_detector.hpp b/src/perception/tag_detector/tag_detector.hpp index b14541478..457ec41e4 100644 --- a/src/perception/tag_detector/tag_detector.hpp +++ b/src/perception/tag_detector/tag_detector.hpp @@ -1,12 +1,14 @@ #include "pch.hpp" +using manif::SE3d; + namespace mrover { struct Tag { int id = -1; int hitCount = 0; cv::Point2f imageCenter{}; - std::optional tagInCam; + std::optional tagInCam; }; class TagDetectorNodelet : public nodelet::Nodelet { @@ -53,7 +55,7 @@ namespace mrover { void publishThresholdedImage(); - std::optional getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v); + std::optional getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v); public: TagDetectorNodelet() = default; diff --git a/src/perception/tag_detector/tag_detector.processing.cpp b/src/perception/tag_detector/tag_detector.processing.cpp index 66fc415a8..15054d389 100644 --- a/src/perception/tag_detector/tag_detector.processing.cpp +++ b/src/perception/tag_detector/tag_detector.processing.cpp @@ -8,7 +8,7 @@ namespace mrover { * @param u X Pixel Position * @param v Y Pixel Position */ - std::optional TagDetectorNodelet::getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v) { + std::optional TagDetectorNodelet::getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v) { assert(cloudPtr); if (u >= cloudPtr->width || v >= cloudPtr->height) { @@ -23,7 +23,7 @@ namespace mrover { return std::nullopt; } - return std::make_optional(R3{point.x, point.y, point.z}, SO3{}); + return std::make_optional(Eigen::Vector3d{point.x, point.y, point.z}, Eigen::Quaterniond{1, 0, 0, 0}); } /** @@ -89,7 +89,7 @@ namespace mrover { if (tag.tagInCam) { // Publish tag to immediate std::string immediateFrameId = "immediateFiducial" + std::to_string(tag.id); - SE3::pushToTfTree(mTfBroadcaster, immediateFrameId, mCameraFrameId, tag.tagInCam.value()); + pushSE3ToTfTree(mTfBroadcaster, immediateFrameId, mCameraFrameId, tag.tagInCam.value()); } } @@ -116,8 +116,8 @@ namespace mrover { std::string immediateFrameId = "immediateFiducial" + std::to_string(tag.id); // Publish tag to odom std::string const& parentFrameId = mUseOdom ? mOdomFrameId : mMapFrameId; - SE3 tagInParent = SE3::fromTfTree(mTfBuffer, parentFrameId, immediateFrameId); - SE3::pushToTfTree(mTfBroadcaster, "fiducial" + std::to_string(id), parentFrameId, tagInParent); + SE3d tagInParent = SE3fromTfTree(mTfBuffer, parentFrameId, immediateFrameId); + pushSE3ToTfTree(mTfBroadcaster, "fiducial" + std::to_string(id), parentFrameId, tagInParent); } catch (tf2::ExtrapolationException const&) { NODELET_WARN("Old data for immediate tag"); } catch (tf2::LookupException const&) { diff --git a/src/perception/zed_wrapper/pch.hpp b/src/perception/zed_wrapper/pch.hpp index b984fad48..579a5aace 100644 --- a/src/perception/zed_wrapper/pch.hpp +++ b/src/perception/zed_wrapper/pch.hpp @@ -26,6 +26,7 @@ #include #include -#include -#include #include +#include +#include +#include diff --git a/src/perception/zed_wrapper/zed_wrapper.cpp b/src/perception/zed_wrapper/zed_wrapper.cpp index dd943c431..eb806419a 100644 --- a/src/perception/zed_wrapper/zed_wrapper.cpp +++ b/src/perception/zed_wrapper/zed_wrapper.cpp @@ -252,11 +252,11 @@ namespace mrover { sl::Translation const& translation = pose.getTranslation(); sl::Orientation const& orientation = pose.getOrientation(); try { - SE3 leftCameraInOdom{{translation.x, translation.y, translation.z}, - Eigen::Quaterniond{orientation.w, orientation.x, orientation.y, orientation.z}.normalized()}; - SE3 leftCameraInBaseLink = SE3::fromTfTree(mTfBuffer, "base_link", "zed2i_left_camera_frame"); - SE3 baseLinkInOdom = leftCameraInBaseLink * leftCameraInOdom; - SE3::pushToTfTree(mTfBroadcaster, "base_link", "odom", baseLinkInOdom); + SE3d leftCameraInOdom{{translation.x, translation.y, translation.z}, + Eigen::Quaterniond{orientation.w, orientation.x, orientation.y, orientation.z}.normalized()}; + SE3d leftCameraInBaseLink = SE3fromTfTree(mTfBuffer, "base_link", "zed2i_left_camera_frame"); + SE3d baseLinkInOdom = leftCameraInBaseLink * leftCameraInOdom; + pushSE3ToTfTree(mTfBroadcaster, "base_link", "odom", baseLinkInOdom); } catch (tf2::TransformException& e) { NODELET_WARN_STREAM("Failed to get transform: " << e.what()); } diff --git a/src/simulator/pch.hpp b/src/simulator/pch.hpp index 89c9bfeec..adf465862 100644 --- a/src/simulator/pch.hpp +++ b/src/simulator/pch.hpp @@ -23,11 +23,11 @@ #include #include #include -#include #include #include #include #include +#include #include #include #include @@ -64,11 +64,12 @@ #include #include +#include #include #include #include #include #include -#include #include +#include diff --git a/src/util/se3_ros_utils.cpp b/src/util/se3_ros_utils.cpp new file mode 100644 index 000000000..19129a807 --- /dev/null +++ b/src/util/se3_ros_utils.cpp @@ -0,0 +1,61 @@ +#include "se3_ros_utils.hpp" + +SE3d SE3fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId) { + geometry_msgs::TransformStamped transform = buffer.lookupTransform(fromFrameId, toFrameId, ros::Time{}); + return SE3fromTf(transform.transform); +} + +void pushSE3ToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3d const& tf) { + broadcaster.sendTransform(SE3toTransformStamped(tf, parentFrameId, childFrameId)); +} + +SE3d SE3fromTf(geometry_msgs::Transform const& transform) { + return {{transform.translation.x, transform.translation.y, transform.translation.z}, + Eigen::Quaterniond{transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z}}; +} + +SE3d SE3fromPose(geometry_msgs::Pose const& pose) { + return {{pose.position.x, pose.position.y, pose.position.z}, + Eigen::Quaterniond{pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z}}; +} + +geometry_msgs::Pose SE3toPose(SE3d const& tf) { + geometry_msgs::Pose pose; + pose.position.x = tf.x(); + pose.position.y = tf.y(); + pose.position.z = tf.z(); + pose.orientation.x = tf.quat().x(); + pose.orientation.y = tf.quat().y(); + pose.orientation.z = tf.quat().z(); + pose.orientation.w = tf.quat().w(); + return pose; +} + +geometry_msgs::Transform SE3toTransform(SE3d const& tf) { + geometry_msgs::Transform transform; + transform.translation.x = tf.x(); + transform.translation.y = tf.y(); + transform.translation.z = tf.z(); + transform.rotation.x = tf.quat().x(); + transform.rotation.y = tf.quat().y(); + transform.rotation.z = tf.quat().z(); + transform.rotation.w = tf.quat().w(); + return transform; +} + +geometry_msgs::PoseStamped SE3toPoseStamped(SE3d const& tf, std::string const& frameId) { + geometry_msgs::PoseStamped pose; + pose.pose = SE3toPose(tf); + pose.header.frame_id = frameId; + pose.header.stamp = ros::Time::now(); + return pose; +} + +geometry_msgs::TransformStamped SE3toTransformStamped(SE3d const& tf, std::string const& parentFrameId, std::string const& childFrameId) { + geometry_msgs::TransformStamped transform; + transform.transform = SE3toTransform(tf); + transform.header.frame_id = parentFrameId; + transform.header.stamp = ros::Time::now(); + transform.child_frame_id = childFrameId; + return transform; +} diff --git a/src/util/se3_ros_utils.hpp b/src/util/se3_ros_utils.hpp new file mode 100644 index 000000000..b1629f587 --- /dev/null +++ b/src/util/se3_ros_utils.hpp @@ -0,0 +1,33 @@ +#pragma once + +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include + +using manif::SE3d; + + +SE3d SE3fromTf(geometry_msgs::Transform const& transform); + +SE3d SE3fromPose(geometry_msgs::Pose const& pose); + +[[nodiscard]] geometry_msgs::Pose SE3toPose(SE3d const& tf); + +[[nodiscard]] geometry_msgs::Transform SE3toTransform(SE3d const& tf); + +[[nodiscard]] geometry_msgs::PoseStamped SE3toPoseStamped(SE3d const& tf, std::string const& frameId); + +[[nodiscard]] geometry_msgs::TransformStamped SE3toTransformStamped(SE3d const& tf, std::string const& parentFrameId, std::string const& childFrameId); + +[[nodiscard]] SE3d SE3fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId); + +void pushSE3ToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3d const& tf); From 95e019852b4060a37e2f46d20c36b61f95ea58e4 Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Wed, 31 Jan 2024 23:11:15 -0500 Subject: [PATCH 50/81] debugging --- CMakeLists.txt | 2 +- src/perception/tag_detector/tag_detector.cpp | 61 ++++++++++ src/perception/tag_detector/tag_detector.hpp | 16 +++ src/util/se3_ros_utils.cpp | 120 ++++++++++--------- src/util/se3_ros_utils.hpp | 18 +-- 5 files changed, 149 insertions(+), 68 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2a3d11867..91d8819ea 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -168,7 +168,7 @@ endif () ## Perception -mrover_add_nodelet(tag_detector src/perception/tag_detector/*.cpp src/perception/tag_detector src/perception/tag_detector/pch.hpp) +mrover_add_nodelet(tag_detector src/perception/tag_detector/*.cpp src/perception/tag_detector src/perception/tag_detector/pch.hpp src/util/se3_ros_utils.hpp src/util/se3_ros_utils.cpp) mrover_nodelet_link_libraries(tag_detector opencv_core opencv_objdetect opencv_aruco opencv_imgproc opencv_highgui lie manif) if (CUDA_FOUND) diff --git a/src/perception/tag_detector/tag_detector.cpp b/src/perception/tag_detector/tag_detector.cpp index ee9af68ca..fbcb7bf70 100644 --- a/src/perception/tag_detector/tag_detector.cpp +++ b/src/perception/tag_detector/tag_detector.cpp @@ -141,4 +141,65 @@ namespace mrover { return true; } + // SE3d TagDetectorNodelet::SE3fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId) { + // geometry_msgs::TransformStamped transform = buffer.lookupTransform(fromFrameId, toFrameId, ros::Time{}); + // return SE3fromTf(transform.transform); + // } + + // void TagDetectorNodelet::pushSE3ToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3d const& tf) { + // broadcaster.sendTransform(SE3toTransformStamped(tf, parentFrameId, childFrameId)); + // } + + // SE3d TagDetectorNodelet::SE3fromTf(geometry_msgs::Transform const& transform) { + // return {{transform.translation.x, transform.translation.y, transform.translation.z}, + // Eigen::Quaterniond{transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z}}; + // } + + // SE3d TagDetectorNodelet::SE3fromPose(geometry_msgs::Pose const& pose) { + // return {{pose.position.x, pose.position.y, pose.position.z}, + // Eigen::Quaterniond{pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z}}; + // } + + // geometry_msgs::Pose TagDetectorNodelet::SE3toPose(SE3d const& tf) { + // geometry_msgs::Pose pose; + // pose.position.x = tf.x(); + // pose.position.y = tf.y(); + // pose.position.z = tf.z(); + // pose.orientation.x = tf.quat().x(); + // pose.orientation.y = tf.quat().y(); + // pose.orientation.z = tf.quat().z(); + // pose.orientation.w = tf.quat().w(); + // return pose; + // } + + // geometry_msgs::Transform TagDetectorNodelet::SE3toTransform(SE3d const& tf) { + // geometry_msgs::Transform transform; + // transform.translation.x = tf.x(); + // transform.translation.y = tf.y(); + // transform.translation.z = tf.z(); + // transform.rotation.x = tf.quat().x(); + // transform.rotation.y = tf.quat().y(); + // transform.rotation.z = tf.quat().z(); + // transform.rotation.w = tf.quat().w(); + // return transform; + // } + + // geometry_msgs::PoseStamped TagDetectorNodelet::SE3toPoseStamped(SE3d const& tf, std::string const& frameId) { + // geometry_msgs::PoseStamped pose; + // pose.pose = SE3toPose(tf); + // pose.header.frame_id = frameId; + // pose.header.stamp = ros::Time::now(); + // return pose; + // } + + // geometry_msgs::TransformStamped TagDetectorNodelet::SE3toTransformStamped(SE3d const& tf, std::string const& parentFrameId, std::string const& childFrameId) { + // geometry_msgs::TransformStamped transform; + // transform.transform = SE3toTransform(tf); + // transform.header.frame_id = parentFrameId; + // transform.header.stamp = ros::Time::now(); + // transform.child_frame_id = childFrameId; + // return transform; + // } + + } // namespace mrover diff --git a/src/perception/tag_detector/tag_detector.hpp b/src/perception/tag_detector/tag_detector.hpp index 457ec41e4..28e3c4dc5 100644 --- a/src/perception/tag_detector/tag_detector.hpp +++ b/src/perception/tag_detector/tag_detector.hpp @@ -67,6 +67,22 @@ namespace mrover { void configCallback(mrover::DetectorParamsConfig& config, uint32_t level); bool enableDetectionsCallback(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res); + + // SE3d SE3fromTf(geometry_msgs::Transform const& transform); + + // SE3d SE3fromPose(geometry_msgs::Pose const& pose); + + // [[nodiscard]] geometry_msgs::Pose SE3toPose(SE3d const& tf); + + // [[nodiscard]] geometry_msgs::Transform SE3toTransform(SE3d const& tf); + + // [[nodiscard]] geometry_msgs::PoseStamped SE3toPoseStamped(SE3d const& tf, std::string const& frameId); + + // [[nodiscard]] geometry_msgs::TransformStamped SE3toTransformStamped(SE3d const& tf, std::string const& parentFrameId, std::string const& childFrameId); + + // [[nodiscard]] SE3d SE3fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId); + + // void pushSE3ToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3d const& tf); }; } // namespace mrover diff --git a/src/util/se3_ros_utils.cpp b/src/util/se3_ros_utils.cpp index 19129a807..0a1a8fb1b 100644 --- a/src/util/se3_ros_utils.cpp +++ b/src/util/se3_ros_utils.cpp @@ -1,61 +1,63 @@ #include "se3_ros_utils.hpp" -SE3d SE3fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId) { - geometry_msgs::TransformStamped transform = buffer.lookupTransform(fromFrameId, toFrameId, ros::Time{}); - return SE3fromTf(transform.transform); -} - -void pushSE3ToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3d const& tf) { - broadcaster.sendTransform(SE3toTransformStamped(tf, parentFrameId, childFrameId)); -} - -SE3d SE3fromTf(geometry_msgs::Transform const& transform) { - return {{transform.translation.x, transform.translation.y, transform.translation.z}, - Eigen::Quaterniond{transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z}}; -} - -SE3d SE3fromPose(geometry_msgs::Pose const& pose) { - return {{pose.position.x, pose.position.y, pose.position.z}, - Eigen::Quaterniond{pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z}}; -} - -geometry_msgs::Pose SE3toPose(SE3d const& tf) { - geometry_msgs::Pose pose; - pose.position.x = tf.x(); - pose.position.y = tf.y(); - pose.position.z = tf.z(); - pose.orientation.x = tf.quat().x(); - pose.orientation.y = tf.quat().y(); - pose.orientation.z = tf.quat().z(); - pose.orientation.w = tf.quat().w(); - return pose; -} - -geometry_msgs::Transform SE3toTransform(SE3d const& tf) { - geometry_msgs::Transform transform; - transform.translation.x = tf.x(); - transform.translation.y = tf.y(); - transform.translation.z = tf.z(); - transform.rotation.x = tf.quat().x(); - transform.rotation.y = tf.quat().y(); - transform.rotation.z = tf.quat().z(); - transform.rotation.w = tf.quat().w(); - return transform; -} - -geometry_msgs::PoseStamped SE3toPoseStamped(SE3d const& tf, std::string const& frameId) { - geometry_msgs::PoseStamped pose; - pose.pose = SE3toPose(tf); - pose.header.frame_id = frameId; - pose.header.stamp = ros::Time::now(); - return pose; -} - -geometry_msgs::TransformStamped SE3toTransformStamped(SE3d const& tf, std::string const& parentFrameId, std::string const& childFrameId) { - geometry_msgs::TransformStamped transform; - transform.transform = SE3toTransform(tf); - transform.header.frame_id = parentFrameId; - transform.header.stamp = ros::Time::now(); - transform.child_frame_id = childFrameId; - return transform; -} +namespace mrover { + SE3d SE3fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId) { + geometry_msgs::TransformStamped transform = buffer.lookupTransform(fromFrameId, toFrameId, ros::Time{}); + return SE3fromTf(transform.transform); + } + + void pushSE3ToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3d const& tf) { + broadcaster.sendTransform(SE3toTransformStamped(tf, parentFrameId, childFrameId)); + } + + SE3d SE3fromTf(geometry_msgs::Transform const& transform) { + return {{transform.translation.x, transform.translation.y, transform.translation.z}, + Eigen::Quaterniond{transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z}}; + } + + SE3d SE3fromPose(geometry_msgs::Pose const& pose) { + return {{pose.position.x, pose.position.y, pose.position.z}, + Eigen::Quaterniond{pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z}}; + } + + geometry_msgs::Pose SE3toPose(SE3d const& tf) { + geometry_msgs::Pose pose; + pose.position.x = tf.x(); + pose.position.y = tf.y(); + pose.position.z = tf.z(); + pose.orientation.x = tf.quat().x(); + pose.orientation.y = tf.quat().y(); + pose.orientation.z = tf.quat().z(); + pose.orientation.w = tf.quat().w(); + return pose; + } + + geometry_msgs::Transform SE3toTransform(SE3d const& tf) { + geometry_msgs::Transform transform; + transform.translation.x = tf.x(); + transform.translation.y = tf.y(); + transform.translation.z = tf.z(); + transform.rotation.x = tf.quat().x(); + transform.rotation.y = tf.quat().y(); + transform.rotation.z = tf.quat().z(); + transform.rotation.w = tf.quat().w(); + return transform; + } + + geometry_msgs::PoseStamped SE3toPoseStamped(SE3d const& tf, std::string const& frameId) { + geometry_msgs::PoseStamped pose; + pose.pose = SE3toPose(tf); + pose.header.frame_id = frameId; + pose.header.stamp = ros::Time::now(); + return pose; + } + + geometry_msgs::TransformStamped SE3toTransformStamped(SE3d const& tf, std::string const& parentFrameId, std::string const& childFrameId) { + geometry_msgs::TransformStamped transform; + transform.transform = SE3toTransform(tf); + transform.header.frame_id = parentFrameId; + transform.header.stamp = ros::Time::now(); + transform.child_frame_id = childFrameId; + return transform; + } +} // namespace mrover \ No newline at end of file diff --git a/src/util/se3_ros_utils.hpp b/src/util/se3_ros_utils.hpp index b1629f587..1b96d1faa 100644 --- a/src/util/se3_ros_utils.hpp +++ b/src/util/se3_ros_utils.hpp @@ -16,18 +16,20 @@ using manif::SE3d; -SE3d SE3fromTf(geometry_msgs::Transform const& transform); +namespace mrover { + SE3d SE3fromTf(geometry_msgs::Transform const& transform); -SE3d SE3fromPose(geometry_msgs::Pose const& pose); + SE3d SE3fromPose(geometry_msgs::Pose const& pose); -[[nodiscard]] geometry_msgs::Pose SE3toPose(SE3d const& tf); + [[nodiscard]] geometry_msgs::Pose SE3toPose(SE3d const& tf); -[[nodiscard]] geometry_msgs::Transform SE3toTransform(SE3d const& tf); + [[nodiscard]] geometry_msgs::Transform SE3toTransform(SE3d const& tf); -[[nodiscard]] geometry_msgs::PoseStamped SE3toPoseStamped(SE3d const& tf, std::string const& frameId); + [[nodiscard]] geometry_msgs::PoseStamped SE3toPoseStamped(SE3d const& tf, std::string const& frameId); -[[nodiscard]] geometry_msgs::TransformStamped SE3toTransformStamped(SE3d const& tf, std::string const& parentFrameId, std::string const& childFrameId); + [[nodiscard]] geometry_msgs::TransformStamped SE3toTransformStamped(SE3d const& tf, std::string const& parentFrameId, std::string const& childFrameId); -[[nodiscard]] SE3d SE3fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId); + [[nodiscard]] SE3d SE3fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId); -void pushSE3ToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3d const& tf); + void pushSE3ToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3d const& tf); +} // namespace mrover \ No newline at end of file From 20825831c6315208f6350db1b14ff5c27f13cf36 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sun, 4 Feb 2024 21:04:49 -0500 Subject: [PATCH 51/81] Init --- CMakeLists.txt | 22 ++++++---- deps/libde265 | 1 + .../hardware_h265_encoder.cu} | 16 +++---- .../hardware_h265_encoder.hpp} | 11 +++-- .../software_h265_encoder.cpp | 42 +++++++++++++++++++ .../software_h265_encoder.hpp | 6 +++ 6 files changed, 75 insertions(+), 23 deletions(-) create mode 160000 deps/libde265 rename src/esw/{streaming/encoding.cu => hardware_h265_encoder/hardware_h265_encoder.cu} (93%) rename src/esw/{streaming/encoding.hpp => hardware_h265_encoder/hardware_h265_encoder.hpp} (88%) create mode 100644 src/esw/software_h265_encoder/software_h265_encoder.cpp create mode 100644 src/esw/software_h265_encoder/software_h265_encoder.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index f59ca5030..cd479b848 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -81,13 +81,9 @@ if (MROVER_BUILD_SIM) if (NOT Assimp_FOUND AND NOT assimp_FOUND) message(FATAL_ERROR "Assimp not found") endif () - find_package(Bullet REQUIRED) - find_package(glfw3 REQUIRED) - include(cmake/webgpu.cmake) - add_subdirectory(deps/glfw3webgpu) endif () find_package(OpenCV REQUIRED) @@ -185,13 +181,21 @@ mrover_add_nodelet(tag_detector src/perception/tag_detector/*.cpp src/perception mrover_nodelet_link_libraries(tag_detector opencv_core opencv_objdetect opencv_aruco opencv_imgproc opencv_highgui lie) if (CUDA_FOUND) - mrover_add_library(streaming src/esw/streaming/*.c* src/esw/streaming) - # target_link_libraries(streaming PUBLIC opencv_core opencv_cudacodec) - target_link_libraries(streaming PUBLIC cuda nvidia-encode opencv_core) - target_include_directories(streaming SYSTEM PUBLIC deps/nvenc) - target_compile_definitions(streaming PUBLIC BOOST_ASIO_NO_DEPRECATED) + mrover_add_library(hardware_h265_encoder src/esw/hardware_h265_encoder/*.c* src/esw/hardware_h265_encoder) + target_link_libraries(hardware_h265_encoder PUBLIC cuda nvidia-encode opencv_core) + target_include_directories(hardware_h265_encoder SYSTEM PUBLIC deps/nvenc) endif () +add_subdirectory(deps/libde265) +#target_include_directories(de265 SYSTEM INTERFACE deps/libde265) + +mrover_add_library(software_h265_encoder src/esw/software_h265_encoder/*.c* src/esw/software_h265_encoder) +target_link_libraries(software_h265_encoder PUBLIC opencv_core de265) +target_include_directories(software_h265_encoder SYSTEM PUBLIC deps/nvenc) + +mrover_add_library(streaming src/esw/streaming/*.cpp src/esw/streaming) +target_compile_definitions(streaming PUBLIC BOOST_ASIO_NO_DEPRECATED) + if (ZED_FOUND) mrover_add_nodelet(zed src/perception/zed_wrapper/*.c* src/perception/zed_wrapper src/perception/zed_wrapper/pch.hpp) mrover_nodelet_include_directories(zed ${ZED_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS}) diff --git a/deps/libde265 b/deps/libde265 new file mode 160000 index 000000000..a267c8470 --- /dev/null +++ b/deps/libde265 @@ -0,0 +1 @@ +Subproject commit a267c84707ab264928fa9b86de2ee749c48c318c diff --git a/src/esw/streaming/encoding.cu b/src/esw/hardware_h265_encoder/hardware_h265_encoder.cu similarity index 93% rename from src/esw/streaming/encoding.cu rename to src/esw/hardware_h265_encoder/hardware_h265_encoder.cu index 09efffd82..870b5c44b 100644 --- a/src/esw/streaming/encoding.cu +++ b/src/esw/hardware_h265_encoder/hardware_h265_encoder.cu @@ -1,4 +1,4 @@ -#include "encoding.hpp" +#include "hardware_h265_encoder.hpp" #include #include @@ -60,7 +60,7 @@ std::unordered_map GUID_TO_NAME{ {NV_ENC_CODEC_AV1_GUID, "AV1"}, }; -NvEncoder::NvEncoder(cv::Size const& size) : m_size{size} { +Encoder::Encoder(cv::Size const& size) : m_size{size} { cudaCheck(cudaSetDevice(0)); CUcontext context; cuCheck(cuCtxGetCurrent(&context)); @@ -148,7 +148,7 @@ NvEncoder::NvEncoder(cv::Size const& size) : m_size{size} { m_output = createBitstreamBufferParams.bitstreamBuffer; } -NvEncoder::BitstreamView NvEncoder::feed(cv::InputArray frameI420) { +Encoder::BitstreamView Encoder::feed(cv::InputArray frameI420) { if (!frameI420.isContinuous()) throw std::runtime_error("Frame is not continuous"); if (frameI420.type() != CV_8UC1) throw std::runtime_error("Not single channel, note that YUV420 is expected"); if (frameI420.size() != cv::Size{m_size.width, m_size.height + m_size.height / 2}) throw std::runtime_error("Wrong size, note that YUV420 is expected"); @@ -182,22 +182,22 @@ NvEncoder::BitstreamView NvEncoder::feed(cv::InputArray frameI420) { return {&m_nvenc, m_encoder, m_output}; } -NvEncoder::BitstreamView::BitstreamView(NV_ENCODE_API_FUNCTION_LIST* nvenc, void* encoder, NV_ENC_OUTPUT_PTR output) +Encoder::BitstreamView::BitstreamView(NV_ENCODE_API_FUNCTION_LIST* nvenc, void* encoder, NV_ENC_OUTPUT_PTR output) : nvenc{nvenc}, encoder{encoder}, output{output}, lockParams{.version = NV_ENC_LOCK_BITSTREAM_VER, .outputBitstream = output} { NvCheck(nvenc->nvEncLockBitstream(encoder, &lockParams)); } -NvEncoder::BitstreamView::~BitstreamView() { +Encoder::BitstreamView::~BitstreamView() { if (nvenc && encoder && output) { NvCheck(nvenc->nvEncUnlockBitstream(encoder, output)); } } -NvEncoder::BitstreamView::BitstreamView(NvEncoder::BitstreamView&& other) noexcept { +Encoder::BitstreamView::BitstreamView(BitstreamView&& other) noexcept { *this = std::move(other); } -NvEncoder::BitstreamView& NvEncoder::BitstreamView::operator=(NvEncoder::BitstreamView&& other) noexcept { +Encoder::BitstreamView& Encoder::BitstreamView::operator=(BitstreamView&& other) noexcept { std::swap(nvenc, other.nvenc); std::swap(encoder, other.encoder); std::swap(output, other.output); @@ -205,7 +205,7 @@ NvEncoder::BitstreamView& NvEncoder::BitstreamView::operator=(NvEncoder::Bitstre return *this; } -NvEncoder::~NvEncoder() { +Encoder::~Encoder() { NvCheck(m_nvenc.nvEncDestroyInputBuffer(m_encoder, m_input)); NvCheck(m_nvenc.nvEncDestroyBitstreamBuffer(m_encoder, m_output)); NvCheck(m_nvenc.nvEncDestroyEncoder(m_encoder)); diff --git a/src/esw/streaming/encoding.hpp b/src/esw/hardware_h265_encoder/hardware_h265_encoder.hpp similarity index 88% rename from src/esw/streaming/encoding.hpp rename to src/esw/hardware_h265_encoder/hardware_h265_encoder.hpp index 241a013f8..6fcfe9b2e 100644 --- a/src/esw/streaming/encoding.hpp +++ b/src/esw/hardware_h265_encoder/hardware_h265_encoder.hpp @@ -1,12 +1,11 @@ #pragma once #include -#include #include #include -struct NvEncoder { +struct Encoder { struct BitstreamView { NV_ENCODE_API_FUNCTION_LIST* nvenc = nullptr; @@ -30,13 +29,13 @@ struct NvEncoder { void* m_encoder = nullptr; NV_ENC_INPUT_PTR m_input = nullptr; NV_ENC_OUTPUT_PTR m_output = nullptr; - uint32_t m_frame_index = 0; + std::uint32_t m_frame_index = 0; std::chrono::high_resolution_clock m_clock; public: - NvEncoder(cv::Size const& size); + Encoder(cv::Size const& size); [[nodiscard]] BitstreamView feed(cv::InputArray frameI420); - ~NvEncoder(); -}; \ No newline at end of file + ~Encoder(); +}; diff --git a/src/esw/software_h265_encoder/software_h265_encoder.cpp b/src/esw/software_h265_encoder/software_h265_encoder.cpp new file mode 100644 index 000000000..d5f0e11fc --- /dev/null +++ b/src/esw/software_h265_encoder/software_h265_encoder.cpp @@ -0,0 +1,42 @@ +#include "software_h265_encoder.hpp" + +#include + +#include + +#include "../../../deps/libde265/libde265/en265.h" + +en265_encoder_context* encoder = nullptr; + +auto imageCallback(const sensor_msgs::ImageConstPtr& msg) -> void { + de265_image* image = en265_allocate_image(encoder, msg->width, msg->height, de265_chroma_420); + if (image == nullptr) { + ROS_ERROR("Failed to allocate image"); + ros::requestShutdown(); + } + std::memcpy(image->data, msg->data.data(), msg->data.size()); + + en265_push_image(encoder, image); + en265_encode(); + + en265_packet* packet = nullptr; + while ((packet = en265_get_packet(encoder, 100)) != nullptr) { + + en265_free_packet(encoder, packet); + } +} + +auto main(int argc, char** argv) -> int { + ros::init(argc, argv, "software_h265_encoder"); + + encoder = en265_new_encoder(); + if (encoder == nullptr) { + ROS_ERROR("Failed to create encoder context"); + ros::requestShutdown(); + } + + en265_start_encoder(encoder); + + ros::spin(); + return EXIT_SUCCESS; +} \ No newline at end of file diff --git a/src/esw/software_h265_encoder/software_h265_encoder.hpp b/src/esw/software_h265_encoder/software_h265_encoder.hpp new file mode 100644 index 000000000..b71113648 --- /dev/null +++ b/src/esw/software_h265_encoder/software_h265_encoder.hpp @@ -0,0 +1,6 @@ +#pragma once + +#include + +#include + From 190ee1bd40809e1b4d0c1da64cd3568f8ff884a2 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Mon, 5 Feb 2024 12:43:54 -0500 Subject: [PATCH 52/81] Work on encoders --- .gitmodules | 5 ++ CMakeLists.txt | 16 ++-- deps/emsdk | 1 + deps/libde265 | 2 +- .../hardware_h265_encoder.cpp | 52 +++++++++++ .../hardware_h265_encoder.cu | 8 +- .../hardware_h265_encoder.hpp | 13 +-- .../software_h265_encoder.cpp | 89 ++++++++++++++----- src/teleoperation/streaming/stream_client.cpp | 2 +- 9 files changed, 146 insertions(+), 42 deletions(-) create mode 160000 deps/emsdk create mode 100644 src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp diff --git a/.gitmodules b/.gitmodules index e013bbf1b..1be2daefd 100644 --- a/.gitmodules +++ b/.gitmodules @@ -8,3 +8,8 @@ url = https://dawn.googlesource.com/dawn shallow = true branch = chromium/6108 +[submodule "deps/emsdk"] + path = deps/emsdk + url = git@github.com:emscripten-core/emsdk.git + shallow = true + branch = 3.1.53 diff --git a/CMakeLists.txt b/CMakeLists.txt index cd479b848..a094d8a62 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -180,22 +180,22 @@ endif () mrover_add_nodelet(tag_detector src/perception/tag_detector/*.cpp src/perception/tag_detector src/perception/tag_detector/pch.hpp) mrover_nodelet_link_libraries(tag_detector opencv_core opencv_objdetect opencv_aruco opencv_imgproc opencv_highgui lie) +mrover_add_library(streaming src/esw/streaming/*.cpp src/esw/streaming) +target_compile_definitions(streaming PUBLIC BOOST_ASIO_NO_DEPRECATED) + if (CUDA_FOUND) - mrover_add_library(hardware_h265_encoder src/esw/hardware_h265_encoder/*.c* src/esw/hardware_h265_encoder) - target_link_libraries(hardware_h265_encoder PUBLIC cuda nvidia-encode opencv_core) + mrover_add_node(hardware_h265_encoder src/esw/hardware_h265_encoder/*.c*) + target_link_libraries(hardware_h265_encoder PUBLIC cuda nvidia-encode opencv_core opencv_imgproc streaming) target_include_directories(hardware_h265_encoder SYSTEM PUBLIC deps/nvenc) endif () add_subdirectory(deps/libde265) -#target_include_directories(de265 SYSTEM INTERFACE deps/libde265) +include_directories(deps/libde265/extra deps/libde265 SYSTEM) -mrover_add_library(software_h265_encoder src/esw/software_h265_encoder/*.c* src/esw/software_h265_encoder) -target_link_libraries(software_h265_encoder PUBLIC opencv_core de265) +mrover_add_node(software_h265_encoder src/esw/software_h265_encoder/*.c*) +target_link_libraries(software_h265_encoder PUBLIC opencv_core opencv_imgproc opencv_imgcodecs de265 streaming) target_include_directories(software_h265_encoder SYSTEM PUBLIC deps/nvenc) -mrover_add_library(streaming src/esw/streaming/*.cpp src/esw/streaming) -target_compile_definitions(streaming PUBLIC BOOST_ASIO_NO_DEPRECATED) - if (ZED_FOUND) mrover_add_nodelet(zed src/perception/zed_wrapper/*.c* src/perception/zed_wrapper src/perception/zed_wrapper/pch.hpp) mrover_nodelet_include_directories(zed ${ZED_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS}) diff --git a/deps/emsdk b/deps/emsdk new file mode 160000 index 000000000..2aa749071 --- /dev/null +++ b/deps/emsdk @@ -0,0 +1 @@ +Subproject commit 2aa74907151b2caa9da865fd0d36436fdce792f0 diff --git a/deps/libde265 b/deps/libde265 index a267c8470..17bb8d9fc 160000 --- a/deps/libde265 +++ b/deps/libde265 @@ -1 +1 @@ -Subproject commit a267c84707ab264928fa9b86de2ee749c48c318c +Subproject commit 17bb8d9fcea62db8cdeb0fc7ef8d15dbd19a22e4 diff --git a/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp b/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp new file mode 100644 index 000000000..0b810c992 --- /dev/null +++ b/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp @@ -0,0 +1,52 @@ +#include "hardware_h265_encoder.hpp" + +#include +#include +#include + +#include + +#include + +#include + +std::optional streamServer; + +std::optional encoder; + +auto imageCallback(sensor_msgs::ImageConstPtr const& msg) -> void { + try { + cv::Size size{static_cast(msg->width), static_cast(msg->height)}; + if (!encoder) encoder.emplace(size); + + cv::Mat bgraFrame{size, CV_8UC4, const_cast(msg->data.data()), msg->step}; + cv::Mat i420Frame; + cvtColor(bgraFrame, i420Frame, cv::COLOR_BGRA2YUV_I420); + + Encoder::BitstreamView view = encoder->feed(i420Frame); + std::span span{static_cast(view.lockParams.bitstreamBufferPtr), view.lockParams.bitstreamSizeInBytes}; + streamServer->feed(span); + + } catch (std::exception const& e) { + ROS_ERROR_STREAM(std::format("Exception encoding frame: {}", e.what())); + ros::requestShutdown(); + } +} + +auto main(int argc, char** argv) -> int { + try { + ros::init(argc, argv, "software_h265_encoder"); + ros::NodeHandle nh; + + streamServer.emplace("0.0.0.0", 8080); + + ros::Subscriber imageSubscriber = nh.subscribe("/camera/left/image", 1, imageCallback); + + ros::spin(); + return EXIT_SUCCESS; + + } catch (std::exception const& e) { + ROS_ERROR_STREAM(std::format("Exception initializing: {}", e.what())); + return EXIT_FAILURE; + } +} \ No newline at end of file diff --git a/src/esw/hardware_h265_encoder/hardware_h265_encoder.cu b/src/esw/hardware_h265_encoder/hardware_h265_encoder.cu index 870b5c44b..1d44b3b27 100644 --- a/src/esw/hardware_h265_encoder/hardware_h265_encoder.cu +++ b/src/esw/hardware_h265_encoder/hardware_h265_encoder.cu @@ -41,7 +41,7 @@ namespace std { template<> struct hash { - std::size_t operator()(GUID const& g) const { + auto operator()(GUID const& g) const noexcept -> std::size_t { std::size_t seed = 0; seed ^= std::hash{}(g.Data1); seed ^= std::hash{}(g.Data2); @@ -148,7 +148,7 @@ Encoder::Encoder(cv::Size const& size) : m_size{size} { m_output = createBitstreamBufferParams.bitstreamBuffer; } -Encoder::BitstreamView Encoder::feed(cv::InputArray frameI420) { +auto Encoder::feed(cv::InputArray frameI420) -> BitstreamView { if (!frameI420.isContinuous()) throw std::runtime_error("Frame is not continuous"); if (frameI420.type() != CV_8UC1) throw std::runtime_error("Not single channel, note that YUV420 is expected"); if (frameI420.size() != cv::Size{m_size.width, m_size.height + m_size.height / 2}) throw std::runtime_error("Wrong size, note that YUV420 is expected"); @@ -159,7 +159,7 @@ Encoder::BitstreamView Encoder::feed(cv::InputArray frameI420) { }; NvCheck(m_nvenc.nvEncLockInputBuffer(m_encoder, &lockInputBufferParams)); for (int r = 0; r < frameI420.rows(); ++r) { - std::byte* row = static_cast(lockInputBufferParams.bufferDataPtr) + r * lockInputBufferParams.pitch; + auto* row = static_cast(lockInputBufferParams.bufferDataPtr) + r * lockInputBufferParams.pitch; std::memcpy(row, frameI420.getMat().ptr(r), frameI420.cols()); } NvCheck(m_nvenc.nvEncUnlockInputBuffer(m_encoder, m_input)); @@ -197,7 +197,7 @@ Encoder::BitstreamView::BitstreamView(BitstreamView&& other) noexcept { *this = std::move(other); } -Encoder::BitstreamView& Encoder::BitstreamView::operator=(BitstreamView&& other) noexcept { +auto Encoder::BitstreamView::operator=(BitstreamView&& other) noexcept -> Encoder::BitstreamView& { std::swap(nvenc, other.nvenc); std::swap(encoder, other.encoder); std::swap(output, other.output); diff --git a/src/esw/hardware_h265_encoder/hardware_h265_encoder.hpp b/src/esw/hardware_h265_encoder/hardware_h265_encoder.hpp index 6fcfe9b2e..51219b46b 100644 --- a/src/esw/hardware_h265_encoder/hardware_h265_encoder.hpp +++ b/src/esw/hardware_h265_encoder/hardware_h265_encoder.hpp @@ -5,8 +5,8 @@ #include #include -struct Encoder { - +class Encoder { +public: struct BitstreamView { NV_ENCODE_API_FUNCTION_LIST* nvenc = nullptr; void* encoder = nullptr; @@ -18,12 +18,13 @@ struct Encoder { ~BitstreamView(); BitstreamView(BitstreamView const&) = delete; - BitstreamView& operator=(BitstreamView const&) = delete; + auto operator=(BitstreamView const&) -> BitstreamView& = delete; BitstreamView(BitstreamView&& other) noexcept; - BitstreamView& operator=(BitstreamView&& other) noexcept; + auto operator=(BitstreamView&& other) noexcept -> BitstreamView&; }; +private: cv::Size m_size; NV_ENCODE_API_FUNCTION_LIST m_nvenc{.version = NV_ENCODE_API_FUNCTION_LIST_VER}; void* m_encoder = nullptr; @@ -33,9 +34,9 @@ struct Encoder { std::chrono::high_resolution_clock m_clock; public: - Encoder(cv::Size const& size); + explicit Encoder(cv::Size const& size); - [[nodiscard]] BitstreamView feed(cv::InputArray frameI420); + [[nodiscard]] auto feed(cv::InputArray frameI420) -> BitstreamView; ~Encoder(); }; diff --git a/src/esw/software_h265_encoder/software_h265_encoder.cpp b/src/esw/software_h265_encoder/software_h265_encoder.cpp index d5f0e11fc..15fdd4671 100644 --- a/src/esw/software_h265_encoder/software_h265_encoder.cpp +++ b/src/esw/software_h265_encoder/software_h265_encoder.cpp @@ -1,42 +1,87 @@ #include "software_h265_encoder.hpp" #include +#include #include -#include "../../../deps/libde265/libde265/en265.h" +#include +#include + +#include +#include + +#include +#include +#include + +std::optional streamServer; en265_encoder_context* encoder = nullptr; -auto imageCallback(const sensor_msgs::ImageConstPtr& msg) -> void { - de265_image* image = en265_allocate_image(encoder, msg->width, msg->height, de265_chroma_420); - if (image == nullptr) { - ROS_ERROR("Failed to allocate image"); - ros::requestShutdown(); - } - std::memcpy(image->data, msg->data.data(), msg->data.size()); +auto imageCallback(sensor_msgs::ImageConstPtr const& msg) -> void { + try { + cv::Mat bgraImage{static_cast(msg->height), static_cast(msg->width), CV_8UC4, const_cast(msg->data.data()), msg->step}; + cv::Mat yuv420Image; + cvtColor(bgraImage, yuv420Image, cv::COLOR_BGRA2YUV_I420); + + de265_image* frame = en265_allocate_image(encoder, static_cast(msg->width), static_cast(msg->height), de265_chroma_420, 0, nullptr); + if (frame == nullptr) { + throw std::runtime_error{"Failed to allocate image"}; + } + std::uint8_t* y = frame->get_image_plane(0); + std::uint8_t* u = frame->get_image_plane(1); + std::uint8_t* v = frame->get_image_plane(2); + // int yStride = frame->get_image_stride(0); + // int uStride = frame->get_image_stride(1); + // int vStride = frame->get_image_stride(2); + std::size_t area = bgraImage.total(); + std::memcpy(y, yuv420Image.data, area); + std::memcpy(u, yuv420Image.data + area, area / 4); + std::memcpy(v, yuv420Image.data + area * 5 / 4, area / 4); - en265_push_image(encoder, image); - en265_encode(); + if (en265_push_image(encoder, frame) != DE265_OK) { + throw std::runtime_error{"Failed to push image"}; + } + if (en265_encode(encoder) != DE265_OK) { + throw std::runtime_error{"Failed to encode"}; + } - en265_packet* packet = nullptr; - while ((packet = en265_get_packet(encoder, 100)) != nullptr) { + en265_packet* packet; + while ((packet = en265_get_packet(encoder, 0)) != nullptr) { + std::span span{std::bit_cast(packet->data), static_cast(packet->length)}; + streamServer->feed(span); + en265_free_packet(encoder, packet); + } - en265_free_packet(encoder, packet); + } catch (std::exception const& e) { + ROS_ERROR_STREAM(std::format("Unhandled exception: {}", e.what())); + ros::requestShutdown(); } } auto main(int argc, char** argv) -> int { - ros::init(argc, argv, "software_h265_encoder"); + try { + ros::init(argc, argv, "software_h265_encoder"); + ros::NodeHandle nh; - encoder = en265_new_encoder(); - if (encoder == nullptr) { - ROS_ERROR("Failed to create encoder context"); - ros::requestShutdown(); - } + streamServer.emplace("0.0.0.0", 8080); - en265_start_encoder(encoder); + encoder = en265_new_encoder(); + if (encoder == nullptr) { + throw std::runtime_error{"Failed to create encoder context"}; + } + if (en265_start_encoder(encoder, 0) != DE265_OK) { + throw std::runtime_error{"Failed to start encoder"}; + } - ros::spin(); - return EXIT_SUCCESS; + ros::Subscriber imageSubscriber = nh.subscribe("/camera/left/image", 1, imageCallback); + + ros::spin(); + return EXIT_SUCCESS; + + } catch (std::exception const& e) { + ROS_ERROR_STREAM(std::format("Unhandled exception: {}", e.what())); + return EXIT_FAILURE; + } } \ No newline at end of file diff --git a/src/teleoperation/streaming/stream_client.cpp b/src/teleoperation/streaming/stream_client.cpp index 48531ed33..86e6f80b4 100644 --- a/src/teleoperation/streaming/stream_client.cpp +++ b/src/teleoperation/streaming/stream_client.cpp @@ -77,7 +77,7 @@ EM_BOOL on_message(int _event_type, const EmscriptenWebSocketMessageEvent* webso const vStride = $7; const canvas = document.getElementById('canvas'); - const ctx = canvas.getContext ('2d'); + const ctx = canvas.getContext('2d'); if (Module.imageBuffer === undefined) { Module.imageBuffer = ctx.createImageData(width, height); canvas.width = width; From 96d0ec42d3df236753f03c03526ee2ae16ec70e0 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Mon, 5 Feb 2024 13:06:51 -0500 Subject: [PATCH 53/81] Update --- CMakeLists.txt | 2 +- src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp | 5 ++++- src/esw/hardware_h265_encoder/hardware_h265_encoder.cu | 8 +++++--- 3 files changed, 10 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index a094d8a62..5ef55d2b7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -185,7 +185,7 @@ target_compile_definitions(streaming PUBLIC BOOST_ASIO_NO_DEPRECATED) if (CUDA_FOUND) mrover_add_node(hardware_h265_encoder src/esw/hardware_h265_encoder/*.c*) - target_link_libraries(hardware_h265_encoder PUBLIC cuda nvidia-encode opencv_core opencv_imgproc streaming) + target_link_libraries(hardware_h265_encoder PUBLIC cuda nvidia-encode opencv_core opencv_imgproc opencv_imgcodecs streaming) target_include_directories(hardware_h265_encoder SYSTEM PUBLIC deps/nvenc) endif () diff --git a/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp b/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp index 0b810c992..21ab0793b 100644 --- a/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp +++ b/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp @@ -8,6 +8,7 @@ #include +#include #include std::optional streamServer; @@ -21,7 +22,9 @@ auto imageCallback(sensor_msgs::ImageConstPtr const& msg) -> void { cv::Mat bgraFrame{size, CV_8UC4, const_cast(msg->data.data()), msg->step}; cv::Mat i420Frame; - cvtColor(bgraFrame, i420Frame, cv::COLOR_BGRA2YUV_I420); + cvtColor(bgraFrame, i420Frame, cv::COLOR_BGRA2YUV_IYUV); + + // imwrite("/home/quintin/catkin_ws/src/mrover/image.png", i420Frame); Encoder::BitstreamView view = encoder->feed(i420Frame); std::span span{static_cast(view.lockParams.bitstreamBufferPtr), view.lockParams.bitstreamSizeInBytes}; diff --git a/src/esw/hardware_h265_encoder/hardware_h265_encoder.cu b/src/esw/hardware_h265_encoder/hardware_h265_encoder.cu index 1d44b3b27..55403c6ea 100644 --- a/src/esw/hardware_h265_encoder/hardware_h265_encoder.cu +++ b/src/esw/hardware_h265_encoder/hardware_h265_encoder.cu @@ -31,7 +31,7 @@ void cudaCheck(cudaError_t status) { namespace std { template<> struct equal_to { - bool operator()(GUID const& g1, GUID const& g2) const { + auto operator()(GUID const& g1, GUID const& g2) const -> bool { return g1.Data1 == g2.Data1 && g1.Data2 == g2.Data2 && g1.Data3 == g2.Data3 && g1.Data4[0] == g2.Data4[0] && g1.Data4[1] == g2.Data4[1] && g1.Data4[2] == g2.Data4[2] && g1.Data4[3] == g2.Data4[3] && g1.Data4[4] == g2.Data4[4] && g1.Data4[5] == g2.Data4[5] && @@ -60,6 +60,8 @@ std::unordered_map GUID_TO_NAME{ {NV_ENC_CODEC_AV1_GUID, "AV1"}, }; +constexpr NV_ENC_BUFFER_FORMAT FRAME_FORMAT = NV_ENC_BUFFER_FORMAT_IYUV; + Encoder::Encoder(cv::Size const& size) : m_size{size} { cudaCheck(cudaSetDevice(0)); CUcontext context; @@ -136,7 +138,7 @@ Encoder::Encoder(cv::Size const& size) : m_size{size} { .version = NV_ENC_CREATE_INPUT_BUFFER_VER, .width = static_cast(m_size.width), .height = static_cast(m_size.height), - .bufferFmt = NV_ENC_BUFFER_FORMAT_IYUV, + .bufferFmt = FRAME_FORMAT, }; NvCheck(m_nvenc.nvEncCreateInputBuffer(m_encoder, &createInputBufferParams)); m_input = createInputBufferParams.inputBuffer; @@ -173,7 +175,7 @@ auto Encoder::feed(cv::InputArray frameI420) -> BitstreamView { .inputBuffer = m_input, .outputBitstream = m_output, .completionEvent = nullptr, - .bufferFmt = NV_ENC_BUFFER_FORMAT_IYUV, + .bufferFmt = FRAME_FORMAT, .pictureStruct = NV_ENC_PIC_STRUCT_FRAME, }; From 2b03b97c68691f643411285aed93c3d5d94e4812 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Mon, 5 Feb 2024 16:50:34 -0500 Subject: [PATCH 54/81] Breh --- CMakeLists.txt | 2 +- .../hardware_h265_encoder.cpp | 19 +++++++++++++++++-- .../software_h265_encoder.cpp | 1 + src/esw/streaming/streaming.hpp | 2 +- 4 files changed, 20 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5ef55d2b7..086c4da12 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -185,7 +185,7 @@ target_compile_definitions(streaming PUBLIC BOOST_ASIO_NO_DEPRECATED) if (CUDA_FOUND) mrover_add_node(hardware_h265_encoder src/esw/hardware_h265_encoder/*.c*) - target_link_libraries(hardware_h265_encoder PUBLIC cuda nvidia-encode opencv_core opencv_imgproc opencv_imgcodecs streaming) + target_link_libraries(hardware_h265_encoder PUBLIC cuda nvidia-encode opencv_core opencv_imgproc opencv_imgcodecs opencv_videoio streaming) target_include_directories(hardware_h265_encoder SYSTEM PUBLIC deps/nvenc) endif () diff --git a/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp b/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp index 21ab0793b..77d6f8ee0 100644 --- a/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp +++ b/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp @@ -3,13 +3,14 @@ #include #include #include - +#include #include #include #include #include +#include std::optional streamServer; @@ -17,6 +18,8 @@ std::optional encoder; auto imageCallback(sensor_msgs::ImageConstPtr const& msg) -> void { try { + if (msg->encoding != sensor_msgs::image_encodings::BGRA8) throw std::runtime_error{"Unsupported encoding"}; + cv::Size size{static_cast(msg->width), static_cast(msg->height)}; if (!encoder) encoder.emplace(size); @@ -41,6 +44,18 @@ auto main(int argc, char** argv) -> int { ros::init(argc, argv, "software_h265_encoder"); ros::NodeHandle nh; + // cv::VideoCapture cap{std::format("v4l2src ! videoconvert ! video/x-raw,width={},height={},format=I420,framerate=10/1 ! appsink", 640, 480), cv::CAP_GSTREAMER}; + // if (!cap.isOpened()) { + // throw std::runtime_error{"Failed to open capture"}; + // } + // + // cv::Mat frame; + // if (!cap.read(frame)) { + // throw std::runtime_error{"Failed to read frame"}; + // } + // + // imwrite("/home/quintin/catkin_ws/src/mrover/image.png", frame); + streamServer.emplace("0.0.0.0", 8080); ros::Subscriber imageSubscriber = nh.subscribe("/camera/left/image", 1, imageCallback); @@ -52,4 +67,4 @@ auto main(int argc, char** argv) -> int { ROS_ERROR_STREAM(std::format("Exception initializing: {}", e.what())); return EXIT_FAILURE; } -} \ No newline at end of file +} diff --git a/src/esw/software_h265_encoder/software_h265_encoder.cpp b/src/esw/software_h265_encoder/software_h265_encoder.cpp index 15fdd4671..53cd716ce 100644 --- a/src/esw/software_h265_encoder/software_h265_encoder.cpp +++ b/src/esw/software_h265_encoder/software_h265_encoder.cpp @@ -50,6 +50,7 @@ auto imageCallback(sensor_msgs::ImageConstPtr const& msg) -> void { en265_packet* packet; while ((packet = en265_get_packet(encoder, 0)) != nullptr) { std::span span{std::bit_cast(packet->data), static_cast(packet->length)}; + ROS_INFO_STREAM(span.size()); streamServer->feed(span); en265_free_packet(encoder, packet); } diff --git a/src/esw/streaming/streaming.hpp b/src/esw/streaming/streaming.hpp index 0510655f6..e820b5602 100644 --- a/src/esw/streaming/streaming.hpp +++ b/src/esw/streaming/streaming.hpp @@ -16,7 +16,7 @@ namespace websocket = beast::websocket; using tcp = net::ip::tcp; -struct StreamServer { +class StreamServer { net::io_context m_context; tcp::acceptor m_acceptor; From b377bf84f2f07539655c691812e44ed2f0a81d61 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Mon, 5 Feb 2024 17:55:07 -0500 Subject: [PATCH 55/81] Use bgra instead of i420 --- scripts/build_teleop_stream_client.sh | 5 +++ .../hardware_h265_encoder.cpp | 44 +++++++++++-------- .../hardware_h265_encoder.cu | 38 ++++++++-------- .../hardware_h265_encoder.hpp | 2 +- 4 files changed, 49 insertions(+), 40 deletions(-) create mode 100755 scripts/build_teleop_stream_client.sh diff --git a/scripts/build_teleop_stream_client.sh b/scripts/build_teleop_stream_client.sh new file mode 100755 index 000000000..8faa444fc --- /dev/null +++ b/scripts/build_teleop_stream_client.sh @@ -0,0 +1,5 @@ +#!/bin/bash + +source deps/emsdk/emsdk_env.sh +emcmake cmake -B src/teleoperation/streaming/embuild -G Ninja -DCMAKE_BUILD_TYPE=Release src/teleoperation/streaming/ +cmake --build src/teleoperation/streaming/embuild diff --git a/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp b/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp index 77d6f8ee0..c3e476620 100644 --- a/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp +++ b/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp @@ -3,8 +3,8 @@ #include #include #include -#include #include +#include #include @@ -21,15 +21,10 @@ auto imageCallback(sensor_msgs::ImageConstPtr const& msg) -> void { if (msg->encoding != sensor_msgs::image_encodings::BGRA8) throw std::runtime_error{"Unsupported encoding"}; cv::Size size{static_cast(msg->width), static_cast(msg->height)}; - if (!encoder) encoder.emplace(size); cv::Mat bgraFrame{size, CV_8UC4, const_cast(msg->data.data()), msg->step}; - cv::Mat i420Frame; - cvtColor(bgraFrame, i420Frame, cv::COLOR_BGRA2YUV_IYUV); - // imwrite("/home/quintin/catkin_ws/src/mrover/image.png", i420Frame); - - Encoder::BitstreamView view = encoder->feed(i420Frame); + Encoder::BitstreamView view = encoder->feed(bgraFrame); std::span span{static_cast(view.lockParams.bitstreamBufferPtr), view.lockParams.bitstreamSizeInBytes}; streamServer->feed(span); @@ -39,27 +34,38 @@ auto imageCallback(sensor_msgs::ImageConstPtr const& msg) -> void { } } +auto capture() -> void { + cv::VideoCapture cap{std::format("v4l2src ! videoconvert ! video/x-raw,width={},height={},format=I420,framerate=30/1 ! appsink", 640, 480), cv::CAP_GSTREAMER}; + if (!cap.isOpened()) throw std::runtime_error{"Failed to open capture"}; + + while (cap.isOpened()) { + cv::Mat i420Frame; + if (!cap.read(i420Frame)) throw std::runtime_error{"Failed to read frame"}; + + cv::Mat bgraFrame; + cvtColor(i420Frame, bgraFrame, cv::COLOR_YUV2BGRA_I420); + + Encoder::BitstreamView view = encoder->feed(bgraFrame); + std::span span{static_cast(view.lockParams.bitstreamBufferPtr), view.lockParams.bitstreamSizeInBytes}; + streamServer->feed(span); + + ros::spinOnce(); + } +} + auto main(int argc, char** argv) -> int { try { ros::init(argc, argv, "software_h265_encoder"); ros::NodeHandle nh; - // cv::VideoCapture cap{std::format("v4l2src ! videoconvert ! video/x-raw,width={},height={},format=I420,framerate=10/1 ! appsink", 640, 480), cv::CAP_GSTREAMER}; - // if (!cap.isOpened()) { - // throw std::runtime_error{"Failed to open capture"}; - // } - // - // cv::Mat frame; - // if (!cap.read(frame)) { - // throw std::runtime_error{"Failed to read frame"}; - // } - // - // imwrite("/home/quintin/catkin_ws/src/mrover/image.png", frame); - streamServer.emplace("0.0.0.0", 8080); + encoder.emplace(cv::Size{640, 480}); + ros::Subscriber imageSubscriber = nh.subscribe("/camera/left/image", 1, imageCallback); + // capture(); + ros::spin(); return EXIT_SUCCESS; diff --git a/src/esw/hardware_h265_encoder/hardware_h265_encoder.cu b/src/esw/hardware_h265_encoder/hardware_h265_encoder.cu index 55403c6ea..426026e0c 100644 --- a/src/esw/hardware_h265_encoder/hardware_h265_encoder.cu +++ b/src/esw/hardware_h265_encoder/hardware_h265_encoder.cu @@ -10,12 +10,6 @@ #include #include -void NvCheck(NVENCSTATUS status) { - if (status != NV_ENC_SUCCESS) { - throw std::runtime_error("NvEnc failed"); - } -} - void cuCheck(CUresult status) { if (status != CUDA_SUCCESS) { throw std::runtime_error("CUDA failed"); @@ -28,6 +22,12 @@ void cudaCheck(cudaError_t status) { } } +void NvCheck(NVENCSTATUS status) { + if (status != NV_ENC_SUCCESS) { + throw std::runtime_error("NvEnc failed"); + } +} + namespace std { template<> struct equal_to { @@ -46,8 +46,8 @@ namespace std { seed ^= std::hash{}(g.Data1); seed ^= std::hash{}(g.Data2); seed ^= std::hash{}(g.Data3); - for (std::size_t i = 0; i < 8; ++i) { - seed ^= std::hash{}(g.Data4[i]); + for (unsigned char i: g.Data4) { + seed ^= std::hash{}(i); } return seed; } @@ -60,8 +60,6 @@ std::unordered_map GUID_TO_NAME{ {NV_ENC_CODEC_AV1_GUID, "AV1"}, }; -constexpr NV_ENC_BUFFER_FORMAT FRAME_FORMAT = NV_ENC_BUFFER_FORMAT_IYUV; - Encoder::Encoder(cv::Size const& size) : m_size{size} { cudaCheck(cudaSetDevice(0)); CUcontext context; @@ -138,7 +136,7 @@ Encoder::Encoder(cv::Size const& size) : m_size{size} { .version = NV_ENC_CREATE_INPUT_BUFFER_VER, .width = static_cast(m_size.width), .height = static_cast(m_size.height), - .bufferFmt = FRAME_FORMAT, + .bufferFmt = NV_ENC_BUFFER_FORMAT_ARGB, }; NvCheck(m_nvenc.nvEncCreateInputBuffer(m_encoder, &createInputBufferParams)); m_input = createInputBufferParams.inputBuffer; @@ -150,19 +148,19 @@ Encoder::Encoder(cv::Size const& size) : m_size{size} { m_output = createBitstreamBufferParams.bitstreamBuffer; } -auto Encoder::feed(cv::InputArray frameI420) -> BitstreamView { - if (!frameI420.isContinuous()) throw std::runtime_error("Frame is not continuous"); - if (frameI420.type() != CV_8UC1) throw std::runtime_error("Not single channel, note that YUV420 is expected"); - if (frameI420.size() != cv::Size{m_size.width, m_size.height + m_size.height / 2}) throw std::runtime_error("Wrong size, note that YUV420 is expected"); +auto Encoder::feed(cv::InputArray frameBgra) -> BitstreamView { + if (!frameBgra.isContinuous()) throw std::runtime_error("Frame is not continuous"); + // if (frameI420.type() != CV_8UC1) throw std::runtime_error("Not single channel, note that YUV420 is expected"); + // if (frameI420.size() != cv::Size{m_size.width, m_size.height + m_size.height / 2}) throw std::runtime_error("Wrong size, note that YUV420 is expected"); NV_ENC_LOCK_INPUT_BUFFER lockInputBufferParams{ .version = NV_ENC_LOCK_INPUT_BUFFER_VER, .inputBuffer = m_input, }; NvCheck(m_nvenc.nvEncLockInputBuffer(m_encoder, &lockInputBufferParams)); - for (int r = 0; r < frameI420.rows(); ++r) { + for (int r = 0; r < frameBgra.rows(); ++r) { auto* row = static_cast(lockInputBufferParams.bufferDataPtr) + r * lockInputBufferParams.pitch; - std::memcpy(row, frameI420.getMat().ptr(r), frameI420.cols()); + std::memcpy(row, frameBgra.getMat().ptr(r), frameBgra.cols() * 4); } NvCheck(m_nvenc.nvEncUnlockInputBuffer(m_encoder, m_input)); @@ -171,11 +169,11 @@ auto Encoder::feed(cv::InputArray frameI420) -> BitstreamView { .inputWidth = static_cast(m_size.width), .inputHeight = static_cast(m_size.height), .frameIdx = m_frame_index++, - .inputTimeStamp = static_cast(m_clock.now().time_since_epoch().count()), + .inputTimeStamp = static_cast(std::chrono::high_resolution_clock::now().time_since_epoch().count()), .inputBuffer = m_input, .outputBitstream = m_output, .completionEvent = nullptr, - .bufferFmt = FRAME_FORMAT, + .bufferFmt = NV_ENC_BUFFER_FORMAT_IYUV, .pictureStruct = NV_ENC_PIC_STRUCT_FRAME, }; @@ -199,7 +197,7 @@ Encoder::BitstreamView::BitstreamView(BitstreamView&& other) noexcept { *this = std::move(other); } -auto Encoder::BitstreamView::operator=(BitstreamView&& other) noexcept -> Encoder::BitstreamView& { +auto Encoder::BitstreamView::operator=(BitstreamView&& other) noexcept -> BitstreamView& { std::swap(nvenc, other.nvenc); std::swap(encoder, other.encoder); std::swap(output, other.output); diff --git a/src/esw/hardware_h265_encoder/hardware_h265_encoder.hpp b/src/esw/hardware_h265_encoder/hardware_h265_encoder.hpp index 51219b46b..6ffc3ab0d 100644 --- a/src/esw/hardware_h265_encoder/hardware_h265_encoder.hpp +++ b/src/esw/hardware_h265_encoder/hardware_h265_encoder.hpp @@ -36,7 +36,7 @@ class Encoder { public: explicit Encoder(cv::Size const& size); - [[nodiscard]] auto feed(cv::InputArray frameI420) -> BitstreamView; + [[nodiscard]] auto feed(cv::InputArray frameBgra) -> BitstreamView; ~Encoder(); }; From e971c58642158c304ad37e338cd3798c20a886e2 Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Tue, 13 Feb 2024 17:21:29 -0500 Subject: [PATCH 56/81] partially fixed script --- scripts/toggle_basestation_networking | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/scripts/toggle_basestation_networking b/scripts/toggle_basestation_networking index 114e1dbeb..c4824a57f 100755 --- a/scripts/toggle_basestation_networking +++ b/scripts/toggle_basestation_networking @@ -1,12 +1,21 @@ -# !/usr/bin/env bash +# !/usr/bin/env zsh # Script to add or remove ROS basestation networking script from bashrc # TODO: make this work for bash and other catkin workspace locations -if grep -q "source ~/catkin_ws/src/mrover/ansible/roles/basestation_networks/files/networking_setup_basestation.sh" ~/.zshrc; then +CATKIN_WORKSPACE_PATH=~/catkin_ws +network_script_path="${CATKIN_WORKSPACE_PATH}/src/mrover/ansible/roles/basestation_networks/files/networking_setup_basestation" +enable_message="Basestation network setup enabled, disable with scripts/toggle_basestation_networking" + +if grep -q "source ${network_script_path}" ~/.zshrc; then echo "Removing basestation networking script from .zshrc" - sed -i '/source ~/catkin_ws/src/mrover/ansible/roles/basestation_networks/files/networking_setup_basestation.sh/d' ~/.zshrc + sed -i "\:source ${network_script_path}:d" ~/.zshrc + sed -i "\:echo '${enable_message}':d" ~/.zshrc else echo "Adding basestation networking script to .zshrc" - echo "source ~/catkin_ws/src/mrover/ansible/roles/basestation_networks/files/networking_setup_basestation.sh" >> ~/.zshrc + if [[ $(tail -c1 ~/.zshrc | wc -l) -eq 0 ]]; then + echo "\n" >> ~/.zshrc + fi + echo "echo '${enable_message}'" >> ~/.zshrc + echo "source ${network_script_path}" >> ~/.zshrc fi From 257b684e8665ad6b3f1272d8fb1338dd63c75941 Mon Sep 17 00:00:00 2001 From: nehakankanala Date: Tue, 13 Feb 2024 18:32:42 -0500 Subject: [PATCH 57/81] fixed macos std::for_each error --- src/preload/execution | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/preload/execution b/src/preload/execution index 8d49b61ff..c1e64bf8e 100644 --- a/src/preload/execution +++ b/src/preload/execution @@ -4,6 +4,8 @@ #ifdef __APPLE__ +#include + namespace std { enum class execution { From 686d2e8055de7828f19587f2cea5334cdacabbb9 Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Wed, 14 Feb 2024 23:39:00 -0500 Subject: [PATCH 58/81] more factoring test --- CMakeLists.txt | 3 ++- src/perception/tag_detector/pch.hpp | 2 +- src/perception/tag_detector/tag_detector.processing.cpp | 6 +++--- 3 files changed, 6 insertions(+), 5 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 91d8819ea..c9faac057 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -168,7 +168,8 @@ endif () ## Perception -mrover_add_nodelet(tag_detector src/perception/tag_detector/*.cpp src/perception/tag_detector src/perception/tag_detector/pch.hpp src/util/se3_ros_utils.hpp src/util/se3_ros_utils.cpp) +# mrover_add_nodelet(tag_detector src/perception/tag_detector/*.cpp src/perception/tag_detector src/perception/tag_detector/pch.hpp src/util/se3_ros_utils.hpp src/util/se3_ros_utils.cpp) +mrover_add_nodelet(tag_detector src/perception/tag_detector/*.cpp src/perception/tag_detector src/perception/tag_detector/pch.hpp) mrover_nodelet_link_libraries(tag_detector opencv_core opencv_objdetect opencv_aruco opencv_imgproc opencv_highgui lie manif) if (CUDA_FOUND) diff --git a/src/perception/tag_detector/pch.hpp b/src/perception/tag_detector/pch.hpp index 302a16d34..0838e5bb7 100644 --- a/src/perception/tag_detector/pch.hpp +++ b/src/perception/tag_detector/pch.hpp @@ -35,4 +35,4 @@ #include #include #include -#include +// #include diff --git a/src/perception/tag_detector/tag_detector.processing.cpp b/src/perception/tag_detector/tag_detector.processing.cpp index 15054d389..0a1e9c4dc 100644 --- a/src/perception/tag_detector/tag_detector.processing.cpp +++ b/src/perception/tag_detector/tag_detector.processing.cpp @@ -89,7 +89,7 @@ namespace mrover { if (tag.tagInCam) { // Publish tag to immediate std::string immediateFrameId = "immediateFiducial" + std::to_string(tag.id); - pushSE3ToTfTree(mTfBroadcaster, immediateFrameId, mCameraFrameId, tag.tagInCam.value()); + SE3Conversions::pushToTfTree(mTfBroadcaster, immediateFrameId, mCameraFrameId, tag.tagInCam.value()); } } @@ -116,8 +116,8 @@ namespace mrover { std::string immediateFrameId = "immediateFiducial" + std::to_string(tag.id); // Publish tag to odom std::string const& parentFrameId = mUseOdom ? mOdomFrameId : mMapFrameId; - SE3d tagInParent = SE3fromTfTree(mTfBuffer, parentFrameId, immediateFrameId); - pushSE3ToTfTree(mTfBroadcaster, "fiducial" + std::to_string(id), parentFrameId, tagInParent); + SE3d tagInParent = SE3Conversions::fromTfTree(mTfBuffer, parentFrameId, immediateFrameId); + SE3Conversions::pushToTfTree(mTfBroadcaster, "fiducial" + std::to_string(id), parentFrameId, tagInParent); } catch (tf2::ExtrapolationException const&) { NODELET_WARN("Old data for immediate tag"); } catch (tf2::LookupException const&) { From 39ef5d2e03a532d677b2ca425f0960c96530c543 Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Wed, 14 Feb 2024 23:47:32 -0500 Subject: [PATCH 59/81] refactored SE3 util functions back into lie library --- src/util/lie/se3.hpp | 22 ++++++++++++ src/util/lie/se3_conversion.cpp | 62 ++++++++++++++++++++++++++++++++- 2 files changed, 83 insertions(+), 1 deletion(-) diff --git a/src/util/lie/se3.hpp b/src/util/lie/se3.hpp index 923192188..444766243 100644 --- a/src/util/lie/se3.hpp +++ b/src/util/lie/se3.hpp @@ -11,6 +11,9 @@ #include #include +#include + +using manif::SE3d; using R3 = Eigen::Vector3d; using S3 = Eigen::Quaterniond; @@ -98,6 +101,25 @@ class SE3 { EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; +class SE3Conversions { +public: + static SE3d fromTf(geometry_msgs::Transform const& transform); + + static SE3d fromPose(geometry_msgs::Pose const& pose); + + [[nodiscard]] static geometry_msgs::Pose toPose(SE3d const& tf); + + [[nodiscard]] static geometry_msgs::Transform toTransform(SE3d const& tf); + + [[nodiscard]] static geometry_msgs::PoseStamped toPoseStamped(SE3d const& tf, std::string const& frameId); + + [[nodiscard]] static geometry_msgs::TransformStamped toTransformStamped(SE3d const& tf, std::string const& parentFrameId, std::string const& childFrameId); + + [[nodiscard]] static SE3d fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId); + + static void pushToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3d const& tf); +}; + class SIM3 { using Transform = Eigen::Transform; diff --git a/src/util/lie/se3_conversion.cpp b/src/util/lie/se3_conversion.cpp index 44a28774f..b3f031102 100644 --- a/src/util/lie/se3_conversion.cpp +++ b/src/util/lie/se3_conversion.cpp @@ -1,5 +1,65 @@ #include "se3.hpp" +SE3d SE3Conversions::fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId) { + geometry_msgs::TransformStamped transform = buffer.lookupTransform(fromFrameId, toFrameId, ros::Time{}); + return fromTf(transform.transform); +} + +void SE3Conversions::pushToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3d const& tf) { + broadcaster.sendTransform(toTransformStamped(tf, parentFrameId, childFrameId)); +} + +SE3d SE3Conversions::fromTf(geometry_msgs::Transform const& transform) { + return {{transform.translation.x, transform.translation.y, transform.translation.z}, + Eigen::Quaterniond{transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z}}; +} + +SE3d SE3Conversions::fromPose(geometry_msgs::Pose const& pose) { + return {{pose.position.x, pose.position.y, pose.position.z}, + Eigen::Quaterniond{pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z}}; +} + +geometry_msgs::Pose SE3Conversions::toPose(SE3d const& tf) { + geometry_msgs::Pose pose; + pose.position.x = tf.x(); + pose.position.y = tf.y(); + pose.position.z = tf.z(); + pose.orientation.x = tf.quat().x(); + pose.orientation.y = tf.quat().y(); + pose.orientation.z = tf.quat().z(); + pose.orientation.w = tf.quat().w(); + return pose; +} + +geometry_msgs::Transform SE3Conversions::toTransform(SE3d const& tf) { + geometry_msgs::Transform transform; + transform.translation.x = tf.x(); + transform.translation.y = tf.y(); + transform.translation.z = tf.z(); + transform.rotation.x = tf.quat().x(); + transform.rotation.y = tf.quat().y(); + transform.rotation.z = tf.quat().z(); + transform.rotation.w = tf.quat().w(); + return transform; +} + +geometry_msgs::PoseStamped SE3Conversions::toPoseStamped(SE3d const& tf, std::string const& frameId) { + geometry_msgs::PoseStamped pose; + pose.pose = toPose(tf); + pose.header.frame_id = frameId; + pose.header.stamp = ros::Time::now(); + return pose; +} + +geometry_msgs::TransformStamped SE3Conversions::toTransformStamped(SE3d const& tf, std::string const& parentFrameId, std::string const& childFrameId) { + geometry_msgs::TransformStamped transform; + transform.transform = toTransform(tf); + transform.header.frame_id = parentFrameId; + transform.header.stamp = ros::Time::now(); + transform.child_frame_id = childFrameId; + return transform; +} + SE3 SE3::fromTf(geometry_msgs::Transform const& transform) { return {{transform.translation.x, transform.translation.y, transform.translation.z}, {Eigen::Quaterniond{transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z}}}; @@ -42,7 +102,7 @@ geometry_msgs::PoseStamped SE3::toPoseStamped(std::string const& frameId) const return pose; } -geometry_msgs::TransformStamped SE3::toTransformStamped(const std::string& parentFrameId, const std::string& childFrameId) const { +geometry_msgs::TransformStamped SE3::toTransformStamped(std::string const& parentFrameId, std::string const& childFrameId) const { geometry_msgs::TransformStamped transform; transform.transform = toTransform(); transform.header.frame_id = parentFrameId; From 77d3d1576d857cbe2acbff26351b51cb7378d1e0 Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Thu, 15 Feb 2024 01:40:26 -0500 Subject: [PATCH 60/81] refactored most of sim --- src/perception/tag_detector/tag_detector.cpp | 62 -------------------- src/perception/tag_detector/tag_detector.hpp | 16 ----- src/perception/zed_wrapper/zed_wrapper.cpp | 4 +- src/simulator/simulator.controls.cpp | 25 +++++--- src/simulator/simulator.gui.cpp | 12 ++-- src/simulator/simulator.hpp | 8 +-- src/simulator/simulator.physics.cpp | 10 ++-- src/simulator/simulator.render.cpp | 28 ++++----- src/simulator/simulator.sensors.cpp | 26 ++++---- src/util/lie/se3.hpp | 2 +- 10 files changed, 61 insertions(+), 132 deletions(-) diff --git a/src/perception/tag_detector/tag_detector.cpp b/src/perception/tag_detector/tag_detector.cpp index fbcb7bf70..1d1d90b4c 100644 --- a/src/perception/tag_detector/tag_detector.cpp +++ b/src/perception/tag_detector/tag_detector.cpp @@ -140,66 +140,4 @@ namespace mrover { res.success = true; return true; } - - // SE3d TagDetectorNodelet::SE3fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId) { - // geometry_msgs::TransformStamped transform = buffer.lookupTransform(fromFrameId, toFrameId, ros::Time{}); - // return SE3fromTf(transform.transform); - // } - - // void TagDetectorNodelet::pushSE3ToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3d const& tf) { - // broadcaster.sendTransform(SE3toTransformStamped(tf, parentFrameId, childFrameId)); - // } - - // SE3d TagDetectorNodelet::SE3fromTf(geometry_msgs::Transform const& transform) { - // return {{transform.translation.x, transform.translation.y, transform.translation.z}, - // Eigen::Quaterniond{transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z}}; - // } - - // SE3d TagDetectorNodelet::SE3fromPose(geometry_msgs::Pose const& pose) { - // return {{pose.position.x, pose.position.y, pose.position.z}, - // Eigen::Quaterniond{pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z}}; - // } - - // geometry_msgs::Pose TagDetectorNodelet::SE3toPose(SE3d const& tf) { - // geometry_msgs::Pose pose; - // pose.position.x = tf.x(); - // pose.position.y = tf.y(); - // pose.position.z = tf.z(); - // pose.orientation.x = tf.quat().x(); - // pose.orientation.y = tf.quat().y(); - // pose.orientation.z = tf.quat().z(); - // pose.orientation.w = tf.quat().w(); - // return pose; - // } - - // geometry_msgs::Transform TagDetectorNodelet::SE3toTransform(SE3d const& tf) { - // geometry_msgs::Transform transform; - // transform.translation.x = tf.x(); - // transform.translation.y = tf.y(); - // transform.translation.z = tf.z(); - // transform.rotation.x = tf.quat().x(); - // transform.rotation.y = tf.quat().y(); - // transform.rotation.z = tf.quat().z(); - // transform.rotation.w = tf.quat().w(); - // return transform; - // } - - // geometry_msgs::PoseStamped TagDetectorNodelet::SE3toPoseStamped(SE3d const& tf, std::string const& frameId) { - // geometry_msgs::PoseStamped pose; - // pose.pose = SE3toPose(tf); - // pose.header.frame_id = frameId; - // pose.header.stamp = ros::Time::now(); - // return pose; - // } - - // geometry_msgs::TransformStamped TagDetectorNodelet::SE3toTransformStamped(SE3d const& tf, std::string const& parentFrameId, std::string const& childFrameId) { - // geometry_msgs::TransformStamped transform; - // transform.transform = SE3toTransform(tf); - // transform.header.frame_id = parentFrameId; - // transform.header.stamp = ros::Time::now(); - // transform.child_frame_id = childFrameId; - // return transform; - // } - - } // namespace mrover diff --git a/src/perception/tag_detector/tag_detector.hpp b/src/perception/tag_detector/tag_detector.hpp index 28e3c4dc5..457ec41e4 100644 --- a/src/perception/tag_detector/tag_detector.hpp +++ b/src/perception/tag_detector/tag_detector.hpp @@ -67,22 +67,6 @@ namespace mrover { void configCallback(mrover::DetectorParamsConfig& config, uint32_t level); bool enableDetectionsCallback(std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res); - - // SE3d SE3fromTf(geometry_msgs::Transform const& transform); - - // SE3d SE3fromPose(geometry_msgs::Pose const& pose); - - // [[nodiscard]] geometry_msgs::Pose SE3toPose(SE3d const& tf); - - // [[nodiscard]] geometry_msgs::Transform SE3toTransform(SE3d const& tf); - - // [[nodiscard]] geometry_msgs::PoseStamped SE3toPoseStamped(SE3d const& tf, std::string const& frameId); - - // [[nodiscard]] geometry_msgs::TransformStamped SE3toTransformStamped(SE3d const& tf, std::string const& parentFrameId, std::string const& childFrameId); - - // [[nodiscard]] SE3d SE3fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId); - - // void pushSE3ToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3d const& tf); }; } // namespace mrover diff --git a/src/perception/zed_wrapper/zed_wrapper.cpp b/src/perception/zed_wrapper/zed_wrapper.cpp index eb806419a..89cdbc53c 100644 --- a/src/perception/zed_wrapper/zed_wrapper.cpp +++ b/src/perception/zed_wrapper/zed_wrapper.cpp @@ -254,9 +254,9 @@ namespace mrover { try { SE3d leftCameraInOdom{{translation.x, translation.y, translation.z}, Eigen::Quaterniond{orientation.w, orientation.x, orientation.y, orientation.z}.normalized()}; - SE3d leftCameraInBaseLink = SE3fromTfTree(mTfBuffer, "base_link", "zed2i_left_camera_frame"); + SE3d leftCameraInBaseLink = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "zed2i_left_camera_frame"); SE3d baseLinkInOdom = leftCameraInBaseLink * leftCameraInOdom; - pushSE3ToTfTree(mTfBroadcaster, "base_link", "odom", baseLinkInOdom); + SE3Conversions::pushToTfTree(mTfBroadcaster, "base_link", "odom", baseLinkInOdom); } catch (tf2::TransformException& e) { NODELET_WARN_STREAM("Failed to get transform: " << e.what()); } diff --git a/src/simulator/simulator.controls.cpp b/src/simulator/simulator.controls.cpp index 10dc71c8d..e63106f43 100644 --- a/src/simulator/simulator.controls.cpp +++ b/src/simulator/simulator.controls.cpp @@ -1,5 +1,7 @@ #include "simulator.hpp" +using manif::SE3d, manif::SO3d; + namespace mrover { auto SimulatorNodelet::twistCallback(geometry_msgs::Twist::ConstPtr const& twist) -> void { @@ -77,22 +79,22 @@ namespace mrover { auto SimulatorNodelet::freeLook(Clock::duration dt) -> void { float flySpeed = mFlySpeed * std::chrono::duration_cast>(dt).count(); if (glfwGetKey(mWindow.get(), mCamRightKey) == GLFW_PRESS) { - mCameraInWorld = SE3{R3{0.0, -flySpeed, 0}, SO3{}} * mCameraInWorld; + mCameraInWorld = SE3d{R3{0.0, -flySpeed, 0}, SO3d{}} * mCameraInWorld; } if (glfwGetKey(mWindow.get(), mCamLeftKey) == GLFW_PRESS) { - mCameraInWorld = SE3{R3{0.0, flySpeed, 0}, SO3{}} * mCameraInWorld; + mCameraInWorld = SE3d{R3{0.0, flySpeed, 0}, SO3d{}} * mCameraInWorld; } if (glfwGetKey(mWindow.get(), mCamForwardKey) == GLFW_PRESS) { - mCameraInWorld = SE3{R3{flySpeed, 0.0, 0.0}, SO3{}} * mCameraInWorld; + mCameraInWorld = SE3d{R3{flySpeed, 0.0, 0.0}, SO3d{}} * mCameraInWorld; } if (glfwGetKey(mWindow.get(), mCamBackwardKey) == GLFW_PRESS) { - mCameraInWorld = SE3{R3{-flySpeed, 0.0, 0.0}, SO3{}} * mCameraInWorld; + mCameraInWorld = SE3d{R3{-flySpeed, 0.0, 0.0}, SO3d{}} * mCameraInWorld; } if (glfwGetKey(mWindow.get(), mCamUpKey) == GLFW_PRESS) { - mCameraInWorld = mCameraInWorld * SE3{R3{0.0, 0.0, flySpeed}, SO3{}}; + mCameraInWorld = mCameraInWorld * SE3d{R3{0.0, 0.0, flySpeed}, SO3d{}}; } if (glfwGetKey(mWindow.get(), mCamDownKey) == GLFW_PRESS) { - mCameraInWorld = mCameraInWorld * SE3{R3{0.0, 0.0, -flySpeed}, SO3{}}; + mCameraInWorld = mCameraInWorld * SE3d{R3{0.0, 0.0, -flySpeed}, SO3d{}}; } Eigen::Vector2i size; @@ -106,9 +108,14 @@ namespace mrover { Eigen::Vector2d delta = (mouse - center) * mLookSense; // TODO(quintin): use lie algebra more here? we have a perturbation in the tangent space - R3 p = mCameraInWorld.position(); - SO3 q = SO3{delta.y(), Eigen::Vector3d::UnitY()} * mCameraInWorld.rotation() * SO3{-delta.x(), Eigen::Vector3d::UnitZ()}; - mCameraInWorld = SE3{p, q}; + R3 p = mCameraInWorld.translation(); + Eigen::Matrix3d Ry = Eigen::AngleAxisd{delta.y(), Eigen::Vector3d::UnitY()}.toRotationMatrix(); + Eigen::Matrix3d Rz = Eigen::AngleAxisd{-delta.x(), Eigen::Vector3d::UnitZ()}.toRotationMatrix(); + Eigen::Matrix3d R0 = mCameraInWorld.rotation(); + static_assert(R0.RowsAtCompileTime == 3 && R0.ColsAtCompileTime == 3); + Eigen::Matrix3d R = Ry * mCameraInWorld.rotation() * Rz; + // SO3d q{SO3d{Eigen::AngleAxisd{delta.y(), Eigen::Vector3d::UnitY()}}.rotation() * mCameraInWorld.rotation() * SO3d{Eigen::AngleAxisd{-delta.x(), Eigen::Vector3d::UnitZ()}}.rotation()}; + mCameraInWorld = SE3d{p, SO3d{Eigen::Quaterniond(R)}}; centerCursor(); } diff --git a/src/simulator/simulator.gui.cpp b/src/simulator/simulator.gui.cpp index 1574658a8..d0b1a69fb 100644 --- a/src/simulator/simulator.gui.cpp +++ b/src/simulator/simulator.gui.cpp @@ -90,15 +90,15 @@ namespace mrover { URDF const& rover = it->second; { - SE3 baseLinkInMap = rover.linkInWorld("base_link"); - R3 p = baseLinkInMap.position(); - S3 q = baseLinkInMap.rotation().quaternion(); + SE3d baseLinkInMap = rover.linkInWorld("base_link"); + R3 p = baseLinkInMap.translation(); + S3 q = baseLinkInMap.quat(); ImGui::Text("Rover Position: (%.2f, %.2f, %.2f)", p.x(), p.y(), p.z()); ImGui::Text("Rover Orientation: (%.2f, %.2f, %.2f, %.2f)", q.w(), q.x(), q.y(), q.z()); } { - R3 p = mCameraInWorld.position(); - S3 q = mCameraInWorld.rotation().quaternion(); + R3 p = mCameraInWorld.translation(); + S3 q = mCameraInWorld.quat(); ImGui::Text("Camera Position: (%.2f, %.2f, %.2f)", p.x(), p.y(), p.z()); ImGui::Text("Camera Orientation: (%.2f, %.2f, %.2f, %.2f)", q.w(), q.x(), q.y(), q.z()); } @@ -114,7 +114,7 @@ namespace mrover { } { - R3 rayStart = mCameraInWorld.position(); + R3 rayStart = mCameraInWorld.translation(); R3 rayEnd = rayStart + mCameraInWorld.rotation().matrix().col(0); // btMul // mDynamicsWorld->rayTest(r3ToBtVector3(rayStart), r3ToBtVector3(rayEnd), [ diff --git a/src/simulator/simulator.hpp b/src/simulator/simulator.hpp index dc7904a12..f04cbb2f7 100644 --- a/src/simulator/simulator.hpp +++ b/src/simulator/simulator.hpp @@ -2,8 +2,8 @@ #include "pch.hpp" -#include "wgpu_objects.hpp" #include "glfw_pointer.hpp" +#include "wgpu_objects.hpp" using namespace std::literals; @@ -98,7 +98,7 @@ namespace mrover { auto makeCameraForLink(SimulatorNodelet& simulator, btMultibodyLink const* link) -> Camera; - [[nodiscard]] auto linkInWorld(std::string const& linkName) const -> SE3; + [[nodiscard]] auto linkInWorld(std::string const& linkName) const -> SE3d; }; struct PeriodicTask { @@ -291,7 +291,7 @@ namespace mrover { return it->second; } - SE3 mCameraInWorld{R3{-3.0, 0.0, 1.5}, SO3{}}; + SE3d mCameraInWorld{R3{-3.0, 0.0, 1.5}, SO3d{}}; std::vector mCameras; @@ -370,7 +370,7 @@ namespace mrover { auto urdfPoseToBtTransform(urdf::Pose const& pose) -> btTransform; - auto btTransformToSe3(btTransform const& transform) -> SE3; + auto btTransformToSe3(btTransform const& transform) -> SE3d; auto computeCameraToClip(float fovY, float aspect, float zNear, float zFar) -> Eigen::Matrix4f; diff --git a/src/simulator/simulator.physics.cpp b/src/simulator/simulator.physics.cpp index fd178f5ac..d600aa418 100644 --- a/src/simulator/simulator.physics.cpp +++ b/src/simulator/simulator.physics.cpp @@ -2,10 +2,10 @@ namespace mrover { - auto btTransformToSe3(btTransform const& transform) -> SE3 { + auto btTransformToSe3(btTransform const& transform) -> SE3d { btVector3 const& p = transform.getOrigin(); btQuaternion const& q = transform.getRotation(); - return SE3{R3{p.x(), p.y(), p.z()}, SO3{q.w(), q.x(), q.y(), q.z()}}; + return SE3d{R3{p.x(), p.y(), p.z()}, SO3d{q.w(), q.x(), q.y(), q.z()}}; } auto SimulatorNodelet::initPhysics() -> void { @@ -75,9 +75,9 @@ namespace mrover { int index = urdf.linkNameToMeta.at(link->name).index; // TODO(quintin): figure out why we need to negate rvector btTransform parentToChild{urdf.physics->getParentToLocalRot(index), -urdf.physics->getRVector(index)}; - SE3 childInParent = btTransformToSe3(parentToChild.inverse()); + SE3d childInParent = btTransformToSe3(parentToChild.inverse()); - SE3::pushToTfTree(mTfBroadcaster, link->name, link->getParent()->name, childInParent); + SE3Conversions::pushToTfTree(mTfBroadcaster, link->name, link->getParent()->name, childInParent); } for (urdf::JointSharedPtr const& child_joint: link->child_joints) { @@ -89,7 +89,7 @@ namespace mrover { } } - auto URDF::linkInWorld(std::string const& linkName) const -> SE3 { + auto URDF::linkInWorld(std::string const& linkName) const -> SE3d { int index = linkNameToMeta.at(linkName).index; return btTransformToSe3(index == -1 ? physics->getBaseWorldTransform() : physics->getLink(index).m_cachedWorldTransform); } diff --git a/src/simulator/simulator.render.cpp b/src/simulator/simulator.render.cpp index a0dedec86..aacaaa707 100644 --- a/src/simulator/simulator.render.cpp +++ b/src/simulator/simulator.render.cpp @@ -410,10 +410,10 @@ namespace mrover { if (auto urdfMesh = std::dynamic_pointer_cast(visual->geometry)) { Model& model = mUriToModel.at(urdfMesh->filename); URDF::LinkMeta& meta = urdf.linkNameToMeta.at(link->name); - SE3 linkInWorld = urdf.linkInWorld(link->name); - SE3 modelInLink = btTransformToSe3(urdfPoseToBtTransform(link->visual->origin)); - SE3 modelInWorld = linkInWorld * modelInLink; - renderModel(pass, model, meta.visualUniforms.at(visualIndex), SIM3{modelInWorld.position(), modelInWorld.rotation(), R3::Ones()}); + SE3d linkInWorld = urdf.linkInWorld(link->name); + SE3d modelInLink = btTransformToSe3(urdfPoseToBtTransform(link->visual->origin)); + SE3d modelInWorld = linkInWorld * modelInLink; + renderModel(pass, model, meta.visualUniforms.at(visualIndex), SIM3{modelInWorld.translation(), modelInWorld.rotation(), R3::Ones()}); visualIndex++; } } @@ -427,7 +427,7 @@ namespace mrover { } } - auto SimulatorNodelet::renderWireframeColliders(wgpu::RenderPassEncoder& pass) -> void { + auto SimulatorNodelet::renderWireframeColliders(wgpu::position& pass) -> void { pass.setPipeline(mWireframePipeline); for (auto& [_, urdf]: mUrdfs) { @@ -440,27 +440,27 @@ namespace mrover { assert(collider); btCollisionShape* shape = collider->getCollisionShape(); assert(shape); - SE3 linkInWorld = urdf.linkInWorld(link->name); + SE3d linkInWorld = urdf.linkInWorld(link->name); if (auto* compound = dynamic_cast(shape)) { for (int i = 0; i < compound->getNumChildShapes(); ++i) { - SE3 modelInLink = btTransformToSe3(urdfPoseToBtTransform(link->collision_array.at(i)->origin)); - SE3 shapeToWorld = modelInLink * linkInWorld; + SE3d modelInLink = btTransformToSe3(urdfPoseToBtTransform(link->collision_array.at(i)->origin)); + SE3d shapeToWorld = modelInLink * linkInWorld; auto* shape = compound->getChildShape(i); if (auto* box = dynamic_cast(shape)) { btVector3 extents = box->getHalfExtentsWithoutMargin() * 2; - SIM3 modelToWorld{shapeToWorld.position(), shapeToWorld.rotation(), R3{extents.x(), extents.y(), extents.z()}}; + SIM3 modelToWorld{shapeToWorld.translation(), shapeToWorld.rotation(), R3{extents.x(), extents.y(), extents.z()}}; renderModel(pass, mUriToModel.at(CUBE_PRIMITIVE_URI), meta.collisionUniforms.at(i), modelToWorld); } else if (auto* sphere = dynamic_cast(shape)) { btScalar diameter = sphere->getRadius() * 2; - SIM3 modelToWorld{shapeToWorld.position(), shapeToWorld.rotation(), R3{diameter, diameter, diameter}}; + SIM3 modelToWorld{shapeToWorld.translation(), shapeToWorld.rotation(), R3{diameter, diameter, diameter}}; renderModel(pass, mUriToModel.at(SPHERE_PRIMITIVE_URI), meta.collisionUniforms.at(i), modelToWorld); } else if (auto* cylinder = dynamic_cast(shape)) { btVector3 extents = cylinder->getHalfExtentsWithoutMargin() * 2; - SIM3 modelToWorld{shapeToWorld.position(), shapeToWorld.rotation(), R3{extents.x(), extents.y(), extents.z()}}; + SIM3 modelToWorld{shapeToWorld.translation(), shapeToWorld.rotation(), R3{extents.x(), extents.y(), extents.z()}}; renderModel(pass, mUriToModel.at(CYLINDER_PRIMITIVE_URI), meta.collisionUniforms.at(i), modelToWorld); } else if (auto* mesh = dynamic_cast(shape)) { - SIM3 modelToWorld{shapeToWorld.position(), shapeToWorld.rotation(), R3::Ones()}; + SIM3 modelToWorld{shapeToWorld.translation(), shapeToWorld.rotation(), R3::Ones()}; renderModel(pass, mUriToModel.at(mMeshToUri.at(const_cast(mesh))), meta.collisionUniforms.at(i), modelToWorld); } else { NODELET_WARN_STREAM_ONCE(std::format("Tried to render unsupported collision shape: {}", shape->getName())); @@ -573,8 +573,8 @@ namespace mrover { glfwGetFramebufferSize(mWindow.get(), &width, &height); float aspect = static_cast(width) / static_cast(height); mSceneUniforms.value.cameraToClip = computeCameraToClip(mFovDegrees * DEG_TO_RAD, aspect, NEAR, FAR).cast(); - mSceneUniforms.value.worldToCamera = mCameraInWorld.matrix().inverse().cast(); - mSceneUniforms.value.cameraInWorld = mCameraInWorld.position().cast().homogeneous(); + mSceneUniforms.value.worldToCamera = mCameraInWorld.inverse().transform().cast(); + mSceneUniforms.value.cameraInWorld = mCameraInWorld.translation().cast().homogeneous(); mSceneUniforms.enqueueWrite(); wgpu::BindGroupEntry entry; diff --git a/src/simulator/simulator.sensors.cpp b/src/simulator/simulator.sensors.cpp index 24eb27fbe..895063ea2 100644 --- a/src/simulator/simulator.sensors.cpp +++ b/src/simulator/simulator.sensors.cpp @@ -12,9 +12,9 @@ namespace mrover { camera.sceneUniforms.value.lightInWorld = {0, 0, 5, 1}; float aspect = static_cast(camera.resolution.x()) / static_cast(camera.resolution.y()); camera.sceneUniforms.value.cameraToClip = computeCameraToClip(mFovDegrees * DEG_TO_RAD, aspect, NEAR, FAR).cast(); - SE3 cameraInWorld = btTransformToSe3(camera.link->m_cachedWorldTransform); - camera.sceneUniforms.value.worldToCamera = cameraInWorld.matrix().inverse().cast(); - camera.sceneUniforms.value.cameraInWorld = cameraInWorld.position().cast().homogeneous(); + SE3d cameraInWorld = btTransformToSe3(camera.link->m_cachedWorldTransform); + camera.sceneUniforms.value.worldToCamera = cameraInWorld.inverse().transform().cast(); + camera.sceneUniforms.value.cameraInWorld = cameraInWorld.translation().cast().homogeneous(); camera.sceneUniforms.enqueueWrite(); wgpu::BindGroupEntry entry; @@ -112,22 +112,22 @@ namespace mrover { return {lat, lon, alt}; } - auto computeNavSatFix(SE3 const& gpsInMap, Eigen::Vector3d const& referenceGeodetic, double referenceHeadingDegrees) -> sensor_msgs::NavSatFix { + auto computeNavSatFix(SE3d const& gpsInMap, Eigen::Vector3d const& referenceGeodetic, double referenceHeadingDegrees) -> sensor_msgs::NavSatFix { sensor_msgs::NavSatFix gpsMessage; gpsMessage.header.stamp = ros::Time::now(); gpsMessage.header.frame_id = "map"; - auto geodetic = cartesianToGeodetic(gpsInMap.position(), referenceGeodetic, referenceHeadingDegrees); + auto geodetic = cartesianToGeodetic(gpsInMap.translation(), referenceGeodetic, referenceHeadingDegrees); gpsMessage.latitude = geodetic(0); gpsMessage.longitude = geodetic(1); gpsMessage.altitude = geodetic(2); return gpsMessage; } - auto computeImu(SE3 const& imuInMap, R3 const& imuAngularVelocity, R3 const& linearAcceleration, R3 const& magneticField) -> ImuAndMag { + auto computeImu(SE3d const& imuInMap, R3 const& imuAngularVelocity, R3 const& linearAcceleration, R3 const& magneticField) -> ImuAndMag { ImuAndMag imuMessage; imuMessage.header.stamp = ros::Time::now(); imuMessage.header.frame_id = "map"; - S3 q = imuInMap.rotation().quaternion(); + S3 q = imuInMap.quat(); imuMessage.imu.orientation.w = q.w(); imuMessage.imu.orientation.x = q.x(); imuMessage.imu.orientation.y = q.y(); @@ -153,15 +153,15 @@ namespace mrover { URDF const& rover = *lookup; if (mLinearizedPosePub) { - SE3 baseLinkInMap = rover.linkInWorld("base_link"); + SE3d baseLinkInMap = rover.linkInWorld("base_link"); geometry_msgs::PoseWithCovarianceStamped pose; pose.header.stamp = ros::Time::now(); pose.header.frame_id = "map"; - R3 p = baseLinkInMap.position(); + R3 p = baseLinkInMap.translation(); pose.pose.pose.position.x = p.x(); pose.pose.pose.position.y = p.y(); pose.pose.pose.position.z = p.z(); - S3 q = baseLinkInMap.rotation().quaternion(); + S3 q = baseLinkInMap.quat(); pose.pose.pose.orientation.w = q.w(); pose.pose.pose.orientation.x = q.x(); pose.pose.pose.orientation.y = q.y(); @@ -171,10 +171,10 @@ namespace mrover { } if (mGpsTask.shouldUpdate()) { - SE3 leftGpsInMap = rover.linkInWorld("left_gps"); + SE3d leftGpsInMap = rover.linkInWorld("left_gps"); mLeftGpsPub.publish(computeNavSatFix(leftGpsInMap, mGpsLinerizationReferencePoint, mGpsLinerizationReferenceHeading)); - SE3 rightGpsInMap = rover.linkInWorld("right_gps"); + SE3d rightGpsInMap = rover.linkInWorld("right_gps"); mRightGpsPub.publish(computeNavSatFix(rightGpsInMap, mGpsLinerizationReferencePoint, mGpsLinerizationReferenceHeading)); } if (mImuTask.shouldUpdate()) { @@ -182,7 +182,7 @@ namespace mrover { R3 roverLinearVelocity = btVector3ToR3(rover.physics->getBaseVel()); R3 imuLinearAcceleration = (roverLinearVelocity - mRoverLinearVelocity) / std::chrono::duration_cast>(dt).count(); mRoverLinearVelocity = roverLinearVelocity; - SE3 imuInMap = rover.linkInWorld("imu"); + SE3d imuInMap = rover.linkInWorld("imu"); mImuPub.publish(computeImu(imuInMap, imuAngularVelocity, imuLinearAcceleration, imuInMap.rotation().matrix().transpose().col(1))); } } diff --git a/src/util/lie/se3.hpp b/src/util/lie/se3.hpp index 444766243..d40f841f7 100644 --- a/src/util/lie/se3.hpp +++ b/src/util/lie/se3.hpp @@ -13,7 +13,7 @@ #include #include -using manif::SE3d; +using manif::SE3d, manif::SO3d; using R3 = Eigen::Vector3d; using S3 = Eigen::Quaterniond; From 8f540a6f697e5a628436aef6a7357a98ebfb74c0 Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Thu, 15 Feb 2024 12:12:38 -0500 Subject: [PATCH 61/81] more debugging --- launch/new_sim.launch | 4 ++-- src/simulator/simulator.hpp | 5 ++++- src/simulator/simulator.render.cpp | 2 +- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/launch/new_sim.launch b/launch/new_sim.launch index 0b36b3f15..7c11f1a02 100644 --- a/launch/new_sim.launch +++ b/launch/new_sim.launch @@ -9,8 +9,8 @@ - + \ No newline at end of file diff --git a/src/simulator/simulator.hpp b/src/simulator/simulator.hpp index f04cbb2f7..8e606db55 100644 --- a/src/simulator/simulator.hpp +++ b/src/simulator/simulator.hpp @@ -291,7 +291,10 @@ namespace mrover { return it->second; } - SE3d mCameraInWorld{R3{-3.0, 0.0, 1.5}, SO3d{}}; + SO3d test = SO3d::Identity(); + // SE3d mCameraInWorld{R3{-3.0, 0.0, 1.5}, test}; + SE3d mCameraInWorld{R3{-3.0, 0.0, 1.5}, SO3d{0, 0, 0, 1}}; + // SE3d mCameraInWorld{R3{-3.0, 0.0, 1.5}, SO3d::Identity()}; std::vector mCameras; diff --git a/src/simulator/simulator.render.cpp b/src/simulator/simulator.render.cpp index aacaaa707..1c133f273 100644 --- a/src/simulator/simulator.render.cpp +++ b/src/simulator/simulator.render.cpp @@ -427,7 +427,7 @@ namespace mrover { } } - auto SimulatorNodelet::renderWireframeColliders(wgpu::position& pass) -> void { + auto SimulatorNodelet::renderWireframeColliders(wgpu::RenderPassEncoder& pass) -> void { pass.setPipeline(mWireframePipeline); for (auto& [_, urdf]: mUrdfs) { From 15065f2c894428895d35d625663c5b2e7c29c8d9 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Thu, 15 Feb 2024 13:05:41 -0500 Subject: [PATCH 62/81] Add manif as submodule, find out some disgustig things about transforms, normalize some vars before so3d ctor --- .gitmodules | 4 +++ CMakeLists.txt | 6 ++--- deps/manif | 1 + launch/new_sim.launch | 4 +-- src/perception/zed_wrapper/pch.hpp | 2 -- .../zed_wrapper/zed_wrapper.bridge.cu | 1 - src/perception/zed_wrapper/zed_wrapper.cpp | 7 ++++-- src/simulator/simulator.controls.cpp | 25 ++++++++----------- src/simulator/simulator.hpp | 5 +--- src/simulator/simulator.physics.cpp | 6 +++-- 10 files changed, 31 insertions(+), 30 deletions(-) create mode 160000 deps/manif diff --git a/.gitmodules b/.gitmodules index bc116e39e..beb75e894 100644 --- a/.gitmodules +++ b/.gitmodules @@ -6,3 +6,7 @@ path = deps/dawn url = https://dawn.googlesource.com/dawn shallow = true +[submodule "deps/manif"] + path = deps/manif + url = https://github.com/artivis/manif.git + shallow = true diff --git a/CMakeLists.txt b/CMakeLists.txt index c9faac057..9518bacaf 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -73,7 +73,7 @@ set(MROVER_CMAKE_INCLUDES if (MROVER_BUILD_SIM) include(cmake/webgpu.cmake) - add_subdirectory(deps/glfw3webgpu) + add_subdirectory(deps/glfw3webgpu SYSTEM) find_package(Assimp REQUIRED) find_package(Bullet REQUIRED) find_package(glfw3 REQUIRED) @@ -81,7 +81,7 @@ endif () find_package(OpenCV REQUIRED) find_package(ZED QUIET) find_package(Eigen3 REQUIRED) -find_package(manif REQUIRED) +add_subdirectory(deps/manif SYSTEM) find_package(PkgConfig REQUIRED) pkg_search_module(NetLink libnl-3.0) pkg_search_module(NetLinkRoute libnl-route-3.0) @@ -131,7 +131,7 @@ catkin_package() ## Libraries mrover_add_library(lie src/util/lie/*.cpp src/util/lie) -mrover_add_header_only_library(manif ${manif_INCLUDE_DIRS}) +target_link_libraries(lie PUBLIC manif) ## ESW diff --git a/deps/manif b/deps/manif new file mode 160000 index 000000000..c8a9fcbfe --- /dev/null +++ b/deps/manif @@ -0,0 +1 @@ +Subproject commit c8a9fcbfe157b55a7a860577febdc5350043a722 diff --git a/launch/new_sim.launch b/launch/new_sim.launch index 7c11f1a02..7f743b4b3 100644 --- a/launch/new_sim.launch +++ b/launch/new_sim.launch @@ -9,8 +9,8 @@ - + \ No newline at end of file diff --git a/src/perception/zed_wrapper/pch.hpp b/src/perception/zed_wrapper/pch.hpp index 579a5aace..1512db7f4 100644 --- a/src/perception/zed_wrapper/pch.hpp +++ b/src/perception/zed_wrapper/pch.hpp @@ -27,6 +27,4 @@ #include #include -#include #include -#include diff --git a/src/perception/zed_wrapper/zed_wrapper.bridge.cu b/src/perception/zed_wrapper/zed_wrapper.bridge.cu index 5be3946bd..cacb63f77 100644 --- a/src/perception/zed_wrapper/zed_wrapper.bridge.cu +++ b/src/perception/zed_wrapper/zed_wrapper.bridge.cu @@ -1,5 +1,4 @@ // Be careful what you include in this file, it is compiled with nvcc (NVIDIA CUDA compiler) -// For example OpenCV and lie includes cause problems #include "zed_wrapper.hpp" diff --git a/src/perception/zed_wrapper/zed_wrapper.cpp b/src/perception/zed_wrapper/zed_wrapper.cpp index 89cdbc53c..0b6d3aafd 100644 --- a/src/perception/zed_wrapper/zed_wrapper.cpp +++ b/src/perception/zed_wrapper/zed_wrapper.cpp @@ -1,5 +1,8 @@ #include "zed_wrapper.hpp" +#include +#include + namespace mrover { /** @@ -254,8 +257,8 @@ namespace mrover { try { SE3d leftCameraInOdom{{translation.x, translation.y, translation.z}, Eigen::Quaterniond{orientation.w, orientation.x, orientation.y, orientation.z}.normalized()}; - SE3d leftCameraInBaseLink = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "zed2i_left_camera_frame"); - SE3d baseLinkInOdom = leftCameraInBaseLink * leftCameraInOdom; + SE3d baseLinkToLeftCamera = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "zed2i_left_camera_frame"); + SE3d baseLinkInOdom = leftCameraInOdom * baseLinkToLeftCamera; SE3Conversions::pushToTfTree(mTfBroadcaster, "base_link", "odom", baseLinkInOdom); } catch (tf2::TransformException& e) { NODELET_WARN_STREAM("Failed to get transform: " << e.what()); diff --git a/src/simulator/simulator.controls.cpp b/src/simulator/simulator.controls.cpp index e63106f43..fa3300deb 100644 --- a/src/simulator/simulator.controls.cpp +++ b/src/simulator/simulator.controls.cpp @@ -79,22 +79,22 @@ namespace mrover { auto SimulatorNodelet::freeLook(Clock::duration dt) -> void { float flySpeed = mFlySpeed * std::chrono::duration_cast>(dt).count(); if (glfwGetKey(mWindow.get(), mCamRightKey) == GLFW_PRESS) { - mCameraInWorld = SE3d{R3{0.0, -flySpeed, 0}, SO3d{}} * mCameraInWorld; + mCameraInWorld = mCameraInWorld * SE3d{R3{0.0, -flySpeed, 0}, SO3d::Identity()}; } if (glfwGetKey(mWindow.get(), mCamLeftKey) == GLFW_PRESS) { - mCameraInWorld = SE3d{R3{0.0, flySpeed, 0}, SO3d{}} * mCameraInWorld; + mCameraInWorld = mCameraInWorld * SE3d{R3{0.0, flySpeed, 0}, SO3d::Identity()}; } if (glfwGetKey(mWindow.get(), mCamForwardKey) == GLFW_PRESS) { - mCameraInWorld = SE3d{R3{flySpeed, 0.0, 0.0}, SO3d{}} * mCameraInWorld; + mCameraInWorld = mCameraInWorld * SE3d{R3{flySpeed, 0.0, 0.0}, SO3d::Identity()}; } if (glfwGetKey(mWindow.get(), mCamBackwardKey) == GLFW_PRESS) { - mCameraInWorld = SE3d{R3{-flySpeed, 0.0, 0.0}, SO3d{}} * mCameraInWorld; + mCameraInWorld = mCameraInWorld * SE3d{R3{-flySpeed, 0.0, 0.0}, SO3d::Identity()}; } if (glfwGetKey(mWindow.get(), mCamUpKey) == GLFW_PRESS) { - mCameraInWorld = mCameraInWorld * SE3d{R3{0.0, 0.0, flySpeed}, SO3d{}}; + mCameraInWorld = SE3d{R3{0.0, 0.0, flySpeed}, SO3d::Identity()} * mCameraInWorld; } if (glfwGetKey(mWindow.get(), mCamDownKey) == GLFW_PRESS) { - mCameraInWorld = mCameraInWorld * SE3d{R3{0.0, 0.0, -flySpeed}, SO3d{}}; + mCameraInWorld = SE3d{R3{0.0, 0.0, -flySpeed}, SO3d::Identity()} * mCameraInWorld; } Eigen::Vector2i size; @@ -108,14 +108,11 @@ namespace mrover { Eigen::Vector2d delta = (mouse - center) * mLookSense; // TODO(quintin): use lie algebra more here? we have a perturbation in the tangent space - R3 p = mCameraInWorld.translation(); - Eigen::Matrix3d Ry = Eigen::AngleAxisd{delta.y(), Eigen::Vector3d::UnitY()}.toRotationMatrix(); - Eigen::Matrix3d Rz = Eigen::AngleAxisd{-delta.x(), Eigen::Vector3d::UnitZ()}.toRotationMatrix(); - Eigen::Matrix3d R0 = mCameraInWorld.rotation(); - static_assert(R0.RowsAtCompileTime == 3 && R0.ColsAtCompileTime == 3); - Eigen::Matrix3d R = Ry * mCameraInWorld.rotation() * Rz; - // SO3d q{SO3d{Eigen::AngleAxisd{delta.y(), Eigen::Vector3d::UnitY()}}.rotation() * mCameraInWorld.rotation() * SO3d{Eigen::AngleAxisd{-delta.x(), Eigen::Vector3d::UnitZ()}}.rotation()}; - mCameraInWorld = SE3d{p, SO3d{Eigen::Quaterniond(R)}}; + R3 P = mCameraInWorld.translation(); + SO3d R = mCameraInWorld.asSO3(); + SO3d::Tangent RX = Eigen::Vector3d::UnitZ() * -delta.x(); + SO3d::Tangent RY = Eigen::Vector3d::UnitY() * delta.y(); + mCameraInWorld = SE3d{P, RX + R + RY}; centerCursor(); } diff --git a/src/simulator/simulator.hpp b/src/simulator/simulator.hpp index 8e606db55..94185cdcd 100644 --- a/src/simulator/simulator.hpp +++ b/src/simulator/simulator.hpp @@ -291,10 +291,7 @@ namespace mrover { return it->second; } - SO3d test = SO3d::Identity(); - // SE3d mCameraInWorld{R3{-3.0, 0.0, 1.5}, test}; - SE3d mCameraInWorld{R3{-3.0, 0.0, 1.5}, SO3d{0, 0, 0, 1}}; - // SE3d mCameraInWorld{R3{-3.0, 0.0, 1.5}, SO3d::Identity()}; + SE3d mCameraInWorld{R3{-3.0, 0.0, 1.5}, SO3d::Identity()}; std::vector mCameras; diff --git a/src/simulator/simulator.physics.cpp b/src/simulator/simulator.physics.cpp index d600aa418..dc8e13a91 100644 --- a/src/simulator/simulator.physics.cpp +++ b/src/simulator/simulator.physics.cpp @@ -5,7 +5,9 @@ namespace mrover { auto btTransformToSe3(btTransform const& transform) -> SE3d { btVector3 const& p = transform.getOrigin(); btQuaternion const& q = transform.getRotation(); - return SE3d{R3{p.x(), p.y(), p.z()}, SO3d{q.w(), q.x(), q.y(), q.z()}}; + // Note: Must convert the Bullet quaternion (floats) to a normalized Eigen quaternion (doubles). + // Otherwise the normality check will fail in the SO3 constructor. + return SE3d{R3{p.x(), p.y(), p.z()}, Eigen::Quaterniond{q.w(), q.x(), q.y(), q.z()}.normalized()}; } auto SimulatorNodelet::initPhysics() -> void { @@ -94,4 +96,4 @@ namespace mrover { return btTransformToSe3(index == -1 ? physics->getBaseWorldTransform() : physics->getLink(index).m_cachedWorldTransform); } -} // namespace mrover +} // namespace mrover \ No newline at end of file From dd19c5515c97538898ac9f1b02871cd22f1b7f45 Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Thu, 15 Feb 2024 14:59:14 -0500 Subject: [PATCH 63/81] refactored camera controls using tangent elements --- src/simulator/simulator.controls.cpp | 41 ++++++++++++++++------------ 1 file changed, 23 insertions(+), 18 deletions(-) diff --git a/src/simulator/simulator.controls.cpp b/src/simulator/simulator.controls.cpp index fa3300deb..7be20afd0 100644 --- a/src/simulator/simulator.controls.cpp +++ b/src/simulator/simulator.controls.cpp @@ -1,7 +1,5 @@ #include "simulator.hpp" -using manif::SE3d, manif::SO3d; - namespace mrover { auto SimulatorNodelet::twistCallback(geometry_msgs::Twist::ConstPtr const& twist) -> void { @@ -78,23 +76,32 @@ namespace mrover { auto SimulatorNodelet::freeLook(Clock::duration dt) -> void { float flySpeed = mFlySpeed * std::chrono::duration_cast>(dt).count(); - if (glfwGetKey(mWindow.get(), mCamRightKey) == GLFW_PRESS) { - mCameraInWorld = mCameraInWorld * SE3d{R3{0.0, -flySpeed, 0}, SO3d::Identity()}; - } - if (glfwGetKey(mWindow.get(), mCamLeftKey) == GLFW_PRESS) { - mCameraInWorld = mCameraInWorld * SE3d{R3{0.0, flySpeed, 0}, SO3d::Identity()}; - } + SE3d::Tangent delta_x, delta_y, delta_z; + delta_x << Eigen::Vector3d::UnitX(), Eigen::Vector3d::Zero(); + delta_y << Eigen::Vector3d::UnitY(), Eigen::Vector3d::Zero(); + delta_z << Eigen::Vector3d::UnitZ(), Eigen::Vector3d::Zero(); + + // TODO: turn this into a single rplus and lplus operation + // overloaded rplus for movement relative to camera frame if (glfwGetKey(mWindow.get(), mCamForwardKey) == GLFW_PRESS) { - mCameraInWorld = mCameraInWorld * SE3d{R3{flySpeed, 0.0, 0.0}, SO3d::Identity()}; + mCameraInWorld += delta_x * flySpeed; } if (glfwGetKey(mWindow.get(), mCamBackwardKey) == GLFW_PRESS) { - mCameraInWorld = mCameraInWorld * SE3d{R3{-flySpeed, 0.0, 0.0}, SO3d::Identity()}; + mCameraInWorld += delta_x * -flySpeed; } + if (glfwGetKey(mWindow.get(), mCamLeftKey) == GLFW_PRESS) { + mCameraInWorld += delta_y * flySpeed; + } + if (glfwGetKey(mWindow.get(), mCamRightKey) == GLFW_PRESS) { + mCameraInWorld += delta_y * -flySpeed; + } + + // overloaded lplus for movement relative to world frame if (glfwGetKey(mWindow.get(), mCamUpKey) == GLFW_PRESS) { - mCameraInWorld = SE3d{R3{0.0, 0.0, flySpeed}, SO3d::Identity()} * mCameraInWorld; + mCameraInWorld = delta_z * flySpeed + mCameraInWorld; } if (glfwGetKey(mWindow.get(), mCamDownKey) == GLFW_PRESS) { - mCameraInWorld = SE3d{R3{0.0, 0.0, -flySpeed}, SO3d::Identity()} * mCameraInWorld; + mCameraInWorld = delta_z * -flySpeed + mCameraInWorld; } Eigen::Vector2i size; @@ -107,12 +114,10 @@ namespace mrover { Eigen::Vector2d delta = (mouse - center) * mLookSense; - // TODO(quintin): use lie algebra more here? we have a perturbation in the tangent space - R3 P = mCameraInWorld.translation(); - SO3d R = mCameraInWorld.asSO3(); - SO3d::Tangent RX = Eigen::Vector3d::UnitZ() * -delta.x(); - SO3d::Tangent RY = Eigen::Vector3d::UnitY() * delta.y(); - mCameraInWorld = SE3d{P, RX + R + RY}; + // lplus for tilt in camera frame, rplus for pan in world frame + SO3d::Tangent delta_Ry = Eigen::Vector3d::UnitY() * delta.y(); + SO3d::Tangent delta_Rz = Eigen::Vector3d::UnitZ() * -delta.x(); + mCameraInWorld.asSO3() = delta_Rz + mCameraInWorld.asSO3() + delta_Ry; centerCursor(); } From 3dd06d3220f308a5871fdd77718fbec3d8ff7d7f Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Thu, 15 Feb 2024 22:09:27 -0500 Subject: [PATCH 64/81] pr cleanup --- CMakeLists.txt | 1 - launch/auton.launch | 56 --------- launch/auton_sim.launch | 28 ----- launch/rtk.launch | 18 --- launch/ublox_device.launch | 10 -- src/localization/Test-Subscriber.py | 113 ----------------- src/localization/basestation_gps_driver.py | 47 ------- src/localization/dual_gnss.py | 113 ----------------- src/localization/gps_config_plots.py | 8 -- src/localization/temp_gps_sim.py | 77 ------------ src/util/se3_ros_utils.cpp | 63 ---------- src/util/se3_ros_utils.hpp | 35 ------ urdf/rover/rover_gazebo_plugins.urdf.xacro | 140 --------------------- 13 files changed, 709 deletions(-) delete mode 100644 launch/auton.launch delete mode 100644 launch/auton_sim.launch delete mode 100644 launch/rtk.launch delete mode 100644 launch/ublox_device.launch delete mode 100755 src/localization/Test-Subscriber.py delete mode 100755 src/localization/basestation_gps_driver.py delete mode 100644 src/localization/dual_gnss.py delete mode 100644 src/localization/gps_config_plots.py delete mode 100755 src/localization/temp_gps_sim.py delete mode 100644 src/util/se3_ros_utils.cpp delete mode 100644 src/util/se3_ros_utils.hpp delete mode 100644 urdf/rover/rover_gazebo_plugins.urdf.xacro diff --git a/CMakeLists.txt b/CMakeLists.txt index 39e244b0e..ae81f7276 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -190,7 +190,6 @@ endif () ## Perception -# mrover_add_nodelet(tag_detector src/perception/tag_detector/*.cpp src/perception/tag_detector src/perception/tag_detector/pch.hpp src/util/se3_ros_utils.hpp src/util/se3_ros_utils.cpp) mrover_add_nodelet(tag_detector src/perception/tag_detector/*.cpp src/perception/tag_detector src/perception/tag_detector/pch.hpp) mrover_nodelet_link_libraries(tag_detector opencv_core opencv_objdetect opencv_aruco opencv_imgproc opencv_highgui lie manif) diff --git a/launch/auton.launch b/launch/auton.launch deleted file mode 100644 index 3984155e1..000000000 --- a/launch/auton.launch +++ /dev/null @@ -1,56 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/launch/auton_sim.launch b/launch/auton_sim.launch deleted file mode 100644 index 4419a65df..000000000 --- a/launch/auton_sim.launch +++ /dev/null @@ -1,28 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/launch/rtk.launch b/launch/rtk.launch deleted file mode 100644 index e7a8a841a..000000000 --- a/launch/rtk.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/launch/ublox_device.launch b/launch/ublox_device.launch deleted file mode 100644 index eddcc0e7c..000000000 --- a/launch/ublox_device.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/src/localization/Test-Subscriber.py b/src/localization/Test-Subscriber.py deleted file mode 100755 index e354af24c..000000000 --- a/src/localization/Test-Subscriber.py +++ /dev/null @@ -1,113 +0,0 @@ -#!/usr/bin/env python3 - -# python linear algebra library -import numpy as np - -# library for interacting with ROS and TF tree -import rospy -import csv -import tf2_ros -import math - -# ROS message types we need to use -from sensor_msgs.msg import NavSatFix, Imu - -# SE3 library for handling poses and TF tree -# from util.SE3 import SE3 -# from util.SO3 import SO3 -from matplotlib import pyplot as plt - -# import gps_linearization - - -lat_arr = [] -long_arr = [] - -coord_arr_left_latitude = [] -coord_arr_left_longitude = [] -coord_arr_right_latitude = [] -coord_arr_right_longitude = [] -# coord_arr_left = [] -# coord_arr_right = [] -distances = [] - - -class Localization: - # pose: SE3 - - def __init__(self): - # create subscribers for GPS data, linking them to our callback functions - # TODO - rospy.Subscriber("gps/fix/rover_gps_left", NavSatFix, self.gps_left_callback) - rospy.Subscriber("gps/fix/rover_gps_right", NavSatFix, self.gps_right_callback) - - # csvfile = open("/home/daniel/catkin_ws/src/mrover/src/localization/test_gps_sim.csv", "w") - # csvwriter = csv.writer(csvfile) - # csvwriter.writerow([9, 10]) - # create a transform broadcaster for publishing to the TF tree - # self.tf_broadcaster = tf2_ros.TransformBroadcaster() - - # initialize pose to all zeros - # self.pose = SE3() - - def gps_left_callback(self, msg: NavSatFix): - print(msg) - # coord_arr_left_latitude.append(gps_linearization.geodetic2enu(msg.latitude, msg.longitude) - # coord_arr_left_longitude.append(msg.longitude) - - coord_arr_left_latitude.append(msg.latitude) - coord_arr_left_longitude.append(msg.longitude) - # coord_arr_left.append([msg.latitude, msg.longitude]) - - # lat_arr.append(msg.latitude) - # long_arr.append(msg.longitude) - - def gps_right_callback(self, msg: NavSatFix): - print(msg) - # coord_arr_right.append([msg.latitude, msg.longitude]) - coord_arr_right_latitude.append(msg.latitude) - coord_arr_right_longitude.append(msg.longitude) - # coord_arr_right.append([msg.latitude, msg.longitude]) - - def main(): - # initialize the node - rospy.init_node("localization_test") - - # create and start our localization system - localization = Localization() - - # let the callback functions run asynchronously in the background - rospy.spin() - - -if __name__ == "__main__": - Localization.main() - - # plt.plot(coord_arr_left_latitude[i], coord_arr_left_longitude[i], color="red") - # plt.plot(coord_arr_right_latitude[i], coord_arr_right_longitude[i], color="blue") - # plt.show() - - # plt.scatter(coord_arr_left_latitude, coord_arr_left_longitude, color="red") - # plt.scatter(coord_arr_right_latitude, coord_arr_right_longitude, color="blue") - - # plt.show() - - for i in range(len(coord_arr_left_latitude)): - print("latitude" + str(coord_arr_left_latitude[i] - coord_arr_right_latitude[i])) - print("longitude" + str(coord_arr_left_longitude[i] - coord_arr_right_longitude[i])) - - # print("\n") - # print(coord_arr_right_latitude[:10]) - # print(coord_arr_left_latitude[:10]) - - # print("\n") - # print(coord_arr_right_longitude[:10]) - # print(coord_arr_left_longitude[:10]) - # print( - # (sum(coord_arr_left_latitude) / len(coord_arr_left_latitude)) - # - (sum(coord_arr_right_latitude) / len(coord_arr_right_latitude)) - # ) - # print( - # (sum(coord_arr_left_longitude) / len(coord_arr_right_longitude)) - # - (sum(coord_arr_right_longitude) / len(coord_arr_right_longitude)) - # ) diff --git a/src/localization/basestation_gps_driver.py b/src/localization/basestation_gps_driver.py deleted file mode 100755 index c5414ff38..000000000 --- a/src/localization/basestation_gps_driver.py +++ /dev/null @@ -1,47 +0,0 @@ -#!/usr/bin/env python3 -import rospy -from serial import Serial -from pyubx2 import UBXReader, UBX_PROTOCOL, RTCM3_PROTOCOL, protocol -from rtcm_msgs.msg import Message - -def main() -> None: - rtcm_pub = rospy.Publisher('rtcm', Message, queue_size=1) - rospy.init_node('basestation_driver') - port = rospy.get_param("basestation_gps_driver/port") - baud = rospy.get_param("basestation_gps_driver/baud") - svin_started = False - svin_complete = False - - # connect to GPS over serial, only read UBX and RTCM messages - with Serial(port, baud, timeout=1) as ser: - reader = UBXReader(ser, protfilter=(UBX_PROTOCOL | RTCM3_PROTOCOL)) - while not rospy.is_shutdown(): - if ser.in_waiting: - (raw_msg, msg) = reader.read() - - # skip if message could not be parsed - if not msg: - continue - - # publish RTCM messages - if protocol(raw_msg) == RTCM3_PROTOCOL: - rtcm_pub.publish(Message(message=raw_msg)) - - # print survey-in status - elif msg.identity == "NAV-SVIN": - if not svin_started and msg.active: - svin_started = True - rospy.loginfo("basestation survey-in started") - if not svin_complete and msg.valid: - svin_complete = True - rospy.loginfo(f"basestation survey-in complete, accuracy = {msg.meanAcc}") - if svin_started and not svin_complete: - print(f"current accuracy: {msg.meanAcc}") - - # fix quality information - elif msg.identity == "NAV-PVT": - print(f"{'valid' if msg.gnssFixOk else 'invalid'} fix, {msg.numSV} satellites used") - - -if __name__ == '__main__': - main() \ No newline at end of file diff --git a/src/localization/dual_gnss.py b/src/localization/dual_gnss.py deleted file mode 100644 index 596f1e622..000000000 --- a/src/localization/dual_gnss.py +++ /dev/null @@ -1,113 +0,0 @@ -import numpy as np -import matplotlib.pyplot as plt - - -# def get_heading(antenna_point_A, antenna_point_B): -# latitude_A = np.radians(antenna_point_A[0]) -# latitude_B = np.radians(antenna_point_B[0]) -# longitude_A = np.radians(antenna_point_A[1]) -# longitude_B = np.radians(antenna_point_B[1]) - -# x = np.cos(latitude_B) * np.sin(longitude_B - longitude_A) -# y = np.cos(latitude_A) * np.sin(latitude_B) - np.sin(latitude_A) * np.cos(latitude_B) * np.cos( -# longitude_B - longitude_A -# ) - -# bearing = np.arctan2(x, y) -# return bearing - - -# test_bearing = get_heading(np.array([39.099912, -94.581213]), np.array([38.627089, -90.200203])) -# print(np.degrees(test_bearing)) - - -# reference point should be close, needs to be specified based on location -def spherical_to_cartesian(spherical_coord: np.ndarray, reference_coord: np.ndarray) -> np.ndarray: - r = 6371000 - x = r * (np.radians(spherical_coord[1]) - np.radians(reference_coord[1])) * np.cos(np.radians(reference_coord[0])) - y = r * (np.radians(spherical_coord[0]) - np.radians(reference_coord[0])) - z = 0 - return np.array([x, y, z]) - - -def get_heading_vector_from_angle(heading_angle): - x = np.sin(heading_angle) - y = np.cos(heading_angle) - return np.array([x, y]) - - -def get_heading_vector(point_L: np.array, point_R: np.array): - vector_connecting = np.array([point_R[0] - point_L[0], point_R[1] - point_L[1]]) - vector_perp = np.zeros_like(vector_connecting) - vector_perp[0] = -vector_connecting[1] - vector_perp[1] = vector_connecting[0] - # print(vector_perp) - return vector_perp / np.linalg.norm(vector_perp) - - -def plot_vectors(P_1, P_2): - result = P_1 - P_2 - slope_resultant = (P_2[1] - P_1[1]) / (P_2[0] - P_1[0]) - midpoint = np.array([((P_2[0] + P_1[0]) / 2), ((P_2[1] + P_1[1]) / 2)]) - slope_pd = -1 / slope_resultant - - plt.scatter([P_1[0], P_2[0]], [P_1[1], P_2[1]]) - x_intercept = (-midpoint[1] / slope_pd) + midpoint[0] - x_pd = np.linspace(midpoint[0], x_intercept, 300) - y_pd = slope_pd * (x_pd - midpoint[0]) + midpoint[1] - - plt.plot([P_1[0], P_2[0]], [P_1[1], P_2[1]]) - plt.plot(x_pd, y_pd) - - plt.axvline(x=midpoint[0], color="b") - - angle = np.degrees(np.arctan((midpoint[0] - x_intercept) / midpoint[1])) - plt.xlabel("X-axis") - plt.ylabel("Y-axis") - plt.grid(True) - plt.show() - - -def offset_lat_left(P_1, P_2, n): - return P_1 - n * 0.0000001, P_2 - - -def offset_lat_right(P_1, P_2, n): - return P_1 + n * 0.0000001, P_2 - - -def offset_long_left(P_1, P_2, n): - return P_1, P_2 - n * 0.0000001 - - -def offset_long_right(P_1, P_2, n): - return P_1, P_2 + n * 0.0000001 - - -# [1,0], [-1,0] if switched latitude 5th digit -# P_1 = np.array([42.30061, -83.71006]) -# P_2 = np.array([42.30060, -83.71006]) - -# [0,-1], [0,1] if switched longitude 5th digit -P_1 = np.array([42.300611, -83.710071]) -P_2 = np.array([42.300611, -83.710083]) -ref = np.array([42.293195, -83.7096706]) - -for i in range(0, 170): # graph changes based on these bounds due to reference coordinate - new_p1, new_p2 = offset_lat_left(P_1, P_2, i) # optimal bound is 0.0000001 (1e^-7) - new_p1 = spherical_to_cartesian(new_p1, ref) - new_p2 = spherical_to_cartesian(new_p2, ref) - # print("P1") - # print(new_p1) - # print("P2") - # print(new_p2) - print("heading") - print(get_heading_vector(new_p1, new_p2)) - test = get_heading_vector(new_p1, new_p2) - plt.scatter(test[0], test[1]) -plt.xlabel("heading x") -plt.ylabel("heading y") -plt.grid(True) -plt.show() - -# print(get_heading_vector(np.array([0, 0]), np.array([3, 3]))) diff --git a/src/localization/gps_config_plots.py b/src/localization/gps_config_plots.py deleted file mode 100644 index 83c0731ea..000000000 --- a/src/localization/gps_config_plots.py +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python3 - -import rosbag - -if __name__ == "__main__": - bag = rosbag.Bag('base.bag') - for (topic,msg,t) in bag.read_messages(): - print (topic,msg,t) diff --git a/src/localization/temp_gps_sim.py b/src/localization/temp_gps_sim.py deleted file mode 100755 index a3e752ef6..000000000 --- a/src/localization/temp_gps_sim.py +++ /dev/null @@ -1,77 +0,0 @@ -#!/usr/bin/env python3 -import numpy as np -import rospy -from nav_msgs.msg import Odometry -from sensor_msgs.msg import NavSatFix -from util.SE3 import SE3 -from pymap3d.enu import enu2geodetic - -class GPSSim: - latest_odom: Odometry = None - ref_point: np.ndarray - right_gps_pub: rospy.Publisher - left_gps_pub: rospy.Publisher - ground_truth_sub: rospy.Subscriber - - def __init__(self): - ref_lat = rospy.get_param("gps_linearization/reference_point_latitude") - ref_lon = rospy.get_param("gps_linearization/reference_point_longitude") - ref_alt = rospy.get_param("gps_linearization/reference_point_altitude") - self.ref_point = np.array([ref_lat, ref_lon, ref_alt]) - - self.right_gps_pub = rospy.Publisher("right_gps_driver/fix", NavSatFix, queue_size=10) - self.left_gps_pub = rospy.Publisher("left_gps_driver/fix", NavSatFix, queue_size=10) - self.ground_truth_sub = rospy.Subscriber("ground_truth", Odometry, self.ground_truth_callback) - - def ground_truth_callback(self, msg: Odometry): - # don't publish directly from here to reduce rate from 1kHz to 10Hz - self.latest_odom = msg - - def publish_gps(self): - if self.latest_odom is None: - return - - # position of each GPS antenna relative to base_link, hardcoded for now - left_gps_offset = np.array([0, 1, 0]) - right_gps_offset = np.array([0, -1, 0]) - - # get the homogeneous transform matrix from base_link to map from the latest ground truth reading - pose = self.latest_odom.pose.pose - p = np.array([pose.position.x, pose.position.y, pose.position.z]) - q = np.array([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]) - rover_in_map = SE3.from_pos_quat(p, q) - rover_to_map = rover_in_map.transform_matrix() - - # calculate the position of each GPS antenna in the map frame - left_gps_in_map = rover_to_map @ np.append(left_gps_offset, 1) - right_gps_in_map = rover_to_map @ np.append(right_gps_offset, 1) - - # convert GPS positions to geodetic coordinates - (left_lat, left_lon, left_alt) = enu2geodetic(*left_gps_in_map[:3], *self.ref_point, deg=True) - (right_lat, right_lon, right_alt) = enu2geodetic(*right_gps_in_map[:3], *self.ref_point, deg=True) - - # publish GPS messages - left_msg = NavSatFix() - left_msg.header.stamp = rospy.Time.now() - left_msg.latitude = left_lat - left_msg.longitude = left_lon - left_msg.altitude = left_alt - self.left_gps_pub.publish(left_msg) - - right_msg = NavSatFix() - right_msg.header.stamp = rospy.Time.now() - right_msg.latitude = right_lat - right_msg.longitude = right_lon - right_msg.altitude = right_alt - self.right_gps_pub.publish(right_msg) - - def run(self): - rate = rospy.Rate(10) - while not rospy.is_shutdown(): - self.publish_gps() - rate.sleep() - -if __name__ == "__main__": - rospy.init_node("temp_gps_sim") - node = GPSSim() - node.run() diff --git a/src/util/se3_ros_utils.cpp b/src/util/se3_ros_utils.cpp deleted file mode 100644 index 0a1a8fb1b..000000000 --- a/src/util/se3_ros_utils.cpp +++ /dev/null @@ -1,63 +0,0 @@ -#include "se3_ros_utils.hpp" - -namespace mrover { - SE3d SE3fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId) { - geometry_msgs::TransformStamped transform = buffer.lookupTransform(fromFrameId, toFrameId, ros::Time{}); - return SE3fromTf(transform.transform); - } - - void pushSE3ToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3d const& tf) { - broadcaster.sendTransform(SE3toTransformStamped(tf, parentFrameId, childFrameId)); - } - - SE3d SE3fromTf(geometry_msgs::Transform const& transform) { - return {{transform.translation.x, transform.translation.y, transform.translation.z}, - Eigen::Quaterniond{transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z}}; - } - - SE3d SE3fromPose(geometry_msgs::Pose const& pose) { - return {{pose.position.x, pose.position.y, pose.position.z}, - Eigen::Quaterniond{pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z}}; - } - - geometry_msgs::Pose SE3toPose(SE3d const& tf) { - geometry_msgs::Pose pose; - pose.position.x = tf.x(); - pose.position.y = tf.y(); - pose.position.z = tf.z(); - pose.orientation.x = tf.quat().x(); - pose.orientation.y = tf.quat().y(); - pose.orientation.z = tf.quat().z(); - pose.orientation.w = tf.quat().w(); - return pose; - } - - geometry_msgs::Transform SE3toTransform(SE3d const& tf) { - geometry_msgs::Transform transform; - transform.translation.x = tf.x(); - transform.translation.y = tf.y(); - transform.translation.z = tf.z(); - transform.rotation.x = tf.quat().x(); - transform.rotation.y = tf.quat().y(); - transform.rotation.z = tf.quat().z(); - transform.rotation.w = tf.quat().w(); - return transform; - } - - geometry_msgs::PoseStamped SE3toPoseStamped(SE3d const& tf, std::string const& frameId) { - geometry_msgs::PoseStamped pose; - pose.pose = SE3toPose(tf); - pose.header.frame_id = frameId; - pose.header.stamp = ros::Time::now(); - return pose; - } - - geometry_msgs::TransformStamped SE3toTransformStamped(SE3d const& tf, std::string const& parentFrameId, std::string const& childFrameId) { - geometry_msgs::TransformStamped transform; - transform.transform = SE3toTransform(tf); - transform.header.frame_id = parentFrameId; - transform.header.stamp = ros::Time::now(); - transform.child_frame_id = childFrameId; - return transform; - } -} // namespace mrover \ No newline at end of file diff --git a/src/util/se3_ros_utils.hpp b/src/util/se3_ros_utils.hpp deleted file mode 100644 index 1b96d1faa..000000000 --- a/src/util/se3_ros_utils.hpp +++ /dev/null @@ -1,35 +0,0 @@ -#pragma once - -#include - -#include -#include -#include - -#include -#include -#include - -#include -#include - -using manif::SE3d; - - -namespace mrover { - SE3d SE3fromTf(geometry_msgs::Transform const& transform); - - SE3d SE3fromPose(geometry_msgs::Pose const& pose); - - [[nodiscard]] geometry_msgs::Pose SE3toPose(SE3d const& tf); - - [[nodiscard]] geometry_msgs::Transform SE3toTransform(SE3d const& tf); - - [[nodiscard]] geometry_msgs::PoseStamped SE3toPoseStamped(SE3d const& tf, std::string const& frameId); - - [[nodiscard]] geometry_msgs::TransformStamped SE3toTransformStamped(SE3d const& tf, std::string const& parentFrameId, std::string const& childFrameId); - - [[nodiscard]] SE3d SE3fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId); - - void pushSE3ToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3d const& tf); -} // namespace mrover \ No newline at end of file diff --git a/urdf/rover/rover_gazebo_plugins.urdf.xacro b/urdf/rover/rover_gazebo_plugins.urdf.xacro deleted file mode 100644 index 4efe1e841..000000000 --- a/urdf/rover/rover_gazebo_plugins.urdf.xacro +++ /dev/null @@ -1,140 +0,0 @@ - - - - - - false - 50 - center_left_wheel_joint - center_right_wheel_joint - front_left_wheel_joint - front_right_wheel_joint - back_left_wheel_joint - back_right_wheel_joint - ${base_width} - ${wheel_radius * 2} - 200 - /cmd_vel - base_link - - - - - - - - - - - - true - 100 - true - __default_topic__ - - imu/imu_only - base_link - 100.0 - 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0.0 0.0 0.0 - 0 0 0 - 0 0 0 - base_link - false - - 0 0 0 0 0 0 - - - - - - - 100 - base_link - imu/mag_only - 90.0 - 3.0333 - 60.0 - 0 0 0 - 0 0 0 - - - - - - - 15 - - 1.919862 - - 320 - 240 - R8G8B8 - - - 0.2 - 20 - - - - 0 - true - 5 - camera/left - image - camera_info - depth_image - camera_info - points - left_camera_link - 0.3 - 20.0 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - 0.00000001 - 0 - 0 - 0 - 0 - 0 - - - - \ No newline at end of file From a282a9ece83e5c2c273f5fdabe877c769e303592 Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Thu, 15 Feb 2024 22:15:55 -0500 Subject: [PATCH 65/81] more cleanup --- config/esw.yaml | 16 ---- config/zed_f9p.yaml | 7 -- launch/new_sim.launch | 16 ---- launch/rover_core.launch | 10 --- src/localization/gps_linearization.py | 112 ++++++++++---------------- 5 files changed, 44 insertions(+), 117 deletions(-) delete mode 100644 config/zed_f9p.yaml delete mode 100644 launch/new_sim.launch delete mode 100644 launch/rover_core.launch diff --git a/config/esw.yaml b/config/esw.yaml index d11a89964..ea3de9910 100644 --- a/config/esw.yaml +++ b/config/esw.yaml @@ -4,22 +4,6 @@ gps_driver: useRMC: false # get covariance instead of velocity, see wiki for more info frame_id: "base_link" -right_gps_driver: - port: "/dev/ttyACM0" - baud: 38400 - frame_id: "base_link" - -left_gps_driver: - port: "/dev/ttyACM0" - baud: 38400 - frame_id: "base_link" - - -basestation_gps_driver: - port: "/dev/ttyACM1" - baud: 38400 - frame_id: "base_link" - imu_driver: port: "/dev/tty.usbmodem2101" baud: 115200 diff --git a/config/zed_f9p.yaml b/config/zed_f9p.yaml deleted file mode 100644 index 1e70a463a..000000000 --- a/config/zed_f9p.yaml +++ /dev/null @@ -1,7 +0,0 @@ -device: /dev/ttyACM0 -frame_id: base_link -uart1: - baudrate: 115200 -config_on_startup: false -publish/rxm/all: true -publish/all: true diff --git a/launch/new_sim.launch b/launch/new_sim.launch deleted file mode 100644 index 7f743b4b3..000000000 --- a/launch/new_sim.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/launch/rover_core.launch b/launch/rover_core.launch deleted file mode 100644 index 47a65ea80..000000000 --- a/launch/rover_core.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/src/localization/gps_linearization.py b/src/localization/gps_linearization.py index 0d5364981..983e84156 100755 --- a/src/localization/gps_linearization.py +++ b/src/localization/gps_linearization.py @@ -9,9 +9,7 @@ from sensor_msgs.msg import NavSatFix from std_msgs.msg import Header from util.SE3 import SE3 -from util.SO3 import SO3 from util.np_utils import numpify -import message_filters class GPSLinearization: @@ -43,64 +41,32 @@ def __init__(self): self.ref_alt = rospy.get_param("gps_linearization/reference_point_altitude") self.world_frame = rospy.get_param("world_frame") self.use_dop_covariance = rospy.get_param("global_ekf/use_gps_dop_covariance") - - # config gps and imu convariance matrices self.config_gps_covariance = np.array(rospy.get_param("global_ekf/gps_covariance", None)) - self.config_imu_covariance = np.array(rospy.get_param("global_ekf/imu_orientation_covariance", None)) self.last_gps_msg = None self.last_imu_msg = None - right_gps_sub = message_filters.Subscriber("right_gps_driver/fix", NavSatFix) - left_gps_sub = message_filters.Subscriber("left_gps_driver/fix", NavSatFix) - - sync_gps_sub = message_filters.ApproximateTimeSynchronizer([right_gps_sub, left_gps_sub], 10, 0.5) - sync_gps_sub.registerCallback(self.gps_callback) - + rospy.Subscriber("gps/fix", NavSatFix, self.gps_callback) rospy.Subscriber("imu/data", ImuAndMag, self.imu_callback) self.pose_publisher = rospy.Publisher("linearized_pose", PoseWithCovarianceStamped, queue_size=1) - def gps_callback(self, right_gps_msg: NavSatFix, left_gps_msg: NavSatFix): + def gps_callback(self, msg: NavSatFix): """ Callback function that receives GPS messages, assigns their covariance matrix, and then publishes the linearized pose. :param msg: The NavSatFix message containing GPS data that was just received - TODO: Handle invalid PVTs """ - if np.any(np.isnan([right_gps_msg.latitude, right_gps_msg.longitude, right_gps_msg.altitude])): + if np.any(np.isnan([msg.latitude, msg.longitude, msg.altitude])): return - if np.any(np.isnan([left_gps_msg.latitude, left_gps_msg.longitude, left_gps_msg.altitude])): - return - - ref_coord = np.array([self.ref_lat, self.ref_lon, self.ref_alt]) - - right_cartesian = np.array( - geodetic2enu(right_gps_msg.latitude, right_gps_msg.longitude, right_gps_msg.altitude, *ref_coord, deg=True) - ) - left_cartesian = np.array( - geodetic2enu(left_gps_msg.latitude, left_gps_msg.longitude, left_gps_msg.altitude, *ref_coord, deg=True) - ) - pose = GPSLinearization.compute_gps_pose(right_cartesian=right_cartesian, left_cartesian=left_cartesian) + if not self.use_dop_covariance: + msg.position_covariance = self.config_gps_covariance - covariance_matrix = np.zeros((6, 6)) - covariance_matrix[:3, :3] = self.config_gps_covariance.reshape(3, 3) - covariance_matrix[3:, 3:] = self.config_imu_covariance.reshape(3, 3) - - # TODO: publish to ekf - pose_msg = PoseWithCovarianceStamped( - header=Header(stamp=rospy.Time.now(), frame_id=self.world_frame), - pose=PoseWithCovariance( - pose=Pose( - position=Point(*pose.position), - orientation=Quaternion(*pose.rotation.quaternion), - ), - covariance=covariance_matrix.flatten().tolist(), - ), - ) + self.last_gps_msg = msg - self.pose_publisher.publish(pose_msg) + if self.last_imu_msg is not None: + self.publish_pose() def imu_callback(self, msg: ImuAndMag): """ @@ -113,6 +79,42 @@ def imu_callback(self, msg: ImuAndMag): if self.last_gps_msg is not None: self.publish_pose() + @staticmethod + def get_linearized_pose_in_world( + gps_msg: NavSatFix, imu_msg: ImuAndMag, ref_coord: np.ndarray + ) -> Tuple[SE3, np.ndarray]: + """ + Linearizes the GPS geodetic coordinates into ENU cartesian coordinates, + then combines them with the IMU orientation into a pose estimate relative + to the world frame, with corresponding covariance matrix. + + :param gps_msg: Message containing the rover's GPS coordinates and their corresponding + covariance matrix. + :param imu_msg: Message containing the rover's orientation from IMU, with + corresponding covariance matrix. + :param ref_coord: numpy array containing the geodetic coordinate which will be the origin + of the tangent plane, [latitude, longitude, altitude] + :returns: The pose consisting of linearized GPS and IMU orientation, and the corresponding + covariance matrix as a 6x6 numpy array where each row is [x, y, z, roll, pitch, yaw] + """ + # linearize GPS coordinates into cartesian + cartesian = np.array(geodetic2enu(gps_msg.latitude, gps_msg.longitude, gps_msg.altitude, *ref_coord, deg=True)) + + # ignore Z + cartesian[2] = 0 + + imu_quat = numpify(imu_msg.imu.orientation) + + # normalize to avoid rounding errors + imu_quat = imu_quat / np.linalg.norm(imu_quat) + pose = SE3.from_pos_quat(position=cartesian, quaternion=imu_quat) + + covariance = np.zeros((6, 6)) + covariance[:3, :3] = np.array([gps_msg.position_covariance]).reshape(3, 3) + covariance[3:, 3:] = np.array([imu_msg.imu.orientation_covariance]).reshape(3, 3) + + return pose, covariance + def publish_pose(self): """ Publishes the linearized pose of the rover relative to the map frame, @@ -133,32 +135,6 @@ def publish_pose(self): ) self.pose_publisher.publish(pose_msg) - @staticmethod - def compute_gps_pose(right_cartesian, left_cartesian) -> np.ndarray: - # TODO: give simulated GPS non zero altitude so we can stop erasing the z component - # left_cartesian[2] = 0 - # right_cartesian[2] = 0 - vector_connecting = left_cartesian - right_cartesian - vector_connecting[2] = 0 - magnitude = np.linalg.norm(vector_connecting) - vector_connecting = vector_connecting / magnitude - - vector_perp = np.zeros(shape=(3, 1)) - vector_perp[0] = vector_connecting[1] - vector_perp[1] = -vector_connecting[0] - - rotation_matrix = np.hstack( - (vector_perp, np.reshape(vector_connecting, (3, 1)), np.array(object=[[0], [0], [1]])) - ) - - # temporary fix, assumes base_link is exactly in the middle of the two GPS antennas - # TODO: use static tf from base_link to left_antenna instead - rover_position = (left_cartesian + right_cartesian) / 2 - - pose = SE3(rover_position, SO3.from_matrix(rotation_matrix=rotation_matrix)) - - return pose - def main(): # start the node and spin to wait for messages to be received From ff5d48a89cadbc35e54634c76eb55d64f304d696 Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Thu, 15 Feb 2024 22:25:53 -0500 Subject: [PATCH 66/81] more cleanup --- src/perception/tag_detector/pch.hpp | 1 - src/perception/tag_detector/tag_detector.cpp | 1 + src/perception/tag_detector/tag_detector.hpp | 2 -- src/perception/tag_detector/tag_detector.processing.cpp | 2 +- 4 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/perception/tag_detector/pch.hpp b/src/perception/tag_detector/pch.hpp index fb2f2d6b1..cb9448d6a 100644 --- a/src/perception/tag_detector/pch.hpp +++ b/src/perception/tag_detector/pch.hpp @@ -33,4 +33,3 @@ #include #include #include -// #include diff --git a/src/perception/tag_detector/tag_detector.cpp b/src/perception/tag_detector/tag_detector.cpp index 1d1d90b4c..7ece83885 100644 --- a/src/perception/tag_detector/tag_detector.cpp +++ b/src/perception/tag_detector/tag_detector.cpp @@ -140,4 +140,5 @@ namespace mrover { res.success = true; return true; } + } // namespace mrover diff --git a/src/perception/tag_detector/tag_detector.hpp b/src/perception/tag_detector/tag_detector.hpp index 457ec41e4..b6fb3a45b 100644 --- a/src/perception/tag_detector/tag_detector.hpp +++ b/src/perception/tag_detector/tag_detector.hpp @@ -1,7 +1,5 @@ #include "pch.hpp" -using manif::SE3d; - namespace mrover { struct Tag { diff --git a/src/perception/tag_detector/tag_detector.processing.cpp b/src/perception/tag_detector/tag_detector.processing.cpp index 20ab69774..b922bb075 100644 --- a/src/perception/tag_detector/tag_detector.processing.cpp +++ b/src/perception/tag_detector/tag_detector.processing.cpp @@ -23,7 +23,7 @@ namespace mrover { return std::nullopt; } - return std::make_optional(Eigen::Vector3d{point.x, point.y, point.z}, Eigen::Quaterniond{1, 0, 0, 0}); + return std::make_optional(R3{point.x, point.y, point.z}, SO3d::Identity()); } /** From dab754f9e757ee993ca14d212f834e7b8525a587 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Fri, 16 Feb 2024 02:14:00 -0500 Subject: [PATCH 67/81] Remove unrelated stuff, clean up some stuff, remove old lie stuff --- CMakeLists.txt | 3 - package.xml | 1 - pyproject.toml | 1 - src/simulator/simulator.controls.cpp | 26 +++---- src/simulator/simulator.gui.cpp | 7 -- src/simulator/simulator.hpp | 1 - src/simulator/simulator.render.cpp | 10 +-- src/util/lie/lie.cpp | 75 ++++++++++++++++++ src/util/lie/se3.cpp | 40 ---------- src/util/lie/se3.hpp | 102 +++--------------------- src/util/lie/se3_conversion.cpp | 112 --------------------------- src/util/lie/sim3.cpp | 13 ---- src/util/lie/so3.cpp | 17 ---- 13 files changed, 102 insertions(+), 306 deletions(-) create mode 100644 src/util/lie/lie.cpp delete mode 100644 src/util/lie/se3.cpp delete mode 100644 src/util/lie/se3_conversion.cpp delete mode 100644 src/util/lie/sim3.cpp delete mode 100644 src/util/lie/so3.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index ae81f7276..5df7f4ffa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -92,9 +92,6 @@ if (MROVER_BUILD_SIM) include(cmake/webgpu.cmake) add_subdirectory(deps/glfw3webgpu SYSTEM) - find_package(Assimp REQUIRED) - find_package(Bullet REQUIRED) - find_package(glfw3 REQUIRED) endif () find_package(OpenCV REQUIRED) find_package(ZED QUIET) diff --git a/package.xml b/package.xml index 656da5c60..0839b8d8f 100644 --- a/package.xml +++ b/package.xml @@ -67,7 +67,6 @@ rviz_imu_plugin robot_localization - rtcm_msgs diff --git a/pyproject.toml b/pyproject.toml index 5b1da3ab5..3d778431c 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -23,7 +23,6 @@ dependencies = [ "moteus==0.3.59", "pymap3d==3.0.1", "aenum==3.1.15", - "pyubx2==1.2.35", "daphne==4.0.0", "channels==4.0.0", ] diff --git a/src/simulator/simulator.controls.cpp b/src/simulator/simulator.controls.cpp index 225fdfc9b..cc1fa8142 100644 --- a/src/simulator/simulator.controls.cpp +++ b/src/simulator/simulator.controls.cpp @@ -83,32 +83,32 @@ namespace mrover { auto SimulatorNodelet::freeLook(Clock::duration dt) -> void { float flySpeed = mFlySpeed * std::chrono::duration_cast>(dt).count(); - SE3d::Tangent delta_x, delta_y, delta_z; - delta_x << Eigen::Vector3d::UnitX(), Eigen::Vector3d::Zero(); - delta_y << Eigen::Vector3d::UnitY(), Eigen::Vector3d::Zero(); - delta_z << Eigen::Vector3d::UnitZ(), Eigen::Vector3d::Zero(); + SE3d::Tangent deltaX, deltaY, deltaZ; + deltaX << Eigen::Vector3d::UnitX(), Eigen::Vector3d::Zero(); + deltaY << Eigen::Vector3d::UnitY(), Eigen::Vector3d::Zero(); + deltaZ << Eigen::Vector3d::UnitZ(), Eigen::Vector3d::Zero(); // TODO: turn this into a single rplus and lplus operation // overloaded rplus for movement relative to camera frame if (glfwGetKey(mWindow.get(), mCamForwardKey) == GLFW_PRESS) { - mCameraInWorld += delta_x * flySpeed; + mCameraInWorld += deltaX * flySpeed; } if (glfwGetKey(mWindow.get(), mCamBackwardKey) == GLFW_PRESS) { - mCameraInWorld += delta_x * -flySpeed; + mCameraInWorld += deltaX * -flySpeed; } if (glfwGetKey(mWindow.get(), mCamLeftKey) == GLFW_PRESS) { - mCameraInWorld += delta_y * flySpeed; + mCameraInWorld += deltaY * flySpeed; } if (glfwGetKey(mWindow.get(), mCamRightKey) == GLFW_PRESS) { - mCameraInWorld += delta_y * -flySpeed; + mCameraInWorld += deltaY * -flySpeed; } // overloaded lplus for movement relative to world frame if (glfwGetKey(mWindow.get(), mCamUpKey) == GLFW_PRESS) { - mCameraInWorld = delta_z * flySpeed + mCameraInWorld; + mCameraInWorld = deltaZ * flySpeed + mCameraInWorld; } if (glfwGetKey(mWindow.get(), mCamDownKey) == GLFW_PRESS) { - mCameraInWorld = delta_z * -flySpeed + mCameraInWorld; + mCameraInWorld = deltaZ * -flySpeed + mCameraInWorld; } Eigen::Vector2i size; @@ -122,9 +122,9 @@ namespace mrover { Eigen::Vector2d delta = (mouse - center) * mLookSense; // lplus for tilt in camera frame, rplus for pan in world frame - SO3d::Tangent delta_Ry = Eigen::Vector3d::UnitY() * delta.y(); - SO3d::Tangent delta_Rz = Eigen::Vector3d::UnitZ() * -delta.x(); - mCameraInWorld.asSO3() = delta_Rz + mCameraInWorld.asSO3() + delta_Ry; + SO3d::Tangent deltaRy = Eigen::Vector3d::UnitY() * delta.y(); + SO3d::Tangent deltaRz = Eigen::Vector3d::UnitZ() * -delta.x(); + mCameraInWorld.asSO3() = deltaRz + mCameraInWorld.asSO3() + deltaRy; centerCursor(); } diff --git a/src/simulator/simulator.gui.cpp b/src/simulator/simulator.gui.cpp index 3bd9d7911..5888cff53 100644 --- a/src/simulator/simulator.gui.cpp +++ b/src/simulator/simulator.gui.cpp @@ -134,13 +134,6 @@ namespace mrover { } } - { - R3 rayStart = mCameraInWorld.translation(); - R3 rayEnd = rayStart + mCameraInWorld.rotation().matrix().col(0); - // btMul - // mDynamicsWorld->rayTest(r3ToBtVector3(rayStart), r3ToBtVector3(rayEnd), [ - } - for (Camera const& camera: mCameras) { float aspect = static_cast(camera.resolution.x()) / static_cast(camera.resolution.y()); ImGui::Image(camera.colorTextureView, {320, 320 / aspect}, {0, 0}, {1, 1}); diff --git a/src/simulator/simulator.hpp b/src/simulator/simulator.hpp index 8494a9b06..8a1782f52 100644 --- a/src/simulator/simulator.hpp +++ b/src/simulator/simulator.hpp @@ -4,7 +4,6 @@ #include "glfw_pointer.hpp" #include "wgpu_objects.hpp" -#include "glfw_pointer.hpp" using namespace std::literals; diff --git a/src/simulator/simulator.render.cpp b/src/simulator/simulator.render.cpp index b6ad2a396..14ed062e4 100644 --- a/src/simulator/simulator.render.cpp +++ b/src/simulator/simulator.render.cpp @@ -412,7 +412,7 @@ namespace mrover { SE3d linkInWorld = urdf.linkInWorld(link->name); SE3d modelInLink = btTransformToSe3(urdfPoseToBtTransform(link->visual->origin)); SE3d modelInWorld = linkInWorld * modelInLink; - renderModel(pass, model, meta.visualUniforms.at(visualIndex), SIM3{modelInWorld.translation(), modelInWorld.rotation(), R3::Ones()}); + renderModel(pass, model, meta.visualUniforms.at(visualIndex), SIM3{modelInWorld, R3::Ones()}); visualIndex++; } } @@ -448,18 +448,18 @@ namespace mrover { auto* shape = compound->getChildShape(i); if (auto* box = dynamic_cast(shape)) { btVector3 extents = box->getHalfExtentsWithoutMargin() * 2; - SIM3 modelToWorld{shapeToWorld.translation(), shapeToWorld.rotation(), R3{extents.x(), extents.y(), extents.z()}}; + SIM3 modelToWorld{shapeToWorld, R3{extents.x(), extents.y(), extents.z()}}; renderModel(pass, mUriToModel.at(CUBE_PRIMITIVE_URI), meta.collisionUniforms.at(i), modelToWorld); } else if (auto* sphere = dynamic_cast(shape)) { btScalar diameter = sphere->getRadius() * 2; - SIM3 modelToWorld{shapeToWorld.translation(), shapeToWorld.rotation(), R3{diameter, diameter, diameter}}; + SIM3 modelToWorld{shapeToWorld, R3{diameter, diameter, diameter}}; renderModel(pass, mUriToModel.at(SPHERE_PRIMITIVE_URI), meta.collisionUniforms.at(i), modelToWorld); } else if (auto* cylinder = dynamic_cast(shape)) { btVector3 extents = cylinder->getHalfExtentsWithoutMargin() * 2; - SIM3 modelToWorld{shapeToWorld.translation(), shapeToWorld.rotation(), R3{extents.x(), extents.y(), extents.z()}}; + SIM3 modelToWorld{shapeToWorld, R3{extents.x(), extents.y(), extents.z()}}; renderModel(pass, mUriToModel.at(CYLINDER_PRIMITIVE_URI), meta.collisionUniforms.at(i), modelToWorld); } else if (auto* mesh = dynamic_cast(shape)) { - SIM3 modelToWorld{shapeToWorld.translation(), shapeToWorld.rotation(), R3::Ones()}; + SIM3 modelToWorld{shapeToWorld, R3::Ones()}; renderModel(pass, mUriToModel.at(mMeshToUri.at(const_cast(mesh))), meta.collisionUniforms.at(i), modelToWorld); } else { NODELET_WARN_STREAM_ONCE(std::format("Tried to render unsupported collision shape: {}", shape->getName())); diff --git a/src/util/lie/lie.cpp b/src/util/lie/lie.cpp new file mode 100644 index 000000000..918051d2d --- /dev/null +++ b/src/util/lie/lie.cpp @@ -0,0 +1,75 @@ +#include "se3.hpp" + +SIM3::SIM3(SE3d const& se3, R3 const& scale) { + mTransform.fromPositionOrientationScale(se3.translation(), se3.rotation(), scale); +} + +auto SIM3::matrix() const -> Eigen::Matrix4d { + return mTransform.matrix(); +} + +auto SIM3::position() const -> R3 { + return mTransform.translation(); +} + +auto SE3Conversions::fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId) -> SE3d { + geometry_msgs::TransformStamped transform = buffer.lookupTransform(fromFrameId, toFrameId, ros::Time{}); + return fromTf(transform.transform); +} + +auto SE3Conversions::pushToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3d const& tf) -> void { + broadcaster.sendTransform(toTransformStamped(tf, parentFrameId, childFrameId)); +} + +auto SE3Conversions::fromTf(geometry_msgs::Transform const& transform) -> SE3d { + return {{transform.translation.x, transform.translation.y, transform.translation.z}, + Eigen::Quaterniond{transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z}}; +} + +auto SE3Conversions::fromPose(geometry_msgs::Pose const& pose) -> SE3d { + return {{pose.position.x, pose.position.y, pose.position.z}, + Eigen::Quaterniond{pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z}}; +} + +auto SE3Conversions::toPose(SE3d const& tf) -> geometry_msgs::Pose { + geometry_msgs::Pose pose; + pose.position.x = tf.x(); + pose.position.y = tf.y(); + pose.position.z = tf.z(); + SE3d::Quaternion const& q = tf.quat(); + pose.orientation.x = q.x(); + pose.orientation.y = q.y(); + pose.orientation.z = q.z(); + pose.orientation.w = q.w(); + return pose; +} + +auto SE3Conversions::toTransform(SE3d const& tf) -> geometry_msgs::Transform { + geometry_msgs::Transform transform; + transform.translation.x = tf.x(); + transform.translation.y = tf.y(); + transform.translation.z = tf.z(); + SE3d::Quaternion const& q = tf.quat(); + transform.rotation.x = q.x(); + transform.rotation.y = q.y(); + transform.rotation.z = q.z(); + transform.rotation.w = q.w(); + return transform; +} + +auto SE3Conversions::toPoseStamped(SE3d const& tf, std::string const& frameId) -> geometry_msgs::PoseStamped { + geometry_msgs::PoseStamped pose; + pose.pose = toPose(tf); + pose.header.frame_id = frameId; + pose.header.stamp = ros::Time::now(); + return pose; +} + +auto SE3Conversions::toTransformStamped(SE3d const& tf, std::string const& parentFrameId, std::string const& childFrameId) -> geometry_msgs::TransformStamped { + geometry_msgs::TransformStamped transform; + transform.transform = toTransform(tf); + transform.header.frame_id = parentFrameId; + transform.header.stamp = ros::Time::now(); + transform.child_frame_id = childFrameId; + return transform; +} diff --git a/src/util/lie/se3.cpp b/src/util/lie/se3.cpp deleted file mode 100644 index ce4e4593d..000000000 --- a/src/util/lie/se3.cpp +++ /dev/null @@ -1,40 +0,0 @@ -#include "se3.hpp" - -SE3 SE3::fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId) { - geometry_msgs::TransformStamped transform = buffer.lookupTransform(fromFrameId, toFrameId, ros::Time{}); - return fromTf(transform.transform); -} - -void SE3::pushToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3 const& tf) { - broadcaster.sendTransform(tf.toTransformStamped(parentFrameId, childFrameId)); -} - -SE3::SE3(R3 const& position, SO3 const& rotation) { - assert(!position.hasNaN()); - mTransform.translate(position); - mTransform.rotate(rotation.mAngleAxis); -} - -Eigen::Matrix4d SE3::matrix() const { - return mTransform.matrix(); -} - -R3 SE3::position() const { - return mTransform.translation(); -} - -SO3 SE3::rotation() const { - return mTransform.rotation(); -} - -double SE3::distanceTo(SE3 const& other) const { - return (position() - other.position()).norm(); -} - -auto SE3::inverse() const -> SE3 { - return SE3{mTransform.inverse()}; -} - -SE3 SE3::operator*(SE3 const& other) const { - return other.mTransform * mTransform; -} diff --git a/src/util/lie/se3.hpp b/src/util/lie/se3.hpp index 2d3c74a4e..1e51e4dab 100644 --- a/src/util/lie/se3.hpp +++ b/src/util/lie/se3.hpp @@ -2,7 +2,6 @@ #include #include -#include #include #include @@ -16,106 +15,23 @@ using manif::SE3d, manif::SO3d; using R3 = Eigen::Vector3d; using S3 = Eigen::Quaterniond; -/** - * @brief A 3D rotation. - */ -class SO3 { - using AngleAxis = Eigen::AngleAxis; - - AngleAxis mAngleAxis = AngleAxis::Identity(); - -public: - friend class SE3; - friend class SIM3; - - SO3() = default; - - SO3(double w, double x, double y, double z) : mAngleAxis{Eigen::Quaterniond{w, x, y, z}} {} - - template>, - typename = std::enable_if_t<(sizeof...(Args) > 0)>> - SO3(Args&&... args) : mAngleAxis{std::forward(args)...} {} - - [[nodiscard]] SO3 operator*(SO3 const& other) const; - - [[nodiscard]] R3 operator*(R3 const& other) const; - - [[nodiscard]] Eigen::Matrix3d matrix() const; - - [[nodiscard]] Eigen::Quaterniond quaternion() const; - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -/** - * @brief A 3D rigid transformation (direct isometry). - * - * In simpler terms: a 3D rotation followed by a 3D translation. - */ -class SE3 { - using Transform = Eigen::Transform; - - Transform mTransform = Transform::Identity(); - - static SE3 fromTf(geometry_msgs::Transform const& transform); - - static SE3 fromPose(geometry_msgs::Pose const& pose); - - [[nodiscard]] geometry_msgs::Pose toPose() const; - - [[nodiscard]] geometry_msgs::Transform toTransform() const; - - [[nodiscard]] geometry_msgs::PoseStamped toPoseStamped(std::string const& frameId) const; - - [[nodiscard]] geometry_msgs::TransformStamped toTransformStamped(std::string const& parentFrameId, std::string const& childFrameId) const; - -public: - [[nodiscard]] static SE3 fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId); - - static void pushToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3 const& tf); - - SE3() = default; - - SE3(R3 const& position, SO3 const& rotation); - - template>, - typename = std::enable_if_t<(sizeof...(Args) > 0)>> - SE3(Args&&... args) : mTransform{std::forward(args)...} {} - - [[nodiscard]] SE3 operator*(SE3 const& other) const; - - [[nodiscard]] Eigen::Matrix4d matrix() const; - - [[nodiscard]] R3 position() const; - - [[nodiscard]] SO3 rotation() const; - - [[nodiscard]] double distanceTo(SE3 const& other) const; - - [[nodiscard]] auto inverse() const -> SE3; - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - class SE3Conversions { public: - static SE3d fromTf(geometry_msgs::Transform const& transform); + static auto fromTf(geometry_msgs::Transform const& transform) -> SE3d; - static SE3d fromPose(geometry_msgs::Pose const& pose); + static auto fromPose(geometry_msgs::Pose const& pose) -> SE3d; - [[nodiscard]] static geometry_msgs::Pose toPose(SE3d const& tf); + [[nodiscard]] static auto toPose(SE3d const& tf) -> geometry_msgs::Pose; - [[nodiscard]] static geometry_msgs::Transform toTransform(SE3d const& tf); + [[nodiscard]] static auto toTransform(SE3d const& tf) -> geometry_msgs::Transform; - [[nodiscard]] static geometry_msgs::PoseStamped toPoseStamped(SE3d const& tf, std::string const& frameId); + [[nodiscard]] static auto toPoseStamped(SE3d const& tf, std::string const& frameId) -> geometry_msgs::PoseStamped; - [[nodiscard]] static geometry_msgs::TransformStamped toTransformStamped(SE3d const& tf, std::string const& parentFrameId, std::string const& childFrameId); + [[nodiscard]] static auto toTransformStamped(SE3d const& tf, std::string const& parentFrameId, std::string const& childFrameId) -> geometry_msgs::TransformStamped; - [[nodiscard]] static SE3d fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId); + [[nodiscard]] static auto fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId) -> SE3d; - static void pushToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3d const& tf); + static auto pushToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3d const& tf) -> void; }; class SIM3 { @@ -126,7 +42,7 @@ class SIM3 { public: SIM3() = default; - SIM3(R3 const& position, SO3 const& rotation, R3 const& scale); + SIM3(SE3d const& se3, R3 const& scale); [[nodiscard]] auto matrix() const -> Eigen::Matrix4d; diff --git a/src/util/lie/se3_conversion.cpp b/src/util/lie/se3_conversion.cpp deleted file mode 100644 index b3f031102..000000000 --- a/src/util/lie/se3_conversion.cpp +++ /dev/null @@ -1,112 +0,0 @@ -#include "se3.hpp" - -SE3d SE3Conversions::fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId) { - geometry_msgs::TransformStamped transform = buffer.lookupTransform(fromFrameId, toFrameId, ros::Time{}); - return fromTf(transform.transform); -} - -void SE3Conversions::pushToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3d const& tf) { - broadcaster.sendTransform(toTransformStamped(tf, parentFrameId, childFrameId)); -} - -SE3d SE3Conversions::fromTf(geometry_msgs::Transform const& transform) { - return {{transform.translation.x, transform.translation.y, transform.translation.z}, - Eigen::Quaterniond{transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z}}; -} - -SE3d SE3Conversions::fromPose(geometry_msgs::Pose const& pose) { - return {{pose.position.x, pose.position.y, pose.position.z}, - Eigen::Quaterniond{pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z}}; -} - -geometry_msgs::Pose SE3Conversions::toPose(SE3d const& tf) { - geometry_msgs::Pose pose; - pose.position.x = tf.x(); - pose.position.y = tf.y(); - pose.position.z = tf.z(); - pose.orientation.x = tf.quat().x(); - pose.orientation.y = tf.quat().y(); - pose.orientation.z = tf.quat().z(); - pose.orientation.w = tf.quat().w(); - return pose; -} - -geometry_msgs::Transform SE3Conversions::toTransform(SE3d const& tf) { - geometry_msgs::Transform transform; - transform.translation.x = tf.x(); - transform.translation.y = tf.y(); - transform.translation.z = tf.z(); - transform.rotation.x = tf.quat().x(); - transform.rotation.y = tf.quat().y(); - transform.rotation.z = tf.quat().z(); - transform.rotation.w = tf.quat().w(); - return transform; -} - -geometry_msgs::PoseStamped SE3Conversions::toPoseStamped(SE3d const& tf, std::string const& frameId) { - geometry_msgs::PoseStamped pose; - pose.pose = toPose(tf); - pose.header.frame_id = frameId; - pose.header.stamp = ros::Time::now(); - return pose; -} - -geometry_msgs::TransformStamped SE3Conversions::toTransformStamped(SE3d const& tf, std::string const& parentFrameId, std::string const& childFrameId) { - geometry_msgs::TransformStamped transform; - transform.transform = toTransform(tf); - transform.header.frame_id = parentFrameId; - transform.header.stamp = ros::Time::now(); - transform.child_frame_id = childFrameId; - return transform; -} - -SE3 SE3::fromTf(geometry_msgs::Transform const& transform) { - return {{transform.translation.x, transform.translation.y, transform.translation.z}, - {Eigen::Quaterniond{transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z}}}; -} - -SE3 SE3::fromPose(geometry_msgs::Pose const& pose) { - return {{pose.position.x, pose.position.y, pose.position.z}, - {Eigen::Quaterniond{pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z}}}; -} - -geometry_msgs::Pose SE3::toPose() const { - geometry_msgs::Pose pose; - pose.position.x = position().x(); - pose.position.y = position().y(); - pose.position.z = position().z(); - pose.orientation.x = rotation().quaternion().x(); - pose.orientation.y = rotation().quaternion().y(); - pose.orientation.z = rotation().quaternion().z(); - pose.orientation.w = rotation().quaternion().w(); - return pose; -} - -geometry_msgs::Transform SE3::toTransform() const { - geometry_msgs::Transform transform; - transform.translation.x = position().x(); - transform.translation.y = position().y(); - transform.translation.z = position().z(); - transform.rotation.x = rotation().quaternion().x(); - transform.rotation.y = rotation().quaternion().y(); - transform.rotation.z = rotation().quaternion().z(); - transform.rotation.w = rotation().quaternion().w(); - return transform; -} - -geometry_msgs::PoseStamped SE3::toPoseStamped(std::string const& frameId) const { - geometry_msgs::PoseStamped pose; - pose.pose = toPose(); - pose.header.frame_id = frameId; - pose.header.stamp = ros::Time::now(); - return pose; -} - -geometry_msgs::TransformStamped SE3::toTransformStamped(std::string const& parentFrameId, std::string const& childFrameId) const { - geometry_msgs::TransformStamped transform; - transform.transform = toTransform(); - transform.header.frame_id = parentFrameId; - transform.header.stamp = ros::Time::now(); - transform.child_frame_id = childFrameId; - return transform; -} diff --git a/src/util/lie/sim3.cpp b/src/util/lie/sim3.cpp deleted file mode 100644 index 65f9110f9..000000000 --- a/src/util/lie/sim3.cpp +++ /dev/null @@ -1,13 +0,0 @@ -#include "se3.hpp" - -SIM3::SIM3(R3 const& position, SO3 const& rotation, R3 const& scale) { - mTransform.fromPositionOrientationScale(position, rotation.mAngleAxis, scale); -} - -auto SIM3::matrix() const -> Eigen::Matrix4d { - return mTransform.matrix(); -} - -auto SIM3::position() const -> R3 { - return mTransform.translation(); -} diff --git a/src/util/lie/so3.cpp b/src/util/lie/so3.cpp deleted file mode 100644 index 422cd5c42..000000000 --- a/src/util/lie/so3.cpp +++ /dev/null @@ -1,17 +0,0 @@ -#include "se3.hpp" - -Eigen::Quaterniond SO3::quaternion() const { - return Eigen::Quaterniond{mAngleAxis}; -} - -Eigen::Matrix3d SO3::matrix() const { - return mAngleAxis.toRotationMatrix(); -} - -SO3 SO3::operator*(SO3 const& other) const { - return other.quaternion() * quaternion(); -} - -R3 SO3::operator*(R3 const& other) const { - return quaternion() * other; -} From 3fc501434803774239879b74d23c90df4d760592 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Fri, 16 Feb 2024 02:16:08 -0500 Subject: [PATCH 68/81] Rename to lie.hpp --- src/perception/tag_detector/pch.hpp | 2 +- src/perception/zed_wrapper/zed_wrapper.cpp | 2 +- src/simulator/pch.hpp | 2 +- src/util/lie/lie.cpp | 2 +- src/util/lie/{se3.hpp => lie.hpp} | 0 5 files changed, 4 insertions(+), 4 deletions(-) rename src/util/lie/{se3.hpp => lie.hpp} (100%) diff --git a/src/perception/tag_detector/pch.hpp b/src/perception/tag_detector/pch.hpp index cb9448d6a..dfa943021 100644 --- a/src/perception/tag_detector/pch.hpp +++ b/src/perception/tag_detector/pch.hpp @@ -32,4 +32,4 @@ #include #include #include -#include +#include diff --git a/src/perception/zed_wrapper/zed_wrapper.cpp b/src/perception/zed_wrapper/zed_wrapper.cpp index 0b6d3aafd..e05d61b61 100644 --- a/src/perception/zed_wrapper/zed_wrapper.cpp +++ b/src/perception/zed_wrapper/zed_wrapper.cpp @@ -1,7 +1,7 @@ #include "zed_wrapper.hpp" #include -#include +#include namespace mrover { diff --git a/src/simulator/pch.hpp b/src/simulator/pch.hpp index 08d94740c..e6ca84c02 100644 --- a/src/simulator/pch.hpp +++ b/src/simulator/pch.hpp @@ -71,7 +71,7 @@ #include #include #include -#include +#include #include #include diff --git a/src/util/lie/lie.cpp b/src/util/lie/lie.cpp index 918051d2d..b58beee26 100644 --- a/src/util/lie/lie.cpp +++ b/src/util/lie/lie.cpp @@ -1,4 +1,4 @@ -#include "se3.hpp" +#include "lie.hpp" SIM3::SIM3(SE3d const& se3, R3 const& scale) { mTransform.fromPositionOrientationScale(se3.translation(), se3.rotation(), scale); diff --git a/src/util/lie/se3.hpp b/src/util/lie/lie.hpp similarity index 100% rename from src/util/lie/se3.hpp rename to src/util/lie/lie.hpp From f65068c670284ef54cbc147af4adc3b2a592b48e Mon Sep 17 00:00:00 2001 From: qhdwight Date: Fri, 16 Feb 2024 02:58:32 -0500 Subject: [PATCH 69/81] Refactor look controls --- src/simulator/simulator.controls.cpp | 56 ++++++++++------------------ 1 file changed, 20 insertions(+), 36 deletions(-) diff --git a/src/simulator/simulator.controls.cpp b/src/simulator/simulator.controls.cpp index cc1fa8142..a5ebb9b3d 100644 --- a/src/simulator/simulator.controls.cpp +++ b/src/simulator/simulator.controls.cpp @@ -82,49 +82,33 @@ namespace mrover { } auto SimulatorNodelet::freeLook(Clock::duration dt) -> void { + auto axis = [this](int positive, int negative) -> double { + return (glfwGetKey(mWindow.get(), positive) == GLFW_PRESS) - (glfwGetKey(mWindow.get(), negative) == GLFW_PRESS); + }; + float flySpeed = mFlySpeed * std::chrono::duration_cast>(dt).count(); - SE3d::Tangent deltaX, deltaY, deltaZ; - deltaX << Eigen::Vector3d::UnitX(), Eigen::Vector3d::Zero(); - deltaY << Eigen::Vector3d::UnitY(), Eigen::Vector3d::Zero(); - deltaZ << Eigen::Vector3d::UnitZ(), Eigen::Vector3d::Zero(); - - // TODO: turn this into a single rplus and lplus operation - // overloaded rplus for movement relative to camera frame - if (glfwGetKey(mWindow.get(), mCamForwardKey) == GLFW_PRESS) { - mCameraInWorld += deltaX * flySpeed; - } - if (glfwGetKey(mWindow.get(), mCamBackwardKey) == GLFW_PRESS) { - mCameraInWorld += deltaX * -flySpeed; - } - if (glfwGetKey(mWindow.get(), mCamLeftKey) == GLFW_PRESS) { - mCameraInWorld += deltaY * flySpeed; - } - if (glfwGetKey(mWindow.get(), mCamRightKey) == GLFW_PRESS) { - mCameraInWorld += deltaY * -flySpeed; - } + R3 keyDelta = R3{axis(mCamForwardKey, mCamBackwardKey), axis(mCamLeftKey, mCamRightKey), axis(mCamUpKey, mCamDownKey)} * flySpeed; - // overloaded lplus for movement relative to world frame - if (glfwGetKey(mWindow.get(), mCamUpKey) == GLFW_PRESS) { - mCameraInWorld = deltaZ * flySpeed + mCameraInWorld; - } - if (glfwGetKey(mWindow.get(), mCamDownKey) == GLFW_PRESS) { - mCameraInWorld = deltaZ * -flySpeed + mCameraInWorld; - } + Eigen::Vector2i windowSize; + glfwGetWindowSize(mWindow.get(), &windowSize.x(), &windowSize.y()); + Eigen::Vector2d windowCenter = (windowSize / 2).cast(); + Eigen::Vector2d mouse; + glfwGetCursorPos(mWindow.get(), &mouse.x(), &mouse.y()); + Eigen::Vector2d mouseDelta = (mouse - windowCenter) * mLookSense; - Eigen::Vector2i size; - glfwGetWindowSize(mWindow.get(), &size.x(), &size.y()); + SE3d::Tangent cameraRelativeTwist; + cameraRelativeTwist << keyDelta.x(), keyDelta.y(), 0.0, 0.0, mouseDelta.y(), 0.0; - Eigen::Vector2d center = (size / 2).cast(); + SE3d::Tangent worldRelativeTwist; + worldRelativeTwist << 0.0, 0.0, keyDelta.z(), 0.0, 0.0, 0.0; - Eigen::Vector2d mouse; - glfwGetCursorPos(mWindow.get(), &mouse.x(), &mouse.y()); + mCameraInWorld = worldRelativeTwist + mCameraInWorld + cameraRelativeTwist; - Eigen::Vector2d delta = (mouse - center) * mLookSense; + SO3d::Tangent worldRelativeAngularVelocity; + worldRelativeAngularVelocity << 0.0, 0.0, -mouseDelta.x(); - // lplus for tilt in camera frame, rplus for pan in world frame - SO3d::Tangent deltaRy = Eigen::Vector3d::UnitY() * delta.y(); - SO3d::Tangent deltaRz = Eigen::Vector3d::UnitZ() * -delta.x(); - mCameraInWorld.asSO3() = deltaRz + mCameraInWorld.asSO3() + deltaRy; + // TODO: Is there a way to combine this with the above? + mCameraInWorld.asSO3() = worldRelativeAngularVelocity + mCameraInWorld.asSO3(); centerCursor(); } From a45e9b9ac104596b20ebe62f46fa4c0dd60301a4 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Fri, 16 Feb 2024 09:55:00 -0500 Subject: [PATCH 70/81] Comments --- src/simulator/simulator.controls.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/simulator/simulator.controls.cpp b/src/simulator/simulator.controls.cpp index a5ebb9b3d..24ac9e169 100644 --- a/src/simulator/simulator.controls.cpp +++ b/src/simulator/simulator.controls.cpp @@ -102,6 +102,10 @@ namespace mrover { SE3d::Tangent worldRelativeTwist; worldRelativeTwist << 0.0, 0.0, keyDelta.z(), 0.0, 0.0, 0.0; + // The plus operator is overloaded and corresponds to lplus and rplus in the micro Lie paper + // It applies the exponential map to the tangent space element + // The result can be acted upon a group element (in this case SE3) + // The side of the plus operator determines word vs. camera space application mCameraInWorld = worldRelativeTwist + mCameraInWorld + cameraRelativeTwist; SO3d::Tangent worldRelativeAngularVelocity; @@ -153,4 +157,4 @@ namespace mrover { } } -} // namespace mrover +} // namespace mrover \ No newline at end of file From 831c73aa235d0997bcefb4eee84cb0266c8916aa Mon Sep 17 00:00:00 2001 From: Riley Bridges <32557768+rbridges12@users.noreply.github.com> Date: Fri, 16 Feb 2024 17:16:08 -0500 Subject: [PATCH 71/81] Manif refactor (#648) --- .gitmodules | 4 + CMakeLists.txt | 11 +- deps/manif | 1 + src/perception/tag_detector/pch.hpp | 3 +- src/perception/tag_detector/tag_detector.cpp | 2 +- src/perception/tag_detector/tag_detector.hpp | 4 +- .../tag_detector/tag_detector.processing.cpp | 10 +- src/perception/zed_wrapper/pch.hpp | 3 +- .../zed_wrapper/zed_wrapper.bridge.cu | 1 - src/perception/zed_wrapper/zed_wrapper.cpp | 13 +- src/simulator/pch.hpp | 3 +- src/simulator/simulator.controls.cpp | 55 ++++----- src/simulator/simulator.gui.cpp | 10 +- src/simulator/simulator.hpp | 6 +- src/simulator/simulator.physics.cpp | 20 ++-- src/simulator/simulator.render.cpp | 26 ++-- src/simulator/simulator.sensors.cpp | 26 ++-- src/util/lie/lie.cpp | 75 ++++++++++++ src/util/lie/lie.hpp | 50 ++++++++ src/util/lie/se3.cpp | 40 ------- src/util/lie/se3.hpp | 112 ------------------ src/util/lie/se3_conversion.cpp | 52 -------- src/util/lie/sim3.cpp | 13 -- src/util/lie/so3.cpp | 17 --- 24 files changed, 228 insertions(+), 329 deletions(-) create mode 160000 deps/manif create mode 100644 src/util/lie/lie.cpp create mode 100644 src/util/lie/lie.hpp delete mode 100644 src/util/lie/se3.cpp delete mode 100644 src/util/lie/se3.hpp delete mode 100644 src/util/lie/se3_conversion.cpp delete mode 100644 src/util/lie/sim3.cpp delete mode 100644 src/util/lie/so3.cpp diff --git a/.gitmodules b/.gitmodules index e013bbf1b..547ba513a 100644 --- a/.gitmodules +++ b/.gitmodules @@ -8,3 +8,7 @@ url = https://dawn.googlesource.com/dawn shallow = true branch = chromium/6108 +[submodule "deps/manif"] + path = deps/manif + url = https://github.com/artivis/manif.git + shallow = true diff --git a/CMakeLists.txt b/CMakeLists.txt index 65406ed51..5df7f4ffa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -91,12 +91,12 @@ if (MROVER_BUILD_SIM) find_package(glfw3 REQUIRED) include(cmake/webgpu.cmake) - - add_subdirectory(deps/glfw3webgpu) + add_subdirectory(deps/glfw3webgpu SYSTEM) endif () find_package(OpenCV REQUIRED) find_package(ZED QUIET) find_package(Eigen3 REQUIRED) +add_subdirectory(deps/manif SYSTEM) find_package(PkgConfig REQUIRED) pkg_search_module(NetLink libnl-3.0) pkg_search_module(NetLinkRoute libnl-route-3.0) @@ -146,6 +146,7 @@ catkin_package() ## Libraries mrover_add_library(lie src/util/lie/*.cpp src/util/lie) +target_link_libraries(lie PUBLIC manif) ## ESW @@ -187,7 +188,7 @@ endif () ## Perception mrover_add_nodelet(tag_detector src/perception/tag_detector/*.cpp src/perception/tag_detector src/perception/tag_detector/pch.hpp) -mrover_nodelet_link_libraries(tag_detector opencv_core opencv_objdetect opencv_aruco opencv_imgproc opencv_highgui lie) +mrover_nodelet_link_libraries(tag_detector opencv_core opencv_objdetect opencv_aruco opencv_imgproc opencv_highgui lie manif) if (CUDA_FOUND) mrover_add_library(streaming src/esw/streaming/*.c* src/esw/streaming) @@ -200,7 +201,7 @@ endif () if (ZED_FOUND) mrover_add_nodelet(zed src/perception/zed_wrapper/*.c* src/perception/zed_wrapper src/perception/zed_wrapper/pch.hpp) mrover_nodelet_include_directories(zed ${ZED_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS}) - mrover_nodelet_link_libraries(zed ${ZED_LIBRARIES} ${SPECIAL_OS_LIBS} lie) + mrover_nodelet_link_libraries(zed ${ZED_LIBRARIES} ${SPECIAL_OS_LIBS} lie manif) mrover_nodelet_defines(zed ALLOW_BUILD_DEBUG # Ignore ZED warnings about Debug mode __CUDA_INCLUDE_COMPILER_INTERNAL_HEADERS__ # Eigen includes some files it should not, ignore @@ -216,7 +217,7 @@ mrover_add_node(arm_controller src/teleoperation/arm_controller/*.cpp) if (MROVER_BUILD_SIM) mrover_add_nodelet(simulator src/simulator/*.cpp src/simulator src/simulator/pch.hpp) mrover_nodelet_include_directories(simulator ${ASSIMP_INCLUDE_DIRS} ${BULLET_INCLUDE_DIRS} ${OPENCV_INCLUDE_DIRS} src/simulator/imgui) - mrover_nodelet_link_libraries(simulator ${ASSIMP_LIBRARIES} ${BULLET_LIBRARIES} glfw3webgpu webgpu glfw opencv_core opencv_imgcodecs opencv_imgproc lie) + mrover_nodelet_link_libraries(simulator ${ASSIMP_LIBRARIES} ${BULLET_LIBRARIES} glfw3webgpu webgpu glfw opencv_core opencv_imgcodecs opencv_imgproc lie manif) mrover_nodelet_defines(simulator BOOST_THREAD_PROVIDES_FUTURE) endif () diff --git a/deps/manif b/deps/manif new file mode 160000 index 000000000..c8a9fcbfe --- /dev/null +++ b/deps/manif @@ -0,0 +1 @@ +Subproject commit c8a9fcbfe157b55a7a860577febdc5350043a722 diff --git a/src/perception/tag_detector/pch.hpp b/src/perception/tag_detector/pch.hpp index 8b8ed1c34..dfa943021 100644 --- a/src/perception/tag_detector/pch.hpp +++ b/src/perception/tag_detector/pch.hpp @@ -30,5 +30,6 @@ #include #include +#include #include -#include +#include diff --git a/src/perception/tag_detector/tag_detector.cpp b/src/perception/tag_detector/tag_detector.cpp index ee9af68ca..7ece83885 100644 --- a/src/perception/tag_detector/tag_detector.cpp +++ b/src/perception/tag_detector/tag_detector.cpp @@ -140,5 +140,5 @@ namespace mrover { res.success = true; return true; } - + } // namespace mrover diff --git a/src/perception/tag_detector/tag_detector.hpp b/src/perception/tag_detector/tag_detector.hpp index b14541478..b6fb3a45b 100644 --- a/src/perception/tag_detector/tag_detector.hpp +++ b/src/perception/tag_detector/tag_detector.hpp @@ -6,7 +6,7 @@ namespace mrover { int id = -1; int hitCount = 0; cv::Point2f imageCenter{}; - std::optional tagInCam; + std::optional tagInCam; }; class TagDetectorNodelet : public nodelet::Nodelet { @@ -53,7 +53,7 @@ namespace mrover { void publishThresholdedImage(); - std::optional getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v); + std::optional getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v); public: TagDetectorNodelet() = default; diff --git a/src/perception/tag_detector/tag_detector.processing.cpp b/src/perception/tag_detector/tag_detector.processing.cpp index b3e47fbc0..b922bb075 100644 --- a/src/perception/tag_detector/tag_detector.processing.cpp +++ b/src/perception/tag_detector/tag_detector.processing.cpp @@ -8,7 +8,7 @@ namespace mrover { * @param u X Pixel Position * @param v Y Pixel Position */ - std::optional TagDetectorNodelet::getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v) { + std::optional TagDetectorNodelet::getTagInCamFromPixel(sensor_msgs::PointCloud2ConstPtr const& cloudPtr, size_t u, size_t v) { assert(cloudPtr); if (u >= cloudPtr->width || v >= cloudPtr->height) { @@ -23,7 +23,7 @@ namespace mrover { return std::nullopt; } - return std::make_optional(R3{point.x, point.y, point.z}, SO3{}); + return std::make_optional(R3{point.x, point.y, point.z}, SO3d::Identity()); } /** @@ -81,7 +81,7 @@ namespace mrover { if (tag.tagInCam) { // Publish tag to immediate std::string immediateFrameId = "immediateFiducial" + std::to_string(tag.id); - SE3::pushToTfTree(mTfBroadcaster, immediateFrameId, mCameraFrameId, tag.tagInCam.value()); + SE3Conversions::pushToTfTree(mTfBroadcaster, immediateFrameId, mCameraFrameId, tag.tagInCam.value()); } } @@ -108,8 +108,8 @@ namespace mrover { std::string immediateFrameId = "immediateFiducial" + std::to_string(tag.id); // Publish tag to odom std::string const& parentFrameId = mUseOdom ? mOdomFrameId : mMapFrameId; - SE3 tagInParent = SE3::fromTfTree(mTfBuffer, parentFrameId, immediateFrameId); - SE3::pushToTfTree(mTfBroadcaster, "fiducial" + std::to_string(id), parentFrameId, tagInParent); + SE3d tagInParent = SE3Conversions::fromTfTree(mTfBuffer, parentFrameId, immediateFrameId); + SE3Conversions::pushToTfTree(mTfBroadcaster, "fiducial" + std::to_string(id), parentFrameId, tagInParent); } catch (tf2::ExtrapolationException const&) { NODELET_WARN("Old data for immediate tag"); } catch (tf2::LookupException const&) { diff --git a/src/perception/zed_wrapper/pch.hpp b/src/perception/zed_wrapper/pch.hpp index 8088686af..4df657b64 100644 --- a/src/perception/zed_wrapper/pch.hpp +++ b/src/perception/zed_wrapper/pch.hpp @@ -24,6 +24,5 @@ #include #include -#include -#include #include +#include diff --git a/src/perception/zed_wrapper/zed_wrapper.bridge.cu b/src/perception/zed_wrapper/zed_wrapper.bridge.cu index 5be3946bd..cacb63f77 100644 --- a/src/perception/zed_wrapper/zed_wrapper.bridge.cu +++ b/src/perception/zed_wrapper/zed_wrapper.bridge.cu @@ -1,5 +1,4 @@ // Be careful what you include in this file, it is compiled with nvcc (NVIDIA CUDA compiler) -// For example OpenCV and lie includes cause problems #include "zed_wrapper.hpp" diff --git a/src/perception/zed_wrapper/zed_wrapper.cpp b/src/perception/zed_wrapper/zed_wrapper.cpp index dd943c431..e05d61b61 100644 --- a/src/perception/zed_wrapper/zed_wrapper.cpp +++ b/src/perception/zed_wrapper/zed_wrapper.cpp @@ -1,5 +1,8 @@ #include "zed_wrapper.hpp" +#include +#include + namespace mrover { /** @@ -252,11 +255,11 @@ namespace mrover { sl::Translation const& translation = pose.getTranslation(); sl::Orientation const& orientation = pose.getOrientation(); try { - SE3 leftCameraInOdom{{translation.x, translation.y, translation.z}, - Eigen::Quaterniond{orientation.w, orientation.x, orientation.y, orientation.z}.normalized()}; - SE3 leftCameraInBaseLink = SE3::fromTfTree(mTfBuffer, "base_link", "zed2i_left_camera_frame"); - SE3 baseLinkInOdom = leftCameraInBaseLink * leftCameraInOdom; - SE3::pushToTfTree(mTfBroadcaster, "base_link", "odom", baseLinkInOdom); + SE3d leftCameraInOdom{{translation.x, translation.y, translation.z}, + Eigen::Quaterniond{orientation.w, orientation.x, orientation.y, orientation.z}.normalized()}; + SE3d baseLinkToLeftCamera = SE3Conversions::fromTfTree(mTfBuffer, "base_link", "zed2i_left_camera_frame"); + SE3d baseLinkInOdom = leftCameraInOdom * baseLinkToLeftCamera; + SE3Conversions::pushToTfTree(mTfBroadcaster, "base_link", "odom", baseLinkInOdom); } catch (tf2::TransformException& e) { NODELET_WARN_STREAM("Failed to get transform: " << e.what()); } diff --git a/src/simulator/pch.hpp b/src/simulator/pch.hpp index 233333e9b..e6ca84c02 100644 --- a/src/simulator/pch.hpp +++ b/src/simulator/pch.hpp @@ -68,9 +68,10 @@ #include #include +#include #include #include -#include +#include #include #include diff --git a/src/simulator/simulator.controls.cpp b/src/simulator/simulator.controls.cpp index 6d50b1f91..24ac9e169 100644 --- a/src/simulator/simulator.controls.cpp +++ b/src/simulator/simulator.controls.cpp @@ -82,40 +82,37 @@ namespace mrover { } auto SimulatorNodelet::freeLook(Clock::duration dt) -> void { - float flySpeed = mFlySpeed * std::chrono::duration_cast>(dt).count(); - if (glfwGetKey(mWindow.get(), mCamRightKey) == GLFW_PRESS) { - mCameraInWorld = SE3{R3{0, -flySpeed, 0}, SO3{}} * mCameraInWorld; - } - if (glfwGetKey(mWindow.get(), mCamLeftKey) == GLFW_PRESS) { - mCameraInWorld = SE3{R3{0, flySpeed, 0}, SO3{}} * mCameraInWorld; - } - if (glfwGetKey(mWindow.get(), mCamForwardKey) == GLFW_PRESS) { - mCameraInWorld = SE3{R3{flySpeed, 0, 0}, SO3{}} * mCameraInWorld; - } - if (glfwGetKey(mWindow.get(), mCamBackwardKey) == GLFW_PRESS) { - mCameraInWorld = SE3{R3{-flySpeed, 0, 0}, SO3{}} * mCameraInWorld; - } - if (glfwGetKey(mWindow.get(), mCamUpKey) == GLFW_PRESS) { - mCameraInWorld = mCameraInWorld * SE3{R3{0, 0, flySpeed}, SO3{}}; - } - if (glfwGetKey(mWindow.get(), mCamDownKey) == GLFW_PRESS) { - mCameraInWorld = mCameraInWorld * SE3{R3{0, 0, -flySpeed}, SO3{}}; - } + auto axis = [this](int positive, int negative) -> double { + return (glfwGetKey(mWindow.get(), positive) == GLFW_PRESS) - (glfwGetKey(mWindow.get(), negative) == GLFW_PRESS); + }; - Eigen::Vector2i size; - glfwGetWindowSize(mWindow.get(), &size.x(), &size.y()); - - Eigen::Vector2d center = (size / 2).cast(); + float flySpeed = mFlySpeed * std::chrono::duration_cast>(dt).count(); + R3 keyDelta = R3{axis(mCamForwardKey, mCamBackwardKey), axis(mCamLeftKey, mCamRightKey), axis(mCamUpKey, mCamDownKey)} * flySpeed; + Eigen::Vector2i windowSize; + glfwGetWindowSize(mWindow.get(), &windowSize.x(), &windowSize.y()); + Eigen::Vector2d windowCenter = (windowSize / 2).cast(); Eigen::Vector2d mouse; glfwGetCursorPos(mWindow.get(), &mouse.x(), &mouse.y()); + Eigen::Vector2d mouseDelta = (mouse - windowCenter) * mLookSense; + + SE3d::Tangent cameraRelativeTwist; + cameraRelativeTwist << keyDelta.x(), keyDelta.y(), 0.0, 0.0, mouseDelta.y(), 0.0; + + SE3d::Tangent worldRelativeTwist; + worldRelativeTwist << 0.0, 0.0, keyDelta.z(), 0.0, 0.0, 0.0; + + // The plus operator is overloaded and corresponds to lplus and rplus in the micro Lie paper + // It applies the exponential map to the tangent space element + // The result can be acted upon a group element (in this case SE3) + // The side of the plus operator determines word vs. camera space application + mCameraInWorld = worldRelativeTwist + mCameraInWorld + cameraRelativeTwist; - Eigen::Vector2d delta = (mouse - center) * mLookSense; + SO3d::Tangent worldRelativeAngularVelocity; + worldRelativeAngularVelocity << 0.0, 0.0, -mouseDelta.x(); - // TODO(quintin): use lie algebra more here? we have a perturbation in the tangent space - R3 p = mCameraInWorld.position(); - SO3 q = SO3{delta.y(), R3::UnitY()} * mCameraInWorld.rotation() * SO3{-delta.x(), R3::UnitZ()}; - mCameraInWorld = SE3{p, q}; + // TODO: Is there a way to combine this with the above? + mCameraInWorld.asSO3() = worldRelativeAngularVelocity + mCameraInWorld.asSO3(); centerCursor(); } @@ -160,4 +157,4 @@ namespace mrover { } } -} // namespace mrover +} // namespace mrover \ No newline at end of file diff --git a/src/simulator/simulator.gui.cpp b/src/simulator/simulator.gui.cpp index be565cdd5..5888cff53 100644 --- a/src/simulator/simulator.gui.cpp +++ b/src/simulator/simulator.gui.cpp @@ -94,15 +94,15 @@ namespace mrover { URDF const& rover = it->second; { - SE3 baseLinkInMap = rover.linkInWorld("base_link"); - R3 p = baseLinkInMap.position(); - S3 q = baseLinkInMap.rotation().quaternion(); + SE3d baseLinkInMap = rover.linkInWorld("base_link"); + R3 p = baseLinkInMap.translation(); + S3 q = baseLinkInMap.quat(); ImGui::Text("Rover Position: (%.2f, %.2f, %.2f)", p.x(), p.y(), p.z()); ImGui::Text("Rover Orientation: (%.2f, %.2f, %.2f, %.2f)", q.w(), q.x(), q.y(), q.z()); } { - R3 p = mCameraInWorld.position(); - S3 q = mCameraInWorld.rotation().quaternion(); + R3 p = mCameraInWorld.translation(); + S3 q = mCameraInWorld.quat(); ImGui::Text("Camera Position: (%.2f, %.2f, %.2f)", p.x(), p.y(), p.z()); ImGui::Text("Camera Orientation: (%.2f, %.2f, %.2f, %.2f)", q.w(), q.x(), q.y(), q.z()); } diff --git a/src/simulator/simulator.hpp b/src/simulator/simulator.hpp index 1076d3aa1..8a1782f52 100644 --- a/src/simulator/simulator.hpp +++ b/src/simulator/simulator.hpp @@ -97,7 +97,7 @@ namespace mrover { auto makeCameraForLink(SimulatorNodelet& simulator, btMultibodyLink const* link) -> Camera; - [[nodiscard]] auto linkInWorld(std::string const& linkName) const -> SE3; + [[nodiscard]] auto linkInWorld(std::string const& linkName) const -> SE3d; }; struct PeriodicTask { @@ -305,7 +305,7 @@ namespace mrover { auto getUrdf(std::string const& name) -> std::optional>; - SE3 mCameraInWorld{R3{-3.0, 0.0, 1.5}, SO3{}}; + SE3d mCameraInWorld{R3{-3.0, 0.0, 1.5}, SO3d::Identity()}; std::vector mStereoCameras; std::vector mCameras; @@ -432,7 +432,7 @@ namespace mrover { auto urdfPoseToBtTransform(urdf::Pose const& pose) -> btTransform; - auto btTransformToSe3(btTransform const& transform) -> SE3; + auto btTransformToSe3(btTransform const& transform) -> SE3d; auto computeCameraToClip(float fovY, float aspect, float zNear, float zFar) -> Eigen::Matrix4f; diff --git a/src/simulator/simulator.physics.cpp b/src/simulator/simulator.physics.cpp index 5f6d82d2e..b8f0c9c3e 100644 --- a/src/simulator/simulator.physics.cpp +++ b/src/simulator/simulator.physics.cpp @@ -2,10 +2,12 @@ namespace mrover { - auto btTransformToSe3(btTransform const& transform) -> SE3 { + auto btTransformToSe3(btTransform const& transform) -> SE3d { btVector3 const& p = transform.getOrigin(); btQuaternion const& q = transform.getRotation(); - return SE3{R3{p.x(), p.y(), p.z()}, SO3{q.w(), q.x(), q.y(), q.z()}}; + // Note: Must convert the Bullet quaternion (floats) to a normalized Eigen quaternion (doubles). + // Otherwise the normality check will fail in the SO3 constructor. + return SE3d{R3{p.x(), p.y(), p.z()}, Eigen::Quaterniond{q.w(), q.x(), q.y(), q.z()}.normalized()}; } auto SimulatorNodelet::initPhysics() -> void { @@ -76,9 +78,9 @@ namespace mrover { int index = urdf.linkNameToMeta.at(link->name).index; // TODO(quintin): figure out why we need to negate rvector btTransform parentToChild{urdf.physics->getParentToLocalRot(index), -urdf.physics->getRVector(index)}; - SE3 childInParent = btTransformToSe3(parentToChild.inverse()); + SE3d childInParent = btTransformToSe3(parentToChild.inverse()); - SE3::pushToTfTree(mTfBroadcaster, link->name, link->getParent()->name, childInParent); + SE3Conversions::pushToTfTree(mTfBroadcaster, link->name, link->getParent()->name, childInParent); } for (urdf::JointSharedPtr const& child_joint: link->child_joints) { @@ -95,17 +97,17 @@ namespace mrover { if (auto modelOpt = getUrdf(modelName)) { URDF const& model = *modelOpt; - SE3 modelInMap = btTransformToSe3(model.physics->getBaseWorldTransform()); - SE3 roverInMap = btTransformToSe3(rover.physics->getBaseWorldTransform()); + SE3d modelInMap = btTransformToSe3(model.physics->getBaseWorldTransform()); + SE3d roverInMap = btTransformToSe3(rover.physics->getBaseWorldTransform()); - R3 roverToModel = modelInMap.position() - roverInMap.position(); + R3 roverToModel = modelInMap.translation() - roverInMap.translation(); double roverDistanceToModel = roverToModel.norm(); roverToModel /= roverDistanceToModel; R3 roverForward = roverInMap.rotation().matrix().col(0); double roverDotModel = roverToModel.dot(roverForward); if (roverDotModel > 0 && roverDistanceToModel < threshold) { - SE3::pushToTfTree(mTfBroadcaster, modelName, "map", modelInMap); + SE3Conversions::pushToTfTree(mTfBroadcaster, modelName, "map", modelInMap); } } }; @@ -115,7 +117,7 @@ namespace mrover { } } - auto URDF::linkInWorld(std::string const& linkName) const -> SE3 { + auto URDF::linkInWorld(std::string const& linkName) const -> SE3d { int index = linkNameToMeta.at(linkName).index; return btTransformToSe3(index == -1 ? physics->getBaseWorldTransform() : physics->getLink(index).m_cachedWorldTransform); } diff --git a/src/simulator/simulator.render.cpp b/src/simulator/simulator.render.cpp index 6c2a3c116..14ed062e4 100644 --- a/src/simulator/simulator.render.cpp +++ b/src/simulator/simulator.render.cpp @@ -409,10 +409,10 @@ namespace mrover { if (auto urdfMesh = std::dynamic_pointer_cast(visual->geometry)) { Model& model = mUriToModel.at(urdfMesh->filename); URDF::LinkMeta& meta = urdf.linkNameToMeta.at(link->name); - SE3 linkInWorld = urdf.linkInWorld(link->name); - SE3 modelInLink = btTransformToSe3(urdfPoseToBtTransform(link->visual->origin)); - SE3 modelInWorld = linkInWorld * modelInLink; - renderModel(pass, model, meta.visualUniforms.at(visualIndex), SIM3{modelInWorld.position(), modelInWorld.rotation(), R3::Ones()}); + SE3d linkInWorld = urdf.linkInWorld(link->name); + SE3d modelInLink = btTransformToSe3(urdfPoseToBtTransform(link->visual->origin)); + SE3d modelInWorld = linkInWorld * modelInLink; + renderModel(pass, model, meta.visualUniforms.at(visualIndex), SIM3{modelInWorld, R3::Ones()}); visualIndex++; } } @@ -439,27 +439,27 @@ namespace mrover { assert(collider); btCollisionShape* shape = collider->getCollisionShape(); assert(shape); - SE3 linkInWorld = urdf.linkInWorld(link->name); + SE3d linkInWorld = urdf.linkInWorld(link->name); if (auto* compound = dynamic_cast(shape)) { for (int i = 0; i < compound->getNumChildShapes(); ++i) { - SE3 modelInLink = btTransformToSe3(urdfPoseToBtTransform(link->collision_array.at(i)->origin)); - SE3 shapeToWorld = modelInLink * linkInWorld; + SE3d modelInLink = btTransformToSe3(urdfPoseToBtTransform(link->collision_array.at(i)->origin)); + SE3d shapeToWorld = modelInLink * linkInWorld; auto* shape = compound->getChildShape(i); if (auto* box = dynamic_cast(shape)) { btVector3 extents = box->getHalfExtentsWithoutMargin() * 2; - SIM3 modelToWorld{shapeToWorld.position(), shapeToWorld.rotation(), R3{extents.x(), extents.y(), extents.z()}}; + SIM3 modelToWorld{shapeToWorld, R3{extents.x(), extents.y(), extents.z()}}; renderModel(pass, mUriToModel.at(CUBE_PRIMITIVE_URI), meta.collisionUniforms.at(i), modelToWorld); } else if (auto* sphere = dynamic_cast(shape)) { btScalar diameter = sphere->getRadius() * 2; - SIM3 modelToWorld{shapeToWorld.position(), shapeToWorld.rotation(), R3{diameter, diameter, diameter}}; + SIM3 modelToWorld{shapeToWorld, R3{diameter, diameter, diameter}}; renderModel(pass, mUriToModel.at(SPHERE_PRIMITIVE_URI), meta.collisionUniforms.at(i), modelToWorld); } else if (auto* cylinder = dynamic_cast(shape)) { btVector3 extents = cylinder->getHalfExtentsWithoutMargin() * 2; - SIM3 modelToWorld{shapeToWorld.position(), shapeToWorld.rotation(), R3{extents.x(), extents.y(), extents.z()}}; + SIM3 modelToWorld{shapeToWorld, R3{extents.x(), extents.y(), extents.z()}}; renderModel(pass, mUriToModel.at(CYLINDER_PRIMITIVE_URI), meta.collisionUniforms.at(i), modelToWorld); } else if (auto* mesh = dynamic_cast(shape)) { - SIM3 modelToWorld{shapeToWorld.position(), shapeToWorld.rotation(), R3::Ones()}; + SIM3 modelToWorld{shapeToWorld, R3::Ones()}; renderModel(pass, mUriToModel.at(mMeshToUri.at(const_cast(mesh))), meta.collisionUniforms.at(i), modelToWorld); } else { NODELET_WARN_STREAM_ONCE(std::format("Tried to render unsupported collision shape: {}", shape->getName())); @@ -694,8 +694,8 @@ namespace mrover { glfwGetFramebufferSize(mWindow.get(), &width, &height); float aspect = static_cast(width) / static_cast(height); mSceneUniforms.value.cameraToClip = computeCameraToClip(mFovDegrees * DEG_TO_RAD, aspect, NEAR, FAR).cast(); - mSceneUniforms.value.worldToCamera = mCameraInWorld.matrix().inverse().cast(); - mSceneUniforms.value.cameraInWorld = mCameraInWorld.position().cast().homogeneous(); + mSceneUniforms.value.worldToCamera = mCameraInWorld.inverse().transform().cast(); + mSceneUniforms.value.cameraInWorld = mCameraInWorld.translation().cast().homogeneous(); mSceneUniforms.enqueueWrite(); wgpu::BindGroupEntry entry; diff --git a/src/simulator/simulator.sensors.cpp b/src/simulator/simulator.sensors.cpp index af4bc60db..798fc9f12 100644 --- a/src/simulator/simulator.sensors.cpp +++ b/src/simulator/simulator.sensors.cpp @@ -11,9 +11,9 @@ namespace mrover { camera.sceneUniforms.value.lightInWorld = {0, 0, 5, 1}; float aspect = static_cast(camera.resolution.x()) / static_cast(camera.resolution.y()); camera.sceneUniforms.value.cameraToClip = computeCameraToClip(camera.fov * DEG_TO_RAD, aspect, NEAR, FAR).cast(); - SE3 cameraInWorld = btTransformToSe3(camera.link->m_cachedWorldTransform); - camera.sceneUniforms.value.worldToCamera = cameraInWorld.matrix().inverse().cast(); - camera.sceneUniforms.value.cameraInWorld = cameraInWorld.position().cast().homogeneous(); + SE3d cameraInWorld = btTransformToSe3(camera.link->m_cachedWorldTransform); + camera.sceneUniforms.value.worldToCamera = cameraInWorld.inverse().transform().cast(); + camera.sceneUniforms.value.cameraInWorld = cameraInWorld.translation().cast().homogeneous(); camera.sceneUniforms.enqueueWrite(); wgpu::BindGroupEntry entry; @@ -111,22 +111,22 @@ namespace mrover { return {lat, lon, alt}; } - auto computeNavSatFix(SE3 const& gpuInMap, Eigen::Vector3d const& referenceGeodetic, double referenceHeadingDegrees) -> sensor_msgs::NavSatFix { + auto computeNavSatFix(SE3d const& gpsInMap, Eigen::Vector3d const& referenceGeodetic, double referenceHeadingDegrees) -> sensor_msgs::NavSatFix { sensor_msgs::NavSatFix gpsMessage; gpsMessage.header.stamp = ros::Time::now(); gpsMessage.header.frame_id = "map"; - auto geodetic = cartesianToGeodetic(gpuInMap.position(), referenceGeodetic, referenceHeadingDegrees); + auto geodetic = cartesianToGeodetic(gpsInMap.translation(), referenceGeodetic, referenceHeadingDegrees); gpsMessage.latitude = geodetic(0); gpsMessage.longitude = geodetic(1); gpsMessage.altitude = geodetic(2); return gpsMessage; } - auto computeImu(SE3 const& imuInMap, R3 const& imuAngularVelocity, R3 const& linearAcceleration, R3 const& magneticField) -> ImuAndMag { + auto computeImu(SE3d const& imuInMap, R3 const& imuAngularVelocity, R3 const& linearAcceleration, R3 const& magneticField) -> ImuAndMag { ImuAndMag imuMessage; imuMessage.header.stamp = ros::Time::now(); imuMessage.header.frame_id = "map"; - S3 q = imuInMap.rotation().quaternion(); + S3 q = imuInMap.quat(); imuMessage.imu.orientation.w = q.w(); imuMessage.imu.orientation.x = q.x(); imuMessage.imu.orientation.y = q.y(); @@ -152,15 +152,15 @@ namespace mrover { URDF const& rover = *lookup; { - SE3 baseLinkInMap = rover.linkInWorld("base_link"); + SE3d baseLinkInMap = rover.linkInWorld("base_link"); nav_msgs::Odometry odometry; odometry.header.stamp = ros::Time::now(); odometry.header.frame_id = "map"; - R3 p = baseLinkInMap.position(); + R3 p = baseLinkInMap.translation(); odometry.pose.pose.position.x = p.x(); odometry.pose.pose.position.y = p.y(); odometry.pose.pose.position.z = p.z(); - S3 q = baseLinkInMap.rotation().quaternion(); + S3 q = baseLinkInMap.quat(); odometry.pose.pose.orientation.w = q.w(); odometry.pose.pose.orientation.x = q.x(); odometry.pose.pose.orientation.y = q.y(); @@ -169,7 +169,7 @@ namespace mrover { mGroundTruthPub.publish(odometry); } if (mGpsTask.shouldUpdate()) { - SE3 gpsInMap = rover.linkInWorld("chassis_link"); + SE3d gpsInMap = rover.linkInWorld("chassis_link"); mGpsPub.publish(computeNavSatFix(gpsInMap, mGpsLinerizationReferencePoint, mGpsLinerizationReferenceHeading)); } if (mImuTask.shouldUpdate()) { @@ -177,8 +177,8 @@ namespace mrover { R3 roverLinearVelocity = btVector3ToR3(rover.physics->getBaseVel()); R3 imuLinearAcceleration = (roverLinearVelocity - mRoverLinearVelocity) / std::chrono::duration_cast>(dt).count(); mRoverLinearVelocity = roverLinearVelocity; - SE3 imuInMap = rover.linkInWorld("imu"); - mImuPub.publish(computeImu(imuInMap, imuAngularVelocity, imuLinearAcceleration, imuInMap.rotation().matrix().transpose().col(1))); + SE3d imuInMap = rover.linkInWorld("imu"); + mImuPub.publish(computeImu(imuInMap, imuAngularVelocity, imuLinearAcceleration, imuInMap.asSO3().inverse().rotation().col(1))); } } } diff --git a/src/util/lie/lie.cpp b/src/util/lie/lie.cpp new file mode 100644 index 000000000..b58beee26 --- /dev/null +++ b/src/util/lie/lie.cpp @@ -0,0 +1,75 @@ +#include "lie.hpp" + +SIM3::SIM3(SE3d const& se3, R3 const& scale) { + mTransform.fromPositionOrientationScale(se3.translation(), se3.rotation(), scale); +} + +auto SIM3::matrix() const -> Eigen::Matrix4d { + return mTransform.matrix(); +} + +auto SIM3::position() const -> R3 { + return mTransform.translation(); +} + +auto SE3Conversions::fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId) -> SE3d { + geometry_msgs::TransformStamped transform = buffer.lookupTransform(fromFrameId, toFrameId, ros::Time{}); + return fromTf(transform.transform); +} + +auto SE3Conversions::pushToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3d const& tf) -> void { + broadcaster.sendTransform(toTransformStamped(tf, parentFrameId, childFrameId)); +} + +auto SE3Conversions::fromTf(geometry_msgs::Transform const& transform) -> SE3d { + return {{transform.translation.x, transform.translation.y, transform.translation.z}, + Eigen::Quaterniond{transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z}}; +} + +auto SE3Conversions::fromPose(geometry_msgs::Pose const& pose) -> SE3d { + return {{pose.position.x, pose.position.y, pose.position.z}, + Eigen::Quaterniond{pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z}}; +} + +auto SE3Conversions::toPose(SE3d const& tf) -> geometry_msgs::Pose { + geometry_msgs::Pose pose; + pose.position.x = tf.x(); + pose.position.y = tf.y(); + pose.position.z = tf.z(); + SE3d::Quaternion const& q = tf.quat(); + pose.orientation.x = q.x(); + pose.orientation.y = q.y(); + pose.orientation.z = q.z(); + pose.orientation.w = q.w(); + return pose; +} + +auto SE3Conversions::toTransform(SE3d const& tf) -> geometry_msgs::Transform { + geometry_msgs::Transform transform; + transform.translation.x = tf.x(); + transform.translation.y = tf.y(); + transform.translation.z = tf.z(); + SE3d::Quaternion const& q = tf.quat(); + transform.rotation.x = q.x(); + transform.rotation.y = q.y(); + transform.rotation.z = q.z(); + transform.rotation.w = q.w(); + return transform; +} + +auto SE3Conversions::toPoseStamped(SE3d const& tf, std::string const& frameId) -> geometry_msgs::PoseStamped { + geometry_msgs::PoseStamped pose; + pose.pose = toPose(tf); + pose.header.frame_id = frameId; + pose.header.stamp = ros::Time::now(); + return pose; +} + +auto SE3Conversions::toTransformStamped(SE3d const& tf, std::string const& parentFrameId, std::string const& childFrameId) -> geometry_msgs::TransformStamped { + geometry_msgs::TransformStamped transform; + transform.transform = toTransform(tf); + transform.header.frame_id = parentFrameId; + transform.header.stamp = ros::Time::now(); + transform.child_frame_id = childFrameId; + return transform; +} diff --git a/src/util/lie/lie.hpp b/src/util/lie/lie.hpp new file mode 100644 index 000000000..1e51e4dab --- /dev/null +++ b/src/util/lie/lie.hpp @@ -0,0 +1,50 @@ +#pragma once + +#include +#include + +#include +#include +#include + +#include +#include + +using manif::SE3d, manif::SO3d; + +using R3 = Eigen::Vector3d; +using S3 = Eigen::Quaterniond; + +class SE3Conversions { +public: + static auto fromTf(geometry_msgs::Transform const& transform) -> SE3d; + + static auto fromPose(geometry_msgs::Pose const& pose) -> SE3d; + + [[nodiscard]] static auto toPose(SE3d const& tf) -> geometry_msgs::Pose; + + [[nodiscard]] static auto toTransform(SE3d const& tf) -> geometry_msgs::Transform; + + [[nodiscard]] static auto toPoseStamped(SE3d const& tf, std::string const& frameId) -> geometry_msgs::PoseStamped; + + [[nodiscard]] static auto toTransformStamped(SE3d const& tf, std::string const& parentFrameId, std::string const& childFrameId) -> geometry_msgs::TransformStamped; + + [[nodiscard]] static auto fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId) -> SE3d; + + static auto pushToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3d const& tf) -> void; +}; + +class SIM3 { + using Transform = Eigen::Transform; + + Transform mTransform = Transform::Identity(); + +public: + SIM3() = default; + + SIM3(SE3d const& se3, R3 const& scale); + + [[nodiscard]] auto matrix() const -> Eigen::Matrix4d; + + [[nodiscard]] auto position() const -> R3; +}; diff --git a/src/util/lie/se3.cpp b/src/util/lie/se3.cpp deleted file mode 100644 index ce4e4593d..000000000 --- a/src/util/lie/se3.cpp +++ /dev/null @@ -1,40 +0,0 @@ -#include "se3.hpp" - -SE3 SE3::fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId) { - geometry_msgs::TransformStamped transform = buffer.lookupTransform(fromFrameId, toFrameId, ros::Time{}); - return fromTf(transform.transform); -} - -void SE3::pushToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3 const& tf) { - broadcaster.sendTransform(tf.toTransformStamped(parentFrameId, childFrameId)); -} - -SE3::SE3(R3 const& position, SO3 const& rotation) { - assert(!position.hasNaN()); - mTransform.translate(position); - mTransform.rotate(rotation.mAngleAxis); -} - -Eigen::Matrix4d SE3::matrix() const { - return mTransform.matrix(); -} - -R3 SE3::position() const { - return mTransform.translation(); -} - -SO3 SE3::rotation() const { - return mTransform.rotation(); -} - -double SE3::distanceTo(SE3 const& other) const { - return (position() - other.position()).norm(); -} - -auto SE3::inverse() const -> SE3 { - return SE3{mTransform.inverse()}; -} - -SE3 SE3::operator*(SE3 const& other) const { - return other.mTransform * mTransform; -} diff --git a/src/util/lie/se3.hpp b/src/util/lie/se3.hpp deleted file mode 100644 index f821d32eb..000000000 --- a/src/util/lie/se3.hpp +++ /dev/null @@ -1,112 +0,0 @@ -#pragma once - -#include -#include -#include - -#include -#include -#include - -#include - -using R3 = Eigen::Vector3d; -using S3 = Eigen::Quaterniond; - -/** - * @brief A 3D rotation. - */ -class SO3 { - using AngleAxis = Eigen::AngleAxis; - - AngleAxis mAngleAxis = AngleAxis::Identity(); - -public: - friend class SE3; - friend class SIM3; - - SO3() = default; - - SO3(double w, double x, double y, double z) : mAngleAxis{Eigen::Quaterniond{w, x, y, z}} {} - - template>, - typename = std::enable_if_t<(sizeof...(Args) > 0)>> - SO3(Args&&... args) : mAngleAxis{std::forward(args)...} {} - - [[nodiscard]] SO3 operator*(SO3 const& other) const; - - [[nodiscard]] R3 operator*(R3 const& other) const; - - [[nodiscard]] Eigen::Matrix3d matrix() const; - - [[nodiscard]] Eigen::Quaterniond quaternion() const; - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -/** - * @brief A 3D rigid transformation (direct isometry). - * - * In simpler terms: a 3D rotation followed by a 3D translation. - */ -class SE3 { - using Transform = Eigen::Transform; - - Transform mTransform = Transform::Identity(); - - static SE3 fromTf(geometry_msgs::Transform const& transform); - - static SE3 fromPose(geometry_msgs::Pose const& pose); - - [[nodiscard]] geometry_msgs::Pose toPose() const; - - [[nodiscard]] geometry_msgs::Transform toTransform() const; - - [[nodiscard]] geometry_msgs::PoseStamped toPoseStamped(std::string const& frameId) const; - - [[nodiscard]] geometry_msgs::TransformStamped toTransformStamped(std::string const& parentFrameId, std::string const& childFrameId) const; - -public: - [[nodiscard]] static SE3 fromTfTree(tf2_ros::Buffer const& buffer, std::string const& fromFrameId, std::string const& toFrameId); - - static void pushToTfTree(tf2_ros::TransformBroadcaster& broadcaster, std::string const& childFrameId, std::string const& parentFrameId, SE3 const& tf); - - SE3() = default; - - SE3(R3 const& position, SO3 const& rotation); - - template>, - typename = std::enable_if_t<(sizeof...(Args) > 0)>> - SE3(Args&&... args) : mTransform{std::forward(args)...} {} - - [[nodiscard]] SE3 operator*(SE3 const& other) const; - - [[nodiscard]] Eigen::Matrix4d matrix() const; - - [[nodiscard]] R3 position() const; - - [[nodiscard]] SO3 rotation() const; - - [[nodiscard]] double distanceTo(SE3 const& other) const; - - [[nodiscard]] auto inverse() const -> SE3; - - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -}; - -class SIM3 { - using Transform = Eigen::Transform; - - Transform mTransform = Transform::Identity(); - -public: - SIM3() = default; - - SIM3(R3 const& position, SO3 const& rotation, R3 const& scale); - - [[nodiscard]] auto matrix() const -> Eigen::Matrix4d; - - [[nodiscard]] auto position() const -> R3; -}; diff --git a/src/util/lie/se3_conversion.cpp b/src/util/lie/se3_conversion.cpp deleted file mode 100644 index 44a28774f..000000000 --- a/src/util/lie/se3_conversion.cpp +++ /dev/null @@ -1,52 +0,0 @@ -#include "se3.hpp" - -SE3 SE3::fromTf(geometry_msgs::Transform const& transform) { - return {{transform.translation.x, transform.translation.y, transform.translation.z}, - {Eigen::Quaterniond{transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z}}}; -} - -SE3 SE3::fromPose(geometry_msgs::Pose const& pose) { - return {{pose.position.x, pose.position.y, pose.position.z}, - {Eigen::Quaterniond{pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z}}}; -} - -geometry_msgs::Pose SE3::toPose() const { - geometry_msgs::Pose pose; - pose.position.x = position().x(); - pose.position.y = position().y(); - pose.position.z = position().z(); - pose.orientation.x = rotation().quaternion().x(); - pose.orientation.y = rotation().quaternion().y(); - pose.orientation.z = rotation().quaternion().z(); - pose.orientation.w = rotation().quaternion().w(); - return pose; -} - -geometry_msgs::Transform SE3::toTransform() const { - geometry_msgs::Transform transform; - transform.translation.x = position().x(); - transform.translation.y = position().y(); - transform.translation.z = position().z(); - transform.rotation.x = rotation().quaternion().x(); - transform.rotation.y = rotation().quaternion().y(); - transform.rotation.z = rotation().quaternion().z(); - transform.rotation.w = rotation().quaternion().w(); - return transform; -} - -geometry_msgs::PoseStamped SE3::toPoseStamped(std::string const& frameId) const { - geometry_msgs::PoseStamped pose; - pose.pose = toPose(); - pose.header.frame_id = frameId; - pose.header.stamp = ros::Time::now(); - return pose; -} - -geometry_msgs::TransformStamped SE3::toTransformStamped(const std::string& parentFrameId, const std::string& childFrameId) const { - geometry_msgs::TransformStamped transform; - transform.transform = toTransform(); - transform.header.frame_id = parentFrameId; - transform.header.stamp = ros::Time::now(); - transform.child_frame_id = childFrameId; - return transform; -} diff --git a/src/util/lie/sim3.cpp b/src/util/lie/sim3.cpp deleted file mode 100644 index 65f9110f9..000000000 --- a/src/util/lie/sim3.cpp +++ /dev/null @@ -1,13 +0,0 @@ -#include "se3.hpp" - -SIM3::SIM3(R3 const& position, SO3 const& rotation, R3 const& scale) { - mTransform.fromPositionOrientationScale(position, rotation.mAngleAxis, scale); -} - -auto SIM3::matrix() const -> Eigen::Matrix4d { - return mTransform.matrix(); -} - -auto SIM3::position() const -> R3 { - return mTransform.translation(); -} diff --git a/src/util/lie/so3.cpp b/src/util/lie/so3.cpp deleted file mode 100644 index 422cd5c42..000000000 --- a/src/util/lie/so3.cpp +++ /dev/null @@ -1,17 +0,0 @@ -#include "se3.hpp" - -Eigen::Quaterniond SO3::quaternion() const { - return Eigen::Quaterniond{mAngleAxis}; -} - -Eigen::Matrix3d SO3::matrix() const { - return mAngleAxis.toRotationMatrix(); -} - -SO3 SO3::operator*(SO3 const& other) const { - return other.quaternion() * quaternion(); -} - -R3 SO3::operator*(R3 const& other) const { - return quaternion() * other; -} From fb09d58d47f13839fd21bf79e1dd2457bc057ddb Mon Sep 17 00:00:00 2001 From: qhdwight Date: Fri, 16 Feb 2024 17:48:23 -0500 Subject: [PATCH 72/81] Fix collider rendering with new se3 --- src/simulator/simulator.render.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/simulator/simulator.render.cpp b/src/simulator/simulator.render.cpp index 14ed062e4..dfc5c6845 100644 --- a/src/simulator/simulator.render.cpp +++ b/src/simulator/simulator.render.cpp @@ -411,8 +411,8 @@ namespace mrover { URDF::LinkMeta& meta = urdf.linkNameToMeta.at(link->name); SE3d linkInWorld = urdf.linkInWorld(link->name); SE3d modelInLink = btTransformToSe3(urdfPoseToBtTransform(link->visual->origin)); - SE3d modelInWorld = linkInWorld * modelInLink; - renderModel(pass, model, meta.visualUniforms.at(visualIndex), SIM3{modelInWorld, R3::Ones()}); + SE3d modelToWorld = linkInWorld * modelInLink; + renderModel(pass, model, meta.visualUniforms.at(visualIndex), SIM3{modelToWorld, R3::Ones()}); visualIndex++; } } @@ -439,27 +439,27 @@ namespace mrover { assert(collider); btCollisionShape* shape = collider->getCollisionShape(); assert(shape); - SE3d linkInWorld = urdf.linkInWorld(link->name); + SE3d linkToWorld = urdf.linkInWorld(link->name); if (auto* compound = dynamic_cast(shape)) { for (int i = 0; i < compound->getNumChildShapes(); ++i) { SE3d modelInLink = btTransformToSe3(urdfPoseToBtTransform(link->collision_array.at(i)->origin)); - SE3d shapeToWorld = modelInLink * linkInWorld; + SE3d modelInWorld = linkToWorld * modelInLink; auto* shape = compound->getChildShape(i); if (auto* box = dynamic_cast(shape)) { btVector3 extents = box->getHalfExtentsWithoutMargin() * 2; - SIM3 modelToWorld{shapeToWorld, R3{extents.x(), extents.y(), extents.z()}}; + SIM3 modelToWorld{modelInWorld, R3{extents.x(), extents.y(), extents.z()}}; renderModel(pass, mUriToModel.at(CUBE_PRIMITIVE_URI), meta.collisionUniforms.at(i), modelToWorld); } else if (auto* sphere = dynamic_cast(shape)) { btScalar diameter = sphere->getRadius() * 2; - SIM3 modelToWorld{shapeToWorld, R3{diameter, diameter, diameter}}; + SIM3 modelToWorld{modelInWorld, R3{diameter, diameter, diameter}}; renderModel(pass, mUriToModel.at(SPHERE_PRIMITIVE_URI), meta.collisionUniforms.at(i), modelToWorld); } else if (auto* cylinder = dynamic_cast(shape)) { btVector3 extents = cylinder->getHalfExtentsWithoutMargin() * 2; - SIM3 modelToWorld{shapeToWorld, R3{extents.x(), extents.y(), extents.z()}}; + SIM3 modelToWorld{modelInWorld, R3{extents.x(), extents.y(), extents.z()}}; renderModel(pass, mUriToModel.at(CYLINDER_PRIMITIVE_URI), meta.collisionUniforms.at(i), modelToWorld); } else if (auto* mesh = dynamic_cast(shape)) { - SIM3 modelToWorld{shapeToWorld, R3::Ones()}; + SIM3 modelToWorld{modelInWorld, R3::Ones()}; renderModel(pass, mUriToModel.at(mMeshToUri.at(const_cast(mesh))), meta.collisionUniforms.at(i), modelToWorld); } else { NODELET_WARN_STREAM_ONCE(std::format("Tried to render unsupported collision shape: {}", shape->getName())); From 8ef4b5dec13f329a26f103645f7ca539a8891f88 Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Fri, 16 Feb 2024 18:10:12 -0500 Subject: [PATCH 73/81] slight refactoring of simulated sensors, added untested noise --- src/simulator/pch.hpp | 1 + src/simulator/simulator.hpp | 4 +++ src/simulator/simulator.sensors.cpp | 43 +++++++++++++++++++++++------ 3 files changed, 39 insertions(+), 9 deletions(-) diff --git a/src/simulator/pch.hpp b/src/simulator/pch.hpp index e6ca84c02..9fdcd1abb 100644 --- a/src/simulator/pch.hpp +++ b/src/simulator/pch.hpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include diff --git a/src/simulator/simulator.hpp b/src/simulator/simulator.hpp index 8a1782f52..917e1115a 100644 --- a/src/simulator/simulator.hpp +++ b/src/simulator/simulator.hpp @@ -225,6 +225,10 @@ namespace mrover { R3 mGpsLinerizationReferencePoint{}; double mGpsLinerizationReferenceHeading{}; + // TODO: make variances configurable + std::default_random_engine mRNG; + std::normal_distribution mGPSDist{0, 0.5}, mAccelDist{0, 0.1}, mGyroDist{0, 0.1}, mMagDist{0, 0.1}, mOrientationDist{0, 0.1}; + PeriodicTask mGpsTask; PeriodicTask mImuTask; diff --git a/src/simulator/simulator.sensors.cpp b/src/simulator/simulator.sensors.cpp index 798fc9f12..831d9b0b8 100644 --- a/src/simulator/simulator.sensors.cpp +++ b/src/simulator/simulator.sensors.cpp @@ -111,18 +111,18 @@ namespace mrover { return {lat, lon, alt}; } - auto computeNavSatFix(SE3d const& gpsInMap, Eigen::Vector3d const& referenceGeodetic, double referenceHeadingDegrees) -> sensor_msgs::NavSatFix { + auto computeNavSatFix(R3 const& gpsInMap, Eigen::Vector3d const& referenceGeodetic, double referenceHeadingDegrees) -> sensor_msgs::NavSatFix { sensor_msgs::NavSatFix gpsMessage; gpsMessage.header.stamp = ros::Time::now(); gpsMessage.header.frame_id = "map"; - auto geodetic = cartesianToGeodetic(gpsInMap.translation(), referenceGeodetic, referenceHeadingDegrees); + auto geodetic = cartesianToGeodetic(gpsInMap, referenceGeodetic, referenceHeadingDegrees); gpsMessage.latitude = geodetic(0); gpsMessage.longitude = geodetic(1); gpsMessage.altitude = geodetic(2); return gpsMessage; } - auto computeImu(SE3d const& imuInMap, R3 const& imuAngularVelocity, R3 const& linearAcceleration, R3 const& magneticField) -> ImuAndMag { + auto computeImu(SO3d const& imuInMap, R3 const& imuAngularVelocity, R3 const& linearAcceleration, R3 const& magneticField) -> ImuAndMag { ImuAndMag imuMessage; imuMessage.header.stamp = ros::Time::now(); imuMessage.header.frame_id = "map"; @@ -165,20 +165,45 @@ namespace mrover { odometry.pose.pose.orientation.x = q.x(); odometry.pose.pose.orientation.y = q.y(); odometry.pose.pose.orientation.z = q.z(); - // TODO(quintin, riley): fill in twist? + R3 v = btVector3ToR3(rover.physics->getBaseVel()); + odometry.twist.twist.linear.x = v.x(); + odometry.twist.twist.linear.y = v.y(); + odometry.twist.twist.linear.z = v.z(); + R3 w = btVector3ToR3(rover.physics->getBaseOmega()); + odometry.twist.twist.angular.x = w.x(); + odometry.twist.twist.angular.y = w.y(); + odometry.twist.twist.angular.z = w.z(); mGroundTruthPub.publish(odometry); } if (mGpsTask.shouldUpdate()) { - SE3d gpsInMap = rover.linkInWorld("chassis_link"); + R3 gpsInMap = rover.linkInWorld("chassis_link").translation(); + R3 gpsNoise; + gpsNoise << mGPSDist(mRNG), mGPSDist(mRNG), mGPSDist(mRNG); + gpsInMap += gpsNoise; + mGpsPub.publish(computeNavSatFix(gpsInMap, mGpsLinerizationReferencePoint, mGpsLinerizationReferenceHeading)); } if (mImuTask.shouldUpdate()) { - R3 imuAngularVelocity = btVector3ToR3(rover.physics->getBaseOmega()); + R3 roverAngularVelocity = btVector3ToR3(rover.physics->getBaseOmega()); R3 roverLinearVelocity = btVector3ToR3(rover.physics->getBaseVel()); - R3 imuLinearAcceleration = (roverLinearVelocity - mRoverLinearVelocity) / std::chrono::duration_cast>(dt).count(); + R3 roverLinearAcceleration = (roverLinearVelocity - mRoverLinearVelocity) / std::chrono::duration_cast>(dt).count(); mRoverLinearVelocity = roverLinearVelocity; - SE3d imuInMap = rover.linkInWorld("imu"); - mImuPub.publish(computeImu(imuInMap, imuAngularVelocity, imuLinearAcceleration, imuInMap.asSO3().inverse().rotation().col(1))); + SO3d imuInMap = rover.linkInWorld("imu").asSO3(); + R3 roverMagVector = imuInMap.inverse().rotation().col(1); + + R3 accelNoise, gyroNoise, magNoise; + accelNoise << mAccelDist(mRNG), mAccelDist(mRNG), mAccelDist(mRNG); + gyroNoise << mGyroDist(mRNG), mGyroDist(mRNG), mGyroDist(mRNG); + magNoise << mMagDist(mRNG), mMagDist(mRNG), mMagDist(mRNG); + roverLinearAcceleration += accelNoise; + roverAngularVelocity += gyroNoise; + roverMagVector += magNoise; + + SO3d::Tangent orientationNoise; + orientationNoise << mOrientationDist(mRNG), mOrientationDist(mRNG), mOrientationDist(mRNG); + imuInMap += orientationNoise; + + mImuPub.publish(computeImu(imuInMap, roverAngularVelocity, roverLinearAcceleration, roverMagVector)); } } } From b22ec362c24e41be60d287a7addae47d29f7cf91 Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Sat, 17 Feb 2024 11:04:38 -0500 Subject: [PATCH 74/81] changed some type names, added rviz to launch file, separate roll pitch yaw noises --- launch/simulator.launch | 6 ++++++ src/simulator/simulator.hpp | 2 +- src/simulator/simulator.sensors.cpp | 18 +++++++++--------- 3 files changed, 16 insertions(+), 10 deletions(-) diff --git a/launch/simulator.launch b/launch/simulator.launch index 78ecfbae8..ffc6a0e9b 100644 --- a/launch/simulator.launch +++ b/launch/simulator.launch @@ -1,4 +1,5 @@ + @@ -13,4 +14,9 @@ args="load mrover/SimulatorNodelet nodelet_manager" output="screen" /> + + + + + \ No newline at end of file diff --git a/src/simulator/simulator.hpp b/src/simulator/simulator.hpp index 917e1115a..127e40fe7 100644 --- a/src/simulator/simulator.hpp +++ b/src/simulator/simulator.hpp @@ -227,7 +227,7 @@ namespace mrover { // TODO: make variances configurable std::default_random_engine mRNG; - std::normal_distribution mGPSDist{0, 0.5}, mAccelDist{0, 0.1}, mGyroDist{0, 0.1}, mMagDist{0, 0.1}, mOrientationDist{0, 0.1}; + std::normal_distribution mGPSDist{0, 0.5}, mAccelDist{0, 0.05}, mGyroDist{0, 0.02}, mMagDist{0, 0.1}, mRollDist{0, 0.05}, mPitchDist{0, 0.05}, mYawDist{0, 0.1}; PeriodicTask mGpsTask; PeriodicTask mImuTask; diff --git a/src/simulator/simulator.sensors.cpp b/src/simulator/simulator.sensors.cpp index 831d9b0b8..4b5594cb5 100644 --- a/src/simulator/simulator.sensors.cpp +++ b/src/simulator/simulator.sensors.cpp @@ -122,7 +122,7 @@ namespace mrover { return gpsMessage; } - auto computeImu(SO3d const& imuInMap, R3 const& imuAngularVelocity, R3 const& linearAcceleration, R3 const& magneticField) -> ImuAndMag { + auto computeImu(SO3d const& imuInMap, Eigen::Vector3d const& imuAngularVelocity, Eigen::Vector3d const& linearAcceleration, Eigen::Vector3d const& magneticField) -> ImuAndMag { ImuAndMag imuMessage; imuMessage.header.stamp = ros::Time::now(); imuMessage.header.frame_id = "map"; @@ -165,11 +165,11 @@ namespace mrover { odometry.pose.pose.orientation.x = q.x(); odometry.pose.pose.orientation.y = q.y(); odometry.pose.pose.orientation.z = q.z(); - R3 v = btVector3ToR3(rover.physics->getBaseVel()); + Eigen::Vector3d v = btVector3ToR3(rover.physics->getBaseVel()); odometry.twist.twist.linear.x = v.x(); odometry.twist.twist.linear.y = v.y(); odometry.twist.twist.linear.z = v.z(); - R3 w = btVector3ToR3(rover.physics->getBaseOmega()); + Eigen::Vector3d w = btVector3ToR3(rover.physics->getBaseOmega()); odometry.twist.twist.angular.x = w.x(); odometry.twist.twist.angular.y = w.y(); odometry.twist.twist.angular.z = w.z(); @@ -184,14 +184,14 @@ namespace mrover { mGpsPub.publish(computeNavSatFix(gpsInMap, mGpsLinerizationReferencePoint, mGpsLinerizationReferenceHeading)); } if (mImuTask.shouldUpdate()) { - R3 roverAngularVelocity = btVector3ToR3(rover.physics->getBaseOmega()); - R3 roverLinearVelocity = btVector3ToR3(rover.physics->getBaseVel()); - R3 roverLinearAcceleration = (roverLinearVelocity - mRoverLinearVelocity) / std::chrono::duration_cast>(dt).count(); + Eigen::Vector3d roverAngularVelocity = btVector3ToR3(rover.physics->getBaseOmega()); + Eigen::Vector3d roverLinearVelocity = btVector3ToR3(rover.physics->getBaseVel()); + Eigen::Vector3d roverLinearAcceleration = (roverLinearVelocity - mRoverLinearVelocity) / std::chrono::duration_cast>(dt).count(); mRoverLinearVelocity = roverLinearVelocity; SO3d imuInMap = rover.linkInWorld("imu").asSO3(); - R3 roverMagVector = imuInMap.inverse().rotation().col(1); + Eigen::Vector3d roverMagVector = imuInMap.inverse().rotation().col(1); - R3 accelNoise, gyroNoise, magNoise; + Eigen::Vector3d accelNoise, gyroNoise, magNoise; accelNoise << mAccelDist(mRNG), mAccelDist(mRNG), mAccelDist(mRNG); gyroNoise << mGyroDist(mRNG), mGyroDist(mRNG), mGyroDist(mRNG); magNoise << mMagDist(mRNG), mMagDist(mRNG), mMagDist(mRNG); @@ -200,7 +200,7 @@ namespace mrover { roverMagVector += magNoise; SO3d::Tangent orientationNoise; - orientationNoise << mOrientationDist(mRNG), mOrientationDist(mRNG), mOrientationDist(mRNG); + orientationNoise << mRollDist(mRNG), mPitchDist(mRNG), mYawDist(mRNG); imuInMap += orientationNoise; mImuPub.publish(computeImu(imuInMap, roverAngularVelocity, roverLinearAcceleration, roverMagVector)); From d172136de41b60290ba5c371112288e77b0ef3c1 Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Sat, 17 Feb 2024 11:56:11 -0500 Subject: [PATCH 75/81] fixed type names and ctors --- src/simulator/simulator.hpp | 8 +++++++- src/simulator/simulator.sensors.cpp | 25 ++++++++++++------------- 2 files changed, 19 insertions(+), 14 deletions(-) diff --git a/src/simulator/simulator.hpp b/src/simulator/simulator.hpp index 127e40fe7..6809cf675 100644 --- a/src/simulator/simulator.hpp +++ b/src/simulator/simulator.hpp @@ -227,7 +227,13 @@ namespace mrover { // TODO: make variances configurable std::default_random_engine mRNG; - std::normal_distribution mGPSDist{0, 0.5}, mAccelDist{0, 0.05}, mGyroDist{0, 0.02}, mMagDist{0, 0.1}, mRollDist{0, 0.05}, mPitchDist{0, 0.05}, mYawDist{0, 0.1}; + std::normal_distribution mGPSDist{0, 0.5}, + mAccelDist{0, 0.05}, + mGyroDist{0, 0.02}, + mMagDist{0, 0.1}, + mRollDist{0, 0.05}, + mPitchDist{0, 0.05}, + mYawDist{0, 0.1}; PeriodicTask mGpsTask; PeriodicTask mImuTask; diff --git a/src/simulator/simulator.sensors.cpp b/src/simulator/simulator.sensors.cpp index 4b5594cb5..ec3acfbdd 100644 --- a/src/simulator/simulator.sensors.cpp +++ b/src/simulator/simulator.sensors.cpp @@ -90,7 +90,7 @@ namespace mrover { computePass.release(); } - auto cartesianToGeodetic(R3 const& cartesian, Eigen::Vector3d const& referenceGeodetic, double referenceHeadingDegrees) -> Eigen::Vector3d { + auto cartesianToGeodetic(R3 const& cartesian, R3 const& referenceGeodetic, double referenceHeadingDegrees) -> R3 { constexpr double equatorialRadius = 6378137.0; constexpr double flattening = 1.0 / 298.257223563; constexpr double eccentricity2 = 2 * flattening - flattening * flattening; @@ -111,7 +111,7 @@ namespace mrover { return {lat, lon, alt}; } - auto computeNavSatFix(R3 const& gpsInMap, Eigen::Vector3d const& referenceGeodetic, double referenceHeadingDegrees) -> sensor_msgs::NavSatFix { + auto computeNavSatFix(R3 const& gpsInMap, R3 const& referenceGeodetic, double referenceHeadingDegrees) -> sensor_msgs::NavSatFix { sensor_msgs::NavSatFix gpsMessage; gpsMessage.header.stamp = ros::Time::now(); gpsMessage.header.frame_id = "map"; @@ -122,7 +122,7 @@ namespace mrover { return gpsMessage; } - auto computeImu(SO3d const& imuInMap, Eigen::Vector3d const& imuAngularVelocity, Eigen::Vector3d const& linearAcceleration, Eigen::Vector3d const& magneticField) -> ImuAndMag { + auto computeImu(SO3d const& imuInMap, R3 const& imuAngularVelocity, R3 const& linearAcceleration, R3 const& magneticField) -> ImuAndMag { ImuAndMag imuMessage; imuMessage.header.stamp = ros::Time::now(); imuMessage.header.frame_id = "map"; @@ -165,11 +165,11 @@ namespace mrover { odometry.pose.pose.orientation.x = q.x(); odometry.pose.pose.orientation.y = q.y(); odometry.pose.pose.orientation.z = q.z(); - Eigen::Vector3d v = btVector3ToR3(rover.physics->getBaseVel()); + R3 v = btVector3ToR3(rover.physics->getBaseVel()); odometry.twist.twist.linear.x = v.x(); odometry.twist.twist.linear.y = v.y(); odometry.twist.twist.linear.z = v.z(); - Eigen::Vector3d w = btVector3ToR3(rover.physics->getBaseOmega()); + R3 w = btVector3ToR3(rover.physics->getBaseOmega()); odometry.twist.twist.angular.x = w.x(); odometry.twist.twist.angular.y = w.y(); odometry.twist.twist.angular.z = w.z(); @@ -184,17 +184,16 @@ namespace mrover { mGpsPub.publish(computeNavSatFix(gpsInMap, mGpsLinerizationReferencePoint, mGpsLinerizationReferenceHeading)); } if (mImuTask.shouldUpdate()) { - Eigen::Vector3d roverAngularVelocity = btVector3ToR3(rover.physics->getBaseOmega()); - Eigen::Vector3d roverLinearVelocity = btVector3ToR3(rover.physics->getBaseVel()); - Eigen::Vector3d roverLinearAcceleration = (roverLinearVelocity - mRoverLinearVelocity) / std::chrono::duration_cast>(dt).count(); + R3 roverAngularVelocity = btVector3ToR3(rover.physics->getBaseOmega()); + R3 roverLinearVelocity = btVector3ToR3(rover.physics->getBaseVel()); + R3 roverLinearAcceleration = (roverLinearVelocity - mRoverLinearVelocity) / std::chrono::duration_cast>(dt).count(); mRoverLinearVelocity = roverLinearVelocity; SO3d imuInMap = rover.linkInWorld("imu").asSO3(); - Eigen::Vector3d roverMagVector = imuInMap.inverse().rotation().col(1); + R3 roverMagVector = imuInMap.inverse().rotation().col(1); - Eigen::Vector3d accelNoise, gyroNoise, magNoise; - accelNoise << mAccelDist(mRNG), mAccelDist(mRNG), mAccelDist(mRNG); - gyroNoise << mGyroDist(mRNG), mGyroDist(mRNG), mGyroDist(mRNG); - magNoise << mMagDist(mRNG), mMagDist(mRNG), mMagDist(mRNG); + R3 accelNoise{mAccelDist(mRNG), mAccelDist(mRNG), mAccelDist(mRNG)}, + gyroNoise{mGyroDist(mRNG), mGyroDist(mRNG), mGyroDist(mRNG)}, + magNoise{mMagDist(mRNG), mMagDist(mRNG), mMagDist(mRNG)}; roverLinearAcceleration += accelNoise; roverAngularVelocity += gyroNoise; roverMagVector += magNoise; From 27eaa1c4a2d2fdca8fdfefa46082003240d396c0 Mon Sep 17 00:00:00 2001 From: Riley Bridges Date: Sat, 17 Feb 2024 12:12:57 -0500 Subject: [PATCH 76/81] slightly reduced noise and moved waypoint --- config/simulator/simulator.yaml | 2 +- src/simulator/simulator.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/config/simulator/simulator.yaml b/config/simulator/simulator.yaml index acd6866b2..94fa962fc 100644 --- a/config/simulator/simulator.yaml +++ b/config/simulator/simulator.yaml @@ -9,7 +9,7 @@ objects: - type: urdf name: tag_0 uri: package://mrover/urdf/world/tag_0.urdf.xacro - translation: [ -3, -3, 0.7 ] + translation: [ -2, -2, 0.7 ] - type: urdf name: tag_1 uri: package://mrover/urdf/world/tag_1.urdf.xacro diff --git a/src/simulator/simulator.hpp b/src/simulator/simulator.hpp index 6809cf675..e37a5aae3 100644 --- a/src/simulator/simulator.hpp +++ b/src/simulator/simulator.hpp @@ -227,7 +227,7 @@ namespace mrover { // TODO: make variances configurable std::default_random_engine mRNG; - std::normal_distribution mGPSDist{0, 0.5}, + std::normal_distribution mGPSDist{0, 0.2}, mAccelDist{0, 0.05}, mGyroDist{0, 0.02}, mMagDist{0, 0.1}, From 91cd8ed768771b221dc74f47460e79c32ca7457e Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sun, 18 Feb 2024 12:45:58 -0500 Subject: [PATCH 77/81] Restream --- .../hardware_h265_encoder.cpp | 46 ++++++++++--------- src/esw/streaming/streaming.cpp | 13 ++++-- 2 files changed, 32 insertions(+), 27 deletions(-) diff --git a/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp b/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp index c3e476620..1cb038c7e 100644 --- a/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp +++ b/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp @@ -20,6 +20,8 @@ auto imageCallback(sensor_msgs::ImageConstPtr const& msg) -> void { try { if (msg->encoding != sensor_msgs::image_encodings::BGRA8) throw std::runtime_error{"Unsupported encoding"}; + if (!encoder) encoder.emplace(cv::Size{static_cast(msg->width), static_cast(msg->height)}); + cv::Size size{static_cast(msg->width), static_cast(msg->height)}; cv::Mat bgraFrame{size, CV_8UC4, const_cast(msg->data.data()), msg->step}; @@ -34,35 +36,35 @@ auto imageCallback(sensor_msgs::ImageConstPtr const& msg) -> void { } } -auto capture() -> void { - cv::VideoCapture cap{std::format("v4l2src ! videoconvert ! video/x-raw,width={},height={},format=I420,framerate=30/1 ! appsink", 640, 480), cv::CAP_GSTREAMER}; - if (!cap.isOpened()) throw std::runtime_error{"Failed to open capture"}; - - while (cap.isOpened()) { - cv::Mat i420Frame; - if (!cap.read(i420Frame)) throw std::runtime_error{"Failed to read frame"}; - - cv::Mat bgraFrame; - cvtColor(i420Frame, bgraFrame, cv::COLOR_YUV2BGRA_I420); - - Encoder::BitstreamView view = encoder->feed(bgraFrame); - std::span span{static_cast(view.lockParams.bitstreamBufferPtr), view.lockParams.bitstreamSizeInBytes}; - streamServer->feed(span); - - ros::spinOnce(); - } -} +// auto capture() -> void { +// cv::VideoCapture cap{std::format("v4l2src ! videoconvert ! video/x-raw,width={},height={},format=I420,framerate=30/1 ! appsink", 640, 480), cv::CAP_GSTREAMER}; +// if (!cap.isOpened()) throw std::runtime_error{"Failed to open capture"}; +// +// while (cap.isOpened()) { +// cv::Mat i420Frame; +// if (!cap.read(i420Frame)) throw std::runtime_error{"Failed to read frame"}; +// +// cv::Mat bgraFrame; +// cvtColor(i420Frame, bgraFrame, cv::COLOR_YUV2BGRA_I420); +// +// Encoder::BitstreamView view = encoder->feed(bgraFrame); +// std::span span{static_cast(view.lockParams.bitstreamBufferPtr), view.lockParams.bitstreamSizeInBytes}; +// streamServer->feed(span); +// +// ros::spinOnce(); +// } +// } auto main(int argc, char** argv) -> int { try { ros::init(argc, argv, "software_h265_encoder"); - ros::NodeHandle nh; + ros::NodeHandle nh, pnh{"~"}; - streamServer.emplace("0.0.0.0", 8080); + std::string imageTopic = pnh.param("image_topic", std::string{"/camera/left/image"}); - encoder.emplace(cv::Size{640, 480}); + streamServer.emplace("0.0.0.0", 8080); - ros::Subscriber imageSubscriber = nh.subscribe("/camera/left/image", 1, imageCallback); + ros::Subscriber imageSubscriber = nh.subscribe(imageTopic, 1, imageCallback); // capture(); diff --git a/src/esw/streaming/streaming.cpp b/src/esw/streaming/streaming.cpp index c3bc815b7..d5dc83b27 100644 --- a/src/esw/streaming/streaming.cpp +++ b/src/esw/streaming/streaming.cpp @@ -4,9 +4,10 @@ #include #include -#include #include +#include + StreamServer::StreamServer(std::string_view host, std::uint16_t port) : m_acceptor{m_context} { @@ -14,7 +15,7 @@ StreamServer::StreamServer(std::string_view host, std::uint16_t port) beast::error_code ec; tcp::endpoint endpoint{net::ip::make_address(host), port}; - std::cout << "Starting H.265 streaming server @" << endpoint << std::endl; + ROS_INFO_STREAM("Starting H.265 streaming server @" << endpoint); m_acceptor.open(endpoint.protocol(), ec); if (ec) throw std::runtime_error{std::format("Failed to open acceptor: {}", ec.message())}; @@ -35,12 +36,14 @@ StreamServer::StreamServer(std::string_view host, std::uint16_t port) m_client->set_option(websocket::stream_base::decorator([](websocket::response_type& res) { res.set(http::field::server, std::format("{} {}", BOOST_BEAST_VERSION_STRING, "mrover-stream-server")); })); - m_client->accept(); - - std::cout << "Client connected" << std::endl; } void StreamServer::feed(std::span data) { + if (!m_client->is_open()) { + m_client->accept(); + ROS_INFO_STREAM("Client connected"); + } + net::mutable_buffer buffer{data.data(), data.size()}; m_client->write(buffer); } From e8c397bd059fa87a272298e6991c0d1ada16fa6e Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sun, 18 Feb 2024 13:02:26 -0500 Subject: [PATCH 78/81] Update --- CMakeLists.txt | 4 +- src/esw/cameras/cameras.cpp | 99 ------------------------------------- 2 files changed, 2 insertions(+), 101 deletions(-) delete mode 100644 src/esw/cameras/cameras.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index acbe9f296..e67a88670 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -221,7 +221,7 @@ if (ZED_FOUND) mrover_add_nodelet(zed src/perception/zed_wrapper/*.c* src/perception/zed_wrapper src/perception/zed_wrapper/pch.hpp) mrover_nodelet_include_directories(zed ${ZED_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS}) - mrover_nodelet_link_libraries(zed ${ZED_LIBRARIES} ${SPECIAL_OS_LIBS} lie manif) + mrover_nodelet_link_libraries(zed ${ZED_LIBRARIES} ${SPECIAL_OS_LIBS} lie) mrover_nodelet_defines(zed ALLOW_BUILD_DEBUG # Ignore ZED warnings about Debug mode __CUDA_INCLUDE_COMPILER_INTERNAL_HEADERS__ # Eigen includes some files it should not, ignore @@ -237,7 +237,7 @@ mrover_add_node(arm_controller src/teleoperation/arm_controller/*.cpp) if (MROVER_BUILD_SIM) mrover_add_nodelet(simulator src/simulator/*.cpp src/simulator src/simulator/pch.hpp) mrover_nodelet_include_directories(simulator ${ASSIMP_INCLUDE_DIRS} ${BULLET_INCLUDE_DIRS} ${OPENCV_INCLUDE_DIRS} src/simulator/imgui) - mrover_nodelet_link_libraries(simulator ${ASSIMP_LIBRARIES} ${BULLET_LIBRARIES} glfw3webgpu webgpu glfw opencv_core opencv_imgcodecs opencv_imgproc lie manif) + mrover_nodelet_link_libraries(simulator ${ASSIMP_LIBRARIES} ${BULLET_LIBRARIES} glfw3webgpu webgpu glfw opencv_core opencv_imgcodecs opencv_imgproc lie) mrover_nodelet_defines(simulator BOOST_THREAD_PROVIDES_FUTURE) endif () diff --git a/src/esw/cameras/cameras.cpp b/src/esw/cameras/cameras.cpp deleted file mode 100644 index 0836ba162..000000000 --- a/src/esw/cameras/cameras.cpp +++ /dev/null @@ -1,99 +0,0 @@ -#include -#include - -#include -#include -#include - -#include -#include -#include - -//#include - -int main() { - StreamServer streamServer{"0.0.0.0", 8080}; - - cv::Size size{1280, 720}; - - NvEncoder nvEncoder{size}; - - cv::VideoCapture cap{std::format("v4l2src ! videoconvert ! video/x-raw,width={},height={},format=I420,framerate=10/1 ! appsink", size.width, size.height), cv::CAP_GSTREAMER}; - - // cv::VideoCapture cap{std::format("videotestsrc ! video/x-raw,width={},height={},format=I420,framerate=30/1 ! appsink", size.width, size.height), cv::CAP_GSTREAMER}; - - if (!cap.isOpened()) throw std::runtime_error{"Failed to open capture"}; - - size_t totalSent = 0; - size_t totalFrames = 0; - auto now = std::chrono::high_resolution_clock::now(); - - cv::Mat frameI420; - while (streamServer.m_client->is_open() && cap.read(frameI420)) { - assert(frameI420.type() == CV_8UC1); - - cv::imshow("USB", frameI420); - cv::waitKey(1); - - NvEncoder::BitstreamView view = nvEncoder.feed(frameI420); - std::span span{static_cast(view.lockParams.bitstreamBufferPtr), view.lockParams.bitstreamSizeInBytes}; - streamServer.feed(span); - - totalSent += span.size_bytes(); - totalFrames++; - double MB = totalSent / 1024.0 / 1024.0; - auto elapsed = std::chrono::duration(std::chrono::high_resolution_clock::now() - now); - double MBps = MB / elapsed.count(); - double fps = totalFrames / elapsed.count(); - - std::cout << std::format("MBps: {} FPS: {}\n", MBps, fps); - } - - // static de265_decoder_context* decoder = de265_new_decoder(); - // if (!decoder) return EXIT_FAILURE; - // if (de265_error error = de265_start_worker_threads(decoder, 0); error != DE265_OK) throw std::runtime_error{"Errored starting worker threads"}; - // - // int more = 1; - // while (more) { - // de265_error status = de265_decode(decoder, &more); - // - // if (status == DE265_ERROR_WAITING_FOR_INPUT_DATA) { - // cv::Mat frame; - // cap.read(frame); - // assert(frame.type() == CV_8UC1); - // - // cv::imshow("F", frame); - // cv::waitKey(0); - // - // NvEncoder::BitstreamView view = nvEncoder.feed(frame); - // if (de265_error error = de265_push_data(decoder, view.lockParams.bitstreamBufferPtr, view.lockParams.bitstreamSizeInBytes, clock(), nullptr); error != DE265_OK) { - // throw std::runtime_error{"Errored pushing encoder data"}; - // } - // } - // - // const de265_image* image; - // do { - // image = de265_peek_next_picture(decoder); - // if (image) { - // int width = de265_get_image_width(image, 0); - // int height = de265_get_image_height(image, 0); - // int format = de265_get_chroma_format(image); - // int bpp = de265_get_bits_per_pixel(image, 0); - // - // if (cv::Size{width, height} != size) throw std::runtime_error{"Unexpected image size"}; - // if (format != de265_chroma_420) throw std::runtime_error{"Unsupported chroma format"}; - // if (bpp != 8) throw std::runtime_error{"Unsupported bits per pixel"}; - // - // int ystride; - // auto* y = de265_get_image_plane(image, 0, &ystride); - // cv::Mat Y{height, width, CV_8UC1, (void*) y, static_cast(ystride)}; - // cv::imshow("Y", Y); - // cv::waitKey(1); - // - // de265_release_next_picture(decoder); - // } - // } while (image); - // } - - return EXIT_SUCCESS; -} \ No newline at end of file From e8f68f6dfc6da63124291e5bdc68d12c80358e66 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sun, 18 Feb 2024 13:07:04 -0500 Subject: [PATCH 79/81] Fix format --- .../hardware_h265_encoder/hardware_h265_encoder.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp b/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp index 1cb038c7e..3e8a7e7d6 100644 --- a/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp +++ b/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp @@ -18,13 +18,15 @@ std::optional encoder; auto imageCallback(sensor_msgs::ImageConstPtr const& msg) -> void { try { - if (msg->encoding != sensor_msgs::image_encodings::BGRA8) throw std::runtime_error{"Unsupported encoding"}; - - if (!encoder) encoder.emplace(cv::Size{static_cast(msg->width), static_cast(msg->height)}); + if (msg->encoding != sensor_msgs::image_encodings::BGR8) throw std::runtime_error{"Unsupported encoding"}; cv::Size size{static_cast(msg->width), static_cast(msg->height)}; - cv::Mat bgraFrame{size, CV_8UC4, const_cast(msg->data.data()), msg->step}; + if (!encoder) encoder.emplace(size); + + cv::Mat bgrFrame{size, CV_8UC3, const_cast(msg->data.data()), msg->step}; + cv::Mat bgraFrame; + cv::cvtColor(bgrFrame, bgraFrame, cv::COLOR_BGR2BGRA); Encoder::BitstreamView view = encoder->feed(bgraFrame); std::span span{static_cast(view.lockParams.bitstreamBufferPtr), view.lockParams.bitstreamSizeInBytes}; From 8ded24836497743c1646a477c281697bb77a0cf1 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sun, 18 Feb 2024 14:08:29 -0500 Subject: [PATCH 80/81] Stream that can be reused --- .../hardware_h265_encoder.cpp | 10 +++- .../hardware_h265_encoder.cu | 53 +++++++++-------- .../hardware_h265_encoder.hpp | 1 - src/esw/streaming/streaming.cpp | 38 ++++++++----- src/esw/streaming/streaming.hpp | 2 +- .../long_range_cam/long_range_cam.cpp | 57 +++++++++++-------- 6 files changed, 95 insertions(+), 66 deletions(-) diff --git a/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp b/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp index 3e8a7e7d6..590ab7efe 100644 --- a/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp +++ b/src/esw/hardware_h265_encoder/hardware_h265_encoder.cpp @@ -28,9 +28,13 @@ auto imageCallback(sensor_msgs::ImageConstPtr const& msg) -> void { cv::Mat bgraFrame; cv::cvtColor(bgrFrame, bgraFrame, cv::COLOR_BGR2BGRA); - Encoder::BitstreamView view = encoder->feed(bgraFrame); - std::span span{static_cast(view.lockParams.bitstreamBufferPtr), view.lockParams.bitstreamSizeInBytes}; - streamServer->feed(span); + bool feedSuccessful = false; + { + Encoder::BitstreamView view = encoder->feed(bgraFrame); + std::span span{static_cast(view.lockParams.bitstreamBufferPtr), view.lockParams.bitstreamSizeInBytes}; + feedSuccessful = streamServer->feed(span); + } + if (!feedSuccessful) encoder.reset(); } catch (std::exception const& e) { ROS_ERROR_STREAM(std::format("Exception encoding frame: {}", e.what())); diff --git a/src/esw/hardware_h265_encoder/hardware_h265_encoder.cu b/src/esw/hardware_h265_encoder/hardware_h265_encoder.cu index 426026e0c..660a92b10 100644 --- a/src/esw/hardware_h265_encoder/hardware_h265_encoder.cu +++ b/src/esw/hardware_h265_encoder/hardware_h265_encoder.cu @@ -7,6 +7,8 @@ #include #include +#include + #include #include @@ -60,34 +62,39 @@ std::unordered_map GUID_TO_NAME{ {NV_ENC_CODEC_AV1_GUID, "AV1"}, }; +NV_ENCODE_API_FUNCTION_LIST NVENV_API{.version = NV_ENCODE_API_FUNCTION_LIST_VER}; +CUcontext CUDA_CONTEXT = nullptr; + Encoder::Encoder(cv::Size const& size) : m_size{size} { - cudaCheck(cudaSetDevice(0)); - CUcontext context; - cuCheck(cuCtxGetCurrent(&context)); + if (!CUDA_CONTEXT) { + cudaCheck(cudaSetDevice(0)); + cuCheck(cuCtxGetCurrent(&CUDA_CONTEXT)); + + NvCheck(NvEncodeAPICreateInstance(&NVENV_API)); + } - NvCheck(NvEncodeAPICreateInstance(&m_nvenc)); NV_ENC_OPEN_ENCODE_SESSION_EX_PARAMS params{ .version = NV_ENC_OPEN_ENCODE_SESSION_EX_PARAMS_VER, .deviceType = NV_ENC_DEVICE_TYPE_CUDA, - .device = context, + .device = CUDA_CONTEXT, .apiVersion = NVENCAPI_VERSION, }; - NvCheck(m_nvenc.nvEncOpenEncodeSessionEx(¶ms, &m_encoder)); + NvCheck(NVENV_API.nvEncOpenEncodeSessionEx(¶ms, &m_encoder)); if (!m_encoder) { throw std::runtime_error("No encoder"); } std::uint32_t guidCount; - NvCheck(m_nvenc.nvEncGetEncodeGUIDCount(m_encoder, &guidCount)); + NvCheck(NVENV_API.nvEncGetEncodeGUIDCount(m_encoder, &guidCount)); if (guidCount == 0) { throw std::runtime_error("No GUIDs"); } std::vector guids(guidCount); - NvCheck(m_nvenc.nvEncGetEncodeGUIDs(m_encoder, guids.data(), guidCount, &guidCount)); - std::cout << "Supported encoders:" << std::endl; + NvCheck(NVENV_API.nvEncGetEncodeGUIDs(m_encoder, guids.data(), guidCount, &guidCount)); + ROS_INFO("Supported encoders:"); for (GUID const& guid: guids) { - std::cout << "\t" << GUID_TO_NAME[guid] << std::endl; + ROS_INFO_STREAM("\t" << GUID_TO_NAME[guid]); } GUID desiredEncodeGuid = NV_ENC_CODEC_HEVC_GUID; @@ -100,9 +107,9 @@ Encoder::Encoder(cv::Size const& size) : m_size{size} { } std::uint32_t presetCount; - NvCheck(m_nvenc.nvEncGetEncodePresetCount(m_encoder, desiredEncodeGuid, &presetCount)); + NvCheck(NVENV_API.nvEncGetEncodePresetCount(m_encoder, desiredEncodeGuid, &presetCount)); std::vector presetGuids(presetCount); - NvCheck(m_nvenc.nvEncGetEncodePresetGUIDs(m_encoder, desiredEncodeGuid, presetGuids.data(), presetCount, &presetCount)); + NvCheck(NVENV_API.nvEncGetEncodePresetGUIDs(m_encoder, desiredEncodeGuid, presetGuids.data(), presetCount, &presetCount)); if (std::none_of(presetGuids.begin(), presetGuids.end(), [&](const GUID& guid) { return std::equal_to{}(guid, desiredPresetGuid); })) { @@ -116,7 +123,7 @@ Encoder::Encoder(cv::Size const& size) : m_size{size} { .version = NV_ENC_CONFIG_VER, }, }; - NvCheck(m_nvenc.nvEncGetEncodePresetConfigEx(m_encoder, desiredEncodeGuid, desiredPresetGuid, tuningInfo, &presetConfig)); + NvCheck(NVENV_API.nvEncGetEncodePresetConfigEx(m_encoder, desiredEncodeGuid, desiredPresetGuid, tuningInfo, &presetConfig)); NV_ENC_INITIALIZE_PARAMS initEncParams{ .version = NV_ENC_INITIALIZE_PARAMS_VER, @@ -130,7 +137,7 @@ Encoder::Encoder(cv::Size const& size) : m_size{size} { .encodeConfig = &presetConfig.presetCfg, .tuningInfo = tuningInfo, }; - NvCheck(m_nvenc.nvEncInitializeEncoder(m_encoder, &initEncParams)); + NvCheck(NVENV_API.nvEncInitializeEncoder(m_encoder, &initEncParams)); NV_ENC_CREATE_INPUT_BUFFER createInputBufferParams{ .version = NV_ENC_CREATE_INPUT_BUFFER_VER, @@ -138,13 +145,13 @@ Encoder::Encoder(cv::Size const& size) : m_size{size} { .height = static_cast(m_size.height), .bufferFmt = NV_ENC_BUFFER_FORMAT_ARGB, }; - NvCheck(m_nvenc.nvEncCreateInputBuffer(m_encoder, &createInputBufferParams)); + NvCheck(NVENV_API.nvEncCreateInputBuffer(m_encoder, &createInputBufferParams)); m_input = createInputBufferParams.inputBuffer; NV_ENC_CREATE_BITSTREAM_BUFFER createBitstreamBufferParams{ .version = NV_ENC_CREATE_BITSTREAM_BUFFER_VER, }; - NvCheck(m_nvenc.nvEncCreateBitstreamBuffer(m_encoder, &createBitstreamBufferParams)); + NvCheck(NVENV_API.nvEncCreateBitstreamBuffer(m_encoder, &createBitstreamBufferParams)); m_output = createBitstreamBufferParams.bitstreamBuffer; } @@ -157,12 +164,12 @@ auto Encoder::feed(cv::InputArray frameBgra) -> BitstreamView { .version = NV_ENC_LOCK_INPUT_BUFFER_VER, .inputBuffer = m_input, }; - NvCheck(m_nvenc.nvEncLockInputBuffer(m_encoder, &lockInputBufferParams)); + NvCheck(NVENV_API.nvEncLockInputBuffer(m_encoder, &lockInputBufferParams)); for (int r = 0; r < frameBgra.rows(); ++r) { auto* row = static_cast(lockInputBufferParams.bufferDataPtr) + r * lockInputBufferParams.pitch; std::memcpy(row, frameBgra.getMat().ptr(r), frameBgra.cols() * 4); } - NvCheck(m_nvenc.nvEncUnlockInputBuffer(m_encoder, m_input)); + NvCheck(NVENV_API.nvEncUnlockInputBuffer(m_encoder, m_input)); NV_ENC_PIC_PARAMS picParams{ .version = NV_ENC_PIC_PARAMS_VER, @@ -177,9 +184,9 @@ auto Encoder::feed(cv::InputArray frameBgra) -> BitstreamView { .pictureStruct = NV_ENC_PIC_STRUCT_FRAME, }; - NvCheck(m_nvenc.nvEncEncodePicture(m_encoder, &picParams)); + NvCheck(NVENV_API.nvEncEncodePicture(m_encoder, &picParams)); - return {&m_nvenc, m_encoder, m_output}; + return {&NVENV_API, m_encoder, m_output}; } Encoder::BitstreamView::BitstreamView(NV_ENCODE_API_FUNCTION_LIST* nvenc, void* encoder, NV_ENC_OUTPUT_PTR output) @@ -206,7 +213,7 @@ auto Encoder::BitstreamView::operator=(BitstreamView&& other) noexcept -> Bitstr } Encoder::~Encoder() { - NvCheck(m_nvenc.nvEncDestroyInputBuffer(m_encoder, m_input)); - NvCheck(m_nvenc.nvEncDestroyBitstreamBuffer(m_encoder, m_output)); - NvCheck(m_nvenc.nvEncDestroyEncoder(m_encoder)); + NvCheck(NVENV_API.nvEncDestroyInputBuffer(m_encoder, m_input)); + NvCheck(NVENV_API.nvEncDestroyBitstreamBuffer(m_encoder, m_output)); + NvCheck(NVENV_API.nvEncDestroyEncoder(m_encoder)); } diff --git a/src/esw/hardware_h265_encoder/hardware_h265_encoder.hpp b/src/esw/hardware_h265_encoder/hardware_h265_encoder.hpp index 6ffc3ab0d..394ec7a14 100644 --- a/src/esw/hardware_h265_encoder/hardware_h265_encoder.hpp +++ b/src/esw/hardware_h265_encoder/hardware_h265_encoder.hpp @@ -26,7 +26,6 @@ class Encoder { private: cv::Size m_size; - NV_ENCODE_API_FUNCTION_LIST m_nvenc{.version = NV_ENCODE_API_FUNCTION_LIST_VER}; void* m_encoder = nullptr; NV_ENC_INPUT_PTR m_input = nullptr; NV_ENC_OUTPUT_PTR m_output = nullptr; diff --git a/src/esw/streaming/streaming.cpp b/src/esw/streaming/streaming.cpp index d5dc83b27..e5af3cd07 100644 --- a/src/esw/streaming/streaming.cpp +++ b/src/esw/streaming/streaming.cpp @@ -3,15 +3,15 @@ #include #include -#include #include #include +using namespace std::chrono_literals; + StreamServer::StreamServer(std::string_view host, std::uint16_t port) : m_acceptor{m_context} { - tcp::socket socket{m_context}; beast::error_code ec; tcp::endpoint endpoint{net::ip::make_address(host), port}; @@ -28,22 +28,32 @@ StreamServer::StreamServer(std::string_view host, std::uint16_t port) m_acceptor.listen(net::socket_base::max_listen_connections, ec); if (ec) throw std::runtime_error{std::format("Failed to listen: {}", ec.message())}; - - m_client.emplace(m_acceptor.accept()); - - m_client->binary(true); - m_client->set_option(websocket::stream_base::timeout::suggested(beast::role_type::server)); - m_client->set_option(websocket::stream_base::decorator([](websocket::response_type& res) { - res.set(http::field::server, std::format("{} {}", BOOST_BEAST_VERSION_STRING, "mrover-stream-server")); - })); } -void StreamServer::feed(std::span data) { - if (!m_client->is_open()) { +auto StreamServer::feed(std::span data) -> bool { + if (!m_client) { + m_client.emplace(m_acceptor.accept()); + m_client->binary(true); + m_client->set_option(websocket::stream_base::timeout{ + .handshake_timeout = 3s, + .idle_timeout = 3s, + .keep_alive_pings = true, + }); + m_client->set_option(websocket::stream_base::decorator([](websocket::response_type& res) { + res.set(http::field::server, std::format("{} {}", BOOST_BEAST_VERSION_STRING, "mrover-stream-server")); + })); + ROS_INFO("Waiting for client.."); m_client->accept(); - ROS_INFO_STREAM("Client connected"); + ROS_INFO("Client connected"); } net::mutable_buffer buffer{data.data(), data.size()}; - m_client->write(buffer); + try { + m_client->write(buffer); + return true; + } catch (std::exception const& e) { + ROS_WARN_STREAM(std::format("Exception writing to client: {}", e.what())); + m_client.reset(); + return false; + } } diff --git a/src/esw/streaming/streaming.hpp b/src/esw/streaming/streaming.hpp index e820b5602..e2437e6ae 100644 --- a/src/esw/streaming/streaming.hpp +++ b/src/esw/streaming/streaming.hpp @@ -25,5 +25,5 @@ class StreamServer { public: StreamServer(std::string_view host, std::uint16_t port); - void feed(std::span data); + auto feed(std::span data) -> bool; }; diff --git a/src/perception/long_range_cam/long_range_cam.cpp b/src/perception/long_range_cam/long_range_cam.cpp index 83b24c97d..c63fc60a5 100644 --- a/src/perception/long_range_cam/long_range_cam.cpp +++ b/src/perception/long_range_cam/long_range_cam.cpp @@ -28,33 +28,42 @@ namespace mrover { } auto LongRangeCamNodelet::grabUpdate() -> void { - // See: http://wiki.ros.org/roscpp/Overview/NodeHandles for public vs. private node handle - // MT means multithreaded - mNh = getMTNodeHandle(); - mPnh = getMTPrivateNodeHandle(); - mCamInfoPub = mNh.advertise("long_range_cam/camera_info", 1); - mImgPub = mNh.advertise("long_range_image", 1); - auto width = mPnh.param("width", 1920); - auto height = mPnh.param("height", 1080); - auto framerate = mPnh.param("framerate", 5); - auto device = mPnh.param("device", "/dev/arducam"); - cv::VideoCapture capture{std::format("v4l2src device={} ! videoconvert ! video/x-raw,width={},height={},format=I420,framerate={}/1 ! appsink", device, width, height, framerate), cv::CAP_GSTREAMER}; - if (!capture.isOpened()) throw std::runtime_error{"Long range cam failed to open"}; + try { + NODELET_INFO("Starting USB grab thread..."); - cv::Mat frame; - while (ros::ok()) { - capture.read(frame); - if (frame.empty()) break; + // See: http://wiki.ros.org/roscpp/Overview/NodeHandles for public vs. private node handle + // MT means multithreaded + mNh = getMTNodeHandle(); + mPnh = getMTPrivateNodeHandle(); + mCamInfoPub = mNh.advertise("long_range_cam/camera_info", 1); + mImgPub = mNh.advertise("long_range_image", 1); + auto width = mPnh.param("width", 1920); + auto height = mPnh.param("height", 1080); + auto framerate = mPnh.param("framerate", 5); + auto device = mPnh.param("device", "/dev/arducam"); + cv::VideoCapture capture{std::format("v4l2src device={} ! videoconvert ! video/x-raw,width={},height={},format=I420,framerate={}/1 ! appsink", device, width, height, framerate), cv::CAP_GSTREAMER}; + if (!capture.isOpened()) throw std::runtime_error{"Long range cam failed to open"}; - if (mImgPub.getNumSubscribers()) { - auto imageMessage = boost::make_shared(); - cv::Mat bgr; - cv::cvtColor(frame, bgr, cv::COLOR_YUV2BGR_I420); - fillImageMessage(bgr, imageMessage); - imageMessage->header.frame_id = "long_range_cam_frame"; - imageMessage->header.stamp = ros::Time::now(); - mImgPub.publish(imageMessage); + NODELET_INFO_STREAM(std::format("Long range cam opened: {}x{} @ {} fps", width, height, framerate)); + + cv::Mat frame; + while (ros::ok()) { + capture.read(frame); + if (frame.empty()) break; + + if (mImgPub.getNumSubscribers()) { + auto imageMessage = boost::make_shared(); + cv::Mat bgr; + cv::cvtColor(frame, bgr, cv::COLOR_YUV2BGR_I420); + fillImageMessage(bgr, imageMessage); + imageMessage->header.frame_id = "long_range_cam_frame"; + imageMessage->header.stamp = ros::Time::now(); + mImgPub.publish(imageMessage); + } } + } catch (std::exception const& e) { + NODELET_FATAL_STREAM(std::format("USB camera exception: {}", e.what())); + ros::requestShutdown(); } } From 7f80ce88ab2277e7b32e5b977b48bb15c915b762 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sun, 18 Feb 2024 14:08:52 -0500 Subject: [PATCH 81/81] Naming --- src/perception/long_range_cam/long_range_cam.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/perception/long_range_cam/long_range_cam.cpp b/src/perception/long_range_cam/long_range_cam.cpp index c63fc60a5..f9b5f6db9 100644 --- a/src/perception/long_range_cam/long_range_cam.cpp +++ b/src/perception/long_range_cam/long_range_cam.cpp @@ -44,7 +44,7 @@ namespace mrover { cv::VideoCapture capture{std::format("v4l2src device={} ! videoconvert ! video/x-raw,width={},height={},format=I420,framerate={}/1 ! appsink", device, width, height, framerate), cv::CAP_GSTREAMER}; if (!capture.isOpened()) throw std::runtime_error{"Long range cam failed to open"}; - NODELET_INFO_STREAM(std::format("Long range cam opened: {}x{} @ {} fps", width, height, framerate)); + NODELET_INFO_STREAM(std::format("USB camera opened: {}x{} @ {} fps", width, height, framerate)); cv::Mat frame; while (ros::ok()) {