Program Listing for File camera_driver_gv.h

↰ Return to documentation for file (include/camera_aravis2/camera_driver_gv.h)

// Copyright (c) 2024 Fraunhofer IOSB and contributors
//
// 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 Fraunhofer IOSB 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.

#ifndef CAMERA_ARAVIS2__CAMERA_DRIVER_GV_H_
#define CAMERA_ARAVIS2__CAMERA_DRIVER_GV_H_

// Yaml-cpp
#include <yaml-cpp/yaml.h>

// Std
#include <memory>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>

// Aravis
extern "C"
{
#include "aravis-0.8/arv.h"
}

// ROS
#include <camera_info_manager/camera_info_manager.hpp>
#include <image_transport/image_transport.hpp>
#include <rclcpp/rclcpp.hpp>

// camera_aravis2
#include "camera_aravis2/camera_aravis_node_base.h"
#include "camera_aravis2/concurrent_queue.hpp"
#include "camera_aravis2/config_structs.h"
#include "camera_aravis2/conversion_utils.h"
#include "camera_aravis2/image_buffer_pool.h"
#include <camera_aravis2_msgs/msg/camera_diagnostics.hpp>

namespace camera_aravis2
{
class CameraDriverGv : public CameraAravisNodeBase
{
    //--- STRUCT DECLARATION ---//

    struct Stream
    {
        Stream() :
          p_arv_stream(nullptr),
          p_buffer_pool(ImageBufferPool::SharedPtr()),
          name(""),
          sensor(Sensor()),
          image_roi(ImageRoi()),
          camera_info_url(""),
          p_camera_info_manager(nullptr),
          p_cam_info_msg(nullptr),
          is_buffer_processed(false)
        {
        }

        ArvStream* p_arv_stream;

        ImageBufferPool::SharedPtr p_buffer_pool;

        std::string name;

        Sensor sensor;

        ImageRoi image_roi;

        AcquisitionControl acquisition_control;

        AnalogControl analog_control;

        std::string camera_info_url;

        ConversionFunction cvt_pixel_format;

        image_transport::CameraPublisher camera_pub;

        std::unique_ptr<camera_info_manager::CameraInfoManager> p_camera_info_manager;

        sensor_msgs::msg::CameraInfo::SharedPtr p_cam_info_msg;

        bool is_buffer_processed;

        std::thread buffer_processing_thread;

        ConcurrentQueue<std::pair<ArvBuffer*, sensor_msgs::msg::Image::SharedPtr>>
          buffer_queue;
    };

    //--- METHOD DECLARATION ---//

  public:
    explicit CameraDriverGv(const rclcpp::NodeOptions& options =
                              rclcpp::NodeOptions());

    virtual ~CameraDriverGv();

    bool isSpawningOrInitialized() const;

  protected:
    void setUpParameters() override;

    [[nodiscard]] bool setUpCameraStreamStructs();

    [[nodiscard]] bool getDeviceControlParameterList(
      const std::string& param_name,
      std::vector<std::pair<std::string, rclcpp::ParameterValue>>& param_values) const;

    [[nodiscard]] bool setDeviceControlSettings();

    [[nodiscard]] bool getTransportLayerControlParameter(
      const std::string& param_name,
      rclcpp::ParameterValue& param_value) const;

    [[nodiscard]] bool getTransportLayerControlParameterList(
      const std::string& param_name,
      std::vector<std::pair<std::string, rclcpp::ParameterValue>>& param_values) const;

    [[nodiscard]] bool setTransportLayerControlSettings();

    [[nodiscard]] bool getImageFormatControlParameter(
      const std::string& param_name,
      rclcpp::ParameterValue& param_value) const;

    [[nodiscard]] bool getImageFormatControlParameterList(
      const std::string& param_name,
      std::vector<std::pair<std::string, rclcpp::ParameterValue>>& param_values) const;

    [[nodiscard]] bool setImageFormatControlSettings();

    [[nodiscard]] bool getAcquisitionControlParameter(
      const std::string& param_name,
      rclcpp::ParameterValue& param_value) const;

    [[nodiscard]] bool getAcquisitionControlParameterList(
      const std::string& param_name,
      std::vector<std::pair<std::string, rclcpp::ParameterValue>>& param_values) const;

    [[nodiscard]] bool setAcquisitionControlSettings();

    [[nodiscard]] bool getAnalogControlParameter(
      const std::string& param_name,
      rclcpp::ParameterValue& param_value) const;

    [[nodiscard]] bool getAnalogControlParameterList(
      const std::string& param_name,
      std::vector<std::pair<std::string, rclcpp::ParameterValue>>& param_values) const;

    [[nodiscard]] bool setAnalogControlSettings();

    int discoverNumberOfStreams();

    void spawnCameraStreams();

    void tuneGvStream(ArvGvStream* p_stream) const;

#ifdef WITH_MATCHED_EVENTS
    void handleMessageSubscriptionChange(rclcpp::MatchedInfo& iEventInfo);
#endif

    void setUpCameraDiagnosticPublisher();

    void publishCameraDiagnosticsLoop(double rate) const;

    void checkPtp();

    void processStreamBuffer(const uint stream_id);

    bool adjustImageRoi(ImageRoi& img_roi, ArvBuffer* p_buffer) const;

    void fillImageMsgMetadata(sensor_msgs::msg::Image::SharedPtr& p_img_msg,
                              ArvBuffer* p_buffer, const Sensor& sensor,
                              const ImageRoi& img_roi) const;

    void fillCameraInfoMsg(Stream& stream,
                           const sensor_msgs::msg::Image::SharedPtr& p_img_msg) const;

    void printCameraConfiguration() const;

    void printStreamStatistics() const;

    //--- FUNCTION DECLARATION ---//

  protected:
    static void handleNewBufferSignal(ArvStream* p_stream, gpointer p_user_data);

    //--- MEMBER DECLARATION ---//

  protected:
    TransportLayerControl tl_control_;

    std::vector<Stream> streams_;

    std::atomic<bool> is_spawning_;

    std::thread spawn_stream_thread_;

    std::atomic<bool> is_diagnostics_published_;

    std::thread diagnostic_thread_;

    rclcpp::Publisher<camera_aravis2_msgs::msg::CameraDiagnostics>::SharedPtr p_diagnostic_pub_;

    int current_num_subscribers_;

    YAML::Node diagnostic_features_;

    std::vector<std::shared_ptr<std::pair<CameraDriverGv*, uint>>> new_buffer_cb_data_ptrs;

    std::vector<std::string> config_warn_msgs_;
};

}  // namespace camera_aravis2

#endif  // CAMERA_ARAVIS2__CAMERA_DRIVER_GV_H_