Gazebo simulation yaw and doc update

GEMstack/onboard/interface/gem_gazebo.py

@@ -7,6 +7,10 @@
 import rospy
 from sensor_msgs.msg import Image, PointCloud2, Imu, NavSatFix
 from septentrio_gnss_driver.msg import INSNavGeod
+try:
+    from novatel_gps_msgs.msg import NovatelPosition, NovatelXYZ, Inspva
+except ImportError:
+    pass
 from geometry_msgs.msg import Vector3Stamped
 from sensor_msgs.msg import JointState  # For reading joint states from Gazebo
 # Changed from AckermannDriveStamped
@@ -40,6 +44,7 @@ def __init__(self):
         GEMInterface.__init__(self)
         self.max_send_rate = settings.get('vehicle.max_command_rate', 10.0)
         self.ros_sensor_topics = settings.get('vehicle.sensors.ros_topics')
+        self.debug = settings.get('vehicle.debug', True)
         self.last_command_time = 0.0
         self.last_reading = GEMVehicleReading()
         self.last_reading.speed = 0.0
@@ -53,9 +58,12 @@ def __init__(self):
         self.last_reading.wiper_level = 0
         self.last_reading.headlights_on = False
 
-
-
-
+        # Determine the vehicle type based on the GNSS topic
+        gnss_topic = self.ros_sensor_topics.get('gnss', '')
+        self.is_gem_e2 = 'novatel' in gnss_topic or gnss_topic.endswith('inspva')
+        if self.debug:
+            print(f"Detected vehicle type: {'GEM e2' if self.is_gem_e2 else 'GEM e4'}")
+            print(f"GNSS topic: {gnss_topic}")
 
         # GNSS data subscriber
         self.gnss_sub = None
@@ -79,7 +87,8 @@ def __init__(self):
         self.clock_sub = rospy.Subscriber('/clock', Clock, self.clock_callback)
 
     def start(self):
-        print("Starting GEM Gazebo Interface")
+        if self.debug:
+            print("Starting GEM Gazebo Interface")
 
     def clock_callback(self, msg):
         self.sim_time = msg.clock
@@ -94,57 +103,96 @@ def get_reading(self) -> GEMVehicleReading:
 
     def subscribe_sensor(self, name, callback, type=None):
         if name == 'gnss':
-            topic = self.ros_sensor_topics['gnss']
-            def gnss_callback_wrapper(gnss_msg: INSNavGeod):
-                roll, pitch, yaw = gnss_msg.roll, gnss_msg.pitch, gnss_msg.heading
-                # Convert from degrees to radians
-                roll, pitch, yaw = math.radians(roll), math.radians(pitch), math.radians(yaw)
-
-                # Transform yaw to correct frame - Gazebo typically uses ROS standard frame (x-forward)
-                # while navigation uses x-east reference frame
-                # Need to convert from Gazebo's frame to navigation heading, then to navigation yaw
-
-                # Assuming Gazebo's yaw is 0 when facing east (ROS REP 103 convention)
-                # Convert IMU's yaw to heading (CW from North), then to navigation yaw (CCW from East)
-                # This handles the coordinate frame differences between Gazebo and the navigation frame
-                # Negate yaw to convert from ROS to heading
-                heading = transforms.yaw_to_heading(-yaw - np.pi/2, degrees=False)
-                navigation_yaw = transforms.heading_to_yaw(
-                    heading, degrees=False)
-
-                # Create fused pose with transformed yaw
-                pose = ObjectPose(
-                    frame=ObjectFrameEnum.GLOBAL,
-                    t=gnss_msg.header.stamp,
-                    x=gnss_msg.longitude,
-                    y=gnss_msg.latitude,
-                    z=gnss_msg.height,
-                    roll=roll,
-                    pitch=pitch,
-                    yaw=navigation_yaw
-                )
-
-                # Calculate speed from GNSS
-                self.last_reading.speed = np.linalg.norm([gnss_msg.ve, gnss_msg.vn])
-
-                # Create GNSS reading with fused data
-                reading = GNSSReading(
-                    pose=pose,
-                    speed=self.last_reading.speed,
-                    status='error' if gnss_msg.error else 'ok'
-                )
-                # Added debug
-                print(
-                    f"[GNSS] Raw coordinates: Lat={gnss_msg.latitude:.6f}, Lon={gnss_msg.longitude:.6f}")
-                # Added debug
-                print(
-                    f"[GNSS-FUSED] Orientation: Roll={roll:.2f}, Pitch={pitch:.2f}, Yaw={yaw:.2f} rad")
-                # Added debug
-                print(f"[GNSS-FUSED] Speed: {self.last_reading.speed:.2f} m/s")
-
-                callback(reading)
-
-            self.gnss_sub = rospy.Subscriber(topic, INSNavGeod, gnss_callback_wrapper)
+            topic = self.ros_sensor_topics[name]
+            if self.is_gem_e2:  # GEM e2 uses Novatel GNSS
+                if self.debug:
+                    print(f"Setting up GEM e2 GNSS subscriber for topic: {topic}")
+
+                if type is Inspva:
+                    self.gnss_sub = rospy.Subscriber(topic, Inspva, callback)
+                else:
+                    def callback_with_gnss_reading(inspva_msg):
+                        # Convert from degrees to radians for roll, pitch, azimuth
+                        roll = math.radians(inspva_msg.roll)
+                        pitch = math.radians(inspva_msg.pitch)
+                        yaw = math.radians(inspva_msg.azimuth)  # azimuth is heading from north in degrees
+
+                        # Create fused pose with yaw
+                        pose = ObjectPose(
+                            frame=ObjectFrameEnum.GLOBAL,
+                            t=inspva_msg.header.stamp,
+                            x=inspva_msg.longitude,
+                            y=inspva_msg.latitude,
+                            z=inspva_msg.height,
+                            roll=roll,
+                            pitch=pitch,
+                            yaw=yaw
+                        )
+
+                        # Calculate speed from velocity components
+                        speed = np.linalg.norm([inspva_msg.east_velocity, inspva_msg.north_velocity])
+                        self.last_reading.speed = speed
+
+                        # Create GNSS reading with fused data
+                        reading = GNSSReading(
+                            pose=pose,
+                            speed=speed,
+                            status=inspva_msg.status
+                        )
+
+                        # Only print debug info if debug flag is enabled
+                        if self.debug:
+                            print(f"[GNSS] Raw coordinates: Lat={inspva_msg.latitude:.6f}, Lon={inspva_msg.longitude:.6f}")
+                            print(f"[GNSS-FUSED] Orientation: Roll={roll:.2f}, Pitch={pitch:.2f}, Azimuth={inspva_msg.azimuth}°, Nav Yaw={yaw:.2f} rad")
+                            print(f"[GNSS-FUSED] Speed: {speed:.2f} m/s")
+
+                        callback(reading)
+
+                    self.gnss_sub = rospy.Subscriber(topic, Inspva, callback_with_gnss_reading)
+
+            else:  # GEM e4 uses Septentrio GNSS
+                if self.debug:
+                    print(f"Setting up GEM e4 GNSS subscriber for topic: {topic}")
+
+                if type is INSNavGeod:
+                    self.gnss_sub = rospy.Subscriber(topic, INSNavGeod, callback)
+                else:
+                    def callback_with_gnss_reading(gnss_msg):
+                        roll, pitch, heading = gnss_msg.roll, gnss_msg.pitch, gnss_msg.heading
+                        # Convert from degrees to radians
+                        roll, pitch, yaw = math.radians(roll), math.radians(pitch), math.radians(heading)
+
+                        # Create fused pose with transformed yaw
+                        pose = ObjectPose(
+                            frame=ObjectFrameEnum.GLOBAL,
+                            t=gnss_msg.header.stamp,
+                            x=gnss_msg.longitude,
+                            y=gnss_msg.latitude,
+                            z=gnss_msg.height,
+                            roll=roll,
+                            pitch=pitch,
+                            yaw=yaw
+                        )
+
+                        # Calculate speed from GNSS
+                        self.last_reading.speed = np.linalg.norm([gnss_msg.ve, gnss_msg.vn])
+
+                        # Create GNSS reading with fused data
+                        reading = GNSSReading(
+                            pose=pose,
+                            speed=self.last_reading.speed,
+                            status='error' if gnss_msg.error else 'ok'
+                        )
+
+                        # Only print debug info if debug flag is enabled
+                        if self.debug:
+                            print(f"[GNSS] Raw coordinates: Lat={gnss_msg.latitude:.6f}, Lon={gnss_msg.longitude:.6f}")
+                            print(f"[GNSS-FUSED] Orientation: Roll={roll:.2f}, Pitch={pitch:.2f}, Heading={heading}°, Nav Yaw={yaw:.2f} rad")
+                            print(f"[GNSS-FUSED] Speed: {self.last_reading.speed:.2f} m/s")
+
+                        callback(reading)
+
+                    self.gnss_sub = rospy.Subscriber(topic, INSNavGeod, callback_with_gnss_reading)
 
         elif name == 'top_lidar':
             topic = self.ros_sensor_topics[name]
@@ -250,8 +298,9 @@ def send_command(self, command : GEMVehicleCommand):
         msg.steering_angle = phides
         msg.steering_angle_velocity = command.steering_wheel_speed  # Respect steering velocity limit
 
-        # Debug output
-        print(f"[ACKERMANN] Speed: {msg.speed:.2f}, Accel: {msg.acceleration:.2f}, Steer: {msg.steering_angle:.2f}")
+        # Debug output only if debug flag is enabled
+        if self.debug:
+            print(f"[ACKERMANN] Speed: {msg.speed:.2f}, Accel: {msg.acceleration:.2f}, Steer: {msg.steering_angle:.2f}")
 
         self.ackermann_pub.publish(msg)
         self.last_command = command

README.md

@@ -23,6 +23,8 @@ You should also have the following Python dependencies installed, which you can
   - pyyaml
 - Perception Dependencies
   - ultralytics
+- Gazebo Simulation Dependencies (only needed for Gazebo simulation)
+  - ros-noetic-ackermann-msgs
 
 In order to interface with the actual GEM e2 vehicle, you will need [PACMOD2](https://github.com/astuff/pacmod2) - Autonomoustuff's low level interface to vehicle. You will also need Autonomoustuff's [sensor message packages](https://github.com/astuff/astuff_sensor_msgs).  The onboard computer uses Ubuntu 20.04 with Python 3.8, CUDA 11.6, and NVIDIA driver 515, so to minimize compatibility issues you should ensure that these are installed on your development system.
 
@@ -107,7 +109,18 @@ bash stop_docker_container.sh
 ```
 
 ## Installing for Mac
-To install Ubuntu and setup ROS for Mac, follow this [link](https://doc.clickup.com/9011960452/d/h/8cjf6m4-11191/e694fcfb47a015e) for in-depth instructions and troubleshooting.
+
+For detailed step-by-step instructions on setting up GEMstack on Mac systems using UTM (virtual machine):
+
+- See the [Mac Setup Instructions](docs/Mac%20Setup%20Instructions.md) document for comprehensive guidance
+- Follow along with the recommended [YouTube tutorial](https://www.youtube.com/watch?v=MVLbb1aMk24) for visual reference
+
+This guide covers:
+- Setting up UTM virtual machine
+- Installing Ubuntu 20.04
+- Configuring the environment 
+- Installing ROS Noetic
+- Setting up GEMstack
 
 ## In this folder
 
@@ -239,13 +252,14 @@ Legend:
     - 🟨 `multiprocess_execution`: Component executors that work in separate process.  (Stdout logging not done yet. Still hangs on exception.)
 
   - `visualization/`: Visualization components on-board the vehicle
-    - 🟨 `mpl_visualization`: Matplotlib visualization
+    - 🟩 `mpl_visualization`: Matplotlib visualization
     - 🟩 `klampt_visualization`: Klampt visualization
 
   - `interface/`: Defines interfaces to vehicle hardware and simulators.
     - 🟩 `gem`: Base class for the Polaris GEM e2 vehicle.
     - 🟩 `gem_hardware`: Interface to the real GEM vehicle.
     - 🟩 `gem_simulator`: Interfaces to simulated GEM vehicles.
+    - 🟩 `gem_gazebo`: Interface to the GEM vehicle in Gazebo simulation.
     - 🟩 `gem_mixed`: Interfaces to the real GEM e2 vehicle's sensors but simulated motion.
 
 
@@ -255,6 +269,16 @@ You will launch a simulation using:
 
 - `python3 main.py --variant=sim launch/LAUNCH_FILE.yaml` where `LAUNCH_FILE.yaml` is your preferred launch file.  Try `python3 main.py --variant=sim launch/fixed_route.yaml`.  Inspect the simulator classes in `GEMstack/onboard/interface/gem_simulator.py` for more information about configuring the simulator.
 
+### Gazebo Simulation
+
+For a more realistic 3D simulation environment, you can use the Gazebo simulator:
+
+- `python3 main.py --variant=gazebo launch/LAUNCH_FILE.yaml` to launch with Gazebo integration.
+
+For detailed setup instructions, sensor configuration, and usage guidelines, see the [Gazebo Simulation Documentation](docs/Gazebo%20Simulation%20Documentation.md).
+
+### Launching the Onboard Stack
+
 To launch onboard behavior you will open Terminator / tmux and split it into three terminal windows. In each of them run:
 
 - `cd GEMstack`
@@ -392,6 +416,11 @@ drive:
 
 A launch file can contain a `variants` key that may specify certain changes to the launch stack that may be named via `--variant=X` on the command line.  As an example, see `launch/fixed_route.yaml`.  This specifies two variants, `sim` and `log_ros` which would run a simulation or log ROS topics.  You can specify multiple variants on the command line using the format `--variant=X,Y`.
 
+Common variants include:
+- `sim`: Uses the simplified Python simulator
+- `gazebo`: Uses the Gazebo 3D simulator (requires additional setup, see [documentation](docs/Gazebo%20Simulation%20Documentation.md))
+- `log_ros`: Logs ROS topics
+
 ### Managing and modifying state
 
 When implementing your computation graph, you should think of `AllState` as a strictly typed blackboard architecture in which items can be read from and written to.  If you need to pass data between components, you should add it to the state rather than use alternative techniques, e.g., global variables.  This will allow the logging / replay to save and restore system state.  Over a long development period, it would be best to be disciplined at versioning.
@@ -408,8 +437,8 @@ If you wish to override the executor to add more pipelines, you will need to cre
 To count as a contribution to the team, you will need to check in your code via pull requests (PRs).  PRs should be reviewed by at least one other approver.
 
 - `main`: will contain content that persists between years.  Approver: Kris Hauser.
-- `s2024`: is the "official class vehicle" for this semester's class.  Approver: instructor, TAs.
-- `s2024_groupX`: will be your group's branch. Approver: instructor, TAs, team members.  
+- `s2025`: is the "official class vehicle" for this semester's class.  Approver: instructor, TAs.
+- `s2025_groupX`: will be your group's branch. Approver: instructor, TAs, team members.  
 
 Guidelines:
 - DO NOT check in large datasets.  Instead, keep these around on SSDs.

docs/Gazebo Simulation Documentation.md

@@ -65,37 +65,38 @@ In one terminal, run the Gazebo simulator (using the instructions provided in th
 
 ### 2. Launch the GEM Stack
 
-Open a second terminal and launch GEMStack with your configured launch file.
+Open a second terminal and launch GEMStack with your configured launch file. Make sure to set the variant to `gazebo`.
 
-```bash
-python3 main.py --variant=gazebo --settings={your_file}.yaml launch/{your_file}.yaml
+#### For GEM e2 Vehicle:
 
+```bash
+python3 main.py --variant=gazebo --settings=GEMstack/knowledge/defaults/e2.yaml launch/fixed_route.yaml
 ```
-- Make sure to set the variant to `gazebo`.
-- You can specify the settings file to  GEMstack/knowledge/defaults/e4_gazebo.yaml or e2_gazebo.yaml till we work on closing the sim to real gap.
 
-Example command launching the fixed route with e4 vehicle:
+#### For GEM e4 Vehicle:
 
 ```bash
-python3 main.py --variant=gazebo --settings=GEMstack/knowledge/defaults/e2.yaml launch/fixed_route.yaml
-or
 python3 main.py --variant=gazebo --settings=GEMstack/knowledge/defaults/current.yaml launch/fixed_route.yaml
 ```
-**Variants:**
- - sim
- - gazebo
 
-**Vehicle types:**
-- e2
-- e4
+You can replace `fixed_route.yaml` with your specific launch file.
+
+**Note:** By default, the system uses `GEMstack/knowledge/defaults/current.yaml` which contains GEM e4 vehicle configuration settings.
 
-**Setting:**
+## Available Variants and Vehicle Types
+
+**Variants:**
+- `sim` - Simple simulation mode
+- `gazebo` - 3D Gazebo simulation mode
 
-By default it takes GEMstack/knowledge/defaults/current.yaml which is GEM E4 Vehicle configuration settings.
+**Vehicle Types:**
+- `e2` - GEM e2 vehicle (uses Novatel GNSS)
+- `e4` - GEM e4 vehicle (uses Septentrio GNSS)
 
-Other available configuration files:
+## Available Configuration Files
 
 GEMstack/knowledge/defaults/
-- e2.yaml
+- `current.yaml` - Default configuration (GEM e4)
+- `e2.yaml` - GEM e2 configuration
 
 ---