59 #include <sensor_msgs/CameraInfo.h> 61 #include <wfov_camera_msgs/WFOVImage.h> 62 #include <image_exposure_msgs/ExposureSequence.h> 67 #include <boost/thread.hpp> 69 #include <dynamic_reconfigure/server.h> 105 catch (
const std::runtime_error& e)
123 void paramCallback(
const spinnaker_camera_driver::SpinnakerConfig& config, uint32_t level)
134 wb_blue_ = config.white_balance_blue_ratio;
135 wb_red_ = config.white_balance_red_ratio;
138 binning_x_ = config.image_format_x_binning * config.image_format_x_decimation;
139 binning_y_ = config.image_format_y_binning * config.image_format_y_decimation;
145 if ((config.image_format_roi_width + config.image_format_roi_height) > 0 &&
165 catch (std::runtime_error& e)
167 NODELET_ERROR(
"Reconfigure Callback failed with error: %s", e.what());
263 pnh.
getParam(
"serial", serial_xmlrpc);
266 pnh.
param<
int>(
"serial", serial, 0);
270 std::string serial_str;
271 pnh.
param<std::string>(
"serial", serial_str,
"0");
272 std::istringstream(serial_str) >> serial;
280 std::string camera_serial_path;
281 pnh.
param<std::string>(
"camera_serial_path", camera_serial_path,
"");
285 while (serial == 0 && !camera_serial_path.empty())
308 std::string camera_info_url;
309 pnh.
param<std::string>(
"camera_info_url", camera_info_url,
"");
316 srv_ = std::make_shared<dynamic_reconfigure::Server<spinnaker_camera_driver::SpinnakerConfig> >(pnh);
317 dynamic_reconfigure::Server<spinnaker_camera_driver::SpinnakerConfig>::CallbackType
f =
320 srv_->setCallback(f);
324 pnh.
param<
int>(
"queue_size", queue_size, 5);
327 std::stringstream cinfo_name;
328 cinfo_name << serial;
334 it_pub_ =
it_->advertiseCamera(
"image_raw", queue_size, cb, cb);
341 pnh.
param<
double>(
"desired_freq", desired_freq, 30.0);
344 double freq_tolerance;
346 pnh.
param<
double>(
"freq_tolerance", freq_tolerance, 0.1);
348 pnh.
param<
int>(
"window_size", window_size, 100);
349 double min_acceptable;
351 pnh.
param<
double>(
"min_acceptable_delay", min_acceptable, 0.0);
352 double max_acceptable;
353 pnh.
param<
double>(
"max_acceptable_delay", max_acceptable, 0.2);
357 nh.
advertise<wfov_camera_msgs::WFOVImage>(
"image", queue_size, cb2, cb2),
359 &min_freq_, &max_freq_, freq_tolerance, window_size),
368 "/diagnostics", 1, diag_cb, diag_cb)));
372 diag_man->addDiagnostic(
"DeviceTemperature",
true, std::make_pair(0.0f, 90.0f), -10.0f, 95.0f);
373 diag_man->addDiagnostic(
"AcquisitionResultingFrameRate",
true, std::make_pair(10.0f, 60.0f), 5.0f, 90.0f);
374 diag_man->addDiagnostic(
"PowerSupplyVoltage",
true, std::make_pair(4.5f, 5.2f), 4.4f, 5.3f);
375 diag_man->addDiagnostic(
"PowerSupplyCurrent",
true, std::make_pair(0.4f, 0.6f), 0.3f, 1.0f);
376 diag_man->addDiagnostic<
int>(
"DeviceUptime");
377 diag_man->addDiagnostic<
int>(
"U3VMessageChannelID");
389 NODELET_DEBUG_ONCE(
"Reading camera serial file from: %s", camera_serial_path.c_str());
391 std::ifstream serial_file(camera_serial_path.c_str());
392 std::stringstream buffer;
395 if (serial_file.is_open())
397 std::string serial_str((std::istreambuf_iterator<char>(serial_file)), std::istreambuf_iterator<char>());
399 buffer << std::hex << serial_str;
412 while (!boost::this_thread::interruption_requested())
440 State state = DISCONNECTED;
441 State previous_state =
NONE;
443 while (!boost::this_thread::interruption_requested())
445 bool state_changed = state != previous_state;
447 previous_state = state;
469 catch (std::runtime_error& e)
489 state = DISCONNECTED;
491 catch (std::runtime_error& e)
495 NODELET_ERROR(
"Failed to disconnect with error: %s", e.what());
524 catch (
const std::runtime_error& e)
539 catch (
const std::runtime_error& e)
557 NODELET_DEBUG(
"Attention: if nothing subscribes to the camera topic, the camera_info is not published " 558 "on the correspondent topic.");
561 catch (std::runtime_error& e)
575 wfov_camera_msgs::WFOVImagePtr wfov_image(
new wfov_camera_msgs::WFOVImage);
583 wfov_image->gain =
gain_;
584 wfov_image->white_balance_blue =
wb_blue_;
585 wfov_image->white_balance_red =
wb_red_;
590 wfov_image->header.stamp = time;
591 wfov_image->image.header.stamp = time;
594 ci_.reset(
new sensor_msgs::CameraInfo(
cinfo_->getCameraInfo()));
595 ci_->header.stamp = wfov_image->image.header.stamp;
596 ci_->header.frame_id = wfov_image->header.frame_id;
606 wfov_image->info = *
ci_;
609 pub_->publish(wfov_image);
614 sensor_msgs::ImagePtr image(
new sensor_msgs::Image(wfov_image->image));
623 catch (std::runtime_error& e)
644 NODELET_DEBUG_ONCE(
"Gain callback: Setting gain to %f and white balances to %u, %u", msg.gain,
645 msg.white_balance_blue, msg.white_balance_red);
650 wb_red_ = msg.white_balance_red;
655 catch (std::runtime_error& e)
657 NODELET_ERROR(
"gainWBCallback failed with error: %s", e.what());
662 std::shared_ptr<dynamic_reconfigure::Server<spinnaker_camera_driver::SpinnakerConfig> >
srv_;
663 std::shared_ptr<image_transport::ImageTransport>
it_;
668 std::shared_ptr<camera_info_manager::CameraInfoManager>
cinfo_;
672 std::shared_ptr<diagnostic_updater::DiagnosedPublisher<wfov_camera_msgs::WFOVImage> >
pub_;
687 sensor_msgs::CameraInfoPtr
ci_;
717 spinnaker_camera_driver::SpinnakerConfig
config_;
std::unique_ptr< DiagnosticsManager > diag_man
void setGain(const float &gain)
void grabImage(sensor_msgs::Image *image, const std::string &frame_id)
Loads the raw data from the cameras buffer.
void disconnect()
Disconnects from the camera.
void publish(const sensor_msgs::Image &image, const sensor_msgs::CameraInfo &info) const
void connect()
Function that connects to a specified camera.
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
#define NODELET_ERROR(...)
#define NODELET_WARN(...)
size_t roi_x_offset_
Camera Info ROI x offset.
#define ROS_INFO_ONCE(...)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
std::shared_ptr< image_transport::ImageTransport > it_
size_t binning_x_
Camera Info pixel binning along the image x axis.
uint32_t getNumSubscribers() const
void setHardwareID(const std::string &hwid)
static const uint8_t LEVEL_RECONFIGURE_STOP
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
ros::NodeHandle & getMTNodeHandle() const
size_t roi_width_
Camera Info ROI width.
std::mutex connect_mutex_
Type const & getType() const
sensor_msgs::CameraInfoPtr ci_
Camera Info message.
std::shared_ptr< camera_info_manager::CameraInfoManager > cinfo_
diagnostic_updater::Updater updater_
Handles publishing diagnostics messages.
int packet_delay_
GigE packet delay:
std::shared_ptr< ros::Publisher > diagnostics_pub_
ros::NodeHandle & getMTPrivateNodeHandle() const
ros::Subscriber sub_
Subscriber for gain and white balance changes.
void gainWBCallback(const image_exposure_msgs::ExposureSequence &msg)
std::shared_ptr< dynamic_reconfigure::Server< spinnaker_camera_driver::SpinnakerConfig > > srv_
size_t roi_height_
Camera Info ROI height.
bool auto_packet_size_
If true, GigE packet size is automatically determined, otherwise packet_size_ is used: ...
void connectCb()
Connection callback to only do work when someone is listening.
void start()
Starts the camera loading data into its buffer.
PLUGINLIB_EXPORT_CLASS(image_proc::CropNonZeroNodelet, nodelet::Nodelet)
void onInit()
Serves as a psuedo constructor for nodelets.
Class to support ROS Diagnostics Aggregator for Spinnaker Camera.
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
std::shared_ptr< boost::thread > pubThread_
The thread that reads and publishes the images.
void setTimeout(const double &timeout)
Will set grabImage timeout for the camera.
spinnaker_camera_driver::SpinnakerConfig config_
Configuration:
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
#define NODELET_WARN_ONCE(...)
void setDesiredCamera(const uint32_t &id)
Used to set the serial number for the camera you wish to connect to.
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...
#define NODELET_DEBUG_ONCE(...)
size_t binning_y_
Camera Info pixel binning along the image y axis.
std::shared_ptr< boost::thread > diagThread_
The thread that reads and publishes the diagnostics.
size_t roi_y_offset_
Camera Info ROI y offset.
bool getParam(const std::string &key, std::string &s) const
void paramCallback(const spinnaker_camera_driver::SpinnakerConfig &config, uint32_t level)
Function that allows reconfiguration of the camera.
int packet_size_
GigE packet size:
void devicePoll()
Function for the boost::thread to grabImages and publish them.
Interface to Point Grey cameras.
std::shared_ptr< diagnostic_updater::DiagnosedPublisher< wfov_camera_msgs::WFOVImage > > pub_
Diagnosed.
void stop()
Stops the camera loading data into its buffer.
#define NODELET_DEBUG(...)
image_transport::CameraPublisher it_pub_
CameraInfoManager ROS publisher.
SpinnakerCamera spinnaker_
Instance of the SpinnakerCamera library, used to interface with the hardware.
~SpinnakerCameraNodelet()
void setNewConfiguration(const spinnaker_camera_driver::SpinnakerConfig &config, const uint32_t &level)
Function that allows reconfiguration of the camera.
std::string frame_id_
Frame id for the camera messages, defaults to 'camera'.