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