Skip to content

Commit f086752

Browse files
harishkumarbalajiharishkumarbalaji
committed
fix for pacmod2
1 parent 5f785f7 commit f086752

2 files changed

Lines changed: 90 additions & 90 deletions

File tree

‎GEMstack/knowledge/defaults/standard_ros_topics.yaml‎

Lines changed: 16 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -13,20 +13,20 @@
1313
- /zed2/zed_node/imu/data
1414
- /zed2/zed_node/imu/data_raw
1515
- /zed2/zed_node/imu/mag
16-
- /pacmod/as_rx/enable
17-
- /pacmod/as_rx/accel_cmd
18-
- /pacmod/as_rx/brake_cmd
19-
- /pacmod/as_rx/shift_cmd
20-
- /pacmod/as_rx/steer_cmd
21-
- /pacmod/as_rx/turn_cmd
22-
- /pacmod/as_rx/turn_signal_cmd
23-
- /pacmod/as_rx/vehicle_speed_cmd
24-
- /pacmod/as_tx/enable
25-
- /pacmod/as_tx/global_rpt
26-
- /pacmod/as_tx/accel_rpt
27-
- /pacmod/as_tx/brake_rpt
28-
- /pacmod/as_tx/shift_rpt
29-
- /pacmod/as_tx/steer_rpt
30-
- /pacmod/as_tx/turn_rpt
31-
- /pacmod/as_tx/vehicle_speed_rpt
16+
# - /pacmod/enabled
17+
- /pacmod/accel_cmd
18+
- /pacmod/brake_cmd
19+
- /pacmod/shift_cmd
20+
- /pacmod/steering_cmd
21+
- /pacmod/turn_cmd
22+
- /pacmod/turn_signal_cmd
23+
- /pacmod/vehicle_speed_cmd
24+
- /pacmod/enabled
25+
- /pacmod/global_rpt
26+
- /pacmod/accel_rpt
27+
- /pacmod/brake_rpt
28+
- /pacmod/shift_rpt
29+
- /pacmod/steering_rpt
30+
- /pacmod/turn_rpt
31+
- /pacmod/vehicle_speed_rpt
3232

‎GEMstack/onboard/interface/gem_hardware.py‎

Lines changed: 74 additions & 74 deletions
Original file line numberDiff line numberDiff line change
@@ -26,7 +26,7 @@
2626
from tf_transformations import euler_from_quaternion, quaternion_from_euler
2727

2828
# GEM PACMod Headers
29-
from pacmod2_msgs.msg import PositionWithSpeed, PacmodCmd, SystemRptFloat, VehicleSpeedRpt, GlobalRpt
29+
from pacmod2_msgs.msg import PositionWithSpeed, PacmodCmd, SystemRptFloat, VehicleSpeedRpt, GlobalRpt, SystemCmdInt, SystemCmdFloat
3030

3131
# OpenCV and cv2 bridge
3232
import cv2
@@ -62,9 +62,9 @@ def __init__(self):
6262
self.last_reading.wiper_level = 0
6363
self.last_reading.headlights_on = False
6464

65-
self.speed_sub = self.node.create_subscription(VehicleSpeedRpt, "/pacmod/parsed_tx/vehicle_speed_rpt", self.speed_callback, qos_profile)
66-
self.steer_sub = self.node.create_subscription(SystemRptFloat, "/pacmod/parsed_tx/steer_rpt", self.steer_callback, qos_profile)
67-
self.global_sub = self.node.create_subscription(GlobalRpt, "/pacmod/parsed_tx/global_rpt", self.global_callback, qos_profile)
65+
self.speed_sub = self.node.create_subscription(VehicleSpeedRpt, "/pacmod/vehicle_speed_rpt", self.speed_callback, qos_profile)
66+
self.steer_sub = self.node.create_subscription(SystemRptFloat, "/pacmod/steering_rpt", self.steer_callback, qos_profile)
67+
self.global_sub = self.node.create_subscription(GlobalRpt, "/pacmod/global_rpt", self.global_callback, qos_profile)
6868
self.gnss_sub = None
6969
self.imu_sub = None
7070
self.front_radar_sub = None
@@ -76,50 +76,50 @@ def __init__(self):
7676

7777
# -------------------- PACMod setup --------------------
7878
# GEM vehicle enable
79-
self.enable_pub = self.node.create_publisher(Bool, '/pacmod/as_rx/enable', 1)
80-
self.pacmod_enable = False
79+
# self.enable_pub = self.node.create_publisher(Bool, '/pacmod/enable', 1)
80+
self.pacmod_enable = True
8181

8282
# GEM vehicle gear control, neutral, forward and reverse, publish once
83-
self.gear_pub = self.node.create_publisher(PacmodCmd, '/pacmod/as_rx/shift_cmd', 1)
84-
self.gear_cmd = PacmodCmd()
85-
self.gear_cmd.enable = True
86-
self.gear_cmd.ui16_cmd = PacmodCmd.SHIFT_NEUTRAL
83+
self.gear_pub = self.node.create_publisher(SystemCmdInt, '/pacmod/shift_cmd', 1)
84+
self.gear_cmd = SystemCmdInt()
85+
# self.gear_cmd.enable = True
86+
self.gear_cmd.command = SystemCmdInt.SHIFT_NEUTRAL
8787

8888
# GEM vehicle brake control
89-
self.brake_pub = self.node.create_publisher(PacmodCmd, '/pacmod/as_rx/brake_cmd', 1)
90-
self.brake_cmd = PacmodCmd()
91-
self.brake_cmd.enable = False
92-
self.brake_cmd.clear = True
93-
self.brake_cmd.ignore = True
89+
self.brake_pub = self.node.create_publisher(SystemCmdFloat, '/pacmod/brake_cmd', 1)
90+
self.brake_cmd = SystemCmdFloat()
91+
# self.brake_cmd.enable = False
92+
# self.brake_cmd.clear = True
93+
# self.brake_cmd.ignore = True
9494

9595
# GEM vehicle forward motion control
96-
self.accel_pub = self.node.create_publisher(PacmodCmd, '/pacmod/as_rx/accel_cmd', 1)
97-
self.accel_cmd = PacmodCmd()
98-
self.accel_cmd.enable = False
99-
self.accel_cmd.clear = True
100-
self.accel_cmd.ignore = True
96+
self.accel_pub = self.node.create_publisher(SystemCmdFloat, '/pacmod/accel_cmd', 1)
97+
self.accel_cmd = SystemCmdFloat()
98+
# self.accel_cmd.enable = False
99+
# self.accel_cmd.clear = True
100+
# self.accel_cmd.ignore = True
101101

102102
# GEM vehicle turn signal control
103-
self.turn_pub = self.node.create_publisher(PacmodCmd, '/pacmod/as_rx/turn_cmd', 1)
104-
self.turn_cmd = PacmodCmd()
105-
self.turn_cmd.ui16_cmd = 1 # None
103+
self.turn_pub = self.node.create_publisher(SystemCmdInt, '/pacmod/turn_cmd', 1)
104+
self.turn_cmd = SystemCmdInt()
105+
self.turn_cmd.command = 1 # None
106106

107107
# GEM vechile steering wheel control
108-
self.steer_pub = self.node.create_publisher(PositionWithSpeed, '/pacmod/as_rx/steer_cmd', 1)
108+
self.steer_pub = self.node.create_publisher(PositionWithSpeed, '/pacmod/steering_cmd', 1)
109109
self.steer_cmd = PositionWithSpeed()
110110
self.steer_cmd.angular_position = 0.0 # radians, -: clockwise, +: counter-clockwise
111111
self.steer_cmd.angular_velocity_limit = 2.0 # radians/second
112112

113113
"""TODO: other commands
114-
/pacmod/as_rx/headlight_cmd
115-
/pacmod/as_rx/horn_cmd
116-
/pacmod/as_rx/wiper_cmd
114+
/pacmod/headlight_cmd
115+
/pacmod/horn_cmd
116+
/pacmod/wiper_cmd
117117
"""
118118

119119
#TODO: publish TwistStamped to /front_radar/front_radar/vehicle_motion to get better radar tracks
120120

121121
#subscribers should go last because the callback might be called before the object is initialized
122-
self.enable_sub = self.node.create_subscription(Bool, '/pacmod/as_tx/enable', self.pacmod_enable_callback, qos_profile)
122+
# self.enable_sub = self.node.create_subscription(Bool, '/pacmod/enabled', self.pacmod_enable_callback, qos_profile)
123123
executor = MultiThreadedExecutor()
124124
executor.add_node(self.node)
125125
executor_thread = threading.Thread(target=executor.spin, daemon=True)
@@ -133,7 +133,7 @@ def start(self):
133133
print("ENABLING PACMOD")
134134
enable_cmd = Bool()
135135
enable_cmd.data = True
136-
self.enable_pub.publish(enable_cmd)
136+
# self.enable_pub.publish(enable_cmd)
137137
#this doesn't seem to work super well, need to send enable command multiple times
138138

139139
def time(self):
@@ -150,20 +150,20 @@ def global_callback(self, msg):
150150
self.faults = []
151151
if msg.override_active:
152152
self.faults.append("override_active")
153-
if msg.config_fault_active:
154-
self.faults.append("config_fault_active")
155-
if msg.user_can_timeout:
153+
# if msg.config_fault_active:
154+
# self.faults.append("config_fault_active")
155+
if msg.usr_can_timeout:
156156
self.faults.append("user_can_timeout")
157157
if msg.user_can_read_errors:
158158
self.faults.append("user_can_read_errors")
159-
if msg.brake_can_timeout:
159+
if msg.brk_can_timeout:
160160
self.faults.append("brake_can_timeout")
161-
if msg.steering_can_timeout:
161+
if msg.str_can_timeout:
162162
self.faults.append("steering_can_timeout")
163-
if msg.vehicle_can_timeout:
163+
if msg.veh_can_timeout:
164164
self.faults.append("vehicle_can_timeout")
165-
if msg.subsystem_can_timeout:
166-
self.faults.append("subsystem_can_timeout")
165+
# if msg.subsystem_can_timeout:
166+
# self.faults.append("subsystem_can_timeout")
167167

168168
def get_reading(self) -> GEMVehicleReading:
169169
return self.last_reading
@@ -265,13 +265,13 @@ def callback_with_cv2(msg : Image):
265265

266266

267267
# PACMod enable callback function
268-
def pacmod_enable_callback(self, msg):
269-
if self.pacmod_enable == False and msg.data == True:
270-
print("PACMod enabled, enabling gear, brake, accel, steer, and turn")
271-
self.send_first_command()
272-
elif self.pacmod_enable == True and msg.data == False:
273-
print("PACMod disabled")
274-
self.pacmod_enable = msg.data
268+
# def pacmod_enable_callback(self, msg):
269+
# if self.pacmod_enable == False and msg.data == True:
270+
# print("PACMod enabled, enabling gear, brake, accel, steer, and turn")
271+
# self.send_first_command()
272+
# elif self.pacmod_enable == True and msg.data == False:
273+
# print("PACMod disabled")
274+
# self.pacmod_enable = msg.data
275275

276276
def hardware_faults(self) -> List[str]:
277277
if self.pacmod_enable == False:
@@ -282,22 +282,22 @@ def send_first_command(self):
282282
# ---------- Enable PACMod ----------
283283

284284
# enable forward gear
285-
self.gear_cmd.enable = True
286-
self.gear_cmd.ui16_cmd = PacmodCmd.SHIFT_FORWARD
285+
# self.gear_cmd.enable = True
286+
self.gear_cmd.command = SystemCmdInt.SHIFT_FORWARD
287287
#helps debug whether gear command is being sent since you'll hear the backup beep
288288
#self.gear_cmd.ui16_cmd = PacmodCmd.SHIFT_REVERSE
289289

290290
# enable brake
291-
self.brake_cmd.enable = True
292-
self.brake_cmd.clear = False
293-
self.brake_cmd.ignore = False
294-
self.brake_cmd.f64_cmd = 0.0
291+
# self.brake_cmd.enable = True
292+
# self.brake_cmd.clear = False
293+
# self.brake_cmd.ignore = False
294+
self.brake_cmd.command = 0.0
295295

296296
# enable gas
297-
self.accel_cmd.enable = True
298-
self.accel_cmd.clear = False
299-
self.accel_cmd.ignore = False
300-
self.accel_cmd.f64_cmd = 0.0
297+
# self.accel_cmd.enable = True
298+
# self.accel_cmd.clear = False
299+
# self.accel_cmd.ignore = False
300+
self.accel_cmd.command = 0.0
301301

302302
self.gear_pub.publish(self.gear_cmd)
303303
self.turn_pub.publish(self.turn_cmd)
@@ -314,44 +314,44 @@ def send_command(self, command : GEMVehicleCommand):
314314
self.last_command_time = t
315315

316316
if command.left_turn_signal and command.right_turn_signal:
317-
self.turn_cmd.ui16_cmd = PacmodCmd.TURN_HAZARDS
317+
self.turn_cmd.command = SystemCmdInt.TURN_HAZARDS
318318
elif command.left_turn_signal:
319-
self.turn_cmd.ui16_cmd = PacmodCmd.TURN_LEFT
319+
self.turn_cmd.command = SystemCmdInt.TURN_LEFT
320320
elif command.right_turn_signal:
321-
self.turn_cmd.ui16_cmd = PacmodCmd.TURN_RIGHT
321+
self.turn_cmd.command = SystemCmdInt.TURN_RIGHT
322322
else:
323-
self.turn_cmd.ui16_cmd = PacmodCmd.TURN_NONE
323+
self.turn_cmd.command = SystemCmdInt.TURN_NONE
324324

325-
self.accel_cmd.f64_cmd = command.accelerator_pedal_position
325+
self.accel_cmd.command = command.accelerator_pedal_position
326326
if command.brake_pedal_position > 0.0:
327-
self.accel_cmd.f64_cmd = 0.0
327+
self.accel_cmd.command = 0.0
328328
print("command.brake_pedal_position",command.brake_pedal_position)
329-
self.brake_cmd.f64_cmd = float(command.brake_pedal_position)
329+
self.brake_cmd.command = float(command.brake_pedal_position)
330330
self.steer_cmd.angular_position = command.steering_wheel_angle
331331
self.steer_cmd.angular_velocity_limit = command.steering_wheel_speed
332332
print("**************************")
333333
print("Steer cmd angular position {} velocity limit {}".format(self.steer_cmd.angular_position,self.steer_cmd.angular_velocity_limit))
334-
print("Accel pedal position {} brake position {}".format(self.accel_cmd.f64_cmd,self.brake_cmd.f64_cmd))
334+
print("Accel pedal position {} brake position {}".format(self.accel_cmd.command,self.brake_cmd.command))
335335
maxacc = settings.get('vehicle.limits.max_accelerator_pedal')
336336
maxbrake = settings.get('vehicle.limits.max_brake_pedal')
337-
if self.accel_cmd.f64_cmd > maxacc:
337+
if self.accel_cmd.command > maxacc:
338338
print("Warning: commanded acceleration exceeded accel pedal limit")
339-
self.accel_cmd.f64_cmd = maxacc
340-
if self.brake_cmd.f64_cmd > maxbrake:
339+
self.accel_cmd.command = maxacc
340+
if self.brake_cmd.command > maxbrake:
341341
print("Warning: commanded braking exceeded brake pedal limit")
342-
self.brake_cmd.f64_cmd = maxbrake
342+
self.brake_cmd.command = maxbrake
343343
print("**************************")
344344

345-
self.brake_cmd.enable = True
346-
self.brake_cmd.clear = False
347-
self.brake_cmd.ignore = False
345+
# self.brake_cmd.enable = True
346+
# self.brake_cmd.clear = False
347+
# self.brake_cmd.ignore = False
348348

349-
self.accel_cmd.enable = True
350-
self.accel_cmd.clear = False
351-
self.accel_cmd.ignore = False
349+
# self.accel_cmd.enable = True
350+
# self.accel_cmd.clear = False
351+
# self.accel_cmd.ignore = False
352352

353-
self.gear_cmd.ui16_cmd = PacmodCmd.SHIFT_FORWARD
354-
self.gear_cmd.enable = True
353+
self.gear_cmd.command = SystemCmdInt.SHIFT_FORWARD
354+
# self.gear_cmd.enable = True
355355
self.gear_pub.publish(self.gear_cmd)
356356
self.accel_pub.publish(self.accel_cmd)
357357
self.brake_pub.publish(self.brake_cmd)

0 commit comments

Comments
 (0)