def callback(self, data): # print(data.frame_no) # reshape self.getmeanpoint(data) # publish, write to channel if not cyber.is_shutdown(): self.write_to_channel()
def main(argv): """ Main node """ node = cyber.Node("rtk_recorder") argv = FLAGS(argv) log_dir = os.path.dirname( os.path.abspath(__file__)) + "/../../../data/log/" 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 __init__(self, node): self.node = node self.start_x = 0 self.start_y = 0 self.goal_x = 0 self.goal_y = 0 self.global_path = Trajectory() self.node.create_reader("/geek/uwb/localization", pos, self.localizationcallback) self.node.create_reader("/planning/mission_point", Point, self.missioncallback) self.writer = self.node.create_writer("/planning/global_trajectory", Trajectory) signal.signal(signal.SIGINT, self.sigint_handler) signal.signal(signal.SIGHUP, self.sigint_handler) signal.signal(signal.SIGTERM, self.sigint_handler) self.is_sigint_up = False while True: time.sleep(0.05) if not cyber.is_shutdown() and self.global_path: self.writer.write(self.global_path) if self.is_sigint_up: print("Exit!") self.is_sigint_up = False sys.exit()
def test_init(self): """ Test cyber. """ self.assertTrue(cyber.init()) self.assertTrue(cyber.ok()) cyber.shutdown() self.assertTrue(cyber.is_shutdown())
def callback(self, data): # TODO # print(data.frame_no) # TODO infer self.infer(data) # TODO publish, write to channel if not cyber.is_shutdown(): self.write_to_channel()
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 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 _wait_for_callback(self): time_exceed = 0.0 while not (self.parameters_flag) and not cyber.is_shutdown(): time.sleep(WAIT_FOR_PARAMETER) time_exceed += WAIT_FOR_PARAMETER if time_exceed >= SV_TIMEOUT: print("Connection timeout!") break return
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 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 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 missioncallback(self, Point): self.goal_x = int(Point.x) self.goal_y = int(Point.y) pathList = self.start(self.start_x, self.start_y, self.goal_x, self.goal_y) self.planning_path = Trajectory() if not pathList: print("Failed to find a path") else: for path_point in pathList: point_xy.x = path_point[0] point_xy.y = path_point[1] self.planning_path.point.append(point_xy) if not cyber.is_shutdown() and self.planning_path: self.writer.write(self.planning_path)
def loop(self): try: while not cyber.is_shutdown(): try: c = sys.stdin.read(1) if c: # print("Got character", repr(c)) if c == 'w': self.hotkey_w() if c == 's': self.hotkey_s() if c == 'a': self.hotkey_a() if c == 'd': self.hotkey_d() print(self.msg) self.writer.write(self.msg) # ratio domain 100hz time.sleep(0.01) except IOError: pass finally: termios.tcsetattr(fd, termios.TCSAFLUSH, oldterm) fcntl.fcntl(fd, fcntl.F_SETFL, oldflags)
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 loop(self): try: while not cyber.is_shutdown(): try: c = sys.stdin.read(1) if c: # print("Got character", repr(c)) # TODO 3 update your logic by keyboad # if c == 'w': self.hotkey_w() # if c == 's': self.hotkey_s() # if c == 'a': self.hotkey_a() # if c == 'd': self.hotkey_d() print(self.msg) # TODO 4 write control message to channel /control # self.writer.write(self.msg) # ratio domain 100hz time.sleep(0.01) except IOError: pass finally: termios.tcsetattr(fd, termios.TCSAFLUSH, oldterm) fcntl.fcntl(fd, fcntl.F_SETFL, oldflags)
def callback(self, data): # reshape self.reshape(data) # publish, write to channel if not cyber.is_shutdown(): self.write_to_channel()
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()
if args.t == 0 and args.f == 0: timer_s = 0 timer_kind = 'OK' timer_set = False print("Starting infinite OK message loop") else: print("Only one timer can be non-zero value!") sys.exit() cyber.init("Dump_IPC_Supervisor") dump_sv_node = cyber.Node("dump_sv_talker") writer = dump_sv_node.create_writer("supervisor/general", SV_info, 5) begin_time = time.time() while not(cyber.is_shutdown()): current_time = time.time() msg = sv_decision() msg.header.timestamp_sec = current_time if current_time - begin_time < timer_s or not(timer_set): msg.status = 1 msg.message = "It's okay, chill" time.sleep(STD_DELAY) else: if timer_kind == 'FATAL': msg.status = 4 msg.message = 'Oops, something gone wrong!' time.sleep(STD_DELAY)
def callback(self, pos): global obslist, planning_path, best_trajectory start_x = int(pos.x * scale) start_y = int(pos.y * scale) obslist = [] num_point = 0 minr = float("inf") for i, point in enumerate(self.pathList): r = math.sqrt((point[0] - start_x)**2 + (point[1] - start_y)**2) if minr >= r: minr = r num_point = i if (num_point - 8) <= 0: num_point = 8 #规划预瞄点 f_point = self.pathList[num_point - 8] self.goal = f_point print("goal:", self.goal) #将预瞄点坐标从地图坐标系转换到车身坐标系 f_point = [(f_point[0] - start_x) / scale, -((f_point[1] - start_y) / scale)] yaw = math.pi - pos.yaw x_f = f_point[0] * math.cos(yaw) + f_point[1] * math.sin(yaw) y_f = f_point[1] * math.cos(yaw) - f_point[0] * math.sin(yaw) print("trans-goal:", [x_f, y_f]) #将预设障碍物坐标从地图坐标系转换到车身坐标系 ob_point = [643, 353] ob_point = [(ob_point[0] - start_x) / scale, -((ob_point[1] - start_y) / scale)] x_ob = ob_point[0] * math.cos(yaw) + ob_point[1] * math.sin(yaw) y_ob = ob_point[1] * math.cos(yaw) - ob_point[0] * math.sin(yaw) print("ob:", [x_ob, y_ob]) print(" planning start!") # initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)] x = np.array([0.0, 0.0, 0, 0.3, 0.0]) # goal position [x(m), y(m)] goal = np.array([x_f, y_f]) ob = np.matrix(self.obstacleList) ob = np.array([[x_ob, y_ob, 0.24 / scale]]) # input [forward speed, yawrate] u = np.array([0.3, 0.0]) config = Config() best_trajectory = np.array(x) u, best_trajectory = dwa_control(x, u, config, goal, ob) x = motion(x, u, config.dt) #接近终点减速 dist_car_goal = ((((start_x - self.pathList[0][0])**2 + (start_y - self.pathList[0][1])**2)**0.5) / scale) if dist_car_goal <= 0.2: u[0] = 0 self.planning_path = Trajectory() self.speed = Control_Reference() self.speed.vehicle_speed = u[0] if not best_trajectory.any(): print("Failed to find a path") else: for path_point in best_trajectory: point_xy.x = path_point[0] point_xy.y = path_point[1] self.planning_path.point.append(point_xy) point_xy.x = self.goal[0] point_xy.y = self.goal[1] self.planning_path.point.append(point_xy) if not cyber.is_shutdown() and self.planning_path: self.writer.write(self.planning_path) self.vwriter.write(self.speed) print("planning done")