diff --git a/GEMstack/knowledge/routes/highbay_new_layout.csv b/GEMstack/knowledge/routes/highbay_new_layout.csv new file mode 100644 index 000000000..8bfc40fe1 --- /dev/null +++ b/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 diff --git a/GEMstack/knowledge/vehicle/gem_e4_sensor_environment.yaml b/GEMstack/knowledge/vehicle/gem_e4_sensor_environment.yaml index a0c611b37..22ed4e439 100644 --- a/GEMstack/knowledge/vehicle/gem_e4_sensor_environment.yaml +++ b/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 - \ No newline at end of file + 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 \ No newline at end of file diff --git a/GEMstack/onboard/interface/gem_gazebo.py b/GEMstack/onboard/interface/gem_gazebo.py index 2c19d60b0..d402beb08 100644 --- a/GEMstack/onboard/interface/gem_gazebo.py +++ b/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': diff --git a/GEMstack/onboard/perception/test_gazebo_sensors.py b/GEMstack/onboard/perception/test_gazebo_sensors.py new file mode 100644 index 000000000..6f96975f9 --- /dev/null +++ b/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'] + diff --git a/launch/gazebo_simulation.yaml b/launch/gazebo_simulation.yaml index be77df511..b405b1a0a 100644 --- a/launch/gazebo_simulation.yaml +++ b/launch/gazebo_simulation.yaml @@ -91,6 +91,36 @@ variants: print: True # visualization: !include "klampt_visualization.yaml" # visualization: !include "mpl_visualization.yaml" + gazebo_sensor_check: + run: + mode: simulation + vehicle_interface: + type: gem_gazebo.GEMGazeboInterface + # args: + # scene: !relative_path '../scenes/gazebo.yaml' + + drive: + perception: + state_estimation : GNSSStateEstimator + agent_detection : test_gazebo_sensors.SensorCheck + perception_normalization : StandardPerceptionNormalizer + + planning: + # Adding your custom relation estimation + # relations_estimation: pedestrian_yield_logic.PedestrianYielder + # Fixed route with pure pursuit + route_planning: + type: StaticRoutePlanner + args: [!relative_path '../GEMstack/knowledge/routes/xyhead_highbay_backlot_p.csv'] # Fixed path + motion_planning: + type: RouteToTrajectoryPlanner + args: [null] #desired speed in m/s. If null, this will keep the route untimed for the trajectory tracker + trajectory_tracking: + type: pure_pursuit.PurePursuitTrajectoryTracker + args: {desired_speed: 2.5} #approximately 5mph + print: True + # visualization: !include "klampt_visualization.yaml" + # visualization: !include "mpl_visualization.yaml" log_ros: log: ros_topics : !include "../GEMstack/knowledge/defaults/standard_ros_topics.yaml" \ No newline at end of file