def spin(self): self.cyber_node.create_reader('/apollo/localization/pose', localization_pb2.LocalizationEstimate, self.localization_callback) # wait for first localization message while not cyber.is_shutdown() and not self.localization_received: time.sleep(0.1) while not cyber.is_shutdown(): now = cyber_time.Time.now().to_sec() self.publish_trajectory() if (self.at_end_of_trajectory()): if (self.params.loop): self.reset_trajectory_tracking() else: sys.exit(0) sleep_time = 1.0 / self.params.rate - ( cyber_time.Time.now().to_sec() - now) if sleep_time > 0.0: time.sleep(sleep_time)
def gnss_simulation(q): cyber.init() #rospy.init_node("pose_offline",anonymous=True) node = cyber.Node("pose_offline") #localization_pub = rospy.Publisher( # "/apollo/localization/pose",localization_pb2.LocalizationEstimate,queue_size=1) localization_pub = node.create_writer( "/apollo/localization/pose", localization_pb2.LocalizationEstimate) #generate localization info LocalizationEstimate = localization_pb2.LocalizationEstimate() #send pose to /apollo/localization/pose count = 0 #r = rospy.Rate(20) #while not rospy.is_shutdown(): while not cyber.is_shutdown(): now = time.time() #localization_pub.publish(gps_list(LocalizationEstimate,count,q)) localization_pub.write(gps_list(LocalizationEstimate, count, q)) sleep_time = 0.05 - (time.time() - now) if sleep_time > 0: time.sleep(sleep_time) count += 1
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 loop(self): while not cyber.is_shutdown(): # self.msg.steer_angle = random.random() # self.msg.throttle = random.random() self.writer.write(self.msg) # ratio domain 100hz time.sleep(0.01)
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 callback(self, data): # TODO # print(data.frame_no) # TODO reshape self.reshape(data) # TODO publish, write to channel if not cyber.is_shutdown(): self.write_to_channel()
def test_init(self): """ Test cyber. """ self.assertTrue(cyber.init()) self.assertTrue(cyber.ok()) cyber.shutdown() self.assertTrue(cyber.is_shutdown())
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 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(): raw_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) print(msg) time.sleep(period)
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 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 = time.time() player.publish_planningmsg() sleep_time = 0.1 - (time.time() - now) if sleep_time > 0: time.sleep(sleep_time)
def plan(self): print(hasattr(self, 'data')) while not cyber.is_shutdown(): print("in plan ********************************************") if not hasattr(self, 'data'): time.sleep(0.2) print("no data sleep firstly") continue sx = self.data.start_point.x # start x position [m] sy = self.data.start_point.y # start y positon [m] gx = self.data.end_point.x # goal x position [m] gy = self.data.end_point.y # goal y position [m] grid_size = 0.051 # potential grid size [m] robot_radius = 0.0725 # robot radius [m] print('start point,{},{} goal point,{}, {}'.format(sx, sy, gx, gy)) ox, oy = [], [] for obstacle in self.data.obs_points: ox.append(obstacle.x) oy.append(obstacle.y) print('obstacle information, ox:{}, oy:{} '.format(ox, oy)) # ox = [-0.03464780002832413] # obstacle x position list [m] # oy = [0.6250196099281311] # obstacle y position list [m] # if show_animation: # plt.grid(True) # plt.axis("equal") # path generation rx, ry = [], [] start = time.time() rx, ry = potential_field_planning( sx, sy, gx, gy, ox, oy, grid_size, robot_radius) end = time.time() print('time cost: {}'.format(end - start)) print('rx,{}, ry.{}'.format(rx, ry)) self.planning_path = Trajectory() if not rx: print("Failed to find a path") else: point = Point() for i, _ in enumerate(rx): point.x = rx[i] point.y = ry[i] self.planning_path.point.append(point) print('trajectory,{}'.format(self.planning_path)) self.write_to_channel()
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(): raw_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(): """ 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 = time.time() player.publish_planningmsg() sleep_time = 0.1 - (time.time() - 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 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 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 __init__(self, node): self.node = node self.msg = Tags() self.localization = localization() self.position_0 = pos() self.position_1 = pos() self.pos = pos() self.kalman_pos = pos() self.kalman_pos.x = 0 self.kalman_pos.y = 0 self.kalman_pos.z = 0 self.kalman_pos.yaw = 0 self.obstacle_pos = pos() self.marker_pos = {0: [1.0, 0], 1: [2.0, 0]} self.node.create_reader("/localization/tag", Tags, self.callback) self.node.create_reader("/realsense/pose", Pose, self.posecallback) self.node.create_reader("/chassis", Chassis, self.Chassiscallback) self.writer = self.node.create_writer("/localization", localization) self.writer_kalman = self.node.create_writer( "/localization/kalman_filter", pos) self.writer_obstacle = self.node.create_writer( "/localization/obstacle", pos) self.start_yaw = 0 self.init_flag = 0 self.obstacle_flag = 0 self.speed = 0 self.yaw = 0 self.kalman = kalman_filter() while not cyber.is_shutdown(): time.sleep(0.05) self.localization_with_odometer_calculation( self.speed, self.yaw, 0.05) # self.localization_with_kalman(); self.kalman.localization_with_kalman(self.speed, self.yaw) self.writer.write(self.localization) self.writer_kalman.write(self.kalman.kalman_pos) if self.obstacle_flag: self.obstacle_flag = 0 else: self.obstacle_pos.x = -1 self.obstacle_pos.y = -1 self.obstacle_pos.z = -1 self.obstacle_pos.yaw = -1 self.writer_obstacle.write(self.kalman.obstacle_pos) print(self.localization)
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 = count + 1 req.seq = count print "-" * 80 response = client.send_request(req) print "get Response [ ", response, " ]"
def test_talker_class(): """ Test talker. """ msg = SimpleMessage() msg.text = "talker:send Alex!" msg.integer = 0 test_node = cyber.Node("node_name1") g_count = 1 writer = test_node.create_writer("channel/chatter", SimpleMessage, 6) while not cyber.is_shutdown(): time.sleep(1) g_count = g_count + 1 msg.integer = g_count print "=" * 80 print "write msg -> %s" % msg writer.write(msg)
def callback(self, data): global obslist, planning_path, best_trajectory obslist = [] print(" start!!") # initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)] x = np.array([0.0, 0.0, math.pi / 2.0, 0.3, 0.0]) # goal position [x(m), y(m)] goal = np.array([data.end_point.x, data.end_point.y]) # obstacles [x(m) y(m), ....] for point in data.obs_points: obslist.append([point.x, point.y]) ob = np.matrix(obslist) # 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) self.planning_path = Trajectory() 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) if not cyber.is_shutdown() and self.planning_path: self.writer.write(self.planning_path) print("Done")
def callback(self, data): global obslist, map_w, map_h, map_x_min, map_x_max, map_y_min, map_y_max obslist = [] pStart_x = int(20 * data.start_point.x) pStart_y = int(20 * data.start_point.y) pEnd_x = int(20 * data.end_point.x) pEnd_y = int(20 * data.end_point.y - 1) pStart = Point(pStart_x, pStart_y) pEnd = Point(pEnd_x, pEnd_y) for point in data.obs_points: obslist.append([int(20 * point.x), int(20 * point.y)]) for i in range(0): obslist.append( [random.uniform(2, pEnd.y), random.uniform(2, pEnd.y)]) #time_start = time.time() # 创建AStar对象,并设置起点终点 aStar = AStar(pStart, pEnd) aStar.expansion(offset=9) # 开始寻路 pathList = aStar.start() #time_end = time.time() #print('totally cost', time_end - time_start) self.planning_path = Trajectory() if not pathList: print("Failed to find a path") else: for path_point in pathList: point_xy.x = path_point.x * 0.05 point_xy.y = path_point.y * 0.05 self.planning_path.point.append(point_xy) if not cyber.is_shutdown() and self.planning_path: self.writer.write(self.planning_path)
def test_talker_class(): """ test talker. """ msg = SimpleMessage() msg.text = "talker:send Alex!" msg.integer = 0 test_node = cyber.Node("node_name1") g_count = 1 writer = test_node.create_writer("channel/chatter", SimpleMessage, 6) while not cyber.is_shutdown(): time.sleep(1) g_count = g_count + 1 msg.integer = g_count print "="*80 print "write msg -> %s" % msg writer.write(msg)
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 __init__(self, node): self.node = node self.msg = Tags() self.localization = localization() self.position_0 = pos() self.position_1 = pos() self.pos = pos() self.marker_pos = {0: [1.0, 0], 1: [2.0, 0]} self.node.create_reader("/localization/tag", Tags, self.callback) self.node.create_reader("/realsense/pose", Pose, self.posecallback) self.node.create_reader("/chassis", Chassis, self.chassiscallback) self.writer = self.node.create_writer("/localization", localization) self.start_yaw = 0 self.init_flag = 0 self.speed = 0 self.yaw = 0 while not cyber.is_shutdown(): time.sleep(0.05) self.localization_with_odometer_calculation() self.writer.write(self.localization) print(self.localization)
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 chassis_simulation(q, q2, q3): cyber.init() #rospy.init_node("chassis_offline", anonymous=True) node = cyber.Node("chassis_offline") #chassis_pub = rospy.Publisher( # "/apollo/canbus/chassis", chassis_pb2.Chassis, queue_size=1) chassis_pub = node.create_writer("/apollo/canbus/chassis", chassis_pb2.Chassis) # generate chassis info chassis = chassis_pb2.Chassis() # send pose to /apollo/canbus/chassis count = 0 #r = rospy.Rate(20) #while not rospy.is_shutdown(): while not cyber.is_shutdown(): now = time.time() chassis_pub.write(chassis_data(chassis, count, q, q2, q3)) count += 1 sleep_time = 0.05 - (time.time() - now) if sleep_time > 0: time.sleep(sleep_time)
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() elif c == 's': self.hotkey_s() elif c == 'a': self.hotkey_a() elif c == 'd': self.hotkey_d() else: print('please type one of " W A S D"/n') print('*'*80) 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 mobileye_simulation(q): cyber.init() #rospy.init_node("mobileye_offline", anonymous=True) node = cyber.Node("mobileye_offline") #mobileye_pub = rospy.Publisher( #"/apollo/sensor/mobileye", mobileye_pb2.Mobileye, queue_size=1) mobileye_pub = node.create_writer("/apollo/sensor/mobileye", mobileye_pb2.Mobileye) # generate mobileye info mobileye = mobileye_pb2.Mobileye() # send pose to /apollo/drivers/mobileye count = 0 #r = rospy.Rate(10) while not cyber.is_shutdown(): now = time.time() #mobileye_pub.publish(mobileye_data(mobileye,count,q)) mobileye_pub.write(mobileye_data(mobileye,count,q)) count += 1 sleep_time = 0.1 - (time.time() - now) if sleep_time > 0: time.sleep(sleep_time)
if __name__ == "__main__": parser = argparse.ArgumentParser( description="Recode Analyzer is a tool to analyze record files.", 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()
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()
seq = "%05d" % header.sequence_num name = header.module_name file_path = os.path.join(g_args.out_dir, seq + "_" + name + ".pb.txt") print file_path proto_utils.write_pb_to_text_file(proto_msg, file_path) if __name__ == "__main__": parser = argparse.ArgumentParser( description="A tool to transcribe received protobuf messages into text files") parser.add_argument( "topic", action="store", help="the topic that you want to transcribe.") parser.add_argument( "--out_dir", action="store", default='.', help="the output directory for the dumped file, the default value is current directory" ) g_args = parser.parse_args() if not os.path.exists(g_args.out_dir): os.makedirs(g_args.out_dir) meta_msg = g_message_manager.get_msg_meta_by_topic(g_args.topic) if not meta_msg: print "Unknown topic name: %s" % (g_args.topic) sys.exit(0) cyber.init() node = cyber.Node("transcribe_node") node.create_reader(g_args.topic, meta_msg.msg_type, transcribe) while not cyber.is_shutdown(): time.sleep(0.005)
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()
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 - 20) <= 0: num_point = 20 #规划预瞄点 f_point = self.pathList[num_point - 20] 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_point[0] * math.cos(yaw) + f_point[1] * math.sin(yaw) y = f_point[1] * math.cos(yaw) - f_point[0] * math.sin(yaw) print("trans-goal:", [x, y]) 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, y]) ob = np.matrix(self.obstacleList) # 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) #接近终点 car_r = ((((start_x - self.pathList[0][0])**2 + (start_y - self.pathList[0][1])**2)**0.5) / scale) if car_r <= 0.3: 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")
trajGenerator = CarTrajectoryGenerator() if (params.trajectory_type == "8type"): trajectory = trajGenerator.generate_8type_traj_using_4_clothoids(car, params.min_turn_radius, params.max_vel, params.acc, params.dec, frequency=CTRL_FREQUENCY) else: trajectory = trajGenerator.generate_0type_traj_using_4_clothoids(car, params.min_turn_radius, params.max_vel, params.acc, params.dec, frequency=CTRL_FREQUENCY) while (not localization_recived and not cyber.is_shutdown()): time.sleep(0.2) cyber.shutdown() if not localization_recived: sys.exit("Cyber had been shutted down before localization message was recived!\n" + \ "Trajectory was not saved!") trajectory.transform_to(car.x, car.y, car.yaw, car.z) if not trajectory_file_handler.save_trajectory(trajectory, params.rtk_player_compatible): sys.exit("Trajectory was not saved!")