00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
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
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),
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
00197
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
00213
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_;
00225 retries_ = 0;
00226 success = true;
00227 }
00228 catch (const ALError& e)
00229 {
00230 state_ = Driver::CLOSED;
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
00241 diagnostics_.setHardwareID(camera_name_);
00242 double delta = newconfig.frame_rate * 0.1;
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
00254
00255
00256
00257
00258
00259
00260 bool do_sleep = true;
00261
00262
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_);
00278 }
00279 do_sleep = (state_ == Driver::CLOSED);
00280 if (!do_sleep)
00281 {
00282
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 }
00291
00292
00293 diagnostics_.update();
00294
00295 if (do_sleep)
00296 {
00297
00298
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
00312 sensor_msgs::CameraInfoPtr
00313 ci(new sensor_msgs::CameraInfo(cinfo_->getCameraInfo()));
00314
00315
00316
00317 if (image->width != ci->width ||
00318 image->height != ci->height)
00319 {
00320
00321
00322 if (calibration_matches_)
00323 {
00324
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
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
00346 image_pub_.publish(image, ci);
00347
00348
00349
00350
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
00365 ROS_DEBUG_STREAM("[" << camera_name_ << "] reading data");
00366
00367
00368
00369
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
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
00417 boost::mutex::scoped_lock scopedLock(reconfiguration_mutex_);
00418 ROS_DEBUG("dynamic reconfigure level 0x%x", level);
00419
00420
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
00437 closeCamera();
00438 }
00439
00440 if (config_.camera_info_url != newconfig.camera_info_url)
00441 {
00442
00443 if (cinfo_->validateURL(newconfig.camera_info_url))
00444 {
00445 cinfo_->loadCameraInfo(newconfig.camera_info_url);
00446 }
00447 else
00448 {
00449
00450 newconfig.camera_info_url = config_.camera_info_url;
00451 }
00452 }
00453
00454 if (state_ != Driver::CLOSED)
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;
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 };