multicam_capture_exec.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2008, 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('camera_pose_calibration')
00035 import rospy
00036 import yaml
00037 import sys
00038 import actionlib
00039 import time
00040 import threading
00041 import message_filters
00042 
00043 from camera_pose_calibration.robot_measurement_cache import RobotMeasurementCache
00044 
00045 from calibration_msgs.msg import Interval, CalibrationPattern
00046 from camera_pose_calibration.msg import RobotMeasurement, CameraMeasurement
00047 from sensor_msgs.msg import CameraInfo, Image
00048 
00049 
00050 class CameraCaptureExecutive:
00051     def __init__(self, cam_ids):
00052         self.cam_ids = cam_ids
00053         self.cache = RobotMeasurementCache()
00054         self.lock = threading.Lock()
00055 
00056 
00057         # Specifies if we're currently waiting for a sample
00058         self.active = False
00059 
00060         # Construct a manager for each sensor stream (Don't enable any of them)
00061         cam_info_topic = rospy.get_param('~cam_info_topic', 'camera_info')
00062         #print "cam_ids"
00063         self.cam_managers   = [ (cam_id,   CamManager(  cam_id,  cam_info_topic, 
00064                                                         self.add_cam_measurement) )   for cam_id   in cam_ids ]
00065 
00066         # Turn on all of the camera modes into verbose model, since we want the CalibrationPattern data
00067         for cam_manager in zip(*self.cam_managers)[1]:
00068             cam_manager.enable(verbose=True)
00069 
00070         # Subscribe to topic containing stable intervals
00071         self.measurement_pub = rospy.Publisher("robot_measurement", RobotMeasurement)
00072         self.request_interval_sub = rospy.Subscriber("request_interval", Interval, self.request_callback)
00073 
00074         # Set up the cache with only the sensors we care about
00075         chain_ids = []
00076         laser_ids = []
00077         self.cache.reconfigure(cam_ids, chain_ids, laser_ids)
00078 
00079     def request_callback(self, msg):
00080 
00081         print "\n\n\n\n\033[34;1m------\n\033[0mGot a request callback"
00082 
00083         # See if the interval is big enough to care
00084         #if (msg.end - msg.start) < rospy.Duration(1,0):
00085         #    return
00086 
00087         with self.lock:
00088             # Do some query into the cache
00089             m_robot = self.cache.request_robot_measurement(msg.start, msg.end, min_duration=rospy.Duration(0.01))
00090 
00091             # We found a sample, so we can deactive (kind of a race condition, since 'active' triggers capture() to exit... I don't care)
00092             if m_robot:
00093                 # Change camera ids to be the tf frame IDs
00094                 for cam in m_robot.M_cam:
00095                     cam.camera_id = cam.cam_info.header.frame_id
00096                     m_robot.header.stamp = cam.cam_info.header.stamp
00097                 self.measurement_pub.publish(m_robot)
00098             else:
00099                 print "Couldn't get measurement in interval"
00100 
00101 
00102     def add_cam_measurement(self, cam_id, msg):
00103         #print "Adding [%s]" 
00104         if len(msg.features.image_points) > 0:
00105             with self.lock:
00106                 self.cache.add_cam_measurement(cam_id, msg)
00107 
00108 
00109 class CamManager:
00110     def __init__(self, cam_id, cam_info_topic, callback):
00111         self._cam_id = cam_id
00112         self._callback = callback
00113         self._mode = "off"
00114 
00115         self._lock = threading.Lock()
00116 
00117         # How do I specify a queue size of 1?                                                                                                                                
00118         self._features_sub = message_filters.Subscriber(cam_id + "/features", CalibrationPattern)
00119         self._cam_info_sub = message_filters.Subscriber(cam_id + "/" + cam_info_topic, CameraInfo)
00120 
00121         self._verbose_sync = message_filters.TimeSynchronizer([self._cam_info_sub, self._features_sub], 10)
00122 
00123         self._verbose_sync.registerCallback(self.verbose_callback)
00124 
00125     def verbose_callback(self, cam_info, features):
00126         with self._lock:
00127             if self._mode is "verbose":
00128                 # Populate measurement message                                                                                                                                   
00129                 msg = CameraMeasurement()
00130                 msg.header.stamp = cam_info.header.stamp
00131                 msg.camera_id = self._cam_id
00132                 msg.cam_info = cam_info
00133                 msg.features = features
00134                 self._callback(self._cam_id, msg)
00135 
00136     def enable(self, verbose=False):
00137         with self._lock:
00138             self._mode = "verbose"
00139 
00140     def disable(self):
00141         with self._lock:
00142             self._mode = "off"
00143 
00144 
00145 if __name__ == '__main__':
00146     rospy.init_node('capture_exec')
00147     args = rospy.myargv(sys.argv)
00148 
00149     # get list of camera's from arguments
00150     if len(args) < 2:
00151         rospy.logfatal("multicam_capture_exec.py needs camera's as arguments")
00152         raise
00153     camera_list = args[1:]
00154     executive = CameraCaptureExecutive(camera_list)
00155     rospy.spin()
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


camera_pose_calibration
Author(s): Vijay Pradeep, Wim Meeussen
autogenerated on Thu Aug 15 2013 12:02:24