Adding changes for ros2

Author: animeshsingh98

GEMstack/offboard/calibration/capture_lidar_zed.py

@@ -1,5 +1,5 @@
 # ROS Headers
-import rospy
+import rclpy
 from sensor_msgs.msg import Image,PointCloud2
 import sensor_msgs.point_cloud2 as pc2
 import ctypes
@@ -67,10 +67,11 @@ def save_scan(lidar_fn,color_fn,depth_fn):
     cv2.imwrite(depth_fn,dimage)
 
 def main(folder='data',start_index=1):
-    rospy.init_node("capture_lidar_zed",disable_signals=True)
-    lidar_sub = rospy.Subscriber("/lidar1/velodyne_points", PointCloud2, lidar_callback)
-    camera_sub = rospy.Subscriber("/zed2/zed_node/rgb/image_rect_color", Image, camera_callback)
-    depth_sub = rospy.Subscriber("/zed2/zed_node/depth/depth_registered", Image, depth_callback)
+    rclpy.init(args=sys.argv)
+    node = rclpy.create_node("capture_lidar_zed",disable_signals=True)
+    lidar_sub = node.create_subscription(PointCloud2, "/lidar1/velodyne_points", lidar_callback)
+    camera_sub = node.create_subscription(Image, "/zed2/zed_node/rgb/image_rect_color", camera_callback)
+    depth_sub = node.create_subscription(Image, "/zed2/zed_node/depth/depth_registered", depth_callback)
     index = start_index
     print("Press any key to:")
     print("  store lidar point clouds as npz")

GEMstack/onboard/execution/entrypoint.py

@@ -46,8 +46,9 @@ def caution_callback(k,variant):
     #initialize ros node
     has_ros = False
     try:
-        import rospy
-        rospy.init_node('GEM_executor',disable_signals=True)
+        import rclpy
+        rclpy.init(args=sys.argv)
+        rclpy.create_node('GEM_executor')#,disable_signals=True)
         has_ros = True
     except (ImportError,ModuleNotFoundError):
         if mode == 'simulation':
@@ -160,7 +161,7 @@ def caution_callback(k,variant):
 
     if has_ros:
         #need manual ros node shutdown due to disable_signals=True
-        rospy.signal_shutdown('GEM_executor finished')
+        rclpy.shutdown()
 
     print(EXECUTION_PREFIX,"---------------- DONE ----------------")
     if log_settings and settings.get('run.after.show_log_folder',False):

GEMstack/onboard/interface/gem_hardware.py

@@ -4,7 +4,9 @@
 import time
 
 # ROS Headers
-import rospy
+import rclpy
+from rclpy.node import Node
+from rclpy.clock import Clock
 from std_msgs.msg import String, Bool, Float32, Float64
 from sensor_msgs.msg import Image,PointCloud2
 try:
@@ -17,20 +19,30 @@
     pass
 
 from radar_msgs.msg import RadarTracks
-from tf.transformations import euler_from_quaternion, quaternion_from_euler
+from tf_transformations import euler_from_quaternion, quaternion_from_euler
 
 # GEM PACMod Headers
-from pacmod_msgs.msg import PositionWithSpeed, PacmodCmd, SystemRptFloat, VehicleSpeedRpt, GlobalRpt
+from pacmod2_msgs.msg import PositionWithSpeed, PacmodCmd, SystemRptFloat, VehicleSpeedRpt, GlobalRpt
 
 # OpenCV and cv2 bridge
 import cv2
 import numpy as np
 from ...utils import conversions
 
+from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
+
+# Define a default QoS profile
+qos_profile = QoSProfile(
+    reliability=QoSReliabilityPolicy.BEST_EFFORT,  # Use RELIABLE if needed
+    history=QoSHistoryPolicy.KEEP_LAST,
+    depth=10
+)
+
 class GEMHardwareInterface(GEMInterface):
     """Interface for connnecting to the physical GEM e2 vehicle."""
     def __init__(self):
         GEMInterface.__init__(self)
+        self.node = rclpy.create_node('gem_hardware_interface')
         self.max_send_rate = settings.get('vehicle.max_command_rate',10.0)
         self.ros_sensor_topics = settings.get('vehicle.sensors.ros_topics')
         self.last_command_time = 0.0
@@ -46,9 +58,9 @@ def __init__(self):
         self.last_reading.wiper_level = 0
         self.last_reading.headlights_on = False
 
-        self.speed_sub  = rospy.Subscriber("/pacmod/parsed_tx/vehicle_speed_rpt", VehicleSpeedRpt, self.speed_callback)
-        self.steer_sub = rospy.Subscriber("/pacmod/parsed_tx/steer_rpt", SystemRptFloat, self.steer_callback)
-        self.global_sub = rospy.Subscriber("/pacmod/parsed_tx/global_rpt", GlobalRpt, self.global_callback)
+        self.speed_sub  = self.node.create_subscription(VehicleSpeedRpt, "/pacmod/parsed_tx/vehicle_speed_rpt", self.speed_callback, qos_profile)
+        self.steer_sub = self.node.create_subscription(SystemRptFloat, "/pacmod/parsed_tx/steer_rpt", self.steer_callback, qos_profile)
+        self.global_sub = self.node.create_subscription(GlobalRpt, "/pacmod/parsed_tx/global_rpt", self.global_callback, qos_profile)
         self.gnss_sub = None
         self.imu_sub = None
         self.front_radar_sub = None
@@ -60,36 +72,36 @@ def __init__(self):
 
         # -------------------- PACMod setup --------------------
         # GEM vehicle enable
-        self.enable_pub = rospy.Publisher('/pacmod/as_rx/enable', Bool, queue_size=1)
+        self.enable_pub = self.node.create_publisher(Bool, '/pacmod/as_rx/enable', 1)
         self.pacmod_enable = False
 
         # GEM vehicle gear control, neutral, forward and reverse, publish once
-        self.gear_pub = rospy.Publisher('/pacmod/as_rx/shift_cmd', PacmodCmd, queue_size=1)
+        self.gear_pub = self.node.create_publisher(PacmodCmd, '/pacmod/as_rx/shift_cmd', 1)
         self.gear_cmd = PacmodCmd()
         self.gear_cmd.enable = True
         self.gear_cmd.ui16_cmd = PacmodCmd.SHIFT_NEUTRAL 
 
         # GEM vehicle brake control
-        self.brake_pub = rospy.Publisher('/pacmod/as_rx/brake_cmd', PacmodCmd, queue_size=1)
+        self.brake_pub = self.node.create_publisher(PacmodCmd, '/pacmod/as_rx/brake_cmd', 1)
         self.brake_cmd = PacmodCmd()
         self.brake_cmd.enable = False
         self.brake_cmd.clear  = True
         self.brake_cmd.ignore = True
 
         # GEM vehicle forward motion control
-        self.accel_pub = rospy.Publisher('/pacmod/as_rx/accel_cmd', PacmodCmd, queue_size=1)
+        self.accel_pub = self.node.create_publisher(PacmodCmd, '/pacmod/as_rx/accel_cmd', 1)
         self.accel_cmd = PacmodCmd()
         self.accel_cmd.enable = False
         self.accel_cmd.clear  = True
         self.accel_cmd.ignore = True
 
         # GEM vehicle turn signal control
-        self.turn_pub = rospy.Publisher('/pacmod/as_rx/turn_cmd', PacmodCmd, queue_size=1)
+        self.turn_pub = self.node.create_publisher(PacmodCmd, '/pacmod/as_rx/turn_cmd', 1)
         self.turn_cmd = PacmodCmd()
         self.turn_cmd.ui16_cmd = 1 # None
 
         # GEM vechile steering wheel control
-        self.steer_pub = rospy.Publisher('/pacmod/as_rx/steer_cmd', PositionWithSpeed, queue_size=1)
+        self.steer_pub = self.node.create_publisher(PositionWithSpeed, '/pacmod/as_rx/steer_cmd', 1)
         self.steer_cmd = PositionWithSpeed()
         self.steer_cmd.angular_position = 0.0 # radians, -: clockwise, +: counter-clockwise
         self.steer_cmd.angular_velocity_limit = 2.0 # radians/second
@@ -103,7 +115,7 @@ def __init__(self):
         #TODO: publish TwistStamped to /front_radar/front_radar/vehicle_motion to get better radar tracks
 
         #subscribers should go last because the callback might be called before the object is initialized
-        self.enable_sub = rospy.Subscriber('/pacmod/as_tx/enable', Bool, self.pacmod_enable_callback)
+        self.enable_sub = self.node.create_subscription(Bool, '/pacmod/as_tx/enable', self.pacmod_enable_callback, qos_profile)
 
 
     def start(self):
@@ -117,7 +129,7 @@ def start(self):
             #this doesn't seem to work super well, need to send enable command multiple times
 
     def time(self):
-        seconds = rospy.get_time()
+        seconds = Clock().now().to_msg().sec
         return seconds
 
     def speed_callback(self,msg : VehicleSpeedRpt):
@@ -156,7 +168,7 @@ def subscribe_sensor(self, name, callback, type = None):
                 if type is not None and (type is not Inspva and type is not GNSSReading):
                     raise ValueError("GEMHardwareInterface GEM e2 only supports Inspva/GNSSReading for GNSS")
                 if type is Inspva:
-                    self.gnss_sub = rospy.Subscriber(topic, Inspva, callback)
+                    self.gnss_sub = self.node.create_subscription(Inspva, topic, callback, qos_profile)
                 else:
                     def callback_with_gnss_reading(inspva_msg: Inspva):
                         pose = ObjectPose(ObjectFrameEnum.GLOBAL,
@@ -169,13 +181,13 @@ def callback_with_gnss_reading(inspva_msg: Inspva):
                                     )
                         speed = np.sqrt(inspva_msg.east_velocity**2 + inspva_msg.north_velocity**2)
                         callback(GNSSReading(pose,speed,inspva_msg.status))
-                    self.gnss_sub = rospy.Subscriber(topic, Inspva, callback_with_gnss_reading)
+                    self.gnss_sub = self.node.create_subscription(Inspva, topic, callback_with_gnss_reading, qos_profile)
             else:
                 #assume it's septentrio on GEM e4
                 if type is not None and (type is not INSNavGeod and type is not GNSSReading):
                     raise ValueError("GEMHardwareInterface GEM e4 only supports INSNavGeod/GNSSReading for GNSS")
                 if type is INSNavGeod:
-                    self.gnss_sub = rospy.Subscriber(topic, INSNavGeod, callback)
+                    self.gnss_sub = self.node.create_subscription(INSNavGeod, topic, callback, qos_profile)
                 else:
                     def callback_with_gnss_reading(msg: INSNavGeod):
                         pose = ObjectPose(ObjectFrameEnum.GLOBAL,
@@ -189,47 +201,47 @@ def callback_with_gnss_reading(msg: INSNavGeod):
                                     )
                         speed = np.sqrt(msg.ve**2 + msg.vn**2)
                         callback(GNSSReading(pose,speed,('error' if msg.error else 'ok')))
-                    self.gnss_sub = rospy.Subscriber(topic, INSNavGeod, callback_with_gnss_reading)
+                    self.gnss_sub = self.node.create_subscription(INSNavGeod, topic, callback_with_gnss_reading, qos_profile)
         elif name == 'top_lidar':
             topic = self.ros_sensor_topics[name]
             if type is not None and (type is not PointCloud2 and type is not np.ndarray):
                 raise ValueError("GEMHardwareInterface only supports PointCloud2 or numpy array for top lidar")
             if type is None or type is PointCloud2:
-                self.top_lidar_sub = rospy.Subscriber(topic, PointCloud2, callback)
+                self.top_lidar_sub = self.node.create_subscription(PointCloud2, topic, callback, qos_profile)
             else:
                 def callback_with_numpy(msg : Image):
                     #print("received image with size",msg.width,msg.height,"encoding",msg.encoding)                    
                     points = conversions.ros_PointCloud2_to_numpy(msg, want_rgb=False)
                     callback(points)
-                self.top_lidar_sub = rospy.Subscriber(topic, PointCloud2, callback_with_numpy)
+                self.top_lidar_sub = self.node.create_subscription(PointCloud2, topic, callback_with_numpy, qos_profile)
         elif name == 'front_radar':
             if type is not None and type is not RadarTracks:
                 raise ValueError("GEMHardwareInterface only supports RadarTracks for front radar")
-            self.front_radar_sub = rospy.Subscriber("/front_radar/front_radar/radar_tracks", RadarTracks, callback)
+            self.front_radar_sub = self.node.create_subscription(RadarTracks, "/front_radar/front_radar/radar_tracks", callback, qos_profile)
         elif name == 'front_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("GEMHardwareInterface only supports Image or OpenCV for front camera")
             if type is None or type is Image:
-                self.front_camera_sub = rospy.Subscriber(topic, Image, callback)
+                self.front_camera_sub = self.node.create_subscription(Image, topic, callback, qos_profile)
             else:
                 def callback_with_cv2(msg : Image):
                     #print("received image with size",msg.width,msg.height,"encoding",msg.encoding)                    
                     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)
+                self.front_camera_sub = self.node.create_subscription(Image, topic, callback_with_cv2, qos_profile)
         elif name == 'front_depth':
             topic = self.ros_sensor_topics[name]
             if type is not None and (type is not Image and type is not cv2.Mat):
                 raise ValueError("GEMHardwareInterface only supports Image or OpenCV for front depth")
             if type is None or type is Image:
-                self.front_depth_sub = rospy.Subscriber(topic, Image, callback)
+                self.front_depth_sub = self.node.create_subscription(Image, topic, callback, qos_profile)
             else:
                 def callback_with_cv2(msg : Image):
                     #print("received image with size",msg.width,msg.height,"encoding",msg.encoding)                    
                     cv_image = conversions.ros_Image_to_cv2(msg, desired_encoding="passthrough")
                     callback(cv_image)
-                self.front_depth_sub = rospy.Subscriber(topic, Image, callback_with_cv2)
+                self.front_depth_sub = self.node.create_subscription(Image, topic, callback_with_cv2, qos_profile)
 
 
     # PACMod enable callback function

GEMstack/onboard/perception/pedestrian_detection.py

@@ -0,0 +1,84 @@
+from ...state import AllState,VehicleState,ObjectPose,ObjectFrameEnum,AgentState,AgentEnum,AgentActivityEnum
+from ..interface.gem import GEMInterface
+from ..component import Component
+#from ultralytics import YOLO
+#import cv2
+from typing import Dict
+
+def box_to_fake_agent(box):
+    """Creates a fake agent state from an (x,y,w,h) bounding box.
+    
+    The location and size are pretty much meaningless since this is just giving a 2D location.
+    """
+    x,y,w,h = box
+    pose = ObjectPose(t=0,x=x+w/2,y=y+h/2,z=0,yaw=0,pitch=0,roll=0,frame=ObjectFrameEnum.CURRENT)
+    dims = (w,h,0)
+    return AgentState(pose=pose,dimensions=dims,outline=None,type=AgentEnum.PEDESTRIAN,activity=AgentActivityEnum.MOVING,velocity=(0,0,0),yaw_rate=0)
+
+
+class PedestrianDetector2D(Component):
+    """Detects pedestrians."""
+    def __init__(self,vehicle_interface : GEMInterface):
+        self.vehicle_interface = vehicle_interface
+        #self.detector = YOLO('../../knowledge/detection/yolov8n.pt')
+        self.last_person_boxes = []
+
+    def rate(self):
+        return 4.0
+    
+    def state_inputs(self):
+        return ['vehicle']
+    
+    def state_outputs(self):
+        return ['agents']
+    
+    def initialize(self):
+        #tell the vehicle to use image_callback whenever 'front_camera' gets a reading, and it expects images of type cv2.Mat
+        #self.vehicle_interface.subscribe_sensor('front_camera',self.image_callback,cv2.Mat)
+        pass
+    
+    #def image_callback(self, image : cv2.Mat):
+    #    detection_result = self.detector(image)
+    #    self.last_person_boxes = []
+    #    #uncomment if you want to debug the detector...
+    #    #for bb in self.last_person_boxes:
+    #    #    x,y,w,h = bb
+    #    #    cv2.rectangle(image, (int(x-w/2), int(y-h/2)), (int(x+w/2), int(y+h/2)), (255, 0, 255), 3)
+    #    #cv2.imwrite("pedestrian_detections.png",image)
+    
+    def update(self, vehicle : VehicleState) -> Dict[str,AgentState]:
+        res = {}
+        for i,b in enumerate(self.last_person_boxes):
+            x,y,w,h = b
+            res['pedestrian'+str(i)] = box_to_fake_agent(b)
+        if len(res) > 0:
+            print("Detected",len(res),"pedestrians")
+        return res
+
+
+class FakePedestrianDetector2D(Component):
+    """Triggers a pedestrian detection at some random time ranges"""
+    def __init__(self,vehicle_interface : GEMInterface):
+        self.vehicle_interface = vehicle_interface
+        self.times = [(5.0,20.0),(30.0,35.0)]
+        self.t_start = None
+
+    def rate(self):
+        return 4.0
+    
+    def state_inputs(self):
+        return ['vehicle']
+    
+    def state_outputs(self):
+        return ['agents']
+    
+    def update(self, vehicle : VehicleState) -> Dict[str,AgentState]:
+        if self.t_start is None:
+            self.t_start = self.vehicle_interface.time()
+        t = self.vehicle_interface.time() - self.t_start
+        res = {}
+        for times in self.times:
+            if t >= times[0] and t <= times[1]:
+                res['pedestrian0'] = box_to_fake_agent((0,0,0,0))
+                print("Detected a pedestrian")
+        return res

GEMstack/onboard/perception/person_detector.py

@@ -0,0 +1,48 @@
+#from ultralytics import YOLO
+import cv2
+import sys
+
+def person_detector(img : cv2.Mat):
+    #TODO: implement me to produce a list of (x,y,w,h) bounding boxes of people in the image
+    return []
+
+def main(fn):
+    image = cv2.imread(fn)
+    bboxes = person_detector(image)
+    print("Detected",len(bboxes),"people")
+    for bb in bboxes:
+        x,y,w,h = bb
+        if not isinstance(x,(int,float)) or not isinstance(y,(int,float)) or not isinstance(w,(int,float)) or not isinstance(h,(int,float)):
+            print("WARNING: make sure to return Python numbers rather than PyTorch Tensors")
+        print("Corner",(x,y),"size",(w,h))
+        cv2.rectangle(image, (int(x-w/2), int(y-h/2)), (int(x+w/2), int(y+h/2)), (255, 0, 255), 3)
+    cv2.imshow('Results', image)
+    cv2.waitKey(0)
+
+def main_webcam(): 
+    cap = cv2.VideoCapture(0)
+    cap.set(3, 640)
+    cap.set(4, 480)
+
+    print("Press space to exit")
+    while True:
+        _, image = cap.read()
+        
+        bboxes = person_detector(image)
+        for bb in bboxes:
+            x,y,w,h = bb
+            cv2.rectangle(image, (int(x-w/2), int(y-h/2)), (int(x+w/2), int(y+h/2)), (255, 0, 255), 3)
+              
+        cv2.imshow('Person detection', image)     
+        if cv2.waitKey(1) & 0xFF == ord(' '):
+            break
+
+    cap.release()
+
+
+if __name__ == '__main__':
+    fn = sys.argv[1]
+    if fn != 'webcam':
+        main(fn)
+    else:
+        main_webcam()

GEMstack/onboard/perception/state_estimation.py

@@ -37,7 +37,8 @@ def state_outputs(self) -> List[str]:
         return ['vehicle']
 
     def healthy(self):
-        return self.gnss_pose is not None
+        #return self.gnss_pose is not None
+        return True
 
     def update(self) -> VehicleState:
         if self.gnss_pose is None: