def test_read_write(self): """ Unit test of reader. """ self.assertTrue(cyber.ok()) # Read. reader_node = cyber.Node("listener") reader = reader_node.create_reader("channel/chatter", SimpleMessage, self.callback) self.assertEqual(reader.name, "channel/chatter") self.assertEqual(reader.data_type, SimpleMessage) self.assertEqual(SimpleMessage.DESCRIPTOR.full_name, "apollo.common.util.test.SimpleMessage") # Write. msg = SimpleMessage() msg.text = "talker:send Alex!" msg.integer = 0 self.assertTrue(cyber.ok()) writer_node = cyber.Node("writer") writer = writer_node.create_writer("channel/chatter", SimpleMessage, 7) self.assertEqual(writer.name, "channel/chatter") self.assertEqual( writer.data_type, "apollo.common.util.test.SimpleMessage") self.assertTrue(writer.write(msg)) # Wait for data to be processed by callback function. time.sleep(0.1)
def main(): """ Main rosnode """ cyber.init() node = cyber.Node("mock_routing_requester") sequence_num = 0 routing_request = routing_pb2.RoutingRequest() routing_request.header.timestamp_sec = cyber_time.Time.now().to_sec() routing_request.header.module_name = 'routing_request' routing_request.header.sequence_num = sequence_num sequence_num = sequence_num + 1 waypoint = routing_request.waypoint.add() waypoint.pose.x = 587696.82286 waypoint.pose.y = 4141446.66696 waypoint.id = '1-1' waypoint.s = 1 waypoint = routing_request.waypoint.add() waypoint.pose.x = 586948.740120 waypoint.pose.y = 4141171.118641 waypoint.id = '1-1' waypoint.s = 80 writer = node.create_writer('/apollo/routing_request', routing_pb2.RoutingRequest) time.sleep(2.0) print("routing_request", routing_request) writer.write(routing_request)
def __init__(self): self.node = cyber.Node("sendor_calibration_preprocessor") self.writer = self.node.create_writer("/apollo/dreamview/progress", preprocess_table_pb2.Progress, 6) self.config = extractor_config_pb2.DataExtractionConfig() self.progress = preprocess_table_pb2.Progress() self.progress.percentage = 0.0 self.progress.log_string = "Preprocessing in progress..." self.progress.status = preprocess_table_pb2.Status.UNKNOWN try: get_pb_from_text_file(FLAGS.config, self.config) except text_format.ParseError: print(f'Error: Cannot parse {FLAGS.config} as text proto') self.records = [] for r in self.config.records.record_path: self.records.append(str(r)) self.start_timestamp = -1 self.end_timestamp = -1 if self.config.io_config.start_timestamp == "FLOAT_MIN": self.start_timestamp = np.finfo(np.float32).min else: self.start_timestamp = np.float32( self.config.io_config.start_timestamp) if self.config.io_config.end_timestamp == "FLOAT_MAX": self.end_timestamp = np.finfo(np.float32).max else: self.end_timestamp = np.float32( self.config.io_config.end_timestamp)
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 listener(): cyber.init() test_node = cyber.Node("control_listener") test_node.create_reader("/apollo/control", control_cmd_pb2.ControlCommand, callback) test_node.spin() cyber.shutdown()
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_listener_class(): """ Reader message. """ print("=" * 120) test_node = cyber.Node("listener") test_node.create_reader("channel/chatter", ChatterBenchmark, callback) test_node.spin()
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 test_params(self): param1 = parameter.Parameter("author_name", "WanderingEarth") param2 = parameter.Parameter("author_age", 5000) param3 = parameter.Parameter("author_score", 888.88) test_node = cyber.Node(PARAM_SERVICE_NAME) srv = parameter.ParameterServer(test_node) node_handle = cyber.Node("service_client_node") clt = parameter.ParameterClient(test_node, PARAM_SERVICE_NAME) clt.set_parameter(param1) clt.set_parameter(param2) clt.set_parameter(param3) param_list = clt.get_paramslist() self.assertEqual(3, len(param_list)) param_list = srv.get_paramslist() self.assertEqual(3, len(param_list))
def test_service_class(): """ Reader message. """ print("=" * 120) node = cyber.Node("service_node") r = node.create_service("server_01", ChatterBenchmark, ChatterBenchmark, callback) node.spin()
def __init__(self): self.record_num = FLAGS.record_num self.vehicle_type = self.folder_case(FLAGS.vehicle_type) self.config_file = self.get_config_path() self.node = cyber.Node("vehicle_calibration_preprocessor") self.writer = self.node.create_writer("/apollo/dreamview/progress", preprocess_table_pb2.Progress, 10) self.progress = preprocess_table_pb2.Progress() self.progress.percentage = 0.0 self.progress.log_string = "Press the button to start preprocessing"
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 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 print_param_srv(): param1 = parameter.Parameter("author_name", "WanderingEarth") param2 = parameter.Parameter("author_age", 5000) param3 = parameter.Parameter("author_score", 888.88) test_node = cyber.Node(PARAM_SERVICE_NAME) srv = parameter.ParameterServer(test_node) node_handle = cyber.Node("service_client_node") clt = parameter.ParameterClient(test_node, PARAM_SERVICE_NAME) clt.set_parameter(param1) clt.set_parameter(param2) clt.set_parameter(param3) param_list = clt.get_paramslist() print("clt param lst len is ", len(param_list)) for param in param_list: print(param.debug_string()) print("") param_list = srv.get_paramslist() print("srv param lst len is ", len(param_list)) for param in param_list: print(param.debug_string())
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 main(): """ Main function """ node = cyber.Node("data_collector") data_collector = DataCollector(node) plotter = Plotter() node.create_reader('/apollo/localization/pose', localization_pb2.LocalizationEstimate, data_collector.callback_localization) node.create_reader('/apollo/canbus/chassis', chassis_pb2.Chassis, data_collector.callback_canbus) print('Enter q to quit.') print('Enter p to plot result from last run.') print('Enter x to remove result from last run.') print('Enter x y z, where x is acceleration command, ' + 'y is speed limit, z is decceleration command.') print('Positive number for throttle and negative number for brake.') while True: cmd = input("Enter commands: ").split() if len(cmd) == 0: print('Quiting.') break elif len(cmd) == 1: if cmd[0] == "q": break elif cmd[0] == "p": print('Plotting result.') if os.path.exists(data_collector.outfile): plotter.process_data(data_collector.outfile) plotter.plot_result() else: print('File does not exist: %s' % data_collector.outfile) elif cmd[0] == "x": print('Removing last result.') if os.path.exists(data_collector.outfile): os.remove(data_collector.outfile) else: print('File does not exist: %s' % date_collector.outfile) elif len(cmd) == 3: data_collector.run(cmd)
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 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)
if abs(self.acc) > self.acclimit: print(t, "\t", (sum(self.buff) / len(self.buff)) * 3.6, "\t", self.acc, "\t", self.count, "\t", self.acclimit) self.last_acc = self.acc self.last_t = t self.last_speed = sum(self.buff) / len(self.buff) self.buff = [] if __name__ == '__main__': """Main function""" parser = argparse.ArgumentParser(description="Test car realtime status.", prog="RealTimeTest.py", usage="python realtime_test.py") parser.add_argument("--acc", type=int, required=False, default=2, help="Acc limit default must > 0") args = parser.parse_args() if args.acc < 0: print("acc must larger than 0") cyber.init() rttest = RealTimeTest() rttest.acclimit = args.acc cyber_node = cyber.Node("RealTimeTest") cyber_node.create_reader("/apollo/canbus/chassis", chassis_pb2.Chassis, rttest.chassis_callback) cyber_node.spin() cyber.shutdown()
def run(): print("=" * 120) test_node = cyber.Node("audiosaver") test_node.create_reader(RESPEAKER_CHANNEL, AudioData, callback) test_node.spin()
adc_trajectory.ParseFromString(msg.message) controlinfo.callback_planning(adc_trajectory) elif msg.topic == "/apollo/control": control_cmd = control_cmd_pb2.ControlCommand() control_cmd.ParseFromString(msg.message) controlinfo.callback_control(control_cmd) elif msg.topic == "/apollo/canbus/chassis": chassis = chassis_pb2.Chassis() chassis.ParseFromString(msg.message) controlinfo.callback_canbus(chassis) print("Done reading the file") else: cyber.init() # rospy.init_node('control_info', anonymous=True) node = cyber.Node("rtk_recorder") planningsub = node.create_reader('/apollo/planning', planning_pb2.ADCTrajectory, controlinfo.callback_planning) localizationsub = node.create_reader( '/apollo/localization/pose', localization_pb2.LocalizationEstimate, controlinfo.callback_localization) controlsub = node.create_reader('/apollo/control', control_cmd_pb2.ControlCommand, controlinfo.callback_control) canbussub = node.create_reader('/apollo/canbus/chassis', chassis_pb2.Chassis, controlinfo.callback_canbus) input("Press Enter To Stop") mng = plt.get_current_fig_manager()
def listener(): cyber.init() test_node = cyber.Node("planning_acc_listener") test_node.create_reader("/apollo/planning", planning_pb2.ADCTrajectory, callback)
def add_listener(): planning_sub = cyber.Node("st_plot") planning_sub.create_reader('/apollo/planning', planning_pb2.ADCTrajectory, planning_callback)
own record file]: cyber_record play -f your_file.record """) parser.add_argument( "trace", action='store', type=str, help='the manual driving trace produced by rtk_player') args = parser.parse_args() fig, ax = plt.subplots() with open(args.trace, 'r') as fp: trace_data = np.genfromtxt(handle, delimiter=',', names=True) ax.plot(trace_data['x'], trace_data['y'], 'b-', alpha=0.5, linewidth=1) cyber.init() node = cyber.Node("plot_trace") setup_listener(node) x_min = min(trace_data['x']) x_max = max(trace_data['x']) y_min = min(trace_data['y']) y_max = max(trace_data['y']) GPS_LINE, = ax.plot(GPS_X, GPS_Y, 'r', linewidth=3, label="gps") ani = animation.FuncAnimation(fig, update, interval=100) plt.show()
def perception_receiver(perception_channel): """publisher""" cyber.init() node = cyber.Node("perception") node.create_reader(perception_channel, PerceptionObstacles, receiver) node.spin()