diff --git a/modules/tools/replay/replay_file.py b/modules/tools/replay/replay_file.py index 59d1116e3242d00b74d616b9fbb3154e6fcca99d..a2c22584e6acc9fe94c4ca2ccb15d7601b9cc699 100644 --- a/modules/tools/replay/replay_file.py +++ b/modules/tools/replay/replay_file.py @@ -18,7 +18,7 @@ """ This program can replay a planning output pb file via ros """ - +import os.path import sys import argparse import rospy @@ -33,13 +33,12 @@ from modules.planning.proto import planning_pb2 from modules.prediction.proto import prediction_obstacle_pb2 from modules.routing.proto import routing_pb2 - def generate_message(topic, filename): """generate message from file""" message = None if topic == "/apollo/planning": message = planning_pb2.ADCTrajectory() - elif topic == "/apollo/localizatin/pose": + elif topic == "/apollo/localization/pose": message = localization_pb2.LocalizationEstimate() elif topic == "/apollo/perception/obstacles": message = perception_obstacle_pb2.PerceptionObstacles() @@ -48,8 +47,10 @@ def generate_message(topic, filename): elif topic == "/apollo/routing_response": message = routing_pb2.RoutingResponse() if not message: - print "Unknown topic : " << topic << ", you can add it here" + print "Unknown topic:", topic sys.exit(0) + if not os.path.exists(filename): + return None f_handle = file(filename, 'r') text_format.Merge(f_handle.read(), message) f_handle.close() @@ -66,12 +67,45 @@ def topic_publisher(topic, filename, period): pub.publish(str(message)) rate.sleep() +def seq_publisher(seq_num, period): + """publisher""" + rospy.init_node('replay_node', anonymous=True) + + # topic_name => module_name, pb type, pb, publish_handler + topic_name_map = { + "/apollo/localization/pose" : ["localization", + localization_pb2.LocalizationEstimate, None, None], + "/apollo/routing_response" : ["routing", + routing_pb2.RoutingResponse, None, None], + "/apollo/perception/obstacles" : ["perception", + perception_obstacle_pb2.PerceptionObstacles, None, None], + "/apollo/prediction" : ["prediction", + prediction_obstacle_pb2.PredictionObstacles, None, None], + "/apollo/planning" : ["planning", + planning_pb2.ADCTrajectory, None, None], + } + for topic, module_features in topic_name_map.iteritems(): + filename = str(seq_num) + "_" + module_features[0] + ".pb.txt" + print "trying to load pb file:", filename + module_features[3] = rospy.Publisher(topic, + module_features[1], queue_size=1) + module_features[2] = generate_message(topic, filename) + if module_features[2] is None: + print topic, " pb is none" + + rate = rospy.Rate(int(1.0 / period)) # 10hz + while not rospy.is_shutdown(): + for topic, module_features in topic_name_map.iteritems(): + if not module_features[2] is None: + module_features[3].publish(module_features[2]) + rate.sleep() + if __name__ == '__main__': parser = argparse.ArgumentParser( description="replay a planning result pb file") parser.add_argument( - "filename", action="store", type=str, help="planning result files") + "--filename", action="store", type=str, help="planning result files") parser.add_argument( "--topic", action="store", @@ -82,10 +116,20 @@ if __name__ == '__main__': "--period", action="store", type=float, - default=0.1, + default=1, help="set the topic publish time duration") + parser.add_argument( + "--seq", + action="store", + type=int, + default=-1, + help="set sequence number to replay") args = parser.parse_args() try: - topic_publisher(args.topic, args.filename, args.period) + if (args.seq > 0): + seq_publisher(args.seq, args.period) + else: + topic_publisher(args.topic, args.filename, args.period) + except rospy.ROSInterruptException: print "failed to replay message"