From cae7982a830c3f87869b360d2be8defae183ac16 Mon Sep 17 00:00:00 2001 From: RomainTH Date: Mon, 30 May 2022 12:33:56 +0200 Subject: [PATCH 1/3] GigE camera - parameters handling --- spinnaker_camera_driver/cfg/Spinnaker.cfg | 7 + .../spinnaker_camera_driver/SpinnakerCamera.h | 50 +++ .../include/spinnaker_camera_driver/camera.h | 5 + .../launch/camera_gige.launch | 131 ++++++ spinnaker_camera_driver/launch/stereo.launch | 16 +- .../src/SpinnakerCamera.cpp | 422 ++++++++++++++++-- spinnaker_camera_driver/src/camera.cpp | 42 +- spinnaker_camera_driver/src/nodelet.cpp | 10 + 8 files changed, 648 insertions(+), 35 deletions(-) create mode 100644 spinnaker_camera_driver/launch/camera_gige.launch diff --git a/spinnaker_camera_driver/cfg/Spinnaker.cfg b/spinnaker_camera_driver/cfg/Spinnaker.cfg index ab976ec0..1f5b0d12 100755 --- a/spinnaker_camera_driver/cfg/Spinnaker.cfg +++ b/spinnaker_camera_driver/cfg/Spinnaker.cfg @@ -306,6 +306,13 @@ auto_lighting_mode = gen.enum([ ], "Auto algorithms lighting modes") gen.add("auto_exposure_lighting_mode", str_t, SensorLevels.RECONFIGURE_RUNNING, "Auto exposure lighting mode.", "Normal", edit_method=auto_lighting_mode) +# Control device link maximum authorized throughput +gen.add("device_link_throughput_limit_enable", bool_t, SensorLevels.RECONFIGURE_STOP, "Device link throughput limit enable", False) +gen.add("device_link_throughput_limit", int_t, SensorLevels.RECONFIGURE_RUNNING, "Device link throughput limit", 14353165, 0) + +# GigE cameras packet size +gen.add("camera_packet_size", int_t, SensorLevels.RECONFIGURE_STOP, "GigE camera packet size", 1400, 0) + # Other gen.add("time_offset", double_t, SensorLevels.RECONFIGURE_RUNNING, "Time offset to add to image time stamps.", 0.0, -5.0, 5.0) diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/SpinnakerCamera.h b/spinnaker_camera_driver/include/spinnaker_camera_driver/SpinnakerCamera.h index 4cd9251b..51749f01 100644 --- a/spinnaker_camera_driver/include/spinnaker_camera_driver/SpinnakerCamera.h +++ b/spinnaker_camera_driver/include/spinnaker_camera_driver/SpinnakerCamera.h @@ -52,6 +52,7 @@ THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include // Header generated by dynamic_reconfigure #include @@ -160,6 +161,36 @@ class SpinnakerCamera */ void setDesiredCamera(const uint32_t& id); + /*! + * \brief Used to set the IP of the camera you wish to connect to. + * + * Sets the desired IP. If this value is not set, the driver will try to automatically + * assign an IP address to the camera. + * This function should be called before connect(). + * \param ip IP for the camera. Should be something like 192.168.0.2. + */ + void setDesiredIP(const std::string& ip); + + /*! + * \brief Used to set the subnet mask of the network the camera is connected to. + * + * Sets the subnet mask. If this value is not set, the driver will assign the default subnet mask + * '255.255.255.0'. + * This function should be called before connect(). + * \param mask Subnet mask of the network. Should be something like 255.255.255.0. + */ + void setDesiredSubnetMask(const std::string& mask); + + /*! + * \brief Used to set the gateway of the subnet the camera will connect to. + * + * Sets the desired gateway. If this value is not set, the driver will automatically + * assign the gateway to the first address of the subnet. + * This function should be called before connect(). + * \param gateway Gateway address on the subnet. Should be something like 192.168.0.1. + */ + void setDesiredGateway(const std::string& gateway); + void setGain(const float& gain); int getHeightMax(); int getWidthMax(); @@ -171,8 +202,27 @@ class SpinnakerCamera return serial_; } +protected: + // Routine to find the desired camera to connect to. + void findCameraPtr(); + // Routine to update camera the desired camera's pointer after changing a camera parameter (e.g. IP address) + void updateCameraPtr(); + int64_t fromStrToIP(const std::string ip_str); + std::string convertIPtoStr(const int64_t ip_int); + + /*! + * \brief Used to apply the desired subnet configuration to the camera. + * + * This function sets the IP of the camera, the subnet mask and the gateway address for the network being used. + */ + void setIP(); + void tryAutoForceIP(); + private: uint32_t serial_; ///< A variable to hold the serial number of the desired camera. + int64_t ip_; ///< A variable to hold the IP to associate to the desired camera. + int64_t mask_; ///< A variable to hold the subnet mask of the network the desired camera is attached to. + int64_t gateway_; ///< A variable to hold the address of the gateway in the subnet the camera is connected to. Spinnaker::SystemPtr system_; Spinnaker::CameraList camList_; diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.h b/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.h index 5454588d..a49011e8 100644 --- a/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.h +++ b/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.h @@ -63,6 +63,7 @@ class Camera static const uint8_t LEVEL_RECONFIGURE_RUNNING = 0; virtual void setGain(const float& gain); + inline void setPacketSizeMax(const unsigned int size) { packet_size_max_ = size; }; int getHeightMax(); int getWidthMax(); @@ -78,6 +79,7 @@ class Camera int height_max_; int width_max_; + unsigned int packet_size_max_; /*! * \brief Changes the video mode of the connected camera. @@ -87,6 +89,9 @@ class Camera */ virtual void setFrameRate(const float frame_rate); virtual void setImageControlFormats(const spinnaker_camera_driver::SpinnakerConfig& config); + virtual void setDeviceLinkThroughput(const int throughput_limit, const bool enable); + + void setGigEPacketSize(const int size); /*! * \brief Set parameters relative to GigE cameras. * diff --git a/spinnaker_camera_driver/launch/camera_gige.launch b/spinnaker_camera_driver/launch/camera_gige.launch new file mode 100644 index 00000000..28db3ca3 --- /dev/null +++ b/spinnaker_camera_driver/launch/camera_gige.launch @@ -0,0 +1,131 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/spinnaker_camera_driver/launch/stereo.launch b/spinnaker_camera_driver/launch/stereo.launch index ce0c2d7d..18fef300 100644 --- a/spinnaker_camera_driver/launch/stereo.launch +++ b/spinnaker_camera_driver/launch/stereo.launch @@ -33,10 +33,10 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI - + - + @@ -57,6 +57,12 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI + + + + + + @@ -79,6 +85,12 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI + + + + + + diff --git a/spinnaker_camera_driver/src/SpinnakerCamera.cpp b/spinnaker_camera_driver/src/SpinnakerCamera.cpp index 3ecaac9f..8d1d8fe7 100644 --- a/spinnaker_camera_driver/src/SpinnakerCamera.cpp +++ b/spinnaker_camera_driver/src/SpinnakerCamera.cpp @@ -142,38 +142,84 @@ Spinnaker::GenApi::CNodePtr SpinnakerCamera::readProperty(const Spinnaker::GenIC } } -void SpinnakerCamera::connect() +void SpinnakerCamera::findCameraPtr() { - if (!pCam_) + // If we have a specific camera to connect to (specified by a serial number) + if (serial_ != 0) { - // If we have a specific camera to connect to (specified by a serial number) - if (serial_ != 0) - { - const auto serial_string = std::to_string(serial_); + const auto serial_string = std::to_string(serial_); - try - { - pCam_ = camList_.GetBySerial(serial_string); - } - catch (const Spinnaker::Exception& e) - { - throw std::runtime_error("[SpinnakerCamera::connect] Could not find camera with serial number " + - serial_string + ". Is that camera plugged in? Error: " + std::string(e.what())); - } + try + { + pCam_ = camList_.GetBySerial(serial_string); } - else + catch (const Spinnaker::Exception& e) { - // Connect to any camera (the first) - try - { - pCam_ = camList_.GetByIndex(0); - } - catch (const Spinnaker::Exception& e) - { - throw std::runtime_error("[SpinnakerCamera::connect] Failed to get first connected camera. Error: " + - std::string(e.what())); - } + throw std::runtime_error("[SpinnakerCamera::connect] Could not find camera with serial number " + + serial_string + ". Is that camera plugged in? Error: " + std::string(e.what())); + } + } + else + { + // Connect to any camera (the first) + try + { + pCam_ = camList_.GetByIndex(0); + } + catch (const Spinnaker::Exception& e) + { + throw std::runtime_error("[SpinnakerCamera::connect] Failed to get first connected camera. Error: " + + std::string(e.what())); + } + } +} + +void SpinnakerCamera::updateCameraPtr() +{ + if (pCam_->IsInitialized()) + { + pCam_->DeInit(); + } + int prev_cam_list_size = camList_.GetSize(); + camList_.Clear(); + + // If a parameter changed on a camera (e.g. IP address), we need to update the cameras + for (size_t i = 0; i < 10; i++) + { + std::this_thread::sleep_for(std::chrono::milliseconds(500)); // Delay to let time to completely release the camera + system_->UpdateCameras(); + camList_ = system_->GetCameras(); + if (camList_.GetSize() == prev_cam_list_size) + { + break; } + } + if (camList_.GetSize() != prev_cam_list_size) + { + throw std::runtime_error("[SpinnakerCamera::updateCameraPtr] Failed to reconnect to camera after changing IP. " + "Try to reboot your camera."); + } + + // For debugging purpose + // for (size_t idx = 0; idx < camList_.GetSize(); idx++) + // { + // Spinnaker::CameraPtr cam = camList_.GetByIndex(idx); + // Spinnaker::GenApi::INodeMap& node_map_local = cam->GetTLDeviceNodeMap(); + + // Spinnaker::GenApi::CStringPtr sn = node_map_local.GetNode("DeviceSerialNumber"); + // std::cout << "Found SN " << sn->GetValue() << std::endl; + // } + + // std::cout << "Targetting serial " << serial_ << std::endl; + pCam_ = nullptr; + findCameraPtr(); +} + +void SpinnakerCamera::connect() +{ + if (!pCam_) + { + findCameraPtr(); if (!pCam_ || !pCam_->IsValid()) { throw std::runtime_error("[SpinnakerCamera::connect] Failed to obtain camera reference."); @@ -187,7 +233,7 @@ void SpinnakerCamera::connect() if (serial_ == 0) { Spinnaker::GenApi::CStringPtr serial_ptr = - static_cast(genTLNodeMap.GetNode("DeviceID")); + static_cast(genTLNodeMap.GetNode("DeviceSerialNumber")); if (IsAvailable(serial_ptr) && IsReadable(serial_ptr)) { serial_ = atoi(serial_ptr->GetValue().c_str()); @@ -217,6 +263,30 @@ void SpinnakerCamera::connect() } } // TODO(mhosmar): - check if interface is GigE and connect to GigE cam + // Actually: if GigE camera, auto force IP if no IP is set in config + // and if the camera is not on the right subnetwork, + // or update IP according to config (if specified) + if (device_type_ptr->GetIntValue() == Spinnaker::DeviceTypeEnum::DeviceType_GigEVision) + { + + Spinnaker::GenApi::CBooleanPtr is_wrong_subnet = genTLNodeMap.GetNode("GevDeviceIsWrongSubnet"); + + if ((ip_ == 0) && is_wrong_subnet->GetValue()) + { + ROS_WARN_STREAM("NO IP set for camera with serial " << serial_ << ". " + << "Trying to auto force IP, which can lead to adresses issues."); + tryAutoForceIP(); + } + else if (ip_ != 0) + { + setIP(); + } + + if (pCam_->IsInitialized()) + { + pCam_->DeInit(); + } + } } } catch (const Spinnaker::Exception& e) @@ -250,6 +320,10 @@ void SpinnakerCamera::connect() ROS_WARN("SpinnakerCamera::connect: Could not detect camera model name."); } + // Need to do this here: no reference to camera in Camera class + unsigned int packet_size_max = pCam_->DiscoverMaxPacketSize(); + camera_->setPacketSizeMax(packet_size_max); + // Configure chunk data - Enable Metadata // SpinnakerCamera::ConfigureChunkData(*node_map_); } @@ -478,6 +552,300 @@ void SpinnakerCamera::setDesiredCamera(const uint32_t& id) { serial_ = id; } +void SpinnakerCamera::setDesiredIP(const std::string& ip) +{ + ip_ = fromStrToIP(ip); +} +void SpinnakerCamera::setDesiredSubnetMask(const std::string& mask) +{ + mask_ = fromStrToIP(mask); +} +void SpinnakerCamera::setDesiredGateway(const std::string& gateway) +{ + gateway_ = fromStrToIP(gateway); +} + +int64_t SpinnakerCamera::fromStrToIP(const std::string ip_str) +{ + int64_t ip_int = 0; + std::ostringstream ip_part; + const char *ip_char = ip_str.c_str(); + int n=3; + for (size_t i=0 ; i(ip_int); + std::ostringstream ip_str; + ip_str << ((ip_us & 0xFF000000) >> 24) << "."; + ip_str << ((ip_us & 0x00FF0000) >> 16) << "."; + ip_str << ((ip_us & 0x0000FF00) >> 8) << "."; + ip_str << (ip_us & 0x000000FF); + + return ip_str.str(); +} + +void SpinnakerCamera::setIP() +{ + try + { + // Need to deinit the camera before updating its IP + bool is_cam_init = pCam_->IsInitialized(); + if (is_cam_init) + { + pCam_->DeInit(); + } + + if (gateway_ == 0) + { + gateway_ = (ip_ & mask_) + 1; + } + + if (pCam_ != nullptr) + { + Spinnaker::GenApi::INodeMap& node_mapTL = pCam_->GetTLDeviceNodeMap(); + Spinnaker::GenApi::CEnumerationPtr device_type = node_mapTL.GetNode("DeviceType"); + if (device_type->GetIntValue() != Spinnaker::DeviceTypeEnum::DeviceType_GigEVision) + { + ROS_INFO("[SpinnakerCamera::setIP] IP can only be set for GigEVision system"); + return; + } + + Spinnaker::GenApi::CStringPtr cam_serial = node_mapTL.GetNode("DeviceSerialNumber"); + + Spinnaker::GenApi::CIntegerPtr curr_ip = node_mapTL.GetNode("GevDeviceIPAddress"); + if (ip_ == 0) + { + ROS_INFO_STREAM("[SpinnakerCamera::setIP] No changes made to camera's IP. Current value is " + << curr_ip->GetValue()); + return; + } + if (!IsReadable(curr_ip)) + { + ROS_ERROR("[SpinnakerCamera::setIP] Cannot read the current IP address"); + } + if (curr_ip->GetValue() == ip_) + { + return; + } + + // First, update the network config at the TL level + Spinnaker::GenApi::CIntegerPtr camTLIP = node_mapTL.GetNode("GevDeviceForceIPAddress"); + if (!IsWritable(camTLIP)) + { + ROS_ERROR("[SpinnakerCamera::setIP] Cannot force IP on this device (TL level)"); + return; + } + else + { + camTLIP->SetValue(ip_); + ROS_INFO_STREAM("[SpinnakerCamera::setIP] IP address set to " << convertIPtoStr(camTLIP->GetValue())); + } + + Spinnaker::GenApi::CIntegerPtr camTL_mask = node_mapTL.GetNode("GevDeviceForceSubnetMask"); + + if (!IsWritable(camTL_mask)) + { + ROS_ERROR("[SpinnakerCamera::setIP] Cannot force subnet mask on this device (TL level)"); + return; + } + else + { + camTL_mask->SetValue(mask_); + ROS_INFO_STREAM("[SpinnakerCamera::setIP] Subnet mask set to " << convertIPtoStr(camTL_mask->GetValue())); + } + + Spinnaker::GenApi::CIntegerPtr camTL_gateway = node_mapTL.GetNode("GevDeviceForceGateway"); + if (!IsWritable(camTL_gateway)) + { + ROS_ERROR("[SpinnakerCamera::setIP] Cannot force gateway on this device (TL level)"); + return; + } + else + { + camTL_gateway->SetValue(gateway_); + ROS_INFO_STREAM("[SpinnakerCamera::setIP] Gateway set to " << convertIPtoStr(camTL_gateway->GetValue())); + } + + Spinnaker::GenApi::CCommandPtr force_IP = node_mapTL.GetNode("GevDeviceForceIP"); + if (!IsWritable(force_IP)) + { + ROS_ERROR("[SpinnakerCamera::setIP] Cannot execute device force IP on this device"); + return; + } + else + { + force_IP->Execute(); + } + + updateCameraPtr(); + } + + // Update parameters at camera level + if (pCam_ != nullptr) + { + Spinnaker::GenApi::INodeMap& node_mapTL = pCam_->GetTLDeviceNodeMap(); // Updated camera => update TL node map + Spinnaker::GenApi::CBooleanPtr is_wrong_subnet = node_mapTL.GetNode("GevDeviceIsWrongSubnet"); + + Spinnaker::GenApi::CStringPtr cam_serial = node_mapTL.GetNode("DeviceSerialNumber"); + + Spinnaker::GenApi::CIntegerPtr cam_ip = node_mapTL.GetNode("GevDeviceIPAddress"); + + if (!is_wrong_subnet->GetValue()) + { + pCam_->Init(); + Spinnaker::GenApi::INodeMap& node_map = pCam_->GetNodeMap(); + + Spinnaker::GenApi::CBooleanPtr is_IPConfig_persistent = node_map.GetNode("GevCurrentIPConfigurationPersistentIP"); + if (!IsWritable(is_IPConfig_persistent)) + { + ROS_ERROR("[SpinnakerCamera::setIP] Cannot set persistent IP address"); + return; + } + else + { + is_IPConfig_persistent->SetValue(true); + ROS_INFO_STREAM("[SpinnakerCamera::setIP] Persistent IP config set to " << is_IPConfig_persistent->GetValue()); + } + + Spinnaker::GenApi::CIntegerPtr camIP = node_map.GetNode("GevPersistentIPAddress"); + if (!IsWritable(camIP)) + { + ROS_ERROR("[SpinnakerCamera::setIP] Cannot force IP on this device (camera level)"); + is_IPConfig_persistent->SetValue(false); + return; + } + else + { + camIP->SetValue(ip_); + } + + Spinnaker::GenApi::CIntegerPtr cam_mask = node_map.GetNode("GevPersistentSubnetMask"); + if (!IsWritable(cam_mask)) + { + ROS_ERROR("[SpinnakerCamera::setIP] Cannot force subnet mask on this device (camera level)"); + is_IPConfig_persistent->SetValue(false); + return; + } + else + { + cam_mask->SetValue(mask_); + } + + Spinnaker::GenApi::CIntegerPtr cam_gateway = node_map.GetNode("GevPersistentDefaultGateway"); + if (!IsWritable(cam_gateway)) + { + ROS_ERROR("[SpinnakerCamera::setIP] Cannot force gateway on this device (camera level)"); + is_IPConfig_persistent->SetValue(false); + return; + } + else + { + cam_gateway->SetValue(gateway_); + } + + ROS_INFO("[SpinnakerCamera::setIP] Finished camera network configuration"); + } + else + { + throw std::runtime_error("[SpinnakerCamera::setIP] Camera is still on the wrong subnet. " + "Check your network configuration."); + } + + // Let pCam_ state unchanged after changing IP + if (!is_cam_init && pCam_->IsInitialized()) + { + pCam_->DeInit(); + } + else if (is_cam_init && !pCam_->IsInitialized()) + { + pCam_->Init(); + } + } + } + catch(const Spinnaker::Exception& e) + { + std::runtime_error(e.what()); + } +} // end setIP + +void SpinnakerCamera::tryAutoForceIP() +{ + Spinnaker::InterfaceList interfaces = system_->GetInterfaces(); + for (unsigned int i = 0; i < interfaces.GetSize(); i++) + { + Spinnaker::InterfacePtr pInterface = interfaces.GetByIndex(i); + pInterface->UpdateCameras(); + Spinnaker::GenApi::INodeMap& nodeMapInterface = pInterface->GetTLNodeMap(); + + Spinnaker::GenApi::CEnumerationPtr ptrInterfaceType = nodeMapInterface.GetNode("InterfaceType"); + if (!IsAvailable(ptrInterfaceType) || !IsReadable(ptrInterfaceType)) + { + ROS_ERROR_STREAM("[SpinnakerCamera::tryAutoForceIP] Unable to read InterfaceType for interface at index " << i); + continue; + } + + if (ptrInterfaceType->GetIntValue() != Spinnaker::InterfaceTypeEnum::InterfaceType_GigEVision) + { + // Only force IP on GEV interface + continue; + } + + Spinnaker::GenApi::CCommandPtr ptrAutoForceIP = nodeMapInterface.GetNode("GevDeviceAutoForceIP"); + if (IsAvailable(ptrAutoForceIP) && IsWritable(ptrAutoForceIP)) + { + if (!IsWritable(pInterface->TLInterface.DeviceSelector.GetAccessMode())) + { + ROS_ERROR("[SpinnakerCamera::tryAutoForceIP] Unable to write to the DeviceSelector node while forcing IP"); + } + else + { + Spinnaker::CameraList cam_list = pInterface->GetCameras(); + for (int i = 0; i < cam_list.GetSize(); i++) + { + Spinnaker::CameraPtr pCam = cam_list.GetByIndex(i); + if (!pCam_->GetUniqueID().compare(pCam->GetUniqueID())) + { + Spinnaker::GenApi::CStringPtr cam_serial = pCam->GetTLDeviceNodeMap().GetNode("DeviceSerialNumber"); + ROS_INFO_STREAM("[SpinnakerCamera::tryAutoForceIP] Forced IP for camera with serial " + << cam_serial->GetValue()); + + pInterface->TLInterface.DeviceSelector.SetValue(i); + pInterface->TLInterface.GevDeviceAutoForceIP.Execute(); + break; + } + } + break; + } + } + else + { + ROS_WARN("[SpinnakerCamera::tryAutoForceIP] AutoForceIP not available for this interface"); + } + } + + interfaces.Clear(); + updateCameraPtr(); +} void SpinnakerCamera::ConfigureChunkData(const Spinnaker::GenApi::INodeMap& nodeMap) { diff --git a/spinnaker_camera_driver/src/camera.cpp b/spinnaker_camera_driver/src/camera.cpp index c7b4956f..eaf3236b 100644 --- a/spinnaker_camera_driver/src/camera.cpp +++ b/spinnaker_camera_driver/src/camera.cpp @@ -42,10 +42,8 @@ void Camera::init() throw std::runtime_error("[Camera::init] Unable to read WidthMax"); } width_max_ = width_max_ptr->GetValue(); - // Set Throughput to maximum - //===================================== - setMaxInt(node_map_, "DeviceLinkThroughputLimit"); } + void Camera::setFrameRate(const float frame_rate) { // This enables the "AcquisitionFrameRateEnabled" @@ -65,6 +63,20 @@ void Camera::setFrameRate(const float frame_rate) ROS_DEBUG_STREAM("Current Frame rate: \t " << ptrAcquisitionFrameRate->GetValue()); } +void Camera::setDeviceLinkThroughput(const int throughput_limit, const bool enable) +{ + if (enable) + { + setProperty(node_map_, "DeviceLinkThroughputLimit", throughput_limit); + } + else + { + // Set Throughput to maximum + //===================================== + setMaxInt(node_map_, "DeviceLinkThroughputLimit"); + } +} + void Camera::setNewConfiguration(const SpinnakerConfig& config, const uint32_t& level) { try @@ -72,9 +84,12 @@ void Camera::setNewConfiguration(const SpinnakerConfig& config, const uint32_t& if (level >= LEVEL_RECONFIGURE_STOP) setImageControlFormats(config); - setFrameRate(static_cast(config.acquisition_frame_rate)); - // Set enable after frame rate encase its false - setProperty(node_map_, "AcquisitionFrameRateEnable", config.acquisition_frame_rate_enable); + if (config.acquisition_frame_rate_enable) + { + setFrameRate(static_cast(config.acquisition_frame_rate)); + // Set enable after frame rate encase its false + setProperty(node_map_, "AcquisitionFrameRateEnable", config.acquisition_frame_rate_enable); + } // Set Trigger and Strobe Settings // NOTE: The trigger must be disabled (i.e. TriggerMode = "Off") in order to configure whether the source is @@ -176,6 +191,9 @@ void Camera::setNewConfiguration(const SpinnakerConfig& config, const uint32_t& { setProperty(node_map_, "AutoExposureLightingMode", config.auto_exposure_lighting_mode); } + + setDeviceLinkThroughput(config.device_link_throughput_limit, config.device_link_throughput_limit_enable); + setGigEPacketSize(config.camera_packet_size); } catch (const Spinnaker::Exception& e) { @@ -242,6 +260,18 @@ void Camera::setGain(const float& gain) setProperty(node_map_, "Gain", static_cast(gain)); } +void Camera::setGigEPacketSize(const int size) +{ + if (size < packet_size_max_) + { + setProperty(node_map_, "GevSCPSPacketSize", size); + } + else + { + setProperty(node_map_, "GevSCPSPacketSize", static_cast(packet_size_max_)); + } +} + /* void Camera::setGigEParameters(bool auto_packet_size, unsigned int packet_size, unsigned int packet_delay) { diff --git a/spinnaker_camera_driver/src/nodelet.cpp b/spinnaker_camera_driver/src/nodelet.cpp index 1c8588d0..f072ba7c 100644 --- a/spinnaker_camera_driver/src/nodelet.cpp +++ b/spinnaker_camera_driver/src/nodelet.cpp @@ -299,6 +299,16 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet spinnaker_.setDesiredCamera((uint32_t)serial); // Get GigE camera parameters: + std::string cam_ip; + pnh.param("camera_ip", cam_ip, "0"); + spinnaker_.setDesiredIP(cam_ip); + std::string cam_subnet; + pnh.param("camera_mask", cam_subnet, "255.255.255.0"); + spinnaker_.setDesiredSubnetMask(cam_subnet); + std::string gateway; + pnh.param("subnet_gateway", gateway, "0"); + spinnaker_.setDesiredGateway(gateway); + pnh.param("packet_size", packet_size_, 1400); pnh.param("auto_packet_size", auto_packet_size_, true); pnh.param("packet_delay", packet_delay_, 4000); From 3fefe4ce86b7bdd52df054e82c8b9cb4c3f9dfc7 Mon Sep 17 00:00:00 2001 From: RomainTH Date: Mon, 30 May 2022 14:29:29 +0200 Subject: [PATCH 2/3] Reversed to original project's file --- spinnaker_camera_driver/launch/stereo.launch | 19 ++----------------- 1 file changed, 2 insertions(+), 17 deletions(-) diff --git a/spinnaker_camera_driver/launch/stereo.launch b/spinnaker_camera_driver/launch/stereo.launch index 18fef300..1bfa1da9 100644 --- a/spinnaker_camera_driver/launch/stereo.launch +++ b/spinnaker_camera_driver/launch/stereo.launch @@ -1,11 +1,9 @@ - @@ -85,12 +76,6 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI - - - - - - From 2624fa12ba2c197bc97621ba70a073501a03694f Mon Sep 17 00:00:00 2001 From: RomainTH Date: Mon, 30 May 2022 15:16:14 +0200 Subject: [PATCH 3/3] Avoid crashing when using an USB3 camera --- spinnaker_camera_driver/cfg/Spinnaker.cfg | 6 ++--- .../include/spinnaker_camera_driver/camera.h | 1 - .../launch/camera_gige.launch | 4 ++-- spinnaker_camera_driver/src/camera.cpp | 23 ++++++------------- 4 files changed, 11 insertions(+), 23 deletions(-) diff --git a/spinnaker_camera_driver/cfg/Spinnaker.cfg b/spinnaker_camera_driver/cfg/Spinnaker.cfg index 1f5b0d12..dacd6893 100755 --- a/spinnaker_camera_driver/cfg/Spinnaker.cfg +++ b/spinnaker_camera_driver/cfg/Spinnaker.cfg @@ -306,11 +306,9 @@ auto_lighting_mode = gen.enum([ ], "Auto algorithms lighting modes") gen.add("auto_exposure_lighting_mode", str_t, SensorLevels.RECONFIGURE_RUNNING, "Auto exposure lighting mode.", "Normal", edit_method=auto_lighting_mode) -# Control device link maximum authorized throughput -gen.add("device_link_throughput_limit_enable", bool_t, SensorLevels.RECONFIGURE_STOP, "Device link throughput limit enable", False) +# GigE camera parameters +gen.add("gige_parameter_enable", bool_t, SensorLevels.RECONFIGURE_STOP, "GigE camera parameters enabled", False) gen.add("device_link_throughput_limit", int_t, SensorLevels.RECONFIGURE_RUNNING, "Device link throughput limit", 14353165, 0) - -# GigE cameras packet size gen.add("camera_packet_size", int_t, SensorLevels.RECONFIGURE_STOP, "GigE camera packet size", 1400, 0) # Other diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.h b/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.h index a49011e8..efc73eb3 100644 --- a/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.h +++ b/spinnaker_camera_driver/include/spinnaker_camera_driver/camera.h @@ -89,7 +89,6 @@ class Camera */ virtual void setFrameRate(const float frame_rate); virtual void setImageControlFormats(const spinnaker_camera_driver::SpinnakerConfig& config); - virtual void setDeviceLinkThroughput(const int throughput_limit, const bool enable); void setGigEPacketSize(const int size); /*! diff --git a/spinnaker_camera_driver/launch/camera_gige.launch b/spinnaker_camera_driver/launch/camera_gige.launch index 28db3ca3..703a81a2 100644 --- a/spinnaker_camera_driver/launch/camera_gige.launch +++ b/spinnaker_camera_driver/launch/camera_gige.launch @@ -67,7 +67,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI --> - + @@ -95,7 +95,7 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI - + diff --git a/spinnaker_camera_driver/src/camera.cpp b/spinnaker_camera_driver/src/camera.cpp index eaf3236b..9c6fa6ab 100644 --- a/spinnaker_camera_driver/src/camera.cpp +++ b/spinnaker_camera_driver/src/camera.cpp @@ -63,20 +63,6 @@ void Camera::setFrameRate(const float frame_rate) ROS_DEBUG_STREAM("Current Frame rate: \t " << ptrAcquisitionFrameRate->GetValue()); } -void Camera::setDeviceLinkThroughput(const int throughput_limit, const bool enable) -{ - if (enable) - { - setProperty(node_map_, "DeviceLinkThroughputLimit", throughput_limit); - } - else - { - // Set Throughput to maximum - //===================================== - setMaxInt(node_map_, "DeviceLinkThroughputLimit"); - } -} - void Camera::setNewConfiguration(const SpinnakerConfig& config, const uint32_t& level) { try @@ -192,8 +178,13 @@ void Camera::setNewConfiguration(const SpinnakerConfig& config, const uint32_t& setProperty(node_map_, "AutoExposureLightingMode", config.auto_exposure_lighting_mode); } - setDeviceLinkThroughput(config.device_link_throughput_limit, config.device_link_throughput_limit_enable); - setGigEPacketSize(config.camera_packet_size); + Spinnaker::GenApi::CEnumerationPtr device_type_ptr = node_map_->GetNode("DeviceType"); + if (config.gige_parameter_enable) + { + // setDeviceLinkThroughput(config.device_link_throughput_limit); + setProperty(node_map_, "DeviceLinkThroughputLimit", config.device_link_throughput_limit); + setGigEPacketSize(config.camera_packet_size); + } } catch (const Spinnaker::Exception& e) {