CameraNode.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <ros/ros.h>
29 #include <sensor_msgs/Image.h>
31 #include <cv_bridge/cv_bridge.h>
32 #include <std_msgs/Empty.h>
34 #include <std_srvs/Empty.h>
35 
36 #include <rtabmap/core/CameraRGB.h>
39 
44 #include <rtabmap/utilite/UFile.h>
45 
46 #include <dynamic_reconfigure/server.h>
47 #include <rtabmap_ros/CameraConfig.h>
48 
50 {
51 public:
52  // Usb device like a Webcam
53  CameraWrapper(int usbDevice = 0,
54  float imageRate = 0,
55  unsigned int imageWidth = 0,
56  unsigned int imageHeight = 0) :
57  cameraThread_(0),
58  camera_(0),
59  frameId_("camera")
60  {
61  ros::NodeHandle nh;
62  ros::NodeHandle pnh("~");
63  pnh.param("frame_id", frameId_, frameId_);
65  rosPublisher_ = it.advertise("image", 1);
66  startSrv_ = nh.advertiseService("start_camera", &CameraWrapper::startSrv, this);
67  stopSrv_ = nh.advertiseService("stop_camera", &CameraWrapper::stopSrv, this);
69  }
70 
71  virtual ~CameraWrapper()
72  {
73  if(cameraThread_)
74  {
75  cameraThread_->join(true);
76  delete cameraThread_;
77  }
78  }
79 
80  bool init()
81  {
82  if(cameraThread_)
83  {
84  return cameraThread_->camera()->init();
85  }
86  return false;
87  }
88 
89  void start()
90  {
91  if(cameraThread_)
92  {
94  }
95  }
96 
97  bool startSrv(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
98  {
99  ROS_INFO("Camera started...");
100  if(cameraThread_)
101  {
102  cameraThread_->start();
103  }
104  return true;
105  }
106 
107  bool stopSrv(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
108  {
109  ROS_INFO("Camera stopped...");
110  if(cameraThread_)
111  {
112  cameraThread_->kill();
113  }
114  return true;
115  }
116 
117  void setParameters(int deviceId, double frameRate, const std::string & path, bool pause)
118  {
119  ROS_INFO("Parameters changed: deviceId=%d, path=%s frameRate=%f pause=%s",
120  deviceId, path.c_str(), frameRate, pause?"true":"false");
121  if(cameraThread_)
122  {
123  rtabmap::CameraVideo * videoCam = dynamic_cast<rtabmap::CameraVideo *>(camera_);
124  rtabmap::CameraImages * imagesCam = dynamic_cast<rtabmap::CameraImages *>(camera_);
125 
126  if(imagesCam)
127  {
128  // images
129  if(!path.empty() && UDirectory::getDir(path+"/").compare(UDirectory::getDir(imagesCam->getPath())) == 0)
130  {
131  imagesCam->setImageRate(frameRate);
132  if(pause && !cameraThread_->isPaused())
133  {
134  cameraThread_->join(true);
135  }
136  else if(!pause && cameraThread_->isPaused())
137  {
138  cameraThread_->start();
139  }
140  }
141  else
142  {
143  delete cameraThread_;
144  cameraThread_ = 0;
145  }
146  }
147  else if(videoCam)
148  {
149  if(!path.empty() && path.compare(videoCam->getFilePath()) == 0)
150  {
151  // video
152  videoCam->setImageRate(frameRate);
153  if(pause && !cameraThread_->isPaused())
154  {
155  cameraThread_->join(true);
156  }
157  else if(!pause && cameraThread_->isPaused())
158  {
159  cameraThread_->start();
160  }
161  }
162  else if(path.empty() &&
163  videoCam->getFilePath().empty() &&
164  videoCam->getUsbDevice() == deviceId)
165  {
166  // usb device
167  videoCam->setImageRate(frameRate);
168  if(pause && !cameraThread_->isPaused())
169  {
170  cameraThread_->join(true);
171  }
172  else if(!pause && cameraThread_->isPaused())
173  {
174  cameraThread_->start();
175  }
176  }
177  else
178  {
179  delete cameraThread_;
180  cameraThread_ = 0;
181  }
182  }
183  else
184  {
185  ROS_ERROR("Wrong camera type ?!?");
186  delete cameraThread_;
187  cameraThread_ = 0;
188  }
189  }
190 
191  if(!cameraThread_)
192  {
193  if(!path.empty() && UDirectory::exists(path))
194  {
195  //images
196  camera_ = new rtabmap::CameraImages(path, frameRate);
197  }
198  else if(!path.empty() && UFile::exists(path))
199  {
200  //video
201  camera_ = new rtabmap::CameraVideo(path, false, frameRate);
202  }
203  else
204  {
205  if(!path.empty() && !UDirectory::exists(path) && !UFile::exists(path))
206  {
207  ROS_ERROR("Path \"%s\" does not exist (or you don't have the permissions to read)... falling back to usb device...", path.c_str());
208  }
209  //usb device
210  camera_ = new rtabmap::CameraVideo(deviceId, false, frameRate);
211  }
213  init();
214  if(!pause)
215  {
216  start();
217  }
218  }
219  }
220 
221 protected:
222  virtual bool handleEvent(UEvent * event)
223  {
224  if(event->getClassName().compare("CameraEvent") == 0)
225  {
227  const cv::Mat & image = e->data().imageRaw();
228  if(!image.empty() && image.depth() == CV_8U)
229  {
230  cv_bridge::CvImage img;
231  if(image.channels() == 1)
232  {
234  }
235  else
236  {
238  }
239  img.image = image;
240  sensor_msgs::ImagePtr rosMsg = img.toImageMsg();
241  rosMsg->header.frame_id = frameId_;
242  rosMsg->header.stamp = ros::Time::now();
243  rosPublisher_.publish(rosMsg);
244  }
245  }
246  return false;
247  }
248 
249 private:
255  std::string frameId_;
256 };
257 
259 void callback(rtabmap_ros::CameraConfig &config, uint32_t level)
260 {
261  if(camera)
262  {
263  camera->setParameters(config.device_id, config.frame_rate, config.video_or_images_path, config.pause);
264  }
265 }
266 
267 int main(int argc, char** argv)
268 {
270  //ULogger::setLevel(ULogger::kDebug);
272 
273  ros::init(argc, argv, "camera");
274 
275  ros::NodeHandle nh("~");
276 
277  camera = new CameraWrapper(); // webcam device 0
278 
279  dynamic_reconfigure::Server<rtabmap_ros::CameraConfig> server;
280  dynamic_reconfigure::Server<rtabmap_ros::CameraConfig>::CallbackType f;
281  f = boost::bind(&callback, _1, _2);
282  server.setCallback(f);
283 
284  ros::spin();
285 
286  //cleanup
287  if(camera)
288  {
289  delete camera;
290  }
291 
292  return 0;
293 }
void start()
f
image_transport::Publisher rosPublisher_
Definition: CameraNode.cpp:250
void setParameters(int deviceId, double frameRate, const std::string &path, bool pause)
Definition: CameraNode.cpp:117
int main(int argc, char **argv)
Definition: CameraNode.cpp:267
static std::string getDir(const std::string &filePath)
ros::ServiceServer stopSrv_
Definition: CameraNode.cpp:254
void setImageRate(float imageRate)
void kill()
GLM_FUNC_DECL genType e()
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
boost::shared_ptr< interactive_markers::InteractiveMarkerServer > server
std::string encoding
Publisher advertise(const std::string &base_topic, uint32_t queue_size, bool latch=false)
ros::ServiceServer startSrv_
Definition: CameraNode.cpp:253
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
const cv::Mat & imageRaw() const
rtabmap::CameraThread * cameraThread_
Definition: CameraNode.cpp:251
ROSCPP_DECL void spin(Spinner &spinner)
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="")=0
int getUsbDevice() const
CameraWrapper * camera
Definition: CameraNode.cpp:258
rtabmap::Camera * camera_
Definition: CameraNode.cpp:252
void publish(const sensor_msgs::Image &message) const
virtual std::string getClassName() const =0
std::string frameId_
Definition: CameraNode.cpp:255
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
static void setType(Type type, const std::string &fileName=kDefaultLogFileName, bool append=true)
CameraWrapper(int usbDevice=0, float imageRate=0, unsigned int imageWidth=0, unsigned int imageHeight=0)
Definition: CameraNode.cpp:53
static void setEventLevel(ULogger::Level eventSentLevel)
detail::uint32 uint32_t
void callback(rtabmap_ros::CameraConfig &config, uint32_t level)
Definition: CameraNode.cpp:259
virtual ~CameraWrapper()
Definition: CameraNode.cpp:71
bool exists()
static void addHandler(UEventsHandler *handler)
static Time now()
bool startSrv(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: CameraNode.cpp:97
static bool exists(const std::string &dirPath)
std::string getPath() const
const SensorData & data() const
const std::string & getFilePath() const
void join(bool killFirst=false)
bool stopSrv(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: CameraNode.cpp:107
virtual bool handleEvent(UEvent *event)
Definition: CameraNode.cpp:222
bool isPaused() const
#define ROS_ERROR(...)
sensor_msgs::ImagePtr toImageMsg() const


rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Fri Jun 7 2019 21:55:03