scripts to analyse

bag/.gitignore

 *
 */
 !.gittignore
+!*.bash
+!*.py

bag/filter_bag.bash

+#!/bin/bash
+BAG="ana_lab_2"
+rm $BAG\_filtered.bag
+rm $BAG\_filtered_notf.bag
+rosbag filter $BAG.bag $BAG\_filtered.bag "topic == '/ana/imu/data' or topic == '/ana/odom' or topic == '/ana/sensors/bno055_imu/imu' or topic == '/ana/sensors/scan' or topic == '/tf'"
+python3 filter_tf.py $BAG\_filtered.bag $BAG\_filtered_notf.bag
+#python3 filter_tf2.py -i $BAG\_filtered.bag -o $BAG\_filtered_notf.bag -f ana/front_left_axle
+

bag/filter_tf.py

+#!/usr/bin/env python
+
+import rosbag
+import sys
+
+
+def filter_topics(in_bag, out_bag, frames_we_want):
+    with rosbag.Bag(out_bag, 'w') as outbag:
+        print("Writing to " + out_bag)
+        print("Reading from " + in_bag)
+        for topic, msg, t in rosbag.Bag(in_bag).read_messages():
+            if topic == "/tf" and msg.transforms:
+                transforms_to_keep = []
+                for i in range(len(msg.transforms)):
+                    # if its one of the frames we want we keep it
+                    if msg.transforms[i].header.frame_id in frames_we_want and msg.transforms[i].child_frame_id in frames_we_want:
+                        transforms_to_keep.append(msg.transforms[i])
+                        #print("Keeping: " + str(msg.transforms[i]))
+                    # else:
+                    #     print("Discarding: " + str(msg.transforms[i]))
+
+                msg.transforms = transforms_to_keep
+                outbag.write(topic, msg, t)
+            elif topic != '/tf':
+                outbag.write(topic, msg, t)
+
+def filter_topics_2(in_bag, out_bag, frames_we_dont_want):
+    with rosbag.Bag(out_bag, 'w') as outbag:
+        print("Writing to " + out_bag)
+        print("Reading from " + in_bag)
+        for topic, msg, t in rosbag.Bag(in_bag).read_messages():
+            if topic == "/tf" and msg.transforms:
+                transforms_to_keep = []
+                for i in range(len(msg.transforms)):
+                    # if its one of the frames we want we keep it
+                    if msg.transforms[i].header.frame_id not in frames_we_dont_want and msg.transforms[i].child_frame_id not in frames_we_dont_want:
+                        transforms_to_keep.append(msg.transforms[i])
+                    #    print("Keeping: " + str(msg.transforms[i].header.frame_id))
+                    #else:
+                    #    print("Discarding: " + str(msg.transforms[i].header.frame_id))
+
+                msg.transforms = transforms_to_keep
+                outbag.write(topic, msg, t)
+            elif topic != '/tf':
+                outbag.write(topic, msg, t)
+
+
+if __name__ == '__main__':
+    in_bag = sys.argv[1]
+    out_bag = sys.argv[2]
+    # filter_topics(in_bag, out_bag, ['base_link', 'odom', 'map',
+    #                                 'torso', 'Hip', 'Pelvis', 'Tibia', 'base_footprint'])
+    filter_topics_2(in_bag, out_bag, ['/ana/rear_right_axle', '/ana/top_plate', '/ana/front_left_axle' , '/ana/rear_left_axle', '/ana/front_right_axle', '/ana/camera_link'])
+    print("Done")

bag/filter_tf2.py

+#!/usr/bin/python
+"""
+Copyright (c) 2012,
+Systems, Robotics and Vision Group
+University of the Balearican Islands
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of Systems, Robotics and Vision Group, University of 
+      the Balearican Islands nor the names of its contributors may be used to 
+      endorse or promote products derived from this software without specific 
+      prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+"""
+
+
+PKG = 'bag_tools' # this package name
+
+import roslib; roslib.load_manifest(PKG)
+import rospy
+import rosbag
+import os
+import sys
+import argparse
+
+def remove_tf(inbag,outbag,frame_ids):
+  print( '   Processing input bagfile: ', inbag)
+  print( '  Writing to output bagfile: ', outbag)
+  print( '         Removing frame_ids: ', ' '.join(frame_ids))
+
+  outbag = rosbag.Bag(outbag,'w')
+  for topic, msg, t in rosbag.Bag(inbag,'r').read_messages():
+      if topic == "/tf":
+          new_transforms = []
+          for transform in msg.transforms:
+              if transform.header.frame_id not in frame_ids and transform.child_frame_id not in frame_ids:
+                  new_transforms.append(transform)
+          msg.transforms = new_transforms
+      outbag.write(topic, msg, t)
+  print( 'Closing output bagfile and exit...')
+  outbag.close();
+
+if __name__ == "__main__":
+
+  parser = argparse.ArgumentParser(
+      description='removes all transforms from the /tf topic that contain one of the given frame_ids in the header as parent or child.')
+  parser.add_argument('-i', metavar='INPUT_BAGFILE', required=True, help='input bagfile')
+  parser.add_argument('-o', metavar='OUTPUT_BAGFILE', required=True, help='output bagfile')
+  parser.add_argument('-f', metavar='FRAME_ID', required=True, help='frame_id(s) of the transforms to remove from the /tf topic', nargs='+')
+  args = parser.parse_args()
+
+  try:
+    remove_tf(args.i,args.o,args.f)
+  except Exception:
+    import traceback
+    traceback.print_exc()
+

bag/generate_trajectory_csv.bash

+#!/bin/bash
+rosbag filter trajectory_recording.bag trajectory_recording_filtered.bag "t.to_sec() >=1620124021"
+rostopic echo -b trajectory_recording_filtered.bag /wolf_ros_node/trajectory -p > trajectory.csv
+python3 transpose_csv.py 

bag/shorten_topic.bash

+#!/bin/bash
+BAG="ana_lab_2"
+rosbag filter $BAG\_filtered_notf.bag $BAG\_shortened.bag "topic != '/ana/sensors/scan' or (topic == '/ana/sensors/scan' and t.to_sec() <= 1620124012)"

bag/transpose_csv.py

+import pandas as pd
+pd.read_csv('trajectory.csv', header=None).T.to_csv('trajectory_transp.csv', header=False, index=False)