Clean up ROS 2 node, update parameter logic

Signed-off-by: Evan Flynn <evanflynn.msu@gmail.com>

include/usb_cam/usb_cam.hpp

@@ -66,6 +66,34 @@ typedef struct
   struct v4l2_frmivalenum v4l2_fmt;
 } capture_format_t;
 
+typedef struct
+{
+  std::string camera_name;  // can be anything
+  std::string device_name;  // usually /dev/video0 or something similiar
+  std::string frame_id;
+  std::string io_method_name;
+  std::string camera_info_url;
+  // these parameters all have to be a combination supported by the device
+  // Use
+  // v4l2-ctl --device=0 --list-formats-ext
+  // to discover them,
+  // or guvcview
+  std::string pixel_format_name;
+  int image_width;
+  int image_height;
+  int framerate;
+  int brightness;
+  int contrast;
+  int saturation;
+  int sharpness;
+  int gain;
+  int white_balance;
+  int exposure;
+  int focus;
+  bool auto_white_balance;
+  bool autoexposure;
+  bool autofocus;
+} parameters_t;
 
 typedef struct
 {
@@ -109,9 +137,7 @@ class UsbCam
   ~UsbCam();
 
   /// @brief Configure device, should be called before start
-  void configure(
-    const std::string & dev, const io_method_t & io_method, const std::string & pixel_format_str,
-    const uint32_t & image_width, const uint32_t & image_height, const int & framerate);
+  void configure(parameters_t & parameters, const io_method_t & io_method);
 
   /// @brief Start the configured device
   void start();
@@ -170,9 +196,9 @@ class UsbCam
     return m_image.bytes_per_line;
   }
 
-  inline std::string get_camera_dev()
+  inline std::string get_device_name()
   {
-    return m_camera_dev;
+    return m_device_name;
   }
 
   inline std::shared_ptr<pixel_format_base> get_pixel_format()
@@ -302,16 +328,13 @@ class UsbCam
   void uninit_device();
   void close_device();
 
-  std::string m_camera_dev;
+  std::string m_device_name;
   usb_cam::utils::io_method_t m_io;
   int m_fd;
   usb_cam::utils::buffer * m_buffers;
   unsigned int m_number_of_buffers;
   image_t m_image;
 
-  // Only used for MJPEG to RGB conversion using ffmpeg utilities
-  /// @brief Initialize ffmpeg utilities to decode the image
-  void init_decoder();
   AVFrame * m_avframe;
   int m_avframe_size;
   AVCodec * m_avcodec;

include/usb_cam/usb_cam_node.hpp

@@ -54,7 +54,6 @@ std::ostream & operator<<(std::ostream & ostr, const rclcpp::Time & tm)
 namespace usb_cam
 {
 
-
 class UsbCamNode : public rclcpp::Node
 {
 public:
@@ -64,56 +63,32 @@ class UsbCamNode : public rclcpp::Node
   void init();
   void get_params();
   void assign_params(const std::vector<rclcpp::Parameter> & parameters);
+  void set_v4l2_params();
   void update();
   bool take_and_send_image();
 
-  rcl_interfaces::msg::SetParametersResult parametersCallback(
+  rcl_interfaces::msg::SetParametersResult parameters_callback(
     const std::vector<rclcpp::Parameter> & parameters);
 
   void service_capture(
     const std::shared_ptr<rmw_request_id_t> request_header,
     const std::shared_ptr<std_srvs::srv::SetBool::Request> request,
     std::shared_ptr<std_srvs::srv::SetBool::Response> response);
 
-  UsbCam cam_;
-
-  // shared image message
-  sensor_msgs::msg::Image::UniquePtr img_;
-  std::shared_ptr<image_transport::CameraPublisher> image_pub_;
-  // parameters
-  std::string video_device_name_;
-  std::string frame_id_;
-
-  std::string io_method_name_;
-  // these parameters all have to be a combination supported by the device
-  // Use
-  // v4l2-ctl --device=0 --list-formats-ext
-  // to discover them,
-  // or guvcview
-  std::string pixel_format_name_;
-  int image_width_;
-  int image_height_;
-  int framerate_;
-  int brightness_;
-  int contrast_;
-  int saturation_;
-  int sharpness_;
-  int gain_;
-  int white_balance_;
-  int exposure_;
-  int focus_;
-  bool auto_white_balance_;
-  bool autoexposure_;
-  bool autofocus_;
-
-  std::string camera_name_;
-  std::string camera_info_url_;
-  std::shared_ptr<camera_info_manager::CameraInfoManager> cinfo_;
-
-  rclcpp::TimerBase::SharedPtr timer_;
-
-  rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr service_capture_;
-  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameters_callback_handle_;
+  UsbCam * m_camera;
+
+  sensor_msgs::msg::Image::UniquePtr m_image_msg;
+  std::shared_ptr<image_transport::CameraPublisher> m_image_publisher;
+
+  parameters_t m_parameters;
+
+  sensor_msgs::msg::CameraInfo::UniquePtr m_camera_info_msg;
+  std::shared_ptr<camera_info_manager::CameraInfoManager> m_camera_info;
+
+  rclcpp::TimerBase::SharedPtr m_timer;
+
+  rclcpp::Service<std_srvs::srv::SetBool>::SharedPtr m_service_capture;
+  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr m_parameters_callback_handle;
 };
 }  // namespace usb_cam
 #endif  // USB_CAM__USB_CAM_NODE_HPP_

src/usb_cam.cpp

@@ -58,7 +58,7 @@ using utils::io_method_t;
 
 
 UsbCam::UsbCam()
-: m_camera_dev(), m_io(io_method_t::IO_METHOD_MMAP), m_fd(-1),
+: m_device_name(), m_io(io_method_t::IO_METHOD_MMAP), m_fd(-1),
   m_buffers(NULL), m_number_of_buffers(0), m_image(),
   m_avframe(NULL), m_avcodec(NULL), m_avoptions(NULL),
   m_avcodec_context(NULL),
@@ -530,41 +530,39 @@ void UsbCam::open_device()
 {
   struct stat st;
 
-  if (-1 == stat(m_camera_dev.c_str(), &st)) {
+  if (-1 == stat(m_device_name.c_str(), &st)) {
     throw strerror(errno);
   }
 
   if (!S_ISCHR(st.st_mode)) {
     throw strerror(errno);
   }
 
-  m_fd = open(m_camera_dev.c_str(), O_RDWR /* required */ | O_NONBLOCK, 0);
+  m_fd = open(m_device_name.c_str(), O_RDWR /* required */ | O_NONBLOCK, 0);
 
   if (-1 == m_fd) {
     throw strerror(errno);
   }
 }
 
 void UsbCam::configure(
-  const std::string & dev, const io_method_t & io_method,
-  const std::string & pixel_format_str,
-  const uint32_t & image_width, const uint32_t & image_height, const int & framerate)
+  parameters_t & parameters, const io_method_t & io_method)
 {
-  m_camera_dev = dev;
+  m_device_name = parameters.device_name;
   m_io = io_method;
 
   // Open device file descriptor before anything else
   open_device();
 
-  m_image.width = static_cast<int>(image_width);
-  m_image.height = static_cast<int>(image_height);
+  m_image.width = static_cast<int>(parameters.image_width);
+  m_image.height = static_cast<int>(parameters.image_height);
   m_image.set_number_of_pixels();
 
   // Do this before calling set_bytes_per_line and set_size_in_bytes
-  m_image.pixel_format = set_pixel_format_from_string(pixel_format_str);
+  m_image.pixel_format = set_pixel_format_from_string(parameters.pixel_format_name);
   m_image.set_bytes_per_line();
   m_image.set_size_in_bytes();
-  m_framerate = framerate;
+  m_framerate = parameters.framerate;
 
   // Allocate memory for the image
   m_image.data = reinterpret_cast<char *>(calloc(m_image.size_in_bytes, sizeof(char *)));
@@ -746,7 +744,7 @@ bool UsbCam::set_v4l_parameter(const std::string & param, const std::string & va
   int retcode = 0;
   // build the command
   std::stringstream ss;
-  ss << "v4l2-ctl --device=" << m_camera_dev << " -c " << param << "=" << value << " 2>&1";
+  ss << "v4l2-ctl --device=" << m_device_name << " -c " << param << "=" << value << " 2>&1";
   std::string cmd = ss.str();
 
   // capture the output