PR from Iago Gomes to work around issues with multiple interfaces

spinnaker_camera_driver/CMakeLists.txt

@@ -65,6 +65,16 @@ message(STATUS "libSpinnaker include location: ${SPINNAKER_INCLUDE_DIRS}")
 
 find_package(SPINNAKER REQUIRED)
 
+# special case the older Spinnaker API
+if(DEFINED ENV{ROS_DISTRO})
+  if($ENV{ROS_DISTRO} STREQUAL "foxy" OR
+      $ENV{ROS_DISTRO} STREQUAL "galactic")
+    add_definitions(-DUSE_OLD_SPINNAKER_API)
+  endif()
+else()
+  message(FATAL_ERROR "ROS_DISTRO environment variable is not set!")
+endif()
+
 include_directories(SYSTEM
   ${SPINNAKER_INCLUDE_DIRS})
 
@@ -79,7 +89,6 @@ set(ROS_DEPENDENCIES
   "image_transport"
   "flir_camera_msgs")
 
-
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(ament_cmake_ros REQUIRED)

spinnaker_camera_driver/package.xml

@@ -16,6 +16,7 @@
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
+  <buildtool_depend>ros_environment</buildtool_depend> <!-- ROS_VERSION + ROS_DISTRO -->
 
   <build_depend>python3-distro</build_depend>  <!-- to get lsb_release for downloading Spinnaker -->
   <build_depend>curl</build_depend>  <!-- to get ca-certificates for downloading Spinnaker -->

spinnaker_camera_driver/src/spinnaker_wrapper_impl.cpp

@@ -18,6 +18,7 @@
 #include <chrono>
 #include <cmath>
 #include <iostream>
+#include <spinnaker_camera_driver/logging.hpp>
 #include <string>
 #include <vector>
 
@@ -104,10 +105,58 @@ SpinnakerWrapperImpl::SpinnakerWrapperImpl()
 
 void SpinnakerWrapperImpl::refreshCameraList()
 {
+#ifdef USE_OLD_SPINNAKER_API
   cameraList_ = system_->GetCameras();
   for (size_t cam_idx = 0; cam_idx < cameraList_.GetSize(); cam_idx++) {
     const auto cam = cameraList_[cam_idx];
   }
+#else
+  cameraList_.Clear();
+
+  Spinnaker::InterfaceList interfaceList = system_->GetInterfaces();
+
+  for (size_t i = 0; i < interfaceList.GetSize(); i++) {
+    Spinnaker::InterfacePtr iface = interfaceList.GetByIndex(i);
+
+    Spinnaker::GenApi::INodeMap & nodeMapInterface = iface->GetTLNodeMap();
+
+    Spinnaker::GenApi::CEnumerationPtr ptrInterfaceType = nodeMapInterface.GetNode("InterfaceType");
+
+    if (IsAvailable(ptrInterfaceType) && IsReadable(ptrInterfaceType)) {
+      Spinnaker::GenApi::CStringPtr ptrInterfaceDisplayName =
+        nodeMapInterface.GetNode("InterfaceDisplayName");
+
+      if (IsAvailable(ptrInterfaceDisplayName) && IsReadable(ptrInterfaceDisplayName)) {
+        Spinnaker::GenICam::gcstring interfaceDisplayName = ptrInterfaceDisplayName->GetValue();
+
+        Spinnaker::CameraList camList = iface->GetCameras();
+
+        for (size_t cam_idx = 0; cam_idx < camList.GetSize(); cam_idx++) {
+          // try open the cameras in a specific interface
+          Spinnaker::CameraPtr ptrCam = camList.GetByIndex(cam_idx);
+          try {
+            ptrCam->Init();
+            ptrCam->DeInit();
+          } catch (Spinnaker::Exception & e) {
+            // erro while open the cameras in this interface
+            continue;
+          }  // end try-catch ptrCam
+
+          // successfully open the camera in the interface
+          cameraList_.Add(ptrCam);
+
+          LOG_INFO_FMT(
+            "Found camera [serial: %s] from: [%s]", get_serial(ptrCam).c_str(),
+            interfaceDisplayName.c_str());
+        }  // end for camList
+      } else {
+        LOG_ERROR("Unknown Interface (Display name not readable)");
+      }  // end if-else ptrInterfaceDisplayName
+    }    // end if ptrInterfaceType
+  }      // end for interfaceList
+
+  interfaceList.Clear();
+#endif
 }
 
 SpinnakerWrapperImpl::~SpinnakerWrapperImpl()

spinnaker_camera_driver/src/spinnaker_wrapper_impl.hpp

@@ -21,6 +21,7 @@
 
 #include <memory>
 #include <mutex>
+#include <rclcpp/rclcpp.hpp>
 #include <spinnaker_camera_driver/image.hpp>
 #include <spinnaker_camera_driver/spinnaker_wrapper.hpp>
 #include <string>
@@ -67,6 +68,8 @@ class SpinnakerWrapperImpl : public Spinnaker::ImageEventHandler
   bool setInINodeMap(double f, const std::string & field, double * fret);
   void monitorStatus();
 
+  rclcpp::Logger get_logger() { return rclcpp::get_logger("Spinnaker Wrapper"); }
+
   // ----- variables --
   Spinnaker::SystemPtr system_;
   Spinnaker::CameraList cameraList_;