def run(self):
while True:
try:
# `tfds.as_numpy` converts `tf.Tensor` -> `np.array`
for ex in tfds.as_numpy(self.ds):
for image, label in zip(ex['image'],ex['label']):
start=time.time()
header=Header()
set_timestamp(header,time.time())
if not image.shape[2]==1:
image=cv2.cvtColor(image,cv2.COLOR_RGB2BGR)
image_msg=from_ndarray(image,header)
label_msg=Label()
label_msg.header=header
label_msg.confidence=1.0
# `int2str` returns the human readable label ('dog', 'car',...)
label_msg.class_name=self.info.features['label'].int2str(label)
self.publish("image",image_msg)
self.publish("label",label_msg)
if (1.0/self.get_property("frame_rate")) > (time.time()-start):
time.sleep((1.0/self.get_property("frame_rate"))-(time.time()-start))
except:
pass
def _publish_imu(self, carla_imu_measurement):
"""
Function to transform a received imu measurement into a ROS Imu message
:param carla_imu_measurement: carla imu measurement object
:type carla_imu_measurement: carla.IMUMeasurement
"""
imu_msg = Imu()
imu_msg.header.frame_id = self._imu_frame
set_timestamp(imu_msg.header, carla_imu_measurement.timestamp)
# Carla uses a left-handed coordinate convention (X forward, Y right, Z up).
# Here, these measurements are converted to the right-handed ROS convention
# (X forward, Y left, Z up).
imu_msg.angular_velocity.x = -carla_imu_measurement.gyroscope.x
imu_msg.angular_velocity.y = carla_imu_measurement.gyroscope.y
imu_msg.angular_velocity.z = -carla_imu_measurement.gyroscope.z
imu_msg.linear_acceleration.x = carla_imu_measurement.accelerometer.x
imu_msg.linear_acceleration.y = -carla_imu_measurement.accelerometer.y
imu_msg.linear_acceleration.z = carla_imu_measurement.accelerometer.z
imu_rotation = carla_imu_measurement.transform.rotation
quat = trans.carla_rotation_to_numpy_quaternion(imu_rotation)
imu_msg.orientation = trans.numpy_quaternion_to_ros_quaternion(quat)
self.publish("imu", imu_msg)
def _publish_pointcloud(self, carla_lidar_measurement):
"""
Function to transform the a received lidar measurement into a ROS point cloud message
:param carla_lidar_measurement: carla lidar measurement object
:type carla_lidar_measurement: carla.LidarMeasurement
"""
header = Header()
set_timestamp(header, carla_lidar_measurement.timestamp)
header.frame_id = self._lidar_frame
fields = [
PointField("x", 0, PointField.FLOAT32, 1),
PointField("y", 4, PointField.FLOAT32, 1),
PointField("z", 8, PointField.FLOAT32, 1),
PointField("intensity", 12, PointField.FLOAT32, 1),
]
lidar_data = (
np.frombuffer(carla_lidar_measurement.raw_data, dtype=np.float32)
.reshape([-1, 4])
.copy()
)
# we take the oposite of y axis
# (as lidar point are express in left handed coordinate system, and ros need right handed)
lidar_data[:, 1] *= -1
point_cloud_msg = create_cloud(header, fields, lidar_data)
self.publish("lidar", point_cloud_msg)
def __bird_eye_image(self, image):
carla_image_data_array = np.ndarray(
shape=(image.height, image.width, 4), dtype=np.uint8, buffer=image.raw_data
)
header = Header()
set_timestamp(header, image.timestamp)
img_msg = from_ndarray(carla_image_data_array[:, :, :3], header)
self.publish("bird_eye", img_msg)
def training(self):
self.alert("Training Mode", "INFO")
for epoch in range(
self.epochs): # loop over the dataset multiple times
self.alert("Epochs: %i / %i" % (epoch + 1, self.epochs), "INFO")
# Reset the metrics at the start of the next epoch
self.train_loss.reset_states()
self.train_accuracy.reset_states()
while self.mini_batch_counter < self.mini_batches:
if not self.batches.empty():
start = time.time()
self.batch_transform()
# get the inputs; data is a list of [inputs, labels]
images = self.image_batch / 255.0
labels = self.label_batch
self.train_step(images, labels)
self.mini_batch_counter += 1
self.image_batch = None
self.label_batch = []
self.mini_batch_counter = 0
# Publish the loss for every epoch
Loss = Float64()
header = Header()
set_timestamp(header, time.time())
Loss.header = header
Loss.data = self.train_loss.result()
# Publish the accuracy for every epoch
Accuracy = Float64()
Accuracy.header = header
Accuracy.data = self.train_accuracy.result() * 100
self.publish('loss', Loss)
self.publish('accuracy', Accuracy)
with self.writer.as_default():
tf.summary.scalar("epoch_loss",
self.train_loss.result(),
step=(epoch + 1))
tf.summary.scalar("epoch_accuarcy",
self.train_accuracy.result() * 100,
step=(epoch + 1))
if not (epoch + 1) == self.epochs:
path = os.path.join(self.model_path, self.model_name)
self.model.save(path)
self.alert("Training is completed.", "INFO")
self.alert("Saving the model is started", "INFO")
path = os.path.join(self.model_path, self.model_name)
self.model.save(path)
self.alert("Saving the model is completed", "INFO")
def _publish_lane_inv(self, lane_invasion_event):
"""
Function to wrap the lane invasion event into a ros messsage
:param lane_invasion_event: carla lane invasion event object
:type lane_invasion_event: carla.LaneInvasionEvent
"""
lane_invasion_msg = CarlaLaneInvasionEvent()
lane_invasion_msg.header.frame_id = "ego_vehicle"
set_timestamp(lane_invasion_msg.header, lane_invasion_event.timestamp)
for marking in lane_invasion_event.crossed_lane_markings:
lane_invasion_msg.crossed_lane_markings.append(marking.type)
self.publish("lanes_invasion", lane_invasion_msg)
def _publish_gnss(self, carla_gnss_measurement):
"""
Function to transform a received gnss event into a ROS NavSatFix message
:param carla_gnss_measurement: carla gnss measurement object
:type carla_gnss_measurement: carla.GnssMeasurement
"""
navsatfix_msg = NavSatFix()
set_timestamp(navsatfix_msg.header, carla_gnss_measurement.timestamp)
navsatfix_msg.header.frame_id = self._gnss_frame
navsatfix_msg.latitude = carla_gnss_measurement.latitude
navsatfix_msg.longitude = carla_gnss_measurement.longitude
navsatfix_msg.altitude = carla_gnss_measurement.altitude
self.publish("fix", navsatfix_msg)
def _publish_bgr_image(self, image, port_key="rgb_camera"):
"""callback function to rgb camera sensor
it converts the image to ROS image in BGR8 encoding
"""
carla_image_data_array = np.ndarray(
shape=(image.height, image.width, 4), dtype=np.uint8, buffer=image.raw_data
)
header = Header()
set_timestamp(header, image.timestamp)
img_msg = from_ndarray(carla_image_data_array[:, :, :3], header)
self.publish(port_key + "/image_raw", img_msg)
self._camera_info.header = header
self.publish(port_key + "/camera_info", self._camera_info)
def testing(self):
self.alert("Testing Mode", "INFO")
self.alert("Loading the model...", "INFO")
self.model = tf.keras.models.load_model(self.model_path)
self.alert("The model has been loaded.", "INFO")
self.test_loss.reset_states()
self.test_accuracy.reset_states()
while self.mini_batch_counter < self.mini_batches:
if not self.batches.empty():
self.batch_transform()
# get the inputs; data is a list of [inputs, labels]
images = self.image_batch / 255.0
labels = self.label_batch
self.test_step(images, labels)
self.mini_batch_counter += 1
self.image_batch = None
self.label_batch = []
self.mini_batch_counter = 0
# Publish the loss for every epoch
Loss = Float64()
header = Header()
set_timestamp(header, time.time())
Loss.header = header
Loss.data = self.test_loss.result()
# Publish the accuracy for every epoch
Accuracy = Float64()
Accuracy.header = header
Accuracy.data = self.test_accuracy.result() * 100
self.publish('loss', Loss)
self.publish('accuracy', Accuracy)
with self.writer.as_default():
tf.summary.scalar("epoch_loss",
self.test_loss.result(),
step=(epoch + 1))
tf.summary.scalar("epoch_accuarcy",
self.test_accuracy.result() * 100,
step=(epoch + 1))
self.alert("Testing is completed.", "INFO")
self.alert("The Testing Loss: %.3f" % (self.test_loss.result()),
"INFO")
self.alert(
"The Testing Accuracy: %.2f %%" %
(self.test_accuracy.result() * 100), "INFO")
def _publish_collision(self, collision_event):
"""
Function to wrap the collision event into a ros messsage
:param collision_event: carla collision event object
:type collision_event: carla.CollisionEvent
"""
collision_msg = CarlaCollisionEvent()
collision_msg.header.frame_id = "ego_vehicle"
set_timestamp(collision_msg.header, collision_event.timestamp)
collision_msg.other_actor_id = collision_event.other_actor.id
collision_msg.normal_impulse.x = collision_event.normal_impulse.x
collision_msg.normal_impulse.y = collision_event.normal_impulse.y
collision_msg.normal_impulse.z = collision_event.normal_impulse.z
self.publish("collision", collision_msg)
def _publish_semantic_seg(self, carla_image):
"""callback function to semantic segmentation camera sensor
it converts the image to ROS image in BGR8 encoding
"""
carla_image.convert(carla.ColorConverter.CityScapesPalette)
carla_image_data_array = np.ndarray(
shape=(carla_image.height, carla_image.width, 4),
dtype=np.uint8,
buffer=carla_image.raw_data,
)
header = Header()
set_timestamp(header, carla_image.timestamp)
img_msg = from_ndarray(carla_image_data_array[:, :, :3], header)
self.publish("image_seg", img_msg)
def _publish_radar(self, carla_radar_measurement):
"""
Function to transform the a received Radar measurement into a ROS message
:param carla_radar_measurement: carla Radar measurement object
:type carla_radar_measurement: carla.RadarMeasurement
"""
radar_msg = CarlaRadarMeasurement()
set_timestamp(radar_msg.header, carla_radar_measurement.timestamp)
radar_msg.header.frame_id = self._radar_frame
for detection in carla_radar_measurement:
radar_detection = CarlaRadarDetection()
radar_detection.altitude = detection.altitude
radar_detection.azimuth = detection.azimuth
radar_detection.depth = detection.depth
radar_detection.velocity = detection.velocity
radar_msg.detections.append(radar_detection)
self.publish("radar", radar_msg)
def run_scenario(self):
"""execute the TraCI control loop"""
step = 0
vId = ""
# we start with phase 2 where EW has green
traci.trafficlight.setPhase("0", 2)
while traci.simulation.getMinExpectedNumber() > 0:
traci.simulationStep()
vIds = traci.vehicle.getIDList()
if step == 0:
vId = vIds[0]
if vId in vIds:
header = Header()
set_timestamp(header, time.time())
position = traci.vehicle.getPosition(vId)
speed = traci.vehicle.getSpeed(vId)
acceleration = traci.vehicle.getAcceleration(vId)
lateral_speed = traci.vehicle.getLateralSpeed(vId)
waiting_time = traci.vehicle.getWaitingTime(vId)
pos = Point()
pos.x, pos.y, pos.z = position[0], position[1], 0.0
vel, acc, lvel, wt = Float64(), Float64(), Float64(), Float64()
vel.header, vel.data = header, speed
acc.header, acc.data = header, acceleration
lvel.header, lvel.data = header, lateral_speed
wt.header, wt.data = header, waiting_time
self.publish("pos", pos)
self.publish("vel", vel)
self.publish("acc", acc)
self.publish("lateral_speed", lvel)
self.publish("waiting_time", wt)
if traci.trafficlight.getPhase("0") == 2:
# we are not already switching
if traci.inductionloop.getLastStepVehicleNumber("0") > 0:
# there is a vehicle from the north, switch
traci.trafficlight.setPhase("0", 3)
else:
# otherwise try to keep green for EW
traci.trafficlight.setPhase("0", 2)
step += 1
traci.close()
sys.stdout.flush()
def run(self):
while True:
if self.play == True:
self.play = False
# `tfds.as_numpy` converts `tf.Tensor` -> `np.array`
for ex in tfds.as_numpy(self.ds):
start = time.time()
header = Header()
set_timestamp(header, time.time())
image_batch_msg = ImageBatch()
image_batch_msg.header = header
if not ex['image'].shape[3] == 1:
image_batch_msg.Batch = [
from_ndarray(cv2.cvtColor(img, cv2.COLOR_RGB2BGR),
header) for img in ex["image"]
]
else:
image_batch_msg.Batch = [
from_ndarray(img, header) for img in ex["image"]
]
labels_msg = LabelArray()
labels_msg.header = header
for label in ex['label']:
label_msg = Label()
label_msg.header = header
label_msg.confidence = 1.0
# `int2str` returns the human readable label ('dog', 'car',...)
label_msg.class_name = self.info.features[
'label'].int2str(label)
labels_msg.labels.append(label_msg)
self.publish("image_batches", image_batch_msg)
self.publish("labels", labels_msg)
end = time.time() - start
if (1.0 / self.get_property("frame_rate")) > end:
time.sleep((1.0 / self.get_property("frame_rate")) -
end)
time.sleep(0.001)
def run(self):
# Block Main Loop
last_time = time.time() # start time of the loop
while True:
# timer to limit the frame rate
if time.time() - last_time < self.period:
try:
time.sleep(self.period - (time.time() - last_time))
except:
pass
# check for the time limit for the executed command
if self.move_command:
if self.car:
self.car_controls = airsim.CarControls()
self.car_controls.throttle = self.command_list[0]
self.car_controls.steering = self.command_list[1]
self.client.setCarControls(self.car_controls)
self.alert("Moving Forward ", "INFO")
self.car_moved = True
self.stop_at = time.time() + self.command_list[2]
else:
self.alert("Moving To {}".format(self.command_list), "INFO")
self.client.moveToPositionAsync(
self.command_list[0],
self.command_list[1],
self.command_list[2],
self.command_list[3],
)
self.move_command = False
if time.time() >= self.stop_at and self.car_moved:
self.car_controls.throttle = 0
self.client.setCarControls(self.car_controls)
self.car_moved = False
self.alert("Stopping the Car ", "INFO")
# AirSim Default Cameras names
cameras = [
"front_center",
"front_right",
"front_left",
"fpv",
"back_center",
]
# Reading front_center camera
camera_id = 0
responses = self.client.simGetImages(
[airsim.ImageRequest(0, airsim.ImageType.Scene, False, False)]
)
# print('Retrieved images: %d', len(responses))
for response in responses:
# get numpy array
img1d = np.fromstring(response.image_data_uint8, dtype=np.uint8)
# reshape array to 4 channel image array H X W X 4
img_rgb = img1d.reshape(response.height, response.width, 3)
# Create Header for the ROS message
header = Header()
set_timestamp(header, time.time())
header.frame_id = cameras[camera_id]
# Convert Numpy array to ROS message using yonoarc_utils.image
img_msg = from_ndarray(img_rgb, header)
# Publish this message on Block's output port
self.publish("cam_{}".format(camera_id), img_msg)
camera_id += 1
# Read Semantic Segmentation Image from Camera 0
# Create ROS message and publish it like the previous steps
seg_responses = self.client.simGetImages(
[airsim.ImageRequest(0, airsim.ImageType.Segmentation, False, False)]
)
img1d = np.fromstring(seg_responses[0].image_data_uint8, dtype=np.uint8)
img_rgb = img1d.reshape(seg_responses[0].height, seg_responses[0].width, 3)
header = Header()
header.frame_id = "front_center"
img_msg = from_ndarray(img_rgb, header)
self.publish("sem_segm", img_msg)
# Read Lidar Data and convert it to PointCloud2 ROS message
lidarData = self.client.getLidarData()
if len(lidarData.point_cloud) < 3:
self.alert("\tNo points received from Lidar data", "WARN")
else:
points = self.parse_lidarData(lidarData)
self.publish("lidar", points)
try:
states = self.client.getCarState()
float_msg = Float32()
float_msg.data = states.speed
self.publish("speed", float_msg)
except Exception as e:
print(e)
pass
# Update Timer
last_time = time.time()
def run(self):
ltime = time.time()
idx = 0
while True:
if (time.time() - ltime >= 1 / self.frequency):
if (idx == len(self.left_img_list)):
idx = 0
# RGB Images
left_img = self.read_image_file(self.left_img_list[idx])
right_img = self.read_image_file(self.right_img_list[idx])
header = Header()
set_timestamp(header, time.time())
header.frame_id = "left_img"
left_msg = from_ndarray(left_img, header)
self.publish("left_img", left_msg)
header.frame_id = "right_img"
right_msg = from_ndarray(right_img, header)
self.publish("right_img", right_msg)
# Depth Images
left_depth = self.read_numpy_file(self.left_depth_list[idx])
left_depth_vis = depth2vis(left_depth)
header.frame_id = "left_depth"
left_msg = from_ndarray(left_depth_vis, header)
self.publish("left_depth", left_msg)
right_depth = self.read_numpy_file(self.right_depth_list[idx])
right_depth_vis = depth2vis(right_depth)
header.frame_id = "right_depth"
right_msg = from_ndarray(right_depth_vis, header)
self.publish("right_depth", right_msg)
# Semantic Segmentation
left_seg = self.read_numpy_file(self.left_seg_list[idx])
left_seg_vis = seg2vis(left_seg)
header.frame_id = "left_segmentation"
left_msg = from_ndarray(left_seg_vis, header)
self.publish("left_segmentation", left_msg)
right_seg = self.read_numpy_file(self.right_seg_list[idx])
right_seg_vis = seg2vis(right_seg)
header.frame_id = "right_segmentation"
right_msg = from_ndarray(right_seg_vis, header)
self.publish("right_segmentation", right_msg)
# Left Camera Pose
pose_stamped = PoseStamped()
pose_stamped.header = header
pose_stamped.header.frame_id = "left_camera"
pose = Pose()
pose.position.x = self.pose_l[idx][0]
pose.position.y = self.pose_l[idx][1]
pose.position.z = self.pose_l[idx][2]
pose.orientation.x = self.pose_l[idx][3]
pose.orientation.y = self.pose_l[idx][4]
pose.orientation.z = self.pose_l[idx][5]
pose.orientation.w = self.pose_l[idx][6]
pose_stamped.pose = pose
self.publish("left_pose", pose_stamped)
# Right Camera Pose
pose_stamped = PoseStamped()
pose_stamped.header = header
pose_stamped.header.frame_id = "right_camera"
pose = Pose()
pose.position.x = self.pose_r[idx][0]
pose.position.y = self.pose_r[idx][1]
pose.position.z = self.pose_r[idx][2]
pose.orientation.x = self.pose_r[idx][3]
pose.orientation.y = self.pose_r[idx][4]
pose.orientation.z = self.pose_r[idx][5]
pose.orientation.w = self.pose_r[idx][6]
pose_stamped.pose = pose
self.publish("right_pose", pose_stamped)
if (idx > 0):
flow = self.read_numpy_file(self.flow_list[idx - 1])
flow_vis = flow2vis(flow)
header.frame_id = "optical_flow"
left_msg = from_ndarray(flow_vis, header)
self.publish("optical_flow", left_msg)
flow_mask = self.read_numpy_file(self.flow_mask_list[idx -
1])
flow_vis_w_mask = flow2vis(flow, mask=flow_mask)
header.frame_id = "optical_flow_mask"
right_msg = from_ndarray(flow_vis_w_mask, header)
self.publish("optical_flow_mask", right_msg)
ltime = time.time()
idx += 1
