From 149454b02b8d502ab14d8f3c30dbf324d774307e Mon Sep 17 00:00:00 2001 From: Aaron Marburg Date: Tue, 29 Aug 2023 21:44:43 -0700 Subject: [PATCH 1/2] Removal of dead code and minor comment cleanup --- README.md | 10 +- arena_camera/CMakeLists.txt | 1 + .../include/arena_camera/arena_camera_nodes.h | 23 +- .../arena_camera/encoding_conversions.h | 27 +- arena_camera/launch/streaming.launch.py | 25 +- arena_camera/package.xml | 1 + arena_camera/src/arena_camera_parameter.cpp | 385 ------------------ arena_camera/src/base_node.cpp | 91 ++--- arena_camera/src/encoding_conversions.cpp | 4 +- arena_camera/src/nodes/polled_node_main.cpp | 6 +- .../src/nodes/streaming_node_main.cpp | 12 +- arena_camera/src/streaming_node.cpp | 23 +- camera_control_msgs/package.xml | 2 +- 13 files changed, 101 insertions(+), 509 deletions(-) delete mode 100644 arena_camera/src/arena_camera_parameter.cpp diff --git a/README.md b/README.md index d7b8d1a..d1aea01 100644 --- a/README.md +++ b/README.md @@ -1,11 +1,15 @@ ## Arena Camera Driver for ROS2 -**Project Status:** This software is a work in progress. At present it is an in-progrss port of our [ROS1 LucidVision Driver](https://github.com/apl-ocean-engineering/arena_camera_ros) driver and is not feature-complete. +**Project Status:** This software is a work in progress. At present the streaming node is functional with the sample cameras which are available to us (a [PHX122S](https://thinklucid.com/product/phoenix-12-2-mp-imx226/) and a [TRI054S](https://thinklucid.com/product/triton-5-mp-imx490/) for testing HDR modes). The polled node is not functional. -This repo is a ROS2 driver for LucidVision machine vision camera using their ["Arena" SDK for Ubuntu](https://thinklucid.com/downloads-hub/). +--- + +This package is a ROS2 driver for LucidVision machine vision camera using their ["Arena" SDK for Ubuntu](https://thinklucid.com/downloads-hub/). This particular version is forked from the upstream [driver published by LucidVision](https://github.com/lucidvisionlabs/arena_camera_ros2) and includes significant refactoring for functionality / readability. +See also [arena_camera_ros](https://github.com/apl-ocean-engineering/arena_camera_ros) for ROS1. + # Getting Started This package builds as a standard ROS2 package using `colcon`, however the [Arena SDK](https://thinklucid.com/downloads-hub/) must be installed. @@ -14,7 +18,7 @@ See the [ROS2 documentation at LucidVision](https://support.thinklucid.com/using # Getting Started -- with Docker -A full Dockerfile is included in the [.docker](.docker/) directory. See the [README in that directory](.docker/README.md) for more details. +A full [Dockerfile](.docker/Dockerfile) is included in the [.docker/](.docker/) directory. See the [README in that directory](.docker/README.md) for more details. # Contents diff --git a/arena_camera/CMakeLists.txt b/arena_camera/CMakeLists.txt index 80d0a10..215ed1b 100644 --- a/arena_camera/CMakeLists.txt +++ b/arena_camera/CMakeLists.txt @@ -23,6 +23,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ament_cmake camera_control_msgs camera_info_manager + diagnostic_msgs diagnostic_updater image_transport imaging_msgs diff --git a/arena_camera/include/arena_camera/arena_camera_nodes.h b/arena_camera/include/arena_camera/arena_camera_nodes.h index 0ac51bd..a3395fe 100644 --- a/arena_camera/include/arena_camera/arena_camera_nodes.h +++ b/arena_camera/include/arena_camera/arena_camera_nodes.h @@ -188,6 +188,13 @@ class ArenaCameraBaseNode : public rclcpp::Node { /// float currentGamma(); + // ~~~ Camera Lookup Tables ~~~ + + /// Enable/disable lookup table (LUT) in camera. + /// @param enable Whether to enable/disable the camera LUT + /// + void enableLUT(bool enable); + // ~~~ HDR metadata (IMX490 only) ~~~ float currentHdrGain(int channel); @@ -253,12 +260,6 @@ class ArenaCameraBaseNode : public rclcpp::Node { // */ // float calcCurrentBrightness(); - // /** - // * Enable/disable lookup table (LUT) in camera. - // * @param enable Whether to enable/disable the camera LUT - // */ - // void enableLUT(bool enable); - protected: /// @brief /// @param cam_info_msg @@ -302,14 +303,9 @@ class ArenaCameraBaseNode : public rclcpp::Node { // std::vector sampling_indices_; // // std::array brightness_exp_lut_; - // boost::recursive_mutex device_mutex_; - - // /// diagnostics: + /// diagnostics: std::shared_ptr diagnostics_updater_; - // rclcpp::TimerBase::SharedPtr diagnostics_timer_; - // void updateDiagnosticsCb(void); - void create_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); void create_camera_info_diagnostics( diagnostic_updater::DiagnosticStatusWrapper &stat); @@ -353,14 +349,13 @@ class ArenaCameraStreamingNode : public ArenaCameraBaseNode { void newImageCb(Arena::IImage *pImage); }; +/// ArenaCameraPolledNode is not currently implemented. class ArenaCameraPolledNode : public ArenaCameraBaseNode { public: ArenaCameraPolledNode( const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); virtual ~ArenaCameraPolledNode(); - // void onInit() override; - /** * Callback for the grab images action * @param goal the goal diff --git a/arena_camera/include/arena_camera/encoding_conversions.h b/arena_camera/include/arena_camera/encoding_conversions.h index 92385f7..6ce0dc1 100644 --- a/arena_camera/include/arena_camera/encoding_conversions.h +++ b/arena_camera/include/arena_camera/encoding_conversions.h @@ -34,26 +34,27 @@ namespace arena_camera { namespace encoding_conversions { -/** - * Converts an encoding from the sensor_msgs/image_encodings.h list into - * the GenAPI language. - * @return true in case that an corresponding conversion could be found and - * false otherwise. - */ + +/// Converts an encoding from the sensor_msgs/image_encodings.h list into +/// the GenAPI language. +/// +/// @return true in case that an corresponding conversion could be found and +/// false otherwise. +/// bool ros2GenAPI(const std::string& ros_enc, std::string& gen_api_enc); -/** - * Converts an encoding described in GenAPI language into the ROS encoding - * language. The ROS encodings are listed in sensor_msgs/image_encodings.h - * @return true in case that an corresponding conversion could be found and - * false otherwise. - */ +/// Converts an encoding described in GenAPI language into the ROS encoding +/// language. The ROS encodings are listed in sensor_msgs/image_encodings.h +/// +/// @return true in case that an corresponding conversion could be found and +/// false otherwise. +/// bool genAPI2Ros(const std::string& gen_api_enc, std::string& ros_enc); -/// /// @brief Is the given ROS encoding an HDR type? /// /// @return true if ros_enc is an HDR image encoding +/// bool isHDR(const std::string& ros_enc); } // namespace encoding_conversions diff --git a/arena_camera/launch/streaming.launch.py b/arena_camera/launch/streaming.launch.py index 50d3249..c5e98a3 100644 --- a/arena_camera/launch/streaming.launch.py +++ b/arena_camera/launch/streaming.launch.py @@ -1,14 +1,29 @@ # Copyright 2023 University of Washington # -# Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: # -# 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. +# 1. Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. # -# 2. 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. +# 2. 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. # -# 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. +# 3. Neither the name of the copyright holder 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. +# 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. from launch import LaunchDescription diff --git a/arena_camera/package.xml b/arena_camera/package.xml index 9c83498..34bd34a 100644 --- a/arena_camera/package.xml +++ b/arena_camera/package.xml @@ -17,6 +17,7 @@ camera_control_msgs camera_info_manager + diagnostic_msgs diagnostic_updater generate_parameter_library image_transport diff --git a/arena_camera/src/arena_camera_parameter.cpp b/arena_camera/src/arena_camera_parameter.cpp deleted file mode 100644 index 9c58cd4..0000000 --- a/arena_camera/src/arena_camera_parameter.cpp +++ /dev/null @@ -1,385 +0,0 @@ -/****************************************************************************** - * Copyright (C) 2023 University of Washington - * - * based on the arena_camera_ros driver released under the BSD License: - * Copyright (C) 2021, Lucidvision Labs - * Copyright (C) 2016, Magazino GmbH. All rights reserved. - * - * 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 names of Magazino GmbH 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 OWNER 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. - *****************************************************************************/ - -// STD -// #include // std::boolalpha - -// // ROS -// #include - -// // Arena node -// #include - -namespace arena_camera { - -// ArenaCameraParameter::ArenaCameraParameter() -// // clang-format off -// : camera_frame_("arena_camera"), -// device_user_id_(""), -// serial_number_(""), -// frame_rate_(5.0), -// camera_info_url_(""), -// image_encoding_(""), -// image_encoding_given_(false), -// binning_x_(1), binning_y_(1), -// binning_x_given_(false), binning_y_given_(false), -// downsampling_factor_exp_search_(1), -// // ########################## -// gain_auto_(true), -// gain_(0.5), -// gamma_(1.0), -// gamma_given_(false), -// brightness_(100), -// brightness_given_(false), -// brightness_continuous_(false), -// // ######################### -// exposure_auto_(true), -// exposure_ms_(10000.0), -// auto_exposure_max_ms_(0.0), -// enable_lut_(false), -// mtu_size_(1400), -// inter_pkg_delay_(1000), -// shutter_mode_(SM_DEFAULT), -// auto_flash_(false) -// //clang-format on -// {} - -// ArenaCameraParameter::~ArenaCameraParameter() {} - -// void ArenaCameraParameter::readFromRosParameterServer( -// const ros::NodeHandle& nh) { -// if (!nh.getParam("camera_frame", camera_frame_)) { -// ROS_ERROR("\"camera_frame\" not set"); -// } -// ROS_INFO_STREAM("Using camera_frame: " << camera_frame_); - -// if (nh.getParam("device_user_id", device_user_id_)) { -// ROS_INFO_STREAM("Using DeviceUserId: " << device_user_id_); -// } - -// if (nh.getParam("serial_number", serial_number_)) { -// ROS_INFO_STREAM("Using serial_number: " << serial_number_); -// } - -// if (nh.hasParam("frame_rate")) { -// nh.getParam("frame_rate", frame_rate_); -// ROS_DEBUG_STREAM("frame_rate is given and has value " << frame_rate_); -// } - -// nh.param("camera_info_url", camera_info_url_, ""); -// if (nh.hasParam("camera_info_url")) { -// nh.getParam("camera_info_url", camera_info_url_); -// } - -// binning_x_given_ = nh.hasParam("binning_x"); -// if (binning_x_given_) { -// int binning_x; -// nh.getParam("binning_x", binning_x); -// ROS_DEBUG_STREAM("binning x is given and has value " << binning_x); -// if (binning_x > 32 || binning_x < 0) { -// ROS_WARN_STREAM("Desired horizontal binning_x factor not in valid " -// << "range! Binning x = " << binning_x -// << ". Will reset it to " -// << "default value (1)"); -// binning_x_given_ = false; -// } else { -// binning_x_ = static_cast(binning_x); -// } -// } -// binning_y_given_ = nh.hasParam("binning_y"); -// if (binning_y_given_) { -// int binning_y; -// nh.getParam("binning_y", binning_y); -// ROS_DEBUG_STREAM("binning y is given and has value " << binning_y); -// if (binning_y > 32 || binning_y < 0) { -// ROS_WARN_STREAM("Desired vertical binning_y factor not in valid " -// << "range! Binning y = " << binning_y -// << ". Will reset it to " -// << "default value (1)"); -// binning_y_given_ = false; -// } else { -// binning_y_ = static_cast(binning_y); -// } -// } -// nh.param("downsampling_factor_exposure_search", -// downsampling_factor_exp_search_, 20); -// image_encoding_given_ = nh.hasParam("image_encoding"); -// if (nh.hasParam("image_encoding")) { -// std::string encoding; -// nh.getParam("image_encoding", encoding); -// // if (!encoding.empty() && -// // !sensor_msgs::image_encodings::isMono(encoding) && -// // !sensor_msgs::image_encodings::isColor(encoding) && -// // !sensor_msgs::image_encodings::isBayer(encoding) && -// // encoding != sensor_msgs::image_encodings::YUV422) -// // { -// // ROS_WARN_STREAM("Desired image encoding parameter: '" << -// encoding -// // -// << -// // "' is not part of the 'sensor_msgs/image_encodings.h' list!" -// // -// << -// // " Will not set encoding"); encoding = std::string(""); -// // } -// image_encoding_ = encoding; -// } - -// // ########################## -// // image intensity settings -// // ########################## - -// gamma_given_ = nh.hasParam("gamma"); -// if (gamma_given_) { -// nh.getParam("gamma", gamma_); -// ROS_DEBUG_STREAM("gamma is given and has value " << gamma_); -// } - -// bool gain_given = nh.getParam("gain", gain_); -// if (gain_given) { -// ROS_DEBUG_STREAM("gain is given and has value " << gain_); -// } - -// brightness_given_ = nh.hasParam("brightness"); -// if (brightness_given_) { -// nh.getParam("brightness", brightness_); -// ROS_DEBUG_STREAM("brightness is given and has value " << brightness_); -// } - -// // ignore brightness? -// auto ignoreBrightness = brightness_given_ && gain_given; -// if (ignoreBrightness) { -// ROS_WARN_STREAM( -// "Gain ('gain') and Exposure Time ('exposure') " -// << "are given as startup ros-parameter and hence assumed to be " -// << "fix! The desired brightness (" << brightness_ << ") can't " -// << "be reached! Will ignore the brightness by only " -// << "setting gain and exposure . . ."); -// brightness_given_ = false; -// } else if (nh.hasParam("brightness_continuous")) { -// nh.getParam("brightness_continuous", brightness_continuous_); -// ROS_DEBUG_STREAM("brightness is continuous"); -// } - -// // clang-format off -// // exposure_given | exposure_auto_given_ | exposure_auto_ | action | -// // | | received val | | -// // -// ---------------|----------------------|----------------|------------------| -// // 1 F F F | default value -// issue -// // 2 F F T | default case -// notting to do -// // 3 F T F | shoe msg ; and -// set exposure_auto true in nodemap -// // 4 F T T | print param msg -// // 5 T F F | default value -// issue -// // 6 T F T | set default -// exposure_auto to false silently -// // 7 T T F | show param msg -// // 8 T T T | show ignore -// msg; show param msg ;set to false -// // clang-format on - -// // clang-format off -// // -// // gain_given | gain_auto_given_ | gain_auto_ | action | -// // | | received val | | -// // -// ---------------|----------------------|----------------|------------------| -// // 1 F F F | default value -// issue -// // 2 F F T | default case -// notting to do -// // 3 F T F | shoe msg ; and -// set gain_auto true in nodemap -// // 4 F T T | print param msg -// // 5 T F F | default value -// issue -// // 6 T F T | set default -// gain_auto to false silently -// // 7 T T F | show param msg -// // 8 T T T | show ignore -// msg; show param msg ;set to false -// // -// // clang-format on - -// // ignore gain_auto? -// auto gain_auto_given = nh.hasParam("gain_auto"); -// nh.getParam("gain_auto", gain_auto_); - -// // 1 FFF (gain_auto_ 's default value is not set to true) - -// // 2 FFT -// if (!gain_given && !gain_auto_given && gain_auto_) { -// // default case nothing to show/do -// } -// // 3 FTF -// else if (!gain_given && gain_auto_given && !gain_auto_) { -// // it is ok to pass gain_auto explicitly to false -// // with no gain value. Gain value will taken from device nodemap -// ROS_DEBUG_STREAM("gain_auto is given and has value Off/false"); - -// // TODO SET ON THE NODE MAP -// } -// // 4 FTT -// else if (!gain_given && gain_auto_given && gain_auto_) { -// ROS_DEBUG_STREAM("gain_auto is given and has value Continuous/true"); -// } - -// // 5 TFF (gain_auto_ 's default value is not set true) - -// // 6 TFT -// else if (gain_given && !gain_auto_given && gain_auto_) { -// gain_auto_ = false; // change because it defaults to true; -// // no msg it is not take from the param server -// } -// // 7 TTF -// else if (gain_given && gain_auto_given && !gain_auto_) { -// ROS_DEBUG_STREAM("gain_auto is given and has value Off/false"); -// } -// // 8 TTT -// else if (gain_given && gain_auto_given && gain_auto_) // ignore auto -// { -// ROS_DEBUG_STREAM("gain_auto is given and has value Continuous/true"); -// gain_auto_ = false; -// ROS_WARN_STREAM("gain_auto is ignored because gain is given."); -// } - -// nh.param("enable_lut", enable_lut_, false); - -// // Gig-E specific params -// nh.param("gige/mtu_size", mtu_size_, 1400); -// nh.param("gige/inter_pkg_delay", inter_pkg_delay_, 1000); - -// std::string shutter_param_string; -// nh.param("shutter_mode", shutter_param_string, ""); -// if (shutter_param_string == "rolling") { -// shutter_mode_ = SM_ROLLING; -// } else if (shutter_param_string == "global") { -// shutter_mode_ = SM_GLOBAL; -// } else if (shutter_param_string == "global_reset") { -// shutter_mode_ = SM_GLOBAL_RESET_RELEASE; -// } else { -// shutter_mode_ = SM_DEFAULT; -// } - -// nh.param("auto_flash", auto_flash_, false); -// nh.param("auto_flash_line_2", auto_flash_line_2_, true); -// nh.param("auto_flash_line_3", auto_flash_line_3_, true); - -// ROS_WARN("Autoflash: %i, line2: %i , line3: %i ", auto_flash_, -// auto_flash_line_2_, auto_flash_line_3_); -// validateParameterSet(nh); -// return; -// } - -// void ArenaCameraParameter::validateParameterSet(const ros::NodeHandle& nh) { -// // Put this validation closer to the point of use -// // if (!device_user_id_.empty()) { -// // ROS_INFO_STREAM( -// // "Trying to open the following camera: " << -// device_user_id_.c_str()); -// // } else { -// // ROS_INFO_STREAM("No Device User ID set -> Will open the camera device -// " -// // << "found first"); -// // } - -// if (frame_rate_ < 0 && frame_rate_ != -1) { -// ROS_WARN_STREAM("Unexpected frame rate (" -// << frame_rate_ << "). Will " -// << "reset it to default value which is 5 Hz"); -// frame_rate_ = 5.0; -// nh.setParam("frame_rate", frame_rate_); -// } - -// // if (exposure_given_ && (exposure_ <= 0.0 || exposure_ > 1e7)) { -// // ROS_WARN_STREAM("Desired exposure measured in microseconds not in " -// // << "valid range! Exposure time = " << exposure_ << ". -// // Will " -// // << "reset it to default value!"); -// // exposure_given_ = false; -// // } - -// if (gain_ < 0.0 || gain_ > 1.0) { -// ROS_WARN_STREAM("Desired gain (in percent) not in allowed range! " -// << "Gain = " << gain_ -// << ". Will reset it to default value!"); -// } - -// if (brightness_given_ && (brightness_ < 0.0 || brightness_ > 255)) { -// ROS_WARN_STREAM("Desired brightness not in allowed range [0 - 255]! " -// << "Brightness = " << brightness_ << ". Will reset it to -// " -// << "default value!"); -// brightness_given_ = false; -// } - -// if (mtu_size_ < 1400) { -// ROS_WARN_STREAM("MTU packet size too small: " << mtu_size_); -// } else if (mtu_size_ > 11000) { -// ROS_WARN_STREAM("MTU packet size too large: " << mtu_size_); -// } - -// return; -// } - -// std::string ArenaCameraParameter::shutterModeString() const { -// if (shutter_mode_ == SM_ROLLING) { -// return "rolling"; -// } else if (shutter_mode_ == SM_GLOBAL) { -// return "global"; -// } else if (shutter_mode_ == SM_GLOBAL_RESET_RELEASE) { -// return "global_reset"; -// } else { -// return "default_shutter_mode"; -// } -// } - -// // void ArenaCameraParameter::setFrameRate(const ros::NodeHandle& nh, -// // const double& frame_rate) { -// // frame_rate_ = frame_rate; -// // nh.setParam("frame_rate", frame_rate_); -// // } - -// void ArenaCameraParameter::setFrameRate(const double& frame_rate) { -// frame_rate_ = frame_rate; -// } - -// void ArenaCameraParameter::setCameraInfoURL( -// const ros::NodeHandle& nh, const std::string& camera_info_url) { -// camera_info_url_ = camera_info_url; -// nh.setParam("camera_info_url", camera_info_url_); -// } - -} // namespace arena_camera diff --git a/arena_camera/src/base_node.cpp b/arena_camera/src/base_node.cpp index 5023275..75203db 100644 --- a/arena_camera/src/base_node.cpp +++ b/arena_camera/src/base_node.cpp @@ -29,18 +29,8 @@ * POSSIBILITY OF SUCH DAMAGE. *****************************************************************************/ -// STD -// #include -// #include -// #include -// #include -// #include -// #include - // ROS2 #include -// #include -// #include // Arena #include @@ -61,13 +51,11 @@ ArenaCameraBaseNode::ArenaCameraBaseNode(const std::string &node_name, pSystem_(nullptr), pDevice_(nullptr), pNodeMap_(nullptr), - is_streaming_(false) -// arena_camera_parameter_set_(), -// set_user_output_srvs_(), -// pinhole_model_(), -// sampling_indices_() -{ + is_streaming_(false) { diagnostics_updater_ = std::make_shared(this, 2); + + // ~~ Diagnostic updater functions from ros1 node which haven't been + // re-implemented... // diagnostics_updater_.setHardwareID("none"); // diagnostics_updater_.add("camera_availability", this, // &ArenaCameraBaseNode::create_diagnostics); @@ -141,12 +129,6 @@ ArenaCameraBaseNode::ArenaCameraBaseNode(const std::string &node_name, parameter_check_timer_ = this->create_wall_timer( std::chrono::seconds(1), std::bind(&ArenaCameraBaseNode::checkParametersCb, this)); - - // _dynReconfigureServer = - // std::make_shared(pnh); - // _dynReconfigureServer->setCallback(boost::bind( - // &ArenaCameraBaseNode::reconfigureCallbackWrapper, this, _1, - // _2)); } ArenaCameraBaseNode::~ArenaCameraBaseNode() { @@ -370,16 +352,15 @@ bool ArenaCameraBaseNode::configureCamera() { "StreamBufferHandlingMode", "NewestOnly"); - // // bool isTriggerArmed = false; - // // if (GenApi::IsWritable(pTriggerMode)) { - // // do { - // // isTriggerArmed = Arena::GetNodeValue(pNodeMap, - // "TriggerArmed"); - // // } while (isTriggerArmed == false); - // // // Arena::ExecuteNode(pNodeMap, "TriggerSoftware"); - // // } + // bool isTriggerArmed = false; + // if (GenApi::IsWritable(pTriggerMode)) { + // do { + // isTriggerArmed = Arena::GetNodeValue(pNodeMap, "TriggerArmed"); + // } while (isTriggerArmed == false); + // // Arena::ExecuteNode(pNodeMap, "TriggerSoftware"); + // } - // Initial configuration of camera + // ~~ Initial configuration of camera setFrameRate(params_.frame_rate); setGain(params_); @@ -773,25 +754,6 @@ float ArenaCameraBaseNode::currentGain() { } } -// void ArenaCameraBaseNode::disableAllRunningAutoBrightessFunctions() { -// GenApi::CStringPtr pExposureAuto = pNodeMap_->GetNode("ExposureAuto"); -// GenApi::CStringPtr pGainAuto = pNodeMap_->GetNode("GainAuto"); - -// if (!pExposureAuto || !GenApi::IsWritable(pExposureAuto) || !pGainAuto || -// !GenApi::IsWritable(pGainAuto)) { -// Node_WARN_STREAM("Unable to disable auto gain & exposure"); -// return; -// } - -// else { -// Arena::SetNodeValue(pDevice_->GetNodeMap(), -// "ExposureAuto", "Off"); -// Arena::SetNodeValue(pDevice_->GetNodeMap(), -// "GainAuto", -// "Off"); -// } -// } - //======================================================================== // // Gamma @@ -1196,21 +1158,22 @@ float ArenaCameraBaseNode::currentHdrExposure(int channel) { // return sum; // } -// //------------------------------------------------------------------- -// // Functions for dealing with LUT -// // -// // \todo{amarburg} Very simple right now -// // +//------------------------------------------------------------------- +// Functions for dealing with LUT +// +// \todo{amarburg} Very simple right now +// -// void ArenaCameraBaseNode::enableLUT(bool enable) { -// try { -// Arena::SetNodeValue(pDevice_->GetNodeMap(), "LUTEnable", enable); -// } catch (const GenICam::GenericException &e) { -// Node_ERROR_STREAM("An exception while setting LUTEnable to " -// << (enable ? "true" : "false") -// << " occurred: " << e.GetDescription()); -// } -// } +void ArenaCameraBaseNode::enableLUT(bool enable) { + try { + Arena::SetNodeValue(pDevice_->GetNodeMap(), "LUTEnable", enable); + } catch (const GenICam::GenericException &e) { + RCLCPP_ERROR_STREAM(this->get_logger(), + "An exception while setting LUTEnable to " + << (enable ? "true" : "false") + << " occurred: " << e.GetDescription()); + } +} //=================================================================== // Periodic callback to check if parameters have changed diff --git a/arena_camera/src/encoding_conversions.cpp b/arena_camera/src/encoding_conversions.cpp index 6c4edbc..461d30c 100644 --- a/arena_camera/src/encoding_conversions.cpp +++ b/arena_camera/src/encoding_conversions.cpp @@ -1,7 +1,7 @@ /****************************************************************************** * Copyright (C) 2023 University of Washington * - * based on the arena_camera_ros driver released under the BSD License: + * based on the arena_camera_ros package released under the BSD License: * Copyright (C) 2021, Lucidvision Labs * Copyright (C) 2016, Magazino GmbH. All rights reserved. * @@ -81,7 +81,7 @@ bool ros2GenAPI(const std::string& ros_enc, std::string& gen_api_enc) { } */ else { - /* No gen-api pendant existant for following ROS-encodings: + /* No gen-api equivalent existant for following ROS-encodings: * - sensor_msgs::image_encodings::MONO16 * - sensor_msgs::image_encodings::BGRA8 * - sensor_msgs::image_encodings::BGR16 diff --git a/arena_camera/src/nodes/polled_node_main.cpp b/arena_camera/src/nodes/polled_node_main.cpp index 60ebc7e..8ee5c73 100644 --- a/arena_camera/src/nodes/polled_node_main.cpp +++ b/arena_camera/src/nodes/polled_node_main.cpp @@ -1,6 +1,10 @@ /****************************************************************************** * Software License Agreement (BSD License) * + * Copyright (C) 2023, University of Washington. All rights reserved. + * + * based on the arena_camera_ros driver released under the BSD License: + * Copyright (C) 2021, Lucidvision Labs * Copyright (C) 2016, Magazino GmbH. All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -10,7 +14,7 @@ * * 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 names of Magazino GmbH nor the names of its + * * Neither the names of the copyright holders nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * diff --git a/arena_camera/src/nodes/streaming_node_main.cpp b/arena_camera/src/nodes/streaming_node_main.cpp index 8851cc3..dadffc2 100644 --- a/arena_camera/src/nodes/streaming_node_main.cpp +++ b/arena_camera/src/nodes/streaming_node_main.cpp @@ -1,6 +1,11 @@ /****************************************************************************** * Software License Agreement (BSD License) * + * Copyright (C) 2023, University of Washington. All rights reserved. + * + * Derived from LucidVisions ros2 node, which included this original + * copyright notification: + * * Copyright (C) 2016, Magazino GmbH. All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -10,7 +15,7 @@ * * 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 names of Magazino GmbH nor the names of its + * * Neither the names of the copyright holders nor the names of its * contributors may be used to endorse or promote products derived from * this software without specific prior written permission. * @@ -27,11 +32,6 @@ * POSSIBILITY OF SUCH DAMAGE. *****************************************************************************/ -/* Authors: debout@magazino.eu - * grimm@magazino.eu - * engelhard@magazino.eu - */ - #include "arena_camera/arena_camera_nodes.h" int main(int argc, char *argv[]) { diff --git a/arena_camera/src/streaming_node.cpp b/arena_camera/src/streaming_node.cpp index 7af499d..6ab5c65 100644 --- a/arena_camera/src/streaming_node.cpp +++ b/arena_camera/src/streaming_node.cpp @@ -37,9 +37,6 @@ namespace arena_camera { -// using sensor_msgs::CameraInfo; -// using sensor_msgs::CameraInfoPtr; - ArenaCameraStreamingNode::ArenaCameraStreamingNode( const rclcpp::NodeOptions &options) : ArenaCameraBaseNode("arena_streaming", options), @@ -47,7 +44,7 @@ ArenaCameraStreamingNode::ArenaCameraStreamingNode( std::placeholders::_1)) { pDevice_->RegisterImageCallback(&image_callback_obj_); - // Exceptions in constructor ... dodgy + // Exceptions in constructor ... **shudder** try { startStreaming(); } catch (GenICam::GenericException &e) { @@ -108,17 +105,13 @@ void ArenaCameraStreamingNode::newImageCb(Arena::IImage *pImage) { const unsigned int data_size = image->height * image->step; - // // NODELET_INFO_STREAM("Image size " << pImage->GetWidth() << " x " - // << - // // pImage->GetHeight() << " with " << pImage->GetBitsPerPixel() << - // " bits"); - // // NODELET_INFO_STREAM(" expected size " - // // << (pImage->GetWidth() * pImage->GetHeight() - // * - // // (pImage->GetBitsPerPixel() / 8)) - // // << " ; Image size " << data_size << " ; size - // filled " - // // << pImage->GetSizeFilled()); + // NODELET_INFO_STREAM("Image size " << pImage->GetWidth() << " x " << + // pImage->GetHeight() << " with " << pImage->GetBitsPerPixel() << " bits"); + // NODELET_INFO_STREAM(" expected size " + // << (pImage->GetWidth() * pImage->GetHeight() * + // (pImage->GetBitsPerPixel() / 8)) + // << " ; Image size " << data_size << " ; size filled " + // << pImage->GetSizeFilled()); // \todo{amarburg} Validate image by comparing calculated image // size to actual Buffer/Image payload size diff --git a/camera_control_msgs/package.xml b/camera_control_msgs/package.xml index 50c6729..d241504 100644 --- a/camera_control_msgs/package.xml +++ b/camera_control_msgs/package.xml @@ -17,7 +17,7 @@ BSD - http://www.ros.org/wiki/arena_camera + https://github.com/apl-ocean-engineering/arena_camera_ros2/ rosidl_default_generators From ec6c8826449f5ebd4207d2c187753ddfa17bc5c2 Mon Sep 17 00:00:00 2001 From: Aaron Marburg Date: Tue, 29 Aug 2023 21:46:04 -0700 Subject: [PATCH 2/2] A few more minor comment updates. --- arena_camera/src/nodes/streaming_node_main.cpp | 5 ++--- arena_camera/src/polled_node.cpp | 6 ------ 2 files changed, 2 insertions(+), 9 deletions(-) diff --git a/arena_camera/src/nodes/streaming_node_main.cpp b/arena_camera/src/nodes/streaming_node_main.cpp index dadffc2..f2e848a 100644 --- a/arena_camera/src/nodes/streaming_node_main.cpp +++ b/arena_camera/src/nodes/streaming_node_main.cpp @@ -3,9 +3,8 @@ * * Copyright (C) 2023, University of Washington. All rights reserved. * - * Derived from LucidVisions ros2 node, which included this original - * copyright notification: - * + * based on the arena_camera_ros driver released under the BSD License: + * Copyright (C) 2021, Lucidvision Labs * Copyright (C) 2016, Magazino GmbH. All rights reserved. * * Redistribution and use in source and binary forms, with or without diff --git a/arena_camera/src/polled_node.cpp b/arena_camera/src/polled_node.cpp index df729ab..0676538 100644 --- a/arena_camera/src/polled_node.cpp +++ b/arena_camera/src/polled_node.cpp @@ -33,17 +33,11 @@ namespace arena_camera { -// using sensor_msgs::CameraInfo; -// using sensor_msgs::CameraInfoPtr; - ArenaCameraPolledNode::ArenaCameraPolledNode(const rclcpp::NodeOptions &options) : ArenaCameraBaseNode("arena_polled", options) {} ArenaCameraPolledNode::~ArenaCameraPolledNode() {} -// -// Nodelet::onInit function - // void ArenaCameraPolledNode::onInit() { // ArenaCameraNodeletBase::onInit();