def execute(self):
# TODO(ionel): We should send the failure message on a failed stream,
# and get a reply back so that we know the node is not processing or
# issuing any new messages. Otherwise, we run into the risk of having
# inconsistent state because we can't guarantees in-order delivery
# across operators.
# Send failure message.
if self._pre_failure_time_elapse_s is not None:
time.sleep(self._pre_failure_time_elapse_s)
# Fail primary and notify ingress, egress and consumers
fail_replica_num = 0
fail_msg = Message(
(flux_utils.FluxControllerCommand.FAIL, fail_replica_num),
Timestamp(coordinates=[0]))
self.get_output_stream('controller_stream').send(fail_msg)
print("Control send failure message to primary")
if self._failure_duration_s is not None:
# TODO(yika): recovery does not work yet
# Recover failed replica
time.sleep(self._failure_duration_s)
fail_msg = Message((flux_utils.FluxControllerCommand.RECOVER,
fail_replica_num),
Timestamp(coordinates=[0]))
self.get_output_stream('controller_stream').send(fail_msg)
self.spin()
def publish_msg(self):
msg = Message(self._cnt, Timestamp(coordinates=[self._cnt]))
self.get_output_stream('data_stream').send(msg)
tuple_msg = Message((self._cnt, 'counter {}'.format(self._cnt)),
Timestamp(coordinates=[self._cnt]))
self.get_output_stream('tuple_data_stream').send(tuple_msg)
list_msg = Message([self._cnt, self._cnt + 1],
Timestamp(coordinates=[self._cnt]))
self.get_output_stream('list_data_stream').send(list_msg)
self._cnt += 1
def generate_random_position(self, msg):
"""
Generate the Pose commands to move the arm to the given location.
"""
# Move the arm to the object slowly.
steps = 400.0
time = 4.0
r = rospy.Rate(1 / (time / steps))
current_pose = self.limb.endpoint_pose()
ik_delta = Pose()
pose = Pose(position=Point(x=self.des_EE_xyz[0],
y=self.des_EE_xyz[1],
z=self.des_EE_xyz[2]),
orientation=self.orientation)
ik_delta.position.x = (current_pose['position'].x -
pose.position.x) / steps
ik_delta.position.y = (current_pose['position'].y -
pose.position.y) / steps
ik_delta.position.z = (current_pose['position'].z -
pose.position.z) / steps
ik_delta.orientation.x = (current_pose['orientation'].x -
pose.orientation.x) / steps
ik_delta.orientation.y = (current_pose['orientation'].y -
pose.orientation.y) / steps
ik_delta.orientation.z = (current_pose['orientation'].z -
pose.orientation.z) / steps
ik_delta.orientation.w = (current_pose['orientation'].w -
pose.orientation.w) / steps
for d in range(int(steps), -1, -1):
if rospy.is_shutdown():
return
ik_step = Pose()
ik_step.position.x = d * ik_delta.position.x + pose.position.x
ik_step.position.y = d * ik_delta.position.y + pose.position.y
ik_step.position.z = d * ik_delta.position.z + pose.position.z
ik_step.orientation.x = d * ik_delta.orientation.x + pose.orientation.x
ik_step.orientation.y = d * ik_delta.orientation.y + pose.orientation.y
ik_step.orientation.z = d * ik_delta.orientation.z + pose.orientation.z
ik_step.orientation.w = d * ik_delta.orientation.w + pose.orientation.w
joint_angles = self.limb.ik_request(ik_step, "right_gripper_tip")
if joint_angles:
joint_angle_msg = Message(ik_step, msg.timestamp)
self.move_ahead_lock = False
self.get_output_stream(
RandomPositionOperator.position_stream_name).send(
joint_angle_msg)
while not self.move_ahead_lock:
pass
else:
r.sleep()
final_msg = Message(True, msg.timestamp)
self.get_output_stream(
RandomPositionOperator.action_complete_stream_name).send(final_msg)
def on_next(self, action):
# Update environment
reward, state, done, info = self.env.step(action)
self.env.render() # render on display for user enjoyment
ts = Timestamp(coordinates=[0])
# Send data to output streams
self.get_output_stream("race_reward_output").send(Message(reward, ts))
self.get_output_stream("race_state_output").send(Message(state, ts))
self.get_output_stream("race_done_output").send(Message(done, ts))
self.get_output_stream("race_info_output").send(Message(info, ts))
def grasp_object(self, msg):
"""
Send a close action to the gripper.
"""
mock_grasp_object = MockGripType("open")
mock_grasp_msg = Message(mock_grasp_object, msg.timestamp)
self.move_ahead_lock = False
self.get_output_stream(MockUngraspObjectOperator.gripper_stream).\
send(mock_grasp_msg)
while not self.move_ahead_lock:
pass
action_complete_msg = Message(True, msg.timestamp)
self.get_output_stream(MockUngraspObjectOperator.\
action_complete_stream_name).\
send(action_complete_msg)
def on_msg_camera_stream(self, msg):
if self._last_seq_num + 1 != msg.timestamp.coordinates[1]:
self._logger.error(
'Expected msg with seq num {} but received {}'.format(
(self._last_seq_num + 1), msg.timestamp.coordinates[1]))
if self._flags.fail_on_message_loss:
assert self._last_seq_num + 1 == msg.timestamp.coordinates[1]
self._last_seq_num = msg.timestamp.coordinates[1]
self._logger.info('%s received frame %s', self.name, msg.timestamp)
start_time = time.time()
image = bgra_to_bgr(msg.data)
image = np.expand_dims(image.transpose([2, 0, 1]), axis=0)
tensor = torch.tensor(image).float().cuda() / 255.0
output = self._network(tensor)
# XXX(ionel): Check if the model outputs Carla Cityscapes values or
# correct Cityscapes values.
output = transfrom_to_cityscapes(
torch.argmax(output, dim=1).cpu().numpy()[0])
output = rgb_to_bgr(output)
if self._flags.visualize_segmentation_output:
add_timestamp(msg.timestamp, output)
cv2.imshow(self.name, output)
cv2.waitKey(1)
# Get runtime in ms.
runtime = (time.time() - start_time) * 1000
self._csv_logger.info('{},{},"{}",{}'.format(time_epoch_ms(),
self.name, msg.timestamp,
runtime))
output_msg = Message((output, runtime), msg.timestamp)
self.get_output_stream(self._output_stream_name).send(output_msg)
def on_msg_camera_stream(self, msg):
if self._last_seq_num + 1 != msg.timestamp.coordinates[1]:
self._logger.error(
'Expected msg with seq num {} but received {}'.format(
(self._last_seq_num + 1), msg.timestamp.coordinates[1]))
if self._flags.fail_on_message_loss:
assert self._last_seq_num + 1 == msg.timestamp.coordinates[1]
self._last_seq_num = msg.timestamp.coordinates[1]
start_time = time.time()
image_np = bgra_to_bgr(msg.data)
# TODO(ionel): Implement lane detection.
# Get runtime in ms.
runtime = (time.time() - start_time) * 1000
self._csv_logger.info('{},{},"{}",{}'.format(time_epoch_ms(),
self.name, msg.timestamp,
runtime))
if self._flags.visualize_lane_detection:
add_timestamp(msg.timestamp, image_np)
cv2.imshow(self.name, image_np)
cv2.waitKey(1)
output_msg = Message(image_np, msg.timestamp)
self.get_output_stream(self._output_stream_name).send(output_msg)
def execute(self):
while self._seq_num < self._num_messages:
output_msg = Message("data-" + str(self._seq_num),
Timestamp(coordinates=[self._seq_num]))
self.get_output_stream('input_stream').send(output_msg)
self._seq_num += 1
time.sleep(self._time_gap)
def on_notification(self, msg):
self._logger.info("Timestamps {} {} {} {}".format(
self._obstacles[0].timestamp, self._traffic_lights[0].timestamp,
self._depth_imgs[0].timestamp, self._world_transform[0].timestamp))
world_transform = self._world_transform[0].data
self._world_transform = self._world_transform[1:]
depth_img = self._depth_imgs[0].data
self._depth_imgs = self._depth_imgs[1:]
traffic_lights = self.__transform_tl_output(depth_img, world_transform)
self._traffic_lights = self._traffic_lights[1:]
(pedestrians,
vehicles) = self.__transform_detector_output(depth_img,
world_transform)
self._obstacles = self._obstacles[1:]
speed_factor, state = self.__stop_for_agents(self._wp_angle,
self._wp_vector, vehicles,
pedestrians,
traffic_lights)
control = self.get_control(self._wp_angle, self._wp_angle_speed,
speed_factor, self._vehicle_speed * 3.6)
output_msg = Message(control, Timestamp(coordinates=[0]))
self.get_output_stream('action_stream').send(output_msg)
def __send_sensor_data(self, sensor_data, timestamp, watermark):
for name, measurement in sensor_data.items():
data_stream = self.get_output_stream(name)
if data_stream.get_label('camera_type') == 'sensor.camera.rgb':
# Transform the Carla RGB images to BGR.
data_stream.send(
pylot.simulation.messages.FrameMessage(
pylot.utils.bgra_to_bgr(to_bgra_array(measurement)),
timestamp))
elif data_stream.get_label(
'camera_type') == 'sensor.camera.semantic_segmentation':
frame = labels_to_array(measurement)
data_stream.send(SegmentedFrameMessage(frame, 0, timestamp))
elif data_stream.get_label('camera_type') == 'sensor.camera.depth':
# NOTE: depth_to_array flips the image.
data_stream.send(
pylot.simulation.messages.DepthFrameMessage(
depth_to_array(measurement), self._transforms[name],
measurement.fov, timestamp))
elif data_stream.get_label(
'sensor_type') == 'sensor.lidar.ray_cast':
pc_msg = pylot.simulation.messages.PointCloudMessage(
measurement.data, self._transforms[name], timestamp)
data_stream.send(pc_msg)
else:
data_stream.send(Message(measurement, timestamp))
data_stream.send(watermark)
def publish_msg(self):
if self.idx % 2 == 0:
time.sleep(0.02)
data = 'data %d' % self.idx
output_msg = Message(data, Timestamp(coordinates=[0]))
self.get_output_stream('pub_out').send(output_msg)
self.idx += 1
def on_msg_best_sync_data_at_freq(self):
"""Publishes best synced data received since last invocation."""
self._epoch += 1
timestamp = Timestamp(coordinates=[self._epoch - 1])
msg = Message(self._best_sync[self._epoch - 1], timestamp)
self.get_output_stream('sync_stream').send(msg)
del self._best_sync[self._epoch - 1]
def on_msg_camera_stream(self, msg):
if self._last_seq_num + 1 != msg.timestamp.coordinates[1]:
self._logger.error(
'Expected msg with seq num {} but received {}'.format(
(self._last_seq_num + 1), msg.timestamp.coordinates[1]))
if self._flags.fail_on_message_loss:
assert self._last_seq_num + 1 == msg.timestamp.coordinates[1]
self._last_seq_num = msg.timestamp.coordinates[1]
self._logger.info('{} received frame {}'.format(
self.name, msg.timestamp))
start_time = time.time()
image_np = msg.data
results = self._detector.run(image_np)
output = self.__get_output_bboxes(results['results'])
if self._flags.visualize_detector_output:
visualize_bboxes(self.name, msg.timestamp, image_np, output,
self._bbox_colors)
# Get runtime in ms.
runtime = (time.time() - start_time) * 1000
self._csv_logger.info('{},{},"{}",{}'.format(time_epoch_ms(),
self.name, msg.timestamp,
runtime))
output_msg = Message((output, runtime), msg.timestamp)
self.get_output_stream(self._output_stream_name).send(output_msg)
def execute(self):
# Connect to CARLA and retrieve the world running.
self._client, self._world = get_world(self._flags.carla_host,
self._flags.carla_port,
self._flags.carla_timeout)
if self._client is None or self._world is None:
raise ValueError('There was an issue connecting to the simulator.')
# Replayer time factor is only available in > 0.9.5.
# self._client.set_replayer_time_factor(0.1)
print(
self._client.replay_file(self._flags.carla_replay_file,
self._flags.carla_replay_start_time,
self._flags.carla_replay_duration,
self._flags.carla_replay_id))
# Sleep a bit to allow the server to start the replay.
time.sleep(1)
self._driving_vehicle = self._world.get_actors().find(
self._flags.carla_replay_id)
if self._driving_vehicle is None:
raise ValueError("There was an issue finding the vehicle.")
# TODO(ionel): We do not currently have a top message.
timestamp = Timestamp(coordinates=[sys.maxint])
vehicle_id_msg = Message(self._driving_vehicle.id, timestamp)
self.get_output_stream('vehicle_id_stream').send(vehicle_id_msg)
self.get_output_stream('vehicle_id_stream').send(
WatermarkMessage(timestamp))
self._world.on_tick(self.on_world_tick)
self.spin()
def on_loop_complete(self, msg):
"""Publishes best synced data received while the loop was running."""
self._epoch += 1
timestamp = Timestamp(coordinates=[self._epoch - 1])
msg = Message(self._best_sync[self._epoch - 1], timestamp)
self.get_output_stream('sync_stream').send(msg)
del self._best_sync[self._epoch - 1]
def on_frame(self, msg):
cv_img = self._bridge.imgmsg_to_cv2(msg.data, "bgr8")
pylot_utils.do_work(self._logger, self._min_runtime, self._max_runtime)
intersection_prob = random.uniform(0, 1)
output_msg = Message(intersection_prob, msg.timestamp)
output_name = '{}_output'.format(self.name)
self.get_output_stream(output_name).send(output_msg)
def on_frame(self, msg):
cv_img = self._bridge.imgmsg_to_cv2(msg.data, "bgr8")
pylot_utils.do_work(self._logger, self._min_runtime, self._max_runtime)
bboxes = pylot_utils.generate_synthetic_bounding_boxes(1, 1)
output_msg = Message(bboxes, msg.timestamp)
output_name = '{}_output'.format(self.name)
self.get_output_stream(output_name).send(output_msg)
def on_msg_camera_stream(self, msg):
if self._last_seq_num + 1 != msg.timestamp.coordinates[1]:
self._logger.error(
'Expected msg with seq num {} but received {}'.format(
(self._last_seq_num + 1), msg.timestamp.coordinates[1]))
if self._flags.fail_on_message_loss:
assert self._last_seq_num + 1 == msg.timestamp.coordinates[1]
self._last_seq_num = msg.timestamp.coordinates[1]
self._logger.info('{} received frame {}'.format(
self.name, msg.timestamp))
start_time = time.time()
# The models expect BGR images.
image_np = msg.data
# Expand dimensions since the model expects images to have
# shape: [1, None, None, 3]
image_np_expanded = np.expand_dims(image_np, axis=0)
(boxes, scores, classes, num_detections) = self._tf_session.run(
[
self._detection_boxes, self._detection_scores,
self._detection_classes, self._num_detections
],
feed_dict={self._image_tensor: image_np_expanded})
num_detections = int(num_detections[0])
classes = classes[0][:num_detections]
labels = [self._coco_labels[label] for label in classes]
boxes = boxes[0][:num_detections]
scores = scores[0][:num_detections]
self._logger.info('Object boxes {}'.format(boxes))
self._logger.info('Object scores {}'.format(scores))
self._logger.info('Object labels {}'.format(labels))
index = 0
output = []
im_width = image_np.shape[1]
im_height = image_np.shape[0]
while index < len(boxes) and index < len(scores):
if scores[index] >= self._flags.detector_min_score_threshold:
ymin = int(boxes[index][0] * im_height)
xmin = int(boxes[index][1] * im_width)
ymax = int(boxes[index][2] * im_height)
xmax = int(boxes[index][3] * im_width)
corners = (xmin, xmax, ymin, ymax)
output.append((corners, scores[index], labels[index]))
index += 1
if self._flags.visualize_detector_output:
visualize_bboxes(self.name, msg.timestamp, image_np, output,
self._bbox_colors)
# Get runtime in ms.
runtime = (time.time() - start_time) * 1000
self._csv_logger.info('{},{},"{}",{}'.format(time_epoch_ms(),
self.name, msg.timestamp,
runtime))
output_msg = Message((output, runtime), msg.timestamp)
self.get_output_stream(self._output_stream_name).send(output_msg)
def publish_numbers(self):
""" Sends an increasing count of numbers with breaks at
pre-defined steps."""
output_msg = Message(self.counter,
Timestamp(coordinates=[self.batch_number]))
if not self.send_batch(self.batch_number):
# If this batch_number does not need to be sent as a batch, then
# send the individual messages and if the batch is complete, send a
# watermark.
self.get_output_stream(self.output_stream_name).send(output_msg)
if self.counter % self.batch_size == 0:
# The batch has completed, send the watermark.
watermark_msg = WatermarkMessage(
Timestamp(coordinates=[self.batch_number]))
self.batch_number += 1
self.get_output_stream(
self.output_stream_name).send(watermark_msg)
else:
# This batch number needs to be batched, add it to the window and
# then check if the batch_size number of messages exist in the
# window. If yes, send everything including the watermark message.
self.window.append(output_msg)
if self.counter % self.batch_size == 0:
# The batch has completed, send everything including the
# watermark.
for output_msg in self.window:
self.get_output_stream(
self.output_stream_name).send(output_msg)
watermark_msg = WatermarkMessage(
Timestamp(coordinates=[self.batch_number]))
self.batch_number += 1
self.get_output_stream(
self.output_stream_name).send(watermark_msg)
self.window = []
self.counter += 1
def calculate_directions(self):
pylot_utils.do_work(self._logger, self._min_runtime, self._max_runtime)
# Send value 0-5 for direction (e.g., 0 left, 1 right, ...).
direction = np.random.randint(0, 6)
output_msg = Message(direction, Timestamp(coordinates=[self._cnt]))
self.get_output_stream('directions').send(output_msg)
self._cnt += 1
def on_reply(self, msg):
self._cnt += 1
if self._cnt == self._num_iterations:
duration = time.time() - self._start_time
print('Completed {} iterations in {}'.format(self._cnt, duration))
else:
output_msg = Message(self._cnt, Timestamp(coordinates=[0]))
self.get_output_stream('loop_ingest').send(output_msg)
def on_msg_image_stream(self, msg):
"""
On receipt of a message, convert it to a CV2 image, and send it on the
output stream.
"""
img = self.bridge.imgmsg_to_cv2(msg.data, 'bgr8')
img_msg = Message(img, msg.timestamp)
self.get_output_stream(self.output_name).send(img_msg)
def send_can_bus_reading(self, data, timestamp, watermark_msg):
# The can bus dict contains other fields as well, but we don't
# curently use them.
vehicle_transform = to_pylot_transform(data['transform'])
forward_speed = data['speed']
can_bus = pylot.simulation.utils.CanBus(vehicle_transform,
forward_speed)
self._can_bus_stream.send(Message(can_bus, timestamp))
def on_goal(self, msg):
(location, orientation) = msg.data
direction = self.get_next_command(location, orientation,
self._goal_location,
self._goal_orientation)
print('Computed directions {}'.format(direction))
self.get_output_stream('planner_output').send(
Message(direction, msg.timestamp))
def publish_msg(self):
self._cnt += 1
# XXX(ionel): Hack! We send more messages because the first several
# messages are lost in ROS because subscribers may start after
# publishers already publish several messages.
if self._cnt <= self._num_msgs + 10000:
output_msg = Message(self._cnt, Timestamp(coordinates=[self._cnt]))
self.get_output_streams('data_stream').send(output_msg)
def publish_imu_data(self):
roll = random.random()
pitch = random.random()
yaw = random.random()
output_msg = Message((roll, pitch, yaw),
Timestamp(coordinates=[self._cnt]))
self.get_output_stream('imu').send(output_msg)
self._cnt += 1
def predict(self):
predicted_locs = self._objs + self._objs + self._objs
pylot_utils.do_work(self._logger, self._min_runtime, self._max_runtime)
output_msg = Message(predicted_locs,
Timestamp(coordinates=[self._cnt]))
self.get_output_stream('prediction_stream').send(output_msg)
self._objs = []
self._cnt += 1
def on_vehicle_pos_update(self, msg):
start_time = time.time()
payload = self.get_waypoints(msg.data)
runtime = (time.time() - start_time) * 1000
self._csv_logger.info('{},{},{}'.format(time_epoch_ms(), self.name,
runtime))
self.get_output_stream('waypoints').send(
Message(payload, msg.timestamp))
def __publish_hero_vehicle_data(self, timestamp, watermark_msg):
vec_transform = to_pylot_transform(
self._driving_vehicle.get_transform())
forward_speed = pylot.simulation.utils.get_speed(
self._driving_vehicle.get_velocity())
can_bus = pylot.simulation.utils.CanBus(vec_transform, forward_speed)
self.get_output_stream('can_bus').send(Message(can_bus, timestamp))
self.get_output_stream('can_bus').send(watermark_msg)
def localize(self):
pylot_utils.do_work(self._logger, self._min_runtime, self._max_runtime)
# TODO(ionel): Check how synchronized the data is.
# TODO(ionel): Interact with the mapping operator.
location = (random.uniform(0, 180), random.uniform(0, 180))
output_msg = Message(location, Timestamp(coordinates=[self._cnt]))
self.get_output_stream('location').send(output_msg)
self._cnt += 1
