def __init__(self, record_file):
self.firstvalid = False
self.logger = Logger.get_logger("RtkRecord")
self.record_file = record_file
self.logger.info("Record file to: " + record_file)
try:
self.file_handler = open(record_file, 'w')
except:
self.logger.error("open file %s failed" % (record_file))
self.file_handler.close()
sys.exit()
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.chassis_received = False
self.cars = 0.0
self.startmoving = False
self.terminating = False
self.carcurvature = 0.0
self.prev_carspeed = 0.0
def __init__(self, record_file):
self.firstvalid = False
self.logger = Logger.get_logger("RtkRecord")
self.record_file = record_file
self.logger.info("Record file to: " + record_file)
try:
self.file_handler = open(record_file, 'w')
except IOError:
self.logger.error("Open file %s failed" % (record_file))
self.file_handler.close()
sys.exit(1)
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.chassis_received = False
self.cars = 0.0
self.startmoving = False
self.terminating = False
self.carcurvature = 0.0
self.prev_carspeed = 0.0
vehicle_config = vehicle_config_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
def process(control_analyzer, planning_analyzer, lidar_endtoend_analyzer):
is_auto_drive = False
for msg in reader.read_messages():
if msg.topic == "/apollo/canbus/chassis":
chassis = chassis_pb2.Chassis()
chassis.ParseFromString(msg.message)
if chassis.driving_mode == \
chassis_pb2.Chassis.COMPLETE_AUTO_DRIVE:
is_auto_drive = True
else:
is_auto_drive = False
if msg.topic == "/apollo/control":
if not is_auto_drive:
continue
control_cmd = control_cmd_pb2.ControlCommand()
control_cmd.ParseFromString(msg.message)
control_analyzer.put(control_cmd)
lidar_endtoend_analyzer.put_control(control_cmd)
if msg.topic == "/apollo/planning":
if not is_auto_drive:
continue
adc_trajectory = planning_pb2.ADCTrajectory()
adc_trajectory.ParseFromString(msg.message)
planning_analyzer.put(adc_trajectory)
lidar_endtoend_analyzer.put_planning(adc_trajectory)
if msg.topic == "/apollo/sensor/velodyne64/compensator/PointCloud2":
if not is_auto_drive:
continue
point_cloud = pointcloud_pb2.PointCloud()
point_cloud.ParseFromString(msg.message)
lidar_endtoend_analyzer.put_lidar(point_cloud)
def process_file(self, record_file):
"""
Extract information from record file.
Return True if we are done collecting all information.
"""
try:
reader = RecordReader(record_file)
print("Begin to process record file {}".format(record_file))
for msg in reader.read_messages():
print(msg.topic)
if msg.topic == kChassisInfoTopic and self.vehicle_vin is None:
chassis = chassis_pb2.Chassis()
chassis.ParseFromString(msg.message)
if chassis.license.vin:
self.vehicle_vin = chassis.license.vin
elif msg.topic == kHMIInfoTopic and self.vehicle_name is None:
hmistatus = hmi_status_pb2.HMIStatus()
hmistatus.ParseFromString(msg.message)
if hmistatus.current_map:
self.vehicle_name = hmistatus.current_map
print(self.vehicle_name)
if self.done():
return True
except:
return False
print("Finished processing record file {}".format(record_file))
return self.done()
def process_file(self, bag_file):
"""
Extract information from bag file.
Return True if we are done collecting all information.
"""
try:
reader = RecordReader(bag_file)
print "begin"
for msg in reader.read_messages():
if msg.topic == kChassisInfoTopic and self.vehicle_vin == None:
chassis = chassis_pb2.Chassis()
chassis.ParseFromString(msg.message)
if chassis.license.vin:
self.vehicle_vin = chassis.license.vin
elif msg.topic == kHMIInfoTopic and self.vehicle_name == None:
hmistatus = hmi_status_pb2.HMIStatus()
hmistatus.ParseFromString(msg.message)
print hmistatus.MessageType
if hmistatus.current_vehicle:
self.vehicle_name = hmistatus.current_vehicle
print self.vehicle_name
if self.done():
return True
except:
return False
return self.done()
def calculate(self, bag_file):
"""
Calculate mileage
"""
last_pos = None
last_mode = 'Unknown'
mileage = collections.defaultdict(lambda: 0.0)
chassis = chassis_pb2.Chassis()
localization = localization_pb2.LocalizationEstimate()
reader = RecordReader(bag_file)
for msg in reader.read_messages():
if msg.topic == kChassisTopic:
chassis.ParseFromString(msg.message)
# Mode changed
if last_mode != chassis.driving_mode:
if (last_mode == Chassis.COMPLETE_AUTO_DRIVE and
chassis.driving_mode == Chassis.EMERGENCY_MODE):
self.disengagements += 1
last_mode = chassis.driving_mode
# Reset start position.
last_pos = None
elif msg.topic == kLocalizationTopic:
localization.ParseFromString(msg.message)
cur_pos = localization.pose.position
if last_pos:
# Accumulate mileage, from xyz-distance to miles.
mileage[last_mode] += 0.000621371 * math.sqrt(
(cur_pos.x - last_pos.x)**2 +
(cur_pos.y - last_pos.y)**2 +
(cur_pos.z - last_pos.z)**2)
last_pos = cur_pos
self.auto_mileage += mileage[Chassis.COMPLETE_AUTO_DRIVE]
self.manual_mileage += (mileage[Chassis.COMPLETE_MANUAL] +
mileage[Chassis.EMERGENCY_MODE])
def process(control_analyzer, planning_analyzer, lidar_endtoend_analyzer,
is_simulation, plot_planning_path, plot_planning_refpath, all_data):
is_auto_drive = False
for msg in reader.read_messages():
if msg.topic == "/apollo/canbus/chassis":
chassis = chassis_pb2.Chassis()
chassis.ParseFromString(msg.message)
if chassis.driving_mode == \
chassis_pb2.Chassis.COMPLETE_AUTO_DRIVE:
is_auto_drive = True
else:
is_auto_drive = False
if msg.topic == "/apollo/control":
if (not is_auto_drive and not all_data) or \
is_simulation or plot_planning_path or plot_planning_refpath:
continue
control_cmd = control_cmd_pb2.ControlCommand()
control_cmd.ParseFromString(msg.message)
control_analyzer.put(control_cmd)
lidar_endtoend_analyzer.put_pb('control', control_cmd)
if msg.topic == "/apollo/planning":
if (not is_auto_drive) and (not all_data):
continue
adc_trajectory = planning_pb2.ADCTrajectory()
adc_trajectory.ParseFromString(msg.message)
planning_analyzer.put(adc_trajectory)
lidar_endtoend_analyzer.put_pb('planning', adc_trajectory)
if plot_planning_path:
planning_analyzer.plot_path(plt, adc_trajectory)
if plot_planning_refpath:
planning_analyzer.plot_refpath(plt, adc_trajectory)
if msg.topic == "/apollo/sensor/velodyne64/compensator/PointCloud2" or \
msg.topic == "/apollo/sensor/lidar128/compensator/PointCloud2":
if ((not is_auto_drive) and (not all_data)) or is_simulation or \
plot_planning_path or plot_planning_refpath:
continue
point_cloud = pointcloud_pb2.PointCloud()
point_cloud.ParseFromString(msg.message)
lidar_endtoend_analyzer.put_lidar(point_cloud)
if msg.topic == "/apollo/perception/obstacles":
if ((not is_auto_drive) and (not all_data)) or is_simulation or \
plot_planning_path or plot_planning_refpath:
continue
perception = perception_obstacle_pb2.PerceptionObstacles()
perception.ParseFromString(msg.message)
lidar_endtoend_analyzer.put_pb('perception', perception)
if msg.topic == "/apollo/prediction":
if ((not is_auto_drive) and (not all_data)) or is_simulation or \
plot_planning_path or plot_planning_refpath:
continue
prediction = prediction_obstacle_pb2.PredictionObstacles()
prediction.ParseFromString(msg.message)
lidar_endtoend_analyzer.put_pb('prediction', prediction)
def callback_chassis(self, data):
"""
New localization pose
"""
entity = chassis_pb2.Chassis()
entity.CopyFrom(data)
self.carspeed = entity.speed_mps
self.steer_angle = entity.steering_percentage / 100 * MaxSteerAngle / SteerRatio
self.autodrive = (
entity.driving_mode == chassis_pb2.Chassis.COMPLETE_AUTO_DRIVE)
self.carcurvature = math.tan(math.radians(
self.steer_angle)) / VehicleLength
def __init__(self, record_file, speedmultiplier, completepath, replan):
"""Init player."""
self.firstvalid = False
self.logger = Logger.get_logger(tag="RtkPlayer")
self.logger.info("Load record file from: %s" % record_file)
try:
file_handler = open(record_file, 'r')
except:
self.logger.error("Cannot find file: " + record_file)
file_handler.close()
sys.exit(0)
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_received = False
self.chassis_received = False
self.planning_pub = rospy.Publisher('/apollo/planning',
planning_pb2.ADCTrajectory,
queue_size=1)
self.speedmultiplier = speedmultiplier / 100
self.terminating = False
self.sequence_num = 0
b, a = signal.butter(6, 0.05, 'low')
self.data['acceleration'] = signal.filtfilt(b, a,
self.data['acceleration'])
self.start = 0
self.end = 0
self.closestpoint = 0
self.automode = False
self.replan = (replan == 't')
self.completepath = (completepath == 't')
self.estop = False
# Report status to HMI.
status_pb = runtime_status_pb2.RuntimeStatus()
status_pb.tools.planning_ready = True
hmi_status_helper.HMIStatusHelper.report_status(status_pb)
self.logger.info("Planning Ready")
def main():
rospy.init_node("chassis_offline", anonymous=True)
chassis_pub = rospy.Publisher("/apollo/canbus/chassis",
chassis_pb2.Chassis,
queue_size=1)
# generate chassis info
chassis = chassis_pb2.Chassis()
# send pose to /apollo/canbus/chassis
r = rospy.Rate(1)
count = 0
while not rospy.is_shutdown():
chassis_pub.publish(chassis_data(chassis, count))
count += 1
r.sleep()
def __init__(self, record_file, node, speedmultiplier, completepath,
replan):
"""Init player."""
self.firstvalid = False
self.logger = Logger.get_logger(tag="RtkPlayer")
self.logger.info("Load record file from: %s" % record_file)
try:
file_handler = open(record_file, 'r')
except IOError:
self.logger.error("Cannot find file: " + record_file)
file_handler.close()
sys.exit(0)
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_received = False
self.chassis_received = False
self.planning_pub = node.create_writer('/apollo/planning',
planning_pb2.ADCTrajectory)
self.speedmultiplier = speedmultiplier / 100
self.terminating = False
self.sequence_num = 0
b, a = signal.butter(6, 0.05, 'low')
self.data['acceleration'] = signal.filtfilt(b, a,
self.data['acceleration'])
self.start = 0
self.end = 0
self.closestpoint = 0
self.automode = False
self.replan = (replan == 't')
self.completepath = (completepath == 't')
self.estop = False
self.logger.info("Planning Ready")
vehicle_config = vehicle_config_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
def read(self, topics):
reader = RecordReader(self.record_file)
for msg in reader.read_messages():
if msg.topic not in topics:
continue
if msg.topic == "/apollo/canbus/chassis":
chassis = chassis_pb2.Chassis()
chassis.ParseFromString(msg.message)
data = {"chassis": chassis}
yield data
if msg.topic == "/apollo/localization/pose":
location_est = localization_pb2.LocalizationEstimate()
location_est.ParseFromString(msg.message)
data = {"pose": location_est}
yield data
def get_driving_mode(self, bag_file):
"""get driving mode, which is stored in a dict"""
mode = {}
mode["status"] = 'UNKNOW'
mode["start_time"] = 0.0
mode["end_time"] = 0.0
chassis = chassis_pb2.Chassis()
reader = RecordReader(bag_file)
for msg in reader.read_messages():
if msg.topic == kChassisTopic:
chassis.ParseFromString(msg.message)
_t = msg.timestamp
t = long(str(_t)) * pow(10, -9)
cur_status = chassis.driving_mode
if mode["status"] != cur_status:
if mode["status"] != 'UNKNOW':
self.driving_mode.append(mode)
mode["status"] = cur_status
mode["start_time"] = t
mode["end_time"] = t
self.driving_mode.append(mode)
def process(reader):
last_steering_percentage = None
last_speed_mps = None
last_timestamp_sec = None
speed_data = []
d_steering_data = []
for msg in reader.read_messages():
if msg.topic == "/apollo/canbus/chassis":
chassis = chassis_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:
last_steering_percentage = steering_percentage
last_speed_mps = speed_mps
last_timestamp_sec = timestamp_sec
continue
if last_timestamp_sec is None:
last_steering_percentage = steering_percentage
last_speed_mps = speed_mps
last_timestamp_sec = timestamp_sec
continue
if (timestamp_sec - last_timestamp_sec) > 0.02:
d_steering = (steering_percentage - last_steering_percentage) \
/ (timestamp_sec - last_timestamp_sec)
speed_data.append(speed_mps)
d_steering_data.append(d_steering)
last_steering_percentage = steering_percentage
last_speed_mps = speed_mps
last_timestamp_sec = timestamp_sec
return speed_data, d_steering_data
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)
for frecord in frecords:
print("processing " + frecord)
reader = RecordReader(frecord)
for msg in reader.read_messages():
if msg.topic == "/apollo/localization/pose":
localization = localization_pb2.LocalizationEstimate()
localization.ParseFromString(msg.message)
loc_time = localization.header.timestamp_sec
x = localization.pose.position.x
y = localization.pose.position.y
#f.write(str(loc_time) + "," + str(x) + "," + str(y) + "\n")
csv_writer_localization.writerow([loc_time, x, y])
if msg.topic == "/apollo/canbus/chassis":
chassis = chassis_pb2.Chassis()
chassis.ParseFromString(msg.message)
chassis_time = chassis.header.timestamp_sec
speed = chassis.speed_mps
steering_percentage = chassis.steering_percentage
csv_writer_chassis.writerow([chassis_time, speed, steering_percentage])
if msg.topic == "/apollo/routing_response":
routingResponse = routing_pb2.RoutingResponse()
routingResponse.ParseFromString(msg.message)
routing_time = routingResponse.header.timestamp_sec
guidepost_x = routingResponse.routing_request.waypoint[1].pose.x
guidepost_y = routingResponse.routing_request.waypoint[1].pose.y
is_use_guidepost = routingResponse.routing_request.is_use_guidepost
cur_id = routingResponse.routing_request.waypoint[1].id