nodelet.cpp

Go to the documentation of this file.

/*
This code was developed by the National Robotics Engineering Center (NREC), part of the Robotics Institute at Carnegie
Mellon University.
Its development was funded by DARPA under the LS3 program and submitted for public release on June 7th, 2012.
Release was granted on August, 21st 2012 with Distribution Statement "A" (Approved for Public Release, Distribution
Unlimited).

This software is released under a BSD license:

Copyright (c) 2012, Carnegie Mellon University. All rights reserved.
Copyright (c) 2018, Clearpath Robotics, Inc., All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are permitted provided that the
following conditions are met:

Redistributions of source code must retain the above copyright notice, this list of conditions and the following
disclaimer.
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.
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.
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.
*/

// ROS and associated nodelet interface and PLUGINLIB declaration header
#include "ros/ros.h"
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>

#include "spinnaker_camera_driver/SpinnakerCamera.h"  // The actual standalone library for the Spinnakers
#include "spinnaker_camera_driver/diagnostics.h"

#include <image_transport/image_transport.h>          // ROS library that allows sending compressed images
#include <camera_info_manager/camera_info_manager.h>  // ROS library that publishes CameraInfo topics
#include <sensor_msgs/CameraInfo.h>                   // ROS message header for CameraInfo

#include <wfov_camera_msgs/WFOVImage.h>
#include <image_exposure_msgs/ExposureSequence.h>  // Message type for configuring gain and white balance.

#include <diagnostic_updater/diagnostic_updater.h>  // Headers for publishing diagnostic messages.
#include <diagnostic_updater/publisher.h>

#include <boost/thread.hpp>  // Needed for the nodelet to launch the reading thread.

#include <dynamic_reconfigure/server.h>  // Needed for the dynamic_reconfigure gui service to run

#include <fstream>
#include <string>

namespace spinnaker_camera_driver
{
class SpinnakerCameraNodelet : public nodelet::Nodelet
{
public:
  SpinnakerCameraNodelet()
  {
  }

  ~SpinnakerCameraNodelet()
  {
    std::lock_guard<std::mutex> scopedLock(connect_mutex_);

    if (diagThread_)
    {
      diagThread_->interrupt();
      diagThread_->join();
    }

    if (pubThread_)
    {
      pubThread_->interrupt();
      pubThread_->join();

      try
      {
        NODELET_DEBUG_ONCE("Stopping camera capture.");
        spinnaker_.stop();
        NODELET_DEBUG_ONCE("Disconnecting from camera.");
        spinnaker_.disconnect();
      }
      catch (const std::runtime_error& e)
      {
        NODELET_ERROR("%s", e.what());
      }
    }
  }

private:
  void paramCallback(const spinnaker_camera_driver::SpinnakerConfig& config, uint32_t level)
  {
    config_ = config;

    try
    {
      NODELET_DEBUG_ONCE("Dynamic reconfigure callback with level: %u", level);
      spinnaker_.setNewConfiguration(config, level);

      // Store needed parameters for the metadata message
      gain_ = config.gain;
      wb_blue_ = config.white_balance_blue_ratio;
      wb_red_ = config.white_balance_red_ratio;

      // No separate param in CameraInfo for binning/decimation
      binning_x_ = config.image_format_x_binning * config.image_format_x_decimation;
      binning_y_ = config.image_format_y_binning * config.image_format_y_decimation;

      // Store CameraInfo RegionOfInterest information
      // TODO(mhosmar): Not compliant with CameraInfo message: "A particular ROI always denotes the
      //                same window of pixels on the camera sensor, regardless of binning settings."
      //                These values are in the post binned frame.
      if ((config.image_format_roi_width + config.image_format_roi_height) > 0 &&
          (config.image_format_roi_width < spinnaker_.getWidthMax() ||
           config.image_format_roi_height < spinnaker_.getHeightMax()))
      {
        roi_x_offset_ = config.image_format_x_offset;
        roi_y_offset_ = config.image_format_y_offset;
        roi_width_ = config.image_format_roi_width;
        roi_height_ = config.image_format_roi_height;
        do_rectify_ = true;  // Set to true if an ROI is used.
      }
      else
      {
        // Zeros mean the full resolution was captured.
        roi_x_offset_ = 0;
        roi_y_offset_ = 0;
        roi_height_ = 0;
        roi_width_ = 0;
        do_rectify_ = false;  // Set to false if the whole image is captured.
      }
    }
    catch (std::runtime_error& e)
    {
      NODELET_ERROR("Reconfigure Callback failed with error: %s", e.what());
    }
  }

  void diagCb()
  {
    if (!diagThread_)  // We need to connect
    {
      // Start the thread to loop through and publish messages
      diagThread_.reset(
          new boost::thread(boost::bind(&spinnaker_camera_driver::SpinnakerCameraNodelet::diagPoll, this)));
    }
  }

  void connectCb()
  {
    if (!pubThread_)  // We need to connect
    {
      // Start the thread to loop through and publish messages
      pubThread_.reset(
          new boost::thread(boost::bind(&spinnaker_camera_driver::SpinnakerCameraNodelet::devicePoll, this)));
    }

    // @tthomas - removing subscriber check and logic below as it's leading to mutex locks and crashes currently
    /*
    NODELET_DEBUG_ONCE("Connect callback!");
    std::lock_guard<std::mutex> scopedLock(connect_mutex_); // Grab the mutex.  Wait until we're done initializing
    before letting this function through.
    // Check if we should disconnect (there are 0 subscribers to our data)
    if(it_pub_.getNumSubscribers() == 0 && pub_->getPublisher().getNumSubscribers() == 0)
    {
      if (pubThread_)
      {
        NODELET_DEBUG_ONCE("Disconnecting.");
        pubThread_->interrupt();
        scopedLock.unlock();
        pubThread_->join();
        scopedLock.lock();
        pubThread_.reset();
        sub_.shutdown();

        try
        {
          NODELET_DEBUG_ONCE("Stopping camera capture.");
          spinnaker_.stop();
        }
        catch(std::runtime_error& e)
        {
          NODELET_ERROR("%s", e.what());
        }

        try
        {
          NODELET_DEBUG_ONCE("Disconnecting from camera.");
          spinnaker_.disconnect();
        }
        catch(std::runtime_error& e)
        {
          NODELET_ERROR("%s", e.what());
        }
      }
    }
    else if(!pubThread_)     // We need to connect
    {
      // Start the thread to loop through and publish messages
      pubThread_.reset(new boost::thread(boost::bind(&spinnaker_camera_driver::SpinnakerCameraNodelet::devicePoll,
    this)));
    }
    else
    {
      NODELET_DEBUG_ONCE("Do nothing in callback.");
    }
    */
  }

  void onInit()
  {
    // Get nodeHandles
    ros::NodeHandle& nh = getMTNodeHandle();
    ros::NodeHandle& pnh = getMTPrivateNodeHandle();

    // Get a serial number through ros
    int serial = 0;

    XmlRpc::XmlRpcValue serial_xmlrpc;
    pnh.getParam("serial", serial_xmlrpc);
    if (serial_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeInt)
    {
      pnh.param<int>("serial", serial, 0);
    }
    else if (serial_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString)
    {
      std::string serial_str;
      pnh.param<std::string>("serial", serial_str, "0");
      std::istringstream(serial_str) >> serial;
    }
    else
    {
      NODELET_DEBUG_ONCE("Serial XMLRPC type.");
      serial = 0;
    }

    std::string camera_serial_path;
    pnh.param<std::string>("camera_serial_path", camera_serial_path, "");
    NODELET_DEBUG_ONCE("Camera serial path %s", camera_serial_path.c_str());
    // If serial has been provided directly as a param, ignore the path
    // to read in the serial from.
    while (serial == 0 && !camera_serial_path.empty())
    {
      serial = readSerialAsHexFromFile(camera_serial_path);
      if (serial == 0)
      {
        NODELET_WARN_ONCE("Waiting for camera serial path to become available");
        ros::Duration(1.0).sleep();  // Sleep for 1 second, wait for serial device path to become available
      }
    }

    NODELET_DEBUG_ONCE("Using camera serial %d", serial);

    spinnaker_.setDesiredCamera((uint32_t)serial);

    // Get GigE camera parameters:
    pnh.param<int>("packet_size", packet_size_, 1400);
    pnh.param<bool>("auto_packet_size", auto_packet_size_, true);
    pnh.param<int>("packet_delay", packet_delay_, 4000);

    // TODO(mhosmar):  Set GigE parameters:
    // spinnaker_.setGigEParameters(auto_packet_size_, packet_size_, packet_delay_);

    // Get the location of our camera config yaml
    std::string camera_info_url;
    pnh.param<std::string>("camera_info_url", camera_info_url, "");
    // Get the desired frame_id, set to 'camera' if not found
    pnh.param<std::string>("frame_id", frame_id_, "camera");
    // Do not call the connectCb function until after we are done initializing.
    std::lock_guard<std::mutex> scopedLock(connect_mutex_);

    // Start up the dynamic_reconfigure service, note that this needs to stick around after this function ends
    srv_ = std::make_shared<dynamic_reconfigure::Server<spinnaker_camera_driver::SpinnakerConfig> >(pnh);
    dynamic_reconfigure::Server<spinnaker_camera_driver::SpinnakerConfig>::CallbackType f =
        boost::bind(&spinnaker_camera_driver::SpinnakerCameraNodelet::paramCallback, this, _1, _2);

    srv_->setCallback(f);

    // queue size of ros publisher
    int queue_size;
    pnh.param<int>("queue_size", queue_size, 5);

    // Start the camera info manager and attempt to load any configurations
    std::stringstream cinfo_name;
    cinfo_name << serial;
    cinfo_.reset(new camera_info_manager::CameraInfoManager(nh, cinfo_name.str(), camera_info_url));

    // Publish topics using ImageTransport through camera_info_manager (gives cool things like compression)
    it_.reset(new image_transport::ImageTransport(nh));
    image_transport::SubscriberStatusCallback cb = boost::bind(&SpinnakerCameraNodelet::connectCb, this);
    it_pub_ = it_->advertiseCamera("image_raw", queue_size, cb, cb);

    // Set up diagnostics
    updater_.setHardwareID("spinnaker_camera " + cinfo_name.str());

    // Set up a diagnosed publisher
    double desired_freq;
    pnh.param<double>("desired_freq", desired_freq, 30.0);
    pnh.param<double>("min_freq", min_freq_, desired_freq);
    pnh.param<double>("max_freq", max_freq_, desired_freq);
    double freq_tolerance;  // Tolerance before stating error on publish frequency, fractional percent of desired
                            // frequencies.
    pnh.param<double>("freq_tolerance", freq_tolerance, 0.1);
    int window_size;  // Number of samples to consider in frequency
    pnh.param<int>("window_size", window_size, 100);
    double min_acceptable;  // The minimum publishing delay (in seconds) before warning.  Negative values mean future
                            // dated messages.
    pnh.param<double>("min_acceptable_delay", min_acceptable, 0.0);
    double max_acceptable;  // The maximum publishing delay (in seconds) before warning.
    pnh.param<double>("max_acceptable_delay", max_acceptable, 0.2);
    ros::SubscriberStatusCallback cb2 = boost::bind(&SpinnakerCameraNodelet::connectCb, this);
    pub_.reset(
        new diagnostic_updater::DiagnosedPublisher<wfov_camera_msgs::WFOVImage>(
            nh.advertise<wfov_camera_msgs::WFOVImage>("image", queue_size, cb2, cb2),
            updater_, diagnostic_updater::FrequencyStatusParam(
                          &min_freq_, &max_freq_, freq_tolerance, window_size),
            diagnostic_updater::TimeStampStatusParam(min_acceptable,
                                                     max_acceptable)));

    // Set up diagnostics aggregator publisher and diagnostics manager
    ros::SubscriberStatusCallback diag_cb =
        boost::bind(&SpinnakerCameraNodelet::diagCb, this);
    diagnostics_pub_.reset(
        new ros::Publisher(nh.advertise<diagnostic_msgs::DiagnosticArray>(
            "/diagnostics", 1, diag_cb, diag_cb)));

    diag_man = std::unique_ptr<DiagnosticsManager>(new DiagnosticsManager(
        frame_id_, std::to_string(spinnaker_.getSerial()), diagnostics_pub_));
    diag_man->addDiagnostic("DeviceTemperature", true, std::make_pair(0.0f, 90.0f), -10.0f, 95.0f);
    diag_man->addDiagnostic("AcquisitionResultingFrameRate", true, std::make_pair(10.0f, 60.0f), 5.0f, 90.0f);
    diag_man->addDiagnostic("PowerSupplyVoltage", true, std::make_pair(4.5f, 5.2f), 4.4f, 5.3f);
    diag_man->addDiagnostic("PowerSupplyCurrent", true, std::make_pair(0.4f, 0.6f), 0.3f, 1.0f);
    diag_man->addDiagnostic<int>("DeviceUptime");
    diag_man->addDiagnostic<int>("U3VMessageChannelID");
  }

  int readSerialAsHexFromFile(std::string camera_serial_path)
  {
    NODELET_DEBUG_ONCE("Reading camera serial file from: %s", camera_serial_path.c_str());

    std::ifstream serial_file(camera_serial_path.c_str());
    std::stringstream buffer;
    int serial = 0;

    if (serial_file.is_open())
    {
      std::string serial_str((std::istreambuf_iterator<char>(serial_file)), std::istreambuf_iterator<char>());
      NODELET_DEBUG_ONCE("Serial file contents: %s", serial_str.c_str());
      buffer << std::hex << serial_str;
      buffer >> serial;
      NODELET_DEBUG_ONCE("Serial discovered %d", serial);

      return serial;
    }

    NODELET_WARN_ONCE("Unable to open serial path: %s", camera_serial_path.c_str());
    return 0;
  }

  void diagPoll()
  {
    while (!boost::this_thread::interruption_requested())  // Block until we need
                                                           // to stop this
                                                           // thread.
    {
      diag_man->processDiagnostics(&spinnaker_);
    }
  }

  void devicePoll()
  {
    ROS_INFO_ONCE("devicePoll");

    enum State
    {
      NONE,
      ERROR,
      STOPPED,
      DISCONNECTED,
      CONNECTED,
      STARTED
    };

    State state = DISCONNECTED;
    State previous_state = NONE;

    while (!boost::this_thread::interruption_requested())  // Block until we need to stop this thread.
    {
      bool state_changed = state != previous_state;

      previous_state = state;

      switch (state)
      {
        case ERROR:
// Generally there's no need to stop before disconnecting after an
// error. Indeed, stop will usually fail.
#if STOP_ON_ERROR
          // Try stopping the camera
          {
            std::lock_guard<std::mutex> scopedLock(connect_mutex_);
            sub_.shutdown();
          }

          try
          {
            NODELET_DEBUG_ONCE("Stopping camera.");
            spinnaker_.stop();
            NODELET_DEBUG_ONCE("Stopped camera.");

            state = STOPPED;
          }
          catch (std::runtime_error& e)
          {
            if (state_changed)
            {
              NODELET_ERROR("Failed to stop with error: %s", e.what());
              ros::Duration(1.0).sleep();  // sleep for one second each time
            }
            state = ERROR;
          }

          break;
#endif
        case STOPPED:
          // Try disconnecting from the camera
          try
          {
            NODELET_DEBUG("Disconnecting from camera.");
            spinnaker_.disconnect();
            NODELET_DEBUG("Disconnected from camera.");

            state = DISCONNECTED;
          }
          catch (std::runtime_error& e)
          {
            if (state_changed)
            {
              NODELET_ERROR("Failed to disconnect with error: %s", e.what());
              ros::Duration(1.0).sleep();  // sleep for one second each time
            }
            state = ERROR;
          }

          break;
        case DISCONNECTED:
          // Try connecting to the camera
          try
          {
            NODELET_DEBUG("Connecting to camera.");

            spinnaker_.connect();

            NODELET_DEBUG("Connected to camera.");

            // Set last configuration, forcing the reconfigure level to stop
            spinnaker_.setNewConfiguration(config_, SpinnakerCamera::LEVEL_RECONFIGURE_STOP);

            // Set the timeout for grabbing images.
            try
            {
              double timeout;
              getMTPrivateNodeHandle().param("timeout", timeout, 1.0);

              NODELET_DEBUG_ONCE("Setting timeout to: %f.", timeout);
              spinnaker_.setTimeout(timeout);
            }
            catch (const std::runtime_error& e)
            {
              NODELET_ERROR("%s", e.what());
            }

            // Subscribe to gain and white balance changes
            {
              std::lock_guard<std::mutex> scopedLock(connect_mutex_);
              sub_ =
                  getMTNodeHandle().subscribe("image_exposure_sequence", 10,
                                              &spinnaker_camera_driver::SpinnakerCameraNodelet::gainWBCallback, this);
            }

            state = CONNECTED;
          }
          catch (const std::runtime_error& e)
          {
            if (state_changed)
            {
              NODELET_ERROR("Failed to connect with error: %s", e.what());
              ros::Duration(1.0).sleep();  // sleep for one second each time
            }
            state = ERROR;
          }

          break;
        case CONNECTED:
          // Try starting the camera
          try
          {
            NODELET_DEBUG("Starting camera.");
            spinnaker_.start();
            NODELET_DEBUG("Started camera.");
            NODELET_DEBUG("Attention: if nothing subscribes to the camera topic, the camera_info is not published "
                          "on the correspondent topic.");
            state = STARTED;
          }
          catch (std::runtime_error& e)
          {
            if (state_changed)
            {
              NODELET_ERROR("Failed to start with error: %s", e.what());
              ros::Duration(1.0).sleep();  // sleep for one second each time
            }
            state = ERROR;
          }

          break;
        case STARTED:
          try
          {
            wfov_camera_msgs::WFOVImagePtr wfov_image(new wfov_camera_msgs::WFOVImage);
            // Get the image from the camera library
            NODELET_DEBUG_ONCE("Starting a new grab from camera with serial {%d}.", spinnaker_.getSerial());
            spinnaker_.grabImage(&wfov_image->image, frame_id_);

            // Set other values
            wfov_image->header.frame_id = frame_id_;

            wfov_image->gain = gain_;
            wfov_image->white_balance_blue = wb_blue_;
            wfov_image->white_balance_red = wb_red_;

            // wfov_image->temperature = spinnaker_.getCameraTemperature();

            ros::Time time = ros::Time::now() + ros::Duration(config_.time_offset);
            wfov_image->header.stamp = time;
            wfov_image->image.header.stamp = time;

            // Set the CameraInfo message
            ci_.reset(new sensor_msgs::CameraInfo(cinfo_->getCameraInfo()));
            ci_->header.stamp = wfov_image->image.header.stamp;
            ci_->header.frame_id = wfov_image->header.frame_id;
            // The height, width, distortion model, and parameters are all filled in by camera info manager.
            ci_->binning_x = binning_x_;
            ci_->binning_y = binning_y_;
            ci_->roi.x_offset = roi_x_offset_;
            ci_->roi.y_offset = roi_y_offset_;
            ci_->roi.height = roi_height_;
            ci_->roi.width = roi_width_;
            ci_->roi.do_rectify = do_rectify_;

            wfov_image->info = *ci_;

            // Publish the full message
            pub_->publish(wfov_image);

            // Publish the message using standard image transport
            if (it_pub_.getNumSubscribers() > 0)
            {
              sensor_msgs::ImagePtr image(new sensor_msgs::Image(wfov_image->image));
              it_pub_.publish(image, ci_);
            }
          }
          catch (CameraTimeoutException& e)
          {
            NODELET_WARN("%s", e.what());
          }

          catch (std::runtime_error& e)
          {
            NODELET_ERROR("%s", e.what());
            state = ERROR;
          }

          break;
        default:
          NODELET_ERROR("Unknown camera state %d!", state);
      }

      // Update diagnostics
      updater_.update();
    }
    NODELET_DEBUG_ONCE("Leaving thread.");
  }

  void gainWBCallback(const image_exposure_msgs::ExposureSequence& msg)
  {
    try
    {
      NODELET_DEBUG_ONCE("Gain callback:  Setting gain to %f and white balances to %u, %u", msg.gain,
                         msg.white_balance_blue, msg.white_balance_red);
      gain_ = msg.gain;

      spinnaker_.setGain(static_cast<float>(gain_));
      wb_blue_ = msg.white_balance_blue;
      wb_red_ = msg.white_balance_red;

      // TODO(mhosmar):
      // spinnaker_.setBRWhiteBalance(false, wb_blue_, wb_red_);
    }
    catch (std::runtime_error& e)
    {
      NODELET_ERROR("gainWBCallback failed with error: %s", e.what());
    }
  }

  /* Class Fields */
  std::shared_ptr<dynamic_reconfigure::Server<spinnaker_camera_driver::SpinnakerConfig> > srv_;  
  std::shared_ptr<image_transport::ImageTransport> it_;  
  std::shared_ptr<camera_info_manager::CameraInfoManager> cinfo_;  
  image_transport::CameraPublisher it_pub_;                        
  std::shared_ptr<diagnostic_updater::DiagnosedPublisher<wfov_camera_msgs::WFOVImage> > pub_;  
  std::shared_ptr<ros::Publisher> diagnostics_pub_;
  ros::Subscriber sub_;  

  std::mutex connect_mutex_;

  diagnostic_updater::Updater updater_;  
  double min_freq_;
  double max_freq_;

  SpinnakerCamera spinnaker_;      
  sensor_msgs::CameraInfoPtr ci_;  
  std::string frame_id_;           
  std::shared_ptr<boost::thread> pubThread_;  
  std::shared_ptr<boost::thread> diagThread_;  

  std::unique_ptr<DiagnosticsManager> diag_man;

  double gain_;
  uint16_t wb_blue_;
  uint16_t wb_red_;

  // Parameters for cameraInfo
  size_t binning_x_;     
  size_t binning_y_;     
  size_t roi_x_offset_;  
  size_t roi_y_offset_;  
  size_t roi_height_;    
  size_t roi_width_;     
  bool do_rectify_;  

  // For GigE cameras:
  bool auto_packet_size_;
  int packet_size_;
  int packet_delay_;

  spinnaker_camera_driver::SpinnakerConfig config_;
};

PLUGINLIB_EXPORT_CLASS(spinnaker_camera_driver::SpinnakerCameraNodelet,
                       nodelet::Nodelet)  // Needed for Nodelet declaration
}  // namespace spinnaker_camera_driver