nao_camera.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (C) 2009, 2010 Jack O'Quin, Patrick Beeson
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 the author nor other contributors may be
00018 *     used to endorse or promote products derived from this software
00019 *     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 
00035 #include <sstream>
00036 
00037 #include <boost/format.hpp>
00038 
00039 #include <driver_base/SensorLevels.h>
00040 #include <sensor_msgs/image_encodings.h>
00041 #include <tf/transform_listener.h>
00042 
00043 // Aldebaran includes
00044 #include <alproxies/almemoryproxy.h>
00045 #include <alvision/alvisiondefinitions.h>
00046 #include <alerror/alerror.h>
00047 #include <alvalue/alvalue.h>
00048 #include <alcommon/altoolsmain.h>
00049 #include <alproxies/alvideodeviceproxy.h>
00050 #include <alcommon/albroker.h>
00051 #include <alcommon/albrokermanager.h>
00052 #include <alcommon/alproxy.h>
00053 #include <alproxies/alproxies.h>
00054 #include <alcommon/almodule.h>
00055 
00056 
00057 
00058 #include "nao_sensors/NaoCameraConfig.h"
00059 
00060 #include "nao_camera.h"
00079 using namespace std;
00080 using namespace AL;
00081 
00082 namespace naocamera_driver
00083 {
00084   // some convenience typedefs
00085   typedef naocamera::NaoCameraConfig Config;
00086   typedef driver_base::Driver Driver;
00087   typedef driver_base::SensorLevels Levels;
00088 
00089   NaoCameraDriver::NaoCameraDriver(int argc, char ** argv,
00090                                    ros::NodeHandle priv_nh,
00091                                    ros::NodeHandle camera_nh):
00092     NaoNode(argc, argv),
00093     state_(Driver::CLOSED),
00094     reconfiguring_(false),
00095     priv_nh_(priv_nh),
00096     camera_nh_(camera_nh),
00097     camera_name_("camera"),
00098     cycle_(1.0),                        // slow poll when closed
00099     real_frame_rate_(30.0),
00100     retries_(0),
00101     srv_(priv_nh),
00102     cinfo_(new camera_info_manager::CameraInfoManager(camera_nh_)),
00103     calibration_matches_(true),
00104     it_(new image_transport::ImageTransport(camera_nh_)),
00105     image_pub_(it_->advertiseCamera("image_raw", 1)),
00106     diagnostics_(),
00107     topic_diagnostics_min_freq_(0.),
00108     topic_diagnostics_max_freq_(1000.),
00109     topic_diagnostics_("image_raw", diagnostics_,
00110                 diagnostic_updater::FrequencyStatusParam
00111                     (&topic_diagnostics_min_freq_,
00112                      &topic_diagnostics_max_freq_, 0.1, 10),
00113                 diagnostic_updater::TimeStampStatusParam())
00114   {
00115     if ( !connectNaoQi() )
00116     {
00117       ROS_ERROR("Could not connect to NAOqi! Make sure NAOqi is running and you passed the right host/port.");
00118       throw naocamera_driver::Exception("Connection to NAOqi failed");
00119     }
00120 
00121   }
00122 
00123   NaoCameraDriver::~NaoCameraDriver()
00124   {}
00125 
00130   void NaoCameraDriver::closeCamera()
00131   {
00132     if (state_ != Driver::CLOSED)
00133       {
00134         ROS_INFO_STREAM("[" << camera_name_ << "] closing device");
00135         camera_proxy_->unsubscribe(camera_name_);
00136         state_ = Driver::CLOSED;
00137       }
00138   }
00139 
00152   bool NaoCameraDriver::openCamera(Config &newconfig)
00153   {
00154     bool success = false;
00155 
00156     try
00157     {
00158         camera_proxy_ = boost::shared_ptr<ALVideoDeviceProxy>(new ALVideoDeviceProxy(m_broker));
00159 
00160         if (camera_proxy_->getGenericProxy()->isLocal())
00161             ROS_INFO("nao_camera runs directly on the robot.");
00162         else
00163             ROS_WARN("nao_camera runs remotely.");
00164         // build a unique name for the camera that will be used to
00165         // store calibration informations.
00166         stringstream canonical_name;
00167         canonical_name << "nao_camera" 
00168                        << "_src" << newconfig.source 
00169                        << "_res" << newconfig.resolution;
00170 
00171         camera_name_ = camera_proxy_->subscribeCamera(
00172                                     camera_name_, 
00173                                     newconfig.source,
00174                                     newconfig.resolution,
00175                                     kRGBColorSpace,
00176                                     newconfig.frame_rate);
00177 
00178         if (!cinfo_->setCameraName(canonical_name.str()))
00179         {
00180             // GUID is 16 hex digits, which should be valid.
00181             // If not, use it for log messages anyway.
00182             ROS_WARN_STREAM("[" << camera_name_
00183                             << "] name not valid"
00184                             << " for camera_info_manger");
00185         }
00186 
00187         ROS_INFO_STREAM("[" << camera_name_ << "] opened: resolution "
00188                         << newconfig.resolution << " @"
00189                         << newconfig.frame_rate << " fps");
00190         state_ = Driver::OPENED;
00191         calibration_matches_ = true;
00192         newconfig.guid = camera_name_; // update configuration parameter
00193         retries_ = 0;
00194         success = true;
00195     }
00196     catch (const ALError& e)
00197     {
00198         state_ = Driver::CLOSED;    // since the open() failed
00199         if (retries_++ > 0)
00200           ROS_DEBUG_STREAM("[" << camera_name_
00201                            << "] exception opening device (retrying): "
00202                            << e.what());
00203         else
00204           ROS_ERROR_STREAM("[" << camera_name_
00205                            << "] device open failed: " << e.what());
00206     }
00207 
00208     // update diagnostics parameters
00209     diagnostics_.setHardwareID(camera_name_);
00210     double delta = newconfig.frame_rate * 0.1; // allow 10% error margin
00211     topic_diagnostics_min_freq_ = newconfig.frame_rate - delta;
00212     topic_diagnostics_max_freq_ = newconfig.frame_rate + delta;
00213 
00214     return success;
00215   }
00216 
00217 
00219   void NaoCameraDriver::poll(void)
00220   {
00221     // Do not run concurrently with reconfig().
00222     //
00223     // The mutex lock should be sufficient, but the Linux pthreads
00224     // implementation does not guarantee fairness, and the reconfig()
00225     // callback thread generally suffers from lock starvation for many
00226     // seconds before getting to run.  So, we avoid acquiring the lock
00227     // if there is a reconfig() pending.
00228     bool do_sleep = true;
00229     
00230     // publish images only if someone subscribe to the topic
00231     uint32_t nbSubscribers = image_pub_.getNumSubscribers();
00232 
00233     if (nbSubscribers == 0)
00234     {
00235         if (state_ == Driver::OPENED)
00236         {
00237             closeCamera();
00238         }
00239     }
00240     else if (!reconfiguring_)
00241     {
00242         if (state_ == Driver::CLOSED)
00243         {
00244             openCamera(config_);        // open with current configuration
00245         }
00246         do_sleep = (state_ == Driver::CLOSED);
00247         if (!do_sleep)                  // openCamera() succeeded?
00248         {
00249             // driver is open, read the next image still holding lock
00250             sensor_msgs::ImagePtr image(new sensor_msgs::Image);
00251             if (read(image))
00252             {
00253                 publish(image);
00254                 real_frame_rate_.sleep();
00255             }
00256         }
00257     } // release mutex lock
00258 
00259     // Always run the diagnostics updater: no lock required.
00260     diagnostics_.update();
00261 
00262     if (do_sleep)
00263       {
00264         // device was closed or poll is not running, sleeping avoids
00265         // busy wait (DO NOT hold the lock while sleeping)
00266         cycle_.sleep();
00267       }
00268   }
00269 
00274   void NaoCameraDriver::publish(const sensor_msgs::ImagePtr &image)
00275   {
00276     image->header.frame_id = frame_id_;
00277 
00278     // get current CameraInfo data
00279     sensor_msgs::CameraInfoPtr
00280       ci(new sensor_msgs::CameraInfo(cinfo_->getCameraInfo()));
00281 
00282     // check whether CameraInfo matches current video mode
00283     //TODO: attention! we do not check that the camera source has not changed (top camera <-> bottom camera)
00284     if (image->width != ci->width || 
00285         image->height != ci->height)
00286       {
00287         // image size does not match: publish a matching uncalibrated
00288         // CameraInfo instead
00289         if (calibration_matches_)
00290           {
00291             // warn user once
00292             calibration_matches_ = false;
00293             ROS_WARN_STREAM("[" << camera_name_
00294                             << "] calibration does not match video mode "
00295                             << "(publishing uncalibrated data)");
00296           }
00297         ci.reset(new sensor_msgs::CameraInfo());
00298         ci->height = image->height;
00299         ci->width = image->width;
00300       }
00301     else if (!calibration_matches_)
00302       {
00303         // calibration OK now
00304         calibration_matches_ = true;
00305         ROS_WARN_STREAM("[" << camera_name_
00306                         << "] calibration matches video mode now");
00307       }
00308 
00309     ci->header.frame_id = frame_id_;
00310     ci->header.stamp = image->header.stamp;
00311 
00312     // Publish via image_transport
00313     image_pub_.publish(image, ci);
00314 
00315     // Notify diagnostics that a message has been published. That will
00316     // generate a warning if messages are not published at nearly the
00317     // configured frame_rate.
00318     topic_diagnostics_.tick(image->header.stamp);
00319   }
00320 
00326   bool NaoCameraDriver::read(sensor_msgs::ImagePtr &image)
00327   {
00328     bool success = true;
00329     try
00330       {
00331         // Read data from the Camera
00332         ROS_DEBUG_STREAM("[" << camera_name_ << "] reading data");
00333 
00334         //TODO: support local image access. This first suppose
00335         // to write a MAOqi 'local' module.
00336         // cf: http://www.aldebaran-robotics.com/documentation/dev/cpp/tutos/create_module.html#how-to-create-a-local-module
00337         ALValue al_image = camera_proxy_->getImageRemote(camera_name_);
00338 
00339         if (config_.use_ros_time)
00340             image->header.stamp = ros::Time::now();
00341         else { 
00342             // use NAOqi timestamp
00343             image->header.stamp = ros::Time(((double) al_image[4] / 1000000.0) + (double) al_image[5]);
00344         }
00345 
00346         image->width = (int) al_image[0];
00347         image->height = (int) al_image[1];
00348         image->step = image->width * (int) al_image[2];
00349         image->encoding = sensor_msgs::image_encodings::RGB8;
00350 
00351         int image_size = image->height * image->step;
00352         image->data.resize(image_size);
00353 
00354         memcpy(&(image->data)[0], 
00355                 al_image[6].GetBinary(),
00356                 image_size);
00357 
00358         camera_proxy_->releaseImage(camera_name_);
00359 
00360         success = true;
00361         ROS_DEBUG_STREAM("[" << camera_name_ << "] read returned");
00362       }
00363     catch (naocamera_driver::Exception& e)
00364       {
00365         ROS_WARN_STREAM("[" << camera_name_
00366                         << "] Exception reading data: " << e.what());
00367         success = false;
00368       }
00369     return success;
00370   }
00371 
00381   void NaoCameraDriver::reconfig(Config &newconfig, uint32_t level)
00382   {
00383     // Do not run concurrently with poll().  Tell it to stop running,
00384     // and wait on the lock until it does.
00385     reconfiguring_ = true;
00386     ROS_DEBUG("dynamic reconfigure level 0x%x", level);
00387 
00388     // resolve frame ID using tf_prefix parameter
00389     if (newconfig.source == kTopCamera)
00390       frame_id_ = "CameraTop_frame";
00391     else if (newconfig.source == kBottomCamera)
00392       frame_id_ = "CameraBottom_frame";
00393     else {
00394         ROS_ERROR("Unknown video source! (neither top nor bottom camera).");
00395         frame_id_ = "camera";
00396     }
00397 
00398     string tf_prefix = tf::getPrefixParam(priv_nh_);
00399     ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
00400     frame_id_ = tf::resolve(tf_prefix, frame_id_);
00401 
00402     if (state_ != Driver::CLOSED && (level & Levels::RECONFIGURE_CLOSE))
00403     {
00404         // must close the device before updating these parameters
00405         closeCamera();                  // state_ --> CLOSED
00406     }
00407 
00408     if (config_.camera_info_url != newconfig.camera_info_url)
00409       {
00410         // set the new URL and load CameraInfo (if any) from it
00411         if (cinfo_->validateURL(newconfig.camera_info_url))
00412           {
00413             cinfo_->loadCameraInfo(newconfig.camera_info_url);
00414           }
00415         else
00416           {
00417             // new URL not valid, use the old one
00418             newconfig.camera_info_url = config_.camera_info_url;
00419           }
00420       }
00421 
00422     if (state_ != Driver::CLOSED)       // openCamera() succeeded?
00423     {
00424 
00425         if (config_.auto_exposition != newconfig.auto_exposition)
00426             camera_proxy_->setParam(kCameraAutoExpositionID, newconfig.auto_exposition);
00427 
00428         if (config_.auto_exposure_algo != newconfig.auto_exposure_algo)
00429             camera_proxy_->setParam(kCameraExposureAlgorithmID, newconfig.auto_exposure_algo);
00430 
00431         if (config_.exposure != newconfig.exposure) {
00432             newconfig.auto_exposition = 0;
00433             camera_proxy_->setParam(kCameraAutoExpositionID, 0);
00434             camera_proxy_->setParam(kCameraExposureID, newconfig.exposure);
00435         }
00436 
00437         if (config_.gain != newconfig.gain) {
00438             newconfig.auto_exposition = 0;
00439             camera_proxy_->setParam(kCameraAutoExpositionID, 0);
00440             camera_proxy_->setParam(kCameraGainID, newconfig.gain);
00441         }
00442 
00443         if (config_.brightness != newconfig.brightness) {
00444             newconfig.auto_exposition = 1;
00445             camera_proxy_->setParam(kCameraAutoExpositionID, 1);
00446             camera_proxy_->setParam(kCameraBrightnessID, newconfig.brightness);
00447         }
00448 
00449         if (config_.contrast != newconfig.contrast)
00450             camera_proxy_->setParam(kCameraContrastID, newconfig.contrast);
00451 
00452         if (config_.saturation != newconfig.saturation)
00453             camera_proxy_->setParam(kCameraSaturationID, newconfig.saturation);
00454  
00455         if (config_.hue != newconfig.hue)
00456             camera_proxy_->setParam(kCameraHueID, newconfig.hue);
00457 
00458         if (config_.sharpness != newconfig.sharpness)
00459             camera_proxy_->setParam(kCameraSharpnessID, newconfig.sharpness);
00460 
00461         if (config_.auto_white_balance != newconfig.auto_white_balance)
00462             camera_proxy_->setParam(kCameraAutoWhiteBalanceID, newconfig.auto_white_balance);
00463 
00464         if (config_.white_balance != newconfig.white_balance) {
00465             newconfig.auto_white_balance = 0;
00466             camera_proxy_->setParam(kCameraAutoWhiteBalanceID, 0);
00467             camera_proxy_->setParam(kCameraWhiteBalanceID, newconfig.white_balance);
00468         }
00469     }
00470 
00471     config_ = newconfig;                // save new parameters
00472     real_frame_rate_ = ros::Rate(newconfig.frame_rate);
00473 
00474     reconfiguring_ = false;             // let poll() run again
00475 
00476     ROS_DEBUG_STREAM("[" << camera_name_
00477                      << "] reconfigured: frame_id " << frame_id_
00478                      << ", camera_info_url " << newconfig.camera_info_url);
00479   }
00480 
00481 
00488   void NaoCameraDriver::setup(void)
00489   {
00490     srv_.setCallback(boost::bind(&NaoCameraDriver::reconfig, this, _1, _2));
00491     ROS_INFO("Ready to publish Nao cameras. Waiting for someone to subscribe...");
00492   }
00493 
00494 
00496   void NaoCameraDriver::shutdown(void)
00497   {
00498     closeCamera();
00499   }
00500 
00501 }; // end namespace naocamera_driver


nao_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 Mon Oct 6 2014 02:40:25