Merge pull request #214 from krishauser/s2025_Simulation

S2025 simulation

Author: krishauser

GEMstack/knowledge/routes/highbay_new_layout.csv

@@ -0,0 +1,149 @@
+-0.0857276105926097,0.0018948314376441289,0.00153067884808511
+-0.09565841790623253,0.0046115223735956334,0.003354503815703236
+-0.06014663665623843,0.006232957797784877,0.004460305529355502
+0.023989440772734127,0.006869672310715558,0.00485124348995436
+0.15758642159159209,0.0036597944753431477,-0.00019705839551820148
+0.33510461858290697,-0.00215673776990144,-0.006815849380901184
+0.5515107368355636,-0.010818608287681997,-0.01369112626657421
+0.8145857815692068,-0.0239867002745644,-0.022922331082058954
+1.1360286977959646,-0.036294548233357204,-0.029013351271785344
+1.5183955914937535,-0.04841372951287504,-0.033187722926909004
+1.950715810904267,-0.06500072712522353,-0.037480322923599596
+2.427218434013497,-0.06469380199537156,-0.02936188263570511
+2.9508507614553725,-0.06153396227847452,-0.02185051044641142
+3.5175862894943,-0.060669897087057834,-0.018205702202329643
+4.145776578038317,-0.054511147975937035,-0.01179878837967004
+4.80408453759145,-0.05233636220969373,-0.011175392098198773
+5.534498602718227,-0.05290921704791174,-0.011839312989912877
+6.2910627030925514,-0.049572210407506034,-0.009524942023475536
+7.104068401515944,-0.03974486967454638,-0.003942965537870435
+7.954285176217708,-0.032212443656016276,-0.004639929116397212
+8.836931905206749,-0.06091102742715293,-0.025280880787695595
+9.742157096878174,-0.07715790441409887,-0.027656372214577493
+10.678226358190123,-0.08835641397991978,-0.02612612983179635
+11.628286613662667,-0.10316985585698468,-0.0278014404390172
+12.537665861034267,-0.12608386546554762,-0.032967794295231855
+13.411351183947207,-0.1487376492255894,-0.03532109705196678
+14.240609257858473,-0.1795258286115189,-0.042544617250128154
+15.033887502004864,-0.2082837183351156,-0.04451590507727892
+15.782202074224108,-0.23536512714411195,-0.04476640559485715
+16.4976335544533,-0.2620057315994977,-0.045185636691355356
+17.218197733976456,-0.28968821544780177,-0.045576294733306065
+17.976198228931104,-0.30959567321234616,-0.041184026149237996
+18.769853323907903,-0.3288618076061809,-0.03777855776488794
+19.60043996870495,-0.3470042806133282,-0.034788104676874536
+20.458122654729785,-0.36295435733843995,-0.031441616720474616
+21.353214126380553,-0.3834914393929516,-0.03276407116135354
+22.274774245500133,-0.3906986006510511,-0.025173445065675662
+23.179403027111213,-0.4005957725409548,-0.022460581715977375
+24.041557140423023,-0.41571310951929696,-0.025504741215989123
+24.85599225910705,-0.4260879699474671,-0.024433373996822452
+25.632465677200827,-0.43548702108422965,-0.02465581800793001
+26.37688161173339,-0.442568566384713,-0.023532156985957023
+27.091450927833183,-0.4524254221098367,-0.02324454151350405
+27.84588558534285,-0.4608870662815203,-0.023823326943363407
+28.643609745185305,-0.4750186351234511,-0.02496117951202683
+29.455572969774295,-0.4839746756920089,-0.02492890225542156
+30.276696996695733,-0.49719993817869934,-0.02610861774301919
+31.06171499991542,-0.5092271219048499,-0.02583895320194039
+31.852321090300904,-0.5231834477350112,-0.025841202018037208
+32.675401418622556,-0.5298390178967125,-0.022272568196384217
+33.5233289698496,-0.5386331910518862,-0.02037219269908905
+34.389579930164444,-0.5360435072439742,-0.013494264385777366
+35.236753877160055,-0.5346932644645435,-0.011460274330090992
+36.035039533462964,-0.5285796490661685,-0.0063680543945943135
+36.83671855882411,-0.5241026035752654,-0.0051360096201009675
+37.605099624695896,-0.5253441037356321,-0.007218169117970685
+38.33167402819884,-0.5495280481337268,-0.021989211313620183
+39.05817082165711,-0.5550010999952111,-0.020140637051816304
+39.817526644712146,-0.5559879382290411,-0.015844704431925646
+40.55905199047186,-0.5558271546873161,-0.012189214038926198
+41.27077899989261,-0.5583703516606349,-0.012096615601512137
+41.99963895358124,-0.5643289620894372,-0.014771386947605934
+42.73673928597515,-0.5684257527315513,-0.01459501281081003
+43.43286871371001,-0.5646361389240173,-0.009856057209305363
+44.15538635529973,-0.5651537941668838,-0.010996927719192041
+44.90863322652096,-0.5745804590623678,-0.015769101105084886
+45.701344846638875,-0.5810222328467844,-0.018690630569094164
+46.52657537146888,-0.5920235174826374,-0.02216624226620665
+47.37999562190261,-0.6064759218829803,-0.02505191356346923
+48.2664911448435,-0.6309273131765085,-0.0327895838291667
+49.180592137084425,-0.6852580375570572,-0.05419243002182796
+50.09382314699104,-0.73409394655582,-0.059883473851352134
+51.00000617548578,-0.7745052351469219,-0.05880546163782831
+51.921070954705684,-0.8269728971783437,-0.06427681454983204
+52.8692498058257,-0.8890276812704769,-0.07132093346947693
+53.83408589456876,-0.9679204292872843,-0.08326645871955479
+54.80635667311054,-1.0619975132005877,-0.09715741937910005
+55.745046075188554,-1.1365531421184158,-0.09135967560964073
+56.64278601846995,-1.1859468925760783,-0.07835423561571853
+57.54640516342839,-1.2528518029511826,-0.08304876173670878
+58.39777031945327,-1.436406981268112,-0.16666629130792845
+59.26448995919439,-1.6468551653542285,-0.20189396703252693
+60.0962307956477,-1.8472635264186668,-0.22426597399501128
+60.87743804086659,-2.026504557974211,-0.2279258142378304
+61.63506062838546,-2.313938253281383,-0.3087008600300523
+62.30919968804102,-2.799359205676379,-0.4743753278348018
+62.90146742862726,-3.45941188366381,-0.6705993746032345
+63.42545192254602,-4.1757298841524895,-0.8191440492523188
+63.9587498649922,-4.837021378315059,-0.8472453856939255
+64.5030828822261,-5.478911340478051,-0.8800216575371224
+64.88822560778245,-6.265322947356463,-1.0128490505839136
+65.22134552981873,-7.072848319648832,-1.094504166264329
+65.57938329784274,-7.741804157318322,-1.0988781896457513
+65.70743352187063,-8.475164307030063,-1.2449295955573696
+65.6878923342931,-9.281588113972026,-1.416374621985301
+65.5471218049521,-10.132753949989475,-1.585530471550452
+65.21142030010974,-10.981139925274165,-1.7849425096477707
+64.68284452959918,-11.793611191734323,-1.991469958635581
+63.96035509788638,-12.47569543415043,-2.2195394341772223
+63.07806407040732,-13.046911015715825,-2.431537427685788
+62.06489308869702,-13.426528656207722,-2.649294681402156
+61.02294073064036,-13.671546063550101,-2.8094644063793814
+59.95684860927197,-13.874433927921048,-2.901569642115492
+58.86848713610377,-14.047688260636383,-2.95970143961076
+57.75227086116723,-14.1368108334021,-3.032355634376085
+56.617523481646415,-14.113415153178714,-3.1177792155017143
+55.46975776229155,-14.096800757382432,3.138156842787429
+54.31011224373323,-14.105718242632426,3.1392797441775238
+53.15496863950385,-14.09599421676938,3.127172608221629
+51.99116576469032,-14.05378060381612,3.107264909269787
+50.79937118493551,-14.003828873227736,3.0957059125362436
+49.60214951676876,-13.790549525856665,2.993945966084366
+48.558820556760196,-13.287504087365027,2.7943405387218205
+47.53408233798883,-12.743673240619477,2.7100365997320917
+46.46848809407834,-12.197207689112133,2.670295795315682
+45.49137407943175,-11.469376894028574,2.545108355666999
+44.740213592834905,-10.479285309379577,2.309641971421337
+44.287318403815085,-9.307533116654826,2.057404365989762
+43.943100064304645,-8.087209039662175,1.918637601885159
+43.58130138908708,-6.866258631220623,1.8698722781734816
+43.248850097184864,-5.70055562051997,1.8581813094921087
+42.877782063712985,-4.561759884967824,1.867288632269595
+42.4101499119914,-3.520050448840724,1.951029465900434
+41.71614517895227,-2.7981638051469364,2.164327010939246
+40.979499058436964,-2.17978242506722,2.310051173073253
+40.23275690723315,-1.6084216502061093,2.398443692779879
+39.491288858343765,-1.071331173086831,2.4569961026981146
+38.75098266587469,-0.6407651324959431,2.5320256635612988
+38.0226583915218,-0.2873094856238616,2.6067259456167773
+37.296282723389,-0.01954997405059089,2.688034025022633
+36.57893501980182,0.18247947719633295,2.7683468342688737
+35.86014155423487,0.33492042041427617,2.837128673080914
+35.08612058184555,0.48477734549621054,2.8890835745606394
+34.26944676709246,0.6137548777216679,2.9304484262443142
+33.421282565421926,0.7703909131856808,2.93679708280795
+32.530995204458506,0.9382752827153755,2.944674365829883
+31.585481438375282,1.0145545767989486,3.0044400961131674
+30.605095294783116,1.0941918462719542,3.0306977529602306
+29.598809916987918,1.1662216710854985,3.0490143165843935
+28.559203103186732,1.216609397221987,3.0702039435596307
+27.500719333676418,1.2534590398072716,3.0874610572620513
+26.41820515448788,1.280788863773207,3.099155910863486
+25.306970675399334,1.310129258298618,3.103968454047471
+24.183005721716675,1.3400245675471627,3.1022255029521077
+23.099846097926417,1.3694088911561302,3.1038658438041744
+22.053839448145464,1.3732136353038218,3.1194772389507683
+21.02467689043662,1.3388701956514044,-3.1377787113250215
+20.042279345957972,1.3501375206784871,3.1310239946242957
+19.11700586684508,1.3649135301684847,3.1213875062666765

GEMstack/knowledge/vehicle/gem_e4_sensor_environment.yaml

@@ -3,4 +3,7 @@ ros_topics:
   front_camera: /oak/rgb/image_raw
   front_depth: /oak/stereo/image_raw
   gnss: /septentrio_gnss/insnavgeod
-  
+  front_left_camera: /camera_fl/arena_camera_node/image_raw
+  front_right_camera: /camera_fr/arena_camera_node/image_raw
+  rear_left_camera: /camera_rl/arena_camera_node/image_raw
+  rear_right_camera: /camera_rr/arena_camera_node/image_raw

GEMstack/onboard/interface/gem_gazebo.py

@@ -100,6 +100,10 @@ def __init__(self):
 
         # Other sensors
         self.front_camera_sub = None
+        self.front_left_camera_sub = None
+        self.front_right_camera_sub = None
+        self.rear_left_camera_sub = None
+        self.rear_right_camera_sub = None
         self.front_depth_sub = None
         self.top_lidar_sub = None
         self.front_radar_sub = None
@@ -537,6 +541,54 @@ def callback_with_cv2(msg: Image):
                     cv_image = conversions.ros_Image_to_cv2(msg, desired_encoding="bgr8")
                     callback(cv_image)
                 self.front_camera_sub = rospy.Subscriber(topic, Image, callback_with_cv2)
+
+        elif name == 'front_left_camera':
+            topic = self.ros_sensor_topics[name]
+            if type is not None and (type is not Image and type is not cv2.Mat):
+                raise ValueError("GEMGazeboInterface only supports Image or OpenCV for front left camera")
+            if type is None or type is Image:
+                self.front_left_camera_sub = rospy.Subscriber(topic, Image, callback)
+            else:
+                def callback_with_cv2(msg: Image):
+                    cv_image = conversions.ros_Image_to_cv2(msg, desired_encoding="bgr8")
+                    callback(cv_image)
+                self.front_left_camera_sub = rospy.Subscriber(topic, Image, callback_with_cv2)
+
+        elif name == 'front_right_camera':
+            topic = self.ros_sensor_topics[name]
+            if type is not None and (type is not Image and type is not cv2.Mat):
+                raise ValueError("GEMGazeboInterface only supports Image or OpenCV for front right camera")
+            if type is None or type is Image:
+                self.front_right_camera_sub = rospy.Subscriber(topic, Image, callback)
+            else:
+                def callback_with_cv2(msg: Image):
+                    cv_image = conversions.ros_Image_to_cv2(msg, desired_encoding="bgr8")
+                    callback(cv_image)
+                self.front_right_camera_sub = rospy.Subscriber(topic, Image, callback_with_cv2)
+
+        elif name == 'rear_left_camera':
+            topic = self.ros_sensor_topics[name]
+            if type is not None and (type is not Image and type is not cv2.Mat):
+                raise ValueError("GEMGazeboInterface only supports Image or OpenCV for rear left camera")
+            if type is None or type is Image:
+                self.rear_left_camera_sub = rospy.Subscriber(topic, Image, callback)
+            else:
+                def callback_with_cv2(msg: Image):
+                    cv_image = conversions.ros_Image_to_cv2(msg, desired_encoding="bgr8")
+                    callback(cv_image)
+                self.rear_left_camera_sub = rospy.Subscriber(topic, Image, callback_with_cv2)
+
+        elif name == 'rear_right_camera':
+            topic = self.ros_sensor_topics[name]
+            if type is not None and (type is not Image and type is not cv2.Mat):
+                raise ValueError("GEMGazeboInterface only supports Image or OpenCV for rear right camera")
+            if type is None or type is Image:
+                self.rear_right_camera_sub = rospy.Subscriber(topic, Image, callback)
+            else:
+                def callback_with_cv2(msg: Image):
+                    cv_image = conversions.ros_Image_to_cv2(msg, desired_encoding="bgr8")
+                    callback(cv_image)
+                self.rear_right_camera_sub = rospy.Subscriber(topic, Image, callback_with_cv2)
         # Front depth sensor has not been added to gazebo yet.
         # This code is placeholder until we add front depth sensor.
         elif name == 'front_depth':

GEMstack/onboard/perception/test_gazebo_sensors.py

@@ -0,0 +1,140 @@
+"""
+Demo script to run YOLO object detection on a Gazebo simulation.
+
+This code subscribes to the front camera feed from the GEM e2/e4 model and applies YOLO-based object detection to the incoming images.
+
+Visualization:
+- Use RViz or rqt to monitor the topics.
+
+ROS Topics:
+- Raw camera feed: /gem/debug
+- YOLO detection output: /gem/image_detection
+"""
+
+
+# Python 
+import os
+from typing import List, Dict
+from collections import defaultdict
+from datetime import datetime
+import copy
+# ROS, CV
+import rospy
+import message_filters
+import cv2
+import numpy as np
+import tf
+from cv_bridge import CvBridge
+from sensor_msgs.msg import Image, PointCloud2
+from visualization_msgs.msg import MarkerArray
+# GEMStack
+from ...state import AllState,VehicleState,ObjectPose,ObjectFrameEnum,AgentState,AgentEnum,AgentActivityEnum,ObjectFrameEnum
+from ..interface.gem import GEMInterface, GNSSReading
+from ..component import Component
+from scipy.spatial.transform import Rotation as R
+
+
+
+class SensorCheck(Component):
+
+    def __init__(self, vehicle_interface : GEMInterface) -> None:
+        """
+        Initializes essential functions required to run the YOLO model.
+        """
+
+        # vehicle interface
+        self.vehicle_interface = vehicle_interface
+        # cv2 to ros converter
+        self.bridge = CvBridge()
+
+
+   
+    def initialize(self):
+        """
+        Defines callback functions for subscribing to the front camera image stream and sets up publishers for debugging purposes.
+        """
+
+        super().initialize()
+        self.vehicle_interface.subscribe_sensor('front_camera',self.front_camera_callback, type = cv2.Mat)
+        self.vehicle_interface.subscribe_sensor('front_left_camera',self.front_left_camera_callback, type = cv2.Mat)
+        self.vehicle_interface.subscribe_sensor('front_right_camera',self.front_right_camera_callback, type = cv2.Mat)
+        self.vehicle_interface.subscribe_sensor('rear_left_camera',self.rear_left_camera_callback, type = cv2.Mat)
+        self.vehicle_interface.subscribe_sensor('rear_right_camera',self.rear_right_camera_callback, type = cv2.Mat)
+
+        self.pub_front_camera_image = rospy.Publisher("/gem/debug/front_camera", Image, queue_size=1)
+        self.pub_front_left_camera_image = rospy.Publisher("/gem/debug/front_left_camera", Image, queue_size=1)
+        self.pub_front_right_camera_image = rospy.Publisher("/gem/debug/front_right_camera", Image, queue_size=1)
+        self.pub_rear_left_camera_image = rospy.Publisher("/gem/debug/rear_left_camera", Image, queue_size=1)
+        self.pub_rear_right_camera_image = rospy.Publisher("/gem/debug/rear_right_camera", Image, queue_size=1)
+        
+
+
+    def update(self, vehicle : VehicleState) -> Dict[str,AgentState]:
+
+        """
+        Displays vehicle statistics in the GLOBAL reference frame.
+
+        This function also allows switching between coordinate frames using the `VehicleState -> pose` method.
+
+        Returning an `AgentState` will automatically log detected objects.
+        """
+
+        return {}
+
+
+
+    def front_camera_callback(self, image: cv2.Mat):
+        
+        """
+        A simple callback function that re published the topic to validate image coming through gazebo
+        """
+
+        ros_img = self.bridge.cv2_to_imgmsg(image, 'bgr8')
+        self.pub_front_camera_image.publish(ros_img)
+
+    def front_left_camera_callback(self, image: cv2.Mat):
+        
+        """
+        A simple callback function that re published the topic to validate image coming through gazebo
+        """
+
+        ros_img = self.bridge.cv2_to_imgmsg(image, 'bgr8')
+        self.pub_front_left_camera_image.publish(ros_img)
+
+    def front_right_camera_callback(self, image: cv2.Mat):
+        
+        """
+        A simple callback function that re published the topic to validate image coming through gazebo
+        """
+
+        ros_img = self.bridge.cv2_to_imgmsg(image, 'bgr8')
+        self.pub_front_right_camera_image.publish(ros_img)
+
+    def rear_left_camera_callback(self, image: cv2.Mat):
+        
+        """
+        A simple callback function that re published the topic to validate image coming through gazebo
+        """
+
+        ros_img = self.bridge.cv2_to_imgmsg(image, 'bgr8')
+        self.pub_rear_left_camera_image.publish(ros_img)
+
+    def rear_right_camera_callback(self, image: cv2.Mat):
+        
+        """
+        A simple callback function that re published the topic to validate image coming through gazebo
+        """
+
+        ros_img = self.bridge.cv2_to_imgmsg(image, 'bgr8')
+        self.pub_rear_right_camera_image.publish(ros_img)
+
+
+    def rate(self):
+        return 4.0
+    
+    def state_inputs(self):
+        return ['vehicle']
+    
+    def state_outputs(self):
+        return ['agents']
+