def control_command_updated(self, ros_vehicle_control, manual_override):
"""
Receive a CarlaEgoVehicleControl msg and send to CARLA
This function gets called whenever a ROS CarlaEgoVehicleControl is received.
If the mode is valid (either normal or manual), the received ROS message is
converted into carla.VehicleControl command and sent to CARLA.
This bridge is not responsible for any restrictions on velocity or steering.
It's just forwarding the ROS input to CARLA
:param manual_override: manually override the vehicle control command
:param ros_vehicle_control: current vehicle control input received via ROS
:type ros_vehicle_control: carla_msgs.msg.CarlaEgoVehicleControl
:return:
"""
if manual_override == self.vehicle_control_override:
vehicle_control = VehicleControl()
vehicle_control.hand_brake = ros_vehicle_control.hand_brake
vehicle_control.brake = ros_vehicle_control.brake
vehicle_control.steer = ros_vehicle_control.steer
vehicle_control.throttle = ros_vehicle_control.throttle
vehicle_control.reverse = ros_vehicle_control.reverse
vehicle_control.manual_gear_shift = ros_vehicle_control.manual_gear_shift
vehicle_control.gear = ros_vehicle_control.gear
self.carla_actor.apply_control(vehicle_control)
self._vehicle_control_applied_callback(self.get_id())
def step_from_control(
self,
frame: int,
vehicle_control: carla.VehicleControl,
apply: bool = True,
update: bool = True) -> None:
throttle_value = vehicle_control.throttle
if apply:
vehicle_control.manual_gear_shift = False
if throttle_value < 0.4:
vehicle_control.throttle = 0.4 # avoid stopping
# todo: implement PID controller
if self.data_frame_number is not None and self.data_frame_number in self.data_frame_dict:
velocity = self.data_frame_dict[self.data_frame_number].state.velocity
speed = 3.6 * math.sqrt(velocity.x ** 2 + velocity.y ** 2 + velocity.z ** 2)
logger.info('speed {:+5.3f}'.format(speed))
if speed > 20:
vehicle_control.throttle = 0.0
self.vehicle.apply_control(vehicle_control)
if update:
car_control = CarControl.load_from_vehicle_control(vehicle_control)
car_control.throttle = throttle_value
control_with_info = ControlWithInfo(control=car_control, road_option=RoadOption.VOID)
car_state = self.fetch_car_state()
drive_data_frame = DriveDataFrame(car_state, control_with_info)
self.data_frame_number = frame
self.data_frame_dict[self.data_frame_number] = drive_data_frame
def read_control_command(control_stream):
# Wait until the control is set.
while True:
# Read the control command from the control stream.
control_msg = control_stream.read()
if not isinstance(control_msg, erdos.WatermarkMessage):
# We have read a control message. Return the command
# so that the leaderboard can tick the simulator.
output_control = VehicleControl()
output_control.throttle = control_msg.throttle
output_control.brake = control_msg.brake
output_control.steer = control_msg.steer
output_control.reverse = control_msg.reverse
output_control.hand_brake = control_msg.hand_brake
output_control.manual_gear_shift = False
return output_control
def run_step(self, input_data, timestamp):
game_time = int(timestamp * 1000)
self._logger.debug("Current game time {}".format(game_time))
erdos_timestamp = erdos.Timestamp(coordinates=[game_time])
if not self._sent_open_drive:
# We do not have access to the open drive map. Send top watermark.
self._sent_open_drive = True
self._open_drive_stream.send(
erdos.WatermarkMessage(erdos.Timestamp(is_top=True)))
self.send_waypoints_msg(erdos_timestamp)
for key, val in input_data.items():
if key == 'ground_objects':
self.send_ground_objects(val[1], erdos_timestamp)
elif key == 'scene_layout':
self.send_scene_layout(val[1], erdos_timestamp)
elif key == 'can_bus':
self.send_pose_msg(val[1], erdos_timestamp)
elif key == 'gnss':
self.send_gnss_data(val[1], erdos_timestamp)
else:
self._logger.warning("Sensor {} not used".format(key))
# The agent doesn't send any lanes, but send watermarks to ensure
# computation progress.
self._lanes_stream.send(erdos.WatermarkMessage(erdos_timestamp))
# Wait until the control is set.
while True:
control_msg = self._control_stream.read()
if not isinstance(control_msg, erdos.WatermarkMessage):
output_control = VehicleControl()
output_control.throttle = control_msg.throttle
output_control.brake = control_msg.brake
output_control.steer = control_msg.steer
output_control.reverse = control_msg.reverse
output_control.hand_brake = control_msg.hand_brake
output_control.manual_gear_shift = False
return output_control
