Skip to content

Commit a841b2b

Browse files
authored
Merge pull request #214 from krishauser/s2025_Simulation
S2025 simulation
2 parents db73561 + ccf3470 commit a841b2b

5 files changed

Lines changed: 375 additions & 1 deletion

File tree

Lines changed: 149 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,149 @@
1+
-0.0857276105926097,0.0018948314376441289,0.00153067884808511
2+
-0.09565841790623253,0.0046115223735956334,0.003354503815703236
3+
-0.06014663665623843,0.006232957797784877,0.004460305529355502
4+
0.023989440772734127,0.006869672310715558,0.00485124348995436
5+
0.15758642159159209,0.0036597944753431477,-0.00019705839551820148
6+
0.33510461858290697,-0.00215673776990144,-0.006815849380901184
7+
0.5515107368355636,-0.010818608287681997,-0.01369112626657421
8+
0.8145857815692068,-0.0239867002745644,-0.022922331082058954
9+
1.1360286977959646,-0.036294548233357204,-0.029013351271785344
10+
1.5183955914937535,-0.04841372951287504,-0.033187722926909004
11+
1.950715810904267,-0.06500072712522353,-0.037480322923599596
12+
2.427218434013497,-0.06469380199537156,-0.02936188263570511
13+
2.9508507614553725,-0.06153396227847452,-0.02185051044641142
14+
3.5175862894943,-0.060669897087057834,-0.018205702202329643
15+
4.145776578038317,-0.054511147975937035,-0.01179878837967004
16+
4.80408453759145,-0.05233636220969373,-0.011175392098198773
17+
5.534498602718227,-0.05290921704791174,-0.011839312989912877
18+
6.2910627030925514,-0.049572210407506034,-0.009524942023475536
19+
7.104068401515944,-0.03974486967454638,-0.003942965537870435
20+
7.954285176217708,-0.032212443656016276,-0.004639929116397212
21+
8.836931905206749,-0.06091102742715293,-0.025280880787695595
22+
9.742157096878174,-0.07715790441409887,-0.027656372214577493
23+
10.678226358190123,-0.08835641397991978,-0.02612612983179635
24+
11.628286613662667,-0.10316985585698468,-0.0278014404390172
25+
12.537665861034267,-0.12608386546554762,-0.032967794295231855
26+
13.411351183947207,-0.1487376492255894,-0.03532109705196678
27+
14.240609257858473,-0.1795258286115189,-0.042544617250128154
28+
15.033887502004864,-0.2082837183351156,-0.04451590507727892
29+
15.782202074224108,-0.23536512714411195,-0.04476640559485715
30+
16.4976335544533,-0.2620057315994977,-0.045185636691355356
31+
17.218197733976456,-0.28968821544780177,-0.045576294733306065
32+
17.976198228931104,-0.30959567321234616,-0.041184026149237996
33+
18.769853323907903,-0.3288618076061809,-0.03777855776488794
34+
19.60043996870495,-0.3470042806133282,-0.034788104676874536
35+
20.458122654729785,-0.36295435733843995,-0.031441616720474616
36+
21.353214126380553,-0.3834914393929516,-0.03276407116135354
37+
22.274774245500133,-0.3906986006510511,-0.025173445065675662
38+
23.179403027111213,-0.4005957725409548,-0.022460581715977375
39+
24.041557140423023,-0.41571310951929696,-0.025504741215989123
40+
24.85599225910705,-0.4260879699474671,-0.024433373996822452
41+
25.632465677200827,-0.43548702108422965,-0.02465581800793001
42+
26.37688161173339,-0.442568566384713,-0.023532156985957023
43+
27.091450927833183,-0.4524254221098367,-0.02324454151350405
44+
27.84588558534285,-0.4608870662815203,-0.023823326943363407
45+
28.643609745185305,-0.4750186351234511,-0.02496117951202683
46+
29.455572969774295,-0.4839746756920089,-0.02492890225542156
47+
30.276696996695733,-0.49719993817869934,-0.02610861774301919
48+
31.06171499991542,-0.5092271219048499,-0.02583895320194039
49+
31.852321090300904,-0.5231834477350112,-0.025841202018037208
50+
32.675401418622556,-0.5298390178967125,-0.022272568196384217
51+
33.5233289698496,-0.5386331910518862,-0.02037219269908905
52+
34.389579930164444,-0.5360435072439742,-0.013494264385777366
53+
35.236753877160055,-0.5346932644645435,-0.011460274330090992
54+
36.035039533462964,-0.5285796490661685,-0.0063680543945943135
55+
36.83671855882411,-0.5241026035752654,-0.0051360096201009675
56+
37.605099624695896,-0.5253441037356321,-0.007218169117970685
57+
38.33167402819884,-0.5495280481337268,-0.021989211313620183
58+
39.05817082165711,-0.5550010999952111,-0.020140637051816304
59+
39.817526644712146,-0.5559879382290411,-0.015844704431925646
60+
40.55905199047186,-0.5558271546873161,-0.012189214038926198
61+
41.27077899989261,-0.5583703516606349,-0.012096615601512137
62+
41.99963895358124,-0.5643289620894372,-0.014771386947605934
63+
42.73673928597515,-0.5684257527315513,-0.01459501281081003
64+
43.43286871371001,-0.5646361389240173,-0.009856057209305363
65+
44.15538635529973,-0.5651537941668838,-0.010996927719192041
66+
44.90863322652096,-0.5745804590623678,-0.015769101105084886
67+
45.701344846638875,-0.5810222328467844,-0.018690630569094164
68+
46.52657537146888,-0.5920235174826374,-0.02216624226620665
69+
47.37999562190261,-0.6064759218829803,-0.02505191356346923
70+
48.2664911448435,-0.6309273131765085,-0.0327895838291667
71+
49.180592137084425,-0.6852580375570572,-0.05419243002182796
72+
50.09382314699104,-0.73409394655582,-0.059883473851352134
73+
51.00000617548578,-0.7745052351469219,-0.05880546163782831
74+
51.921070954705684,-0.8269728971783437,-0.06427681454983204
75+
52.8692498058257,-0.8890276812704769,-0.07132093346947693
76+
53.83408589456876,-0.9679204292872843,-0.08326645871955479
77+
54.80635667311054,-1.0619975132005877,-0.09715741937910005
78+
55.745046075188554,-1.1365531421184158,-0.09135967560964073
79+
56.64278601846995,-1.1859468925760783,-0.07835423561571853
80+
57.54640516342839,-1.2528518029511826,-0.08304876173670878
81+
58.39777031945327,-1.436406981268112,-0.16666629130792845
82+
59.26448995919439,-1.6468551653542285,-0.20189396703252693
83+
60.0962307956477,-1.8472635264186668,-0.22426597399501128
84+
60.87743804086659,-2.026504557974211,-0.2279258142378304
85+
61.63506062838546,-2.313938253281383,-0.3087008600300523
86+
62.30919968804102,-2.799359205676379,-0.4743753278348018
87+
62.90146742862726,-3.45941188366381,-0.6705993746032345
88+
63.42545192254602,-4.1757298841524895,-0.8191440492523188
89+
63.9587498649922,-4.837021378315059,-0.8472453856939255
90+
64.5030828822261,-5.478911340478051,-0.8800216575371224
91+
64.88822560778245,-6.265322947356463,-1.0128490505839136
92+
65.22134552981873,-7.072848319648832,-1.094504166264329
93+
65.57938329784274,-7.741804157318322,-1.0988781896457513
94+
65.70743352187063,-8.475164307030063,-1.2449295955573696
95+
65.6878923342931,-9.281588113972026,-1.416374621985301
96+
65.5471218049521,-10.132753949989475,-1.585530471550452
97+
65.21142030010974,-10.981139925274165,-1.7849425096477707
98+
64.68284452959918,-11.793611191734323,-1.991469958635581
99+
63.96035509788638,-12.47569543415043,-2.2195394341772223
100+
63.07806407040732,-13.046911015715825,-2.431537427685788
101+
62.06489308869702,-13.426528656207722,-2.649294681402156
102+
61.02294073064036,-13.671546063550101,-2.8094644063793814
103+
59.95684860927197,-13.874433927921048,-2.901569642115492
104+
58.86848713610377,-14.047688260636383,-2.95970143961076
105+
57.75227086116723,-14.1368108334021,-3.032355634376085
106+
56.617523481646415,-14.113415153178714,-3.1177792155017143
107+
55.46975776229155,-14.096800757382432,3.138156842787429
108+
54.31011224373323,-14.105718242632426,3.1392797441775238
109+
53.15496863950385,-14.09599421676938,3.127172608221629
110+
51.99116576469032,-14.05378060381612,3.107264909269787
111+
50.79937118493551,-14.003828873227736,3.0957059125362436
112+
49.60214951676876,-13.790549525856665,2.993945966084366
113+
48.558820556760196,-13.287504087365027,2.7943405387218205
114+
47.53408233798883,-12.743673240619477,2.7100365997320917
115+
46.46848809407834,-12.197207689112133,2.670295795315682
116+
45.49137407943175,-11.469376894028574,2.545108355666999
117+
44.740213592834905,-10.479285309379577,2.309641971421337
118+
44.287318403815085,-9.307533116654826,2.057404365989762
119+
43.943100064304645,-8.087209039662175,1.918637601885159
120+
43.58130138908708,-6.866258631220623,1.8698722781734816
121+
43.248850097184864,-5.70055562051997,1.8581813094921087
122+
42.877782063712985,-4.561759884967824,1.867288632269595
123+
42.4101499119914,-3.520050448840724,1.951029465900434
124+
41.71614517895227,-2.7981638051469364,2.164327010939246
125+
40.979499058436964,-2.17978242506722,2.310051173073253
126+
40.23275690723315,-1.6084216502061093,2.398443692779879
127+
39.491288858343765,-1.071331173086831,2.4569961026981146
128+
38.75098266587469,-0.6407651324959431,2.5320256635612988
129+
38.0226583915218,-0.2873094856238616,2.6067259456167773
130+
37.296282723389,-0.01954997405059089,2.688034025022633
131+
36.57893501980182,0.18247947719633295,2.7683468342688737
132+
35.86014155423487,0.33492042041427617,2.837128673080914
133+
35.08612058184555,0.48477734549621054,2.8890835745606394
134+
34.26944676709246,0.6137548777216679,2.9304484262443142
135+
33.421282565421926,0.7703909131856808,2.93679708280795
136+
32.530995204458506,0.9382752827153755,2.944674365829883
137+
31.585481438375282,1.0145545767989486,3.0044400961131674
138+
30.605095294783116,1.0941918462719542,3.0306977529602306
139+
29.598809916987918,1.1662216710854985,3.0490143165843935
140+
28.559203103186732,1.216609397221987,3.0702039435596307
141+
27.500719333676418,1.2534590398072716,3.0874610572620513
142+
26.41820515448788,1.280788863773207,3.099155910863486
143+
25.306970675399334,1.310129258298618,3.103968454047471
144+
24.183005721716675,1.3400245675471627,3.1022255029521077
145+
23.099846097926417,1.3694088911561302,3.1038658438041744
146+
22.053839448145464,1.3732136353038218,3.1194772389507683
147+
21.02467689043662,1.3388701956514044,-3.1377787113250215
148+
20.042279345957972,1.3501375206784871,3.1310239946242957
149+
19.11700586684508,1.3649135301684847,3.1213875062666765

GEMstack/knowledge/vehicle/gem_e4_sensor_environment.yaml

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,4 +3,7 @@ ros_topics:
33
front_camera: /oak/rgb/image_raw
44
front_depth: /oak/stereo/image_raw
55
gnss: /septentrio_gnss/insnavgeod
6-
6+
front_left_camera: /camera_fl/arena_camera_node/image_raw
7+
front_right_camera: /camera_fr/arena_camera_node/image_raw
8+
rear_left_camera: /camera_rl/arena_camera_node/image_raw
9+
rear_right_camera: /camera_rr/arena_camera_node/image_raw

GEMstack/onboard/interface/gem_gazebo.py

Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -100,6 +100,10 @@ def __init__(self):
100100

101101
# Other sensors
102102
self.front_camera_sub = None
103+
self.front_left_camera_sub = None
104+
self.front_right_camera_sub = None
105+
self.rear_left_camera_sub = None
106+
self.rear_right_camera_sub = None
103107
self.front_depth_sub = None
104108
self.top_lidar_sub = None
105109
self.front_radar_sub = None
@@ -537,6 +541,54 @@ def callback_with_cv2(msg: Image):
537541
cv_image = conversions.ros_Image_to_cv2(msg, desired_encoding="bgr8")
538542
callback(cv_image)
539543
self.front_camera_sub = rospy.Subscriber(topic, Image, callback_with_cv2)
544+
545+
elif name == 'front_left_camera':
546+
topic = self.ros_sensor_topics[name]
547+
if type is not None and (type is not Image and type is not cv2.Mat):
548+
raise ValueError("GEMGazeboInterface only supports Image or OpenCV for front left camera")
549+
if type is None or type is Image:
550+
self.front_left_camera_sub = rospy.Subscriber(topic, Image, callback)
551+
else:
552+
def callback_with_cv2(msg: Image):
553+
cv_image = conversions.ros_Image_to_cv2(msg, desired_encoding="bgr8")
554+
callback(cv_image)
555+
self.front_left_camera_sub = rospy.Subscriber(topic, Image, callback_with_cv2)
556+
557+
elif name == 'front_right_camera':
558+
topic = self.ros_sensor_topics[name]
559+
if type is not None and (type is not Image and type is not cv2.Mat):
560+
raise ValueError("GEMGazeboInterface only supports Image or OpenCV for front right camera")
561+
if type is None or type is Image:
562+
self.front_right_camera_sub = rospy.Subscriber(topic, Image, callback)
563+
else:
564+
def callback_with_cv2(msg: Image):
565+
cv_image = conversions.ros_Image_to_cv2(msg, desired_encoding="bgr8")
566+
callback(cv_image)
567+
self.front_right_camera_sub = rospy.Subscriber(topic, Image, callback_with_cv2)
568+
569+
elif name == 'rear_left_camera':
570+
topic = self.ros_sensor_topics[name]
571+
if type is not None and (type is not Image and type is not cv2.Mat):
572+
raise ValueError("GEMGazeboInterface only supports Image or OpenCV for rear left camera")
573+
if type is None or type is Image:
574+
self.rear_left_camera_sub = rospy.Subscriber(topic, Image, callback)
575+
else:
576+
def callback_with_cv2(msg: Image):
577+
cv_image = conversions.ros_Image_to_cv2(msg, desired_encoding="bgr8")
578+
callback(cv_image)
579+
self.rear_left_camera_sub = rospy.Subscriber(topic, Image, callback_with_cv2)
580+
581+
elif name == 'rear_right_camera':
582+
topic = self.ros_sensor_topics[name]
583+
if type is not None and (type is not Image and type is not cv2.Mat):
584+
raise ValueError("GEMGazeboInterface only supports Image or OpenCV for rear right camera")
585+
if type is None or type is Image:
586+
self.rear_right_camera_sub = rospy.Subscriber(topic, Image, callback)
587+
else:
588+
def callback_with_cv2(msg: Image):
589+
cv_image = conversions.ros_Image_to_cv2(msg, desired_encoding="bgr8")
590+
callback(cv_image)
591+
self.rear_right_camera_sub = rospy.Subscriber(topic, Image, callback_with_cv2)
540592
# Front depth sensor has not been added to gazebo yet.
541593
# This code is placeholder until we add front depth sensor.
542594
elif name == 'front_depth':
Lines changed: 140 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,140 @@
1+
"""
2+
Demo script to run YOLO object detection on a Gazebo simulation.
3+
4+
This code subscribes to the front camera feed from the GEM e2/e4 model and applies YOLO-based object detection to the incoming images.
5+
6+
Visualization:
7+
- Use RViz or rqt to monitor the topics.
8+
9+
ROS Topics:
10+
- Raw camera feed: /gem/debug
11+
- YOLO detection output: /gem/image_detection
12+
"""
13+
14+
15+
# Python
16+
import os
17+
from typing import List, Dict
18+
from collections import defaultdict
19+
from datetime import datetime
20+
import copy
21+
# ROS, CV
22+
import rospy
23+
import message_filters
24+
import cv2
25+
import numpy as np
26+
import tf
27+
from cv_bridge import CvBridge
28+
from sensor_msgs.msg import Image, PointCloud2
29+
from visualization_msgs.msg import MarkerArray
30+
# GEMStack
31+
from ...state import AllState,VehicleState,ObjectPose,ObjectFrameEnum,AgentState,AgentEnum,AgentActivityEnum,ObjectFrameEnum
32+
from ..interface.gem import GEMInterface, GNSSReading
33+
from ..component import Component
34+
from scipy.spatial.transform import Rotation as R
35+
36+
37+
38+
class SensorCheck(Component):
39+
40+
def __init__(self, vehicle_interface : GEMInterface) -> None:
41+
"""
42+
Initializes essential functions required to run the YOLO model.
43+
"""
44+
45+
# vehicle interface
46+
self.vehicle_interface = vehicle_interface
47+
# cv2 to ros converter
48+
self.bridge = CvBridge()
49+
50+
51+
52+
def initialize(self):
53+
"""
54+
Defines callback functions for subscribing to the front camera image stream and sets up publishers for debugging purposes.
55+
"""
56+
57+
super().initialize()
58+
self.vehicle_interface.subscribe_sensor('front_camera',self.front_camera_callback, type = cv2.Mat)
59+
self.vehicle_interface.subscribe_sensor('front_left_camera',self.front_left_camera_callback, type = cv2.Mat)
60+
self.vehicle_interface.subscribe_sensor('front_right_camera',self.front_right_camera_callback, type = cv2.Mat)
61+
self.vehicle_interface.subscribe_sensor('rear_left_camera',self.rear_left_camera_callback, type = cv2.Mat)
62+
self.vehicle_interface.subscribe_sensor('rear_right_camera',self.rear_right_camera_callback, type = cv2.Mat)
63+
64+
self.pub_front_camera_image = rospy.Publisher("/gem/debug/front_camera", Image, queue_size=1)
65+
self.pub_front_left_camera_image = rospy.Publisher("/gem/debug/front_left_camera", Image, queue_size=1)
66+
self.pub_front_right_camera_image = rospy.Publisher("/gem/debug/front_right_camera", Image, queue_size=1)
67+
self.pub_rear_left_camera_image = rospy.Publisher("/gem/debug/rear_left_camera", Image, queue_size=1)
68+
self.pub_rear_right_camera_image = rospy.Publisher("/gem/debug/rear_right_camera", Image, queue_size=1)
69+
70+
71+
72+
def update(self, vehicle : VehicleState) -> Dict[str,AgentState]:
73+
74+
"""
75+
Displays vehicle statistics in the GLOBAL reference frame.
76+
77+
This function also allows switching between coordinate frames using the `VehicleState -> pose` method.
78+
79+
Returning an `AgentState` will automatically log detected objects.
80+
"""
81+
82+
return {}
83+
84+
85+
86+
def front_camera_callback(self, image: cv2.Mat):
87+
88+
"""
89+
A simple callback function that re published the topic to validate image coming through gazebo
90+
"""
91+
92+
ros_img = self.bridge.cv2_to_imgmsg(image, 'bgr8')
93+
self.pub_front_camera_image.publish(ros_img)
94+
95+
def front_left_camera_callback(self, image: cv2.Mat):
96+
97+
"""
98+
A simple callback function that re published the topic to validate image coming through gazebo
99+
"""
100+
101+
ros_img = self.bridge.cv2_to_imgmsg(image, 'bgr8')
102+
self.pub_front_left_camera_image.publish(ros_img)
103+
104+
def front_right_camera_callback(self, image: cv2.Mat):
105+
106+
"""
107+
A simple callback function that re published the topic to validate image coming through gazebo
108+
"""
109+
110+
ros_img = self.bridge.cv2_to_imgmsg(image, 'bgr8')
111+
self.pub_front_right_camera_image.publish(ros_img)
112+
113+
def rear_left_camera_callback(self, image: cv2.Mat):
114+
115+
"""
116+
A simple callback function that re published the topic to validate image coming through gazebo
117+
"""
118+
119+
ros_img = self.bridge.cv2_to_imgmsg(image, 'bgr8')
120+
self.pub_rear_left_camera_image.publish(ros_img)
121+
122+
def rear_right_camera_callback(self, image: cv2.Mat):
123+
124+
"""
125+
A simple callback function that re published the topic to validate image coming through gazebo
126+
"""
127+
128+
ros_img = self.bridge.cv2_to_imgmsg(image, 'bgr8')
129+
self.pub_rear_right_camera_image.publish(ros_img)
130+
131+
132+
def rate(self):
133+
return 4.0
134+
135+
def state_inputs(self):
136+
return ['vehicle']
137+
138+
def state_outputs(self):
139+
return ['agents']
140+

0 commit comments

Comments
 (0)