diff --git a/flir_camera_description/CHANGELOG.rst b/flir_camera_description/CHANGELOG.rst new file mode 100644 index 00000000..bedc752f --- /dev/null +++ b/flir_camera_description/CHANGELOG.rst @@ -0,0 +1,9 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package pointgrey_camera_description +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.0 (2021-11-10) +------------------- +* Initial Release +* Contributors: Luis Camero + diff --git a/flir_camera_description/CMakeLists.txt b/flir_camera_description/CMakeLists.txt new file mode 100644 index 00000000..ff01bf5d --- /dev/null +++ b/flir_camera_description/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 2.8.3) +project(flir_camera_description) + +find_package(catkin REQUIRED) + +catkin_package() + +install(DIRECTORY launch meshes rviz urdf + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) diff --git a/flir_camera_description/meshes/flir_blackflys.stl b/flir_camera_description/meshes/flir_blackflys.stl new file mode 100644 index 00000000..ede2c8d2 Binary files /dev/null and b/flir_camera_description/meshes/flir_blackflys.stl differ diff --git a/flir_camera_description/package.xml b/flir_camera_description/package.xml new file mode 100644 index 00000000..d4a65fbb --- /dev/null +++ b/flir_camera_description/package.xml @@ -0,0 +1,15 @@ + + + flir_camera_description + 0.1.0 + URDF descriptions for Flir cameras + + Luis Camero + + BSD + + catkin + robot_state_publisher + urdf + xacro + diff --git a/flir_camera_description/urdf/flir_blackflys.urdf.xacro b/flir_camera_description/urdf/flir_blackflys.urdf.xacro new file mode 100644 index 00000000..02b6b736 --- /dev/null +++ b/flir_camera_description/urdf/flir_blackflys.urdf.xacro @@ -0,0 +1,108 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Grey + + ${fps} + + 0 0 0 0 -1.5707 1.5707 + ${hfov} + + ${width} + ${height} + ${format} + + + ${near} + ${far} + + + gaussian + 0.0 + 0.007 + + + + + true + ${fps} + ${name} + image_raw + camera_info + ${frame}_optical + 0.07 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + diff --git a/spinnaker_camera_driver/cfg/Spinnaker.cfg b/spinnaker_camera_driver/cfg/Spinnaker.cfg index bd2e1aac..ab976ec0 100755 --- a/spinnaker_camera_driver/cfg/Spinnaker.cfg +++ b/spinnaker_camera_driver/cfg/Spinnaker.cfg @@ -78,18 +78,21 @@ gen = ParameterGenerator() gen.add("acquisition_frame_rate", double_t, SensorLevels.RECONFIGURE_RUNNING, "User controlled acquisition frame rate in Hertz (frames per second).", 10, 0, 120) gen.add("acquisition_frame_rate_enable", bool_t, SensorLevels.RECONFIGURE_RUNNING, "Enables manual (True) and automatic (False) control of the aquisition frame rate", False) - # Set Exposure # Note: For the Auto Exposure feature, gain and/or exposure time must be set to Once or Continuous. +capture_modes = gen.enum([gen.const("AutoOff", str_t, "Off", ""), + gen.const("AutoOnce", str_t, "Once", ""), + gen.const("AutoContinuous", str_t, "Continuous", "")], + "Automatic mode: Off, Once, or Continuous.") gen.add("exposure_mode", str_t, SensorLevels.RECONFIGURE_STOP, "Sets the operation mode of the Exposure (Timed or TriggerWidth).", "Timed") -gen.add("exposure_auto", str_t, SensorLevels.RECONFIGURE_RUNNING, "Sets the automatic exposure mode to: 'Off', 'Once' or 'Continuous'", "Continuous") +gen.add("exposure_auto", str_t, SensorLevels.RECONFIGURE_RUNNING, "Sets the automatic exposure mode to: 'Off', 'Once' or 'Continuous'", "Continuous", edit_method=capture_modes) gen.add("exposure_time", double_t, SensorLevels.RECONFIGURE_RUNNING, "Exposure time in microseconds when Exposure Mode is Timed and Exposure Auto is not Continuous.", 100.0, 0.0, 3000000.0) gen.add("auto_exposure_time_upper_limit", double_t, SensorLevels.RECONFIGURE_RUNNING, "Upper Limit on Shutter Speed.", 5000, 0.0, 3000000.0) # Gain Settings gen.add("gain_selector", str_t, SensorLevels.RECONFIGURE_RUNNING, "Selects which gain to control. The All selection is a total amplification across all channels.", "All") -gen.add("auto_gain", str_t, SensorLevels.RECONFIGURE_RUNNING, "Gain state control. (Off, Once, Continuous)", "Continuous") +gen.add("auto_gain", str_t, SensorLevels.RECONFIGURE_RUNNING, "Gain state control. (Off, Once, Continuous)", "Continuous", edit_method=capture_modes) gen.add("gain", double_t, SensorLevels.RECONFIGURE_RUNNING, "Controls the amplification of the video signal in dB.", 0, -10, 30) # Pan and Tilt not in Spinnaker Driver @@ -113,7 +116,7 @@ gen.add("gamma", double_t, SensorLevels.RECON # White Balance -gen.add("auto_white_balance", str_t, SensorLevels.RECONFIGURE_RUNNING, "White Balance compensates for color shifts caused by different lighting conditions.", "Off") +gen.add("auto_white_balance", str_t, SensorLevels.RECONFIGURE_RUNNING, "White Balance compensates for color shifts caused by different lighting conditions.", "Off", edit_method=capture_modes) gen.add("white_balance_blue_ratio", double_t, SensorLevels.RECONFIGURE_RUNNING, "White balance blue component.", 800, 0, 1023) gen.add("white_balance_red_ratio", double_t, SensorLevels.RECONFIGURE_RUNNING, "White balance red component.", 550, 0, 1023) @@ -174,7 +177,8 @@ codings = gen.enum([gen.const("Mono8", str_t, "Mono8", ""), "Image Color Coding: Format of the pixel provided by the camera.") -gen.add("image_format_color_coding", str_t, SensorLevels.RECONFIGURE_STOP, "Image Color coding", "Mono8", edit_method = codings) +gen.add("image_format_color_coding", str_t, SensorLevels.RECONFIGURE_STOP, "Image Color coding", "Mono8", edit_method = codings) +gen.add("isp_enable", bool_t, SensorLevels.RECONFIGURE_STOP, "Controls whether the image processing core is used for optional pixel format mode", True) # Trigger parameters diff --git a/spinnaker_camera_driver/include/spinnaker_camera_driver/diagnostics.h b/spinnaker_camera_driver/include/spinnaker_camera_driver/diagnostics.h index b9fc5f8c..8a61b0a2 100644 --- a/spinnaker_camera_driver/include/spinnaker_camera_driver/diagnostics.h +++ b/spinnaker_camera_driver/include/spinnaker_camera_driver/diagnostics.h @@ -45,7 +45,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "spinnaker_camera_driver/diagnostics.h" #include #include +#include #include +#include #include #include @@ -56,7 +58,7 @@ namespace spinnaker_camera_driver class DiagnosticsManager { public: - DiagnosticsManager(const std::string name, const std::string serial, std::shared_ptr const& pub); + DiagnosticsManager(const std::string name, const std::string serial, std::shared_ptr const& pub, ros::NodeHandle& nh); ~DiagnosticsManager(); /*! @@ -95,6 +97,8 @@ class DiagnosticsManager std::pair operational = std::make_pair(0.0, 0.0), float lower_bound = 0, float upper_bound = 0); + void addAnalyzers(); + private: /* * diagnostic_params is aData Structure to represent a parameter and its @@ -126,6 +130,8 @@ class DiagnosticsManager // constuctor parameters std::string camera_name_; std::string serial_number_; + ros::NodeHandle nh_; + std::shared_ptr bond_ = nullptr; std::shared_ptr diagnostics_pub_; // vectors to keep track of the items to publish diff --git a/spinnaker_camera_driver/launch/camera.launch b/spinnaker_camera_driver/launch/camera.launch index 65005225..9745dc90 100644 --- a/spinnaker_camera_driver/launch/camera.launch +++ b/spinnaker_camera_driver/launch/camera.launch @@ -29,23 +29,46 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI - + - + + + + + + + - +-> - + @@ -53,6 +76,30 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI + + + + + + + + + diff --git a/spinnaker_camera_driver/launch/diagnostics.launch b/spinnaker_camera_driver/launch/diagnostics.launch index 0696633b..839d4afc 100644 --- a/spinnaker_camera_driver/launch/diagnostics.launch +++ b/spinnaker_camera_driver/launch/diagnostics.launch @@ -29,43 +29,89 @@ ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSI + - + camera itself. Use the parameter 'control_frame_rate' to enable manual frame + rate control, and 'frame_rate' to set the frame rate value. --> + + + + + + + - +-> - + + + + + + + + + + + - - - + name="diagnostic_aggregator"/> diff --git a/spinnaker_camera_driver/src/camera.cpp b/spinnaker_camera_driver/src/camera.cpp index 5ce364b8..d17e6eab 100644 --- a/spinnaker_camera_driver/src/camera.cpp +++ b/spinnaker_camera_driver/src/camera.cpp @@ -231,6 +231,9 @@ void Camera::setImageControlFormats(const spinnaker_camera_driver::SpinnakerConf // Set Pixel Format setProperty(node_map_, "PixelFormat", config.image_format_color_coding); + + // Set ISP Enable + setProperty(node_map_, "IspEnable", config.isp_enable); } void Camera::setGain(const float& gain) diff --git a/spinnaker_camera_driver/src/diagnostics.cpp b/spinnaker_camera_driver/src/diagnostics.cpp index 18e8bfb2..90c9cce1 100644 --- a/spinnaker_camera_driver/src/diagnostics.cpp +++ b/spinnaker_camera_driver/src/diagnostics.cpp @@ -36,8 +36,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace spinnaker_camera_driver { DiagnosticsManager::DiagnosticsManager(const std::string name, const std::string serial, - std::shared_ptr const& pub) - : camera_name_(name), serial_number_(serial), diagnostics_pub_(pub) + std::shared_ptr const& pub, + ros::NodeHandle& nh) + : camera_name_(name), serial_number_(serial), diagnostics_pub_(pub), nh_(nh) { } @@ -72,45 +73,128 @@ void DiagnosticsManager::addDiagnostic(const Spinnaker::GenICam::gcstring name, float_params_.push_back(param); } +void DiagnosticsManager::addAnalyzers() +{ + // Get Namespace + std::string node_name = ros::this_node::getName().substr(1); + std::string node_namespace = ros::this_node::getNamespace(); + std::string node_prefix = ""; + std::string node_path; + std::string node_id = node_name; + + // Create "Fake" Namespace for Diagnostics + if(node_namespace == "/") + { + node_namespace = ros::this_node::getName(); + node_prefix = ros::this_node::getName() + "/"; + } + + // Sanitize Node ID + size_t pos = node_id.find("/"); + while(pos != std::string::npos) + { + node_id.replace(pos, 1, "_"); + pos = node_id.find("/"); + } + + // Sanitize Node Path + node_path = node_id; + pos = node_path.find("_"); + while(pos != std::string::npos) + { + node_path.replace(pos, 1, " "); + pos = node_path.find("_"); + } + + // GroupAnalyzer Parameters + if(!ros::param::has(node_prefix + "analyzers/spinnaker/path")) + { + ros::param::set(node_prefix + "analyzers/spinnaker/path", "Spinnaker"); + ros::param::set(node_prefix + "analyzers/spinnaker/type", "diagnostic_aggregator/AnalyzerGroup"); + } + + // Analyzer Parameters + std::string analyzerPath = node_prefix + "analyzers/spinnaker/analyzers/" + node_id; + if(!ros::param::has(analyzerPath + "/path")) + { + ros::param::set(analyzerPath + "/path", node_path); + ros::param::set(analyzerPath + "/type", "diagnostic_aggregator/GenericAnalyzer"); + ros::param::set(analyzerPath + "/startswith", node_name); + ros::param::set(analyzerPath + "/remove_prefix", node_name); + } + + // Bond to Diagnostics Aggregator + if(bond_ == nullptr) + { + bond_ = std::shared_ptr(new bond::Bond("/diagnostics_agg/bond" + node_namespace, node_namespace)); + } + else if(!bond_->isBroken()) + { + return; + } + bond_->setConnectTimeout(120); + + // Add Diagnostics + diagnostic_msgs::AddDiagnostics srv; + srv.request.load_namespace = node_namespace; + if(!ros::service::waitForService("/diagnostics_agg/add_diagnostics", 1000)) + { + return; + } + bond_->start(); + ros::service::call("/diagnostics_agg/add_diagnostics", srv); +} + template diagnostic_msgs::DiagnosticStatus DiagnosticsManager::getDiagStatus(const diagnostic_params& param, const T value) { + std::string node_name = ros::this_node::getName().substr(1); diagnostic_msgs::KeyValue kv; kv.key = param.parameter_name; kv.value = std::to_string(value); diagnostic_msgs::DiagnosticStatus diag_status; diag_status.values.push_back(kv); - diag_status.name = "Spinnaker " + Spinnaker::GenICam::gcstring(camera_name_.c_str()) + " " + param.parameter_name; + diag_status.name = node_name + ":" + std::string(param.parameter_name.c_str()); diag_status.hardware_id = serial_number_; // Determine status level if (!param.check_ranges || (value > param.operational_range.first && value <= param.operational_range.second)) { diag_status.level = 0; - diag_status.message = "OK"; + diag_status.message = "OK: " + std::string(param.parameter_name) + " performing in expected operational range."; } else if (value >= param.warn_range_lower && value <= param.warn_range_upper) { diag_status.level = 1; - diag_status.message = "WARNING"; + diag_status.message = "WARNING: " + std::string(param.parameter_name.c_str()) + " is not in expected operational range."; } else { diag_status.level = 2; - diag_status.message = "ERROR"; + diag_status.message = "ERROR: " + std::string(param.parameter_name.c_str()) + " is in critical operation range."; } + // Warning Range + kv.key = "Warning Range"; + kv.value = "[" + std::to_string(param.warn_range_lower) + ", " + std::to_string(param.warn_range_upper) + "]"; + diag_status.values.push_back(kv); + + // Operational Range + kv.key = "Operational Range"; + kv.value = "[" + std::to_string(param.operational_range.first) + ", " + std::to_string(param.operational_range.second) + "]"; + diag_status.values.push_back(kv); return diag_status; } void DiagnosticsManager::processDiagnostics(SpinnakerCamera* spinnaker) { + std::string node_name = ros::this_node::getName().substr(1); diagnostic_msgs::DiagnosticArray diag_array; // Manufacturer Info diagnostic_msgs::DiagnosticStatus diag_manufacture_info; - diag_manufacture_info.name = "Spinnaker " + camera_name_ + " Manufacture Info"; + diag_manufacture_info.name = node_name + ": Manufacture Info"; diag_manufacture_info.hardware_id = serial_number_; for (const std::string param : manufacturer_params_) diff --git a/spinnaker_camera_driver/src/nodelet.cpp b/spinnaker_camera_driver/src/nodelet.cpp index 02b44ebd..688b6ba1 100644 --- a/spinnaker_camera_driver/src/nodelet.cpp +++ b/spinnaker_camera_driver/src/nodelet.cpp @@ -347,30 +347,30 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet // frequencies. pnh.param("freq_tolerance", freq_tolerance, 0.1); int window_size; // Number of samples to consider in frequency - pnh.param("window_size", window_size, 100); + pnh.param("window_size", window_size, 30); double min_acceptable; // The minimum publishing delay (in seconds) before warning. Negative values mean future // dated messages. pnh.param("min_acceptable_delay", min_acceptable, 0.0); double max_acceptable; // The maximum publishing delay (in seconds) before warning. pnh.param("max_acceptable_delay", max_acceptable, 0.2); ros::SubscriberStatusCallback cb2 = boost::bind(&SpinnakerCameraNodelet::connectCb, this); - pub_.reset( - new diagnostic_updater::DiagnosedPublisher( - nh.advertise("image", queue_size, cb2, cb2), - updater_, diagnostic_updater::FrequencyStatusParam( - &min_freq_, &max_freq_, freq_tolerance, window_size), - diagnostic_updater::TimeStampStatusParam(min_acceptable, - max_acceptable))); + pub_.reset(new diagnostic_updater::DiagnosedPublisher( + nh.advertise("image", queue_size, cb2, cb2), + updater_, + diagnostic_updater::FrequencyStatusParam(&min_freq_, + &max_freq_, + freq_tolerance, + window_size), + diagnostic_updater::TimeStampStatusParam(min_acceptable, + max_acceptable))); // Set up diagnostics aggregator publisher and diagnostics manager - ros::SubscriberStatusCallback diag_cb = - boost::bind(&SpinnakerCameraNodelet::diagCb, this); - diagnostics_pub_.reset( - new ros::Publisher(nh.advertise( - "/diagnostics", 1, diag_cb, diag_cb))); + ros::SubscriberStatusCallback diag_cb = boost::bind(&SpinnakerCameraNodelet::diagCb, this); + diagnostics_pub_.reset(new ros::Publisher( + nh.advertise("/diagnostics", 1, diag_cb, diag_cb))); diag_man = std::unique_ptr(new DiagnosticsManager( - frame_id_, std::to_string(spinnaker_.getSerial()), diagnostics_pub_)); + frame_id_, std::to_string(spinnaker_.getSerial()), diagnostics_pub_, nh)); diag_man->addDiagnostic("DeviceTemperature", true, std::make_pair(0.0f, 90.0f), -10.0f, 95.0f); diag_man->addDiagnostic("AcquisitionResultingFrameRate", true, std::make_pair(10.0f, 60.0f), 5.0f, 90.0f); diag_man->addDiagnostic("PowerSupplyVoltage", true, std::make_pair(4.5f, 5.2f), 4.4f, 5.3f); @@ -414,6 +414,7 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet void diagPoll() { + diag_man->addAnalyzers(); while (!boost::this_thread::interruption_requested()) // Block until we need // to stop this // thread. @@ -538,7 +539,6 @@ class SpinnakerCameraNodelet : public nodelet::Nodelet getMTNodeHandle().subscribe("image_exposure_sequence", 10, &spinnaker_camera_driver::SpinnakerCameraNodelet::gainWBCallback, this); } - state = CONNECTED; } catch (const std::runtime_error& e)