Skip to content

Subscribe to gnss topic from gazebo simulation #184

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
harishkumarbalaji merged 3 commits into from
May 7, 2025
Merged

Subscribe to gnss topic from gazebo simulation #184

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
6dc31d2
subscribes to combined gnss data
pravshot May 2, 2025
ac5f11a
updates self.last_reading.speed in gnss_callback and removed unused i…
pravshot May 2, 2025
45105e5
Merge branch 's2025_Simulation' into s2025_Simulation_gnss_sub
pravshot May 7, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
96 changes: 34 additions & 62 deletions GEMstack/onboard/interface/gem_gazebo.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,11 +1,12 @@
from .gem import *
from ...utils import settings
import math
import time
import time

# ROS Headers
import rospy
from sensor_msgs.msg import Image, PointCloud2, Imu, NavSatFix
from septentrio_gnss_driver.msg import INSNavGeod
from geometry_msgs.msg import Vector3Stamped
from sensor_msgs.msg import JointState # For reading joint states from Gazebo
# Changed from AckermannDriveStamped
Expand Down Expand Up @@ -52,13 +53,9 @@ def __init__(self):
self.last_reading.wiper_level = 0
self.last_reading.headlights_on = False





# IMU data subscriber
self.imu_sub = None
self.imu_data = None


# GNSS data subscriber
self.gnss_sub = None
Expand All @@ -77,12 +74,6 @@ def __init__(self):
self.ackermann_cmd = AckermannDrive()
self.last_command = None # Store the last command

# Subscribe to IMU topic by default
self.imu_sub = rospy.Subscriber("/imu", Imu, self.imu_callback)

# Subscribe to GNSS Velocitu
self.gnss_vel_sub = rospy.Subscriber("/gps/fix_velocity", Vector3Stamped, self.gnss_vel_callback)

# Add clock subscription for simulation time
self.sim_time = rospy.Time(0)
self.clock_sub = rospy.Subscriber('/clock', Clock, self.clock_callback)
Expand All @@ -97,33 +88,17 @@ def time(self):
# Return Gazebo simulation time
return self.sim_time.to_sec()

def imu_callback(self, msg: Imu):
self.imu_data = msg

def gnss_vel_callback(self, msg):
self.last_reading.speed = np.linalg.norm([msg.vector.x, msg.vector.y] )

def get_reading(self) -> GEMVehicleReading:
return self.last_reading


def subscribe_sensor(self, name, callback, type=None):
if name == 'gnss':
topic = self.ros_sensor_topics['gps']
# Fuse IMU orientation with GNSS position
def gnss_callback_wrapper(gps_msg: NavSatFix):
if self.imu_data is None:
return # Wait for IMU data

# Get orientation from IMU
quaternion = (
self.imu_data.orientation.x,
self.imu_data.orientation.y,
self.imu_data.orientation.z,
self.imu_data.orientation.w
)
print(f"[IMU] Orientation: {quaternion}")
roll, pitch, yaw = euler_from_quaternion(quaternion)
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
Expand All @@ -133,31 +108,34 @@ def gnss_callback_wrapper(gps_msg: NavSatFix):
# 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)
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=gps_msg.header.stamp,
x=gps_msg.longitude,
y=gps_msg.latitude,
z=gps_msg.altitude,
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='FIX' if gps_msg.status.status >= 0 else 'NO_FIX'
speed=self.last_reading.speed,
status='error' if gnss_msg.error else 'ok'
)
# Added debug
print(
f"[GNSS] Raw coordinates: Lat={gps_msg.latitude:.6f}, Lon={gps_msg.longitude:.6f}")
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")
Expand All @@ -166,7 +144,7 @@ def gnss_callback_wrapper(gps_msg: NavSatFix):

callback(reading)

self.gnss_sub = rospy.Subscriber(topic, NavSatFix, gnss_callback_wrapper)
self.gnss_sub = rospy.Subscriber(topic, INSNavGeod, gnss_callback_wrapper)

elif name == 'top_lidar':
topic = self.ros_sensor_topics[name]
Expand All @@ -180,12 +158,6 @@ def callback_with_numpy(msg: PointCloud2):
callback(points)
self.top_lidar_sub = rospy.Subscriber(topic, PointCloud2, callback_with_numpy)

elif name == 'imu':
topic = self.ros_sensor_topics[name]
if type is not None and type is not Imu:
raise ValueError("GEMGazeboInterface only supports Imu for IMU data")
self.imu_sub = rospy.Subscriber(topic, Imu, callback)

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):
Expand Down Expand Up @@ -222,7 +194,7 @@ def send_command(self, command : GEMVehicleCommand):
# Skip command, similar to hardware interface
return
self.last_command_time = t

# Get current speed
v = self.last_reading.speed

Expand All @@ -231,36 +203,36 @@ def send_command(self, command : GEMVehicleCommand):
self.last_reading.accelerator_pedal_position = command.accelerator_pedal_position
self.last_reading.brake_pedal_position = command.brake_pedal_position
self.last_reading.steering_wheel_angle = command.steering_wheel_angle

# Convert pedal to acceleration
accelerator_pedal_position = np.clip(command.accelerator_pedal_position, 0.0, 1.0)
brake_pedal_position = np.clip(command.brake_pedal_position, 0.0, 1.0)

# Zero out accelerator if brake is active (just like hardware interface)
if brake_pedal_position > 0.0:
accelerator_pedal_position = 0.0

# Calculate acceleration from pedal positions
acceleration = pedal_positions_to_acceleration(accelerator_pedal_position, brake_pedal_position, v, 0, 1)

# Apply reasonable limits to acceleration
max_accel = settings.get('vehicle.limits.max_acceleration', 1.0)
max_decel = settings.get('vehicle.limits.max_deceleration', -2.0)
acceleration = np.clip(acceleration, max_decel, max_accel)

# Convert wheel angle to steering angle (front wheel angle)
phides = steer2front(command.steering_wheel_angle)

# Apply steering angle limits
min_wheel_angle = settings.get('vehicle.geometry.min_wheel_angle', -0.6)
max_wheel_angle = settings.get('vehicle.geometry.max_wheel_angle', 0.6)
phides = np.clip(phides, min_wheel_angle, max_wheel_angle)

# Calculate target speed based on acceleration
# Don't use infinite speed, instead calculate a reasonable target speed
current_speed = v
target_speed = current_speed

if acceleration > 0:
# Accelerating - set target speed to current speed plus some increment
# This is more realistic than infinite speed
Expand All @@ -270,16 +242,16 @@ def send_command(self, command : GEMVehicleCommand):
# Braking - set target speed to zero if deceleration is significant
if brake_pedal_position > 0.1:
target_speed = 0.0

# Create and publish drive message
msg = AckermannDrive()
msg.acceleration = acceleration
msg.speed = target_speed
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}")

self.ackermann_pub.publish(msg)
self.last_command = command
self.last_command = command
Loading