sensor_managers.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('calibration_launch')
00035 import sys
00036 import threading
00037 
00038 import rospy
00039 from calibration_msgs.msg import *
00040 import sensor_msgs.msg
00041 
00042 import message_filters
00043 
00044 
00045 class ChainManager:
00046     def __init__(self, chain_id, callback, *cb_args):
00047         self._chain_id = chain_id
00048         self._callback = callback
00049         self._cb_args = cb_args
00050         self._mode = "off"
00051 
00052         self._lock = threading.Lock()
00053 
00054         self._chain_sub = rospy.Subscriber(chain_id + "/chain_state", sensor_msgs.msg.JointState, self.callback)
00055 
00056     def callback(self, msg):
00057         self._lock.acquire()
00058         if self._mode is "on":
00059             # Populate measurement message
00060             m_msg = ChainMeasurement()
00061             m_msg.header.stamp = msg.header.stamp
00062             m_msg.chain_id = self._chain_id
00063             m_msg.chain_state = msg
00064             self._callback(self._chain_id, m_msg, *self._cb_args)
00065         self._lock.release()
00066 
00067     def enable(self):
00068         self._lock.acquire()
00069         self._mode = "on"
00070         self._lock.release()
00071 
00072     def disable(self):
00073         self._lock.acquire()
00074         self._mode = "off"
00075         self._lock.release()
00076 
00077 class CamManager:
00078     def __init__(self, cam_id, callback, *cb_args):
00079         self._cam_id = cam_id
00080         self._callback = callback
00081         self._cb_args = cb_args
00082         self._mode = "off"
00083         
00084         self._lock = threading.Lock()
00085 
00086         # How do I specify a queue size of 1?
00087         self._cam_info_sub   = message_filters.Subscriber(cam_id + "/camera_info", sensor_msgs.msg.CameraInfo)
00088         self._features_sub   = message_filters.Subscriber(cam_id + "/features",    calibration_msgs.msg.CalibrationPattern)
00089         self._image_sub      = message_filters.Subscriber(cam_id + "/image",       sensor_msgs.msg.Image)
00090         self._image_rect_sub = message_filters.Subscriber(cam_id + "/image_rect",  sensor_msgs.msg.Image)
00091         self._verbose_sync = message_filters.TimeSynchronizer([self._cam_info_sub, self._features_sub, self._image_sub, self._image_rect_sub], 10)
00092         self._verbose_sync.registerCallback(self.verbose_callback)
00093         self._minimal_sync = message_filters.TimeSynchronizer([self._cam_info_sub, self._features_sub], 10)
00094         self._minimal_sync.registerCallback(self.minimal_callback)
00095 
00096     def verbose_callback(self, cam_info, features, image, image_rect):
00097         self._lock.acquire()
00098         if self._mode is "verbose":
00099             # Populate measurement message
00100             msg = CameraMeasurement()
00101             msg.header.stamp = cam_info.header.stamp
00102             msg.camera_id = self._cam_id
00103             msg.image_points = features.image_points
00104             msg.cam_info = cam_info
00105             msg.verbose = True
00106             msg.image = image
00107             msg.image_rect = image_rect
00108             msg.features = features
00109             self._callback(self._cam_id, msg, *self._cb_args)
00110         self._lock.release()
00111 
00112     def minimal_callback(self, cam_info, features):
00113         self._lock.acquire()
00114         if self._mode is "minimal":
00115             # Populate measurement message
00116             msg = CameraMeasurement()
00117             msg.header.stamp = cam_info.header.stamp
00118             msg.camera_id = self._cam_id
00119             msg.image_points = features.image_points
00120             msg.cam_info = cam_info
00121             msg.verbose = False
00122             self._callback(self._cam_id, msg, *self._cb_args)
00123         self._lock.release()
00124 
00125     def enable(self, verbose=False):
00126         self._lock.acquire()
00127         if verbose:
00128             self._mode = "verbose"
00129         else:
00130             self._mode = "minimal"
00131         self._lock.release()
00132 
00133     def disable(self):
00134         self._lock.acquire()
00135         self._mode = "off"
00136         self._lock.release()
00137 
00138 class LaserManager:
00139     def __init__(self, laser_id, callback, *cb_args):
00140         self._laser_id = laser_id
00141         self._callback = callback
00142         self._cb_args = cb_args
00143         self._mode = "off"
00144 
00145         self._lock = threading.Lock()
00146 
00147         # How do I specify a queue size of 1?
00148         self._snapshot_sub       = message_filters.Subscriber(laser_id + "/snapshot",       calibration_msgs.msg.DenseLaserSnapshot)
00149         self._laser_image_sub    = message_filters.Subscriber(laser_id + "/laser_image",    sensor_msgs.msg.Image)
00150         self._image_features_sub = message_filters.Subscriber(laser_id + "/image_features", calibration_msgs.msg.CalibrationPattern)
00151         self._joint_features_sub = message_filters.Subscriber(laser_id + "/joint_features", calibration_msgs.msg.JointStateCalibrationPattern)
00152         self._duration_sub       = message_filters.Subscriber(laser_id + "/laser_interval", calibration_msgs.msg.IntervalStamped)
00153 
00154         self._verbose_sync = message_filters.TimeSynchronizer([self._snapshot_sub,
00155                                                                self._laser_image_sub,
00156                                                                self._image_features_sub,
00157                                                                self._joint_features_sub,
00158                                                                self._duration_sub], 10)
00159         self._minimal_sync = message_filters.TimeSynchronizer([self._joint_features_sub,
00160                                                                self._duration_sub], 10)
00161 
00162         self._verbose_sync.registerCallback(self.verbose_callback)
00163         self._minimal_sync.registerCallback(self.minimal_callback)
00164 
00165     def verbose_callback(self, snapshot, laser_image, image_features, joint_features, laser_duration):
00166         self._lock.acquire()
00167         if self._mode is "verbose":
00168             # Populate measurement message
00169             msg = LaserMeasurement()
00170             msg.laser_id = self._laser_id
00171             msg.joint_points = joint_features.joint_points
00172             msg.verbose = True
00173             msg.snapshot = snapshot
00174             msg.laser_image = laser_image
00175             msg.image_features = image_features
00176             msg.joint_features = joint_features
00177             self._callback(self._laser_id, msg, laser_duration.interval.start, laser_duration.interval.end, *self._cb_args)
00178         self._lock.release()
00179 
00180     def minimal_callback(self, joint_features, laser_duration):
00181         self._lock.acquire()
00182         if self._mode is "minimal":
00183             # Populate measurement message
00184             msg = LaserMeasurement()
00185             msg.laser_id = self._laser_id
00186             msg.joint_points = joint_features.joint_points
00187             msg.verbose = True
00188             self._callback(self._laser_id, msg, laser_duration.interval.start, laser_duration.interval.end, *self._cb_args)
00189         self._lock.release()
00190 
00191     def enable(self, verbose=False):
00192         self._lock.acquire()
00193         if verbose:
00194             self._mode = "verbose"
00195         else:
00196             self._mode = "minimal"
00197         self._lock.release()
00198 
00199     def disable(self):
00200         self._lock.acquire()
00201         self._mode = "off"
00202         self._lock.release()
00203 


calibration_launch
Author(s): Michael Ferguson
autogenerated on Sun Oct 5 2014 22:43:37