Skip to content

Commit a052e29

Browse files
danielzhuang11danielzhuang11
committed
documentation (praveen) + longitudinal control (daniel)
1 parent a5cb7e4 commit a052e29

2 files changed

Lines changed: 119 additions & 82 deletions

File tree

‎GEMstack/onboard/interface/gem_gazebo.py‎

Lines changed: 37 additions & 82 deletions
Original file line numberDiff line numberDiff line change
@@ -4,14 +4,19 @@
44

55
# ROS Headers
66
import rospy
7-
from std_msgs.msg import String, Bool, Float32, Float64
87
from sensor_msgs.msg import Image, PointCloud2, Imu, NavSatFix
8+
from geometry_msgs.msg import Vector3Stamped
99
from sensor_msgs.msg import JointState # For reading joint states from Gazebo
1010
# Changed from AckermannDriveStamped
1111
from ackermann_msgs.msg import AckermannDrive
1212
from rosgraph_msgs.msg import Clock
1313
from tf.transformations import euler_from_quaternion
1414

15+
16+
from ...state import ObjectPose,ObjectFrameEnum
17+
from ...knowledge.vehicle.geometry import steer2front
18+
from ...knowledge.vehicle.dynamics import pedal_positions_to_acceleration
19+
1520
# OpenCV and cv2 bridge
1621
import cv2
1722
import numpy as np
@@ -39,16 +44,16 @@ def __init__(self):
3944
self.last_reading.steering_wheel_angle = 0.0
4045
self.last_reading.accelerator_pedal_position = 0.0
4146
self.last_reading.brake_pedal_position = 0.0
42-
self.last_reading.gear = 0
47+
self.last_reading.gear = 1
4348
self.last_reading.left_turn_signal = False
4449
self.last_reading.right_turn_signal = False
4550
self.last_reading.horn_on = False
4651
self.last_reading.wiper_level = 0
4752
self.last_reading.headlights_on = False
4853

49-
# Gazebo joint state subscriber for wheel positions/velocities
50-
self.joint_state_sub = rospy.Subscriber(
51-
"/joint_states", JointState, self.joint_state_callback)
54+
55+
56+
5257

5358
# IMU data subscriber
5459
self.imu_sub = None
@@ -73,6 +78,9 @@ def __init__(self):
7378
# Subscribe to IMU topic by default
7479
self.imu_sub = rospy.Subscriber("/imu", Imu, self.imu_callback)
7580

81+
# Subscribe to GNSS Velocitu
82+
self.gnss_vel_sub = rospy.Subscriber("/gps/fix_velocity", Vector3Stamped, self.gnss_vel_callback)
83+
7684
# Add clock subscription for simulation time
7785
self.sim_time = rospy.Time(0)
7886
self.clock_sub = rospy.Subscriber('/clock', Clock, self.clock_callback)
@@ -87,42 +95,15 @@ def time(self):
8795
# Return Gazebo simulation time
8896
return self.sim_time.to_sec()
8997

90-
def joint_state_callback(self, msg: JointState):
91-
# Extract steering and speed from joint states
92-
# This implementation depends on the specific joint names in your Gazebo model
93-
# Typically find steering joints and wheel rotation joints
94-
try:
95-
# Match your simulation's joint names from rostopic list
96-
if 'left_steering_joint' in msg.name: # Changed from front_left_steering_joint
97-
idx = msg.name.index('left_steering_joint')
98-
self.last_reading.steering_wheel_angle = msg.position[idx]
99-
# Added debug
100-
print(f"[JOINT] Steering angle: {msg.position[idx]:.3f} rad")
101-
102-
# Update wheel joint names to match your simulation
103-
wheel_radius = 0.2
104-
rear_wheel_indices = []
105-
for name in ['left_rear_wheel_joint', 'right_rear_wheel_joint']: # Updated names
106-
if name in msg.name:
107-
rear_wheel_indices.append(msg.name.index(name))
108-
109-
if rear_wheel_indices:
110-
# Average rear wheel velocities to get vehicle speed
111-
avg_wheel_vel = sum(
112-
msg.velocity[i] for i in rear_wheel_indices) / len(rear_wheel_indices)
113-
self.last_reading.speed = avg_wheel_vel * \
114-
wheel_radius # Convert to linear velocity
115-
# Added debug
116-
print(
117-
f"[JOINT] Wheel speed: {self.last_reading.speed:.2f} m/s")
118-
except (ValueError, IndexError) as e:
119-
rospy.logwarn(f"Error processing joint states: {e}")
120-
12198
def imu_callback(self, msg: Imu):
12299
self.imu_data = msg
123100

101+
def gnss_vel_callback(self, msg):
102+
self.last_reading.speed = np.linalg.norm([msg.vector.x, msg.vector.y] )
103+
124104
def get_reading(self) -> GEMVehicleReading:
125105
return self.last_reading
106+
126107

127108
def subscribe_sensor(self, name, callback, type=None):
128109
if name == 'gnss':
@@ -169,7 +150,7 @@ def gnss_callback_wrapper(gps_msg: NavSatFix):
169150
# Create GNSS reading with fused data
170151
reading = GNSSReading(
171152
pose=pose,
172-
speed=self.last_reading.speed,
153+
speed= self.last_reading.speed,
173154
status='FIX' if gps_msg.status.status >= 0 else 'NO_FIX'
174155
)
175156
# Added debug
@@ -232,48 +213,22 @@ def hardware_faults(self) -> List[str]:
232213
# In simulation, we don't have real hardware faults
233214
return self.faults
234215

235-
def send_command(self, command: GEMVehicleCommand):
236-
# throttle rate at which we send commands
237-
t = self.time()
238-
if t < self.last_command_time + 1.0/self.max_send_rate:
239-
# skip command, Gazebo can't handle commands this fast
240-
return
241-
self.last_command_time = t
242-
243-
# Create an AckermannDrive message
244-
self.ackermann_cmd.steering_angle = steer2front(
245-
command.steering_wheel_angle)
246-
247-
# Calculate linear/speed from accelerator/brake
248-
# For simplicity, map 0-1 accelerator to 0-max_speed
249-
max_speed = 3.0 # m/s
250-
251-
# In gazebo we use speed instead of acceleration
252-
desired_speed = 0.0
253-
if command.accelerator_pedal_position > 0.0:
254-
desired_speed = command.accelerator_pedal_position * max_speed
255-
256-
# Apply brake (reduce speed)
257-
if command.brake_pedal_position > 0.0:
258-
brake_factor = 1.0 - command.brake_pedal_position
259-
desired_speed *= max(0.0, brake_factor)
260-
261-
self.ackermann_cmd.speed = desired_speed
262-
263-
# Set acceleration limit if needed
264-
acceleration_limit = 2.0 # m/s²
265-
self.ackermann_cmd.acceleration = acceleration_limit
266-
267-
# Set jerk/steering_angle_velocity if needed
268-
self.ackermann_cmd.steering_angle_velocity = command.steering_wheel_speed
269-
270-
# Print debug info
271-
print("**************************")
272-
print(
273-
f"Gazebo command: speed={desired_speed:.2f} m/s, steering={self.ackermann_cmd.steering_angle:.2f} rad")
274-
print("**************************")
275-
276-
# Publish the command
277-
self.ackermann_pub.publish(self.ackermann_cmd)
278-
279-
self.last_command = command
216+
def send_command(self, command : GEMVehicleCommand):
217+
218+
v = self.last_reading.speed
219+
# convert pedal to acceleration
220+
accelerator_pedal_position = np.clip(command.accelerator_pedal_position,0.0,1.0)
221+
brake_pedal_position = np.clip(command.brake_pedal_position,0.0,1.0)
222+
acceleration = pedal_positions_to_acceleration(accelerator_pedal_position,brake_pedal_position,v,0,1)
223+
acceleration = np.clip(acceleration,-2, 1)
224+
# convert wheel angle to steering angle
225+
phides = steer2front(command.steering_wheel_angle)
226+
227+
#create and publish drive message
228+
msg = AckermannDrive()
229+
msg.acceleration = acceleration
230+
msg.speed = float('inf') if acceleration >0 else 0 #acceleration * self.dt
231+
msg.steering_angle = phides
232+
233+
self.ackermann_pub.publish(msg)
234+
self.last_command = command

‎docs/Gazebo Simulation Documentation.md‎

Lines changed: 82 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,82 @@
1+
# Gazebo Simulation Documentation
2+
3+
## Available Sensors in Gazebo
4+
5+
The following sensors are currently available in the Gazebo simulation environment:
6+
7+
- **Front Camera**
8+
- **GNSS**
9+
- **Lidar**
10+
11+
---
12+
13+
## Gazebo Simulation Setup
14+
15+
To run your code within the Gazebo simulator, you'll first need to set up and install necessary dependencies.
16+
17+
### 1. Set Up the Simulator
18+
19+
Go to this [POLARIS GEM Simulator](https://github.com/harishkumarbalaji/POLARIS_GEM_Simulator/tree/main) repository to set up the Gazebo Docker container and environment for simulation.
20+
21+
Follow the instructions in the linked repo to build and run the Docker container. It also provides a simulation environment of highbay.
22+
23+
### 2. Install Dependencies
24+
25+
Install the required ROS packages:
26+
27+
```bash
28+
sudo apt-get install -y ros-noetic-ackermann-msgs
29+
```
30+
31+
---
32+
33+
## Configuring Your Code for Simulation
34+
35+
To run your code with the Gazebo simulation, modify your launch file according to the following steps:
36+
37+
1. **Add a new variant:**
38+
Under the `variants` section, create a new variant called `"gazebo"`.
39+
40+
2. **Set the vehicle interface:**
41+
In the `"gazebo"` variant, set the `vehicle_interface` to use the Gazebo interface:
42+
```yaml
43+
vehicle_interface:
44+
type: gem_gazebo.GEMGazeboInterface
45+
```
46+
47+
3. **Add modifications (optional):**
48+
You can add any other configuration or parameters needed for your testing under the `"gazebo"` variant.
49+
50+
Setting the `vehicle_interface` to `gem_gazebo.GEMGazeboInterface` will automatically link the **GEMStack** sensor topics with the corresponding **Gazebo vehicle sensors**, allowing you to test your code in the simulation environment.
51+
52+
## Example Launch File
53+
54+
For a full example, refer to the [`launch/fixed_route.yaml`](launch/fixed_route.yaml) file, which includes a sample `gazebo` variant configuration.
55+
56+
---
57+
58+
## Running Gazebo
59+
60+
Follow these steps to run your GEMStack code with Gazebo:
61+
62+
### 1. Launch the Gazebo Simulator
63+
64+
In one terminal, run the Gazebo simulator (using the instructions provided in the [POLARIS GEM Simulator](https://github.com/harishkumarbalaji/POLARIS_GEM_Simulator/tree/main) repository). This will load the simulation environment with the vehicle and sensors.
65+
66+
### 2. Launch the GEM Stack
67+
68+
Open a second terminal and launch GEMStack with your configured launch file.
69+
70+
```bash
71+
python3 main.py --variant=gazebo --vehicle=e4_gazebo launch/{your_file}.yaml
72+
```
73+
- Make sure to set the variant to `gazebo`.
74+
- You can specify the vehicle type to be `e2_gazebo` or `e4_gazebo`.
75+
76+
Example command launching the fixed route with e4 vehicle:
77+
78+
```bash
79+
python3 main.py --variant=gazebo --vehicle=e4_gazebo launch/fixed_route.yaml
80+
```
81+
82+
---

0 commit comments

Comments
 (0)