naoqi_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 "naoqi_sensors/NaoqiCameraConfig.h"
00059 
00060 #include "naoqi_camera.h"
00079 using namespace std;
00080 using namespace AL;
00081 
00082 namespace naoqicamera_driver
00083 {
00084   // some convenience typedefs
00085   typedef naoqicamera::NaoqiCameraConfig Config;
00086   typedef driver_base::Driver Driver;
00087   typedef driver_base::SensorLevels Levels;
00088 
00089   NaoqiCameraDriver::NaoqiCameraDriver(int argc, char ** argv,
00090                                    ros::NodeHandle priv_nh,
00091                                    ros::NodeHandle camera_nh):
00092     NaoqiNode(argc, argv),
00093     state_(Driver::CLOSED),
00094     priv_nh_(priv_nh),
00095     camera_nh_(camera_nh),
00096     camera_name_("camera"),
00097     cycle_(1.0),                        // slow poll when closed
00098     real_frame_rate_(30.0),
00099     retries_(0),
00100     srv_(priv_nh),
00101     cinfo_(new camera_info_manager::CameraInfoManager(camera_nh_)),
00102     calibration_matches_(true),
00103     it_(new image_transport::ImageTransport(camera_nh_)),
00104     image_pub_(it_->advertiseCamera("image_raw", 1)),
00105     diagnostics_(),
00106     topic_diagnostics_min_freq_(0.),
00107     topic_diagnostics_max_freq_(1000.),
00108     topic_diagnostics_("image_raw", diagnostics_,
00109                 diagnostic_updater::FrequencyStatusParam
00110                     (&topic_diagnostics_min_freq_,
00111                      &topic_diagnostics_max_freq_, 0.1, 10),
00112                 diagnostic_updater::TimeStampStatusParam())
00113   {
00114     getNaoqiParams(priv_nh);
00115     
00116     if ( connectNaoQi() )
00117     {
00118       ROS_DEBUG("Connected to remote NAOqi.");
00119       return;
00120     }
00121 
00122     ROS_WARN("Could not connect to remote NAOqi! Trying to connect to local NAOqi.");
00123 
00124     if ( connectLocalNaoQi() )
00125     {
00126       ROS_DEBUG("Connected to local NAOqi.");
00127       return;
00128     }
00129 
00130     throw naoqicamera_driver::Exception("Connection to NAOqi failed");
00131   }
00132 
00133   NaoqiCameraDriver::~NaoqiCameraDriver()
00134   {}
00135 
00136 
00141   void NaoqiCameraDriver::getNaoqiParams(ros::NodeHandle nh)
00142   {
00143     if( !nh.getParam("pip", m_pip) )
00144       ROS_WARN("No pip parameter specified.");
00145     if( !nh.getParam("pport", m_pport) )
00146       ROS_WARN("No pport parameter specified.");
00147     if( !nh.getParam("ip", m_ip) )
00148       ROS_DEBUG("No ip parameter specified.");
00149     if( !nh.getParam("port", m_port) )
00150       ROS_DEBUG("No port parameter specified.");
00151 
00152     ROS_INFO_STREAM("pip: " << m_pip);
00153     ROS_INFO_STREAM("pip: " << m_pport);
00154     ROS_INFO_STREAM("ip:" << m_ip);
00155     ROS_INFO_STREAM("port: " << m_port);
00156   }
00157 
00162   void NaoqiCameraDriver::closeCamera()
00163   {
00164     if (state_ != Driver::CLOSED)
00165       {
00166         ROS_INFO_STREAM("[" << camera_name_ << "] closing device");
00167         camera_proxy_->unsubscribe(camera_name_);
00168         state_ = Driver::CLOSED;
00169       }
00170   }
00171 
00184   bool NaoqiCameraDriver::openCamera(Config &newconfig)
00185   {
00186     bool success = false;
00187 
00188     try
00189     {
00190         camera_proxy_ = boost::shared_ptr<ALVideoDeviceProxy>(new ALVideoDeviceProxy(m_broker));
00191 
00192         if (camera_proxy_->getGenericProxy()->isLocal())
00193             ROS_INFO("naoqi_camera runs directly on the robot.");
00194         else
00195             ROS_WARN("naoqi_camera runs remotely.");
00196         // build a unique name for the camera that will be used to
00197         // store calibration informations.
00198         stringstream canonical_name;
00199         canonical_name << "nao_camera" 
00200                        << "_src" << newconfig.source 
00201                        << "_res" << newconfig.resolution;
00202 
00203         camera_name_ = camera_proxy_->subscribeCamera(
00204                                     camera_name_, 
00205                                     newconfig.source,
00206                                     newconfig.resolution,
00207                                     kRGBColorSpace,
00208                                     newconfig.frame_rate);
00209 
00210         if (!cinfo_->setCameraName(canonical_name.str()))
00211         {
00212             // GUID is 16 hex digits, which should be valid.
00213             // If not, use it for log messages anyway.
00214             ROS_WARN_STREAM("[" << camera_name_
00215                             << "] name not valid"
00216                             << " for camera_info_manger");
00217         }
00218 
00219         ROS_INFO_STREAM("[" << camera_name_ << "] opened: resolution "
00220                         << newconfig.resolution << " @"
00221                         << newconfig.frame_rate << " fps");
00222         state_ = Driver::OPENED;
00223         calibration_matches_ = true;
00224         newconfig.guid = camera_name_; // update configuration parameter
00225         retries_ = 0;
00226         success = true;
00227     }
00228     catch (const ALError& e)
00229     {
00230         state_ = Driver::CLOSED;    // since the open() failed
00231         if (retries_++ > 0)
00232           ROS_DEBUG_STREAM("[" << camera_name_
00233                            << "] exception opening device (retrying): "
00234                            << e.what());
00235         else
00236           ROS_ERROR_STREAM("[" << camera_name_
00237                            << "] device open failed: " << e.what());
00238     }
00239 
00240     // update diagnostics parameters
00241     diagnostics_.setHardwareID(camera_name_);
00242     double delta = newconfig.frame_rate * 0.1; // allow 10% error margin
00243     topic_diagnostics_min_freq_ = newconfig.frame_rate - delta;
00244     topic_diagnostics_max_freq_ = newconfig.frame_rate + delta;
00245 
00246     return success;
00247   }
00248 
00249 
00251   void NaoqiCameraDriver::poll(void)
00252   {
00253     // Do not run concurrently with reconfig().
00254     //
00255     // The mutex lock should be sufficient, but the Linux pthreads
00256     // implementation does not guarantee fairness, and the reconfig()
00257     // callback thread generally suffers from lock starvation for many
00258     // seconds before getting to run.  So, we avoid acquiring the lock
00259     // if there is a reconfig() pending.
00260     bool do_sleep = true;
00261     
00262     // publish images only if someone subscribe to the topic
00263     uint32_t nbSubscribers = image_pub_.getNumSubscribers();
00264 
00265     if (nbSubscribers == 0)
00266     {
00267         if (state_ == Driver::OPENED)
00268         {
00269             closeCamera();
00270         }
00271     }
00272     else
00273     {
00274         boost::mutex::scoped_lock scopedLock(reconfiguration_mutex_);
00275         if (state_ == Driver::CLOSED)
00276         {
00277             openCamera(config_);        // open with current configuration
00278         }
00279         do_sleep = (state_ == Driver::CLOSED);
00280         if (!do_sleep)                  // openCamera() succeeded?
00281         {
00282             // driver is open, read the next image still holding lock
00283             sensor_msgs::ImagePtr image(new sensor_msgs::Image);
00284             if (read(image))
00285             {
00286                 publish(image);
00287                 real_frame_rate_.sleep();
00288             }
00289         }
00290     } // release mutex lock
00291 
00292     // Always run the diagnostics updater: no lock required.
00293     diagnostics_.update();
00294 
00295     if (do_sleep)
00296       {
00297         // device was closed or poll is not running, sleeping avoids
00298         // busy wait (DO NOT hold the lock while sleeping)
00299         cycle_.sleep();
00300       }
00301   }
00302 
00307   void NaoqiCameraDriver::publish(const sensor_msgs::ImagePtr &image)
00308   {
00309     image->header.frame_id = frame_id_;
00310 
00311     // get current CameraInfo data
00312     sensor_msgs::CameraInfoPtr
00313       ci(new sensor_msgs::CameraInfo(cinfo_->getCameraInfo()));
00314 
00315     // check whether CameraInfo matches current video mode
00316     //TODO: attention! we do not check that the camera source has not changed (top camera <-> bottom camera)
00317     if (image->width != ci->width || 
00318         image->height != ci->height)
00319       {
00320         // image size does not match: publish a matching uncalibrated
00321         // CameraInfo instead
00322         if (calibration_matches_)
00323           {
00324             // warn user once
00325             calibration_matches_ = false;
00326             ROS_WARN_STREAM("[" << camera_name_
00327                             << "] calibration does not match video mode "
00328                             << "(publishing uncalibrated data)");
00329           }
00330         ci.reset(new sensor_msgs::CameraInfo());
00331         ci->height = image->height;
00332         ci->width = image->width;
00333       }
00334     else if (!calibration_matches_)
00335       {
00336         // calibration OK now
00337         calibration_matches_ = true;
00338         ROS_WARN_STREAM("[" << camera_name_
00339                         << "] calibration matches video mode now");
00340       }
00341 
00342     ci->header.frame_id = frame_id_;
00343     ci->header.stamp = image->header.stamp;
00344 
00345     // Publish via image_transport
00346     image_pub_.publish(image, ci);
00347 
00348     // Notify diagnostics that a message has been published. That will
00349     // generate a warning if messages are not published at nearly the
00350     // configured frame_rate.
00351     topic_diagnostics_.tick(image->header.stamp);
00352   }
00353 
00359   bool NaoqiCameraDriver::read(sensor_msgs::ImagePtr &image)
00360   {
00361     bool success = true;
00362     try
00363       {
00364         // Read data from the Camera
00365         ROS_DEBUG_STREAM("[" << camera_name_ << "] reading data");
00366 
00367         //TODO: support local image access. This first suppose
00368         // to write a MAOqi 'local' module.
00369         // cf: http://www.aldebaran-robotics.com/documentation/dev/cpp/tutos/create_module.html#how-to-create-a-local-module
00370         ALValue al_image = camera_proxy_->getImageRemote(camera_name_);
00371 
00372         if (config_.use_ros_time)
00373             image->header.stamp = ros::Time::now();
00374         else { 
00375             // use NAOqi timestamp
00376             image->header.stamp = ros::Time(static_cast<int>(al_image[4]), static_cast<int>(al_image[5]) * 1000);
00377         }
00378 
00379         image->width = (int) al_image[0];
00380         image->height = (int) al_image[1];
00381         image->step = image->width * (int) al_image[2];
00382         image->encoding = sensor_msgs::image_encodings::RGB8;
00383 
00384         int image_size = image->height * image->step;
00385         image->data.resize(image_size);
00386 
00387         memcpy(&(image->data)[0], 
00388                 al_image[6].GetBinary(),
00389                 image_size);
00390 
00391         camera_proxy_->releaseImage(camera_name_);
00392 
00393         success = true;
00394         ROS_DEBUG_STREAM("[" << camera_name_ << "] read returned");
00395       }
00396     catch (naoqicamera_driver::Exception& e)
00397       {
00398         ROS_WARN_STREAM("[" << camera_name_
00399                         << "] Exception reading data: " << e.what());
00400         success = false;
00401       }
00402     return success;
00403   }
00404 
00414   void NaoqiCameraDriver::reconfig(Config &newconfig, uint32_t level)
00415   {
00416     // Do not run concurrently with poll().
00417     boost::mutex::scoped_lock scopedLock(reconfiguration_mutex_);
00418     ROS_DEBUG("dynamic reconfigure level 0x%x", level);
00419 
00420     // resolve frame ID using tf_prefix parameter
00421     if (newconfig.source == kTopCamera)
00422       frame_id_ = "CameraTop_frame";
00423     else if (newconfig.source == kBottomCamera)
00424       frame_id_ = "CameraBottom_frame";
00425     else {
00426         ROS_ERROR("Unknown video source! (neither top nor bottom camera).");
00427         frame_id_ = "camera";
00428     }
00429 
00430     string tf_prefix = tf::getPrefixParam(priv_nh_);
00431     ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
00432     frame_id_ = tf::resolve(tf_prefix, frame_id_);
00433 
00434     if (state_ != Driver::CLOSED && (level & Levels::RECONFIGURE_CLOSE))
00435     {
00436         // must close the device before updating these parameters
00437         closeCamera();                  // state_ --> CLOSED
00438     }
00439 
00440     if (config_.camera_info_url != newconfig.camera_info_url)
00441       {
00442         // set the new URL and load CameraInfo (if any) from it
00443         if (cinfo_->validateURL(newconfig.camera_info_url))
00444           {
00445             cinfo_->loadCameraInfo(newconfig.camera_info_url);
00446           }
00447         else
00448           {
00449             // new URL not valid, use the old one
00450             newconfig.camera_info_url = config_.camera_info_url;
00451           }
00452       }
00453 
00454     if (state_ != Driver::CLOSED)       // openCamera() succeeded?
00455     {
00456 
00457         if (config_.auto_exposition != newconfig.auto_exposition)
00458             camera_proxy_->setParam(kCameraAutoExpositionID, newconfig.auto_exposition);
00459 
00460         if (config_.auto_exposure_algo != newconfig.auto_exposure_algo)
00461             camera_proxy_->setParam(kCameraExposureAlgorithmID, newconfig.auto_exposure_algo);
00462 
00463         if (config_.exposure != newconfig.exposure) {
00464             newconfig.auto_exposition = 0;
00465             camera_proxy_->setParam(kCameraAutoExpositionID, 0);
00466             camera_proxy_->setParam(kCameraExposureID, newconfig.exposure);
00467         }
00468 
00469         if (config_.gain != newconfig.gain) {
00470             newconfig.auto_exposition = 0;
00471             camera_proxy_->setParam(kCameraAutoExpositionID, 0);
00472             camera_proxy_->setParam(kCameraGainID, newconfig.gain);
00473         }
00474 
00475         if (config_.brightness != newconfig.brightness) {
00476             newconfig.auto_exposition = 1;
00477             camera_proxy_->setParam(kCameraAutoExpositionID, 1);
00478             camera_proxy_->setParam(kCameraBrightnessID, newconfig.brightness);
00479         }
00480 
00481         if (config_.contrast != newconfig.contrast)
00482             camera_proxy_->setParam(kCameraContrastID, newconfig.contrast);
00483 
00484         if (config_.saturation != newconfig.saturation)
00485             camera_proxy_->setParam(kCameraSaturationID, newconfig.saturation);
00486 
00487         if (config_.hue != newconfig.hue)
00488             camera_proxy_->setParam(kCameraHueID, newconfig.hue);
00489 
00490         if (config_.sharpness != newconfig.sharpness)
00491             camera_proxy_->setParam(kCameraSharpnessID, newconfig.sharpness);
00492 
00493         if (config_.auto_white_balance != newconfig.auto_white_balance)
00494             camera_proxy_->setParam(kCameraAutoWhiteBalanceID, newconfig.auto_white_balance);
00495 
00496         if (config_.white_balance != newconfig.white_balance) {
00497             newconfig.auto_white_balance = 0;
00498             camera_proxy_->setParam(kCameraAutoWhiteBalanceID, 0);
00499             camera_proxy_->setParam(kCameraWhiteBalanceID, newconfig.white_balance);
00500         }
00501     }
00502 
00503     config_ = newconfig;                // save new parameters
00504     real_frame_rate_ = ros::Rate(newconfig.frame_rate);
00505 
00506     ROS_DEBUG_STREAM("[" << camera_name_
00507                      << "] reconfigured: frame_id " << frame_id_
00508                      << ", camera_info_url " << newconfig.camera_info_url);
00509   }
00510 
00511 
00518   void NaoqiCameraDriver::setup(void)
00519   {
00520     srv_.setCallback(boost::bind(&NaoqiCameraDriver::reconfig, this, _1, _2));
00521     ROS_INFO("Ready to publish Nao cameras. Waiting for someone to subscribe...");
00522   }
00523 
00524 
00526   void NaoqiCameraDriver::shutdown(void)
00527   {
00528     closeCamera();
00529   }
00530 
00531 }; // 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