def setup_listener(node): node.create_reader(CHASSIS_TOPIC, chassis_pb2.Chassis, chassis_callback) node.create_reader(LOCALIZATION_TOPIC, localization_pb2.LocalizationEstimate, localization_callback) while not cyber.is_shutdown(): time.sleep(0.002)
def main(argv): """ Main node """ node = cyber.Node("rtk_recorder") argv = FLAGS(argv) log_dir = "/apollo/data/log" if len(argv) > 1: log_dir = argv[1] if not os.path.exists(log_dir): os.makedirs(log_dir) Logger.config(log_file=log_dir + "rtk_recorder.log", use_stdout=True, log_level=logging.DEBUG) print("runtime log is in %s%s" % (log_dir, "rtk_recorder.log")) record_file = log_dir + "/garage.csv" recorder = RtkRecord(record_file) atexit.register(recorder.shutdown) node.create_reader('/apollo/canbus/chassis', chassis_pb2.Chassis, recorder.chassis_callback) node.create_reader('/apollo/localization/pose', localization_pb2.LocalizationEstimate, recorder.localization_callback) while not cyber.is_shutdown(): time.sleep(0.002)
def channel_bw(channel_name, window_size): rt = CyberChannelBw(window_size) node_bw = cyber.Node("listener_node_bw") node_bw.create_rawdata_reader(channel_name, rt.callback_bw) print("reader to [%s]" % channel_name) while not cyber.is_shutdown(): time.sleep(1.0) rt.print_bw()
def test_init(self): """ Test cyber. """ self.assertTrue(cyber.init()) self.assertTrue(cyber.ok()) cyber.shutdown() self.assertTrue(cyber.is_shutdown())
def channel_echo(channel_name): """ Reader message. """ node_echo = cyber.Node("listener_node_echo") echo_cb = CyberChannelecho(channel_name) node_echo.create_rawdata_reader(channel_name, echo_cb.callback) while not cyber.is_shutdown(): pass
def main(args): if not os.path.exists(args.out_dir): os.makedirs(args.out_dir) meta_msg = g_message_manager.get_msg_meta_by_topic(args.topic) if not meta_msg: print('Unknown topic name: %s' % args.topic) sys.exit(1) cyber.init() node = cyber.Node("transcribe_node") node.create_reader(args.topic, meta_msg.msg_type, transcribe) while not cyber.is_shutdown(): time.sleep(0.005)
def main(): """ Main rosnode """ node=cyber.Node('ins_stat_publisher') ins_stat = InsStat(node) while not cyber.is_shutdown(): now = cyber_time.Time.now().to_sec() ins_stat.publish_statmsg() sleep_time = 0.5 - (cyber_time.Time.now().to_sec() - now) if sleep_time > 0: time.sleep(sleep_time)
def main(): """ Main cyber """ parser = argparse.ArgumentParser( description='Generate Planning Trajectory from Data File') parser.add_argument( '-s', '--speedmulti', help='Speed multiplier in percentage (Default is 100) ', type=float, default='100') parser.add_argument('-c', '--complete', help='Generate complete path (t/F)', default='F') parser.add_argument('-r', '--replan', help='Always replan based on current position(t/F)', default='F') args = vars(parser.parse_args()) node = cyber.Node("rtk_player") Logger.config(log_file=os.path.join(APOLLO_ROOT, 'data/log/rtk_player.log'), use_stdout=True, log_level=logging.DEBUG) record_file = os.path.join(APOLLO_ROOT, 'data/log/garage.csv') player = RtkPlayer(record_file, node, args['speedmulti'], args['complete'].lower(), args['replan'].lower()) atexit.register(player.shutdown) node.create_reader('/apollo/canbus/chassis', chassis_pb2.Chassis, player.chassis_callback) node.create_reader('/apollo/localization/pose', localization_pb2.LocalizationEstimate, player.localization_callback) node.create_reader('/apollo/control/pad', pad_msg_pb2.PadMessage, player.padmsg_callback) while not cyber.is_shutdown(): now = cyber_time.Time.now().to_sec() player.publish_planningmsg() sleep_time = 0.1 - (cyber_time.Time.now().to_sec() - now) if sleep_time > 0: time.sleep(sleep_time)
def topic_publisher(topic, filename, period): """publisher""" cyber.init() node = cyber.Node("replay_file") meta_msg = None msg = None if not topic: print("Topic not specified, start to guess") meta_msg, msg = g_message_manager.parse_file(filename) topic = meta_msg.topic() else: meta_msg = g_message_manager.get_msg_meta_by_topic(topic) if not meta_msg: print("Failed to find meta info for topic: %s" % (topic)) return False msg = meta_msg.parse_file(filename) if not msg: print("Failed to parse file[%s] with topic[%s]" % (filename, topic)) return False if not msg or not meta_msg: print("Unknown topic: %s" % topic) return False writer = node.create_writer(topic, meta_msg.msg_type) if period == 0: while not cyber.is_shutdown(): input("Press any key to publish one message...") writer.write(msg) print("Topic[%s] message published" % topic) else: print("started to publish topic[%s] message with rate period %s" % (topic, period)) while not cyber.is_shutdown(): writer.write(msg) time.sleep(period)
def main(args): drive_event_meta_msg = g_message_manager.get_msg_meta_by_topic( args.drive_event_topic) if not drive_event_meta_msg: print('Unknown drive_event topic name: %s' % args.drive_event_topic) sys.exit(1) localization_meta_msg = g_message_manager.get_msg_meta_by_topic( args.localization_topic) if not localization_meta_msg: print('Unknown localization topic name: %s' % args.localization_topic) sys.exit(1) cyber.init() node = cyber.Node("derive_event_node") node.create_reader(localization_meta_msg.topic, localization_meta_msg.msg_type, OnReceiveLocalization) writer = node.create_writer(drive_event_meta_msg.topic, drive_event_meta_msg.msg_type) seq_num = 0 while not cyber.is_shutdown(): event_type = input( "Type in Event Type('d') and press Enter (current time: " + str(datetime.datetime.now()) + ")\n>") event_type = event_type.strip() if len(event_type) != 1 or event_type[0].lower() != 'd': continue current_time = cyber_time.Time.now().to_sec() event_str = None while not event_str: event_str = input("Type Event:>") event_str = event_str.strip() event_msg = drive_event_meta_msg.msg_type() event_msg.header.timestamp_sec = current_time event_msg.header.module_name = 'drive_event' seq_num += 1 event_msg.header.sequence_num = seq_num event_msg.header.version = 1 event_msg.event = event_str if g_localization: event_msg.location.CopyFrom(g_localization.pose) writer.write(event_msg) time_str = datetime.datetime.fromtimestamp(current_time).strftime( "%Y%m%d%H%M%S") filename = os.path.join(args.dir, "%s_drive_event.pb.txt" % time_str) proto_utils.write_pb_to_text_file(event_msg, filename) print('Logged to rosbag and written to file %s' % filename) time.sleep(0.1)
def main(): """ Main rosnode """ node = cyber.Node('odom_publisher') odom = OdomPublisher(node) node.create_reader('/apollo/localization/pose', localization_pb2.LocalizationEstimate, odom.localization_callback) while not cyber.is_shutdown(): now = cyber_time.Time.now().to_sec() odom.publish_odom() sleep_time = 0.01 - (cyber_time.Time.now().to_sec() - now) if sleep_time > 0: time.sleep(sleep_time)
def prediction_publisher(prediction_channel, rate): """publisher""" cyber.init() node = cyber.Node("prediction") writer = node.create_writer(prediction_channel, PredictionObstacles) sleep_time = 1.0 / rate seq_num = 1 while not cyber.is_shutdown(): prediction = PredictionObstacles() prediction.header.sequence_num = seq_num prediction.header.timestamp_sec = cyber_time.Time.now().to_sec() prediction.header.module_name = "prediction" print(str(prediction)) writer.write(prediction) seq_num += 1 time.sleep(sleep_time)
def perception_publisher(perception_channel, files, period): """ Publisher """ cyber.init() node = cyber.Node("perception") writer = node.create_writer(perception_channel, PerceptionObstacles) perception_description = load_descrptions(files) sleep_time = int(1.0 / period) # 10Hz global _s_delta_t _s_delta_t = period perception = None while not cyber.is_shutdown(): perception = generate_perception(perception_description, perception) print(str(perception)) writer.write(perception) time.sleep(sleep_time)
def test_client_class(): """ Client send request """ node = cyber.Node("client_node") client = node.create_client("server_01", ChatterBenchmark, ChatterBenchmark) req = ChatterBenchmark() req.content = "clt:Hello service!" req.seq = 0 count = 0 while not cyber.is_shutdown(): time.sleep(1) count += 1 req.seq = count print("-" * 80) response = client.send_request(req) print("get Response [ ", response, " ]")
def test_talker_class(): """ Test talker. """ msg = ChatterBenchmark() msg.content = "py:talker:send Alex!" msg.stamp = 9999 msg.seq = 0 print(msg) test_node = cyber.Node("node_name1") g_count = 1 writer = test_node.create_writer("channel/chatter", ChatterBenchmark, 6) while not cyber.is_shutdown(): time.sleep(1) g_count = g_count + 1 msg.seq = g_count msg.content = "I am python talker." print("=" * 80) print("write msg -> %s" % msg) writer.write(msg)
def main(args): audio_event_meta_msg = g_message_manager.get_msg_meta_by_topic( args.audio_event_topic) if not audio_event_meta_msg: print('Unknown audio_event topic name: %s' % args.audio_event_topic) sys.exit(1) localization_meta_msg = g_message_manager.get_msg_meta_by_topic( args.localization_topic) if not localization_meta_msg: print('Unknown localization topic name: %s' % args.localization_topic) sys.exit(1) cyber.init() node = cyber.Node("audio_event_node") node.create_reader(localization_meta_msg.topic, localization_meta_msg.msg_type, OnReceiveLocalization) writer = node.create_writer(audio_event_meta_msg.topic, audio_event_meta_msg.msg_type) seq_num = 0 while not cyber.is_shutdown(): obstacle_id = input( "Type in obstacle ID and press Enter (current time: " + str(datetime.datetime.now()) + ")\n>") obstacle_id = obstacle_id.strip() # TODO(QiL) add obstacle id sanity check. current_time = cyber_time.Time.now().to_sec() moving_result = None audio_type = None siren_is_on = None audio_direction = None while not moving_result: moving_result = input("Type MovingResult:>") moving_result = moving_result.strip() while not audio_type: audio_type = input("Type AudioType:>") audio_type = audio_type.strip() while not siren_is_on: siren_is_on = input("Type SirenOnOffStatus:>") siren_is_on = siren_is_on.strip() while not audio_direction: audio_direction = input("Type AudioDirection:>") audio_direction = audio_direction.strip() event_msg = audio_event_meta_msg.msg_type() event_msg.header.timestamp_sec = current_time event_msg.header.module_name = 'audio_event' seq_num += 1 event_msg.header.sequence_num = seq_num event_msg.header.version = 1 event_msg.id = obstacle_id event_msg.moving_result = moving_result event_msg.audio_type = audio_type event_msg.siren_is_on = siren_is_on event_msg.audio_direction = audio_direction if g_localization: event_msg.location.CopyFrom(g_localization.pose) writer.write(event_msg) time_str = datetime.datetime.fromtimestamp(current_time).strftime( "%Y%m%d%H%M%S") filename = os.path.join(args.dir, "%s_audio_event.pb.txt" % time_str) proto_utils.write_pb_to_text_file(event_msg, filename) print('Logged to rosbag and written to file %s' % filename) time.sleep(0.1)
def main(argv): """Main function""" argv = FLAGS(argv) print(""" Keyboard Shortcut: [q]: Quit Tool [s]: Save Figure [a]: Auto-adjust x, y axis to display entire plot [x]: Update Figure to Display last few Planning Trajectory instead of all [h][r]: Go back Home, Display all Planning Trajectory [f]: Toggle Full Screen [n]: Reset all Plots [b]: Unsubscribe Topics Legend Description: Red Line: Current Planning Trajectory Blue Line: Past Car Status History Green Line: Past Planning Target History at every Car Status Frame Cyan Dashed Line: Past Planning Trajectory Frames """) cyber.init() planning_sub = cyber.Node("stat_planning") fig = plt.figure() if not FLAGS.show_st_graph: ax1 = plt.subplot(2, 2, 1) item1 = Xyitem(ax1, WindowSize, VehicleLength, "Trajectory", "X [m]", "Y [m]") ax2 = plt.subplot(2, 2, 2) item2 = Item(ax2, "Speed", "Time [sec]", "Speed [m/s]", 0, 30) ax3 = plt.subplot(2, 2, 3, sharex=ax2) item3 = Item(ax3, "Curvature", "Time [sec]", "Curvature [m-1]", -0.2, 0.2) ax4 = plt.subplot(2, 2, 4, sharex=ax2) if not FLAGS.show_heading: item4 = Item(ax4, "Acceleration", "Time [sec]", "Acceleration [m/sec^2]", -5, 5) else: item4 = Item(ax4, "Heading", "Time [sec]", "Heading [radian]", -4, 4) else: ax1 = plt.subplot(2, 2, 1) item1 = Stitem(ax1, "ST Graph", "Time [sec]", "S [m]") ax2 = plt.subplot(2, 2, 2) item2 = Stitem(ax2, "ST Graph", "Time [sec]", "S [m]") ax3 = plt.subplot(2, 2, 3) item3 = Stitem(ax3, "ST Graph", "Time [sec]", "S [m]") ax4 = plt.subplot(2, 2, 4) item4 = Stitem(ax4, "ST Graph", "Time [sec]", "S [m]") plt.tight_layout(pad=0.20) plt.ion() plt.show() plotter = Plotter(item1, item2, item3, item4, FLAGS.show_st_graph) fig.canvas.mpl_connect('key_press_event', plotter.press) planning_sub.create_reader('/apollo/planning', ADCTrajectory, plotter.callback_planning) if not FLAGS.show_st_graph: localization_sub = cyber.Node("localization_sub") localization_sub.create_reader('/apollo/localization/pose', LocalizationEstimate, plotter.callback_localization) chassis_sub = cyber.Node("chassis_sub") chassis_sub.create_reader('/apollo/canbus/chassis', Chassis, plotter.callback_chassis) while not cyber.is_shutdown(): ax1.draw_artist(ax1.patch) ax2.draw_artist(ax2.patch) ax3.draw_artist(ax3.patch) ax4.draw_artist(ax4.patch) with plotter.lock: item1.draw_lines() item2.draw_lines() item3.draw_lines() item4.draw_lines() fig.canvas.blit(ax1.bbox) fig.canvas.blit(ax2.bbox) fig.canvas.blit(ax3.bbox) fig.canvas.blit(ax4.bbox) fig.canvas.flush_events()
prog="main.py") parser.add_argument("-f", "--file", action="store", type=str, required=True, help="Specify the message file for sending.") args = parser.parse_args() cyber.init() node = cyber.Node("perception_obstacle_sender") perception_pub = node.create_writer( "/apollo/perception/obstacles", perception_obstacle_pb2.PerceptionObstacles) perception_obstacles = perception_obstacle_pb2.PerceptionObstacles() with open(args.file, 'r') as f: text_format.Merge(f.read(), perception_obstacles) while not cyber.is_shutdown(): now = cyber_time.Time.now().to_sec() perception_obstacles = update(perception_obstacles) perception_pub.write(perception_obstacles) sleep_time = 0.1 - (cyber_time.Time.now().to_sec() - now) if sleep_time > 0: time.sleep(sleep_time) cyber.shutdown()