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