stereo_nodelet.cpp
Go to the documentation of this file.
1 /*
2 This code was developed by the National Robotics Engineering Center (NREC), part of the Robotics Institute at Carnegie Mellon University.
3 Its development was funded by DARPA under the LS3 program and submitted for public release on June 7th, 2012.
4 Release was granted on August, 21st 2012 with Distribution Statement "A" (Approved for Public Release, Distribution Unlimited).
5 
6 This software is released under a BSD license:
7 
8 Copyright (c) 2012, Carnegie Mellon University. All rights reserved.
9 
10 Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
11 
12 Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
13 Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
14 Neither the name of the Carnegie Mellon University nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
15 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
16 */
17 
18 
19 
31 // ROS and associated nodelet interface and PLUGINLIB declaration header
32 #include "ros/ros.h"
34 #include <nodelet/nodelet.h>
35 
36 #include "pointgrey_camera_driver/PointGreyCamera.h" // The actual standalone library for the PointGreys
37 
38 #include <image_transport/image_transport.h> // ROS library that allows sending compressed images
39 #include <camera_info_manager/camera_info_manager.h> // ROS library that publishes CameraInfo topics
40 #include <sensor_msgs/CameraInfo.h> // ROS message header for CameraInfo
41 #include <std_msgs/Float64.h>
42 
43 #include <wfov_camera_msgs/WFOVImage.h>
44 #include <image_exposure_msgs/ExposureSequence.h> // Message type for configuring gain and white balance.
45 
46 #include <diagnostic_updater/diagnostic_updater.h> // Headers for publishing diagnostic messages.
48 
49 #include <boost/thread.hpp> // Needed for the nodelet to launch the reading thread.
50 
51 #include <dynamic_reconfigure/server.h> // Needed for the dynamic_reconfigure gui service to run
52 
54 {
55 
57 {
58 public:
60 
62  {
63  pubThread_->interrupt();
64  pubThread_->join();
65  cleanUp();
66  }
67 
68 private:
76  void paramCallback(pointgrey_camera_driver::PointGreyConfig &config, uint32_t level)
77  {
78 
79  // Stereo is only active in this mode (16 bits, 8 for each image)
80  config.video_mode = "format7_mode3";
81 
82  try
83  {
84  NODELET_DEBUG("Dynamic reconfigure callback with level: %d", level);
85  pg_.setNewConfiguration(config, level);
86 
87  // Store needed parameters for the metadata message
88  gain_ = config.gain;
89  wb_blue_ = config.white_balance_blue;
90  wb_red_ = config.white_balance_red;
91 
92 
93  // Store CameraInfo binning information
94  if(config.video_mode == "640x480_mono8" || config.video_mode == "format7_mode1")
95  {
96  binning_x_ = 2;
97  binning_y_ = 2;
98  }
99  else if(config.video_mode == "format7_mode2")
100  {
101  binning_x_ = 0;
102  binning_y_ = 2;
103  }
104  else
105  {
106  binning_x_ = 0;
107  binning_y_ = 0;
108  }
109 
110  // Store CameraInfo RegionOfInterest information
111  if(config.video_mode == "format7_mode0" || config.video_mode == "format7_mode1" || config.video_mode == "format7_mode2")
112  {
113  roi_x_offset_ = config.format7_x_offset;
114  roi_y_offset_ = config.format7_y_offset;
115  roi_width_ = config.format7_roi_width;
116  roi_height_ = config.format7_roi_height;
117  do_rectify_ = true; // Set to true if an ROI is used.
118  }
119  else
120  {
121  // Zeros mean the full resolution was captured.
122  roi_x_offset_ = 0;
123  roi_y_offset_ = 0;
124  roi_height_ = 0;
125  roi_width_ = 0;
126  do_rectify_ = false; // Set to false if the whole image is captured.
127  }
128  }
129  catch(std::runtime_error& e)
130  {
131  NODELET_ERROR("Reconfigure Callback failed with error: %s", e.what());
132  }
133  }
134 
140  void onInit()
141  {
142  // Get nodeHandles
145  std::string firstcam;
146  pnh.param<std::string>("first_namespace", firstcam, "left");
147  ros::NodeHandle lnh(getMTNodeHandle(), firstcam);
148  std::string secondcam;
149  pnh.param<std::string>("second_namespace", secondcam, "right");
150  ros::NodeHandle rnh(getMTNodeHandle(), secondcam);
151 
152  // Get a serial number through ros
153  int serialParam;
154  pnh.param<int>("serial", serialParam, 0);
155  uint32_t serial = (uint32_t)serialParam;
156  // Get the location of our camera config yamls
157  std::string camera_info_url;
158  pnh.param<std::string>("camera_info_url", camera_info_url, "");
159  std::string second_info_url;
160  pnh.param<std::string>("second_info_url", second_info_url, "");
161  // Get the desired frame_id, set to 'camera' if not found
162  pnh.param<std::string>("frame_id", frame_id_, "camera");
163  pnh.param<std::string>("second_frame_id", second_frame_id_, frame_id_); // Default to left frame_id per stereo API
164  // Set the timeout for grabbing images from the network
165  double timeout;
166  pnh.param("timeout", timeout, 1.0);
167 
168  // Try connecting to the camera
169  volatile bool connected = false;
170  while(!connected && ros::ok())
171  {
172  try
173  {
174  NODELET_INFO("Connecting to camera serial: %u", serial);
175  pg_.setDesiredCamera(serial);
176  NODELET_DEBUG("Actually connecting to camera.");
177  pg_.connect();
178  connected = true;
179  NODELET_DEBUG("Setting timeout to: %f.", timeout);
180  pg_.setTimeout(timeout);
181  }
182  catch(std::runtime_error& e)
183  {
184  NODELET_ERROR("%s", e.what());
185  ros::Duration(1.0).sleep(); // sleep for one second each time
186  }
187  }
188 
189  // Start up the dynamic_reconfigure service, note that this needs to stick around after this function ends
190  srv_ = boost::make_shared <dynamic_reconfigure::Server<pointgrey_camera_driver::PointGreyConfig> > (pnh);
191  dynamic_reconfigure::Server<pointgrey_camera_driver::PointGreyConfig>::CallbackType f = boost::bind(&pointgrey_camera_driver::PointGreyStereoCameraNodelet::paramCallback, this, _1, _2);
192  srv_->setCallback(f);
193 
194  // Start the camera info manager and attempt to load any configurations
195  std::stringstream cinfo_name;
196  cinfo_name << serial;
197  cinfo_.reset(new camera_info_manager::CameraInfoManager(lnh, cinfo_name.str(), camera_info_url));
198  rcinfo_.reset(new camera_info_manager::CameraInfoManager(rnh, cinfo_name.str(), second_info_url));
199  /*if (cinfo_->validateURL(camera_info_url)){ // Load CameraInfo (if any) from the URL. Will ROS_ERROR if bad.
200  cinfo_->loadCameraInfo(camera_info_url);
201  }*/
202  ci_.reset(new sensor_msgs::CameraInfo(cinfo_->getCameraInfo()));
203  ci_->header.frame_id = frame_id_;
204  rci_.reset(new sensor_msgs::CameraInfo(rcinfo_->getCameraInfo()));
205  rci_->header.frame_id = second_frame_id_;
206 
207  // Publish topics using ImageTransport through camera_info_manager (gives cool things like compression)
208  it_.reset(new image_transport::ImageTransport(lnh));
209  it_pub_ = it_->advertiseCamera("image_raw", 5);
210  rit_.reset(new image_transport::ImageTransport(rnh));
211  rit_pub_ = rit_->advertiseCamera("image_raw", 5);
212 
213  // Set up diagnostics
214  updater_.setHardwareID("pointgrey_camera " + serial);
215 
217  temp_pub_ = nh.advertise<std_msgs::Float64>("temp", 5);
218 
219  // Subscribe to gain and white balance changes
221 
222  volatile bool started = false;
223  while(!started)
224  {
225  try
226  {
227  NODELET_DEBUG("Starting camera capture.");
228  pg_.start();
229  started = true;
230  // Start the thread to loop through and publish messages
232  }
233  catch(std::runtime_error& e)
234  {
235  NODELET_ERROR("%s", e.what());
236  ros::Duration(1.0).sleep(); // sleep for one second each time
237  }
238  }
239  }
240 
246  void cleanUp()
247  {
248  try
249  {
250  NODELET_DEBUG("Stopping camera capture.");
251  pg_.stop();
252  NODELET_DEBUG("Disconnecting from camera.");
253  pg_.disconnect();
254  }
255  catch(std::runtime_error& e)
256  {
257  NODELET_ERROR("%s", e.what());
258  }
259  }
260 
266  void devicePoll()
267  {
268  while(!boost::this_thread::interruption_requested()) // Block until we need to stop this thread.
269  {
270  try
271  {
272  sensor_msgs::ImagePtr image(new sensor_msgs::Image);
273  sensor_msgs::ImagePtr second_image(new sensor_msgs::Image);
274  pg_.grabStereoImage(*image, frame_id_, *second_image, second_frame_id_);
275 
276  ros::Time time = ros::Time::now();
277  image->header.stamp = time;
278  second_image->header.stamp = time;
279  ci_->header.stamp = time;
280  rci_->header.stamp = time;
281  ci_->binning_x = binning_x_;
282  rci_->binning_x = binning_x_;
283  ci_->binning_y = binning_y_;
284  rci_->binning_y = binning_y_;
285  ci_->roi.x_offset = roi_x_offset_;
286  rci_->roi.x_offset = roi_x_offset_;
287  ci_->roi.y_offset = roi_y_offset_;
288  rci_->roi.y_offset = roi_y_offset_;
289  ci_->roi.height = roi_height_;
290  rci_->roi.height = roi_height_;
291  ci_->roi.width = roi_width_;
292  rci_->roi.width = roi_width_;
293  ci_->roi.do_rectify = do_rectify_;
294  rci_->roi.do_rectify = do_rectify_;
295 
296  it_pub_.publish(image, ci_);
297  rit_pub_.publish(second_image, rci_);
298  std_msgs::Float64 temp;
299  temp.data = pg_.getCameraTemperature();
300  temp_pub_.publish(temp);
301  }
302  catch(CameraTimeoutException& e)
303  {
304  NODELET_WARN("%s", e.what());
305  }
306  catch(std::runtime_error& e)
307  {
308  NODELET_ERROR("%s", e.what());
309  try
310  {
311  // Something terrible has happened, so let's just disconnect and reconnect to see if we can recover.
312  pg_.disconnect();
313  ros::Duration(1.0).sleep(); // sleep for one second each time
314  pg_.connect();
315  pg_.start();
316  }
317  catch(std::runtime_error& e2)
318  {
319  NODELET_ERROR("%s", e2.what());
320  }
321  }
322  // Update diagnostics
323  updater_.update();
324  }
325  }
326 
327  void gainWBCallback(const image_exposure_msgs::ExposureSequence &msg)
328  {
329  try
330  {
331  NODELET_DEBUG("Gain callback: Setting gain to %f and white balances to %u, %u", msg.gain, msg.white_balance_blue, msg.white_balance_red);
332  gain_ = msg.gain;
333  pg_.setGain(gain_);
334  wb_blue_ = msg.white_balance_blue;
335  wb_red_ = msg.white_balance_red;
337  }
338  catch(std::runtime_error& e)
339  {
340  NODELET_ERROR("gainWBCallback failed with error: %s", e.what());
341  }
342  }
343 
350 
352  double min_freq_;
353  double max_freq_;
354 
356  sensor_msgs::CameraInfoPtr ci_;
357  std::string frame_id_;
359 
360  double gain_;
361  uint16_t wb_blue_;
362  uint16_t wb_red_;
363 
364  // For stereo cameras
365  std::string second_frame_id_;
369  sensor_msgs::CameraInfoPtr rci_;
370 
371  // Parameters for cameraInfo
372  size_t binning_x_;
373  size_t binning_y_;
374  size_t roi_x_offset_;
375  size_t roi_y_offset_;
376  size_t roi_height_;
377  size_t roi_width_;
378  bool do_rectify_;
379 };
380 
382 }
std::string second_frame_id_
Frame id used for the second camera.
boost::shared_ptr< boost::thread > pubThread_
The thread that reads and publishes the images.
float getCameraTemperature()
Gets the current operating temperature.
void publish(const sensor_msgs::Image &image, const sensor_msgs::CameraInfo &info) const
#define NODELET_ERROR(...)
#define NODELET_WARN(...)
void setDesiredCamera(const uint32_t &id)
Used to set the serial number for the camera you wish to connect to.
void publish(const boost::shared_ptr< M > &message) const
void setBRWhiteBalance(bool auto_white_balance, uint16_t &blue, uint16_t &red)
boost::shared_ptr< camera_info_manager::CameraInfoManager > cinfo_
Needed to initialize and keep the CameraInfoManager in scope.
f
size_t binning_y_
Camera Info pixel binning along the image y axis.
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void gainWBCallback(const image_exposure_msgs::ExposureSequence &msg)
ros::Subscriber sub_
Subscriber for gain and white balance changes.
Interface to Point Grey cameras.
void setHardwareID(const std::string &hwid)
bool sleep() const
void devicePoll()
Function for the boost::thread to grabImages and publish them.
ros::NodeHandle & getMTNodeHandle() const
void setGain(double &gain)
sensor_msgs::CameraInfoPtr rci_
Camera Info message.
bool do_rectify_
Whether or not to rectify as if part of an image. Set to false if whole image, and true if in ROI mod...
bool stop()
Stops the camera loading data into its buffer.
ros::Publisher temp_pub_
Publisher for current camera temperature.
ros::NodeHandle & getMTPrivateNodeHandle() const
void setTimeout(const double &timeout)
Will set grabImage timeout for the camera.
PLUGINLIB_EXPORT_CLASS(image_proc::CropNonZeroNodelet, nodelet::Nodelet)
void connect()
Function that connects to a specified camera.
PointGreyCamera pg_
Instance of the PointGreyCamera library, used to interface with the hardware.
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ROSCPP_DECL bool ok()
void cleanUp()
Cleans up the memory and disconnects the camera.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
image_transport::CameraPublisher rit_pub_
CameraInfoManager ROS publisher.
std::string frame_id_
Frame id for the camera messages, defaults to &#39;camera&#39;.
void start()
Starts the camera loading data into its buffer.
#define NODELET_INFO(...)
void paramCallback(pointgrey_camera_driver::PointGreyConfig &config, uint32_t level)
Function that allows reconfiguration of the camera.
boost::shared_ptr< camera_info_manager::CameraInfoManager > rcinfo_
Needed to initialize and keep the CameraInfoManager in scope.
boost::shared_ptr< image_transport::ImageTransport > it_
Needed to initialize and keep the ImageTransport in scope.
static Time now()
sensor_msgs::CameraInfoPtr ci_
Camera Info message.
image_transport::CameraPublisher it_pub_
CameraInfoManager ROS publisher.
void disconnect()
Disconnects from the camera.
#define NODELET_DEBUG(...)
void onInit()
Serves as a psuedo constructor for nodelets.
boost::shared_ptr< dynamic_reconfigure::Server< pointgrey_camera_driver::PointGreyConfig > > srv_
Needed to initialize and keep the dynamic_reconfigure::Server in scope.
diagnostic_updater::Updater updater_
Handles publishing diagnostics messages.
void grabStereoImage(sensor_msgs::Image &image, const std::string &frame_id, sensor_msgs::Image &second_image, const std::string &second_frame_id)
bool setNewConfiguration(pointgrey_camera_driver::PointGreyConfig &config, const uint32_t &level)
Function that allows reconfiguration of the camera.
boost::shared_ptr< image_transport::ImageTransport > rit_
Needed to initialize and keep the ImageTransport in scope.
size_t binning_x_
Camera Info pixel binning along the image x axis.


pointgrey_camera_driver
Author(s): Chad Rockey
autogenerated on Fri May 8 2020 03:10:28