naoqi_camera.h
Go to the documentation of this file.
00001 /* -*- mode: C++ -*- */
00002 /* $Id$ */
00003 
00004 /*********************************************************************
00005 * Software License Agreement (BSD License)
00006 *
00007 *  Copyright (c) 2010 Jack O'Quin
00008 *  All rights reserved.
00009 *
00010 *  Redistribution and use in source and binary forms, with or without
00011 *  modification, are permitted provided that the following conditions
00012 *  are met:
00013 *
00014 *   * Redistributions of source code must retain the above copyright
00015 *     notice, this list of conditions and the following disclaimer.
00016 *   * Redistributions in binary form must reproduce the above
00017 *     copyright notice, this list of conditions and the following
00018 *     disclaimer in the documentation and/or other materials provided
00019 *     with the distribution.
00020 *   * Neither the name of the author nor other contributors may be
00021 *     used to endorse or promote products derived from this software
00022 *     without specific prior written permission.
00023 *
00024 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035 *  POSSIBILITY OF SUCH DAMAGE.
00036 *********************************************************************/
00037 
00038 #include <boost/thread/mutex.hpp>
00039 
00040 #include <ros/ros.h>
00041 #include <camera_info_manager/camera_info_manager.h>
00042 #include <diagnostic_updater/diagnostic_updater.h>
00043 #include <diagnostic_updater/publisher.h>
00044 #include <driver_base/driver.h>
00045 #include <dynamic_reconfigure/server.h>
00046 #include <image_transport/image_transport.h>
00047 #include <sensor_msgs/CameraInfo.h>
00048 
00049 // Aldebaran includes
00050 #include <alproxies/alvideodeviceproxy.h>
00051 
00052 #include <naoqi_driver/naoqi_node.h>
00053 
00054 #include "naoqi_sensors/NaoqiCameraConfig.h"
00055 typedef naoqicamera::NaoqiCameraConfig Config;
00056 
00063 namespace naoqicamera_driver
00064 {
00065 
00067   //  (std::runtime_error should be top parent)
00068   // code borrowed from drivers/laser/hokuyo_driver/hokuyo.h
00069 #define DEF_EXCEPTION(name, parent)             \
00070   class name  : public parent {                 \
00071   public:                                       \
00072     name (const char* msg) : parent (msg) {}    \
00073   }
00074 
00076   DEF_EXCEPTION(Exception, std::runtime_error);
00077 
00078 
00079 class NaoqiCameraDriver : public NaoqiNode
00080 {
00081 public:
00082 
00083   // public methods
00084   NaoqiCameraDriver(int argc, char ** argv,
00085                   ros::NodeHandle priv_nh,
00086                   ros::NodeHandle camera_nh);
00087   ~NaoqiCameraDriver();
00088   void poll(void);
00089   void setup(void);
00090   void shutdown(void);
00091 
00092 private:
00093 
00094   // private methods
00095   void getNaoqiParams(ros::NodeHandle priv_nh);
00096   void closeCamera();
00097   bool openCamera(Config &newconfig);
00098   void publish(const sensor_msgs::ImagePtr &image);
00099   bool read(sensor_msgs::ImagePtr &image);
00100   void reconfig(naoqicamera::NaoqiCameraConfig &newconfig, uint32_t level);
00101 
00103   volatile driver_base::Driver::state_t state_; // current driver state
00104   boost::mutex reconfiguration_mutex_;  // critical section of poll() and reconfigure()
00105 
00106   ros::NodeHandle priv_nh_;             // private node handle
00107   ros::NodeHandle camera_nh_;           // camera name space handle
00108   std::string camera_name_;             // camera name
00109   std::string frame_id_;                // TF frame_id, either CameraTop_frame or CameraBottom_frame
00110   ros::Rate cycle_;                     // polling rate when closed
00111   ros::Rate real_frame_rate_;           // requested fps, to cap publishing rate
00112   uint32_t retries_;                    // count of openCamera() retries
00113 
00114 
00116   boost::shared_ptr<AL::ALVideoDeviceProxy> camera_proxy_;
00117 
00119   naoqicamera::NaoqiCameraConfig config_;
00120   dynamic_reconfigure::Server<naoqicamera::NaoqiCameraConfig> srv_;
00121 
00123   boost::shared_ptr<camera_info_manager::CameraInfoManager> cinfo_;
00124   bool calibration_matches_;            // CameraInfo matches video mode
00125 
00127   boost::shared_ptr<image_transport::ImageTransport> it_;
00128   image_transport::CameraPublisher image_pub_;
00129 
00131   diagnostic_updater::Updater diagnostics_;
00132   double topic_diagnostics_min_freq_;
00133   double topic_diagnostics_max_freq_;
00134   diagnostic_updater::TopicDiagnostic topic_diagnostics_;
00135 
00136 }; // end class NaoqiCameraDriver
00137 
00138 }; // end namespace naoqicamera_driver


naoqi_sensors
Author(s): Séverin Lemaignan, Vincent Rabaud, Karsten Knese, Jack O'Quin, Ken Tossell, Patrick Beeson, Nate Koenig, Andrew Howard, Damien Douxchamps, Dan Dennedy, Daniel Maier
autogenerated on Fri Jul 3 2015 12:51:48