#!/usr/bin/env python

###############################################################################
# Copyright 2018 The Apollo Authors. All Rights Reserved.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
###############################################################################

"""
Sample PNC topics. For each /path/to/a.bag, will generate
/path/to/pnc_sample/a.bag.

Usage:
    ./sample_pnc_topics.py <bag_path>
        <bag_path>    Support * and ?.
Example:
    ./sample_pnc_topics.py '/mnt/nfs/public_test/2018-04-??/*/mkz8/*/*.bag'
"""

import glob
import os
import sys

import glog
import rosbag


class SamplePNC(object):
    """Sample bags to contain PNC related topics only."""
    TOPICS = [
        '/apollo/sensor/conti_radar',
        '/apollo/sensor/delphi_esr',
        '/apollo/sensor/gnss/best_pose',
        '/apollo/sensor/gnss/corrected_imu',
        '/apollo/sensor/gnss/gnss_status',
        '/apollo/sensor/gnss/imu',
        '/apollo/sensor/gnss/ins_stat',
        '/apollo/sensor/gnss/odometry',
        '/apollo/sensor/gnss/rtk_eph',
        '/apollo/sensor/gnss/rtk_obs',
        '/apollo/sensor/mobileye',
        '/apollo/canbus/chassis',
        '/apollo/canbus/chassis_detail',
        '/apollo/control',
        '/apollo/control/pad',
        '/apollo/navigation',
        '/apollo/perception/obstacles',
        '/apollo/perception/traffic_light',
        '/apollo/planning',
        '/apollo/prediction',
        '/apollo/routing_request',
        '/apollo/routing_response',
        '/apollo/localization/pose',
        '/apollo/drive_event',
        '/tf',
        '/tf_static',
        '/apollo/monitor',
        '/apollo/monitor/system_status',
        '/apollo/monitor/static_info',
    ]

    @classmethod
    def process_bags(cls, bags):
        for bag_file in bags:
            output_dir = os.path.join(os.path.dirname(bag_file), 'pnc_sample')
            output_bag = os.path.join(output_dir, os.path.basename(bag_file))
            if os.path.exists(output_bag):
                glog.info('Skip {} which has been processed'.format(bag_file))
                continue

            glog.info('Processing bag {}'.format(bag_file))
            if not os.path.exists(output_dir):
                os.makedirs(output_dir)
            with rosbag.Bag(bag_file, 'r') as bag_in:
                with rosbag.Bag(output_bag, 'w') as bag_out:
                    for topic, msg, t in bag_in.read_messages(
                            topics=SamplePNC.TOPICS):
                        bag_out.write(topic, msg, t)

if __name__ == '__main__':
    if len(sys.argv) < 2:
        print("Usage: %s <bag_path> ..." % sys.argv[0])
        sys.exit(1)

    bags = sorted(sum([glob.glob(arg) for arg in sys.argv[1:]], []))
    SamplePNC.process_bags(bags)

@@ -776,17 +776,16 @@
 #######%0A
-%0A
 %22%22%22%0ASamp
@@ -2267,135 +2267,38 @@
 _bag
-s
 (cls, 
-bags):%0A        for bag_file in bags:%0A            output_dir = os.path.join(os.path.dirname(bag_file), 'pnc_sample')%0A    
+input_bag, output_bag):%0A
     
@@ -2308,19 +2308,19 @@
  output_
-bag
+dir
  = os.pa
@@ -2326,265 +2326,28 @@
 ath.
-join(output_dir, os.path.basename(bag_file))%0A            if os.path.exists(output_bag):%0A                glog.info('Skip %7B%7D which has been processed'.format(bag_file))%0A                continue%0A%0A            glog.info('Processing bag %7B%7D'.format(bag_file))%0A    
+dirname(output_bag)%0A
     
@@ -2397,20 +2397,16 @@
         
-   
- 
 os.maked
@@ -2422,31 +2422,44 @@
 t_dir)%0A     
+   try:%0A
        
+     
 with rosbag.
@@ -2466,16 +2466,17 @@
 Bag(
-bag_file
+input_bag
 , 'r
@@ -2489,16 +2489,41 @@
 bag_in:%0A
+                try:%0A    
         
@@ -2595,16 +2595,20 @@
         
+    
 for topi
@@ -2670,16 +2670,20 @@
         
+ 
+   
 topics=S
@@ -2700,16 +2700,20 @@
 OPICS):%0A
+    
         
@@ -2761,206 +2761,805 @@
  t)%0A
-%0Aif __name__ == '__main__':%0A    if len(sys.argv) %3C 2:%0A        print(%22Usage: %25
+                except BagIOException as bag_io_exception:%0A                    print(%22Read file: %7B%7D with error: %7B%7D%22.format(%0A                        input_bag, bag_io_exception))%0A                finally:%0A                    print(%22Failed to write file: %7B%7D%22.format(output_bag))%0A        except BagIOException a
 s 
-%3C
 bag_
-path%3E ...%22 %25 sys.argv%5B0%5D)%0A        sys.exit(1)%0A%0A    bags = sorted(sum(%5Bglob.glob(arg) for arg in sys.argv%5B1:%5D%5D, %5B%5D)
+io_exception:%0A            print(%22Open file: %7B%7D with error: %7B%7D %22.format(%0A                input_bag, bag_io_exception))%0A        finally:%0A            print(%22Failed to open file: %7B%7D%22.format(input_bag))%0A%0A%0Aif __name__ == '__main__':%0A    import argparse%0A    parser = argparse.ArgumentParser(description=%22Filter pnc rosbag%22)%0A    parser.add_argument('input', type=str, help=%22the input rosbag%22)%0A    parser.add_argument('output', type=str, help=%22the output rosbag%22)%0A    args = parser.parse_args(
 )%0A  
@@ -3585,12 +3585,30 @@
 _bag
-s(bags
+(args.input, args.output
 )%0A

create sample_pnc_topics utility (#5397)