40 #include <sensor_msgs/CameraInfo.h> 42 #include <wfov_camera_msgs/WFOVImage.h> 43 #include <image_exposure_msgs/ExposureSequence.h> 48 #include <boost/thread.hpp> 50 #include <dynamic_reconfigure/server.h> 78 catch(std::runtime_error& e)
93 void paramCallback(pointgrey_camera_driver::PointGreyConfig &config, uint32_t level)
99 NODELET_DEBUG(
"Dynamic reconfigure callback with level: %d", level);
104 wb_blue_ = config.white_balance_blue;
105 wb_red_ = config.white_balance_red;
129 if(config.video_mode ==
"format7_mode0" || config.video_mode ==
"format7_mode1" || config.video_mode ==
"format7_mode2")
147 catch(std::runtime_error& e)
149 NODELET_ERROR(
"Reconfigure Callback failed with error: %s", e.what());
180 catch(std::runtime_error& e)
190 catch(std::runtime_error& e)
222 pnh.
getParam(
"serial", serial_xmlrpc);
225 pnh.
param<
int>(
"serial", serial, 0);
229 std::string serial_str;
230 pnh.
param<std::string>(
"serial", serial_str,
"0");
231 std::istringstream(serial_str) >> serial;
239 std::string camera_serial_path;
240 pnh.
param<std::string>(
"camera_serial_path", camera_serial_path,
"");
241 NODELET_INFO(
"Camera serial path %s", camera_serial_path.c_str());
244 while (serial == 0 && !camera_serial_path.empty())
249 NODELET_WARN(
"Waiting for camera serial path to become available");
267 std::string camera_info_url;
268 pnh.
param<std::string>(
"camera_info_url", camera_info_url,
"");
275 srv_ = boost::make_shared <dynamic_reconfigure::Server<pointgrey_camera_driver::PointGreyConfig> > (pnh);
276 dynamic_reconfigure::Server<pointgrey_camera_driver::PointGreyConfig>::CallbackType
f =
278 srv_->setCallback(f);
281 std::stringstream cinfo_name;
282 cinfo_name << serial;
288 it_pub_ =
it_->advertiseCamera(
"image_raw", 5, cb, cb);
295 pnh.
param<
double>(
"desired_freq", desired_freq, 7.0);
298 double freq_tolerance;
299 pnh.
param<
double>(
"freq_tolerance", freq_tolerance, 0.1);
301 pnh.
param<
int>(
"window_size", window_size, 100);
302 double min_acceptable;
303 pnh.
param<
double>(
"min_acceptable_delay", min_acceptable, 0.0);
304 double max_acceptable;
305 pnh.
param<
double>(
"max_acceptable_delay", max_acceptable, 0.2);
322 NODELET_DEBUG(
"Reading camera serial file from: %s", camera_serial_path.c_str());
324 std::ifstream serial_file(camera_serial_path.c_str());
325 std::stringstream buffer;
328 if (serial_file.is_open())
330 std::string serial_str((std::istreambuf_iterator<char>(serial_file)), std::istreambuf_iterator<char>());
331 NODELET_DEBUG(
"Serial file contents: %s", serial_str.c_str());
332 buffer << std::hex << serial_str;
339 NODELET_WARN(
"Unable to open serial path: %s", camera_serial_path.c_str());
361 State state = DISCONNECTED;
362 State previous_state =
NONE;
364 while(!boost::this_thread::interruption_requested())
366 bool state_changed = state != previous_state;
368 previous_state = state;
390 catch(std::runtime_error& e)
393 "Failed to stop error: %s", e.what());
407 state = DISCONNECTED;
409 catch(std::runtime_error& e)
412 "Failed to disconnect with error: %s", e.what());
437 catch(std::runtime_error& e)
450 catch(std::runtime_error& e)
453 "Failed to connect with error: %s", e.what());
465 NODELET_INFO(
"Attention: if nothing subscribes to the camera topic, the camera_info is not published on the correspondent topic.");
468 catch(std::runtime_error& e)
471 "Failed to start with error: %s", e.what());
479 wfov_camera_msgs::WFOVImagePtr wfov_image(
new wfov_camera_msgs::WFOVImage);
487 wfov_image->gain =
gain_;
488 wfov_image->white_balance_blue =
wb_blue_;
489 wfov_image->white_balance_red =
wb_red_;
494 wfov_image->header.stamp = time;
495 wfov_image->image.header.stamp = time;
498 ci_.reset(
new sensor_msgs::CameraInfo(
cinfo_->getCameraInfo()));
499 ci_->header.stamp = wfov_image->image.header.stamp;
500 ci_->header.frame_id = wfov_image->header.frame_id;
510 wfov_image->info = *
ci_;
513 pub_->publish(wfov_image);
518 sensor_msgs::ImagePtr image(
new sensor_msgs::Image(wfov_image->image));
530 catch(std::runtime_error& e)
552 NODELET_DEBUG(
"Gain callback: Setting gain to %f and white balances to %u, %u", msg.gain, msg.white_balance_blue, msg.white_balance_red);
556 wb_red_ = msg.white_balance_red;
559 catch(std::runtime_error& e)
561 NODELET_ERROR(
"gainWBCallback failed with error: %s", e.what());
579 sensor_msgs::CameraInfoPtr
ci_;
605 pointgrey_camera_driver::PointGreyConfig
config_;
static const uint8_t LEVEL_RECONFIGURE_STOP
bool auto_packet_size_
If true, GigE packet size is automatically determined, otherwise packet_size_ is used: ...
float getCameraTemperature()
Gets the current operating temperature.
void publish(const sensor_msgs::Image &image, const sensor_msgs::CameraInfo &info) const
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
#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 setBRWhiteBalance(bool auto_white_balance, uint16_t &blue, uint16_t &red)
boost::mutex connect_mutex_
~PointGreyCameraNodelet()
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
uint32_t getNumSubscribers() const
Interface to Point Grey cameras.
void setHardwareID(const std::string &hwid)
ros::Subscriber sub_
Subscriber for gain and white balance changes.
size_t roi_height_
Camera Info ROI height.
void devicePoll()
Function for the boost::thread to grabImages and publish them.
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
ros::NodeHandle & getMTNodeHandle() const
int packet_delay_
GigE packet delay:
size_t roi_width_
Camera Info ROI width.
#define NODELET_ERROR_COND(cond,...)
image_transport::CameraPublisher it_pub_
CameraInfoManager ROS publisher.
Type const & getType() const
void setGain(double &gain)
void gainWBCallback(const image_exposure_msgs::ExposureSequence &msg)
boost::shared_ptr< boost::thread > pubThread_
The thread that reads and publishes the images.
bool stop()
Stops the camera loading data into its buffer.
size_t binning_y_
Camera Info pixel binning along the image y axis.
ros::NodeHandle & getMTPrivateNodeHandle() const
void setTimeout(const double &timeout)
Will set grabImage timeout for the camera.
int packet_size_
GigE packet size:
PLUGINLIB_EXPORT_CLASS(image_proc::CropNonZeroNodelet, nodelet::Nodelet)
void connect()
Function that connects to a specified camera.
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
void paramCallback(pointgrey_camera_driver::PointGreyConfig &config, uint32_t level)
Function that allows reconfiguration of the camera.
boost::shared_ptr< camera_info_manager::CameraInfoManager > cinfo_
Needed to initialize and keep the CameraInfoManager in scope.
void connectCb()
Connection callback to only do work when someone is listening.
PointGreyCamera pg_
Instance of the PointGreyCamera library, used to interface with the hardware.
void setGigEParameters(bool auto_packet_size, unsigned int packet_size, unsigned int packet_delay)
Set parameters relative to GigE cameras.
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
sensor_msgs::CameraInfoPtr ci_
Camera Info message.
boost::shared_ptr< diagnostic_updater::DiagnosedPublisher< wfov_camera_msgs::WFOVImage > > pub_
Diagnosed publisher, has to be a pointer because of constructor requirements.
pointgrey_camera_driver::PointGreyConfig config_
Configuration:
void grabImage(sensor_msgs::Image &image, const std::string &frame_id)
Loads the raw data from the cameras buffer.
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...
diagnostic_updater::Updater updater_
Handles publishing diagnostics messages.
size_t roi_x_offset_
Camera Info ROI x offset.
void start()
Starts the camera loading data into its buffer.
#define NODELET_INFO(...)
size_t roi_y_offset_
Camera Info ROI y offset.
bool getParam(const std::string &key, std::string &s) const
boost::shared_ptr< image_transport::ImageTransport > it_
Needed to initialize and keep the ImageTransport in scope.
int readSerialAsHexFromFile(std::string camera_serial_path)
Reads in the camera serial from a specified file path. The format of the serial is expected to be bas...
size_t binning_x_
Camera Info pixel binning along the image x axis.
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.
std::string frame_id_
Frame id for the camera messages, defaults to 'camera'.
bool setNewConfiguration(pointgrey_camera_driver::PointGreyConfig &config, const uint32_t &level)
Function that allows reconfiguration of the camera.