diff --git a/modules/tools/plot_control/BUILD b/modules/tools/plot_control/BUILD new file mode 100644 index 0000000000000000000000000000000000000000..49bc178f44230a6dac96a3d31ec438d9fcb489d7 --- /dev/null +++ b/modules/tools/plot_control/BUILD @@ -0,0 +1,22 @@ +load("@rules_python//python:defs.bzl", "py_binary") + +package(default_visibility = ["//visibility:public"]) + +py_binary( + name = "plot_control", + srcs = ["plot_control.py"], + deps = [ + "//cyber/python/cyber_py3:cyber", + "//modules/control/proto:control_cmd_py_pb2", + ], +) + +py_binary( + name = "realtime_test", + srcs = ["realtime_test.py"], + deps = [ + "//cyber/python/cyber_py3:cyber", + "//modules/canbus/proto:chassis_py_pb2", + "//modules/localization/proto:localization_py_pb2", + ], +) diff --git a/modules/tools/plot_control/plot_control.py b/modules/tools/plot_control/plot_control.py index bd1a5b3f7ae6b166ee402d480d21916e08877717..bf25c2c27946dc675d00765d265bf872e17f989b 100644 --- a/modules/tools/plot_control/plot_control.py +++ b/modules/tools/plot_control/plot_control.py @@ -18,10 +18,10 @@ import sys import gflags -from cyber_py3 import cyber +from cyber.python.cyber_py3 import cyber import matplotlib.pyplot as plt import matplotlib.animation as animation -from modules.control.proto import control_cmd_pb2 +from modules.control.proto import control_cmd_py_pb2 BRAKE_LINE_DATA = [] TROTTLE_LINE_DATA = [] STEERING_LINE_DATA = [] @@ -51,7 +51,7 @@ def listener(): cyber.init() test_node = cyber.Node("control_listener") test_node.create_reader("/apollo/control", - control_cmd_pb2.ControlCommand, callback) + control_cmd_py_pb2.ControlCommand, callback) test_node.spin() cyber.shutdown() diff --git a/modules/tools/plot_control/realtime_test.py b/modules/tools/plot_control/realtime_test.py index 28ac0ff371c443b42855be7a2bae72f2c9fd3ffd..a27cc3e0c191d17f52a676a09a0ad82d6a7fc22c 100644 --- a/modules/tools/plot_control/realtime_test.py +++ b/modules/tools/plot_control/realtime_test.py @@ -22,9 +22,9 @@ import datetime import os import sys -from cyber_py3 import cyber -from modules.canbus.proto import chassis_pb2 -from modules.localization.proto import localization_pb2 +from cyber.python.cyber_py3 import cyber +from modules.canbus.proto import chassis_py_pb2 +from modules.localization.proto import localization_py_pb2 SmoothParam = 9 @@ -90,6 +90,6 @@ if __name__ == '__main__': rttest.acclimit = args.acc cyber_node = cyber.Node("RealTimeTest") cyber_node.create_reader("/apollo/canbus/chassis", - chassis_pb2.Chassis, rttest.chassis_callback) + chassis_py_pb2.Chassis, rttest.chassis_callback) cyber_node.spin() cyber.shutdown() diff --git a/modules/tools/plot_planning/BUILD b/modules/tools/plot_planning/BUILD new file mode 100644 index 0000000000000000000000000000000000000000..fd2f680557c696ef9dae6260f009d6beb8ca6f4b --- /dev/null +++ b/modules/tools/plot_planning/BUILD @@ -0,0 +1,148 @@ +load("@rules_python//python:defs.bzl", "py_binary", "py_library") + +package(default_visibility = ["//visibility:public"]) + +py_binary( + name = "chassis_speed", + srcs = ["chassis_speed.py"], + deps = [ + ":record_reader", + ], +) + +py_binary( + name = "imu_acc", + srcs = ["imu_acc.py"], + deps = [ + ":record_reader", + ], +) + +py_binary( + name = "imu_angular_velocity", + srcs = ["imu_angular_velocity.py"], + deps = [ + ":record_reader", + ], +) + +py_binary( + name = "imu_av_curvature", + srcs = ["imu_av_curvature.py"], + deps = [ + ":imu_angular_velocity", + ":imu_speed", + ":record_reader", + ], +) + +py_binary( + name = "imu_speed", + srcs = ["imu_speed.py"], + deps = [ + ":record_reader", + ], +) + +py_binary( + name = "imu_speed_acc", + srcs = ["imu_speed_acc.py"], + deps = [ + ":imu_speed", + ":record_reader", + ], +) + +py_binary( + name = "imu_speed_jerk", + srcs = ["imu_speed_jerk.py"], + deps = [ + ":imu_speed_acc", + ":record_reader", + ], +) + +py_binary( + name = "planning_speed", + srcs = ["planning_speed.py"], +) + +py_binary( + name = "plot_acc_jerk", + srcs = ["plot_acc_jerk.py"], + deps = [ + ":imu_speed_acc", + ":imu_speed_jerk", + ":record_reader", + ], +) + +py_binary( + name = "plot_chassis_acc", + srcs = ["plot_chassis_acc.py"], + deps = [ + "//cyber/python/cyber_py3:cyber", + "//modules/canbus/proto:chassis_py_pb2", + "//modules/control/proto:control_cmd_py_pb2", + ], +) + +py_binary( + name = "plot_planning_acc", + srcs = ["plot_planning_acc.py"], + deps = [ + "//cyber/python/cyber_py3:cyber", + "//modules/canbus/proto:chassis_py_pb2", + "//modules/control/proto:control_cmd_py_pb2", + ], +) + +py_binary( + name = "plot_planning_speed", + srcs = ["plot_planning_speed.py"], + deps = [ + "//cyber/python/cyber_py3:cyber", + "//modules/canbus/proto:chassis_py_pb2", + "//modules/control/proto:control_cmd_py_pb2", + ], +) + +py_binary( + name = "plot_speed_jerk", + srcs = ["plot_speed_jerk.py"], + deps = [ + ":imu_speed", + ":imu_speed_jerk", + ":record_reader", + ], +) + +py_binary( + name = "plot_speed_steering", + srcs = ["plot_speed_steering.py"], + deps = [ + "//cyber/python/cyber_py3:cyber", + "//modules/canbus/proto:chassis_py_pb2", + "//modules/planning/proto:planning_py_pb2", + ], +) + +py_library( + name = "record_reader", + srcs = ["record_reader.py"], + deps = [ + "//cyber/python/cyber_py3:cyber", + "//modules/canbus/proto:chassis_py_pb2", + "//modules/localization/proto:localization_py_pb2", + "//modules/planning/proto:planning_py_pb2", + ], +) + +py_binary( + name = "speed_dsteering_data", + srcs = ["speed_dsteering_data.py"], + deps = [ + "//cyber/python/cyber_py3:cyber", + "//modules/canbus/proto:chassis_py_pb2", + ], +) diff --git a/modules/tools/plot_planning/plot_chassis_acc.py b/modules/tools/plot_planning/plot_chassis_acc.py index 36c53262bec2a3e6a9e4a3051954ea622a986f30..77e1fb0102630429553ded052f2a64431a242f9c 100644 --- a/modules/tools/plot_planning/plot_chassis_acc.py +++ b/modules/tools/plot_planning/plot_chassis_acc.py @@ -23,9 +23,9 @@ import gflags import matplotlib.animation as animation import matplotlib.pyplot as plt -from cyber_py3 import cyber -from modules.canbus.proto import chassis_pb2 -from modules.control.proto import control_cmd_pb2 +from cyber.python.cyber_py3 import cyber +from modules.canbus.proto import chassis_py_pb2 +from modules.control.proto import control_cmd_py_pb2 INIT_ACC_DATA = [] @@ -78,7 +78,7 @@ def listener(): cyber.init() test_node = cyber.Node("chassis_acc_listener") test_node.create_reader("/apollo/canbus/chassis", - chassis_pb2.Chassis, callback) + chassis_py_pb2.Chassis, callback) def compensate(data_list): diff --git a/modules/tools/plot_planning/plot_planning_acc.py b/modules/tools/plot_planning/plot_planning_acc.py index b526399b67ef480afff9c20256531976e885ad47..c8014218e73e496be53b5fba188f12bf751d6d2f 100644 --- a/modules/tools/plot_planning/plot_planning_acc.py +++ b/modules/tools/plot_planning/plot_planning_acc.py @@ -23,9 +23,9 @@ import gflags import matplotlib.animation as animation import matplotlib.pyplot as plt -from cyber_py3 import cyber -from modules.control.proto import control_cmd_pb2 -from modules.planning.proto import planning_pb2 +from cyber.python.cyber_py3 import cyber +from modules.control.proto import control_cmd_py_pb2 +from modules.planning.proto import planning_py_pb2 LAST_TRAJ_DATA = [] @@ -108,7 +108,7 @@ def listener(): cyber.init() test_node = cyber.Node("planning_acc_listener") test_node.create_reader("/apollo/planning", - planning_pb2.ADCTrajectory, callback) + planning_py_pb2.ADCTrajectory, callback) def compensate(data_list): diff --git a/modules/tools/plot_planning/plot_planning_speed.py b/modules/tools/plot_planning/plot_planning_speed.py index f2970816a2737b82a1a01218b62f4c2572430015..1faec4db26e422b56a42598b6fcdd79c6450cdcd 100644 --- a/modules/tools/plot_planning/plot_planning_speed.py +++ b/modules/tools/plot_planning/plot_planning_speed.py @@ -23,9 +23,9 @@ import gflags import matplotlib.animation as animation import matplotlib.pyplot as plt -from cyber_py3 import cyber -from modules.control.proto import control_cmd_pb2 -from modules.planning.proto import planning_pb2 +from cyber.python.cyber_py3 import cyber +from modules.control.proto import control_cmd_py_pb2 +from modules.planning.proto import planning_py_pb2 LAST_TRAJ_DATA = [] @@ -91,7 +91,7 @@ def listener(): cyber.init() test_node = cyber.Node("planning_listener") test_node.create_reader("/apollo/planning", - planning_pb2.ADCTrajectory, callback) + planning_py_pb2.ADCTrajectory, callback) def compensate(data_list): diff --git a/modules/tools/plot_planning/plot_speed_steering.py b/modules/tools/plot_planning/plot_speed_steering.py index bf80897fda2dedbc0317213844af557fe2e943f8..aa8e3ed67706a86d38c22da00a3b601d94c7776b 100644 --- a/modules/tools/plot_planning/plot_speed_steering.py +++ b/modules/tools/plot_planning/plot_speed_steering.py @@ -21,8 +21,8 @@ import sys import matplotlib.pyplot as plt -from cyber_py3.record import RecordReader -from modules.canbus.proto import chassis_pb2 +from cyber.python.cyber_py3.record import RecordReader +from modules.canbus.proto import chassis_py_pb2 def process(reader): @@ -34,14 +34,14 @@ def process(reader): for msg in reader.read_messages(): if msg.topic == "/apollo/canbus/chassis": - chassis = chassis_pb2.Chassis() + chassis = chassis_py_pb2.Chassis() chassis.ParseFromString(msg.message) steering_percentage = chassis.steering_percentage speed_mps = chassis.speed_mps timestamp_sec = chassis.header.timestamp_sec - if chassis.driving_mode != chassis_pb2.Chassis.COMPLETE_AUTO_DRIVE: + if chassis.driving_mode != chassis_py_pb2.Chassis.COMPLETE_AUTO_DRIVE: last_steering_percentage = steering_percentage last_speed_mps = speed_mps last_timestamp_sec = timestamp_sec diff --git a/modules/tools/plot_planning/record_reader.py b/modules/tools/plot_planning/record_reader.py index b930120c365d1b4d5f0578c8087ac8f4636f4741..f879438665a60aa3595f811a14fb2a386a3d08d9 100644 --- a/modules/tools/plot_planning/record_reader.py +++ b/modules/tools/plot_planning/record_reader.py @@ -16,10 +16,10 @@ # limitations under the License. ############################################################################### -from cyber_py3.record import RecordReader -from modules.canbus.proto import chassis_pb2 -from modules.localization.proto import localization_pb2 -from modules.planning.proto import planning_pb2 +from cyber.python.cyber_py3.record import RecordReader +from modules.canbus.proto import chassis_py_pb2 +from modules.localization.proto import localization_py_pb2 +from modules.planning.proto import planning_py_pb2 class RecordItemReader: @@ -32,19 +32,19 @@ class RecordItemReader: if msg.topic not in topics: continue if msg.topic == "/apollo/canbus/chassis": - chassis = chassis_pb2.Chassis() + chassis = chassis_py_pb2.Chassis() chassis.ParseFromString(msg.message) data = {"chassis": chassis} yield data if msg.topic == "/apollo/localization/pose": - location_est = localization_pb2.LocalizationEstimate() + location_est = localization_py_pb2.LocalizationEstimate() location_est.ParseFromString(msg.message) data = {"pose": location_est} yield data if msg.topic == "/apollo/planning": - planning = planning_pb2.ADCTrajectory() + planning = planning_py_pb2.ADCTrajectory() planning.ParseFromString(msg.message) data = {"planning": planning} yield data diff --git a/modules/tools/plot_planning/speed_dsteering_data.py b/modules/tools/plot_planning/speed_dsteering_data.py index 7dd715322307691d45966099a732f4cb77c7743b..1bfaa86c384b40fe49e9dd4a90f5b1817ccabb12 100644 --- a/modules/tools/plot_planning/speed_dsteering_data.py +++ b/modules/tools/plot_planning/speed_dsteering_data.py @@ -19,8 +19,8 @@ import sys from record_reader import RecordItemReader import matplotlib.pyplot as plt -from cyber_py3.record import RecordReader -from modules.canbus.proto import chassis_pb2 +from cyber.python.cyber_py3.record import RecordReader +from modules.canbus.proto import chassis_py_pb2 class SpeedDsteeringData: diff --git a/modules/tools/plot_trace/BUILD b/modules/tools/plot_trace/BUILD new file mode 100644 index 0000000000000000000000000000000000000000..cd9f557e2a084e1d2afecdcf3cd2e68d452743e4 --- /dev/null +++ b/modules/tools/plot_trace/BUILD @@ -0,0 +1,23 @@ +load("@rules_python//python:defs.bzl", "py_binary") + +package(default_visibility = ["//visibility:public"]) + +py_binary( + name = "plot_planning_result", + srcs = ["plot_planning_result.py"], + deps = [ + "//modules/canbus/proto:chassis_py_pb2", + "//modules/localization/proto:localization_py_pb2", + "//modules/planning/proto:planning_py_pb2", + ], +) + +py_binary( + name = "plot_trace", + srcs = ["plot_trace.py"], + deps = [ + "//cyber/python/cyber_py3:cyber", + "//modules/canbus/proto:chassis_py_pb2", + "//modules/localization/proto:localization_py_pb2", + ], +) diff --git a/modules/tools/plot_trace/plot_planning_result.py b/modules/tools/plot_trace/plot_planning_result.py index 60a7126291c022a3ef5533c4b92afa87387f8a09..3f0184d0ebafe3559a4ee7d939af35074dd340c6 100755 --- a/modules/tools/plot_trace/plot_planning_result.py +++ b/modules/tools/plot_trace/plot_planning_result.py @@ -26,9 +26,9 @@ import matplotlib.animation as animation import matplotlib.pyplot as plt import numpy as np -from modules.canbus.proto import chassis_pb2 -from modules.localization.proto import localization_pb2 -from modules.planning.proto import planning_pb2 +from modules.canbus.proto import chassis_py_pb2 +from modules.localization.proto import localization_py_pb2 +from modules.planning.proto import planning_py_pb2 g_args = None @@ -56,7 +56,7 @@ def get_debug_paths(planning_pb): def plot_planning(ax, planning_file): with open(planning_file, 'r') as fp: - planning_pb = planning_pb2.ADCTrajectory() + planning_pb = planning_py_pb2.ADCTrajectory() text_format.Merge(fp.read(), planning_pb) trajectory = get_3d_trajectory(planning_pb) ax.plot(trajectory[0], trajectory[1], trajectory[2], diff --git a/modules/tools/plot_trace/plot_trace.py b/modules/tools/plot_trace/plot_trace.py index daeb4974d6221369b4814e1709eff7b7bad733bc..1f3f8c40edccbea49abeca17d1e403c645cc9945 100755 --- a/modules/tools/plot_trace/plot_trace.py +++ b/modules/tools/plot_trace/plot_trace.py @@ -22,8 +22,9 @@ import matplotlib.animation as animation import matplotlib.pyplot as plt import numpy as np -from modules.canbus.proto import chassis_pb2 -from modules.localization.proto import localization_pb2 +from cyber.python.cyber_py3 import cyber +from modules.canbus.proto import chassis_py_pb2 +from modules.localization.proto import localization_py_pb2 GPS_X = list() @@ -37,7 +38,7 @@ IS_AUTO_MODE = False def chassis_callback(chassis_data): global IS_AUTO_MODE - if chassis_data.driving_mode == chassis_pb2.Chassis.COMPLETE_AUTO_DRIVE: + if chassis_data.driving_mode == chassis_py_pb2.Chassis.COMPLETE_AUTO_DRIVE: IS_AUTO_MODE = True else: IS_AUTO_MODE = False @@ -55,9 +56,9 @@ def localization_callback(localization_data): def setup_listener(node): - node.create_reader(CHASSIS_TOPIC, chassis_pb2.Chassis, chassis_callback) + node.create_reader(CHASSIS_TOPIC, chassis_py_pb2.Chassis, chassis_callback) node.create_reader(LOCALIZATION_TOPIC, - localization_pb2.LocalizationEstimate, + localization_py_pb2.LocalizationEstimate, localization_callback) while not cyber.is_shutdown(): time.sleep(0.002) diff --git a/modules/tools/record_play/BUILD b/modules/tools/record_play/BUILD new file mode 100644 index 0000000000000000000000000000000000000000..d81926002f563d680d03b276b620437c98846cc8 --- /dev/null +++ b/modules/tools/record_play/BUILD @@ -0,0 +1,31 @@ +load("@rules_python//python:defs.bzl", "py_binary") + +package(default_visibility = ["//visibility:public"]) + +py_binary( + name = "rtk_player", + srcs = ["rtk_player.py"], + deps = [ + "//cyber/python/cyber_py3:cyber", + "//cyber/python/cyber_py3:cyber_time", + "//modules/canbus/proto:chassis_py_pb2", + "//modules/common/configs/proto:vehicle_config_py_pb2", + "//modules/common/proto:drive_state_py_pb2", + "//modules/common/proto:pnc_point_py_pb2", + "//modules/control/proto:pad_msg_py_pb2", + "//modules/localization/proto:localization_py_pb2", + "//modules/planning/proto:planning_py_pb2", + "//modules/tools/common:proto_utils", + ], +) + +py_binary( + name = "rtk_recorder", + srcs = ["rtk_recorder.py"], + deps = [ + "//cyber/python/cyber_py3:cyber", + "//modules/canbus/proto:chassis_py_pb2", + "//modules/localization/proto:localization_py_pb2", + "//modules/tools/common:proto_utils", + ], +) diff --git a/modules/tools/record_play/rtk_player.py b/modules/tools/record_play/rtk_player.py index bae74c85fb8955740b05f7c2bdad3a2de323df75..9f12fca0563afbbf84f36db4022c1f434f1ce037 100644 --- a/modules/tools/record_play/rtk_player.py +++ b/modules/tools/record_play/rtk_player.py @@ -30,17 +30,17 @@ import time from numpy import genfromtxt import scipy.signal as signal -from cyber_py3 import cyber -from cyber_py3 import cyber_time -from common.logger import Logger -from modules.canbus.proto import chassis_pb2 -from modules.common.configs.proto import vehicle_config_pb2 -from modules.common.proto import drive_state_pb2 -from modules.common.proto import pnc_point_pb2 -from modules.control.proto import pad_msg_pb2 -from modules.localization.proto import localization_pb2 -from modules.planning.proto import planning_pb2 -import common.proto_utils as proto_utils +from cyber.python.cyber_py3 import cyber +from cyber.python.cyber_py3 import cyber_time +from modules.tools.common.logger import Logger +from modules.canbus.proto import chassis_py_pb2 +from modules.common.configs.proto import vehicle_config_py_pb2 +from modules.common.proto import drive_state_py_pb2 +from modules.common.proto import pnc_point_py_pb2 +from modules.control.proto import pad_msg_py_pb2 +from modules.localization.proto import localization_py_pb2 +from modules.planning.proto import planning_py_pb2 +import modules.tools.common.proto_utils as proto_utils APOLLO_ROOT = os.path.join(os.path.dirname(__file__), '../../../') @@ -69,14 +69,14 @@ class RtkPlayer(object): self.data = genfromtxt(file_handler, delimiter=',', names=True) file_handler.close() - self.localization = localization_pb2.LocalizationEstimate() - self.chassis = chassis_pb2.Chassis() - self.padmsg = pad_msg_pb2.PadMessage() + self.localization = localization_py_pb2.LocalizationEstimate() + self.chassis = chassis_py_pb2.Chassis() + self.padmsg = pad_msg_py_pb2.PadMessage() self.localization_received = False self.chassis_received = False self.planning_pub = node.create_writer('/apollo/planning', - planning_pb2.ADCTrajectory) + planning_py_pb2.ADCTrajectory) self.speedmultiplier = speedmultiplier / 100 self.terminating = False @@ -97,7 +97,7 @@ class RtkPlayer(object): self.estop = False self.logger.info("Planning Ready") - vehicle_config = vehicle_config_pb2.VehicleConfig() + vehicle_config = vehicle_config_py_pb2.VehicleConfig() proto_utils.get_pb_from_text_file( "/apollo/modules/common/data/vehicle_param.pb.txt", vehicle_config) self.vehicle_param = vehicle_config.vehicle_param @@ -118,7 +118,7 @@ class RtkPlayer(object): """ self.chassis.CopyFrom(data) self.automode = (self.chassis.driving_mode == - chassis_pb2.Chassis.COMPLETE_AUTO_DRIVE) + chassis_py_pb2.Chassis.COMPLETE_AUTO_DRIVE) self.chassis_received = True def padmsg_callback(self, data): @@ -212,7 +212,7 @@ class RtkPlayer(object): "localization not received yet when publish_planningmsg") return - planningdata = planning_pb2.ADCTrajectory() + planningdata = planning_py_pb2.ADCTrajectory() now = cyber_time.Time.now().to_sec() planningdata.header.timestamp_sec = now planningdata.header.module_name = "planning" @@ -269,10 +269,10 @@ class RtkPlayer(object): self.data['time'][self.start] planningdata.gear = int(self.data['gear'][self.closest_time()]) planningdata.engage_advice.advice = \ - drive_state_pb2.EngageAdvice.READY_TO_ENGAGE + drive_state_py_pb2.EngageAdvice.READY_TO_ENGAGE for i in range(self.start, self.end): - adc_point = pnc_point_pb2.TrajectoryPoint() + adc_point = pnc_point_py_pb2.TrajectoryPoint() adc_point.path_point.x = self.data['x'][i] adc_point.path_point.y = self.data['y'][i] adc_point.path_point.z = self.data['z'][i] @@ -292,7 +292,7 @@ class RtkPlayer(object): (self.vehicle_param.length // 2 - self.vehicle_param.back_edge_to_center) * \ math.sin(adc_point.path_point.theta) - if planningdata.gear == chassis_pb2.Chassis.GEAR_REVERSE: + if planningdata.gear == chassis_py_pb2.Chassis.GEAR_REVERSE: adc_point.v = -adc_point.v adc_point.path_point.s = -adc_point.path_point.s @@ -359,14 +359,14 @@ def main(): args['complete'].lower(), args['replan'].lower()) atexit.register(player.shutdown) - node.create_reader('/apollo/canbus/chassis', chassis_pb2.Chassis, + node.create_reader('/apollo/canbus/chassis', chassis_py_pb2.Chassis, player.chassis_callback) node.create_reader('/apollo/localization/pose', - localization_pb2.LocalizationEstimate, + localization_py_pb2.LocalizationEstimate, player.localization_callback) - node.create_reader('/apollo/control/pad', pad_msg_pb2.PadMessage, + node.create_reader('/apollo/control/pad', pad_msg_py_pb2.PadMessage, player.padmsg_callback) while not cyber.is_shutdown(): diff --git a/modules/tools/record_play/rtk_recorder.py b/modules/tools/record_play/rtk_recorder.py index 7e2a822fd19c0a7137a6088e87ecc6b60a8f0e57..ac33c9c75b1ee2ca1452288e16ed7548e6704b4b 100644 --- a/modules/tools/record_play/rtk_recorder.py +++ b/modules/tools/record_play/rtk_recorder.py @@ -26,12 +26,12 @@ import os import sys import time -from cyber_py3 import cyber +from cyber.python.cyber_py3 import cyber from gflags import FLAGS -from common.logger import Logger -from modules.canbus.proto import chassis_pb2 -from modules.localization.proto import localization_pb2 +from modules.tools.common.logger import Logger +from modules.canbus.proto import chassis_py_pb2 +from modules.localization.proto import localization_py_pb2 class RtkRecord(object): @@ -60,8 +60,8 @@ class RtkRecord(object): self.write("x,y,z,speed,acceleration,curvature," "curvature_change_rate,time,theta,gear,s,throttle,brake,steering\n") - self.localization = localization_pb2.LocalizationEstimate() - self.chassis = chassis_pb2.Chassis() + self.localization = localization_py_pb2.LocalizationEstimate() + self.chassis = chassis_py_pb2.Chassis() self.chassis_received = False self.cars = 0.0 @@ -185,11 +185,11 @@ def main(argv): recorder = RtkRecord(record_file) atexit.register(recorder.shutdown) node.create_reader('/apollo/canbus/chassis', - chassis_pb2.Chassis, + chassis_py_pb2.Chassis, recorder.chassis_callback) node.create_reader('/apollo/localization/pose', - localization_pb2.LocalizationEstimate, + localization_py_pb2.LocalizationEstimate, recorder.localization_callback) while not cyber.is_shutdown(): diff --git a/modules/tools/routing/BUILD b/modules/tools/routing/BUILD new file mode 100644 index 0000000000000000000000000000000000000000..9134e5f2c06ef79595b0c08d316d98f94ca34e5e --- /dev/null +++ b/modules/tools/routing/BUILD @@ -0,0 +1,54 @@ +load("@rules_python//python:defs.bzl", "py_binary", "py_library") + +package(default_visibility = ["//visibility:public"]) + +py_binary( + name = "debug_passage_region", + srcs = ["debug_passage_region.py"], + deps = [ + ":debug_topo", + "//modules/routing/proto:routing_py_pb2", + "//modules/routing/proto:topo_graph_py_pb2", + "//modules/tools/common:proto_utils", + ], +) + +py_binary( + name = "debug_route", + srcs = ["debug_route.py"], + deps = [ + ":debug_topo", + "//modules/routing/proto:topo_graph_py_pb2", + "//modules/tools/common:proto_utils", + ], +) + +py_library( + name = "debug_topo", + srcs = ["debug_topo.py"], + deps = [ + ":util", + "//modules/routing/proto:topo_graph_py_pb2", + "//modules/tools/common:proto_utils", + ], +) + +py_binary( + name = "road_show", + srcs = ["road_show.py"], + deps = [ + ":util", + "//modules/tools/common:proto_utils", + ], +) + +py_library( + name = "util", + srcs = ["util.py"], + deps = [ + "//modules/map/proto:map_py_pb2", + "//modules/routing/proto:routing_py_pb2", + "//modules/routing/proto:topo_graph_py_pb2", + "//modules/tools/common:proto_utils", + ], +) diff --git a/modules/tools/routing/debug_passage_region.py b/modules/tools/routing/debug_passage_region.py index cdf8335c0b38dfddc80386e3a6d88fa5f1f00044..99105d10c46a12e685fcf2cf871bfa6d89ef5676 100644 --- a/modules/tools/routing/debug_passage_region.py +++ b/modules/tools/routing/debug_passage_region.py @@ -21,10 +21,10 @@ import sys import matplotlib.pyplot as plt -import common.proto_utils as proto_utils -import debug_topo -from modules.routing.proto.routing_pb2 import RoutingResponse -from modules.routing.proto.topo_graph_pb2 import Graph +import modules.tools.common.proto_utils as proto_utils +import modules.tools.routing.debug_topo as debug_topo +from modules.routing.proto.routing_py_pb2 import RoutingResponse +from modules.routing.proto.topo_graph_py_pb2 import Graph color_iter = itertools.cycle( diff --git a/modules/tools/routing/debug_route.py b/modules/tools/routing/debug_route.py index ef1c6a72c06271b1096606a840fe702307724dbb..acf81793496dbdcd80bc364f95523182c22e5568 100644 --- a/modules/tools/routing/debug_route.py +++ b/modules/tools/routing/debug_route.py @@ -23,8 +23,8 @@ import sys import gflags import matplotlib.pyplot as plt -import debug_topo -import modules.routing.proto.topo_graph_pb2 as topo_graph_pb2 +import modules.tools.routing.debug_topo as debug_topo +import modules.routing.proto.topo_graph_py_pb2 as topo_graph_py_pb2 import util diff --git a/modules/tools/routing/debug_topo.py b/modules/tools/routing/debug_topo.py index 5af29aafc0965a2baa1ec939d8d37416b2bbe835..43858c16499d001a3c1fd27532bb79fe8dee8f8a 100644 --- a/modules/tools/routing/debug_topo.py +++ b/modules/tools/routing/debug_topo.py @@ -22,9 +22,9 @@ import sys import matplotlib.pyplot as plt -import common.proto_utils as proto_utils -import modules.routing.proto.topo_graph_pb2 as topo_graph_pb2 -import util +import modules.tools.common.proto_utils as proto_utils +import modules.routing.proto.topo_graph_py_pb2 as topo_graph_py_pb2 +import modules.tools.routing.util as util color_iter = itertools.cycle( @@ -139,7 +139,7 @@ def plot_node(node, plot_id, color): def plot_edge(edge, midddle_point_map): """plot topology graph edge""" - if edge.direction_type == topo_graph_pb2.Edge.FORWARD: + if edge.direction_type == topo_graph_py_pb2.Edge.FORWARD: return # if lane change is allowed, draw an arrow from lane with from_id to lane with to_id from_id = edge.from_lane_id diff --git a/modules/tools/routing/road_show.py b/modules/tools/routing/road_show.py index 967a06ac51bf78f40986c0a0ecd70b50a7f184fc..a374ee39704debd5cf7da152dc8c7a6217bbe766 100644 --- a/modules/tools/routing/road_show.py +++ b/modules/tools/routing/road_show.py @@ -21,8 +21,8 @@ import sys import matplotlib.pyplot as plt -import common.proto_utils as proto_utils -import util +import modules.tools.common.proto_utils as proto_utils +import modules.tools.routing.util as util g_color = [ diff --git a/modules/tools/routing/util.py b/modules/tools/routing/util.py index a810c629886b21389000ecef7ae60e4e4f5ea2a9..b4a1a7e0d055e143da1e58cc1d618f99b81181d6 100644 --- a/modules/tools/routing/util.py +++ b/modules/tools/routing/util.py @@ -23,9 +23,9 @@ import gflags import matplotlib.pyplot as plt import common.proto_utils as proto_utils -import modules.map.proto.map_pb2 as map_pb2 -import modules.routing.proto.topo_graph_pb2 as topo_graph_pb2 -import modules.routing.proto.routing_pb2 as routing_pb2 +import modules.map.proto.map_py_pb2 as map_py_pb2 +import modules.routing.proto.topo_graph_py_pb2 as topo_graph_py_pb2 +import modules.routing.proto.routing_py_pb2 as routing_py_pb2 FLAGS = gflags.FLAGS gflags.DEFINE_string('map_dir', 'modules/map/data/demo', 'map directory') @@ -49,14 +49,14 @@ def get_mapdata(map_dir): print('Please wait for loading map data...') map_data_path = os.path.join(map_dir, 'base_map.bin') print('File: %s' % map_data_path) - return proto_utils.get_pb_from_bin_file(map_data_path, map_pb2.Map()) + return proto_utils.get_pb_from_bin_file(map_data_path, map_py_pb2.Map()) def get_topodata(map_dir): print('Please wait for loading routing topo data...') topo_data_path = os.path.join(map_dir, 'routing_map.bin') print("File: %s" % topo_data_path) - return proto_utils.get_pb_from_bin_file(topo_data_path, topo_graph_pb2.Graph()) + return proto_utils.get_pb_from_bin_file(topo_data_path, topo_graph_py_pb2.Graph()) def get_routingdata(): @@ -65,7 +65,7 @@ def get_routingdata(): os.path.join(os.path.dirname(__file__), '../../../data/log')) route_data_path = os.path.join(log_dir, 'passage_region_debug.bin') print("File: %s" % route_data_path) - return proto_utils.get_pb_from_text_file(route_data_path, routing_pb2.RoutingResponse()) + return proto_utils.get_pb_from_text_file(route_data_path, routing_py_pb2.RoutingResponse()) def onclick(event):