cal_bag_helpers.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2011, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of Willow Garage, Inc. nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 
00034 import roslib; roslib.load_manifest('calibration_estimation')
00035 import rosbag
00036 
00037 from calibration_estimation.sensors.multi_sensor import MultiSensor
00038 
00039 def get_cam_info(bag_filename, cam_name):
00040     """ Look for cam info in the calibration bagfile """
00041     cam_info = None
00042     bag = rosbag.Bag(bag_filename)
00043     for topic, msg, t in bag.read_messages():
00044         if topic == "robot_measurement":
00045             for cam_measurement in msg.M_cam:
00046                 if cam_measurement.camera_id == cam_name:
00047                     print "Found a sample with camera [%s] in it" % cam_measurement.camera_id
00048                     cam_info = cam_measurement.cam_info
00049                     break
00050             if cam_info != None:
00051                 break
00052     return cam_info
00053 
00054 
00055 def get_robot_description(bag_filename, use_topic=False):
00056     """ Get the robot description out of a bagfile """
00057     if use_topic:
00058         import rospy
00059         return rospy.get_param("robot_description")
00060     bag = rosbag.Bag(bag_filename)
00061     for topic, msg, t in bag.read_messages(topics=['robot_description']):
00062         if topic == 'robot_description':
00063             bag.close()
00064             return msg.data
00065     bag.close()
00066     return ""
00067 
00068 
00069 def get_robot_measurement_count(bag_filename, sample_skip_list=[]):
00070     """ Get the number of measurements in our bagfile """
00071     msg_count = 0
00072     bag = rosbag.Bag(bag_filename)
00073     index = 0
00074     for topic, msg, t in bag.read_messages(topics=['robot_measurement']):
00075         if topic == 'robot_measurement':
00076             if index in sample_skip_list:
00077                 print "Skipping sample:", index
00078             else:
00079                 msg_count+=1
00080             index += 1
00081     bag.close()
00082     return msg_count
00083 
00084 
00085 def get_multisensors(bag_filename, cur_sensors, sample_skip_list=[]):
00086     """ Get all sensors from bagfile. """
00087     bag = rosbag.Bag(bag_filename)
00088     multisensors = []
00089     index = 0
00090     for topic, msg, t in bag.read_messages(topics=['robot_measurement']):
00091         if topic == "robot_measurement":
00092             if index in sample_skip_list:
00093                 print "Skipping sample:", index
00094             else:
00095                 # Hack to rename laser id
00096                 for cur_laser in msg.M_laser:
00097                     if cur_laser.laser_id in ["tilt_laser_6x8", "tilt_laser_8x6", "tilt_laser_7x6", "tilt_laser_6x7"]:
00098                         cur_laser.laser_id = "tilt_laser"
00099                 ms = MultiSensor(cur_sensors)
00100                 ms.sensors_from_message(msg)
00101                 multisensors.append(ms)
00102             index += 1
00103     bag.close()
00104     return multisensors
00105 


calibration_estimation
Author(s): Vijay Pradeep, Michael Ferguson
autogenerated on Sat Jun 8 2019 19:41:45