CameraNode.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #include <ros/ros.h>
00029 #include <sensor_msgs/Image.h>
00030 #include <sensor_msgs/image_encodings.h>
00031 #include <cv_bridge/cv_bridge.h>
00032 #include <std_msgs/Empty.h>
00033 #include <image_transport/image_transport.h>
00034 #include <std_srvs/Empty.h>
00035 
00036 #include <rtabmap/core/CameraRGB.h>
00037 #include <rtabmap/core/CameraThread.h>
00038 #include <rtabmap/core/CameraEvent.h>
00039 
00040 #include <rtabmap/utilite/ULogger.h>
00041 #include <rtabmap/utilite/UEventsHandler.h>
00042 #include <rtabmap/utilite/UEventsManager.h>
00043 #include <rtabmap/utilite/UDirectory.h>
00044 #include <rtabmap/utilite/UFile.h>
00045 
00046 #include <dynamic_reconfigure/server.h>
00047 #include <rtabmap_ros/CameraConfig.h>
00048 
00049 class CameraWrapper : public UEventsHandler
00050 {
00051 public:
00052         // Usb device like a Webcam
00053         CameraWrapper(int usbDevice = 0,
00054                               float imageRate = 0,
00055                                   unsigned int imageWidth = 0,
00056                                   unsigned int imageHeight = 0) :
00057                 cameraThread_(0),
00058                 camera_(0),
00059                 frameId_("camera")
00060         {
00061                 ros::NodeHandle nh;
00062                 ros::NodeHandle pnh("~");
00063                 pnh.param("frame_id", frameId_, frameId_);
00064                 image_transport::ImageTransport it(nh);
00065                 rosPublisher_ = it.advertise("image", 1);
00066                 startSrv_ = nh.advertiseService("start_camera", &CameraWrapper::startSrv, this);
00067                 stopSrv_ = nh.advertiseService("stop_camera", &CameraWrapper::stopSrv, this);
00068                 UEventsManager::addHandler(this);
00069         }
00070 
00071         virtual ~CameraWrapper()
00072         {
00073                 if(cameraThread_)
00074                 {
00075                         cameraThread_->join(true);
00076                         delete cameraThread_;
00077                 }
00078         }
00079 
00080         bool init()
00081         {
00082                 if(cameraThread_)
00083                 {
00084                         return cameraThread_->camera()->init();
00085                 }
00086                 return false;
00087         }
00088 
00089         void start()
00090         {
00091                 if(cameraThread_)
00092                 {
00093                         return cameraThread_->start();
00094                 }
00095         }
00096 
00097         bool startSrv(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
00098         {
00099                 ROS_INFO("Camera started...");
00100                 if(cameraThread_)
00101                 {
00102                         cameraThread_->start();
00103                 }
00104                 return true;
00105         }
00106 
00107         bool stopSrv(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
00108         {
00109                 ROS_INFO("Camera stopped...");
00110                 if(cameraThread_)
00111                 {
00112                         cameraThread_->kill();
00113                 }
00114                 return true;
00115         }
00116 
00117         void setParameters(int deviceId, double frameRate, const std::string & path, bool pause)
00118         {
00119                 ROS_INFO("Parameters changed: deviceId=%d, path=%s frameRate=%f pause=%s",
00120                                 deviceId, path.c_str(), frameRate, pause?"true":"false");
00121                 if(cameraThread_)
00122                 {
00123                         rtabmap::CameraVideo * videoCam = dynamic_cast<rtabmap::CameraVideo *>(camera_);
00124                         rtabmap::CameraImages * imagesCam = dynamic_cast<rtabmap::CameraImages *>(camera_);
00125 
00126                         if(imagesCam)
00127                         {
00128                                 // images
00129                                 if(!path.empty() && UDirectory::getDir(path+"/").compare(UDirectory::getDir(imagesCam->getPath())) == 0)
00130                                 {
00131                                         imagesCam->setImageRate(frameRate);
00132                                         if(pause && !cameraThread_->isPaused())
00133                                         {
00134                                                 cameraThread_->join(true);
00135                                         }
00136                                         else if(!pause && cameraThread_->isPaused())
00137                                         {
00138                                                 cameraThread_->start();
00139                                         }
00140                                 }
00141                                 else
00142                                 {
00143                                         delete cameraThread_;
00144                                         cameraThread_ = 0;
00145                                 }
00146                         }
00147                         else if(videoCam)
00148                         {
00149                                 if(!path.empty() && path.compare(videoCam->getFilePath()) == 0)
00150                                 {
00151                                         // video
00152                                         videoCam->setImageRate(frameRate);
00153                                         if(pause && !cameraThread_->isPaused())
00154                                         {
00155                                                 cameraThread_->join(true);
00156                                         }
00157                                         else if(!pause && cameraThread_->isPaused())
00158                                         {
00159                                                 cameraThread_->start();
00160                                         }
00161                                 }
00162                                 else if(path.empty() &&
00163                                                 videoCam->getFilePath().empty() &&
00164                                                 videoCam->getUsbDevice() == deviceId)
00165                                 {
00166                                         // usb device
00167                                         videoCam->setImageRate(frameRate);
00168                                         if(pause && !cameraThread_->isPaused())
00169                                         {
00170                                                 cameraThread_->join(true);
00171                                         }
00172                                         else if(!pause && cameraThread_->isPaused())
00173                                         {
00174                                                 cameraThread_->start();
00175                                         }
00176                                 }
00177                                 else
00178                                 {
00179                                         delete cameraThread_;
00180                                         cameraThread_ = 0;
00181                                 }
00182                         }
00183                         else
00184                         {
00185                                 ROS_ERROR("Wrong camera type ?!?");
00186                                 delete cameraThread_;
00187                                 cameraThread_ = 0;
00188                         }
00189                 }
00190 
00191                 if(!cameraThread_)
00192                 {
00193                         if(!path.empty() && UDirectory::exists(path))
00194                         {
00195                                 //images
00196                                 camera_ = new rtabmap::CameraImages(path, frameRate);
00197                         }
00198                         else if(!path.empty() && UFile::exists(path))
00199                         {
00200                                 //video
00201                                 camera_ = new rtabmap::CameraVideo(path, false, frameRate);
00202                         }
00203                         else
00204                         {
00205                                 if(!path.empty() && !UDirectory::exists(path) && !UFile::exists(path))
00206                                 {
00207                                         ROS_ERROR("Path \"%s\" does not exist (or you don't have the permissions to read)... falling back to usb device...", path.c_str());
00208                                 }
00209                                 //usb device
00210                                 camera_ = new rtabmap::CameraVideo(deviceId, false, frameRate);
00211                         }
00212                         cameraThread_ = new rtabmap::CameraThread(camera_);
00213                         init();
00214                         if(!pause)
00215                         {
00216                                 start();
00217                         }
00218                 }
00219         }
00220 
00221 protected:
00222         virtual void handleEvent(UEvent * event)
00223         {
00224                 if(event->getClassName().compare("CameraEvent") == 0)
00225                 {
00226                         rtabmap::CameraEvent * e = (rtabmap::CameraEvent*)event;
00227                         const cv::Mat & image = e->data().imageRaw();
00228                         if(!image.empty() && image.depth() == CV_8U)
00229                         {
00230                                 cv_bridge::CvImage img;
00231                                 if(image.channels() == 1)
00232                                 {
00233                                         img.encoding = sensor_msgs::image_encodings::MONO8;
00234                                 }
00235                                 else
00236                                 {
00237                                         img.encoding = sensor_msgs::image_encodings::BGR8;
00238                                 }
00239                                 img.image = image;
00240                                 sensor_msgs::ImagePtr rosMsg = img.toImageMsg();
00241                                 rosMsg->header.frame_id = frameId_;
00242                                 rosMsg->header.stamp = ros::Time::now();
00243                                 rosPublisher_.publish(rosMsg);
00244                         }
00245                 }
00246         }
00247 
00248 private:
00249         image_transport::Publisher rosPublisher_;
00250         rtabmap::CameraThread * cameraThread_;
00251         rtabmap::Camera * camera_;
00252         ros::ServiceServer startSrv_;
00253         ros::ServiceServer stopSrv_;
00254         std::string frameId_;
00255 };
00256 
00257 CameraWrapper * camera = 0;
00258 void callback(rtabmap_ros::CameraConfig &config, uint32_t level)
00259 {
00260         if(camera)
00261         {
00262                 camera->setParameters(config.device_id, config.frame_rate, config.video_or_images_path, config.pause);
00263         }
00264 }
00265 
00266 int main(int argc, char** argv)
00267 {
00268         ULogger::setType(ULogger::kTypeConsole);
00269         //ULogger::setLevel(ULogger::kDebug);
00270         ULogger::setEventLevel(ULogger::kWarning);
00271 
00272         ros::init(argc, argv, "camera");
00273 
00274         ros::NodeHandle nh("~");
00275 
00276         camera = new CameraWrapper(); // webcam device 0
00277 
00278         dynamic_reconfigure::Server<rtabmap_ros::CameraConfig> server;
00279         dynamic_reconfigure::Server<rtabmap_ros::CameraConfig>::CallbackType f;
00280         f = boost::bind(&callback, _1, _2);
00281         server.setCallback(f);
00282 
00283         ros::spin();
00284 
00285         //cleanup
00286         if(camera)
00287         {
00288                 delete camera;
00289         }
00290 
00291         return 0;
00292 }


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Sun Jul 24 2016 03:49:07