fix ros2 node spin and alvinxy

Author: harishkumarbalaji

GEMstack/knowledge/vehicle/gem_e4_sensor_environment.yaml

@@ -2,5 +2,5 @@ ros_topics:
   top_lidar: /ouster/points
   front_camera: /oak/rgb/image_raw
   front_depth: /oak/stereo/image_raw
-  gnss: /septentrio_gnss/insnavgeod
+  gnss: /insnavgeod
 

GEMstack/mathutils/transforms.py

@@ -1,8 +1,8 @@
-
 import numpy as np
 from klampt.math import vectorops as vo
 from klampt.math import so2
 from typing import Tuple
+import math
 
 def normalize_angle(angle : float) -> float:
     """Normalizes an angle to be in the range [0,2pi]"""
@@ -112,24 +112,54 @@ def yaw_to_heading(yaw : float, degrees=True) -> float:
     else:
         return np.radians(heading_degrees)
 
-def lat_lon_to_xy(lat : float, lon : float, lat_reference : float, lon_reference : float):
-    """ Conversion of Lat & Lon to X & Y.
-
-    Returns (x,y), where x is east in m, y is north in m.
+def lat_lon_to_xy(lat: float, lon: float, lat_ref: float, lon_ref: float) -> Tuple[float, float]:
+    """Convert geographic coordinates to local Cartesian coordinates.
+    
+    Args:
+        lat, lon: Target point (degrees)
+        lat_ref, lon_ref: Reference origin point (degrees)
+        
+    Returns:
+        (east_x, north_y) in meters
     """
-    import alvinxy.alvinxy as axy 
-    # convert GNSS waypoints into local fixed frame reprented in x and y
-    east_x, north_y = axy.ll2xy(lat, lon, lat_reference, lon_reference)
-    return east_x, north_y
-
-def xy_to_lat_lon(x_east : float, y_north : float, lat_reference : float, lon_reference : float):
-    """ Conversion of X & Y to Lat & Lon.
+    # Earth radius in meters
+    R = 6371000.0  
+    
+    # Convert reference latitude to radians
+    lat_ref_rad = math.radians(lat_ref)
+    
+    # Calculate deltas in degrees
+    delta_lat = lat - lat_ref
+    delta_lon = lon - lon_ref
+    
+    # Convert to meters
+    x = R * math.radians(delta_lon) * math.cos(lat_ref_rad)
+    y = R * math.radians(delta_lat)
+    
+    return x, y
 
-    Returns (lat,lon), where lat and lon are in degrees.
+def xy_to_lat_lon(x: float, y: float, lat_ref: float, lon_ref: float) -> Tuple[float, float]:
+    """Convert local Cartesian coordinates back to geographic coordinates.
+    
+    Args:
+        x: Easting in meters
+        y: Northing in meters
+        lat_ref, lon_ref: Reference origin point (degrees)
+        
+    Returns:
+        (latitude, longitude) in degrees
     """
-    import alvinxy.alvinxy as axy 
-    # convert GNSS waypoints into local fixed frame reprented in x and y
-    lat, lon = axy.xy2ll(x_east, y_north, lat_reference, lon_reference)
+    R = 6371000.0
+    lat_ref_rad = math.radians(lat_ref)
+    
+    # Convert meters to degrees
+    delta_lon = x / (R * math.cos(lat_ref_rad))
+    delta_lat = y / R
+    
+    # Calculate final coordinates
+    lat = lat_ref + math.degrees(delta_lat)
+    lon = lon_ref + math.degrees(delta_lon)
+    
     return lat, lon
 
 def quaternion_to_euler(x : float, y : float, z : float, w : float):

GEMstack/onboard/execution/entrypoint.py

@@ -47,16 +47,17 @@ def caution_callback(k,variant):
     has_ros = False
     try:
         import rclpy
+        from rclpy.executors import MultiThreadedExecutor
+        import threading
         rclpy.init(args=sys.argv)
-        rclpy.create_node('GEM_executor')#,disable_signals=True)
         has_ros = True
     except (ImportError,ModuleNotFoundError):
+
         if mode == 'simulation':
             print(EXECUTION_PREFIX,"Warning, ROS not found, but simulation mode requested")
         else:
             print(EXECUTION_PREFIX,"Error, ROS not found on system.")
             raise
-
     #create top-level components
     vehicle_interface = make_class(vehicle_interface_settings,'default','GEMstack.onboard.interface')
     mission_executor = make_class(mission_executor_settings,'execution','GEMstack.onboard.execution',{'vehicle_interface':vehicle_interface})
@@ -160,8 +161,10 @@ def caution_callback(k,variant):
         vehicle_interface.stop()
 
     if has_ros:
-        #need manual ros node shutdown due to disable_signals=True
-        rclpy.shutdown()
+        executor = MultiThreadedExecutor()
+        executor.add_node(vehicle_interface.node)
+        executor_thread = threading.Thread(target=executor.spin, daemon=True)
+        executor_thread.start()
 
     print(EXECUTION_PREFIX,"---------------- DONE ----------------")
     if log_settings and settings.get('run.after.show_log_folder',False):

GEMstack/onboard/interface/gem_hardware.py

@@ -9,6 +9,10 @@
 from rclpy.clock import Clock
 from std_msgs.msg import String, Bool, Float32, Float64
 from sensor_msgs.msg import Image,PointCloud2
+
+import threading
+from rclpy.executors import MultiThreadedExecutor
+
 try:
     from novatel_gps_msgs.msg import NovatelPosition, NovatelXYZ, Inspva
 except ImportError:
@@ -29,7 +33,7 @@
 import numpy as np
 from ...utils import conversions
 
-from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy
+from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSDurabilityPolicy, QoSLivelinessPolicy
 
 # Define a default QoS profile
 qos_profile = QoSProfile(
@@ -116,6 +120,10 @@ def __init__(self):
 
         #subscribers should go last because the callback might be called before the object is initialized
         self.enable_sub = self.node.create_subscription(Bool, '/pacmod/as_tx/enable', self.pacmod_enable_callback, qos_profile)
+        executor = MultiThreadedExecutor()
+        executor.add_node(self.node)
+        executor_thread = threading.Thread(target=executor.spin, daemon=True)
+        executor_thread.start()
 
 
     def start(self):
@@ -184,6 +192,7 @@ def callback_with_gnss_reading(inspva_msg: Inspva):
                     self.gnss_sub = self.node.create_subscription(Inspva, topic, callback_with_gnss_reading, qos_profile)
             else:
                 #assume it's septentrio on GEM e4
+                print("type",type)
                 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:
@@ -201,7 +210,18 @@ 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 = self.node.create_subscription(INSNavGeod, topic, callback_with_gnss_reading, qos_profile)
+                    # gnss_qos_profile = QoSProfile(
+                    #     reliability=QoSReliabilityPolicy.RELIABLE,
+                    #     history=QoSHistoryPolicy.KEEP_LAST,
+                    #     depth=10,
+                    #     durability=QoSDurabilityPolicy.VOLATILE,
+                    #     liveliness=QoSLivelinessPolicy.AUTOMATIC
+                    # )
+                    self.gnss_sub = self.node.create_subscription(INSNavGeod, topic, callback_with_gnss_reading, QoSProfile(
+                reliability=QoSReliabilityPolicy.RELIABLE,
+                durability=QoSDurabilityPolicy.VOLATILE,
+                depth=10
+            ))
         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):
@@ -210,7 +230,7 @@ def callback_with_gnss_reading(msg: INSNavGeod):
                 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)                    
+                    # 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 = self.node.create_subscription(PointCloud2, topic, callback_with_numpy, qos_profile)
@@ -305,7 +325,8 @@ def send_command(self, command : GEMVehicleCommand):
         self.accel_cmd.f64_cmd = command.accelerator_pedal_position
         if command.brake_pedal_position > 0.0:
             self.accel_cmd.f64_cmd = 0.0
-        self.brake_cmd.f64_cmd = command.brake_pedal_position
+        print("command.brake_pedal_position",command.brake_pedal_position)
+        self.brake_cmd.f64_cmd = float(command.brake_pedal_position)
         self.steer_cmd.angular_position = command.steering_wheel_angle
         self.steer_cmd.angular_velocity_limit = command.steering_wheel_speed
         print("**************************")

GEMstack/onboard/perception/state_estimation.py

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

GEMstack/onboard/planning/recovery.py

@@ -20,8 +20,8 @@ def state_outputs(self):
 
     def update(self):
         print("Stopping, current speed %.3f m/s"%(self.vehicle_interface.last_reading.speed))
-        brake_amount = settings.get('control.recovery.brake_amount')
-        brake_speed = settings.get('control.recovery.brake_speed')
+        brake_amount = float(settings.get('control.recovery.brake_amount'))
+        brake_speed = float(settings.get('control.recovery.brake_speed'))
         if self.vehicle_interface.last_command is not None:
             cmd = copy.copy(self.vehicle_interface.last_command)
             cmd.accelerator_pedal_position = 0.0