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 callback_localization(self, data):
"""
New localization pose
"""
entity = localization_pb2.LocalizationEstimate()
entity.CopyFrom(data)
quat = (entity.pose.orientation.qx, entity.pose.orientation.qy,
entity.pose.orientation.qz, entity.pose.orientation.qw)
heading = tf.transformations.euler_from_quaternion(quat)
carheading = (heading[2] + math.pi / 2 + math.pi) % (2 *
math.pi) - math.pi
carx = entity.pose.position.x
cary = entity.pose.position.y
cartime = entity.header.timestamp_sec
with self.lock:
self.ax1.new_carstatus(cartime, carx, cary, carheading,
self.steer_angle, self.autodrive)
self.ax2.new_carstatus(cartime, self.carspeed, self.autodrive)
if self.ax3.title == "Curvature":
self.ax3.new_carstatus(cartime, self.carcurvature,
self.autodrive)
if self.ax4.title == "Heading":
self.ax4.new_carstatus(cartime, carheading, self.autodrive)
else:
acc = entity.pose.linear_acceleration_vrf.y
self.ax4.new_carstatus(cartime, acc, self.autodrive)
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 __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 __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 __init__(self, node):
self.localization = localization_pb2.LocalizationEstimate()
self.gps_odom_pub = node.create_writer('/apollo/sensor/gnss/odometry', gps_pb2.Gps)
self.sequence_num = 0
self.terminating = False
self.position_x = 0
self.position_y = 0
self.position_z = 0
self.orientation_x = 0
self.orientation_y = 0
self.orientation_z = 0
self.orientation_w = 0
self.linear_velocity_x = 0
self.linear_velocity_y = 0
self.linear_velocity_z = 0
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 main():
rospy.init_node("pose_offline",anonymous=True)
# pose_pub = rospy.Publisher(
# "/apollo/localization/pose",pose_pb2.Pose,queue_size=1)
pose_pub = rospy.Publisher(
"/apollo/localization/pose",localization_pb2.LocalizationEstimate,queue_size=1)
#generate localization info
LocalizationEstimate = localization_pb2.LocalizationEstimate()
#send pose to /apollo/localization/pose
count = 0
r = rospy.Rate(1)
while not rospy.is_shutdown():
pose_pub.publish(gps_data(LocalizationEstimate,count))
count += 1
r.sleep()
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 __init__(self):
self.localization = localization_pb2.LocalizationEstimate()
self.gps_odom_pub = rospy.Publisher('/apollo/sensor/gnss/odometry',
gps_pb2.Gps,
queue_size=1)
self.sequence_num = 0
self.terminating = False
self.position_x = 0
self.position_y = 0
self.position_z = 0
self.orientation_x = 0
self.orientation_y = 0
self.orientation_z = 0
self.orientation_w = 0
self.linear_velocity_x = 0
self.linear_velocity_y = 0
self.linear_velocity_z = 0
def __init__(self, record_file):
self.firstvalid = False
self.logger = Logger.get_logger("LaneRecord")
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,theta,dist_l,conf_l,dist_r,conf_r\n")
self.localization = localization_pb2.LocalizationEstimate()
self.mobileye = mobileye_pb2.Mobileye()
self.mobileye_received = False
self.terminating = False
def generate_message(topic, filename):
"""generate message from file"""
message = None
if topic == "/apollo/planning":
message = planning_pb2.ADCTrajectory()
elif topic == "/apollo/localization/pose":
message = localization_pb2.LocalizationEstimate()
elif topic == "/apollo/perception/obstacles":
message = perception_obstacle_pb2.PerceptionObstacles()
elif topic == "/apollo/prediction":
message = prediction_obstacle_pb2.PredictionObstacles()
elif topic == "/apollo/routing_response":
message = routing_pb2.RoutingResponse()
if not message:
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()
return message
def calculate(self, bag_file):
"""calculate body sensation, it should be after get driving mode"""
localization = localization_pb2.LocalizationEstimate()
reader = RecordReader(bag_file)
for msg in reader.read_messages():
if msg.topic == kLocalizationTopic:
localization.ParseFromString(msg.message)
_t = msg.timestamp
t = long(str(_t)) * pow(10, -9)
self.timestamp = t
diff_bump_time = t - self._last_bump_time
if diff_bump_time <= BUMP_TIME_THRESHOLD:
continue
acc_x = localization.pose.linear_acceleration.x
acc_y = localization.pose.linear_acceleration.y
acc_z = localization.pose.linear_acceleration.z
if abs(
acc_z
) >= SPEED_UP_THRESHOLD_2 and diff_bump_time >= BUMP_TIME_THRESHOLD:
self._bumps_rollback(t)
self._last_bump_time = t
if self._check_status(t):
self.auto_counts["bumps"] += 1
else:
self.manual_counts["bumps"] += 1
else:
if self._speed_down_2_flag:
if acc_y <= SPEED_DOWN_THRESHOLD_4:
self._speed_down_4_flag = 1
continue
if acc_y <= SPEED_DOWN_THRESHOLD_2:
continue
if self._speed_down_4_flag == 1 \
and t - self._last_speed_down_4_time >= ACCELERATE_TIME_THRESHOLD:
self._last_speed_down_4_time = t
if self._check_status(t):
self.auto_counts["speed_down_4"] += 1
else:
self.manual_counts["speed_down_4"] += 1
elif t - self._last_speed_down_2_time >= ACCELERATE_TIME_THRESHOLD:
self._last_speed_down_2_time = t
if self._check_status(t):
self.auto_counts["speed_down_2"] += 1
else:
self.manual_counts["speed_down_2"] += 1
self._speed_down_2_flag = 0
self._speed_down_4_flag = 0
elif acc_y <= SPEED_DOWN_THRESHOLD_2:
self._speed_down_2_flag = 1
if self._speed_up_2_flag:
if acc_y >= SPEED_UP_THRESHOLD_4:
self._speed_up_4_flag = 1
continue
if acc_y >= SPEED_UP_THRESHOLD_2:
continue
if self._speed_up_4_flag == 1 \
and t - self._last_speed_up_4_time >= ACCELERATE_TIME_THRESHOLD:
self._last_speed_up_4_time = t
if self._check_status(t):
self.auto_counts["speed_up_4"] += 1
else:
self.manual_counts["speed_up_4"] += 1
elif t - self._last_speed_up_2_time >= ACCELERATE_TIME_THRESHOLD:
self._last_speed_up_2_time = t
if self._check_status(t):
self.auto_counts["speed_up_2"] += 1
else:
self.manual_counts["speed_up_2"] += 1
self._speed_up_2_flag = 0
self._speed_up_4_flag = 0
elif acc_y >= SPEED_UP_THRESHOLD_2:
self._speed_up_2_flag = 1
if self._turning_flag:
if abs(acc_x) >= SPEED_UP_THRESHOLD_2:
continue
if t - self._last_turning_time >= ACCELERATE_TIME_THRESHOLD:
self._last_turning_time = t
if self._check_status(t):
self.auto_counts["turning"] += 1
else:
self.manual_counts["turning"] += 1
self._turning_flag = 0
elif abs(acc_x) >= SPEED_UP_THRESHOLD_2:
self._turning_flag = 1
def _init_parser(self):
self._msg_parser = localization_pb2.LocalizationEstimate()
def read_localization_pb(localization_pb_file):
localization_pb = localization_pb2.LocalizationEstimate()
f_handle = open(localization_pb_file, 'r')
text_format.Merge(f_handle.read(), localization_pb)
f_handle.close()
return localization_pb
f_chassis = open("chassis_" + frecords[0].split('/')[-1] + ".csv", 'wb')
csv_writer_chassis = csv.writer(f_chassis)
csv_writer_chassis.writerow(["timestamp", "speed", "steering_percentage"])
f_routing = open("routng_" + frecords[0].split('/')[-1] + ".csv", 'wb')
csv_writer_routing = csv.writer(f_routing)
csv_writer_routing.writerow(["timestamp", "guidepost_x", "guidepost_y", "use_guidepost", "cur_id", "end_id"])
#with open("path_" + frecords[0].split('/')[-1] + ".txt", 'w') as f:
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
