From 260502b617664527dc3327231b0c1a472b355c5d Mon Sep 17 00:00:00 2001 From: Chao Qu Date: Thu, 4 Sep 2014 15:49:37 -0400 Subject: [PATCH] finally compiles --- CMakeLists.txt | 19 +- cfg/FlirGigeDyn.cfg | 2 +- include/flir_gige/flir_gige.h | 29 +-- include/flir_gige/flir_gige_ros.h | 3 +- include/flir_gige/planck.h | 11 +- nodelet_plugins.xml | 2 +- src/flir_gige/flir_gige.cpp | 380 ++++++++++++------------------ src/flir_gige/flir_gige_node.cpp | 67 ++---- src/flir_gige/flir_gige_ros.cpp | 8 +- 9 files changed, 209 insertions(+), 312 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8a12979..5eda687 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -14,7 +14,7 @@ else() endif() find_package(catkin REQUIRED COMPONENTS - roscpp nodelet camera_base cv_brdige + roscpp nodelet camera_base cv_bridge ) find_package(OpenCV) @@ -31,8 +31,8 @@ catkin_package( # DEPENDS system_lib ) -include_directories(include) include_directories( + include ${catkin_INCLUDE_DIRS} ${EBUS_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} @@ -49,6 +49,14 @@ target_link_libraries(${PROJECT_NAME} ${EBUS_LIBRARIES} ) +# node +add_executable(${PROJECT_NAME}_node + src/flir_gige/flir_gige_main.cpp + ) +target_link_libraries(${PROJECT_NAME}_node + ${PROJECT_NAME} + ) + ## Add cmake target dependencies of the executable/library add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} @@ -57,10 +65,3 @@ add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ) -# node -add_executable(${PROJECT_NAME}_node - src/flir_gige/flir_gige_main.cpp - ) -target_link_libraries(${PROJECT_NAME}_node - ${PROJECT_NAME} - ) diff --git a/cfg/FlirGigeDyn.cfg b/cfg/FlirGigeDyn.cfg index d2adcc6..300b5a7 100755 --- a/cfg/FlirGigeDyn.cfg +++ b/cfg/FlirGigeDyn.cfg @@ -8,4 +8,4 @@ gen = ParameterGenerator() gen.add("fps", double_t, 0, "frame per second", 20, 1, 60) gen.add("raw", bool_t, 0, "Raw 14-bit data", False) -exit(gen.generate(PACKAGE, "flir_gige", "FlirDyn")) +exit(gen.generate(PACKAGE, "flir_gige", "FlirGigeDyn")) diff --git a/include/flir_gige/flir_gige.h b/include/flir_gige/flir_gige.h index 06ae456..e8c695c 100644 --- a/include/flir_gige/flir_gige.h +++ b/include/flir_gige/flir_gige.h @@ -14,6 +14,7 @@ #include #include +#include #include "flir_gige/planck.h" @@ -32,36 +33,33 @@ class FlirGige { FlirGige(const std::string &ip_address); const std::string &ip_address() const { return ip_address_; } - const std::string &display_id() const { return display_di_; } - - void Connect(); - - void Start(); - - void Stop(); - - void Disconnect(); + const std::string &display_id() const { return display_id_; } + bool ready() const { return ready_; } void Configure(FlirGigeDynConfig &config); + bool GrabImage(sensor_msgs::Image &image_msg); private: using PvDevicePtr = std::unique_ptr; using PvStreamPtr = std::unique_ptr; using PvPipelinePtr = std::unique_ptr; - void FindDevice(const std::string &ip); + bool FindDevice(const std::string &ip); std::string AvailableDevice() const; + + void Connect(); + void Disconnect(); void ConnectDevice(); void OpenStream(); void ConfigureStream(); void CreatePipeline(); void StartAcquisition(); void StopAcquisition(); - void AcquireImages(); - void LabeledOutput(const std::string &msg) const; - double GetSpotPixel(const cv::Mat &image) const; - void SetPixelFormat(int bit); + void SetAoi(int *width, int *height); +// void SetPixelFormat(int bit); + +// double GetSpotPixel(const cv::Mat &image) const; std::string ip_address_; std::string display_id_; @@ -70,6 +68,9 @@ class FlirGige { PvDevicePtr device_; PvStreamPtr stream_; PvPipelinePtr pipeline_; + PvGenParameterArray *param_array_; + bool ready_; + bool raw_; }; } // namespace flir_gige diff --git a/include/flir_gige/flir_gige_ros.h b/include/flir_gige/flir_gige_ros.h index 0cb36e1..8302503 100644 --- a/include/flir_gige/flir_gige_ros.h +++ b/include/flir_gige/flir_gige_ros.h @@ -11,7 +11,8 @@ class FlirGigeRos : public CameraRosBase { FlirGigeRos(const ros::NodeHandle& nh) : CameraRosBase{nh}, flir_gige_{identifier()} {} - virtual bool Grab(const sensor_msgs::ImagePtr* image_msg) override; + FlirGige& camera() { return flir_gige_; } + virtual bool Grab(const sensor_msgs::ImagePtr& image_msg) override; private: FlirGige flir_gige_; diff --git a/include/flir_gige/planck.h b/include/flir_gige/planck.h index 6824ab9..1b3405d 100644 --- a/include/flir_gige/planck.h +++ b/include/flir_gige/planck.h @@ -9,22 +9,21 @@ namespace flir_gige { * @brief The Planck constants from flir thermal camera */ struct Planck { - Planck() = default; - Planck(const double B, const double F, const double O, const double R) - : B{B}, F{F}, O{O}, R{R} {} + Planck() : B{0}, F{0}, O{0}, R{0} {} + Planck(double B, double F, double O, double R) : B{B}, F{F}, O{O}, R{R} {} double B; double F; double O; double R; - static const double kT0{273.15}; ///< Kelvin at 0 celcius + static constexpr double kT0{273.15}; ///< Kelvin at 0 celcius /** * @brief CelsiusToRaw Convert celsius to 16-bit raw data * @param t Celcius * @return raw data */ - inline int CelsiusToRaw(const double t) const { + int CelsiusToRaw(const double t) const { return R / (std::exp(B / (t + kT0)) - F) + O; } @@ -33,7 +32,7 @@ struct Planck { * @param s Raw data * @return temperature in celsius */ - inline double RawToCelsius(const int s) const { + double RawToCelsius(const int s) const { return B / std::log(R / (s - O) + F) - kT0; } }; diff --git a/nodelet_plugins.xml b/nodelet_plugins.xml index f6c875e..6fcda10 100644 --- a/nodelet_plugins.xml +++ b/nodelet_plugins.xml @@ -1,5 +1,5 @@ - This is a nodelet for the Flir thermal camera diff --git a/src/flir_gige/flir_gige.cpp b/src/flir_gige/flir_gige.cpp index f6456bd..4d550c9 100644 --- a/src/flir_gige/flir_gige.cpp +++ b/src/flir_gige/flir_gige.cpp @@ -1,8 +1,11 @@ #include "flir_gige/flir_gige.h" +#include + #include #include +#include #include #include @@ -10,14 +13,18 @@ namespace flir_gige { FlirGige::FlirGige(const std::string &ip_address) - : ip_address{ip_address}, dinfo_{nullptr} { + : ip_address_{ip_address}, dinfo_{nullptr}, ready_{false}, raw_{false} { // Find all devices on the network const PvResult result = system_.Find(); if (!result.IsOK()) { throw std::runtime_error(std::string("PvSystem::Find Error: ") + result.GetCodeString().GetAscii()); } - FindDevice(ip_address); + if (!FindDevice(ip_address)) { + throw std::runtime_error(ip_address + + " not found. Available IP Address(es): " + + AvailableDevice()); + } } void FlirGige::Connect() { @@ -28,31 +35,24 @@ void FlirGige::Connect() { } void FlirGige::Disconnect() { - // Release all the recourses we hold pipeline_.reset(); stream_.reset(); device_.reset(); + param_array_ = nullptr; } void FlirGige::Configure(FlirGigeDynConfig &config) { - SetPixelFormat(config.raw); -} - -void FlirGige::Start() { - StartAcquisition(); - // Set acquire to true + // SetPixelFormat(config.raw); } -void FlirGige::Stop() { StopAcquisition(); } - -void FlirGige::FindDevice(const std::string &ip) { - const auto interface_cnt = system_.GetInterfaceCount(); +bool FlirGige::FindDevice(const std::string &ip) { + const int interface_cnt = system_.GetInterfaceCount(); ROS_INFO_STREAM("Interface count:" << interface_cnt); // Go through all interfaces, but we only care about network interface // For other interfaces such as usb, refer to sample code DeviceFinder.cpp std::vector dinfo_gev_vec; - for (decltype(interface_cnt) i = 0; i < interface_cnt; ++i) { + for (int i = 0; i < interface_cnt; ++i) { // Get pointer to the interface const PvInterface *interface = system_.GetInterface(i); // Is it a PvNetworkAdapter? @@ -60,8 +60,8 @@ void FlirGige::FindDevice(const std::string &ip) { if (nic) { ROS_INFO("Interface: %s", interface->GetDisplayID().GetAscii()); // Go through all the devices attached to the network interface - const auto dev_cnt = interface->GetDeviceCount(); - for (decltype(dev_cnt) j = 0; j < dev_cnt; ++j) { + const int dev_cnt = interface->GetDeviceCount(); + for (int j = 0; j < dev_cnt; ++j) { const PvDeviceInfo *dinfo = interface->GetDeviceInfo(j); // Is it a GigE Vision device? const auto *dinfo_gev = dynamic_cast(dinfo); @@ -74,9 +74,7 @@ void FlirGige::FindDevice(const std::string &ip) { } // Check GigE devices found on network adaptor - if (dinfo_gev_vec.empty()) { - throw std::runtime_error("FlirGige: No device found."); - } + if (dinfo_gev_vec.empty()) return false; // Try finding the device with the correct ip address const auto it = std::find_if(dinfo_gev_vec.cbegin(), dinfo_gev_vec.cend(), @@ -84,102 +82,69 @@ void FlirGige::FindDevice(const std::string &ip) { return ip == dinfo->GetIPAddress().GetAscii(); }); - if (it == dinfo_gev_vec.end()) { - // Did not find device with given ip address - std::ostringstream error_msg; - error_msg << "FlirGige: Device not found. Available IP Address:"; - for (const PvDeviceInfoGEV *dinfo : dinfo_gev_vec) { - error_msg << " " << dinfo->GetIPAddress().GetAscii(); - } - throw std::runtime_error( - "FlirGige: Device not found. Available IP Address: " + - AvailableDevice()); - } else { - // Found device with given ip address - PvDeviceInfoGEV *dinfo_gev = (*it); - display_id_ = std::string(dinfo_gev->GetDisplayID().GetAscii()); - ROS_INFO("Found device: %s", display_id().c_str()); - if (dinfo_gev->IsConfigurationValid()) { - // Try connect and disconnect to verify - dinfo_ = dinfo_gev; - PvResult result; - ROS_INFO("--?-- %s", dinfo_display_id.c_str()); - // Creates and connects the device controller - PvDevice *device = PvDevice::CreateAndConnect(dinfo_, &result); - if (result.IsOK()) { - ROS_INFO("-->-- %s", display_id()); - ROS_INFO("-->-- %s", display_id()); - PvDevice::Free(device); - } else { - // Maybe throw an exception here? - throw std::runtime_error("FlirGige: Unable to connect to " + - display_id()); - } - } - } + if (it == dinfo_gev_vec.end()) return false; + // Found device with given ip address + const PvDeviceInfoGEV *dinfo_gev = *it; + display_id_ = std::string(dinfo_gev->GetDisplayID().GetAscii()); + ROS_INFO("Found device: %s", display_id().c_str()); + + if (!dinfo_gev->IsConfigurationValid()) return false; + // Try connect and disconnect to verify + dinfo_ = dinfo_gev; + PvResult result; + ROS_INFO("--?-- %s", display_id().c_str()); + + // Creates and connects the device controller + PvDevice *device = PvDevice::CreateAndConnect(dinfo_, &result); + if (!result.IsOK()) return false; + ROS_INFO("-->-- %s", display_id().c_str()); + ROS_INFO("--x-- %s", display_id().c_str()); + PvDevice::Free(device); + return true; } std::string FlirGige::AvailableDevice() const {} void FlirGige::ConnectDevice() { - std::cout << label_ << "Connecting to " << dinfo_->GetDisplayID().GetAscii(); + ROS_INFO("Connecting to %s", display_id().c_str()); PvResult result; // Use a unique_ptr to manage device resource device_.reset(PvDevice::CreateAndConnect(dinfo_, &result)); - - if (result.IsOK()) { - std::cout << " ... Done." << std::endl; - } else { - std::ostringstream error_msg; - error_msg << "FlirGige: Unable to connect to " - << dinfo_->GetDisplayID().GetAscii(); - throw std::runtime_error(error_msg.str()); + if (!result.IsOK()) { + throw std::runtime_error("Unable to connect to " + display_id()); } + param_array_ = device_->GetParameters(); } void FlirGige::OpenStream() { - std::cout << label_ << "Opening stream to " - << dinfo_->GetDisplayID().GetAscii(); + ROS_INFO("Openning stream to %s", display_id().c_str()); PvResult result; // Use a unique_ptr to manage stream resource stream_.reset(PvStream::CreateAndOpen(dinfo_->GetConnectionID(), &result)); - - if (stream_) { - std::cout << " ... Done. " << std::endl; - } else { - // Maybe a function for throw exception? - std::ostringstream error_msg; - error_msg << "FlirGige: Unable to stream form " - << dinfo_->GetDisplayID().GetAscii(); - throw std::runtime_error(error_msg.str()); + if (!stream_) { + throw std::runtime_error("Unable to stream from " + display_id()); } } void FlirGige::ConfigureStream() { // If this is a GigE Vision devie, configure GigE Vision specific parameters auto *device_gev = dynamic_cast(device_.get()); - if (device_gev) { - LabeledOutput("Configuring gev stream"); - auto *stream_gev = static_cast(stream_.get()); - // Negotiate packet size - device_gev->NegotiatePacketSize(); - // Configure device streaming destination - device_gev->SetStreamDestination(stream_gev->GetLocalIPAddress(), - stream_gev->GetLocalPort()); - } else { - std::ostringstream error_msg; - error_msg << "FlirGige: This is not a GigE Vision device " - << dinfo_->GetDisplayID().GetAscii(); - throw std::runtime_error(error_msg.str()); + if (!device_gev) { + throw std::runtime_error("Not a GigE vision device " + display_id()); } + ROS_INFO("Configuring gev stream"); + auto *stream_gev = static_cast(stream_.get()); + // Negotiate packet size + device_gev->NegotiatePacketSize(); + // Configure device streaming destination + device_gev->SetStreamDestination(stream_gev->GetLocalIPAddress(), + stream_gev->GetLocalPort()); } void FlirGige::CreatePipeline() { - LabeledOutput("Creating pipeline"); - // Create the PvPipeline object + ROS_INFO("Creating pipeline"); pipeline_.reset(new PvPipeline(stream_.get())); - // Reading payload size from device - auto payload_size = device_->GetPayloadSize(); + const auto payload_size = device_->GetPayloadSize(); // Set the Buffer count and the Buffer size // BufferCount should be at least 4 pipeline_->SetBufferCount(4); @@ -187,175 +152,130 @@ void FlirGige::CreatePipeline() { } void FlirGige::StartAcquisition() { - PvGenParameterArray *device_params = device_->GetParameters(); // Note: the pipeline must be initialized before we start acquisition - std::cout << label_ << "Starting pipeline ... "; + ROS_INFO("Starting acquisition"); pipeline_->Start(); - // Enable streaming - std::cout << "Enabling streaming ... "; device_->StreamEnable(); - // Start acquisition - std::cout << "Start acquisition ... "; - // Get device parameters need to control streaming - device_params->ExecuteCommand("AcquisitionStart"); - std::cout << "Done" << std::endl; + param_array_->ExecuteCommand("AcquisitionStart"); } void FlirGige::StopAcquisition() { // Get device parameters need to control streaming - PvGenParameterArray *device_params = device_->GetParameters(); - // Stop image acquisition - std::cout << label_ << "Stop acquisition ... "; - device_params->ExecuteCommand("AcquisitionStop"); - // Get controller out of streaming - std::cout << "Disabling streaming ... "; + ROS_INFO("Stop acquisition"); + param_array_->ExecuteCommand("AcquisitionStop"); device_->StreamDisable(); - // Stop pipeline - std::cout << "Stoping pipeline ... "; pipeline_->Stop(); - std::cout << "Done" << std::endl; } -void FlirGige::AcquireImages() { - // Get device parameters need to control streaming - PvGenParameterArray *device_params = device_->GetParameters(); - int64_t width{0}, height{0}; - int64_t R{0}; - double F{0.0}, B{0.0}, O{0.0}; - double spot{0}; - device_params->GetIntegerValue("Width", width); - device_params->GetIntegerValue("Height", height); - device_params->GetIntegerValue("R", R); - device_params->GetFloatValue("F", F); - device_params->GetFloatValue("B", B); - device_params->GetFloatValue("O", O); - std::cout << label_ << "R: " << R << " F: " << F << " B: " << B << " O: " << O - << std::endl; - const Planck planck(B, F, O, R); - - bool skip_next_frame = false; +bool FlirGige::GrabImage(sensor_msgs::Image &image_msg) { + static bool skip_next_frame = false; // Start loop for acquisition - while (acquire_) { - PvBuffer *buffer; - PvResult op_result; - - // Skip next frame when operation is not ok - if (skip_next_frame) { - skip_next_frame = false; - sleep(1); - continue; - } - - // Retrieve next buffer - PvResult result = pipeline_->RetrieveNextBuffer(&buffer, 1000, &op_result); + PvBuffer *buffer; + PvResult op_result; - // Failed to retrieve buffer - if (result.IsFailure()) { - LabeledOutput("Retrieve buffer failure"); - continue; - } + // Skip next frame when operation is not ok + if (skip_next_frame) { + skip_next_frame = false; + sleep(1); + } - // Operation not ok, need to return buffer back to pipeline - if (op_result.IsFailure()) { - skip_next_frame = true; - LabeledOutput("Non Ok operation result"); - // Release the buffer back to the pipeline - pipeline_->ReleaseBuffer(buffer); - continue; - } + // Retrieve next buffer + PvResult result = pipeline_->RetrieveNextBuffer(&buffer, 1000, &op_result); - // Buffer is not an image - if ((buffer->GetPayloadType()) != PvPayloadTypeImage) { - LabeledOutput("Buffer does not contain image"); - pipeline_->ReleaseBuffer(buffer); - continue; - } + // Failed to retrieve buffer + if (result.IsFailure()) { + ROS_INFO("Retrieve buffer failure"); + return false; + } - // Get image specific buffer interface - PvImage *image = buffer->GetImage(); - memcpy(image_raw_.data, image->GetDataPointer(), image->GetImageSize()); - // Use the image for temperature calculation only in raw data mode - if (raw_) { - device_params->GetFloatValue("Spot", spot); - double t = planck.RawToCelsius(GetSpotPixel(image_raw_)); - use_temperature(std::make_pair(spot, t)); - use_image(image_raw_, planck); - } else { - // For display purpose in non raw data mode - if (color_) { - cv::Mat image_color; - cv::applyColorMap(image_raw_, image_color, cv::COLORMAP_JET); - use_image(image_color, planck); - } else { - use_image(image_raw_, planck); - } - } + // Operation not ok, need to return buffer back to pipeline + if (op_result.IsFailure()) { + skip_next_frame = true; + ROS_INFO("Non Ok operation result"); // Release the buffer back to the pipeline pipeline_->ReleaseBuffer(buffer); + return false; } -} -void FlirGige::SetAoi(const int width, const int height) { - PvGenParameterArray *device_params = device_->GetParameters(); - // Get width and height parameter - auto *width_param = dynamic_cast(device_params->Get("Width")); - auto *height_param = - dynamic_cast(device_params->Get("Height")); - // Get current width and height - int64_t current_width = 0; - int64_t current_height = 0; - width_param->GetValue(current_width); - height_param->GetValue(current_height); - // Check to see if it's necessary to change width and height - if (current_width != width) { - if (width_param->SetValue(width).IsFailure()) { - LabeledOutput("failed to set width"); - } + // Buffer is not an image + if ((buffer->GetPayloadType()) != PvPayloadTypeImage) { + ROS_INFO("Buffer does not contain image"); + pipeline_->ReleaseBuffer(buffer); + return false; } - if (current_height != height) { - if (height_param->SetValue(height).IsFailure()) { - LabeledOutput("failed to set height"); - } + + // Get image specific buffer interface + PvImage *image = buffer->GetImage(); + + // Get device parameters need to control streaming + int64_t width{0}, height{0}; + param_array_->GetIntegerValue("Width", width); + param_array_->GetIntegerValue("Height", height); + + // Assemble image msg + image_msg.height = height; + image_msg.width = width; + image_msg.step = image_msg.width; + if (raw_) { + image_msg.encoding = sensor_msgs::image_encodings::MONO16; + } else { + image_msg.encoding = sensor_msgs::image_encodings::MONO8; } -} -void FlirGige::SetPixelFormat(BitSize bit) { - PvGenParameterArray *device_params = device_->GetParameters(); - int64_t height = 0, width = 0; - device_params->GetIntegerValue("Width", width); - device_params->GetIntegerValue("Height", height); - // Set digital output and pixel format - if (bit == BIT8BIT) { - device_params->SetEnumValue("PixelFormat", PvPixelMono8); - device_params->SetEnumValue("DigitalOutput", static_cast(bit)); - image_raw_.create(cv::Size(width, height), CV_8UC1); - raw_ = false; - } else if (bit == BIT14BIT) { - device_params->SetEnumValue("PixelFormat", PvPixelMono14); - device_params->SetEnumValue("DigitalOutput", static_cast(bit)); - image_raw_.create(cv::Size(width, height), CV_16UC1); - raw_ = true; + const size_t data_size = image->GetImageSize(); + if (image_msg.data.size() != data_size) { + image_msg.data.resize(data_size); } - // Verify setting - PvString digital_output; - device_params->GetEnumValue("DigitalOutput", digital_output); - std::cout << label_ << "Width: " << width << " Height: " << height - << " Bit: " << digital_output.GetAscii() << std::endl; -} + memcpy(&image_msg.data[0], image->GetDataPointer(), image->GetImageSize()); -double FlirGige::GetSpotPixel(const cv::Mat &image) const { - auto c = image.cols / 2; - auto r = image.rows / 2; - auto s1 = image.at(r - 1, c - 1); - auto s2 = image.at(r - 1, c); - auto s3 = image.at(r, c - 1); - auto s4 = image.at(r, c); - return static_cast(s1 / 4 + s2 / 4 + s3 / 4 + s4 / 4); + // Release the buffer back to the pipeline + pipeline_->ReleaseBuffer(buffer); + return true; } -void FlirGige::LabeledOutput(const std::string &msg) const { - std::cout << label_ << msg << std::endl; +// This function is not intended to be used +void FlirGige::SetAoi(int *width, int *height) { + // Get current width and height + int64_t curr_width = 0; + int64_t curr_height = 0; + param_array_->GetIntegerValue("Width", curr_width); + param_array_->GetIntegerValue("Height", curr_height); + // Check to see if it's necessary to change width and height + if (curr_width != *width) { + param_array_->SetIntegerValue("Width", *width); + } + if (curr_height != *height) { + param_array_->SetIntegerValue("Height", *height); + } } +// void FlirGige::SetPixelFormat(int bit) { +// PvGenParameterArray *device_params = device_->GetParameters(); +// int64_t height = 0, width = 0; +// device_params->GetIntegerValue("Width", width); +// device_params->GetIntegerValue("Height", height); +// // Set digital output and pixel format +// if (bit == BIT8BIT) { +// device_params->SetEnumValue("PixelFormat", PvPixelMono8); +// device_params->SetEnumValue("DigitalOutput", static_cast(bit)); +// } else if (bit == BIT14BIT) { +// device_params->SetEnumValue("PixelFormat", PvPixelMono14); +// device_params->SetEnumValue("DigitalOutput", static_cast(bit)); +// } +// // Verify setting +// PvString digital_output; +// device_params->GetEnumValue("DigitalOutput", digital_output); +//} + +// double FlirGige::GetSpotPixel(const cv::Mat &image) const { +// auto c = image.cols / 2; +// auto r = image.rows / 2; +// auto s1 = image.at(r - 1, c - 1); +// auto s2 = image.at(r - 1, c); +// auto s3 = image.at(r, c - 1); +// auto s4 = image.at(r, c); +// return static_cast(s1 / 4 + s2 / 4 + s3 / 4 + s4 / 4); +//} + } // namespace flir_gige diff --git a/src/flir_gige/flir_gige_node.cpp b/src/flir_gige/flir_gige_node.cpp index 2950097..c2c6067 100644 --- a/src/flir_gige/flir_gige_node.cpp +++ b/src/flir_gige/flir_gige_node.cpp @@ -1,58 +1,16 @@ #include "flir_gige/flir_gige_node.h" -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "flir_gige/gige_camera.h" - namespace flir_gige { -using sensor_msgs::CameraInfo; -using sensor_msgs::CameraInfoPtr; -using camera_info_manager::CameraInfoManager; - -FlirGige::FlirGige(const ros::NodeHandle &nh) - : nh_{nh}, - it_{nh}, - pub_camera_{it_.advertiseCamera("image_raw", 1)}, - pub_temperature_{nh_.advertise("spot", 1)}, - server_{nh} { - // Get ros parameteres - double fps; - nh_.param("fps", fps, 20.0); - ROS_ASSERT_MSG(fps > 0, "FlirGige: fps must be greater than 0"); - rate_.reset(new ros::Rate(fps)); - - // Setup up camera info manager - nh_.param("frame_id", camera_name_, std::string("flir_a5")); - std::string calib_url; - nh_.param("calib_url", calib_url, ""); - - cinfo_manager_.reset(new CameraInfoManager(nh_, camera_name_, calib_url)); - - // Setup camera dynamic reconfigure callback - server_.setCallback(boost::bind(&FlirGige::ConfigCb, this, _1, _2)); - - // Create a camera - std::string ip_address; - nh_.param("ip_address", ip_address, std::string("")); - gige_camera_.reset(new FlirGige(ip_address)); - gige_camera_->use_image = - std::bind(&FlirGige::PublishImage, this, std::placeholders::_1, - std::placeholders::_2); - gige_camera_->use_temperature = - std::bind(&FlirGige::PublishTemperature, this, std::placeholders::_1); +void FlirGigeNode::Setup(FlirGigeDynConfig &config) { + flir_gige_ros_.set_fps(config.fps); + flir_gige_ros_.camera().Configure(config); } +//FlirGige::FlirGige(const ros::NodeHandle &nh) +// pub_temperature_{nh_.advertise("spot", 1)} + +/* void FlirGige::Run() { GigeConfig config; nh_.param("color", config.color, config.color); @@ -61,12 +19,16 @@ void FlirGige::Run() { gige_camera_->Configure(config); gige_camera_->Start(); } +*/ +/* void FlirGige::End() { gige_camera_->Stop(); gige_camera_->Disconnect(); } +*/ +/* void FlirGige::PublishImage(const cv::Mat &image, const Planck &planck) { std_msgs::Header header; header.stamp = ros::Time::now(); @@ -83,7 +45,9 @@ void FlirGige::PublishImage(const cv::Mat &image, const Planck &planck) { pub_camera_.publish(image_, cinfo_); rate_->sleep(); } +*/ +/* void FlirGige::PublishTemperature(const std::pair &spot) { // Construct a temperature mesage sensor_msgs::Temperature temperature; @@ -93,7 +57,9 @@ void FlirGige::PublishTemperature(const std::pair &spot) { temperature.variance = spot.second; pub_temperature_.publish(temperature); } +*/ +/* std::string FlirGige::GetImageEncoding(const cv::Mat &image) const { std::string encoding; switch (image.type()) { @@ -111,7 +77,9 @@ std::string FlirGige::GetImageEncoding(const cv::Mat &image) const { } return encoding; } +*/ +/* void FlirGige::ConfigCb(FlirDynConfig &config, int level) { // Do nothing when first starting if (level < 0) { @@ -133,5 +101,6 @@ void FlirGige::ConfigCb(FlirDynConfig &config, int level) { gige_camera_->Configure(gige_config); gige_camera_->Start(); } +*/ } // namespace flir_gige diff --git a/src/flir_gige/flir_gige_ros.cpp b/src/flir_gige/flir_gige_ros.cpp index 5519b57..671361b 100644 --- a/src/flir_gige/flir_gige_ros.cpp +++ b/src/flir_gige/flir_gige_ros.cpp @@ -2,5 +2,11 @@ namespace flir_gige { +bool FlirGigeRos::Grab(const sensor_msgs::ImagePtr &image_msg) { + // Add expose time to current time stamp + // image_msg->header.stamp += ros::Duration(bluefox2_.expose_us() * 1e-6); + // return bluefox2_.GrabImage(*image_msg); + return false; +} -} //namespace flir_gige +} // namespace flir_gige