From 086f7ada26846c371ba5d5561e9e9988dabb164c Mon Sep 17 00:00:00 2001 From: danielzhuang11 Date: Sat, 5 Apr 2025 21:08:40 -0500 Subject: [PATCH 1/5] added send_command --- GEMstack/onboard/interface/gem_gazebo.py | 442 +++++++++++++---------- 1 file changed, 247 insertions(+), 195 deletions(-) diff --git a/GEMstack/onboard/interface/gem_gazebo.py b/GEMstack/onboard/interface/gem_gazebo.py index 96f64967a..11654aa50 100644 --- a/GEMstack/onboard/interface/gem_gazebo.py +++ b/GEMstack/onboard/interface/gem_gazebo.py @@ -14,7 +14,8 @@ from dataclasses import replace from threading import Thread,Lock - +from ackermann_msgs.msg import AckermannDrive +import roslaunch # ROS Headers import rospy from std_msgs.msg import String, Bool, Float32, Float64 @@ -131,151 +132,151 @@ def seek_target(self,target,dt): self.position = transforms.vector_madd(self.position,self.velocity,dt) -class GEMDoubleIntegratorSimulation: - """Standard simulation of a second-order Dubins car with a double - integrator controller. The simulation is deterministic and accepts - GEMVehicleReading and GEMVehicleCommand objects. - - Gear switching is instantaneous. Signals are activated instantly. - """ - def __init__(self, scene : str = None): - self.dubins = SecondOrderDubinsCar( - wheelAngleMin=settings.get('vehicle.geometry.min_wheel_angle'), - wheelAngleMax=settings.get('vehicle.geometry.max_wheel_angle'), - velocityMin=-settings.get('vehicle.limits.max_reverse_speed'), - velocityMax=settings.get('vehicle.limits.max_speed'), - accelMin=-settings.get('vehicle.limits.max_acceleration'), - accelMax=settings.get('vehicle.limits.max_deceleration'), - wheelAngleRateMin=-settings.get('vehicle.limits.max_steering_rate'), - wheelAngleRateMax=settings.get('vehicle.limits.max_steering_rate'), - wheelBase=settings.get('vehicle.geometry.wheelbase')) +# class GEMDoubleIntegratorSimulation: +# """Standard simulation of a second-order Dubins car with a double +# integrator controller. The simulation is deterministic and accepts +# GEMVehicleReading and GEMVehicleCommand objects. + +# Gear switching is instantaneous. Signals are activated instantly. +# """ +# def __init__(self, scene : str = None): +# self.dubins = SecondOrderDubinsCar( +# wheelAngleMin=settings.get('vehicle.geometry.min_wheel_angle'), +# wheelAngleMax=settings.get('vehicle.geometry.max_wheel_angle'), +# velocityMin=-settings.get('vehicle.limits.max_reverse_speed'), +# velocityMax=settings.get('vehicle.limits.max_speed'), +# accelMin=-settings.get('vehicle.limits.max_acceleration'), +# accelMax=settings.get('vehicle.limits.max_deceleration'), +# wheelAngleRateMin=-settings.get('vehicle.limits.max_steering_rate'), +# wheelAngleRateMax=settings.get('vehicle.limits.max_steering_rate'), +# wheelBase=settings.get('vehicle.geometry.wheelbase')) - self.dt = settings.get('simulator.dt',0.01) - self.roadgraph = None - self.agents = [] - if scene is None: - scene = settings.get('simulator.scene',None) - if isinstance(scene,str): - print("Loading simulator from scene",scene) - scene = config.load_config_recursive(scene) - self.agents = {} - if scene is None: - self.simulation_time = time.time() - self.start_state = (0.0,0.0,0.0) - else: - self.simulation_time = scene.get('time',time.time()) - start_state = scene.get('vehicle_state',[0.0,0.0,0.0,0.0,0.0]) - while len(start_state) < 5: - start_state.append(0.0) - for k,a in scene.get('agents',{}).items(): - self.agents[k] = AgentSimulation(a) - self.cur_vehicle_state = np.array(start_state,dtype=float) - - self.last_reading = GEMVehicleReading() - self.last_reading.speed = 0.0 - self.last_reading.steering_wheel_angle = 0.0 - self.last_reading.accelerator_pedal_position = 0.0 - self.last_reading.brake_pedal_position = 0.0 - self.last_reading.gear = 0 - self.last_reading.left_turn_signal = False - self.last_reading.right_turn_signal = False - self.last_reading.horn_on = False - self.last_reading.wiper_level = 0 - self.last_reading.headlights_on = False - #initialize last command - gear = -2 if self.cur_vehicle_state[3] == 0 else -1 if self.cur_vehicle_state[3] < 0 else 1 - steering_wheel_angle = front2steer(self.cur_vehicle_state[4]) - self.last_command = GEMVehicleCommand(gear,0,0,0,0,steering_wheel_angle,0) +# self.dt = settings.get('simulator.dt',0.01) +# self.roadgraph = None +# self.agents = [] +# if scene is None: +# scene = settings.get('simulator.scene',None) +# if isinstance(scene,str): +# print("Loading simulator from scene",scene) +# scene = config.load_config_recursive(scene) +# self.agents = {} +# if scene is None: +# self.simulation_time = time.time() +# self.start_state = (0.0,0.0,0.0) +# else: +# self.simulation_time = scene.get('time',time.time()) +# start_state = scene.get('vehicle_state',[0.0,0.0,0.0,0.0,0.0]) +# while len(start_state) < 5: +# start_state.append(0.0) +# for k,a in scene.get('agents',{}).items(): +# self.agents[k] = AgentSimulation(a) +# self.cur_vehicle_state = np.array(start_state,dtype=float) + +# self.last_reading = GEMVehicleReading() +# self.last_reading.speed = 0.0 +# self.last_reading.steering_wheel_angle = 0.0 +# self.last_reading.accelerator_pedal_position = 0.0 +# self.last_reading.brake_pedal_position = 0.0 +# self.last_reading.gear = 0 +# self.last_reading.left_turn_signal = False +# self.last_reading.right_turn_signal = False +# self.last_reading.horn_on = False +# self.last_reading.wiper_level = 0 +# self.last_reading.headlights_on = False +# #initialize last command +# gear = -2 if self.cur_vehicle_state[3] == 0 else -1 if self.cur_vehicle_state[3] < 0 else 1 +# steering_wheel_angle = front2steer(self.cur_vehicle_state[4]) +# self.last_command = GEMVehicleCommand(gear,0,0,0,0,steering_wheel_angle,0) - def time(self) -> float: - return self.simulation_time - - def simulate(self, T : float, command : Optional[GEMVehicleCommand]): - if command is not None: - self.last_command = command - for k,a in self.agents.items(): - a.advance(T) - x,y,theta,v,phi = self.cur_vehicle_state - #print("x %.2f y %.2f theta %.2f v %.2f" % (x,y,theta,v)) - #simulate actuators - accelerator_pedal_position = np.clip(self.last_command.accelerator_pedal_position,0.0,1.0) - brake_pedal_position = np.clip(self.last_command.brake_pedal_position,0.0,1.0) - acceleration = pedal_positions_to_acceleration(accelerator_pedal_position,brake_pedal_position,v,0,1) - acceleration = np.clip(acceleration,*self.dubins.accelRange) - phides = steer2front(self.last_command.steering_wheel_angle) - phides = np.clip(phides,*self.dubins.wheelAngleRange) - h = 0.01 #just for finite differencing - front_wheel_angle_rate = (steer2front(self.last_command.steering_wheel_angle + h*self.last_command.steering_wheel_speed) - steer2front(self.last_command.steering_wheel_angle)) / h - front_wheel_angle_rate_max = 2.0 #TODO: get from vehicle model - phi_deadband = 2*self.dt*front_wheel_angle_rate_max - steering_angle_rate = front_wheel_angle_rate if phides > phi + phi_deadband else \ - (-front_wheel_angle_rate if phides < phi - phi_deadband else 0.0) +# def time(self) -> float: +# return self.simulation_time + +# def simulate(self, T : float, command : Optional[GEMVehicleCommand]): +# if command is not None: +# self.last_command = command +# for k,a in self.agents.items(): +# a.advance(T) +# x,y,theta,v,phi = self.cur_vehicle_state +# #print("x %.2f y %.2f theta %.2f v %.2f" % (x,y,theta,v)) +# #simulate actuators +# accelerator_pedal_position = np.clip(self.last_command.accelerator_pedal_position,0.0,1.0) +# brake_pedal_position = np.clip(self.last_command.brake_pedal_position,0.0,1.0) +# acceleration = pedal_positions_to_acceleration(accelerator_pedal_position,brake_pedal_position,v,0,1) +# acceleration = np.clip(acceleration,*self.dubins.accelRange) +# phides = steer2front(self.last_command.steering_wheel_angle) +# phides = np.clip(phides,*self.dubins.wheelAngleRange) +# h = 0.01 #just for finite differencing +# front_wheel_angle_rate = (steer2front(self.last_command.steering_wheel_angle + h*self.last_command.steering_wheel_speed) - steer2front(self.last_command.steering_wheel_angle)) / h +# front_wheel_angle_rate_max = 2.0 #TODO: get from vehicle model +# phi_deadband = 2*self.dt*front_wheel_angle_rate_max +# steering_angle_rate = front_wheel_angle_rate if phides > phi + phi_deadband else \ +# (-front_wheel_angle_rate if phides < phi - phi_deadband else 0.0) - #simulate dynamics - u = np.array([acceleration,steering_angle_rate]) #acceleration, steering angle rate - #print("Accel %.2f, steering angle current %.2f, desired %.2f, rate %.2f" % (acceleration,phi,phides,steering_angle_rate)) - next = simulate(self.dubins, self.cur_vehicle_state, (lambda x,t: u), T, self.dt) - next_state = next['x'][-1] - #braking deadband - if v > 0 and next_state[3] < 0: - next_state[3] = 0 - if v < 0 and next_state[3] > 0: - next_state[3] = 0 - x,y,theta,v,phi = next_state - v = np.clip(v,*self.dubins.velocityRange) - next_state = np.array([x,y,theta,v,phi]) - - #simulate sensors - reading = copy.copy(self.last_reading) - reading.steering_wheel_angle = front2steer(phi) - if acceleration > 0: - reading.brake_pedal_position = 0.0 - reading.accelerator_pedal_position = accelerator_pedal_position - else: - reading.brake_pedal_position = brake_pedal_position - reading.accelerator_pedal_position = 0 - reading.speed = v - if v > 0: - reading.gear = 1 - else: - reading.gear = -1 - #copy signals - reading.left_turn_signal = self.last_command.left_turn_signal - reading.right_turn_signal = self.last_command.right_turn_signal - reading.headlights_on = self.last_command.headlights_on - reading.horn_on = self.last_command.horn_on - reading.wiper_level = self.last_command.wiper_level - self.last_reading = reading - - self.cur_vehicle_state = next_state - self.simulation_time += T - - def pose(self) -> ObjectPose: - x,y,theta,v,phi = self.cur_vehicle_state - return ObjectPose(frame=ObjectFrameEnum.ABSOLUTE_CARTESIAN,t=self.simulation_time,x=x,y=y,yaw=theta) - - def state(self) -> VehicleState: - return self.last_reading.to_state(self.pose()) - - def set_pose(self,pose : ObjectPose): - self.cur_vehicle_state[0] = pose.x - self.cur_vehicle_state[1] = pose.y - self.cur_vehicle_state[2] = pose.yaw +# #simulate dynamics +# u = np.array([acceleration,steering_angle_rate]) #acceleration, steering angle rate +# #print("Accel %.2f, steering angle current %.2f, desired %.2f, rate %.2f" % (acceleration,phi,phides,steering_angle_rate)) +# next = simulate(self.dubins, self.cur_vehicle_state, (lambda x,t: u), T, self.dt) +# next_state = next['x'][-1] +# #braking deadband +# if v > 0 and next_state[3] < 0: +# next_state[3] = 0 +# if v < 0 and next_state[3] > 0: +# next_state[3] = 0 +# x,y,theta,v,phi = next_state +# v = np.clip(v,*self.dubins.velocityRange) +# next_state = np.array([x,y,theta,v,phi]) + +# #simulate sensors +# reading = copy.copy(self.last_reading) +# reading.steering_wheel_angle = front2steer(phi) +# if acceleration > 0: +# reading.brake_pedal_position = 0.0 +# reading.accelerator_pedal_position = accelerator_pedal_position +# else: +# reading.brake_pedal_position = brake_pedal_position +# reading.accelerator_pedal_position = 0 +# reading.speed = v +# if v > 0: +# reading.gear = 1 +# else: +# reading.gear = -1 +# #copy signals +# reading.left_turn_signal = self.last_command.left_turn_signal +# reading.right_turn_signal = self.last_command.right_turn_signal +# reading.headlights_on = self.last_command.headlights_on +# reading.horn_on = self.last_command.horn_on +# reading.wiper_level = self.last_command.wiper_level +# self.last_reading = reading + +# self.cur_vehicle_state = next_state +# self.simulation_time += T + +# def pose(self) -> ObjectPose: +# x,y,theta,v,phi = self.cur_vehicle_state +# return ObjectPose(frame=ObjectFrameEnum.ABSOLUTE_CARTESIAN,t=self.simulation_time,x=x,y=y,yaw=theta) + +# def state(self) -> VehicleState: +# return self.last_reading.to_state(self.pose()) + +# def set_pose(self,pose : ObjectPose): +# self.cur_vehicle_state[0] = pose.x +# self.cur_vehicle_state[1] = pose.y +# self.cur_vehicle_state[2] = pose.yaw - def set_state(self,state : VehicleState): - self.set_pose(state.pose) - self.cur_vehicle_state[3] = state.v - self.cur_vehicle_state[4] = state.front_wheel_angle +# def set_state(self,state : VehicleState): +# self.set_pose(state.pose) +# self.cur_vehicle_state[3] = state.v +# self.cur_vehicle_state[4] = state.front_wheel_angle - def advance_vehicle_state(self, state : AllState, command : GEMVehicleCommand, T : float) -> AllState: - """Advances the vehicle state by the given amount of time T - under the given command. Agents are not touched. - """ - self.simulation_time = state.t - abs_pose = state.vehicle.to_frame(ObjectFrameEnum.ABSOLUTE_CARTESIAN, state.vehicle.pose, state.start_vehicle_pose) - self.set_state(abs_pose) - self.simulate(T, command) - return replace(state,t=self.simulation_time,vehicle=self.state()) +# def advance_vehicle_state(self, state : AllState, command : GEMVehicleCommand, T : float) -> AllState: +# """Advances the vehicle state by the given amount of time T +# under the given command. Agents are not touched. +# """ +# self.simulation_time = state.t +# abs_pose = state.vehicle.to_frame(ObjectFrameEnum.ABSOLUTE_CARTESIAN, state.vehicle.pose, state.start_vehicle_pose) +# self.set_state(abs_pose) +# self.simulate(T, command) +# return replace(state,t=self.simulation_time,vehicle=self.state()) @@ -288,7 +289,7 @@ class GEMDoubleIntegratorSimulationInterface(GEMInterface): """ def __init__(self, scene : str = None): GEMInterface.__init__(self) - self.simulator = GEMDoubleIntegratorSimulation(scene) + #self.simulator = GEMDoubleIntegratorSimulation(scene) self.real_time_multiplier = settings.get('simulator.real_time_multiplier',1.0) self.gnss_emulator_settings = settings.get('simulator.gnss_emulator',{}) self.imu_emulator_settings = settings.get('simulator.imu_emulator',{}) @@ -297,7 +298,7 @@ def __init__(self, scene : str = None): self.imu_dt = self.imu_emulator_settings.get('dt',0.05) self.agent_dt = self.agent_emulator_settings.get('dt',0.1) # self.last_reading = self.simulator.last_reading - # self.last_command = self.simulator.last_command + # self.last_command =GEMRealSensorsWithSimMotionInterface self.simulator.last_command self.last_gnss_time = 0 self.last_imu_time = 0 self.last_agent_time = 0 @@ -336,41 +337,45 @@ def __init__(self, scene : str = None): self.faults = [] - # -------------------- PACMod setup -------------------- - # GEM vehicle enable - self.enable_pub = rospy.Publisher('/pacmod/as_rx/enable', Bool, queue_size=1) - self.pacmod_enable = False - - # GEM vehicle gear control, neutral, forward and reverse, publish once - self.gear_pub = rospy.Publisher('/pacmod/as_rx/shift_cmd', PacmodCmd, queue_size=1) - self.gear_cmd = PacmodCmd() - self.gear_cmd.enable = True - self.gear_cmd.ui16_cmd = PacmodCmd.SHIFT_NEUTRAL - - # GEM vehicle brake control - self.brake_pub = rospy.Publisher('/pacmod/as_rx/brake_cmd', PacmodCmd, queue_size=1) - self.brake_cmd = PacmodCmd() - self.brake_cmd.enable = False - self.brake_cmd.clear = True - self.brake_cmd.ignore = True - - # GEM vehicle forward motion control - self.accel_pub = rospy.Publisher('/pacmod/as_rx/accel_cmd', PacmodCmd, queue_size=1) - self.accel_cmd = PacmodCmd() - self.accel_cmd.enable = False - self.accel_cmd.clear = True - self.accel_cmd.ignore = True - - # GEM vehicle turn signal control - self.turn_pub = rospy.Publisher('/pacmod/as_rx/turn_cmd', PacmodCmd, queue_size=1) - self.turn_cmd = PacmodCmd() - self.turn_cmd.ui16_cmd = 1 # None - - # GEM vechile steering wheel control - self.steer_pub = rospy.Publisher('/pacmod/as_rx/steer_cmd', PositionWithSpeed, queue_size=1) - self.steer_cmd = PositionWithSpeed() - self.steer_cmd.angular_position = 0.0 # radians, -: clockwise, +: counter-clockwise - self.steer_cmd.angular_velocity_limit = 2.0 # radians/second + # # -------------------- PACMod setup -------------------- + # # GEM vehicle enable + # self.enable_pub = rospy.Publisher('/pacmod/as_rx/enable', Bool, queue_size=1) + # self.pacmod_enable = False + + # # GEM vehicle gear control, neutral, forward and reverse, publish once + # self.gear_pub = rospy.Publisher('/pacmod/as_rx/shift_cmd', PacmodCmd, queue_size=1) + # self.gear_cmd = PacmodCmd() + # self.gear_cmd.enable = True + # self.gear_cmd.ui16_cmd = PacmodCmd.SHIFT_NEUTRAL + + # # GEM vehicle brake control + # self.brake_pub = rospy.Publisher('/pacmod/as_rx/brake_cmd', PacmodCmd, queue_size=1) + # self.brake_cmd = PacmodCmd() + # self.brake_cmd.enable = False + # self.brake_cmd.clear = True + # self.brake_cmd.ignore = True + + # # GEM vehicle forward motion control + # self.accel_pub = rospy.Publisher('/pacmod/as_rx/accel_cmd', PacmodCmd, queue_size=1) + # self.accel_cmd = PacmodCmd() + # self.accel_cmd.enable = False + # self.accel_cmd.clear = True + # self.accel_cmd.ignore = True + + # # GEM vehicle turn signal control + # self.turn_pub = rospy.Publisher('/pacmod/as_rx/turn_cmd', PacmodCmd, queue_size=1) + # self.turn_cmd = PacmodCmd() + # self.turn_cmd.ui16_cmd = 1 # None + + # # GEM vechile steering wheel control + # self.steer_pub = rospy.Publisher('/pacmod/as_rx/steer_cmd', PositionWithSpeed, queue_size=1) + # self.steer_cmd = PositionWithSpeed() + # self.steer_cmd.angular_position = 0.0 # radians, -: clockwise, +: counter-clockwise + # self.steer_cmd.angular_velocity_limit = 2.0 # radians/second + + + self.ackermann_pub = rospy.Publisher("ackermann_cmd", AckermannDrive, queue_size=1) + # Need to double check on this time @@ -412,10 +417,17 @@ def start(self): # self.thread_data['stop'] = False # self.thread = Thread(target=self.simulate,args=(self.thread_lock,self.thread_data)) # self.thread.start() - print("ENABLING PACMOD") - enable_cmd = Bool() - enable_cmd.data = True - self.enable_pub.publish(enable_cmd) + # rospy.init_node("gazebo_launcher", anonymous=True) + # uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) + # roslaunch.configure_logging(uuid) + # launch = roslaunch.parent.ROSLaunchParent( + # uuid, ["/path/to/your/launch/file.launch"] + # ) + # launch.start() + # rospy.loginfo("Gazebo started via Python launch.") + + # rospy.spin() + pass def stop(self): print("Stopping simulator thread...") @@ -514,26 +526,66 @@ def callback_with_cv2(msg : Image): def send_command(self, command : GEMVehicleCommand): + + + t = self.time() + if t < self.last_command_time + 1.0/self.max_send_rate: + #skip command, PACMod can't handle commands this fast + return + self.last_command_time = t + + #x,y,theta,v,phi = self.cur_vehicle_state + + phi = ????? + + 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) + acceleration = pedal_positions_to_acceleration(accelerator_pedal_position,brake_pedal_position,v,0,1) + acceleration = np.clip(acceleration,*self.dubins.accelRange) + phides = steer2front(command.steering_wheel_angle) + phides = np.clip(phides,*self.dubins.wheelAngleRange) + h = 0.01 #just for finite differencing + front_wheel_angle_rate = (steer2front(command.steering_wheel_angle + h*command.steering_wheel_speed) - steer2front(command.steering_wheel_angle)) / h + front_wheel_angle_rate_max = 2.0 #TODO: get from vehicle model + phi_deadband = 2*self.dt*front_wheel_angle_rate_max + steering_angle_rate = front_wheel_angle_rate if phides > phi + phi_deadband else \ + (-front_wheel_angle_rate if phides < phi - phi_deadband else 0.0) + + if command.brake_pedal_position > 0.0: + acceleration = 0.0 + + msg = AckermannDrive() + msg.acceleration = acceleration + msg.speed = acceleration * self.dt #float('inf') if acceleration >0 else float('-inf') + msg.steering_angle = command.steering_wheel_angle + msg.steering_angle_velocity = steering_angle_rate + + + self.ackermann_pub.publish(msg) + rospy.loginfo(f"Published AckermannDrive: {msg}") self.last_command = command + + + # incorporate controller publisher def get_reading(self) -> GEMVehicleReading: """Returns current read state of the vehicle""" return self.last_reading - # PACMod enable callback function - def pacmod_enable_callback(self, msg): - if self.pacmod_enable == False and msg.data == True: - print("PACMod enabled, enabling gear, brake, accel, steer, and turn") - self.send_first_command() - elif self.pacmod_enable == True and msg.data == False: - print("PACMod disabled") - self.pacmod_enable = msg.data - - def hardware_faults(self) -> List[str]: - if self.pacmod_enable == False: - return self.faults + ["disengaged"] - return self.faults + # # PACMod enable callback function + # def pacmod_enable_callback(self, msg): + # if self.pacmod_enable == False and msg.data == True: + # print("PACMod enabled, enabling gear, brake, accel, steer, and turn") + # self.send_first_command() + # elif self.pacmod_enable == True and msg.data == False: + # print("PACMod disabled") + # self.pacmod_enable = msg.data + + # def hardware_faults(self) -> List[str]: + # if self.pacmod_enable == False: + # return self.faults + ["disengaged"] + # return self.faults # def simulate(self,lock : Lock, data : dict): # looper = TimedLooper(self.simulator.dt / self.real_time_multiplier,name="Simulation thread") From 454bc7efb57fb3ef92a9255c29f4be85b68413b3 Mon Sep 17 00:00:00 2001 From: danielzhuang11 Date: Sat, 5 Apr 2025 21:19:58 -0500 Subject: [PATCH 2/5] small fixes --- GEMstack/onboard/interface/gem_gazebo.py | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/GEMstack/onboard/interface/gem_gazebo.py b/GEMstack/onboard/interface/gem_gazebo.py index 11654aa50..90fc3ef39 100644 --- a/GEMstack/onboard/interface/gem_gazebo.py +++ b/GEMstack/onboard/interface/gem_gazebo.py @@ -336,6 +336,17 @@ def __init__(self, scene : str = None): self.stereo_sub = None self.faults = [] + self.dubins = SecondOrderDubinsCar( + wheelAngleMin=settings.get('vehicle.geometry.min_wheel_angle'), + wheelAngleMax=settings.get('vehicle.geometry.max_wheel_angle'), + velocityMin=-settings.get('vehicle.limits.max_reverse_speed'), + velocityMax=settings.get('vehicle.limits.max_speed'), + accelMin=-settings.get('vehicle.limits.max_acceleration'), + accelMax=settings.get('vehicle.limits.max_deceleration'), + wheelAngleRateMin=-settings.get('vehicle.limits.max_steering_rate'), + wheelAngleRateMax=settings.get('vehicle.limits.max_steering_rate'), + wheelBase=settings.get('vehicle.geometry.wheelbase')) + # # -------------------- PACMod setup -------------------- # # GEM vehicle enable @@ -538,6 +549,8 @@ def send_command(self, command : GEMVehicleCommand): phi = ????? + + 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) acceleration = pedal_positions_to_acceleration(accelerator_pedal_position,brake_pedal_position,v,0,1) @@ -557,7 +570,7 @@ def send_command(self, command : GEMVehicleCommand): msg = AckermannDrive() msg.acceleration = acceleration msg.speed = acceleration * self.dt #float('inf') if acceleration >0 else float('-inf') - msg.steering_angle = command.steering_wheel_angle + msg.steering_angle = phides msg.steering_angle_velocity = steering_angle_rate From b5a2f39d58dbcdc72c1c380d4433f4d8407fac00 Mon Sep 17 00:00:00 2001 From: danielzhuang11 Date: Sat, 5 Apr 2025 23:53:51 -0500 Subject: [PATCH 3/5] IT MOVES --- GEMstack/onboard/interface/gem_gazebo.py | 60 ++++++++++++++++++++---- launch/gazebo_simulation.yaml | 13 ++++- 2 files changed, 63 insertions(+), 10 deletions(-) diff --git a/GEMstack/onboard/interface/gem_gazebo.py b/GEMstack/onboard/interface/gem_gazebo.py index 90fc3ef39..ac077dbb7 100644 --- a/GEMstack/onboard/interface/gem_gazebo.py +++ b/GEMstack/onboard/interface/gem_gazebo.py @@ -14,6 +14,9 @@ from dataclasses import replace from threading import Thread,Lock +from gazebo_msgs.srv import GetModelState, GetModelStateResponse +from gazebo_msgs.msg import ModelState + from ackermann_msgs.msg import AckermannDrive import roslaunch # ROS Headers @@ -88,6 +91,7 @@ def __init__(self, config): self.start = self.position[:] self.yaw = config.get('yaw',0) + def to_agent_state(self) -> AgentState: pose = ObjectPose(frame=ObjectFrameEnum.ABSOLUTE_CARTESIAN,t=time.time(),x=self.position[0],y=self.position[1],yaw=self.yaw) activity = AgentActivityEnum.MOVING if self.velocity[0] != 0 or self.velocity[1] != 0 else AgentActivityEnum.STOPPED @@ -130,6 +134,7 @@ def seek_target(self,target,dt): v -= AGENT_NOMINAL_ACCELERATION[self.type]*dt self.velocity = [v*direction[0],v*direction[1]] self.position = transforms.vector_madd(self.position,self.velocity,dt) + # class GEMDoubleIntegratorSimulation: @@ -335,6 +340,8 @@ def __init__(self, scene : str = None): self.top_lidar_sub = None self.stereo_sub = None self.faults = [] + self.dt = settings.get('simulator.dt',0.01) + self.dubins = SecondOrderDubinsCar( wheelAngleMin=settings.get('vehicle.geometry.min_wheel_angle'), @@ -386,7 +393,21 @@ def __init__(self, scene : str = None): self.ackermann_pub = rospy.Publisher("ackermann_cmd", AckermannDrive, queue_size=1) - + def getModelState(self): + # Get the current state of the vehicle + # Input: None + # Output: ModelState, the state of the vehicle, contain the + # position, orientation, linear velocity, angular velocity + # of the vehicle + rospy.wait_for_service('/gazebo/get_model_state') + try: + serviceResponse = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) + resp = serviceResponse(model_name='gem_e4') + except rospy.ServiceException as exc: + rospy.loginfo("Service did not process request: "+str(exc)) + resp = GetModelStateResponse() + resp.success = False + return resp # Need to double check on this time @@ -538,16 +559,37 @@ def callback_with_cv2(msg : Image): def send_command(self, command : GEMVehicleCommand): - + print(command) t = self.time() if t < self.last_command_time + 1.0/self.max_send_rate: #skip command, PACMod can't handle commands this fast return self.last_command_time = t - #x,y,theta,v,phi = self.cur_vehicle_state - - phi = ????? + def extract_vehicle_info( currentPose): + def quaternion_to_euler(x, y, z, w): + t0 = +2.0 * (w * x + y * z) + t1 = +1.0 - 2.0 * (x * x + y * y) + roll = np.arctan2(t0, t1) + t2 = +2.0 * (w * y - z * x) + t2 = +1.0 if t2 > +1.0 else t2 + t2 = -1.0 if t2 < -1.0 else t2 + pitch = np.arcsin(t2) + t3 = +2.0 * (w * z + x * y) + t4 = +1.0 - 2.0 * (y * y + z * z) + yaw = np.arctan2(t3, t4) + return [roll, pitch, yaw] + pos_x, pos_y, vel, yaw = 0, 0, 0, 0 + + pos_x = currentPose.pose.position.x + pos_y = currentPose.pose.position.y + vel = np.linalg.norm([currentPose.twist.linear.x, currentPose.twist.linear.y, currentPose.twist.linear.z]) + _,_, yaw = quaternion_to_euler(currentPose.pose.orientation.x, currentPose.pose.orientation.y, currentPose.pose.orientation.z, currentPose.pose.orientation.w) + + + return pos_x, pos_y, vel, yaw # note that yaw is in radian + #what is phi>> + x,y,v,phi = extract_vehicle_info(self.getModelState()) @@ -564,12 +606,14 @@ def send_command(self, command : GEMVehicleCommand): steering_angle_rate = front_wheel_angle_rate if phides > phi + phi_deadband else \ (-front_wheel_angle_rate if phides < phi - phi_deadband else 0.0) - if command.brake_pedal_position > 0.0: - acceleration = 0.0 + # if command.brake_pedal_position > 0.0: + # acceleration = 0.0 + + msg = AckermannDrive() msg.acceleration = acceleration - msg.speed = acceleration * self.dt #float('inf') if acceleration >0 else float('-inf') + msg.speed = float('inf') if acceleration >0 else float('-inf') #acceleration * self.dt msg.steering_angle = phides msg.steering_angle_velocity = steering_angle_rate diff --git a/launch/gazebo_simulation.yaml b/launch/gazebo_simulation.yaml index 06c30dbfa..9983c9c43 100644 --- a/launch/gazebo_simulation.yaml +++ b/launch/gazebo_simulation.yaml @@ -13,7 +13,7 @@ recovery: drive: perception: state_estimation : GNSSStateEstimator - agent_detection : object_detection.ObjectDetection + #agent_detection : object_detection.ObjectDetection perception_normalization : StandardPerceptionNormalizer planning: # relations_estimation: pedestrian_yield_logic.PedestrianYielder @@ -73,7 +73,16 @@ variants: # state_estimation : OmniscientStateEstimator # agent_detection : OmniscientAgentDetector planning: - trajectory_tracking: + route_planning: + type: StaticRoutePlanner + args: [!relative_path '../GEMstack/knowledge/routes/xyhead_highbay_backlot_p.csv'] + motion_planning: + type: RouteToTrajectoryPlanner + args: [null] #desired speed in m/s. If null, this will keep the route untimed for the trajectory tracker + trajectory_tracking: + type: pure_pursuit.PurePursuitTrajectoryTracker + args: {desired_speed: 2.5} #approximately 5mph + print: False # visualization: !include "klampt_visualization.yaml" # visualization: !include "mpl_visualization.yaml" log_ros: From bc6ee356d8a94e0305022f5e00a3ceb836ba0e04 Mon Sep 17 00:00:00 2001 From: danielzhuang11 Date: Sun, 6 Apr 2025 23:24:06 -0500 Subject: [PATCH 4/5] steering is reversed for some reason --- GEMstack/knowledge/routes/gazebo.csv | 32 ++++++++++++++++++++++++ GEMstack/onboard/interface/gem_gazebo.py | 16 ++++++------ launch/gazebo_simulation.yaml | 12 ++++----- 3 files changed, 47 insertions(+), 13 deletions(-) create mode 100644 GEMstack/knowledge/routes/gazebo.csv diff --git a/GEMstack/knowledge/routes/gazebo.csv b/GEMstack/knowledge/routes/gazebo.csv new file mode 100644 index 000000000..ba9be5313 --- /dev/null +++ b/GEMstack/knowledge/routes/gazebo.csv @@ -0,0 +1,32 @@ +-0.030000000000000027 , 0.0 +-0.1499999999999999 , 0.010000000000001563 +-0.45999999999999996 , 0.010000000000001563 +-0.9300000000000002 , 0.019999999999999574 +-1.7200000000000002 , 0.030000000000001137 +-3.7699999999999996 , 0.05000000000000071 +-4.98 , 0.05999999999999872 +-7.029999999999999 , 0.0799999999999983 +-8.33 , 0.08999999999999986 +-9.84 , 0.10999999999999943 +-12.61 , 0.14000000000000057 +-14.14 , 0.14999999999999858 +-15.510000000000002 , 0.1700000000000017 +-18.48 , 0.21000000000000085 +-19.85 , 0.23000000000000043 +-21.07 , 0.23999999999999844 +-22.54 , 0.26000000000000156 +-24.29 , 0.23000000000000043 +-26.47 , -0.05000000000000071 +-28.84 , -0.8399999999999999 +-31.049999999999997 , -2.34 +-32.37 , -4.870000000000001 +-32.12 , -7.75 +-30.13 , -9.96 +-27.68 , -10.510000000000002 +-25.19 , -9.71 +-23.15 , -7.629999999999999 +-21.85 , -4.699999999999999 +-21.25 , -0.9699999999999989 +-21.86 , 0.7800000000000011 +-21.94 , 1.0700000000000003 +-21.91 , 0.8500000000000014 diff --git a/GEMstack/onboard/interface/gem_gazebo.py b/GEMstack/onboard/interface/gem_gazebo.py index ac077dbb7..0300744b5 100644 --- a/GEMstack/onboard/interface/gem_gazebo.py +++ b/GEMstack/onboard/interface/gem_gazebo.py @@ -46,6 +46,8 @@ + + AGENT_DIMENSIONS = { 'pedestrian' : (0.5,0.5,1.6), 'bicyclist' : (1.8,0.5,1.6), @@ -559,7 +561,7 @@ def callback_with_cv2(msg : Image): def send_command(self, command : GEMVehicleCommand): - print(command) + rospy.loginfo(f"got command: {command}") t = self.time() if t < self.last_command_time + 1.0/self.max_send_rate: #skip command, PACMod can't handle commands this fast @@ -588,11 +590,11 @@ def quaternion_to_euler(x, y, z, w): return pos_x, pos_y, vel, yaw # note that yaw is in radian - #what is phi>> - x,y,v,phi = extract_vehicle_info(self.getModelState()) - + _,_,v,phi = extract_vehicle_info(self.getModelState()) + + 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) acceleration = pedal_positions_to_acceleration(accelerator_pedal_position,brake_pedal_position,v,0,1) @@ -613,9 +615,9 @@ def quaternion_to_euler(x, y, z, w): msg = AckermannDrive() msg.acceleration = acceleration - msg.speed = float('inf') if acceleration >0 else float('-inf') #acceleration * self.dt - msg.steering_angle = phides - msg.steering_angle_velocity = steering_angle_rate + msg.speed = float('inf') if acceleration >0 else 0 #acceleration * self.dt + msg.steering_angle = -phides + msg.steering_angle_velocity = -steering_angle_rate self.ackermann_pub.publish(msg) diff --git a/launch/gazebo_simulation.yaml b/launch/gazebo_simulation.yaml index 9983c9c43..1a811ebc2 100644 --- a/launch/gazebo_simulation.yaml +++ b/launch/gazebo_simulation.yaml @@ -14,7 +14,6 @@ drive: perception: state_estimation : GNSSStateEstimator #agent_detection : object_detection.ObjectDetection - perception_normalization : StandardPerceptionNormalizer planning: # relations_estimation: pedestrian_yield_logic.PedestrianYielder # route_planning: @@ -66,16 +65,17 @@ variants: vehicle_interface: type: gem_gazebo.GEMDoubleIntegratorSimulationInterface args: - scene: !relative_path '../scenes/xyhead_demo.yaml' + scene: !relative_path '../scenes/gazebo.yaml' drive: - # perception: - # state_estimation : OmniscientStateEstimator - # agent_detection : OmniscientAgentDetector + perception: + state_estimation : GNSSStateEstimator + perception_normalization : StandardPerceptionNormalizer + planning: route_planning: type: StaticRoutePlanner - args: [!relative_path '../GEMstack/knowledge/routes/xyhead_highbay_backlot_p.csv'] + args: [!relative_path '../GEMstack/knowledge/routes/gazebo.csv'] motion_planning: type: RouteToTrajectoryPlanner args: [null] #desired speed in m/s. If null, this will keep the route untimed for the trajectory tracker From 2e71f7609f75902057f890cf1138ee7a375ac395 Mon Sep 17 00:00:00 2001 From: danielzhuang11 Date: Mon, 7 Apr 2025 10:29:49 -0500 Subject: [PATCH 5/5] added some TODO messages --- GEMstack/onboard/interface/gem_gazebo.py | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/GEMstack/onboard/interface/gem_gazebo.py b/GEMstack/onboard/interface/gem_gazebo.py index 0300744b5..84205ede8 100644 --- a/GEMstack/onboard/interface/gem_gazebo.py +++ b/GEMstack/onboard/interface/gem_gazebo.py @@ -444,7 +444,7 @@ def global_callback(self, msg): if msg.subsystem_can_timeout: self.faults.append("subsystem_can_timeout") - # Maybe need to change for controls? idk + #TODO: Start gazebo def start(self): # assert self.thread is None # print("Running simulator thread...") @@ -562,13 +562,10 @@ def callback_with_cv2(msg : Image): def send_command(self, command : GEMVehicleCommand): rospy.loginfo(f"got command: {command}") - t = self.time() - if t < self.last_command_time + 1.0/self.max_send_rate: - #skip command, PACMod can't handle commands this fast - return - self.last_command_time = t def extract_vehicle_info( currentPose): + + #TODO: replace with already proviced function and make sure convention is correct def quaternion_to_euler(x, y, z, w): t0 = +2.0 * (w * x + y * z) t1 = +1.0 - 2.0 * (x * x + y * y) @@ -585,7 +582,13 @@ def quaternion_to_euler(x, y, z, w): pos_x = currentPose.pose.position.x pos_y = currentPose.pose.position.y + vel = np.linalg.norm([currentPose.twist.linear.x, currentPose.twist.linear.y, currentPose.twist.linear.z]) + + #TODO: figure out velocity sign Is this correct + + # if currentPose.twist.linear.x<0: + # vel = -vel _,_, yaw = quaternion_to_euler(currentPose.pose.orientation.x, currentPose.pose.orientation.y, currentPose.pose.orientation.z, currentPose.pose.orientation.w) @@ -608,8 +611,6 @@ def quaternion_to_euler(x, y, z, w): steering_angle_rate = front_wheel_angle_rate if phides > phi + phi_deadband else \ (-front_wheel_angle_rate if phides < phi - phi_deadband else 0.0) - # if command.brake_pedal_position > 0.0: - # acceleration = 0.0