cal_bag_helpers.py
Go to the documentation of this file.
1 #! /usr/bin/env python
2 # Software License Agreement (BSD License)
3 #
4 # Copyright (c) 2011, Willow Garage, Inc.
5 # All rights reserved.
6 #
7 # Redistribution and use in source and binary forms, with or without
8 # modification, are permitted provided that the following conditions
9 # are met:
10 #
11 # * Redistributions of source code must retain the above copyright
12 # notice, this list of conditions and the following disclaimer.
13 # * Redistributions in binary form must reproduce the above
14 # copyright notice, this list of conditions and the following
15 # disclaimer in the documentation and/or other materials provided
16 # with the distribution.
17 # * Neither the name of Willow Garage, Inc. nor the names of its
18 # contributors may be used to endorse or promote products derived
19 # from this software without specific prior written permission.
20 #
21 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 # POSSIBILITY OF SUCH DAMAGE.
33 
34 import roslib; roslib.load_manifest('calibration_estimation')
35 import rosbag
36 
37 from calibration_estimation.sensors.multi_sensor import MultiSensor
38 
39 def get_cam_info(bag_filename, cam_name):
40  """ Look for cam info in the calibration bagfile """
41  cam_info = None
42  bag = rosbag.Bag(bag_filename)
43  for topic, msg, t in bag.read_messages():
44  if topic == "robot_measurement":
45  for cam_measurement in msg.M_cam:
46  if cam_measurement.camera_id == cam_name:
47  print "Found a sample with camera [%s] in it" % cam_measurement.camera_id
48  cam_info = cam_measurement.cam_info
49  break
50  if cam_info != None:
51  break
52  return cam_info
53 
54 
55 def get_robot_description(bag_filename, use_topic=False):
56  """ Get the robot description out of a bagfile """
57  if use_topic:
58  import rospy
59  return rospy.get_param("robot_description")
60  bag = rosbag.Bag(bag_filename)
61  for topic, msg, t in bag.read_messages(topics=['robot_description']):
62  if topic == 'robot_description':
63  bag.close()
64  return msg.data
65  bag.close()
66  return ""
67 
68 
69 def get_robot_measurement_count(bag_filename, sample_skip_list=[]):
70  """ Get the number of measurements in our bagfile """
71  msg_count = 0
72  bag = rosbag.Bag(bag_filename)
73  index = 0
74  for topic, msg, t in bag.read_messages(topics=['robot_measurement']):
75  if topic == 'robot_measurement':
76  if index in sample_skip_list:
77  print "Skipping sample:", index
78  else:
79  msg_count+=1
80  index += 1
81  bag.close()
82  return msg_count
83 
84 
85 def get_multisensors(bag_filename, cur_sensors, sample_skip_list=[]):
86  """ Get all sensors from bagfile. """
87  bag = rosbag.Bag(bag_filename)
88  multisensors = []
89  index = 0
90  for topic, msg, t in bag.read_messages(topics=['robot_measurement']):
91  if topic == "robot_measurement":
92  if index in sample_skip_list:
93  print "Skipping sample:", index
94  else:
95  # Hack to rename laser id
96  for cur_laser in msg.M_laser:
97  if cur_laser.laser_id in ["tilt_laser_6x8", "tilt_laser_8x6", "tilt_laser_7x6", "tilt_laser_6x7"]:
98  cur_laser.laser_id = "tilt_laser"
99  ms = MultiSensor(cur_sensors)
100  ms.sensors_from_message(msg)
101  multisensors.append(ms)
102  index += 1
103  bag.close()
104  return multisensors
105 
def get_cam_info(bag_filename, cam_name)
def get_robot_measurement_count(bag_filename, sample_skip_list=[])
def get_robot_description(bag_filename, use_topic=False)
def get_multisensors(bag_filename, cur_sensors, sample_skip_list=[])


calibration_estimation
Author(s): Vijay Pradeep, Michael Ferguson
autogenerated on Fri Apr 2 2021 02:12:53