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 compute_noise(self, action, speed_kmh=20):
if self.use_tick:
self.tick += 0.2
if self.noise_type == 'None':
return action
if self.noise_type == 'Spike':
if self.is_time_for_noise():
minmax = lambda x: max(x, min(x, 1.0), -1.0)
steer_noisy = minmax(action.steer + self.get_noise() *
(30 / (1.5 * speed_kmh + 5)))
if __CARLA_VERSION__.startswith("0.9"):
noisy_action = VehicleControl(action.throttle,
action.steer, action.brake,
action.hand_brake,
action.reverse)
else:
noisy_action = copy.deepcopy(action)
noisy_action.steer = steer_noisy
return noisy_action
else:
return action
raise ValueError("invalid noisy type: " + self.noise_type)
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 run_step(self, input_data, timestamp):
if not self.initialized:
self._init()
tick_data = self.tick(input_data)
control = VehicleControl()
control.steer = 0
control.throttle = 1.0
control.brake = False
return control
def run(self, condition):
for i in range(test_steps):
if __CARLA_VERSION__.startswith('0.9.5'):
self._world.tick()
self._world.wait_for_tick(10.0)
data_buffer_lock.acquire()
image_dict = copy.deepcopy(self._data_buffers)
data_buffer_lock.release()
if len(image_dict) < 3:
print("image dict has len ", len(image_dict), " elements, continuing")
continue
action, to_be_viz = self.compute_control(image_dict, condition)
if False:
self.show_image_dict_on_screen(image_dict)
if cv2.waitKey(25) & 0xFF == ord('q'):
break
self.save_to_disk(to_be_viz)
vc = VehicleControl(float(action.throttle),
float(action.steer) * pid_p,
float(action.brake),
bool(action.hand_brake),
bool(action.reverse))
self._vehicle.apply_control(vc)
if i % 10 == 0:
print(i)
self.destroy()
def control_command_updated(self):
"""
Receive a CarlaEgoVehicleControl msg and send to CARLA
This function gets called whenever a icv CarlaEgoVehicleControl is received.
If the mode is valid (either normal or manual), the received icv 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 icv input to CARLA
:param manual_override: manually override the vehicle control command
:param icv_vehicle_control: current vehicle control input received via icv
:type icv_vehicle_control: carla_msgs.msg.CarlaEgoVehicleControl
:return:
"""
if self.vehicle_control_override:
sub2.subscribe(self.manual_control_subscriber)
icv_vehicle_control = self.manual_control_subscriber
else:
sub1.subscribe(self.control_subscriber)
icv_vehicle_control = self.control_subscriber
vehicle_control = VehicleControl()
vehicle_control.hand_brake = icv_vehicle_control.hand_brake
vehicle_control.brake = icv_vehicle_control.brake
vehicle_control.steer = icv_vehicle_control.steer
vehicle_control.throttle = icv_vehicle_control.throttle
vehicle_control.reverse = icv_vehicle_control.reverse
self.carla_actor.apply_control(vehicle_control)
self._vehicle_control_applied_callback(self.get_id())
def predict_control(self, sensor_data):
rgb_array = sensor_data['CenterRGB'].copy()
# numpy shape should be (H x W x C) in range [0,255] with dtype=np.uint8
image = Image.fromarray(rgb_array)
transform = get_transform()
x = transform(image)
with torch.no_grad(): # reduce mem usage and speed up computation
y = self.pilotnet(x.unsqueeze(
0)) # TODO: Ew. Do I have to add a batch dimension?
predicted_steer = y.item()
control = VehicleControl()
control.throttle = 0.5
control.steer = predicted_steer
return control
def _apply_control_msg(self, msg: ControlMessage):
# Transform the message to a simulator control cmd.
vec_control = VehicleControl(throttle=msg.throttle,
steer=msg.steer,
brake=msg.brake,
hand_brake=msg.hand_brake,
reverse=msg.reverse)
self._client.apply_batch_sync(
[command.ApplyVehicleControl(self._ego_vehicle.id, vec_control)])
def cyber_control_command_updated(self, cyber_vehicle_control):
self.control_mode = True
self.carla_actor.set_simulate_physics(True)
vehicle_control = VehicleControl()
vehicle_control.hand_brake = cyber_vehicle_control.parking_brake
vehicle_control.brake = cyber_vehicle_control.brake / 100.0
vehicle_control.steer = cyber_vehicle_control.steering_target / 100.0
vehicle_control.throttle = cyber_vehicle_control.throttle / 100.0
vehicle_control.reverse = cyber_vehicle_control.gear_location == Chassis.GearPosition.GEAR_REVERSE
self.carla_actor.apply_control(vehicle_control)
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 apply_control(self):
"""
Apply current control output to CARLA
:return:
"""
vehicle_control = VehicleControl()
vehicle_control.hand_brake = self.info.output.hand_brake
vehicle_control.brake = self.info.output.brake
vehicle_control.steer = self.info.output.steer
vehicle_control.throttle = self.info.output.throttle
vehicle_control.reverse = self.info.output.reverse
# send control command out
self.carla_actor.apply_control(vehicle_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
def apply_control(self):
"""
Apply current control output to CARLA
:return:
"""
vehicle_control = VehicleControl()
vehicle_control.hand_brake = self.info.output.hand_brake
vehicle_control.brake = self.info.output.brake
vehicle_control.steer = self.info.output.steer
vehicle_control.throttle = self.info.output.throttle
vehicle_control.reverse = self.info.output.reverse
# send control command out, if there is a ROS control publisher
ros_control_topic = rospy.get_published_topics(namespace='/')
if (any('/carla/ego_vehicle/ackermann_cmd' == x[0]
for x in ros_control_topic)
or any('/carla/ego_vehicle/vehicle_control_cmd' == x[0]
for x in ros_control_topic)):
self.carla_actor.apply_control(vehicle_control)
def control_command_updated(self, ros_vehicle_control):
"""
Receive a CarlaEgoVehicleControl msg and send to CARLA
This function gets called whenever a ROS message is received via
'/carla/ego_vehicle/vehicle_control_cmd' topic.
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 ros_vehicle_control: current vehicle control input received via ROS
:type self.info.output: carla_ros_bridge.msg.CarlaEgoVehicleControl
:return:
"""
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
self.carla_actor.apply_control(vehicle_control)
def control_command_updated(self, ros_vehicle_control,
manual_command_override):
"""
Function used to receive CarlaEgoVehicleControl messages and send them to Carla module.
This function gets called whenever a ROS message is received via
'/carla/ego_vehicle/vehicle_control_cmd' topic.
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 module.
:param ros_vehicle_control: current vehicle control input received via ROS.
:type ros_vehicle_control: carla_ros_bridge.msg.CarlaEgoVehicleControl
:param manual_command_override: manually override the vehicle control command message
:return:
"""
if manual_command_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
self.carla_actor.apply_control(vehicle_control)
