From 48fe5b6e62eb6e1f45e395586cc48819ec1babbd Mon Sep 17 00:00:00 2001 From: Aaron Marburg Date: Thu, 10 Aug 2023 12:07:13 -0700 Subject: [PATCH] Stripped out all of the functionality till it compiles --- .docker/Dockerfile | 106 ++ .docker/ros_entrypoint.sh | 20 + arena_camera/CMakeLists.txt | 82 +- .../arena_camera/arena_camera_nodelet.h | 380 ----- .../include/arena_camera/arena_camera_nodes.h | 402 +++++ .../arena_camera/arena_camera_parameter.h | 516 +++--- .../arena_camera/encoding_conversions.h | 4 + arena_camera/package.xml | 12 +- arena_camera/src/arena_camera_parameter.cpp | 671 ++++---- arena_camera/src/base_node.cpp | 1417 +++++++++++++++++ arena_camera/src/encoding_conversions.cpp | 16 +- arena_camera/src/nodelet_base.cpp | 1364 ---------------- arena_camera/src/nodes/polled_node_main.cpp | 23 +- .../src/nodes/streaming_node_main.cpp | 18 +- arena_camera/src/polled_node.cpp | 379 +++++ arena_camera/src/polled_nodelet.cpp | 374 ----- arena_camera/src/streaming_node.cpp | 175 ++ arena_camera/src/streaming_nodelet.cpp | 172 -- 18 files changed, 3196 insertions(+), 2935 deletions(-) create mode 100644 .docker/Dockerfile create mode 100755 .docker/ros_entrypoint.sh delete mode 100644 arena_camera/include/arena_camera/arena_camera_nodelet.h create mode 100644 arena_camera/include/arena_camera/arena_camera_nodes.h create mode 100644 arena_camera/src/base_node.cpp delete mode 100644 arena_camera/src/nodelet_base.cpp create mode 100644 arena_camera/src/polled_node.cpp delete mode 100644 arena_camera/src/polled_nodelet.cpp create mode 100644 arena_camera/src/streaming_node.cpp delete mode 100644 arena_camera/src/streaming_nodelet.cpp diff --git a/.docker/Dockerfile b/.docker/Dockerfile new file mode 100644 index 0000000..c2f3965 --- /dev/null +++ b/.docker/Dockerfile @@ -0,0 +1,106 @@ +ARG BUILD_TYPE=clone +ARG ROS_VERSION=iron +ARG WS_DIR=/root/arena_ws + +## This should work through the magic of multi-arch? +FROM ros:${ROS_VERSION}-perception AS build_image + +# This should be set automatically by the build engine +ARG TARGETPLATFORM +ARG ARENA_SDK_URL_BASE=https://s3.us-west-1.wasabisys.com/marburg/LucidVision_SDK + +# Install the LucidVision SDK +WORKDIR /tmp + +RUN apt-get update && apt-get install -y \ + git \ + python3-vcstool \ + wget \ + && apt autoremove -y \ + && apt clean -y \ + && rm -rf /var/lib/apt/lists/* + +RUN if [ "${TARGETPLATFORM}" = "linux/amd64" ]; then \ + ARENA_SDK_FILE=ArenaSDK_v0.1.68_Linux_x64.tar.bz2; \ + elif [ "${TARGETPLATFORM}" = "linux/arm64" ]; then \ + ARENA_SDK_FILE=ArenaSDK_v0.1.49_Linux_ARM64.tar.bz2; \ + else \ + echo "Unknown target platform ${TARGETPLATFORM}"; \ + exit; \ + fi && \ + ARENA_SDK_URL=${ARENA_SDK_URL_BASE}/${ARENA_SDK_FILE} && \ + echo "Downloading ${ARENA_SDK_URL}" && \ + wget $ARENA_SDK_URL && \ + cd /usr/local && \ + tar -xjvf /tmp/$ARENA_SDK_FILE && \ + rm /tmp/$ARENA_SDK_FILE + +ENV ARENA_ROOT=/usr/local/ArenaSDK +# The name of the directory in the tarball changes by architecture, +# so create a convenience symlink +RUN ln -s /usr/local/ArenaSDK_* ${ARENA_ROOT} +WORKDIR ${ARENA_ROOT} +RUN sh -c "sudo sh Arena_SDK_*.conf" + +# Install the default ROS entrypoint +WORKDIR /root +COPY ros_entrypoint.sh /root/ros_entrypoint.sh +ENTRYPOINT [ "/root/ros_entrypoint.sh" ] + +#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +# This build path clones from github +FROM build_image as build_clone + +ARG ARENA_CAMERA_ROS2_REPO=https://github.com/apl-ocean-engineering/arena_camera_ros2.git +ARG ARENA_CAMERA_ROS2_BRANCH=main + +ARG WS_DIR +ONBUILD WORKDIR ${WS_DIR}/src +ONBUILD RUN echo "Cloning from ${ARENA_CAMERA_ROS2_BRANCH} branch ${ARENA_CAMERA_ROS2_REPO}" + +# This will break cache when the repo changes +ONBUILD ADD https://api.github.com/repos/apl-ocean-engineering/arena_camera_ros2/git/refs/heads/${ARENA_CAMERA_ROS2_BRANCH} version.json +ONBUILD RUN git clone -b ${ARENA_CAMERA_ROS2_BRANCH} ${ARENA_CAMERA_ROS2_REPO} + +#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +# This build path copies from a local tree +# +# I find the structure of the arena_camera_ros drive extremely annoying and +# can see changing away from it. +FROM build_image as build_local + +ARG LOCAL_SRC=local_src + +ARG WS_DIR +ONBUILD WORKDIR ${WS_DIR}/src +ONBUILD RUN echo "Pulling source code from local directory ${LOCAL_SRC}" +ONBUILD COPY ${LOCAL_SRC}/ ${WS_DIR}/src/ + +#~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +# Then the two build paths re-merge here +# dockerfile_lint - ignore +FROM build_${BUILD_TYPE} +LABEL Version=0.1 +LABEL Name=arena_camera_ros2 + +WORKDIR ${WS_DIR}/src +RUN vcs import --shallow --skip-existing < arena_camera_ros2/arena_camera_ros2.repos + +WORKDIR ${WS_DIR} + +# This is quite expensive to run on every build... +# RUN bash -c "apt-get update \ +# && source /opt/ros/noetic/setup.bash \ +# && rosdep install -y --ignore-src \ +# --skip-keys=arena_sdk \ +# --from-paths src/ \ +# && rm -rf /var/lib/apt/lists/*" + +RUN bash -c "/root/ros_entrypoint.sh \ + colcon build && \ + colcon test" + +# Convert ARG to ENV +ENV ROS_WS ${WS_DIR} +#RUN echo "ROS_WS=${ROS_WS}" > /root/.ROS_WS +CMD ["ros", "launch", "arena_camera", "arena_camera_nodelet.launch"] diff --git a/.docker/ros_entrypoint.sh b/.docker/ros_entrypoint.sh new file mode 100755 index 0000000..941ca3b --- /dev/null +++ b/.docker/ros_entrypoint.sh @@ -0,0 +1,20 @@ +#!/bin/bash +#set -e + +ROS_VERSION=${ROS_VERSION:-iron} + +# if [ -f $HOME/.ROS_WS ]; then +# echo "Loading catkin ws from $HOME/.ROS_WS" +# source $HOME/.ROS_WS +# fi + +# setup ros environment +if [ "${ROS_WS}" == "" ]; then + echo "Loading default environment: /opt/ros/$ROS_VERSION/setup.bash" + source /opt/ros/$ROS_VERSION/setup.bash +else + echo "Loading environment from workspace: $ROS_WS" + source $ROS_WS/devel/setup.bash +fi + +exec "$@" diff --git a/arena_camera/CMakeLists.txt b/arena_camera/CMakeLists.txt index 7713d8b..715a85b 100644 --- a/arena_camera/CMakeLists.txt +++ b/arena_camera/CMakeLists.txt @@ -3,7 +3,6 @@ project(arena_camera) set(CMAKE_CXX_STANDARD 14) - # # ARENA SDK # @@ -18,11 +17,18 @@ if (NOT ${arena_sdk_FOUND}) include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/Findarena_sdk.cmake") endif() +set(THIS_PACKAGE_INCLUDE_DEPENDS + rclcpp + rclcpp_components + ament_cmake + camera_control_msgs + imaging_msgs +) -find_package(rclcpp REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() -find_package(camera_control_msgs REQUIRED) -find_package(imaging_msgs REQUIRED) # @@ -101,35 +107,73 @@ find_package(imaging_msgs REQUIRED) # flags for all C++ targets -set(CMAKE_EXPORT_COMPILE_COMMANDS ON) - -set(arena_camera_lib_name ${PROJECT_NAME}) +set(arena_camera_lib ${PROJECT_NAME}) -add_library( ${arena_camera_lib_name} +add_library( ${arena_camera_lib} + SHARED src/encoding_conversions.cpp src/arena_camera_parameter.cpp - src/nodelet_base.cpp - src/polled_nodelet.cpp - src/streaming_nodelet.cpp + src/base_node.cpp + src/polled_node.cpp + src/streaming_node.cpp ) -target_include_directories( ${arena_camera_lib_name} - PRIVATE ${catkin_INCLUDE_DIRS} +ament_target_dependencies(${arena_camera_lib} ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_include_directories( ${arena_camera_lib} PRIVATE ${arena_sdk_INCLUDE_DIRS} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include ) -target_link_libraries( ${arena_camera_lib_name} - ${catkin_LIBRARIES} +target_link_libraries( ${arena_camera_lib} ${arena_sdk_LIBRARIES} ) -ament_target_dependencies(${arena_camera_lib_name} - rclcpp - camera_control_msgs - imaging_msgs +rclcpp_components_register_nodes(${arena_camera_lib} "arena_camera::ArenaCameraPolledNode") +rclcpp_components_register_nodes(${arena_camera_lib} "arena_camera::ArenaCameraStreamingNode") + +## Build stand-alone executables +foreach(Executable IN ITEMS polled_node streaming_node) + + add_executable(${Executable} src/nodes/${Executable}_main.cpp) + + target_include_directories( ${Executable} + PRIVATE ${arena_sdk_INCLUDE_DIRS} + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include + ) + + target_link_libraries(${Executable} ${arena_camera_lib}) + +endforeach() + + +# Install +install( + TARGETS ${arena_camera_lib} + DESTINATION lib/${PROJECT_NAME} +) + +# install(DIRECTORY +# include/ +# DESTINATION include +# ) + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ ) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + + # Run linters found in package.xml except those below +# set(ament_cmake_copyright_FOUND TRUE) +# set(ament_cmake_uncrustify_FOUND TRUE) +# set(ament_cmake_pep257_FOUND TRUE) +# set(ament_cmake_flake8_FOUND TRUE) + + ament_lint_auto_find_test_dependencies() +endif() + # # # # ARENA_CAMERA_NODES diff --git a/arena_camera/include/arena_camera/arena_camera_nodelet.h b/arena_camera/include/arena_camera/arena_camera_nodelet.h deleted file mode 100644 index ffd450f..0000000 --- a/arena_camera/include/arena_camera/arena_camera_nodelet.h +++ /dev/null @@ -1,380 +0,0 @@ -/****************************************************************************** - * Software License Agreement (BSD 3-Clause License) - * Copyright (C) 2023 University of Washington. All rights reserved. - * - * Based on the original arena_camera_ros as follows: - * - * 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. - *****************************************************************************/ - -#pragma once - -// STD -#include -#include - -// ROS sys dep -#include - -// ROS -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Arena -#include -// #include -#include - -// Auto-generated dynamic_reconfigure header file -#include - -#include "imaging_msgs/HdrImagingMetadata.h" -#include "imaging_msgs/ImagingMetadata.h" - -namespace arena_camera { -typedef actionlib::SimpleActionServer - GrabImagesAS; - -/// Base class for both types of nodelets -class ArenaCameraNodeletBase : public nodelet::Nodelet { - public: - ArenaCameraNodeletBase(); - virtual ~ArenaCameraNodeletBase(); - - /** - * initialize the camera and the ros node. - * calls ros::shutdown if an error occurs. - */ - void onInit() override; - - /// Getter for the current frame rate - /// @return the desired frame rate. - /// - const double &frameRate() const { - return arena_camera_parameter_set_.frameRate(); - } - - /// Getter for the tf frame. - /// @return the camera frame. - /// - const std::string &cameraFrame() const { - return arena_camera_parameter_set_.cameraFrame(); - } - - void startStreaming(); - void stopStreaming(); - - protected: - /** - * Creates the camera instance either by UserDeviceId, SerialNumber, - * or taking the first auto-detected camera. - * @return false if an error occurred - */ - bool registerCameraByUserId(const std::string &device_id); - bool registerCameraBySerialNumber(const std::string &serial_number); - bool registerCameraByAuto(); - - /** - * Start the camera and initialize the messages - * @return - */ - bool configureCamera(); - - bool setImageEncoding(const std::string &ros_encoding); - - void updateFrameRate(); - - /** - * Update area of interest in the camera image - * @param target_roi the target roi - * @param reached_roi the roi that could be set - * @return true if the targeted roi could be reached - */ - bool setROI(const sensor_msgs::RegionOfInterest target_roi, - sensor_msgs::RegionOfInterest &reached_roi); - - /** - * Update the horizontal binning_x factor to get downsampled images - * @param target_binning_x the target horizontal binning_x factor - * @param reached_binning_x the horizontal binning_x factor that could be - * reached - * @return true if the targeted binning could be reached - */ - bool setBinningX(const size_t &target_binning_x, size_t &reached_binning_x); - - /** - * Update the vertical binning_y factor to get downsampled images - * @param target_binning_y the target vertical binning_y factor - * @param reached_binning_y the vertical binning_y factor that could be - * reached - * @return true if the targeted binning could be reached - */ - bool setBinningY(const size_t &target_binning_y, size_t &reached_binning_y); - - /** - * Sets the target brightness which is the intensity-mean over all pixels. - * If the target exposure time is not in the range of Arena's auto target - * brightness range the extended brightness search is started. - * The Auto function of the Arena-API supports values from [50 - 205]. - * Using a binary search, this range will be extended up to [1 - 255]. - * @param target_brightness is the desired brightness. Range is [1...255]. - * @param current_brightness is the current brightness with the given - * settings. - * @param exposure_auto flag which indicates if the target_brightness - * should be reached adapting the exposure time - * @param gain_auto flag which indicates if the target_brightness should be - * reached adapting the gain. - * @return true if the brightness could be reached or false otherwise. - */ - // bool setBrightness(const int &target_brightness, int &reached_brightness, - // const bool &exposure_auto, const bool &gain_auto); - - void setTargetBrightness(unsigned int brightness); - - //==== Functions to get/set exposure ==== - - enum class AutoExposureMode : int { Off = 0, Once = 1, Continuous = 2 }; - - // Update exposure based on arena_camera_parameter_set - // - // If exp_mode == Off, exposure_ms is the **fixed exposure** set in the - // camera If exp_mode == Once or Continuous, exposure_ms is the **max - // exposure** allowed - // for the auto-exposure algorithm - void setExposure(AutoExposureMode exp_mode, float exposure_ms); - float currentExposure(); - - //==== Functions to get/set gain ==== - - enum class AutoGainMode : int { Off = 0, Once = 1, Continuous = 2 }; - - /** - * Update the gain from the camera to a target gain in percent - * @param gain_mode Request gain mode - * @param target_gain the targeted gain in percent. Ignored if gain_mode - * isn't "Off" - * @param reached_gain the gain that could be reached - * @return true if the targeted gain could be reached - */ - bool setGain(AutoGainMode gain_mode, float target_gain = 0.0); - float currentGain(); - - //==== Functions to get/set gamma ==== - - /** - * Update the gamma from the camera to a target gamma correction value - * @param target_gamma the targeted gamma - * @return true if the targeted gamma could be reached - */ - bool setGamma(const float &target_gamma); - float currentGamma(); - - //===== Functions for querying HDR channels (IMX490 only) - float currentHdrGain(int channel); - float currentHdrExposure(int channel); - - /** - * Generates the subset of points on which the brightness search will be - * executed in order to speed it up. The subset are the indices of the - * one-dimensional image_raw data vector. The base generation is done in a - * recursive manner, by calling genSamplingIndicesRec - * @return indices describing the subset of points - */ - void setupSamplingIndices(std::vector &indices, std::size_t rows, - std::size_t cols, int downsampling_factor); - - /** - * This function will recursively be called from above setupSamplingIndices() - * to generate the indices of pixels given the actual ROI. - * @return indices describing the subset of points - */ - void genSamplingIndicesRec(std::vector &indices, - const std::size_t &min_window_height, - const cv::Point2i &start, const cv::Point2i &end); - - /** - * Calculates the mean brightness of the image based on the subset indices - * @return the mean brightness of the image - */ - float calcCurrentBrightness(); - - void initCalibrationMatrices(sensor_msgs::CameraInfo &info, const cv::Mat &D, - const cv::Mat &K); - - /** - * 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 - // void initializeCameraInfo(sensor_msgs::CameraInfo &cam_info_msg); - - Arena::ISystem *pSystem_; - Arena::IDevice *pDevice_; - GenApi::INodeMap *pNodeMap_; - - bool is_streaming_; - - // Hardware accessor functions - // These might have originally been in arena_camera.h? - sensor_msgs::RegionOfInterest currentROI(); - int64_t currentBinningX(); - int64_t currentBinningY(); - std::string currentROSEncoding(); - bool setBinningXValue(const size_t &target_binning_x, - size_t &reached_binning_x); - bool setBinningYValue(const size_t &target_binning_y, - size_t &reached_binning_y); - void disableAllRunningAutoBrightessFunctions(); - - ros::Publisher metadata_pub_, hdr_metadata_pub_; - - ArenaCameraParameter arena_camera_parameter_set_; - - std::vector set_user_output_srvs_; - - std::unique_ptr it_; - image_transport::CameraPublisher img_raw_pub_; - - image_geometry::PinholeCameraModel pinhole_model_; - - // Don't like using this global member - sensor_msgs::Image img_raw_msg_; - - camera_info_manager::CameraInfoManager *camera_info_manager_; - - std::vector sampling_indices_; - // std::array brightness_exp_lut_; - - boost::recursive_mutex device_mutex_; - - typedef dynamic_reconfigure::Server - DynReconfigureServer; - std::shared_ptr _dynReconfigureServer; - - // Non-virtual callback which calls virtual function - void reconfigureCallbackWrapper(ArenaCameraConfig &config, uint32_t level) { - reconfigureCallback(config, level); - } - - virtual void reconfigureCallback(ArenaCameraConfig &config, uint32_t level); - ArenaCameraConfig previous_config_; - - /// diagnostics: - diagnostic_updater::Updater diagnostics_updater_; - void diagnostics_timer_callback_(const ros::TimerEvent &); - ros::Timer diagnostics_trigger_; - void create_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); - void create_camera_info_diagnostics( - diagnostic_updater::DiagnosticStatusWrapper &stat); -}; - -class ArenaCameraStreamingNodelet : public ArenaCameraNodeletBase { - public: - ArenaCameraStreamingNodelet(); - virtual ~ArenaCameraStreamingNodelet(); - - void onInit() override; - - protected: - typedef std::function ImageCallback_t; - - class ImageCallback : public Arena::IImageCallback { - public: - ImageCallback(ImageCallback_t cb) : image_callback_(cb) {} - ~ImageCallback() {} - - void OnImage(Arena::IImage *pImage) { image_callback_(pImage); } - - private: - ImageCallback_t image_callback_; - - ImageCallback() = delete; - ImageCallback(const ImageCallback &) = delete; - ImageCallback operator=(const ImageCallback &) = delete; - } image_callback_obj_; - - void imageCallback(Arena::IImage *pImage); - - void reconfigureCallback(ArenaCameraConfig &config, uint32_t level) override; -}; - -class ArenaCameraPolledNodelet : public ArenaCameraNodeletBase { - public: - ArenaCameraPolledNodelet(); - virtual ~ArenaCameraPolledNodelet(); - - void onInit() override; - - /** - * Callback for the grab images action - * @param goal the goal - */ - void grabImagesRawActionExecuteCB( - const camera_control_msgs::GrabImagesGoal::ConstPtr &goal); - - /** - * This function can also be called from the derived ArenaCameraOpenCV-Class - */ - camera_control_msgs::GrabImagesResult grabImagesRaw( - const camera_control_msgs::GrabImagesGoal::ConstPtr &goal, - GrabImagesAS *action_server); - - protected: - virtual bool sendSoftwareTrigger(); - - /// Grabs an image and stores the image in img_raw_msg_ - /// @return false if an error occurred. - virtual bool grabImage(); - - std::unique_ptr grab_imgs_raw_as_; - - void reconfigureCallback(ArenaCameraConfig &config, uint32_t level) override { - ; - } -}; - -} // namespace arena_camera diff --git a/arena_camera/include/arena_camera/arena_camera_nodes.h b/arena_camera/include/arena_camera/arena_camera_nodes.h new file mode 100644 index 0000000..ed2c6be --- /dev/null +++ b/arena_camera/include/arena_camera/arena_camera_nodes.h @@ -0,0 +1,402 @@ +/****************************************************************************** + * Software License Agreement (BSD 3-Clause License) + * + * Copyright (C) 2023 University of Washington. All rights reserved. + * + * Based on the original arena_camera_ros as follows: + * + * 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. + *****************************************************************************/ + +#pragma once + +#include +#include + +#include "rclcpp/rclcpp.hpp" + +// ROS +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include + +// Arena +#include + +// #include +// #include + +// // Auto-generated dynamic_reconfigure header file +// #include + +// #include "imaging_msgs/HdrImagingMetadata.h" +// #include "imaging_msgs/ImagingMetadata.h" + +namespace arena_camera { +// typedef actionlib::SimpleActionServer +// GrabImagesAS; + +/// Base class for both types of Nodes +class ArenaCameraBaseNode : public rclcpp::Node { + public: + ArenaCameraBaseNode(const std::string &node_name, + const rclcpp::NodeOptions &options); + virtual ~ArenaCameraBaseNode(); + + // /** + // * initialize the camera and the ros node. + // * calls ros::shutdown if an error occurs. + // */ + // //void onInit() override; + + // /// Getter for the current frame rate + // /// @return the desired frame rate. + // /// + // const double &frameRate() const { + // return arena_camera_parameter_set_.frameRate(); + // } + + // /// Getter for the tf frame. + // /// @return the camera frame. + // /// + // const std::string &cameraFrame() const { + // return arena_camera_parameter_set_.cameraFrame(); + // } + + // void startStreaming(); + // void stopStreaming(); + + protected: + /** + * Creates the camera instance either by UserDeviceId, SerialNumber, + * or taking the first auto-detected camera. + * @return false if an error occurred + */ + bool registerCameraByUserId(const std::string &device_id); + bool registerCameraBySerialNumber(const std::string &serial_number); + bool registerCameraByAuto(); + + /** + * Start the camera and initialize the messages + * @return + */ + // bool configureCamera(); + + // bool setImageEncoding(const std::string &ros_encoding); + + // void updateFrameRate(); + + /** + * Update area of interest in the camera image + * @param target_roi the target roi + * @param reached_roi the roi that could be set + * @return true if the targeted roi could be reached + */ + // bool setROI(const sensor_msgs::RegionOfInterest target_roi, + // sensor_msgs::RegionOfInterest &reached_roi); + + // /** + // * Update the horizontal binning_x factor to get downsampled images + // * @param target_binning_x the target horizontal binning_x factor + // * @param reached_binning_x the horizontal binning_x factor that could be + // * reached + // * @return true if the targeted binning could be reached + // */ + // bool setBinningX(const size_t &target_binning_x, size_t + // &reached_binning_x); + + // /** + // * Update the vertical binning_y factor to get downsampled images + // * @param target_binning_y the target vertical binning_y factor + // * @param reached_binning_y the vertical binning_y factor that could be + // * reached + // * @return true if the targeted binning could be reached + // */ + // bool setBinningY(const size_t &target_binning_y, size_t + // &reached_binning_y); + + // /** + // * Sets the target brightness which is the intensity-mean over all pixels. + // * If the target exposure time is not in the range of Arena's auto target + // * brightness range the extended brightness search is started. + // * The Auto function of the Arena-API supports values from [50 - 205]. + // * Using a binary search, this range will be extended up to [1 - 255]. + // * @param target_brightness is the desired brightness. Range is [1...255]. + // * @param current_brightness is the current brightness with the given + // * settings. + // * @param exposure_auto flag which indicates if the target_brightness + // * should be reached adapting the exposure time + // * @param gain_auto flag which indicates if the target_brightness should be + // * reached adapting the gain. + // * @return true if the brightness could be reached or false otherwise. + // */ + // // bool setBrightness(const int &target_brightness, int + // &reached_brightness, + // // const bool &exposure_auto, const bool &gain_auto); + + // void setTargetBrightness(unsigned int brightness); + + // //==== Functions to get/set exposure ==== + + // enum class AutoExposureMode : int { Off = 0, Once = 1, Continuous = 2 }; + + // // Update exposure based on arena_camera_parameter_set + // // + // // If exp_mode == Off, exposure_ms is the **fixed exposure** set in the + // // camera If exp_mode == Once or Continuous, exposure_ms is the **max + // // exposure** allowed + // // for the auto-exposure algorithm + // void setExposure(AutoExposureMode exp_mode, float exposure_ms); + // float currentExposure(); + + // //==== Functions to get/set gain ==== + + // enum class AutoGainMode : int { Off = 0, Once = 1, Continuous = 2 }; + + // /** + // * Update the gain from the camera to a target gain in percent + // * @param gain_mode Request gain mode + // * @param target_gain the targeted gain in percent. Ignored if gain_mode + // * isn't "Off" + // * @param reached_gain the gain that could be reached + // * @return true if the targeted gain could be reached + // */ + // bool setGain(AutoGainMode gain_mode, float target_gain = 0.0); + // float currentGain(); + + // //==== Functions to get/set gamma ==== + + // /** + // * Update the gamma from the camera to a target gamma correction value + // * @param target_gamma the targeted gamma + // * @return true if the targeted gamma could be reached + // */ + // bool setGamma(const float &target_gamma); + // float currentGamma(); + + // //===== Functions for querying HDR channels (IMX490 only) + // float currentHdrGain(int channel); + // float currentHdrExposure(int channel); + + // /** + // * Generates the subset of points on which the brightness search will be + // * executed in order to speed it up. The subset are the indices of the + // * one-dimensional image_raw data vector. The base generation is done in a + // * recursive manner, by calling genSamplingIndicesRec + // * @return indices describing the subset of points + // */ + // void setupSamplingIndices(std::vector &indices, std::size_t + // rows, + // std::size_t cols, int downsampling_factor); + + // /** + // * This function will recursively be called from above + // setupSamplingIndices() + // * to generate the indices of pixels given the actual ROI. + // * @return indices describing the subset of points + // */ + // void genSamplingIndicesRec(std::vector &indices, + // const std::size_t &min_window_height, + // const cv::Point2i &start, const cv::Point2i + // &end); + + // /** + // * Calculates the mean brightness of the image based on the subset indices + // * @return the mean brightness of the image + // */ + // float calcCurrentBrightness(); + + // void initCalibrationMatrices(sensor_msgs::CameraInfo &info, const cv::Mat + // &D, + // const cv::Mat &K); + + // /** + // * 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 + // void initializeCameraInfo(sensor_msgs::CameraInfo &cam_info_msg); + + Arena::ISystem *pSystem_; + Arena::IDevice *pDevice_; + GenApi::INodeMap *pNodeMap_; + + bool is_streaming_; + + // Hardware accessor functions + // These might have originally been in arena_camera.h? + // sensor_msgs::RegionOfInterest currentROI(); + // int64_t currentBinningX(); + // int64_t currentBinningY(); + // std::string currentROSEncoding(); + // bool setBinningXValue(const size_t &target_binning_x, + // size_t &reached_binning_x); + // bool setBinningYValue(const size_t &target_binning_y, + // size_t &reached_binning_y); + // void disableAllRunningAutoBrightessFunctions(); + + // ros::Publisher metadata_pub_, hdr_metadata_pub_; + + // ArenaCameraParameter arena_camera_parameter_set_; + + // std::vector set_user_output_srvs_; + + // std::unique_ptr it_; + // image_transport::CameraPublisher img_raw_pub_; + + // image_geometry::PinholeCameraModel pinhole_model_; + + // // Don't like using this global member + // sensor_msgs::Image img_raw_msg_; + + // camera_info_manager::CameraInfoManager *camera_info_manager_; + + // std::vector sampling_indices_; + // // std::array brightness_exp_lut_; + + // boost::recursive_mutex device_mutex_; + + // typedef dynamic_reconfigure::Server + // DynReconfigureServer; + // std::shared_ptr _dynReconfigureServer; + + // // Non-virtual callback which calls virtual function + // void reconfigureCallbackWrapper(ArenaCameraConfig &config, uint32_t level) + // { + // reconfigureCallback(config, level); + // } + + // virtual void reconfigureCallback(ArenaCameraConfig &config, uint32_t + // level); ArenaCameraConfig previous_config_; + + // /// diagnostics: + // diagnostic_updater::Updater diagnostics_updater_; + // void diagnostics_timer_callback_(const ros::TimerEvent &); + // ros::Timer diagnostics_trigger_; + // void create_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); + // void create_camera_info_diagnostics( + // diagnostic_updater::DiagnosticStatusWrapper &stat); +}; + +class ArenaCameraStreamingNode : public ArenaCameraBaseNode { + public: + ArenaCameraStreamingNode( + const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); + virtual ~ArenaCameraStreamingNode(); + + // void onInit() override; + + protected: + typedef std::function ImageCallback_t; + + // The ArenaSDK takes a class derived from Arena::IImageCallback + // for the new-image callback. + // + // ImageCallback meets the Arena requirement, and wraps + // a callback **back** into this ArenaCameraStreamingNode + // + class ImageCallback : public Arena::IImageCallback { + public: + ImageCallback(ImageCallback_t cb) : image_callback_(cb) {} + ~ImageCallback() {} + + void OnImage(Arena::IImage *pImage) { image_callback_(pImage); } + + private: + ImageCallback_t image_callback_; + + ImageCallback() = delete; + ImageCallback(const ImageCallback &) = delete; + ImageCallback operator=(const ImageCallback &) = delete; + }; + + ImageCallback image_callback_obj_; + + void newImageCb(Arena::IImage *pImage); + + // void reconfigureCallback(ArenaCameraConfig &config, uint32_t level) + // override; +}; + +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 + */ + // void grabImagesRawActionExecuteCB( + // const camera_control_msgs::GrabImagesGoal::ConstPtr &goal); + + // /** + // * This function can also be called from the derived + // ArenaCameraOpenCV-Class + // */ + // camera_control_msgs::GrabImagesResult grabImagesRaw( + // const camera_control_msgs::GrabImagesGoal::ConstPtr &goal, + // GrabImagesAS *action_server); + + protected: + // virtual bool sendSoftwareTrigger(); + + // /// Grabs an image and stores the image in img_raw_msg_ + // /// @return false if an error occurred. + // virtual bool grabImage(); + + // std::unique_ptr grab_imgs_raw_as_; + + // void reconfigureCallback(ArenaCameraConfig &config, uint32_t level) + // override { + // ; + // } +}; + +} // namespace arena_camera diff --git a/arena_camera/include/arena_camera/arena_camera_parameter.h b/arena_camera/include/arena_camera/arena_camera_parameter.h index 4e41637..6c055de 100644 --- a/arena_camera/include/arena_camera/arena_camera_parameter.h +++ b/arena_camera/include/arena_camera/arena_camera_parameter.h @@ -50,261 +50,265 @@ enum SHUTTER_MODE { /** *Parameter class for the ArenaCamera */ -class ArenaCameraParameter { - public: - ArenaCameraParameter(); - - virtual ~ArenaCameraParameter(); - - /** - * Read the parameters from the parameter server. - * If invalid parameters can be detected, the interface will reset them - * to the default values. - * @param pnh the **private** ros::NodeHandle to use - */ - void readFromRosParameterServer(const ros::NodeHandle &pnh); - - /// Getter for the device_user_id param - const std::string &deviceUserID() const { return device_user_id_; } - - /// Getter for the serial number param - const std::string &serialNumber() const { return serial_number_; } - - int mtuSize() const { return mtu_size_; } - - /** - * Getter for the string describing the shutter mode - */ - std::string shutterModeString() const; - - // Getter for the camera_frame_ set from ros-parameter server - const std::string &cameraFrame() const { return camera_frame_; } - - // Getter for the frame_rate_ read from ros-parameter server - const double &frameRate() const { return frame_rate_; } - - // Getter for the image_encoding_ read from ros-parameter server - const std::string &imageEncoding() const { return image_encoding_; } - - /** - * Setter for the frame_rate_ initially set from ros-parameter server - * The frame rate needs to be updated with the value the camera supports - */ - // void setFrameRate(const ros::NodeHandle& nh, const double& frame_rate); - void setFrameRate(const double &frame_rate); - - // Getter for the camera_info_url set from ros-parameter server - const std::string &cameraInfoURL() const { return camera_info_url_; } - - /** - * Setter for the camera_info_url_ if a new CameraInfo-Msgs Object is - * provided via the SetCameraInfo-service from the CameraInfoManager - */ - void setCameraInfoURL(const ros::NodeHandle &nh, - const std::string &camera_info_url); - - public: - /** Binning factor to get downsampled images. It refers here to any camera - * setting which combines rectangular neighborhoods of pixels into larger - * "super-pixels." It reduces the resolution of the output image to - * (width / binning_x) x (height / binning_y). - * The default values binning_x = binning_y = 0 are considered the same - * as binning_x = binning_y = 1 (no subsampling). - */ - size_t binning_x_; - size_t binning_y_; - - /** - * Flags which indicate if the binning factors are provided and hence - * should be set during startup - */ - bool binning_x_given_; - bool binning_y_given_; - - bool image_encoding_given_; - - /** - * Factor that describes the image downsampling to speed up the exposure - * search to find the desired brightness. - * The smallest window height is img_rows/downsampling_factor - */ - int downsampling_factor_exp_search_; - - // ####################################################################### - // ###################### Image Intensity Settings ###################### - // ####################################################################### - // The following settings do *NOT* have to be set. Each camera has default - // values which provide an automatic image adjustment - // If one would like to adjust image brightness, it is not - // ####################################################################### - - /** - * The target gain in percent of the maximal value the camera supports - * For USB-Cameras, the gain is in dB, for GigE-Cameras it is given in so - * called 'device specific units'. - */ - double gain_; - - /** - * Flag which indicates if the gain value is provided and hence should be - * set during startup - */ - bool gain_given_; - - /** - * Gamma correction of pixel intensity. - * Adjusts the brightness of the pixel values output by the camera's sensor - * to account for a non-linearity in the human perception of brightness or - * of the display system (such as CRT). - */ - double gamma_; - - /** - * Flag which indicates if the gamma correction value is provided and - * hence should be set during startup - */ - bool gamma_given_; - - /** - * The average intensity value of the images. It depends on the exposure - * time as well as the gain setting. If 'exposure' is provided, the - * interface will try to reach the desired brightness by only varying the - * gain. (What may often fail, because the range of possible exposure - * values is many times higher than the gain range). - * If 'gain' is provided, the interface will try to reach the desired - * brightness by only varying the exposure time. If gain AND exposure are - * given, it is not possible to reach the brightness, because both are - * assumed to be fix. - */ - int brightness_; - - /** - * Flag which indicates if the average brightness is provided and hence - * should be set during startup - */ - bool brightness_given_; - - /** - * Only relevant, if 'brightness' is set as ros-parameter: - * The brightness_continuous flag controls the auto brightness function. - * If it is set to false, the brightness will only be reached once. - * Hence changing light conditions lead to changing brightness values. - * If it is set to true, the given brightness will be reached continuously, - * trying to adapt to changing light conditions. This is only possible for - * values in the possible auto range of the arena API which is - * e.g. [50 - 205] for acA2500-14um and acA1920-40gm - */ - bool brightness_continuous_; - - /** - * Only relevant, if 'brightness' is given as ros-parameter: - * If the camera should try to reach and / or keep the brightness, hence - * adapting to changing light conditions, at least one of the following - * flags must be set. If both are set, the interface will use the profile - * that tries to keep the gain at minimum to reduce white noise. - * The exposure_auto flag indicates, that the desired brightness will - * be reached by adapting the exposure time. - * The gain_auto flag indicates, that the desired brightness will be - * reached by adapting the gain. - */ - bool exposure_auto_; - bool gain_auto_; - - bool enable_lut_; - // ####################################################################### - - double exposure_ms_; - double auto_exposure_max_ms_; - - /** - * The MTU size. Only used for GigE cameras. - * To prevent lost frames the camera has to be configured - * with the MTU size the network card supports. A value greater 3000 - * should be good (1500 for RaspberryPI) - */ - int mtu_size_; - - /** - * The inter-package delay in ticks. Only used for GigE cameras. - * To prevent lost frames it should be greater 0. - * For most of GigE-Cameras, a value of 1000 is reasonable. - * For GigE-Cameras used on a RaspberryPI this value should be set to 11772 - */ - int inter_pkg_delay_; - - /** - Shutter mode - */ - SHUTTER_MODE shutter_mode_; - - /** - * Flag that indicates if the camera has been calibrated and the intrinsic - * calibration matrices are available - */ - bool has_intrinsic_calib_; - - /** - * Flag that indicates if the camera has a flash connected which should be on - * on exposure Only supported for GigE cameras. Default: false - */ - bool auto_flash_; - /** - * Flag that indicates if the camera, when using auto_flash == true, a flash - * connected on line 2 which should be on on exposure Only supported for GigE - * cameras. Default: true - */ - bool auto_flash_line_2_; - /** - * Flag that indicates if the camera has, when using auto_flash == true, a - * flash connected on line 3 which should be on on exposure Only supported for - * GigE cameras. Default: true - */ - bool auto_flash_line_3_; - - protected: - /** - * Validates the parameter set found on the ros parameter server. - * If invalid parameters can be detected, the interface will reset them - * to the default values. - * @param nh the ros::NodeHandle to use - */ - void validateParameterSet(const ros::NodeHandle &nh); - - /** - * The tf frame under which the images were published - */ - std::string camera_frame_; - - /** - * The DeviceUserID of the camera. If empty, the first camera found in the - * device list will be used - */ - std::string device_user_id_; - - std::string serial_number_; - - /** - * The desired publisher frame rate if listening to the topics. - * This parameter can only be set once at startup - * Calling the GrabImages-Action can result in a higher framerate - */ - double frame_rate_; - - /** - * The CameraInfo URL (Uniform Resource Locator) where the optional - * intrinsic camera calibration parameters are stored. This URL string will - * be parsed from the CameraInfoManager: - * http://wiki.ros.org/camera_info_manager - */ - std::string camera_info_url_; - - /** - * The encoding of the pixels -- channel meaning, ordering, size taken - * from the list of strings in include/sensor_msgs/image_encodings.h - * The supported encodings are 'mono8', 'bgr8', 'rgb8', 'bayer_bggr8', - * 'bayer_gbrg8', 'bayer_rggb8' and 'yuv422' - */ - std::string image_encoding_; -}; +// class ArenaCameraParameter { +// public: +// ArenaCameraParameter(); + +// virtual ~ArenaCameraParameter(); + +// /** +// * Read the parameters from the parameter server. +// * If invalid parameters can be detected, the interface will reset them +// * to the default values. +// * @param pnh the **private** ros::NodeHandle to use +// */ +// void readFromRosParameterServer(const ros::NodeHandle &pnh); + +// /// Getter for the device_user_id param +// const std::string &deviceUserID() const { return device_user_id_; } + +// /// Getter for the serial number param +// const std::string &serialNumber() const { return serial_number_; } + +// int mtuSize() const { return mtu_size_; } + +// /** +// * Getter for the string describing the shutter mode +// */ +// std::string shutterModeString() const; + +// // Getter for the camera_frame_ set from ros-parameter server +// const std::string &cameraFrame() const { return camera_frame_; } + +// // Getter for the frame_rate_ read from ros-parameter server +// const double &frameRate() const { return frame_rate_; } + +// // Getter for the image_encoding_ read from ros-parameter server +// const std::string &imageEncoding() const { return image_encoding_; } + +// /** +// * Setter for the frame_rate_ initially set from ros-parameter server +// * The frame rate needs to be updated with the value the camera supports +// */ +// // void setFrameRate(const ros::NodeHandle& nh, const double& frame_rate); +// void setFrameRate(const double &frame_rate); + +// // Getter for the camera_info_url set from ros-parameter server +// const std::string &cameraInfoURL() const { return camera_info_url_; } + +// /** +// * Setter for the camera_info_url_ if a new CameraInfo-Msgs Object is +// * provided via the SetCameraInfo-service from the CameraInfoManager +// */ +// void setCameraInfoURL(const ros::NodeHandle &nh, +// const std::string &camera_info_url); + +// public: +// /** Binning factor to get downsampled images. It refers here to any camera +// * setting which combines rectangular neighborhoods of pixels into larger +// * "super-pixels." It reduces the resolution of the output image to +// * (width / binning_x) x (height / binning_y). +// * The default values binning_x = binning_y = 0 are considered the same +// * as binning_x = binning_y = 1 (no subsampling). +// */ +// size_t binning_x_; +// size_t binning_y_; + +// /** +// * Flags which indicate if the binning factors are provided and hence +// * should be set during startup +// */ +// bool binning_x_given_; +// bool binning_y_given_; + +// bool image_encoding_given_; + +// /** +// * Factor that describes the image downsampling to speed up the exposure +// * search to find the desired brightness. +// * The smallest window height is img_rows/downsampling_factor +// */ +// int downsampling_factor_exp_search_; + +// // ####################################################################### +// // ###################### Image Intensity Settings ###################### +// // ####################################################################### +// // The following settings do *NOT* have to be set. Each camera has default +// // values which provide an automatic image adjustment +// // If one would like to adjust image brightness, it is not +// // ####################################################################### + +// /** +// * The target gain in percent of the maximal value the camera supports +// * For USB-Cameras, the gain is in dB, for GigE-Cameras it is given in so +// * called 'device specific units'. +// */ +// double gain_; + +// /** +// * Flag which indicates if the gain value is provided and hence should be +// * set during startup +// */ +// bool gain_given_; + +// /** +// * Gamma correction of pixel intensity. +// * Adjusts the brightness of the pixel values output by the camera's sensor +// * to account for a non-linearity in the human perception of brightness or +// * of the display system (such as CRT). +// */ +// double gamma_; + +// /** +// * Flag which indicates if the gamma correction value is provided and +// * hence should be set during startup +// */ +// bool gamma_given_; + +// /** +// * The average intensity value of the images. It depends on the exposure +// * time as well as the gain setting. If 'exposure' is provided, the +// * interface will try to reach the desired brightness by only varying the +// * gain. (What may often fail, because the range of possible exposure +// * values is many times higher than the gain range). +// * If 'gain' is provided, the interface will try to reach the desired +// * brightness by only varying the exposure time. If gain AND exposure are +// * given, it is not possible to reach the brightness, because both are +// * assumed to be fix. +// */ +// int brightness_; + +// /** +// * Flag which indicates if the average brightness is provided and hence +// * should be set during startup +// */ +// bool brightness_given_; + +// /** +// * Only relevant, if 'brightness' is set as ros-parameter: +// * The brightness_continuous flag controls the auto brightness function. +// * If it is set to false, the brightness will only be reached once. +// * Hence changing light conditions lead to changing brightness values. +// * If it is set to true, the given brightness will be reached continuously, +// * trying to adapt to changing light conditions. This is only possible for +// * values in the possible auto range of the arena API which is +// * e.g. [50 - 205] for acA2500-14um and acA1920-40gm +// */ +// bool brightness_continuous_; + +// /** +// * Only relevant, if 'brightness' is given as ros-parameter: +// * If the camera should try to reach and / or keep the brightness, hence +// * adapting to changing light conditions, at least one of the following +// * flags must be set. If both are set, the interface will use the profile +// * that tries to keep the gain at minimum to reduce white noise. +// * The exposure_auto flag indicates, that the desired brightness will +// * be reached by adapting the exposure time. +// * The gain_auto flag indicates, that the desired brightness will be +// * reached by adapting the gain. +// */ +// bool exposure_auto_; +// bool gain_auto_; + +// bool enable_lut_; +// // ####################################################################### + +// double exposure_ms_; +// double auto_exposure_max_ms_; + +// /** +// * The MTU size. Only used for GigE cameras. +// * To prevent lost frames the camera has to be configured +// * with the MTU size the network card supports. A value greater 3000 +// * should be good (1500 for RaspberryPI) +// */ +// int mtu_size_; + +// /** +// * The inter-package delay in ticks. Only used for GigE cameras. +// * To prevent lost frames it should be greater 0. +// * For most of GigE-Cameras, a value of 1000 is reasonable. +// * For GigE-Cameras used on a RaspberryPI this value should be set to 11772 +// */ +// int inter_pkg_delay_; + +// /** +// Shutter mode +// */ +// SHUTTER_MODE shutter_mode_; + +// /** +// * Flag that indicates if the camera has been calibrated and the intrinsic +// * calibration matrices are available +// */ +// bool has_intrinsic_calib_; + +// /** +// * Flag that indicates if the camera has a flash connected which should be +// on +// * on exposure Only supported for GigE cameras. Default: false +// */ +// bool auto_flash_; +// /** +// * Flag that indicates if the camera, when using auto_flash == true, a +// flash +// * connected on line 2 which should be on on exposure Only supported for +// GigE +// * cameras. Default: true +// */ +// bool auto_flash_line_2_; +// /** +// * Flag that indicates if the camera has, when using auto_flash == true, a +// * flash connected on line 3 which should be on on exposure Only supported +// for +// * GigE cameras. Default: true +// */ +// bool auto_flash_line_3_; + +// protected: +// /** +// * Validates the parameter set found on the ros parameter server. +// * If invalid parameters can be detected, the interface will reset them +// * to the default values. +// * @param nh the ros::NodeHandle to use +// */ +// void validateParameterSet(const ros::NodeHandle &nh); + +// /** +// * The tf frame under which the images were published +// */ +// std::string camera_frame_; + +// /** +// * The DeviceUserID of the camera. If empty, the first camera found in the +// * device list will be used +// */ +// std::string device_user_id_; + +// std::string serial_number_; + +// /** +// * The desired publisher frame rate if listening to the topics. +// * This parameter can only be set once at startup +// * Calling the GrabImages-Action can result in a higher framerate +// */ +// double frame_rate_; + +// /** +// * The CameraInfo URL (Uniform Resource Locator) where the optional +// * intrinsic camera calibration parameters are stored. This URL string will +// * be parsed from the CameraInfoManager: +// * http://wiki.ros.org/camera_info_manager +// */ +// std::string camera_info_url_; + +// /** +// * The encoding of the pixels -- channel meaning, ordering, size taken +// * from the list of strings in include/sensor_msgs/image_encodings.h +// * The supported encodings are 'mono8', 'bgr8', 'rgb8', 'bayer_bggr8', +// * 'bayer_gbrg8', 'bayer_rggb8' and 'yuv422' +// */ +// std::string image_encoding_; +//}; } // namespace arena_camera diff --git a/arena_camera/include/arena_camera/encoding_conversions.h b/arena_camera/include/arena_camera/encoding_conversions.h index 0ce1aae..92385f7 100644 --- a/arena_camera/include/arena_camera/encoding_conversions.h +++ b/arena_camera/include/arena_camera/encoding_conversions.h @@ -50,6 +50,10 @@ bool ros2GenAPI(const std::string& ros_enc, std::string& gen_api_enc); */ 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/package.xml b/arena_camera/package.xml index eb22e57..9a4de1d 100644 --- a/arena_camera/package.xml +++ b/arena_camera/package.xml @@ -11,14 +11,19 @@ BSD 3-Clause - http://wiki.ros.org/arena_camera + colcon + ament_cmake rclcpp + rclcpp_components imaging_msgs camera_control_msgs + ament_lint_auto + ament_lint_common + - diff --git a/arena_camera/src/arena_camera_parameter.cpp b/arena_camera/src/arena_camera_parameter.cpp index 86e4f5c..9c58cd4 100644 --- a/arena_camera/src/arena_camera_parameter.cpp +++ b/arena_camera/src/arena_camera_parameter.cpp @@ -1,6 +1,8 @@ /****************************************************************************** - * Software License Agreement (BSD License) + * 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 @@ -28,337 +30,356 @@ *****************************************************************************/ // STD -#include // std::boolalpha +// #include // std::boolalpha -// ROS -#include +// // ROS +// #include -// Arena node -#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_); +// 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 double& frame_rate) { - frame_rate_ = frame_rate; -} +// // void ArenaCameraParameter::setFrameRate(const ros::NodeHandle& nh, +// // const double& frame_rate) { +// // frame_rate_ = frame_rate; +// // nh.setParam("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_); -} +// 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 new file mode 100644 index 0000000..ca97223 --- /dev/null +++ b/arena_camera/src/base_node.cpp @@ -0,0 +1,1417 @@ +/****************************************************************************** + * 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 +// #include +// #include +// #include +// #include +// #include + +// ROS2 +#include +// #include +// #include + +// Arena +#include +#include +#include + +// Arena node +#include "arena_camera/arena_camera_nodes.h" +#include "arena_camera/encoding_conversions.h" + +// using diagnostic_msgs::DiagnosticStatus; + +namespace arena_camera { + +// using sensor_msgs::CameraInfo; +// using sensor_msgs::CameraInfoPtr; + +ArenaCameraBaseNode::ArenaCameraBaseNode(const std::string &node_name, + const rclcpp::NodeOptions &options) + : Node(node_name, options), + pSystem_(nullptr), + pDevice_(nullptr), + pNodeMap_(nullptr), + is_streaming_(false) +// arena_camera_parameter_set_(), +// set_user_output_srvs_(), +// it_(nullptr), +// img_raw_pub_(), +// pinhole_model_(), +// camera_info_manager_(nullptr), +// sampling_indices_() +{ + // metadata_pub_ = + // nh.advertise("imaging_metadata", + // 1); + + // it_.reset(new image_transport::ImageTransport(nh)); + // img_raw_pub_ = it_->advertiseCamera("image_raw", 1); + + // camera_info_manager_ = new + // camera_info_manager::CameraInfoManager(nh); + + // diagnostics_updater_.setHardwareID("none"); + // diagnostics_updater_.add("camera_availability", this, + // &ArenaCameraBaseNode::create_diagnostics); + // diagnostics_updater_.add( + // "intrinsic_calibration", this, + // &ArenaCameraBaseNode::create_camera_info_diagnostics); + // diagnostics_trigger_ = nh.createTimer( + // ros::Duration(2), + // &ArenaCameraBaseNode::diagnostics_timer_callback_, + // this); + + // // Initialize parameters from parameter server + // arena_camera_parameter_set_.readFromRosParameterServer(pnh); + + this->declare_parameter("device_user_id", ""); + this->declare_parameter("serial_number", ""); + + std::string device_user_id, serial_number; + + // Open the Arena SDK + pSystem_ = Arena::OpenSystem(); + pSystem_->UpdateDevices(100); + if (pSystem_->GetDevices().size() == 0) { + RCLCPP_FATAL(this->get_logger(), "Did not detect any cameras!!"); + return; + } + + if (this->get_parameter("device_user_id", device_user_id)) { + if (!registerCameraByUserId(device_user_id)) { + RCLCPP_FATAL_STREAM(this->get_logger(), + "Unable to find a camera with DeviceUserId\"" + << device_user_id << "\""); + return; + } + } else if (this->get_parameter("serial_number", serial_number)) { + if (!registerCameraBySerialNumber(serial_number)) { + RCLCPP_FATAL_STREAM( + this->get_logger(), + "Unable to find a camera with Serial Number" << serial_number); + return; + } + } else { + if (!registerCameraByAuto()) { + RCLCPP_FATAL(this->get_logger(), + "Unable to find any cameras to register"); + return; + } + } + + // Validate that the camera is from Lucid + //(otherwise the Arena SDK will segfault) + const auto device_vendor_name = Arena::GetNodeValue( + pDevice_->GetNodeMap(), "DeviceVendorName"); + if (device_vendor_name != "Lucid Vision Labs") { + RCLCPP_FATAL_STREAM( + this->get_logger(), + "Hm, this doesn't appear to be a Lucid Vision camera, got vendor name: " + << device_vendor_name); + } + + // if (!configureCamera()) { + // Node_FATAL_STREAM("Unable to configure camera"); + // return; + // } + + // _dynReconfigureServer = + // std::make_shared(pnh); + // _dynReconfigureServer->setCallback(boost::bind( + // &ArenaCameraBaseNode::reconfigureCallbackWrapper, this, _1, + // _2)); +} + +ArenaCameraBaseNode::~ArenaCameraBaseNode() { + if (pDevice_ != nullptr) { + pSystem_->DestroyDevice(pDevice_); + } + + if (pSystem_ != nullptr) { + Arena::CloseSystem(pSystem_); + } +} + +bool ArenaCameraBaseNode::registerCameraByUserId( + const std::string &device_user_id_to_open) { + rcpputils::assert_true(pSystem_); + std::vector deviceInfos = pSystem_->GetDevices(); + rcpputils::assert_true(deviceInfos.size() > 0); + + RCLCPP_INFO_STREAM( + this->get_logger(), + "Connecting to camera with DeviceUserId" << device_user_id_to_open); + + std::vector::iterator it; + + for (auto &dev : deviceInfos) { + const std::string device_user_id(dev.UserDefinedName()); + // Must be an exact match + if (0 == device_user_id_to_open.compare(device_user_id)) { + RCLCPP_INFO_STREAM(this->get_logger(), + "Found the desired camera with DeviceUserID " + << device_user_id_to_open << ": "); + + pDevice_ = pSystem_->CreateDevice(dev); + return true; + } + } + + RCLCPP_ERROR_STREAM( + this->get_logger(), + "Couldn't find the camera that matches the " + << "given DeviceUserID: \"" << device_user_id_to_open << "\"! " + << "Either the ID is wrong or the cam is not yet connected"); + return false; +} + +bool ArenaCameraBaseNode::registerCameraBySerialNumber( + const std::string &serial_number) { + rcpputils::assert_true(pSystem_); + std::vector deviceInfos = pSystem_->GetDevices(); + rcpputils::assert_true(deviceInfos.size() > 0); + + RCLCPP_INFO_STREAM( + this->get_logger(), + "Connecting to camera with Serial Number " << serial_number); + + for (auto &dev : deviceInfos) { + if (0 == serial_number.compare(dev.SerialNumber())) { + RCLCPP_INFO_STREAM( + this->get_logger(), + "Found the desired camera with Serial Number " << serial_number); + + pDevice_ = pSystem_->CreateDevice(dev); + return true; + } + } + + RCLCPP_ERROR_STREAM( + this->get_logger(), + "Couldn't find the camera that matches the " + << "given Serial Number: " << serial_number << "! " + << "Either the ID is wrong or the cam is not yet connected"); + return false; +} + +bool ArenaCameraBaseNode::registerCameraByAuto() { + rcpputils::assert_true(pSystem_); + std::vector deviceInfos = pSystem_->GetDevices(); + rcpputils::assert_true(deviceInfos.size() > 0); + + int i = 0; + RCLCPP_DEBUG_STREAM(this->get_logger(), + "Found " << deviceInfos.size() << " cameras"); + for (auto &dev : deviceInfos) { + RCLCPP_INFO_STREAM(this->get_logger(), i++ << ": " << dev.SerialNumber() + << " " + << dev.UserDefinedName()); + } + + for (auto &dev : deviceInfos) { + if (dev.VendorName() == "Lucid Vision Labs") { + pDevice_ = pSystem_->CreateDevice(dev); + RCLCPP_INFO_STREAM( + this->get_logger(), + "Connecting to first autodetected Lucid Vision camera: Serial Number " + << dev.SerialNumber() << " ; User ID: " << dev.UserDefinedName()); + return true; + } + } + + return false; +} + +// bool ArenaCameraBaseNode::configureCamera() { +// ros::NodeHandle nh = getNodeHandle(); +// auto pNodeMap = pDevice_->GetNodeMap(); + +// // **NOTE** only configuration which is not also accessible through +// // dynamic_reconfigure. Those will be handled in the callback +// // when it is called for the first time at node startup. + +// try { +// Node_INFO_STREAM( +// " Device model: " << Arena::GetNodeValue( +// pDevice_->GetNodeMap(), "DeviceModelName")); +// Node_INFO_STREAM( +// "Device firmware: " << Arena::GetNodeValue( +// pDevice_->GetNodeMap(), "DeviceFirmwareVersion")); + +// // +// // Arena device prior streaming settings +// // + +// if (Arena::GetNodeValue( +// pDevice_->GetNodeMap(), "DeviceTLType") == "GigEVision") { +// Node_INFO("GigE device, performing GigE specific configuration:"); + +// // Set Jumbo frames (this is only relevant for GigE cameras.) +// auto pPacketSize = pNodeMap->GetNode("DeviceStreamChannelPacketSize"); +// if (GenApi::IsWritable(pPacketSize)) { +// Node_INFO_STREAM(" -> Setting MTU to " +// << arena_camera_parameter_set_.mtuSize()); +// Arena::SetNodeValue(pNodeMap, +// "DeviceStreamChannelPacketSize", +// arena_camera_parameter_set_.mtuSize()); +// } else { +// Node_INFO(" -> Camera MTU is not writeable"); +// } +// } + +// auto payloadSize = Arena::GetNodeValue(pNodeMap, "PayloadSize"); +// Node_INFO_STREAM("Expected payload size: " << payloadSize); + +// // enable stream auto negotiate packet size +// Node_DEBUG("Enabling auto-negotiation of packet size"); +// Arena::SetNodeValue(pDevice_->GetTLStreamNodeMap(), +// "StreamAutoNegotiatePacketSize", true); + +// // enable stream packet resend +// Node_DEBUG("Enabling packet resend"); +// Arena::SetNodeValue(pDevice_->GetTLStreamNodeMap(), +// "StreamPacketResendEnable", true); + +// // +// // PIXELFORMAT +// // +// setImageEncoding(arena_camera_parameter_set_.imageEncoding()); + +// if (encoding_conversions::isHDR( +// arena_camera_parameter_set_.imageEncoding())) { +// hdr_metadata_pub_ = nh.advertise( +// "hdr_imaging_metadata", 1); +// } + +// // +// // TRIGGER MODE +// // +// GenApi::CStringPtr pTriggerMode = pNodeMap->GetNode("TriggerMode"); +// if (GenApi::IsWritable(pTriggerMode)) { +// Arena::SetNodeValue(pNodeMap, "TriggerMode", "On"); +// Arena::SetNodeValue(pNodeMap, "TriggerSource", +// "Software"); +// } + +// //!! Parameters controlled by param / dynamic reonfigure are not set here +// //!! Assume there will be an immediate call from dynamic reconfigure + +// // LUT +// Node_INFO_STREAM( +// (arena_camera_parameter_set_.enable_lut_ ? "Enabling" : "Disabling") +// << " camera LUT"); +// enableLUT(arena_camera_parameter_set_.enable_lut_); + +// // +// ------------------------------------------------------------------------ + +// // +// // Initial setting of the CameraInfo-msg, assuming no calibration given +// CameraInfo initial_cam_info; +// // initializeCameraInfo(initial_cam_info); +// camera_info_manager_->setCameraInfo(initial_cam_info); + +// if (arena_camera_parameter_set_.cameraInfoURL().empty() || +// !camera_info_manager_->validateURL( +// arena_camera_parameter_set_.cameraInfoURL())) { +// Node_INFO_STREAM("CameraInfoURL needed for rectification! ROS-Param: " +// << "'" << nh.getNamespace() << "/camera_info_url' = +// '" +// << arena_camera_parameter_set_.cameraInfoURL() +// << "' is invalid!"); +// Node_DEBUG_STREAM("CameraInfoURL should have following style: " +// << "'file:///full/path/to/local/file.yaml' or " +// << +// "'file://${ROS_HOME}/camera_info/${NAME}.yaml'"); +// Node_WARN_STREAM("Will only provide distorted /image_raw images!"); +// } else { +// // override initial camera info if the url is valid +// if (camera_info_manager_->loadCameraInfo( +// arena_camera_parameter_set_.cameraInfoURL())) { +// // setupRectification(); +// // set the correct tf frame_id +// CameraInfoPtr cam_info( +// new CameraInfo(camera_info_manager_->getCameraInfo())); +// cam_info->header.frame_id = img_raw_msg_.header.frame_id; +// camera_info_manager_->setCameraInfo(*cam_info); +// } else { +// Node_WARN_STREAM("Will only provide distorted /image_raw images!"); +// } +// } + +// if (arena_camera_parameter_set_.binning_x_given_) { +// size_t reached_binning_x; +// if (setBinningX(arena_camera_parameter_set_.binning_x_, +// reached_binning_x)) { +// Node_INFO_STREAM("Setting horizontal binning_x to " +// << arena_camera_parameter_set_.binning_x_); +// Node_WARN_STREAM( +// "The image width of the camera_info-msg will " +// << "be adapted, so that the binning_x value in this msg remains +// 1"); +// } +// } + +// if (arena_camera_parameter_set_.binning_y_given_) { +// size_t reached_binning_y; +// if (setBinningY(arena_camera_parameter_set_.binning_y_, +// reached_binning_y)) { +// Node_INFO_STREAM("Setting vertical binning_y to " +// << arena_camera_parameter_set_.binning_y_); +// Node_WARN_STREAM( +// "The image height of the camera_info-msg will " +// << "be adapted, so that the binning_y value in this msg remains +// 1"); +// } +// } + +// // if (arena_camera_parameter_set_.image_encoding_given_) +// // { +// // float reached_image_encoding; +// // if +// (setImageEncoding(arena_camera_parameter_set_.image_encoding_)) +// // { +// // Node_INFO_STREAM("Setting encoding to " +// // << +// // arena_camera_parameter_set_.image_encoding_); +// // } +// // } + +// Arena::SetNodeValue(pDevice_->GetTLStreamNodeMap(), +// "StreamBufferHandlingMode", +// "NewestOnly"); + +// // bool isTriggerArmed = false; + +// // if (GenApi::IsWritable(pTriggerMode)) { +// // do { +// // isTriggerArmed = Arena::GetNodeValue(pNodeMap, +// "TriggerArmed"); +// // } while (isTriggerArmed == false); +// // // Arena::ExecuteNode(pNodeMap, "TriggerSoftware"); +// // } + +// // pImage_ = pDevice_->GetImage(5000); +// // pData_ = pImage_->GetData(); + +// // img_raw_msg_.data.resize(img_raw_msg_.height * img_raw_msg_.step); +// // memcpy(&img_raw_msg_.data[0], pImage_->GetData(), +// // img_raw_msg_.height * img_raw_msg_.step); +// } catch (GenICam::GenericException &e) { +// Node_ERROR_STREAM("Error while configuring camera: \r\n" +// << e.GetDescription()); +// return false; +// } + +// // +// -------------------------------------------------------------------------- + +// img_raw_msg_.header.frame_id = arena_camera_parameter_set_.cameraFrame(); +// // Encoding of pixels -- channel meaning, ordering, size +// // taken from the list of strings in include/sensor_msgs/image_encodings.h +// // img_raw_msg_.height = pImage_->GetHeight(); +// // img_raw_msg_.width = pImage_->GetWidth(); +// // step = full row length in bytes, img_size = (step * rows), +// imagePixelDepth +// // already contains the number of channels +// // img_raw_msg_.step = img_raw_msg_.width * (pImage_->GetBitsPerPixel() / +// 8); + +// if (!camera_info_manager_->setCameraName(std::string( +// Arena::GetNodeValue(pNodeMap, "DeviceUserID") +// .c_str()))) { +// // valid name contains only alphanumeric signs and '_' +// Node_WARN_STREAM("[" +// << +// std::string(Arena::GetNodeValue( +// pNodeMap, "DeviceUserID") +// .c_str()) +// << "] name not valid for camera_info_manager"); +// } + +// // Node_INFO("=== Startup settings ==="); +// // Node_INFO_STREAM("encoding = " << currentROSEncoding()); +// // Node_INFO_STREAM("binning = [" << currentBinningX() << " x " +// // << currentBinningY() << "]"); +// // Node_INFO_STREAM("exposure = " << currentExposure() << " us"); +// // Node_INFO_STREAM("gain = " << currentGain()); +// // Node_INFO_STREAM("gamma = " << currentGamma()); +// // Node_INFO_STREAM( +// // "shutter mode = " << +// arena_camera_parameter_set_.shutterModeString()); +// // Node_INFO("========================"); + +// // pDevice_->RequeueBuffer(pImage_); +// return true; +// } + +// void ArenaCameraBaseNode::startStreaming() { +// if (!is_streaming_) { +// pDevice_->StartStream(); +// is_streaming_ = true; +// } +// } + +// void ArenaCameraBaseNode::stopStreaming() { +// if (is_streaming_) { +// pDevice_->StopStream(); +// is_streaming_ = false; +// } +// } + +// sensor_msgs::RegionOfInterest ArenaCameraBaseNode::currentROI() { +// sensor_msgs::RegionOfInterest roi; +// // \todo{amarburg} Broke this by getting rid of a member pImage_ +// // Need to save as state? +// // roi.width = pImage_->GetWidth(); +// // roi.height = pImage_->GetHeight(); +// // ; +// // roi.x_offset = pImage_->GetOffsetX(); +// // roi.y_offset = pImage_->GetOffsetY(); +// return roi; +// } + +// float ArenaCameraBaseNode::currentGamma() { +// GenApi::CFloatPtr pGamma = pDevice_->GetNodeMap()->GetNode("Gamma"); + +// if (!pGamma || !GenApi::IsReadable(pGamma)) { +// Node_WARN_STREAM("No gamma value, returning -1"); +// return -1.; +// } else { +// float gammaValue = pGamma->GetValue(); +// return gammaValue; +// } +// } + +// int64_t ArenaCameraBaseNode::currentBinningX() { +// GenApi::CIntegerPtr BinningHorizontal = +// pDevice_->GetNodeMap()->GetNode("BinningHorizontal"); + +// if (!BinningHorizontal || !GenApi::IsReadable(BinningHorizontal)) { +// Node_WARN_STREAM("No binningY value, returning -1"); +// return -1; +// } else { +// float binningXValue = BinningHorizontal->GetValue(); +// return binningXValue; +// } +// } + +// int64_t ArenaCameraBaseNode::currentBinningY() { +// GenApi::CIntegerPtr BinningVertical = +// pDevice_->GetNodeMap()->GetNode("BinningVertical"); + +// if (!BinningVertical || !GenApi::IsReadable(BinningVertical)) { +// Node_WARN_STREAM("No binningY value, returning -1"); +// return -1; +// } else { +// float binningYValue = BinningVertical->GetValue(); +// return binningYValue; +// } +// } + +// float ArenaCameraBaseNode::currentGain() { +// GenApi::CFloatPtr pGain = pDevice_->GetNodeMap()->GetNode("Gain"); + +// if (!pGain || !GenApi::IsReadable(pGain)) { +// Node_WARN_STREAM("No gain value"); +// return -1.; +// } else { +// float gainValue = pGain->GetValue(); +// return gainValue; +// } +// } + +// std::string ArenaCameraBaseNode::currentROSEncoding() { +// std::string gen_api_encoding(Arena::GetNodeValue( +// pDevice_->GetNodeMap(), "PixelFormat")); +// std::string ros_encoding(""); +// if (!encoding_conversions::genAPI2Ros(gen_api_encoding, ros_encoding)) { +// std::stringstream ss; +// ss << "No ROS equivalent to GenApi encoding '" << gen_api_encoding +// << "' found! This is bad because this case " +// "should never occur!"; +// throw std::runtime_error(ss.str()); +// return "NO_ENCODING"; +// } +// return ros_encoding; +// } + +// bool ArenaCameraBaseNode::setImageEncoding(const std::string &ros_encoding) { +// std::string gen_api_encoding; +// bool conversion_found = +// encoding_conversions::ros2GenAPI(ros_encoding, gen_api_encoding); +// if (!conversion_found) { +// if (ros_encoding.empty()) { +// return false; +// } else { +// std::string fallbackPixelFormat = +// Arena::GetNodeValue(pDevice_->GetNodeMap(), +// "PixelFormat") +// .c_str(); +// Node_ERROR_STREAM( +// "Can't convert ROS encoding '" +// << ros_encoding +// << "' to a corresponding GenAPI encoding! Will use current " +// << "pixel format ( " << fallbackPixelFormat << " ) as fallback!"); +// return false; +// } +// } +// try { +// GenApi::CEnumerationPtr pPixelFormat = +// pDevice_->GetNodeMap()->GetNode("PixelFormat"); +// if (GenApi::IsWritable(pPixelFormat)) { +// Arena::SetNodeValue( +// pDevice_->GetNodeMap(), "PixelFormat", gen_api_encoding.c_str()); +// if (currentROSEncoding() == "16UC3" || currentROSEncoding() == "16UC4") +// Node_WARN_STREAM( +// "ROS grabbing image data from 3D pixel format, unable to display +// " "in image viewer"); +// } +// } catch (const GenICam::GenericException &e) { +// Node_ERROR_STREAM("An exception while setting target image encoding to '" +// << ros_encoding +// << "' occurred: " << e.GetDescription()); +// return false; +// } + +// Node_INFO("Have configured image_encoding"); + +// if (arena_camera::encoding_conversions::isHDR(ros_encoding)) { +// Node_INFO_STREAM("Requested HDR encoding \"" +// << ros_encoding << "\", enabling HDR mode in +// camera"); + +// try { +// auto pNodeMap = pDevice_->GetNodeMap(); + +// // GenApi::CStringPtr pHDROutput = pNodeMap->GetNode("HDROutput"); +// // if (GenApi::IsWritable(pHDROutput)) { +// // Arena::SetNodeValue(pNodeMap, "HDROutput", +// // "HDR"); +// // } + +// // Enable HDR image enhancement +// Arena::SetNodeValue(pNodeMap, "HDRImageEnhancementEnable", true); +// Arena::SetNodeValue(pNodeMap, "HDRTuningEnable", false); + +// Arena::SetNodeValue(pNodeMap, "HDROutput", "HDR"); + +// } catch (GenICam::GenericException &e) { +// Node_ERROR_STREAM("Error while configuring camera: " +// << e.GetDescription() +// << ", is this camera capable of HDR?"); +// return false; +// } +// } + +// return true; +// } + +// // Note that streaming must be stopped before updating frame rate +// void ArenaCameraBaseNode::updateFrameRate() { +// try { +// ros::NodeHandle nh = getNodeHandle(); +// auto pNodeMap = pDevice_->GetNodeMap(); + +// // const bool was_streaming = is_streaming_; +// // stopStreaming(); + +// auto cmdlnParamFrameRate = arena_camera_parameter_set_.frameRate(); +// auto currentFrameRate = +// Arena::GetNodeValue(pNodeMap, "AcquisitionFrameRate"); +// auto maximumFrameRate = +// GenApi::CFloatPtr(pNodeMap->GetNode("AcquisitionFrameRate"))->GetMax(); + +// // requested framerate larger than device max so we trancate it +// if (cmdlnParamFrameRate >= maximumFrameRate) { +// arena_camera_parameter_set_.setFrameRate(maximumFrameRate); + +// Node_WARN( +// "Desired framerate %.2f Hz (rounded) is higher than max possible. " +// "Will limit " +// "framerate device max : %.2f Hz (rounded)", +// cmdlnParamFrameRate, maximumFrameRate); +// } +// // special case: +// // dues to inacurate float comparision we skip. If we set it it might +// // throw becase it could be a lil larger than the max avoid the exception +// // (double accuracy issue when setting the node) request frame rate very +// // close to device max +// else if (cmdlnParamFrameRate == maximumFrameRate) { +// Node_INFO("Framerate is %.2f Hz", cmdlnParamFrameRate); +// } +// // requested max frame rate +// else if (cmdlnParamFrameRate == +// -1) // speacial for max frame rate available +// { +// arena_camera_parameter_set_.setFrameRate(maximumFrameRate); + +// Node_WARN("Framerate is set to device max : %.2f Hz", +// maximumFrameRate); +// } + +// // requested framerate is valid so we set it to the device +// try { +// if (!Arena::GetNodeValue(pNodeMap, "AcquisitionFrameRateEnable")) +// { +// Node_INFO( +// "\"AcquisitionFrameRateEnable\" not true, trying to enable"); +// Arena::SetNodeValue(pNodeMap, "AcquisitionFrameRateEnable", +// true); +// } +// Arena::SetNodeValue(pNodeMap, "AcquisitionFrameRate", +// arena_camera_parameter_set_.frameRate()); +// } catch (GenICam::GenericException &e) { +// Node_INFO_STREAM("Exception while changing frame rate: " << e.what()); +// } +// Node_INFO_STREAM( +// "Framerate has been set to " +// << Arena::GetNodeValue(pNodeMap, "AcquisitionFrameRate") +// << " Hz"); + +// // if (was_streaming) startStreaming(); + +// } catch (GenICam::GenericException &e) { +// Node_INFO_STREAM("Exception while changing frame rate: " << e.what()); +// } +// } + +// // void ArenaCameraBaseNode::initializeCameraInfo( +// // sensor_msgs::CameraInfo &cam_info_msg) { +// // // http://www.ros.org/reps/rep-0104.html +// // // If the camera is uncalibrated, the matrices D, K, R, P should be left +// // // zeroed out. In particular, clients may assume that K[0] == 0.0 +// // // indicates an uncalibrated camera. +// // cam_info_msg.header.frame_id = cameraFrame(); +// // cam_info_msg.header.stamp = ros::Time::now(); + +// // // The image dimensions with which the camera was calibrated. Normally +// // // this will be the full camera resolution in pixels. They remain fix, +// // // even if binning is applied rows and colums +// // cam_info_msg.height = +// // Arena::GetNodeValue(pDevice_->GetNodeMap(), "Height"); +// // cam_info_msg.width = +// // Arena::GetNodeValue(pDevice_->GetNodeMap(), "Width"); + +// // // The distortion model used. Supported models are listed in +// // // sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a +// // // simple model of radial and tangential distortion - is sufficient. +// // // Empty D and distortion_model indicate that the CameraInfo cannot be +// // // used to rectify points or images, either because the camera is not +// // // calibrated or because the rectified image was produced using an +// // // unsupported distortion model, e.g. the proprietary one used by +// // // Bumblebee cameras [http://www.ros.org/reps/rep-0104.html]. +// // cam_info_msg.distortion_model = ""; + +// // // The distortion parameters, size depending on the distortion model. +// // // For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3) -> +// // // float64[] D. +// // cam_info_msg.D = std::vector(5, 0.); + +// // // Intrinsic camera matrix for the raw (distorted) images. +// // // [fx 0 cx] +// // // K = [ 0 fy cy] --> 3x3 row-major matrix +// // // [ 0 0 1] +// // // Projects 3D points in the camera coordinate frame to 2D pixel +// // // coordinates using the focal lengths (fx, fy) and principal point (cx, +// // // cy). +// // cam_info_msg.K.assign(0.0); + +// // // Rectification matrix (stereo cameras only) +// // // A rotation matrix aligning the camera coordinate system to the ideal +// // // stereo image plane so that epipolar lines in both stereo images are +// // // parallel. +// // cam_info_msg.R.assign(0.0); + +// // // Projection/camera matrix +// // // [fx' 0 cx' Tx] +// // // P = [ 0 fy' cy' Ty] --> # 3x4 row-major matrix +// // // [ 0 0 1 0] +// // // By convention, this matrix specifies the intrinsic (camera) matrix of +// // // the processed (rectified) image. That is, the left 3x3 portion is the +// // // normal camera intrinsic matrix for the rectified image. It projects +// 3D +// // // points in the camera coordinate frame to 2D pixel coordinates using +// the +// // // focal lengths (fx', fy') and principal point (cx', cy') - these may +// // // differ from the values in K. For monocular cameras, Tx = Ty = 0. +// // // Normally, monocular cameras will also have R = the identity and +// // // P[1:3,1:3] = K. For a stereo pair, the fourth column [Tx Ty 0]' is +// // // related to the position of the optical center of the second camera in +// // // the first camera's frame. We assume Tz = 0 so both cameras are in the +// // // same stereo image plane. The first camera always has Tx = Ty = 0. For +// // // the right (second) camera of a horizontal stereo pair, Ty = 0 and Tx +// = +// // // -fx' * B, where B is the baseline between the cameras. Given a 3D +// point +// // // [X Y Z]', the projection (x, y) of the point onto the rectified image +// // // is given by: [u v w]' = P * [X Y Z 1]' +// // // x = u / w +// // // y = v / w +// // // This holds for both images of a stereo pair. +// // cam_info_msg.P.assign(0.0); + +// // // Binning refers here to any camera setting which combines rectangular +// // // neighborhoods of pixels into larger "super-pixels." It reduces the +// // // resolution of the output image to (width / binning_x) x (height / +// // // binning_y). The default values binning_x = binning_y = 0 is +// considered +// // // the same as binning_x = binning_y = 1 (no subsampling). +// // // cam_info_msg.binning_x = currentBinningX(); +// // // cam_info_msg.binning_y = currentBinningY(); + +// // // Region of interest (subwindow of full camera resolution), given in +// full +// // // resolution (unbinned) image coordinates. A particular ROI always +// // // denotes the same window of pixels on the camera sensor, regardless of +// // // binning settings. The default setting of roi (all values 0) is +// // // considered the same as full resolution (roi.width = width, roi.height +// = +// // // height). + +// // // todo? do these has ti be set via +// // // Arena::GetNodeValue(pDevice_->GetNodeMap(), "OffsetX"); or +// so +// // // ? +// // cam_info_msg.roi.x_offset = cam_info_msg.roi.y_offset = 0; +// // cam_info_msg.roi.height = cam_info_msg.roi.width = 0; +// // } + +// //~~ Binning / ROI ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +// bool ArenaCameraBaseNode::setROI( +// const sensor_msgs::RegionOfInterest target_roi, +// sensor_msgs::RegionOfInterest &reached_roi) { +// boost::lock_guard lock(device_mutex_); +// // TODO: set ROI +// return true; +// } + +// bool ArenaCameraBaseNode::setBinningXValue(const size_t &target_binning_x, +// size_t &reached_binning_x) { +// try { +// GenApi::CIntegerPtr pBinningHorizontal = +// pDevice_->GetNodeMap()->GetNode("BinningHorizontal"); +// if (GenApi::IsWritable(pBinningHorizontal)) { +// size_t binning_x_to_set = target_binning_x; +// if (binning_x_to_set < pBinningHorizontal->GetMin()) { +// Node_WARN_STREAM("Desired horizontal binning_x factor(" +// << binning_x_to_set +// << ") unreachable! Setting to lower " +// << "limit: " << pBinningHorizontal->GetMin()); +// binning_x_to_set = pBinningHorizontal->GetMin(); +// } else if (binning_x_to_set > pBinningHorizontal->GetMax()) { +// Node_WARN_STREAM("Desired horizontal binning_x factor(" +// << binning_x_to_set +// << ") unreachable! Setting to upper " +// << "limit: " << pBinningHorizontal->GetMax()); +// binning_x_to_set = pBinningHorizontal->GetMax(); +// } + +// pBinningHorizontal->SetValue(binning_x_to_set); +// reached_binning_x = currentBinningX(); +// } else { +// Node_WARN_STREAM("Camera does not support binning. Will keep the " +// << "current settings"); +// reached_binning_x = currentBinningX(); +// } +// } + +// catch (const GenICam::GenericException &e) { +// Node_ERROR_STREAM("An exception while setting target horizontal " +// << "binning_x factor to " << target_binning_x +// << " occurred: " << e.GetDescription()); +// return false; +// } +// return true; +// } + +// bool ArenaCameraBaseNode::setBinningX(const size_t &target_binning_x, +// size_t &reached_binning_x) { +// boost::lock_guard lock(device_mutex_); + +// if (!setBinningXValue(target_binning_x, reached_binning_x)) { +// // retry till timeout +// ros::Rate r(10.0); +// ros::Time timeout(ros::Time::now() + ros::Duration(2.0)); +// while (ros::ok()) { +// if (setBinningXValue(target_binning_x, reached_binning_x)) { +// break; +// } +// if (ros::Time::now() > timeout) { +// Node_ERROR_STREAM("Error in setBinningX(): Unable to set target " +// << "binning_x factor before timeout"); +// CameraInfoPtr cam_info( +// new CameraInfo(camera_info_manager_->getCameraInfo())); +// cam_info->binning_x = currentBinningX(); +// camera_info_manager_->setCameraInfo(*cam_info); +// // img_raw_msg_.width = pImage_->GetWidth(); +// // step = full row length in bytes, img_size = (step * rows), +// // imagePixelDepth already contains the number of channels +// // img_raw_msg_.step = img_raw_msg_.width * +// // (pImage_->GetBitsPerPixel() / 8); +// return false; +// } +// r.sleep(); +// } +// } + +// return true; +// } + +// bool ArenaCameraBaseNode::setBinningYValue(const size_t &target_binning_y, +// size_t &reached_binning_y) { +// try { +// GenApi::CIntegerPtr pBinningVertical = +// pDevice_->GetNodeMap()->GetNode("BinningVertical"); +// if (GenApi::IsWritable(pBinningVertical)) { +// size_t binning_y_to_set = target_binning_y; +// if (binning_y_to_set < pBinningVertical->GetMin()) { +// Node_WARN_STREAM("Desired horizontal binning_y factor(" +// << binning_y_to_set +// << ") unreachable! Setting to lower " +// << "limit: " << pBinningVertical->GetMin()); +// binning_y_to_set = pBinningVertical->GetMin(); +// } else if (binning_y_to_set > pBinningVertical->GetMax()) { +// Node_WARN_STREAM("Desired horizontal binning_y factor(" +// << binning_y_to_set +// << ") unreachable! Setting to upper " +// << "limit: " << pBinningVertical->GetMax()); +// binning_y_to_set = pBinningVertical->GetMax(); +// } + +// pBinningVertical->SetValue(binning_y_to_set); +// reached_binning_y = currentBinningY(); +// } else { +// Node_WARN_STREAM("Camera does not support binning. Will keep the " +// << "current settings"); +// reached_binning_y = currentBinningY(); +// } +// } + +// catch (const GenICam::GenericException &e) { +// Node_ERROR_STREAM("An exception while setting target horizontal " +// << "binning_y factor to " << target_binning_y +// << " occurred: " << e.GetDescription()); +// return false; +// } +// return true; +// } + +// bool ArenaCameraBaseNode::setBinningY(const size_t &target_binning_y, +// size_t &reached_binning_y) { +// boost::lock_guard lock(device_mutex_); + +// if (!setBinningYValue(target_binning_y, reached_binning_y)) { +// // retry till timeout +// ros::Rate r(10.0); +// ros::Time timeout(ros::Time::now() + ros::Duration(2.0)); +// while (ros::ok()) { +// if (setBinningYValue(target_binning_y, reached_binning_y)) { +// break; +// } +// if (ros::Time::now() > timeout) { +// Node_ERROR_STREAM("Error in setBinningY(): Unable to set target " +// << "binning_y factor before timeout"); +// CameraInfoPtr cam_info( +// new CameraInfo(camera_info_manager_->getCameraInfo())); +// cam_info->binning_y = currentBinningY(); +// camera_info_manager_->setCameraInfo(*cam_info); + +// // img_raw_msg_.width = pImage_->GetWidth(); +// // // step = full row length in bytes, img_size = (step * rows), +// // // imagePixelDepth already contains the number of channels +// // img_raw_msg_.step = +// // img_raw_msg_.width * (pImage_->GetBitsPerPixel() / 8); +// return false; +// } +// r.sleep(); +// } +// } + +// return true; +// } + +// //~~ Brightness / auto controls ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +// void ArenaCameraBaseNode::setTargetBrightness(unsigned int brightness) { +// // const bool was_streaming = is_streaming_; +// // stopStreaming(); + +// try { +// GenApi::CIntegerPtr pTargetBrightness = +// pDevice_->GetNodeMap()->GetNode("TargetBrightness"); + +// if (GenApi::IsWritable(pTargetBrightness)) { +// if (brightness > 255) brightness = 255; +// pTargetBrightness->SetValue(brightness); + +// Node_INFO_STREAM("Set target brightness to " +// << pTargetBrightness->GetValue()); +// } else { +// Node_INFO_STREAM("TargetBrightness is not writeable; current value " +// << pTargetBrightness->GetValue()); +// } +// } catch (const GenICam::GenericException &e) { +// Node_ERROR_STREAM( +// "An exception while setting TargetBrightness: " << +// e.GetDescription()); +// } + +// // if (was_streaming) +// // startStreaming(); +// } + +// //~~ Exposure ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +// float ArenaCameraBaseNode::currentExposure() { +// GenApi::CFloatPtr pExposureTime = +// pDevice_->GetNodeMap()->GetNode("ExposureTime"); + +// if (!pExposureTime || !GenApi::IsReadable(pExposureTime)) { +// Node_WARN_STREAM("No exposure time value, returning -1"); +// return -1.; +// } else { +// float exposureValue = pExposureTime->GetValue(); +// return exposureValue; +// } +// } + +// void ArenaCameraBaseNode::setExposure( +// ArenaCameraBaseNode::AutoExposureMode exp_mode, float exposure_ms) { +// auto pNodeMap = pDevice_->GetNodeMap(); +// // exposure_auto_ will be already set to false if exposure_given_ is true +// // read params () solved the priority between them +// if (exp_mode == ArenaCameraBaseNode::AutoExposureMode::Off) { +// Arena::SetNodeValue(pNodeMap, "ExposureAuto", "Off"); + +// GenApi::CFloatPtr pExposureTime = +// pDevice_->GetNodeMap()->GetNode("ExposureTime"); + +// float exposure_to_set = exposure_ms * 1000; +// if (exposure_to_set < pExposureTime->GetMin()) { +// Node_WARN_STREAM("Desired exposure (" +// << exposure_to_set << ") " +// << "time unreachable! Setting to lower limit: " +// << pExposureTime->GetMin()); +// exposure_to_set = pExposureTime->GetMin(); +// } else if (exposure_to_set > pExposureTime->GetMax()) { +// Node_WARN_STREAM("Desired exposure (" +// << exposure_to_set << ") " +// << "time unreachable! Setting to upper limit: " +// << pExposureTime->GetMax()); +// exposure_to_set = pExposureTime->GetMax(); +// } + +// pExposureTime->SetValue(exposure_to_set); + +// Node_INFO_STREAM("Setting auto-exposure _off_ with exposure of " +// << pExposureTime->GetValue() << " ms"); +// } else { +// if (exp_mode == ArenaCameraBaseNode::AutoExposureMode::Once) { +// Node_INFO_STREAM("Setting auto-exposure to _on_ / Once"); +// Arena::SetNodeValue(pNodeMap, "ExposureAuto", +// "Once"); +// } else { +// Node_INFO_STREAM("Setting auto-exposure to _on_ / Continuous"); +// Arena::SetNodeValue(pNodeMap, "ExposureAuto", +// "Continuous"); +// } + +// if (exposure_ms > 0) { +// Arena::SetNodeValue(pNodeMap, +// "ExposureAutoLimitAuto", +// "Off"); +// GenApi::CFloatPtr pExposureUpperLimit = +// pDevice_->GetNodeMap()->GetNode("ExposureAutoUpperLimit"); +// if (GenApi::IsWritable(pExposureUpperLimit)) { +// // The parameter in the camera is in us +// pExposureUpperLimit->SetValue(static_cast(exposure_ms) * +// 1000); +// } else { +// Node_INFO("ExposureAutoUpperLimit is not writeable"); +// } + +// } else { +// // Use automatic auto-exposure limits +// Arena::SetNodeValue(pNodeMap, +// "ExposureAutoLimitAuto", +// "Continuous"); +// } + +// Node_INFO_STREAM( +// "Enabling autoexposure with limits " +// << Arena::GetNodeValue(pNodeMap, "ExposureAutoLowerLimit") +// << " to " +// << Arena::GetNodeValue(pNodeMap, "ExposureAutoUpperLimit")); +// } +// } + +// //~~ Gain ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +// bool ArenaCameraBaseNode::setGain( +// ArenaCameraBaseNode::AutoGainMode gain_mode, float target_gain) { +// try { +// auto pNodeMap = pDevice_->GetNodeMap(); + +// Arena::SetNodeValue(pDevice_->GetNodeMap(), +// "GainAuto", +// "Off"); + +// if (gain_mode == ArenaCameraBaseNode::AutoGainMode::Off) { +// Node_INFO_STREAM("Setting auto-gain to _off_"); +// Arena::SetNodeValue(pNodeMap, "GainAuto", "Off"); +// // todo update parameter on the server + +// GenApi::CFloatPtr pGain = pDevice_->GetNodeMap()->GetNode("Gain"); +// float truncated_gain = target_gain; + +// if (truncated_gain < 0.0) { +// Node_WARN_STREAM("Desired gain (" +// << target_gain +// << ") out of range [0.0,1.0]! Setting to 0.0"); +// target_gain = 0.0; +// } else if (truncated_gain > 1.0) { +// Node_WARN_STREAM("Desired gain (" +// << target_gain +// << ") out of range [0.0,1.0]! Setting to 1.0"); +// target_gain = 1.0; +// } + +// const float gain_min = pGain->GetMin(), gain_max = pGain->GetMax(); +// float gain_to_set = gain_min + target_gain * (gain_max - gain_min); +// pGain->SetValue(gain_to_set); + +// } else if (gain_mode == ArenaCameraBaseNode::AutoGainMode::Once) { +// Arena::SetNodeValue(pNodeMap, "GainAuto", "Once"); +// Node_INFO_STREAM("Setting auto-gain to _on_ / Once"); + +// } else if (gain_mode == ArenaCameraBaseNode::AutoGainMode::Continuous) { +// Arena::SetNodeValue(pNodeMap, "GainAuto", +// "Continuous"); +// Node_INFO_STREAM("Setting auto-gain to _on_ / Continuous"); +// } else { +// } + +// } catch (const GenICam::GenericException &e) { +// Node_ERROR_STREAM( +// "An exception while setting gain: " << e.GetDescription()); +// return false; +// } +// return true; +// } + +// 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 ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +// bool ArenaCameraBaseNode::setGamma(const float &target_gamma) { +// // for GigE cameras you have to enable gamma first + +// GenApi::CBooleanPtr pGammaEnable = +// pDevice_->GetNodeMap()->GetNode("GammaEnable"); +// if (GenApi::IsWritable(pGammaEnable)) { +// pGammaEnable->SetValue(true); +// } + +// GenApi::CFloatPtr pGamma = pDevice_->GetNodeMap()->GetNode("Gamma"); +// if (!pGamma || !GenApi::IsWritable(pGamma)) { +// Node_WARN("Cannot set gamma, it is not writeable"); +// return false; +// } else { +// try { +// float gamma_to_set = target_gamma; +// if (pGamma->GetMin() > gamma_to_set) { +// gamma_to_set = pGamma->GetMin(); +// Node_WARN_STREAM( +// "Desired gamma unreachable! Setting to lower limit: " +// << gamma_to_set); +// } else if (pGamma->GetMax() < gamma_to_set) { +// gamma_to_set = pGamma->GetMax(); +// Node_WARN_STREAM( +// "Desired gamma unreachable! Setting to upper limit: " +// << gamma_to_set); +// } + +// Node_INFO_STREAM("Setting gamma to " << gamma_to_set); +// pGamma->SetValue(gamma_to_set); + +// } catch (const GenICam::GenericException &e) { +// Node_ERROR_STREAM("An exception while setting target gamma to " +// << target_gamma +// << " occurred: " << e.GetDescription()); +// return false; +// } +// } +// return true; +// } + +// void ArenaCameraBaseNode::setupSamplingIndices( +// std::vector &indices, std::size_t rows, std::size_t cols, +// int downsampling_factor) { +// indices.clear(); +// std::size_t min_window_height = +// static_cast(rows) / static_cast(downsampling_factor); +// cv::Point2i start_pt(0, 0); +// cv::Point2i end_pt(cols, rows); +// // add the iamge center point only once +// sampling_indices_.push_back(0.5 * rows * cols); +// genSamplingIndicesRec(indices, min_window_height, start_pt, end_pt); +// std::sort(indices.begin(), indices.end()); +// return; +// } + +// void ArenaCameraBaseNode::genSamplingIndicesRec( +// std::vector &indices, const std::size_t &min_window_height, +// const cv::Point2i &s, // start +// const cv::Point2i &e) // end +// { +// if (static_cast(std::abs(e.y - s.y)) <= min_window_height) { +// return; // abort criteria -> shrinked window has the min_col_size +// } +// /* +// * sampled img: point: idx: +// * s 0 0 0 0 0 0 a) [(e.x-s.x)*0.5, (e.y-s.y)*0.5] a.x*a.y*0.5 +// * 0 0 0 d 0 0 0 b) [a.x, 1.5*a.y] b.y*img_rows+b.x +// * 0 0 0 0 0 0 0 c) [0.5*a.x, a.y] c.y*img_rows+c.x +// * 0 c 0 a 0 f 0 d) [a.x, 0.5*a.y] d.y*img_rows+d.x +// * 0 0 0 0 0 0 0 f) [1.5*a.x, a.y] f.y*img_rows+f.x +// * 0 0 0 b 0 0 0 +// * 0 0 0 0 0 0 e +// */ +// cv::Point2i a, b, c, d, f, delta; +// a = s + 0.5 * (e - s); // center point +// delta = 0.5 * (e - s); +// b = s + cv::Point2i(delta.x, 1.5 * delta.y); +// c = s + cv::Point2i(0.5 * delta.x, delta.y); +// d = s + cv::Point2i(delta.x, 0.5 * delta.y); +// f = s + cv::Point2i(1.5 * delta.x, delta.y); +// // \todo{amarburg} This broke when I made pImage_ a non-member +// // indices.push_back(b.y * pImage_->GetWidth() + b.x); +// // indices.push_back(c.y * pImage_->GetWidth() + c.x); +// // indices.push_back(d.y * pImage_->GetWidth() + d.x); +// // indices.push_back(f.y * pImage_->GetWidth() + f.x); +// genSamplingIndicesRec(indices, min_window_height, s, a); +// genSamplingIndicesRec(indices, min_window_height, a, e); +// genSamplingIndicesRec(indices, min_window_height, cv::Point2i(s.x, a.y), +// cv::Point2i(a.x, e.y)); +// genSamplingIndicesRec(indices, min_window_height, cv::Point2i(a.x, s.y), +// cv::Point2i(e.x, a.y)); +// return; +// } + +// float ArenaCameraBaseNode::calcCurrentBrightness() { +// boost::lock_guard lock(device_mutex_); +// if (img_raw_msg_.data.empty()) { +// return 0.0; +// } +// float sum = 0.0; +// if (sensor_msgs::image_encodings::isMono(img_raw_msg_.encoding)) { +// // The mean brightness is calculated using a subset of all pixels +// for (const std::size_t &idx : sampling_indices_) { +// sum += img_raw_msg_.data.at(idx); +// } +// if (sum > 0.0) { +// sum /= static_cast(sampling_indices_.size()); +// } +// } else { +// // The mean brightness is calculated using all pixels and all channels +// sum = +// std::accumulate(img_raw_msg_.data.begin(), img_raw_msg_.data.end(), +// 0); +// if (sum > 0.0) { +// sum /= static_cast(img_raw_msg_.data.size()); +// } +// } +// return sum; +// } + +// //------------------------------------------------------------------- +// // +// // HDR Channel Query and set functions +// // +// float ArenaCameraBaseNode::currentHdrGain(int channel) { +// try { +// Arena::SetNodeValue(pDevice_->GetNodeMap(), +// "HDRTuningChannelSelector", channel); + +// return Arena::GetNodeValue(pDevice_->GetNodeMap(), +// "HDRChannelAnalogGain"); +// } catch (const GenICam::GenericException &e) { +// Node_ERROR_STREAM( +// "Exception while querying HDR gain: " << e.GetDescription()); +// return -1; +// } +// } + +// float ArenaCameraBaseNode::currentHdrExposure(int channel) { +// try { +// Arena::SetNodeValue(pDevice_->GetNodeMap(), +// "HDRTuningChannelSelector", channel); + +// return Arena::GetNodeValue(pDevice_->GetNodeMap(), +// "HDRChannelExposureTime"); +// } catch (const GenICam::GenericException &e) { +// Node_ERROR_STREAM( +// "Exception while querying HDR exposure time: " << +// e.GetDescription()); +// return -1; +// } +// } + +// //------------------------------------------------------------------- +// // 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()); +// } +// } + +// //------------------------------------------------------------------------ +// // ROS Reconfigure callback +// // + +// void ArenaCameraBaseNode::reconfigureCallback(ArenaCameraConfig &config, +// uint32_t level) { +// const auto stop_level = +// (uint32_t)dynamic_reconfigure::SensorLevels::RECONFIGURE_STOP; + +// const bool was_streaming = is_streaming_; +// if (level >= stop_level) { +// ROS_INFO("Stopping sensor for reconfigure"); +// stopStreaming(); +// } + +// Node_INFO_STREAM("In reconfigureCallback"); + +// // -- The following params require stopping streaming, only set if needed +// -- if (config.frame_rate != previous_config_.frame_rate) { +// arena_camera_parameter_set_.setFrameRate(config.frame_rate); +// updateFrameRate(); +// } + +// // if (config.target_brightness != previous_config_.target_brightness) { +// setTargetBrightness(config.target_brightness); +// //} + +// // -- The following can be set while streaming, just set them every time + +// // if ((config.auto_exposure != previous_config_.auto_exposure) || +// // (config.auto_exposure_max_ms != +// previous_config_.auto_exposure_max_ms) +// // || (config.exposure_ms != previous_config_.exposure_ms)) { +// arena_camera_parameter_set_.exposure_auto_ = config.auto_exposure; +// arena_camera_parameter_set_.exposure_ms_ = config.exposure_ms; +// arena_camera_parameter_set_.auto_exposure_max_ms_ = +// config.auto_exposure_max_ms; + +// if (config.auto_exposure) { +// setExposure(ArenaCameraBaseNode::AutoExposureMode::Continuous, +// config.auto_exposure_max_ms); +// } else { +// setExposure(ArenaCameraBaseNode::AutoExposureMode::Off, +// config.exposure_ms); +// } +// // } + +// // if ((config.auto_gain != previous_config_.auto_gain)) { +// arena_camera_parameter_set_.gain_auto_ = config.auto_gain; + +// if (arena_camera_parameter_set_.gain_auto_) { +// setGain(ArenaCameraBaseNode::AutoGainMode::Continuous); +// } else { +// setGain(ArenaCameraBaseNode::AutoGainMode::Off, config.gain); +// } +// // } + +// arena_camera_parameter_set_.gamma_ = config.gamma; +// setGamma(arena_camera_parameter_set_.gamma_); + +// if ((level >= stop_level) && was_streaming) { +// startStreaming(); +// } + +// // Save config +// previous_config_ = config; +// } + +// //------------------------------------------------------------------------ +// // ROS Disagnostics callbacks +// // + +// void ArenaCameraBaseNode::create_diagnostics( +// diagnostic_updater::DiagnosticStatusWrapper &stat) {} + +// void ArenaCameraBaseNode::create_camera_info_diagnostics( +// diagnostic_updater::DiagnosticStatusWrapper &stat) { +// if (camera_info_manager_->isCalibrated()) { +// stat.summaryf(DiagnosticStatus::OK, "Intrinsic calibration found"); +// } else { +// stat.summaryf(DiagnosticStatus::ERROR, "No intrinsic calibration found"); +// } +// } + +// void ArenaCameraBaseNode::diagnostics_timer_callback_( +// const ros::TimerEvent &) { +// diagnostics_updater_.update(); +// } + +} // namespace arena_camera diff --git a/arena_camera/src/encoding_conversions.cpp b/arena_camera/src/encoding_conversions.cpp index 91706d6..6c4edbc 100644 --- a/arena_camera/src/encoding_conversions.cpp +++ b/arena_camera/src/encoding_conversions.cpp @@ -1,6 +1,8 @@ /****************************************************************************** - * Software License Agreement (BSD License) + * 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 @@ -10,7 +12,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 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,14 +29,12 @@ * POSSIBILITY OF SUCH DAMAGE. *****************************************************************************/ -// ROS -#include -#include +#include "arena_camera/encoding_conversions.h" -#include "arena_camera/arena_image_encodings.h" +#include +#include -// Arena node -#include +#include "arena_camera/arena_image_encodings.h" namespace arena_camera { namespace encoding_conversions { diff --git a/arena_camera/src/nodelet_base.cpp b/arena_camera/src/nodelet_base.cpp deleted file mode 100644 index 617e2e8..0000000 --- a/arena_camera/src/nodelet_base.cpp +++ /dev/null @@ -1,1364 +0,0 @@ -/****************************************************************************** - * Software License Agreement (BSD License) - * - * 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 -#include -#include -#include -#include -#include - -// ROS -#include -#include - -// Arena -#include -#include -#include - -// Arena node -#include "arena_camera/arena_camera_nodelet.h" -#include "arena_camera/encoding_conversions.h" - -using diagnostic_msgs::DiagnosticStatus; - -namespace arena_camera { - -using sensor_msgs::CameraInfo; -using sensor_msgs::CameraInfoPtr; - -ArenaCameraNodeletBase::ArenaCameraNodeletBase() - : pSystem_(nullptr), - pDevice_(nullptr), - pNodeMap_(nullptr), - is_streaming_(false), - arena_camera_parameter_set_(), - set_user_output_srvs_(), - it_(nullptr), - img_raw_pub_(), - pinhole_model_(), - camera_info_manager_(nullptr), - sampling_indices_() {} - -ArenaCameraNodeletBase::~ArenaCameraNodeletBase() { - if (pDevice_ != nullptr) { - pSystem_->DestroyDevice(pDevice_); - } - - if (pSystem_ != nullptr) { - Arena::CloseSystem(pSystem_); - } -} - -// -// Nodelet::onInit() function - -void ArenaCameraNodeletBase::onInit() { - ros::NodeHandle &nh = getNodeHandle(); - ros::NodeHandle &pnh = getPrivateNodeHandle(); - - metadata_pub_ = - nh.advertise("imaging_metadata", 1); - - it_.reset(new image_transport::ImageTransport(nh)); - img_raw_pub_ = it_->advertiseCamera("image_raw", 1); - - camera_info_manager_ = new camera_info_manager::CameraInfoManager(nh); - - diagnostics_updater_.setHardwareID("none"); - diagnostics_updater_.add("camera_availability", this, - &ArenaCameraNodeletBase::create_diagnostics); - diagnostics_updater_.add( - "intrinsic_calibration", this, - &ArenaCameraNodeletBase::create_camera_info_diagnostics); - diagnostics_trigger_ = nh.createTimer( - ros::Duration(2), &ArenaCameraNodeletBase::diagnostics_timer_callback_, - this); - - // Initialize parameters from parameter server - arena_camera_parameter_set_.readFromRosParameterServer(pnh); - - // Open the Arena SDK - pSystem_ = Arena::OpenSystem(); - pSystem_->UpdateDevices(100); - if (pSystem_->GetDevices().size() == 0) { - NODELET_FATAL("Did not detect any cameras!!"); - return; - } - - if (!arena_camera_parameter_set_.deviceUserID().empty()) { - if (!registerCameraByUserId(arena_camera_parameter_set_.deviceUserID())) { - NODELET_FATAL_STREAM("Unable to find a camera with DeviceUserId \"" - << arena_camera_parameter_set_.deviceUserID() - << "\""); - return; - } - } else if (!arena_camera_parameter_set_.serialNumber().empty()) { - if (!registerCameraBySerialNumber( - arena_camera_parameter_set_.serialNumber())) { - NODELET_FATAL_STREAM("Unable to find a camera with Serial Number " - << arena_camera_parameter_set_.serialNumber()); - return; - } - } else { - if (!registerCameraByAuto()) { - NODELET_FATAL_STREAM("Unable to find any cameras to register"); - return; - } - } - - // Validate that the camera is from Lucid (otherwise the Arena SDK - // will segfault) - const auto device_vendor_name = Arena::GetNodeValue( - pDevice_->GetNodeMap(), "DeviceVendorName"); - if (device_vendor_name != "Lucid Vision Labs") { - NODELET_FATAL_STREAM( - "Hm, this doesn't appear to be a Lucid Vision camera, got vendor name: " - << device_vendor_name); - } - - if (!configureCamera()) { - NODELET_FATAL_STREAM("Unable to configure camera"); - return; - } - - _dynReconfigureServer = std::make_shared(pnh); - _dynReconfigureServer->setCallback(boost::bind( - &ArenaCameraNodeletBase::reconfigureCallbackWrapper, this, _1, _2)); -} - -bool ArenaCameraNodeletBase::registerCameraByUserId( - const std::string &device_user_id_to_open) { - ROS_ASSERT(pSystem_); - std::vector deviceInfos = pSystem_->GetDevices(); - ROS_ASSERT(deviceInfos.size() > 0); - - NODELET_INFO_STREAM("Connecting to camera with DeviceUserId" - << device_user_id_to_open); - - std::vector::iterator it; - - for (auto &dev : deviceInfos) { - const std::string device_user_id(dev.UserDefinedName()); - // Must be an exact match - if (0 == device_user_id_to_open.compare(device_user_id)) { - NODELET_INFO_STREAM("Found the desired camera with DeviceUserID " - << device_user_id_to_open << ": "); - - pDevice_ = pSystem_->CreateDevice(dev); - return true; - } - } - - NODELET_ERROR_STREAM( - "Couldn't find the camera that matches the " - << "given DeviceUserID: \"" << device_user_id_to_open << "\"! " - << "Either the ID is wrong or the cam is not yet connected"); - return false; -} - -bool ArenaCameraNodeletBase::registerCameraBySerialNumber( - const std::string &serial_number) { - ROS_ASSERT(pSystem_); - std::vector deviceInfos = pSystem_->GetDevices(); - ROS_ASSERT(deviceInfos.size() > 0); - - NODELET_INFO_STREAM("Connecting to camera with Serial Number " - << serial_number); - - for (auto &dev : deviceInfos) { - if (0 == serial_number.compare(dev.SerialNumber())) { - NODELET_INFO_STREAM("Found the desired camera with Serial Number " - << serial_number); - - pDevice_ = pSystem_->CreateDevice(dev); - return true; - } - } - - NODELET_ERROR_STREAM( - "Couldn't find the camera that matches the " - << "given Serial Number: " << serial_number << "! " - << "Either the ID is wrong or the cam is not yet connected"); - return false; -} - -bool ArenaCameraNodeletBase::registerCameraByAuto() { - ROS_ASSERT(pSystem_); - std::vector deviceInfos = pSystem_->GetDevices(); - ROS_ASSERT(deviceInfos.size() > 0); - - int i = 0; - NODELET_INFO_STREAM("Found " << deviceInfos.size() << " cameras"); - for (auto &dev : deviceInfos) { - NODELET_INFO_STREAM(i++ << ": " << dev.SerialNumber() << " " - << dev.UserDefinedName()); - } - - for (auto &dev : deviceInfos) { - if (dev.VendorName() == "Lucid Vision Labs") { - pDevice_ = pSystem_->CreateDevice(dev); - NODELET_INFO_STREAM( - "Connecting to first autodetected Lucid Vision camera: Serial Number " - << dev.SerialNumber() << " ; User ID: " << dev.UserDefinedName()); - return true; - } - } - - return false; -} - -bool ArenaCameraNodeletBase::configureCamera() { - ros::NodeHandle nh = getNodeHandle(); - auto pNodeMap = pDevice_->GetNodeMap(); - - // **NOTE** only configuration which is not also accessible through - // dynamic_reconfigure. Those will be handled in the callback - // when it is called for the first time at node startup. - - try { - NODELET_INFO_STREAM( - " Device model: " << Arena::GetNodeValue( - pDevice_->GetNodeMap(), "DeviceModelName")); - NODELET_INFO_STREAM( - "Device firmware: " << Arena::GetNodeValue( - pDevice_->GetNodeMap(), "DeviceFirmwareVersion")); - - // - // Arena device prior streaming settings - // - - if (Arena::GetNodeValue( - pDevice_->GetNodeMap(), "DeviceTLType") == "GigEVision") { - NODELET_INFO("GigE device, performing GigE specific configuration:"); - - // Set Jumbo frames (this is only relevant for GigE cameras.) - auto pPacketSize = pNodeMap->GetNode("DeviceStreamChannelPacketSize"); - if (GenApi::IsWritable(pPacketSize)) { - NODELET_INFO_STREAM(" -> Setting MTU to " - << arena_camera_parameter_set_.mtuSize()); - Arena::SetNodeValue(pNodeMap, "DeviceStreamChannelPacketSize", - arena_camera_parameter_set_.mtuSize()); - } else { - NODELET_INFO(" -> Camera MTU is not writeable"); - } - } - - auto payloadSize = Arena::GetNodeValue(pNodeMap, "PayloadSize"); - NODELET_INFO_STREAM("Expected payload size: " << payloadSize); - - // enable stream auto negotiate packet size - NODELET_DEBUG("Enabling auto-negotiation of packet size"); - Arena::SetNodeValue(pDevice_->GetTLStreamNodeMap(), - "StreamAutoNegotiatePacketSize", true); - - // enable stream packet resend - NODELET_DEBUG("Enabling packet resend"); - Arena::SetNodeValue(pDevice_->GetTLStreamNodeMap(), - "StreamPacketResendEnable", true); - - // - // PIXELFORMAT - // - setImageEncoding(arena_camera_parameter_set_.imageEncoding()); - - if (encoding_conversions::isHDR( - arena_camera_parameter_set_.imageEncoding())) { - hdr_metadata_pub_ = nh.advertise( - "hdr_imaging_metadata", 1); - } - - // - // TRIGGER MODE - // - GenApi::CStringPtr pTriggerMode = pNodeMap->GetNode("TriggerMode"); - if (GenApi::IsWritable(pTriggerMode)) { - Arena::SetNodeValue(pNodeMap, "TriggerMode", "On"); - Arena::SetNodeValue(pNodeMap, "TriggerSource", - "Software"); - } - - //!! Parameters controlled by param / dynamic reonfigure are not set here - //!! Assume there will be an immediate call from dynamic reconfigure - - // LUT - NODELET_INFO_STREAM( - (arena_camera_parameter_set_.enable_lut_ ? "Enabling" : "Disabling") - << " camera LUT"); - enableLUT(arena_camera_parameter_set_.enable_lut_); - - // ------------------------------------------------------------------------ - - // - // Initial setting of the CameraInfo-msg, assuming no calibration given - CameraInfo initial_cam_info; - // initializeCameraInfo(initial_cam_info); - camera_info_manager_->setCameraInfo(initial_cam_info); - - if (arena_camera_parameter_set_.cameraInfoURL().empty() || - !camera_info_manager_->validateURL( - arena_camera_parameter_set_.cameraInfoURL())) { - NODELET_INFO_STREAM("CameraInfoURL needed for rectification! ROS-Param: " - << "'" << nh.getNamespace() << "/camera_info_url' = '" - << arena_camera_parameter_set_.cameraInfoURL() - << "' is invalid!"); - NODELET_DEBUG_STREAM("CameraInfoURL should have following style: " - << "'file:///full/path/to/local/file.yaml' or " - << "'file://${ROS_HOME}/camera_info/${NAME}.yaml'"); - NODELET_WARN_STREAM("Will only provide distorted /image_raw images!"); - } else { - // override initial camera info if the url is valid - if (camera_info_manager_->loadCameraInfo( - arena_camera_parameter_set_.cameraInfoURL())) { - // setupRectification(); - // set the correct tf frame_id - CameraInfoPtr cam_info( - new CameraInfo(camera_info_manager_->getCameraInfo())); - cam_info->header.frame_id = img_raw_msg_.header.frame_id; - camera_info_manager_->setCameraInfo(*cam_info); - } else { - NODELET_WARN_STREAM("Will only provide distorted /image_raw images!"); - } - } - - if (arena_camera_parameter_set_.binning_x_given_) { - size_t reached_binning_x; - if (setBinningX(arena_camera_parameter_set_.binning_x_, - reached_binning_x)) { - NODELET_INFO_STREAM("Setting horizontal binning_x to " - << arena_camera_parameter_set_.binning_x_); - NODELET_WARN_STREAM( - "The image width of the camera_info-msg will " - << "be adapted, so that the binning_x value in this msg remains 1"); - } - } - - if (arena_camera_parameter_set_.binning_y_given_) { - size_t reached_binning_y; - if (setBinningY(arena_camera_parameter_set_.binning_y_, - reached_binning_y)) { - NODELET_INFO_STREAM("Setting vertical binning_y to " - << arena_camera_parameter_set_.binning_y_); - NODELET_WARN_STREAM( - "The image height of the camera_info-msg will " - << "be adapted, so that the binning_y value in this msg remains 1"); - } - } - - // if (arena_camera_parameter_set_.image_encoding_given_) - // { - // float reached_image_encoding; - // if (setImageEncoding(arena_camera_parameter_set_.image_encoding_)) - // { - // NODELET_INFO_STREAM("Setting encoding to " - // << - // arena_camera_parameter_set_.image_encoding_); - // } - // } - - Arena::SetNodeValue(pDevice_->GetTLStreamNodeMap(), - "StreamBufferHandlingMode", - "NewestOnly"); - - // bool isTriggerArmed = false; - - // if (GenApi::IsWritable(pTriggerMode)) { - // do { - // isTriggerArmed = Arena::GetNodeValue(pNodeMap, "TriggerArmed"); - // } while (isTriggerArmed == false); - // // Arena::ExecuteNode(pNodeMap, "TriggerSoftware"); - // } - - // pImage_ = pDevice_->GetImage(5000); - // pData_ = pImage_->GetData(); - - // img_raw_msg_.data.resize(img_raw_msg_.height * img_raw_msg_.step); - // memcpy(&img_raw_msg_.data[0], pImage_->GetData(), - // img_raw_msg_.height * img_raw_msg_.step); - } catch (GenICam::GenericException &e) { - NODELET_ERROR_STREAM("Error while configuring camera: \r\n" - << e.GetDescription()); - return false; - } - - // -------------------------------------------------------------------------- - - img_raw_msg_.header.frame_id = arena_camera_parameter_set_.cameraFrame(); - // Encoding of pixels -- channel meaning, ordering, size - // taken from the list of strings in include/sensor_msgs/image_encodings.h - // img_raw_msg_.height = pImage_->GetHeight(); - // img_raw_msg_.width = pImage_->GetWidth(); - // step = full row length in bytes, img_size = (step * rows), imagePixelDepth - // already contains the number of channels - // img_raw_msg_.step = img_raw_msg_.width * (pImage_->GetBitsPerPixel() / 8); - - if (!camera_info_manager_->setCameraName(std::string( - Arena::GetNodeValue(pNodeMap, "DeviceUserID") - .c_str()))) { - // valid name contains only alphanumeric signs and '_' - NODELET_WARN_STREAM("[" - << std::string(Arena::GetNodeValue( - pNodeMap, "DeviceUserID") - .c_str()) - << "] name not valid for camera_info_manager"); - } - - // NODELET_INFO("=== Startup settings ==="); - // NODELET_INFO_STREAM("encoding = " << currentROSEncoding()); - // NODELET_INFO_STREAM("binning = [" << currentBinningX() << " x " - // << currentBinningY() << "]"); - // NODELET_INFO_STREAM("exposure = " << currentExposure() << " us"); - // NODELET_INFO_STREAM("gain = " << currentGain()); - // NODELET_INFO_STREAM("gamma = " << currentGamma()); - // NODELET_INFO_STREAM( - // "shutter mode = " << arena_camera_parameter_set_.shutterModeString()); - // NODELET_INFO("========================"); - - // pDevice_->RequeueBuffer(pImage_); - return true; -} - -void ArenaCameraNodeletBase::startStreaming() { - if (!is_streaming_) { - pDevice_->StartStream(); - is_streaming_ = true; - } -} - -void ArenaCameraNodeletBase::stopStreaming() { - if (is_streaming_) { - pDevice_->StopStream(); - is_streaming_ = false; - } -} - -sensor_msgs::RegionOfInterest ArenaCameraNodeletBase::currentROI() { - sensor_msgs::RegionOfInterest roi; - // \todo{amarburg} Broke this by getting rid of a member pImage_ - // Need to save as state? - // roi.width = pImage_->GetWidth(); - // roi.height = pImage_->GetHeight(); - // ; - // roi.x_offset = pImage_->GetOffsetX(); - // roi.y_offset = pImage_->GetOffsetY(); - return roi; -} - -float ArenaCameraNodeletBase::currentGamma() { - GenApi::CFloatPtr pGamma = pDevice_->GetNodeMap()->GetNode("Gamma"); - - if (!pGamma || !GenApi::IsReadable(pGamma)) { - NODELET_WARN_STREAM("No gamma value, returning -1"); - return -1.; - } else { - float gammaValue = pGamma->GetValue(); - return gammaValue; - } -} - -int64_t ArenaCameraNodeletBase::currentBinningX() { - GenApi::CIntegerPtr BinningHorizontal = - pDevice_->GetNodeMap()->GetNode("BinningHorizontal"); - - if (!BinningHorizontal || !GenApi::IsReadable(BinningHorizontal)) { - NODELET_WARN_STREAM("No binningY value, returning -1"); - return -1; - } else { - float binningXValue = BinningHorizontal->GetValue(); - return binningXValue; - } -} - -int64_t ArenaCameraNodeletBase::currentBinningY() { - GenApi::CIntegerPtr BinningVertical = - pDevice_->GetNodeMap()->GetNode("BinningVertical"); - - if (!BinningVertical || !GenApi::IsReadable(BinningVertical)) { - NODELET_WARN_STREAM("No binningY value, returning -1"); - return -1; - } else { - float binningYValue = BinningVertical->GetValue(); - return binningYValue; - } -} - -float ArenaCameraNodeletBase::currentGain() { - GenApi::CFloatPtr pGain = pDevice_->GetNodeMap()->GetNode("Gain"); - - if (!pGain || !GenApi::IsReadable(pGain)) { - NODELET_WARN_STREAM("No gain value"); - return -1.; - } else { - float gainValue = pGain->GetValue(); - return gainValue; - } -} - -std::string ArenaCameraNodeletBase::currentROSEncoding() { - std::string gen_api_encoding(Arena::GetNodeValue( - pDevice_->GetNodeMap(), "PixelFormat")); - std::string ros_encoding(""); - if (!encoding_conversions::genAPI2Ros(gen_api_encoding, ros_encoding)) { - std::stringstream ss; - ss << "No ROS equivalent to GenApi encoding '" << gen_api_encoding - << "' found! This is bad because this case " - "should never occur!"; - throw std::runtime_error(ss.str()); - return "NO_ENCODING"; - } - return ros_encoding; -} - -bool ArenaCameraNodeletBase::setImageEncoding(const std::string &ros_encoding) { - std::string gen_api_encoding; - bool conversion_found = - encoding_conversions::ros2GenAPI(ros_encoding, gen_api_encoding); - if (!conversion_found) { - if (ros_encoding.empty()) { - return false; - } else { - std::string fallbackPixelFormat = - Arena::GetNodeValue(pDevice_->GetNodeMap(), - "PixelFormat") - .c_str(); - NODELET_ERROR_STREAM( - "Can't convert ROS encoding '" - << ros_encoding - << "' to a corresponding GenAPI encoding! Will use current " - << "pixel format ( " << fallbackPixelFormat << " ) as fallback!"); - return false; - } - } - try { - GenApi::CEnumerationPtr pPixelFormat = - pDevice_->GetNodeMap()->GetNode("PixelFormat"); - if (GenApi::IsWritable(pPixelFormat)) { - Arena::SetNodeValue( - pDevice_->GetNodeMap(), "PixelFormat", gen_api_encoding.c_str()); - if (currentROSEncoding() == "16UC3" || currentROSEncoding() == "16UC4") - NODELET_WARN_STREAM( - "ROS grabbing image data from 3D pixel format, unable to display " - "in image viewer"); - } - } catch (const GenICam::GenericException &e) { - NODELET_ERROR_STREAM("An exception while setting target image encoding to '" - << ros_encoding - << "' occurred: " << e.GetDescription()); - return false; - } - - NODELET_INFO("Have configured image_encoding"); - - if (arena_camera::encoding_conversions::isHDR(ros_encoding)) { - NODELET_INFO_STREAM("Requested HDR encoding \"" - << ros_encoding << "\", enabling HDR mode in camera"); - - try { - auto pNodeMap = pDevice_->GetNodeMap(); - - // GenApi::CStringPtr pHDROutput = pNodeMap->GetNode("HDROutput"); - // if (GenApi::IsWritable(pHDROutput)) { - // Arena::SetNodeValue(pNodeMap, "HDROutput", - // "HDR"); - // } - - // Enable HDR image enhancement - Arena::SetNodeValue(pNodeMap, "HDRImageEnhancementEnable", true); - Arena::SetNodeValue(pNodeMap, "HDRTuningEnable", false); - - Arena::SetNodeValue(pNodeMap, "HDROutput", "HDR"); - - } catch (GenICam::GenericException &e) { - NODELET_ERROR_STREAM("Error while configuring camera: " - << e.GetDescription() - << ", is this camera capable of HDR?"); - return false; - } - } - - return true; -} - -// Note that streaming must be stopped before updating frame rate -void ArenaCameraNodeletBase::updateFrameRate() { - try { - ros::NodeHandle nh = getNodeHandle(); - auto pNodeMap = pDevice_->GetNodeMap(); - - // const bool was_streaming = is_streaming_; - // stopStreaming(); - - auto cmdlnParamFrameRate = arena_camera_parameter_set_.frameRate(); - auto currentFrameRate = - Arena::GetNodeValue(pNodeMap, "AcquisitionFrameRate"); - auto maximumFrameRate = - GenApi::CFloatPtr(pNodeMap->GetNode("AcquisitionFrameRate"))->GetMax(); - - // requested framerate larger than device max so we trancate it - if (cmdlnParamFrameRate >= maximumFrameRate) { - arena_camera_parameter_set_.setFrameRate(maximumFrameRate); - - NODELET_WARN( - "Desired framerate %.2f Hz (rounded) is higher than max possible. " - "Will limit " - "framerate device max : %.2f Hz (rounded)", - cmdlnParamFrameRate, maximumFrameRate); - } - // special case: - // dues to inacurate float comparision we skip. If we set it it might - // throw becase it could be a lil larger than the max avoid the exception - // (double accuracy issue when setting the node) request frame rate very - // close to device max - else if (cmdlnParamFrameRate == maximumFrameRate) { - NODELET_INFO("Framerate is %.2f Hz", cmdlnParamFrameRate); - } - // requested max frame rate - else if (cmdlnParamFrameRate == - -1) // speacial for max frame rate available - { - arena_camera_parameter_set_.setFrameRate(maximumFrameRate); - - NODELET_WARN("Framerate is set to device max : %.2f Hz", - maximumFrameRate); - } - - // requested framerate is valid so we set it to the device - try { - if (!Arena::GetNodeValue(pNodeMap, "AcquisitionFrameRateEnable")) { - NODELET_INFO( - "\"AcquisitionFrameRateEnable\" not true, trying to enable"); - Arena::SetNodeValue(pNodeMap, "AcquisitionFrameRateEnable", true); - } - Arena::SetNodeValue(pNodeMap, "AcquisitionFrameRate", - arena_camera_parameter_set_.frameRate()); - } catch (GenICam::GenericException &e) { - NODELET_INFO_STREAM("Exception while changing frame rate: " << e.what()); - } - NODELET_INFO_STREAM( - "Framerate has been set to " - << Arena::GetNodeValue(pNodeMap, "AcquisitionFrameRate") - << " Hz"); - - // if (was_streaming) startStreaming(); - - } catch (GenICam::GenericException &e) { - NODELET_INFO_STREAM("Exception while changing frame rate: " << e.what()); - } -} - -// void ArenaCameraNodeletBase::initializeCameraInfo( -// sensor_msgs::CameraInfo &cam_info_msg) { -// // http://www.ros.org/reps/rep-0104.html -// // If the camera is uncalibrated, the matrices D, K, R, P should be left -// // zeroed out. In particular, clients may assume that K[0] == 0.0 -// // indicates an uncalibrated camera. -// cam_info_msg.header.frame_id = cameraFrame(); -// cam_info_msg.header.stamp = ros::Time::now(); - -// // The image dimensions with which the camera was calibrated. Normally -// // this will be the full camera resolution in pixels. They remain fix, -// // even if binning is applied rows and colums -// cam_info_msg.height = -// Arena::GetNodeValue(pDevice_->GetNodeMap(), "Height"); -// cam_info_msg.width = -// Arena::GetNodeValue(pDevice_->GetNodeMap(), "Width"); - -// // The distortion model used. Supported models are listed in -// // sensor_msgs/distortion_models.h. For most cameras, "plumb_bob" - a -// // simple model of radial and tangential distortion - is sufficient. -// // Empty D and distortion_model indicate that the CameraInfo cannot be -// // used to rectify points or images, either because the camera is not -// // calibrated or because the rectified image was produced using an -// // unsupported distortion model, e.g. the proprietary one used by -// // Bumblebee cameras [http://www.ros.org/reps/rep-0104.html]. -// cam_info_msg.distortion_model = ""; - -// // The distortion parameters, size depending on the distortion model. -// // For "plumb_bob", the 5 parameters are: (k1, k2, t1, t2, k3) -> -// // float64[] D. -// cam_info_msg.D = std::vector(5, 0.); - -// // Intrinsic camera matrix for the raw (distorted) images. -// // [fx 0 cx] -// // K = [ 0 fy cy] --> 3x3 row-major matrix -// // [ 0 0 1] -// // Projects 3D points in the camera coordinate frame to 2D pixel -// // coordinates using the focal lengths (fx, fy) and principal point (cx, -// // cy). -// cam_info_msg.K.assign(0.0); - -// // Rectification matrix (stereo cameras only) -// // A rotation matrix aligning the camera coordinate system to the ideal -// // stereo image plane so that epipolar lines in both stereo images are -// // parallel. -// cam_info_msg.R.assign(0.0); - -// // Projection/camera matrix -// // [fx' 0 cx' Tx] -// // P = [ 0 fy' cy' Ty] --> # 3x4 row-major matrix -// // [ 0 0 1 0] -// // By convention, this matrix specifies the intrinsic (camera) matrix of -// // the processed (rectified) image. That is, the left 3x3 portion is the -// // normal camera intrinsic matrix for the rectified image. It projects 3D -// // points in the camera coordinate frame to 2D pixel coordinates using the -// // focal lengths (fx', fy') and principal point (cx', cy') - these may -// // differ from the values in K. For monocular cameras, Tx = Ty = 0. -// // Normally, monocular cameras will also have R = the identity and -// // P[1:3,1:3] = K. For a stereo pair, the fourth column [Tx Ty 0]' is -// // related to the position of the optical center of the second camera in -// // the first camera's frame. We assume Tz = 0 so both cameras are in the -// // same stereo image plane. The first camera always has Tx = Ty = 0. For -// // the right (second) camera of a horizontal stereo pair, Ty = 0 and Tx = -// // -fx' * B, where B is the baseline between the cameras. Given a 3D point -// // [X Y Z]', the projection (x, y) of the point onto the rectified image -// // is given by: [u v w]' = P * [X Y Z 1]' -// // x = u / w -// // y = v / w -// // This holds for both images of a stereo pair. -// cam_info_msg.P.assign(0.0); - -// // Binning refers here to any camera setting which combines rectangular -// // neighborhoods of pixels into larger "super-pixels." It reduces the -// // resolution of the output image to (width / binning_x) x (height / -// // binning_y). The default values binning_x = binning_y = 0 is considered -// // the same as binning_x = binning_y = 1 (no subsampling). -// // cam_info_msg.binning_x = currentBinningX(); -// // cam_info_msg.binning_y = currentBinningY(); - -// // Region of interest (subwindow of full camera resolution), given in full -// // resolution (unbinned) image coordinates. A particular ROI always -// // denotes the same window of pixels on the camera sensor, regardless of -// // binning settings. The default setting of roi (all values 0) is -// // considered the same as full resolution (roi.width = width, roi.height = -// // height). - -// // todo? do these has ti be set via -// // Arena::GetNodeValue(pDevice_->GetNodeMap(), "OffsetX"); or so -// // ? -// cam_info_msg.roi.x_offset = cam_info_msg.roi.y_offset = 0; -// cam_info_msg.roi.height = cam_info_msg.roi.width = 0; -// } - -//~~ Binning / ROI ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -bool ArenaCameraNodeletBase::setROI( - const sensor_msgs::RegionOfInterest target_roi, - sensor_msgs::RegionOfInterest &reached_roi) { - boost::lock_guard lock(device_mutex_); - // TODO: set ROI - return true; -} - -bool ArenaCameraNodeletBase::setBinningXValue(const size_t &target_binning_x, - size_t &reached_binning_x) { - try { - GenApi::CIntegerPtr pBinningHorizontal = - pDevice_->GetNodeMap()->GetNode("BinningHorizontal"); - if (GenApi::IsWritable(pBinningHorizontal)) { - size_t binning_x_to_set = target_binning_x; - if (binning_x_to_set < pBinningHorizontal->GetMin()) { - NODELET_WARN_STREAM("Desired horizontal binning_x factor(" - << binning_x_to_set - << ") unreachable! Setting to lower " - << "limit: " << pBinningHorizontal->GetMin()); - binning_x_to_set = pBinningHorizontal->GetMin(); - } else if (binning_x_to_set > pBinningHorizontal->GetMax()) { - NODELET_WARN_STREAM("Desired horizontal binning_x factor(" - << binning_x_to_set - << ") unreachable! Setting to upper " - << "limit: " << pBinningHorizontal->GetMax()); - binning_x_to_set = pBinningHorizontal->GetMax(); - } - - pBinningHorizontal->SetValue(binning_x_to_set); - reached_binning_x = currentBinningX(); - } else { - NODELET_WARN_STREAM("Camera does not support binning. Will keep the " - << "current settings"); - reached_binning_x = currentBinningX(); - } - } - - catch (const GenICam::GenericException &e) { - NODELET_ERROR_STREAM("An exception while setting target horizontal " - << "binning_x factor to " << target_binning_x - << " occurred: " << e.GetDescription()); - return false; - } - return true; -} - -bool ArenaCameraNodeletBase::setBinningX(const size_t &target_binning_x, - size_t &reached_binning_x) { - boost::lock_guard lock(device_mutex_); - - if (!setBinningXValue(target_binning_x, reached_binning_x)) { - // retry till timeout - ros::Rate r(10.0); - ros::Time timeout(ros::Time::now() + ros::Duration(2.0)); - while (ros::ok()) { - if (setBinningXValue(target_binning_x, reached_binning_x)) { - break; - } - if (ros::Time::now() > timeout) { - NODELET_ERROR_STREAM("Error in setBinningX(): Unable to set target " - << "binning_x factor before timeout"); - CameraInfoPtr cam_info( - new CameraInfo(camera_info_manager_->getCameraInfo())); - cam_info->binning_x = currentBinningX(); - camera_info_manager_->setCameraInfo(*cam_info); - // img_raw_msg_.width = pImage_->GetWidth(); - // step = full row length in bytes, img_size = (step * rows), - // imagePixelDepth already contains the number of channels - // img_raw_msg_.step = img_raw_msg_.width * - // (pImage_->GetBitsPerPixel() / 8); - return false; - } - r.sleep(); - } - } - - return true; -} - -bool ArenaCameraNodeletBase::setBinningYValue(const size_t &target_binning_y, - size_t &reached_binning_y) { - try { - GenApi::CIntegerPtr pBinningVertical = - pDevice_->GetNodeMap()->GetNode("BinningVertical"); - if (GenApi::IsWritable(pBinningVertical)) { - size_t binning_y_to_set = target_binning_y; - if (binning_y_to_set < pBinningVertical->GetMin()) { - NODELET_WARN_STREAM("Desired horizontal binning_y factor(" - << binning_y_to_set - << ") unreachable! Setting to lower " - << "limit: " << pBinningVertical->GetMin()); - binning_y_to_set = pBinningVertical->GetMin(); - } else if (binning_y_to_set > pBinningVertical->GetMax()) { - NODELET_WARN_STREAM("Desired horizontal binning_y factor(" - << binning_y_to_set - << ") unreachable! Setting to upper " - << "limit: " << pBinningVertical->GetMax()); - binning_y_to_set = pBinningVertical->GetMax(); - } - - pBinningVertical->SetValue(binning_y_to_set); - reached_binning_y = currentBinningY(); - } else { - NODELET_WARN_STREAM("Camera does not support binning. Will keep the " - << "current settings"); - reached_binning_y = currentBinningY(); - } - } - - catch (const GenICam::GenericException &e) { - NODELET_ERROR_STREAM("An exception while setting target horizontal " - << "binning_y factor to " << target_binning_y - << " occurred: " << e.GetDescription()); - return false; - } - return true; -} - -bool ArenaCameraNodeletBase::setBinningY(const size_t &target_binning_y, - size_t &reached_binning_y) { - boost::lock_guard lock(device_mutex_); - - if (!setBinningYValue(target_binning_y, reached_binning_y)) { - // retry till timeout - ros::Rate r(10.0); - ros::Time timeout(ros::Time::now() + ros::Duration(2.0)); - while (ros::ok()) { - if (setBinningYValue(target_binning_y, reached_binning_y)) { - break; - } - if (ros::Time::now() > timeout) { - NODELET_ERROR_STREAM("Error in setBinningY(): Unable to set target " - << "binning_y factor before timeout"); - CameraInfoPtr cam_info( - new CameraInfo(camera_info_manager_->getCameraInfo())); - cam_info->binning_y = currentBinningY(); - camera_info_manager_->setCameraInfo(*cam_info); - - // img_raw_msg_.width = pImage_->GetWidth(); - // // step = full row length in bytes, img_size = (step * rows), - // // imagePixelDepth already contains the number of channels - // img_raw_msg_.step = - // img_raw_msg_.width * (pImage_->GetBitsPerPixel() / 8); - return false; - } - r.sleep(); - } - } - - return true; -} - -//~~ Brightness / auto controls ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -void ArenaCameraNodeletBase::setTargetBrightness(unsigned int brightness) { - // const bool was_streaming = is_streaming_; - // stopStreaming(); - - try { - GenApi::CIntegerPtr pTargetBrightness = - pDevice_->GetNodeMap()->GetNode("TargetBrightness"); - - if (GenApi::IsWritable(pTargetBrightness)) { - if (brightness > 255) brightness = 255; - pTargetBrightness->SetValue(brightness); - - NODELET_INFO_STREAM("Set target brightness to " - << pTargetBrightness->GetValue()); - } else { - NODELET_INFO_STREAM("TargetBrightness is not writeable; current value " - << pTargetBrightness->GetValue()); - } - } catch (const GenICam::GenericException &e) { - NODELET_ERROR_STREAM( - "An exception while setting TargetBrightness: " << e.GetDescription()); - } - - // if (was_streaming) - // startStreaming(); -} - -//~~ Exposure ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -float ArenaCameraNodeletBase::currentExposure() { - GenApi::CFloatPtr pExposureTime = - pDevice_->GetNodeMap()->GetNode("ExposureTime"); - - if (!pExposureTime || !GenApi::IsReadable(pExposureTime)) { - NODELET_WARN_STREAM("No exposure time value, returning -1"); - return -1.; - } else { - float exposureValue = pExposureTime->GetValue(); - return exposureValue; - } -} - -void ArenaCameraNodeletBase::setExposure( - ArenaCameraNodeletBase::AutoExposureMode exp_mode, float exposure_ms) { - auto pNodeMap = pDevice_->GetNodeMap(); - // exposure_auto_ will be already set to false if exposure_given_ is true - // read params () solved the priority between them - if (exp_mode == ArenaCameraNodeletBase::AutoExposureMode::Off) { - Arena::SetNodeValue(pNodeMap, "ExposureAuto", "Off"); - - GenApi::CFloatPtr pExposureTime = - pDevice_->GetNodeMap()->GetNode("ExposureTime"); - - float exposure_to_set = exposure_ms * 1000; - if (exposure_to_set < pExposureTime->GetMin()) { - NODELET_WARN_STREAM("Desired exposure (" - << exposure_to_set << ") " - << "time unreachable! Setting to lower limit: " - << pExposureTime->GetMin()); - exposure_to_set = pExposureTime->GetMin(); - } else if (exposure_to_set > pExposureTime->GetMax()) { - NODELET_WARN_STREAM("Desired exposure (" - << exposure_to_set << ") " - << "time unreachable! Setting to upper limit: " - << pExposureTime->GetMax()); - exposure_to_set = pExposureTime->GetMax(); - } - - pExposureTime->SetValue(exposure_to_set); - - NODELET_INFO_STREAM("Setting auto-exposure _off_ with exposure of " - << pExposureTime->GetValue() << " ms"); - } else { - if (exp_mode == ArenaCameraNodeletBase::AutoExposureMode::Once) { - NODELET_INFO_STREAM("Setting auto-exposure to _on_ / Once"); - Arena::SetNodeValue(pNodeMap, "ExposureAuto", "Once"); - } else { - NODELET_INFO_STREAM("Setting auto-exposure to _on_ / Continuous"); - Arena::SetNodeValue(pNodeMap, "ExposureAuto", - "Continuous"); - } - - if (exposure_ms > 0) { - Arena::SetNodeValue(pNodeMap, "ExposureAutoLimitAuto", - "Off"); - GenApi::CFloatPtr pExposureUpperLimit = - pDevice_->GetNodeMap()->GetNode("ExposureAutoUpperLimit"); - if (GenApi::IsWritable(pExposureUpperLimit)) { - // The parameter in the camera is in us - pExposureUpperLimit->SetValue(static_cast(exposure_ms) * 1000); - } else { - NODELET_INFO("ExposureAutoUpperLimit is not writeable"); - } - - } else { - // Use automatic auto-exposure limits - Arena::SetNodeValue(pNodeMap, "ExposureAutoLimitAuto", - "Continuous"); - } - - NODELET_INFO_STREAM( - "Enabling autoexposure with limits " - << Arena::GetNodeValue(pNodeMap, "ExposureAutoLowerLimit") - << " to " - << Arena::GetNodeValue(pNodeMap, "ExposureAutoUpperLimit")); - } -} - -//~~ Gain ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -bool ArenaCameraNodeletBase::setGain( - ArenaCameraNodeletBase::AutoGainMode gain_mode, float target_gain) { - try { - auto pNodeMap = pDevice_->GetNodeMap(); - - Arena::SetNodeValue(pDevice_->GetNodeMap(), "GainAuto", - "Off"); - - if (gain_mode == ArenaCameraNodeletBase::AutoGainMode::Off) { - NODELET_INFO_STREAM("Setting auto-gain to _off_"); - Arena::SetNodeValue(pNodeMap, "GainAuto", "Off"); - // todo update parameter on the server - - GenApi::CFloatPtr pGain = pDevice_->GetNodeMap()->GetNode("Gain"); - float truncated_gain = target_gain; - - if (truncated_gain < 0.0) { - NODELET_WARN_STREAM("Desired gain (" - << target_gain - << ") out of range [0.0,1.0]! Setting to 0.0"); - target_gain = 0.0; - } else if (truncated_gain > 1.0) { - NODELET_WARN_STREAM("Desired gain (" - << target_gain - << ") out of range [0.0,1.0]! Setting to 1.0"); - target_gain = 1.0; - } - - const float gain_min = pGain->GetMin(), gain_max = pGain->GetMax(); - float gain_to_set = gain_min + target_gain * (gain_max - gain_min); - pGain->SetValue(gain_to_set); - - } else if (gain_mode == ArenaCameraNodeletBase::AutoGainMode::Once) { - Arena::SetNodeValue(pNodeMap, "GainAuto", "Once"); - NODELET_INFO_STREAM("Setting auto-gain to _on_ / Once"); - - } else if (gain_mode == ArenaCameraNodeletBase::AutoGainMode::Continuous) { - Arena::SetNodeValue(pNodeMap, "GainAuto", - "Continuous"); - NODELET_INFO_STREAM("Setting auto-gain to _on_ / Continuous"); - } else { - } - - } catch (const GenICam::GenericException &e) { - NODELET_ERROR_STREAM( - "An exception while setting gain: " << e.GetDescription()); - return false; - } - return true; -} - -void ArenaCameraNodeletBase::disableAllRunningAutoBrightessFunctions() { - GenApi::CStringPtr pExposureAuto = pNodeMap_->GetNode("ExposureAuto"); - GenApi::CStringPtr pGainAuto = pNodeMap_->GetNode("GainAuto"); - - if (!pExposureAuto || !GenApi::IsWritable(pExposureAuto) || !pGainAuto || - !GenApi::IsWritable(pGainAuto)) { - NODELET_WARN_STREAM("Unable to disable auto gain & exposure"); - return; - } - - else { - Arena::SetNodeValue(pDevice_->GetNodeMap(), - "ExposureAuto", "Off"); - Arena::SetNodeValue(pDevice_->GetNodeMap(), "GainAuto", - "Off"); - } -} - -//~~ Gamma ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - -bool ArenaCameraNodeletBase::setGamma(const float &target_gamma) { - // for GigE cameras you have to enable gamma first - - GenApi::CBooleanPtr pGammaEnable = - pDevice_->GetNodeMap()->GetNode("GammaEnable"); - if (GenApi::IsWritable(pGammaEnable)) { - pGammaEnable->SetValue(true); - } - - GenApi::CFloatPtr pGamma = pDevice_->GetNodeMap()->GetNode("Gamma"); - if (!pGamma || !GenApi::IsWritable(pGamma)) { - NODELET_WARN("Cannot set gamma, it is not writeable"); - return false; - } else { - try { - float gamma_to_set = target_gamma; - if (pGamma->GetMin() > gamma_to_set) { - gamma_to_set = pGamma->GetMin(); - NODELET_WARN_STREAM( - "Desired gamma unreachable! Setting to lower limit: " - << gamma_to_set); - } else if (pGamma->GetMax() < gamma_to_set) { - gamma_to_set = pGamma->GetMax(); - NODELET_WARN_STREAM( - "Desired gamma unreachable! Setting to upper limit: " - << gamma_to_set); - } - - NODELET_INFO_STREAM("Setting gamma to " << gamma_to_set); - pGamma->SetValue(gamma_to_set); - - } catch (const GenICam::GenericException &e) { - NODELET_ERROR_STREAM("An exception while setting target gamma to " - << target_gamma - << " occurred: " << e.GetDescription()); - return false; - } - } - return true; -} - -void ArenaCameraNodeletBase::setupSamplingIndices( - std::vector &indices, std::size_t rows, std::size_t cols, - int downsampling_factor) { - indices.clear(); - std::size_t min_window_height = - static_cast(rows) / static_cast(downsampling_factor); - cv::Point2i start_pt(0, 0); - cv::Point2i end_pt(cols, rows); - // add the iamge center point only once - sampling_indices_.push_back(0.5 * rows * cols); - genSamplingIndicesRec(indices, min_window_height, start_pt, end_pt); - std::sort(indices.begin(), indices.end()); - return; -} - -void ArenaCameraNodeletBase::genSamplingIndicesRec( - std::vector &indices, const std::size_t &min_window_height, - const cv::Point2i &s, // start - const cv::Point2i &e) // end -{ - if (static_cast(std::abs(e.y - s.y)) <= min_window_height) { - return; // abort criteria -> shrinked window has the min_col_size - } - /* - * sampled img: point: idx: - * s 0 0 0 0 0 0 a) [(e.x-s.x)*0.5, (e.y-s.y)*0.5] a.x*a.y*0.5 - * 0 0 0 d 0 0 0 b) [a.x, 1.5*a.y] b.y*img_rows+b.x - * 0 0 0 0 0 0 0 c) [0.5*a.x, a.y] c.y*img_rows+c.x - * 0 c 0 a 0 f 0 d) [a.x, 0.5*a.y] d.y*img_rows+d.x - * 0 0 0 0 0 0 0 f) [1.5*a.x, a.y] f.y*img_rows+f.x - * 0 0 0 b 0 0 0 - * 0 0 0 0 0 0 e - */ - cv::Point2i a, b, c, d, f, delta; - a = s + 0.5 * (e - s); // center point - delta = 0.5 * (e - s); - b = s + cv::Point2i(delta.x, 1.5 * delta.y); - c = s + cv::Point2i(0.5 * delta.x, delta.y); - d = s + cv::Point2i(delta.x, 0.5 * delta.y); - f = s + cv::Point2i(1.5 * delta.x, delta.y); - // \todo{amarburg} This broke when I made pImage_ a non-member - // indices.push_back(b.y * pImage_->GetWidth() + b.x); - // indices.push_back(c.y * pImage_->GetWidth() + c.x); - // indices.push_back(d.y * pImage_->GetWidth() + d.x); - // indices.push_back(f.y * pImage_->GetWidth() + f.x); - genSamplingIndicesRec(indices, min_window_height, s, a); - genSamplingIndicesRec(indices, min_window_height, a, e); - genSamplingIndicesRec(indices, min_window_height, cv::Point2i(s.x, a.y), - cv::Point2i(a.x, e.y)); - genSamplingIndicesRec(indices, min_window_height, cv::Point2i(a.x, s.y), - cv::Point2i(e.x, a.y)); - return; -} - -float ArenaCameraNodeletBase::calcCurrentBrightness() { - boost::lock_guard lock(device_mutex_); - if (img_raw_msg_.data.empty()) { - return 0.0; - } - float sum = 0.0; - if (sensor_msgs::image_encodings::isMono(img_raw_msg_.encoding)) { - // The mean brightness is calculated using a subset of all pixels - for (const std::size_t &idx : sampling_indices_) { - sum += img_raw_msg_.data.at(idx); - } - if (sum > 0.0) { - sum /= static_cast(sampling_indices_.size()); - } - } else { - // The mean brightness is calculated using all pixels and all channels - sum = - std::accumulate(img_raw_msg_.data.begin(), img_raw_msg_.data.end(), 0); - if (sum > 0.0) { - sum /= static_cast(img_raw_msg_.data.size()); - } - } - return sum; -} - -//------------------------------------------------------------------- -// -// HDR Channel Query and set functions -// -float ArenaCameraNodeletBase::currentHdrGain(int channel) { - try { - Arena::SetNodeValue(pDevice_->GetNodeMap(), - "HDRTuningChannelSelector", channel); - - return Arena::GetNodeValue(pDevice_->GetNodeMap(), - "HDRChannelAnalogGain"); - } catch (const GenICam::GenericException &e) { - NODELET_ERROR_STREAM( - "Exception while querying HDR gain: " << e.GetDescription()); - return -1; - } -} - -float ArenaCameraNodeletBase::currentHdrExposure(int channel) { - try { - Arena::SetNodeValue(pDevice_->GetNodeMap(), - "HDRTuningChannelSelector", channel); - - return Arena::GetNodeValue(pDevice_->GetNodeMap(), - "HDRChannelExposureTime"); - } catch (const GenICam::GenericException &e) { - NODELET_ERROR_STREAM( - "Exception while querying HDR exposure time: " << e.GetDescription()); - return -1; - } -} - -//------------------------------------------------------------------- -// Functions for dealing with LUT -// -// \todo{amarburg} Very simple right now -// - -void ArenaCameraNodeletBase::enableLUT(bool enable) { - try { - Arena::SetNodeValue(pDevice_->GetNodeMap(), "LUTEnable", enable); - } catch (const GenICam::GenericException &e) { - NODELET_ERROR_STREAM("An exception while setting LUTEnable to " - << (enable ? "true" : "false") - << " occurred: " << e.GetDescription()); - } -} - -//------------------------------------------------------------------------ -// ROS Reconfigure callback -// - -void ArenaCameraNodeletBase::reconfigureCallback(ArenaCameraConfig &config, - uint32_t level) { - const auto stop_level = - (uint32_t)dynamic_reconfigure::SensorLevels::RECONFIGURE_STOP; - - const bool was_streaming = is_streaming_; - if (level >= stop_level) { - ROS_INFO("Stopping sensor for reconfigure"); - stopStreaming(); - } - - NODELET_INFO_STREAM("In reconfigureCallback"); - - // -- The following params require stopping streaming, only set if needed -- - if (config.frame_rate != previous_config_.frame_rate) { - arena_camera_parameter_set_.setFrameRate(config.frame_rate); - updateFrameRate(); - } - - // if (config.target_brightness != previous_config_.target_brightness) { - setTargetBrightness(config.target_brightness); - //} - - // -- The following can be set while streaming, just set them every time - - // if ((config.auto_exposure != previous_config_.auto_exposure) || - // (config.auto_exposure_max_ms != previous_config_.auto_exposure_max_ms) - // || (config.exposure_ms != previous_config_.exposure_ms)) { - arena_camera_parameter_set_.exposure_auto_ = config.auto_exposure; - arena_camera_parameter_set_.exposure_ms_ = config.exposure_ms; - arena_camera_parameter_set_.auto_exposure_max_ms_ = - config.auto_exposure_max_ms; - - if (config.auto_exposure) { - setExposure(ArenaCameraNodeletBase::AutoExposureMode::Continuous, - config.auto_exposure_max_ms); - } else { - setExposure(ArenaCameraNodeletBase::AutoExposureMode::Off, - config.exposure_ms); - } - // } - - // if ((config.auto_gain != previous_config_.auto_gain)) { - arena_camera_parameter_set_.gain_auto_ = config.auto_gain; - - if (arena_camera_parameter_set_.gain_auto_) { - setGain(ArenaCameraNodeletBase::AutoGainMode::Continuous); - } else { - setGain(ArenaCameraNodeletBase::AutoGainMode::Off, config.gain); - } - // } - - arena_camera_parameter_set_.gamma_ = config.gamma; - setGamma(arena_camera_parameter_set_.gamma_); - - if ((level >= stop_level) && was_streaming) { - startStreaming(); - } - - // Save config - previous_config_ = config; -} - -//------------------------------------------------------------------------ -// ROS Disagnostics callbacks -// - -void ArenaCameraNodeletBase::create_diagnostics( - diagnostic_updater::DiagnosticStatusWrapper &stat) {} - -void ArenaCameraNodeletBase::create_camera_info_diagnostics( - diagnostic_updater::DiagnosticStatusWrapper &stat) { - if (camera_info_manager_->isCalibrated()) { - stat.summaryf(DiagnosticStatus::OK, "Intrinsic calibration found"); - } else { - stat.summaryf(DiagnosticStatus::ERROR, "No intrinsic calibration found"); - } -} - -void ArenaCameraNodeletBase::diagnostics_timer_callback_( - const ros::TimerEvent &) { - diagnostics_updater_.update(); -} - -} // namespace arena_camera diff --git a/arena_camera/src/nodes/polled_node_main.cpp b/arena_camera/src/nodes/polled_node_main.cpp index 668eb16..60ebc7e 100644 --- a/arena_camera/src/nodes/polled_node_main.cpp +++ b/arena_camera/src/nodes/polled_node_main.cpp @@ -27,24 +27,11 @@ * POSSIBILITY OF SUCH DAMAGE. *****************************************************************************/ -/* Authors: debout@magazino.eu - * grimm@magazino.eu - * engelhard@magazino.eu - */ +#include "arena_camera/arena_camera_nodes.h" -// ROS -#include -#include - -int main(int argc, char **argv) { - ros::init(argc, argv, "arena_camera_node"); - - nodelet::Loader nodelet; - nodelet::M_string remap(ros::names::getRemappings()); - nodelet::V_string nargv; - std::string nodelet_name = ros::this_node::getName(); - nodelet.load(nodelet_name, "arena_camera/ArenaCameraPolledNodelet", remap, - nargv); - ros::spin(); +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); return 0; } diff --git a/arena_camera/src/nodes/streaming_node_main.cpp b/arena_camera/src/nodes/streaming_node_main.cpp index ca264fd..8851cc3 100644 --- a/arena_camera/src/nodes/streaming_node_main.cpp +++ b/arena_camera/src/nodes/streaming_node_main.cpp @@ -32,19 +32,11 @@ * engelhard@magazino.eu */ -// ROS -#include -#include +#include "arena_camera/arena_camera_nodes.h" -int main(int argc, char **argv) { - ros::init(argc, argv, "arena_camera_node"); - - nodelet::Loader nodelet; - nodelet::M_string remap(ros::names::getRemappings()); - nodelet::V_string nargv; - std::string nodelet_name = ros::this_node::getName(); - nodelet.load(nodelet_name, "arena_camera/ArenaCameraStreamingNodelet", remap, - nargv); - ros::spin(); +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); return 0; } diff --git a/arena_camera/src/polled_node.cpp b/arena_camera/src/polled_node.cpp new file mode 100644 index 0000000..df729ab --- /dev/null +++ b/arena_camera/src/polled_node.cpp @@ -0,0 +1,379 @@ +/****************************************************************************** + * 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. + *****************************************************************************/ + +#include "arena_camera/arena_camera_nodes.h" + +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(); + +// try { +// pDevice_->StartStream(); +// } catch (GenICam::GenericException &e) { +// NODELET_ERROR_STREAM("Error while configuring camera: \r\n" +// << e.GetDescription()); +// return; +// } + +// grab_imgs_raw_as_.reset(new GrabImagesAS( +// getNodeHandle(), "grab_images_raw", +// boost::bind(&ArenaCameraPolledNode::grabImagesRawActionExecuteCB, this, +// _1), +// false)); + +// grab_imgs_raw_as_->start(); +// } + +// bool ArenaCameraPolledNode::sendSoftwareTrigger() { +// boost::lock_guard lock(device_mutex_); +// bool retval = false; + +// try { +// GenApi::CStringPtr pTriggerMode = +// pDevice_->GetNodeMap()->GetNode("TriggerMode"); + +// if (GenApi::IsWritable(pTriggerMode)) { +// bool isTriggerArmed = false; + +// do { +// isTriggerArmed = +// Arena::GetNodeValue(pDevice_->GetNodeMap(), +// "TriggerArmed"); +// } while (isTriggerArmed == false); +// Arena::ExecuteNode(pDevice_->GetNodeMap(), "TriggerSoftware"); +// } +// } catch (GenICam::GenericException &e) { +// ; +// } + +// return true; +// } + +// bool ArenaCameraPolledNode::grabImage() { +// boost::lock_guard lock(device_mutex_); +// bool retval = false; + +// try { +// GenApi::CStringPtr pTriggerMode = +// pDevice_->GetNodeMap()->GetNode("TriggerMode"); + +// if (GenApi::IsWritable(pTriggerMode)) { +// bool isTriggerArmed = false; + +// do { +// isTriggerArmed = +// Arena::GetNodeValue(pDevice_->GetNodeMap(), +// "TriggerArmed"); +// } while (isTriggerArmed == false); +// Arena::ExecuteNode(pDevice_->GetNodeMap(), "TriggerSoftware"); +// } + +// Arena::IBuffer *pBuffer = pDevice_->GetBuffer(250); + +// if (pBuffer->HasImageData()) { +// Arena::IImage *pImage = dynamic_cast(pBuffer); + +// if (pImage->IsIncomplete()) { +// auto node_map = pDevice_->GetNodeMap(); +// const auto payload_size = +// Arena::GetNodeValue(node_map, "PayloadSize"); +// if (pBuffer->DataLargerThanBuffer()) { +// NODELET_WARN_STREAM("Image incomplete: data larger than buffer; " +// << pBuffer->GetSizeOfBuffer() << " > " +// << payload_size); +// } else { +// NODELET_WARN_STREAM("Image incomplete: Payload size = " +// << pBuffer->GetSizeFilled() << " , buffer size +// = " +// << pBuffer->GetSizeOfBuffer() << " , expected " +// << payload_size); +// } + +// goto out; +// } + +// img_raw_msg_.header.stamp = ros::Time::now(); + +// // Will return false if PixelEndiannessUnknown +// img_raw_msg_.is_bigendian = +// (pImage->GetPixelEndianness() == Arena::PixelEndiannessBig); + +// img_raw_msg_.encoding = currentROSEncoding(); +// img_raw_msg_.height = pImage->GetHeight(); +// img_raw_msg_.width = pImage->GetWidth(); + +// const unsigned int bytes_per_pixel = pImage->GetBitsPerPixel() / 8; +// img_raw_msg_.step = img_raw_msg_.width * bytes_per_pixel; + +// const unsigned int data_size = img_raw_msg_.height * img_raw_msg_.step; + +// // \todo{amarburg} Compare to Buffer/Image payload size + +// img_raw_msg_.data.resize(data_size); +// memcpy(&img_raw_msg_.data[0], pImage->GetData(), data_size); + +// retval = true; +// } + +// out: +// pDevice_->RequeueBuffer(pBuffer); +// } catch (GenICam::GenericException &e) { +// ; +// } + +// return retval; +// } + +// void ArenaCameraPolledNode::grabImagesRawActionExecuteCB( +// const camera_control_msgs::GrabImagesGoal::ConstPtr &goal) { +// camera_control_msgs::GrabImagesResult result; +// result = grabImagesRaw(goal, grab_imgs_raw_as_.get()); +// grab_imgs_raw_as_->setSucceeded(result); +// } + +// camera_control_msgs::GrabImagesResult ArenaCameraPolledNode::grabImagesRaw( +// const camera_control_msgs::GrabImagesGoal::ConstPtr &goal, +// GrabImagesAS *action_server) { +// camera_control_msgs::GrabImagesResult result; +// camera_control_msgs::GrabImagesFeedback feedback; + +// #if DEBUG +// std::cout << *goal << std::endl; +// #endif + +// // if (goal->exposure_given && goal->exposure_times.empty()) { +// // NODELET_ERROR_STREAM( +// // "GrabImagesRaw action server received request and " +// // << "'exposure_given' is true, but the 'exposure_times' vector is " +// // << "empty! Not enough information to execute acquisition!"); +// // result.success = false; +// // return result; +// // } + +// if (goal->gain_given && goal->gain_values.empty()) { +// NODELET_ERROR_STREAM( +// "GrabImagesRaw action server received request and " +// << "'gain_given' is true, but the 'gain_values' vector is " +// << "empty! Not enough information to execute acquisition!"); +// result.success = false; +// return result; +// } + +// if (goal->brightness_given && goal->brightness_values.empty()) { +// NODELET_ERROR_STREAM( +// "GrabImagesRaw action server received request and " +// << "'brightness_given' is true, but the 'brightness_values' vector" +// << " is empty! Not enough information to execute acquisition!"); +// result.success = false; +// return result; +// } + +// if (goal->gamma_given && goal->gamma_values.empty()) { +// NODELET_ERROR_STREAM( +// "GrabImagesRaw action server received request and " +// << "'gamma_given' is true, but the 'gamma_values' vector is " +// << "empty! Not enough information to execute acquisition!"); +// result.success = false; +// return result; +// } + +// std::vector candidates; +// candidates.resize(4); // gain, exposure, gamma, brightness +// candidates.at(0) = goal->gain_given ? goal->gain_values.size() : 0; +// candidates.at(1) = goal->exposure_given ? goal->exposure_times.size() : 0; +// candidates.at(2) = +// goal->brightness_given ? goal->brightness_values.size() : 0; +// candidates.at(3) = goal->gamma_given ? goal->gamma_values.size() : 0; + +// size_t n_images = *std::max_element(candidates.begin(), candidates.end()); + +// if (goal->exposure_given && goal->exposure_times.size() != n_images) { +// NODELET_ERROR_STREAM( +// "Size of requested exposure times does not match to " +// << "the size of the requested vaules of brightness, gain or " +// << "gamma! Can't grab!"); +// result.success = false; +// return result; +// } + +// if (goal->gain_given && goal->gain_values.size() != n_images) { +// NODELET_ERROR_STREAM( +// "Size of requested gain values does not match to " +// << "the size of the requested exposure times or the vaules of " +// << "brightness or gamma! Can't grab!"); +// result.success = false; +// return result; +// } + +// if (goal->gamma_given && goal->gamma_values.size() != n_images) { +// NODELET_ERROR_STREAM( +// "Size of requested gamma values does not match to " +// << "the size of the requested exposure times or the vaules of " +// << "brightness or gain! Can't grab!"); +// result.success = false; +// return result; +// } + +// if (goal->brightness_given && goal->brightness_values.size() != n_images) { +// NODELET_ERROR_STREAM( +// "Size of requested brightness values does not match to " +// << "the size of the requested exposure times or the vaules of gain " +// "or " +// << "gamma! Can't grab!"); +// result.success = false; +// return result; +// } + +// if (goal->brightness_given && !(goal->exposure_auto || goal->gain_auto)) { +// NODELET_ERROR_STREAM( +// "Error while executing the GrabImagesRawAction: A " +// << "target brightness is provided but Exposure time AND gain are " +// << "declared as fix, so its impossible to reach the brightness"); +// result.success = false; +// return result; +// } + +// result.images.resize(n_images); +// result.reached_exposure_times.resize(n_images); +// result.reached_gain_values.resize(n_images); +// result.reached_gamma_values.resize(n_images); +// result.reached_brightness_values.resize(n_images); + +// result.success = true; + +// boost::lock_guard lock(device_mutex_); + +// float previous_exp, previous_gain, previous_gamma; +// if (goal->exposure_given) { +// previous_exp = +// Arena::GetNodeValue(pDevice_->GetNodeMap(), "ExposureTime"); +// } +// if (goal->gain_given) { +// previous_gain = currentGain(); +// } +// if (goal->gamma_given) { +// previous_gamma = currentGamma(); +// } +// if (goal->brightness_given) { +// previous_gain = currentGain(); +// previous_exp = currentExposure(); +// } + +// for (std::size_t i = 0; i < n_images; ++i) { +// if (goal->exposure_given) { +// // result.success = setExposure(goal->exposure_times[i], +// // result.reached_exposure_times[i]); +// } +// if (goal->gain_given) { +// result.success = setGain(ArenaCameraPolledNode::AutoGainMode::Off, +// goal->gain_values[i]); +// result.reached_gain_values[i] = currentGain(); +// } +// if (goal->gamma_given) { +// result.success = setGamma(goal->gamma_values[i]); +// result.reached_gamma_values[i] = currentGamma(); +// } +// if (goal->brightness_given) { +// // int reached_brightness; +// // result.success = +// // setBrightness(goal->brightness_values[i], reached_brightness, +// // goal->exposure_auto, goal->gain_auto); +// // result.reached_brightness_values[i] = +// // static_cast(reached_brightness); +// // result.reached_exposure_times[i] = currentExposure(); +// // result.reached_gain_values[i] = currentGain(); +// } +// if (!result.success) { +// NODELET_ERROR_STREAM( +// "Error while setting one of the desired image " +// << "properties in the GrabImagesRawActionCB. Aborting!"); +// break; +// } + +// // \todo{amarburg} What is this? +// // sensor_msgs::Image &img = result.images[i]; +// // img.encoding = currentROSEncoding(); +// // img.height = pImage_->GetHeight(); +// // img.width = pImage_->GetWidth(); +// // // step = full row length in bytes, img_size = (step * rows), +// // // imagePixelDepth already contains the number of channels +// // img_raw_msg_.step = img_raw_msg_.width * (pImage_->GetBitsPerPixel() / +// // 8); + +// // img.header.stamp = ros::Time::now(); +// // img.header.frame_id = cameraFrame(); +// feedback.curr_nr_images_taken = i + 1; + +// if (action_server != nullptr) { +// action_server->publishFeedback(feedback); +// } +// } +// if (camera_info_manager_) { +// sensor_msgs::CameraInfoPtr cam_info( +// new sensor_msgs::CameraInfo(camera_info_manager_->getCameraInfo())); +// result.cam_info = *cam_info; +// } + +// // restore previous settings: +// float reached_val; +// if (goal->exposure_given) { +// // setExposure(previous_exp, reached_val); +// } +// if (goal->gain_given) { +// // setGain(previous_gain, reached_val); +// } +// if (goal->gamma_given) { +// // setGamma(previous_gamma, reached_val); +// } +// if (goal->brightness_given) { +// // setGain(previous_gain, reached_val); +// // setExposure(previous_exp, reached_val); +// } +// return result; +// } + +} // namespace arena_camera + +// Register the component with class_loader. +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(arena_camera::ArenaCameraPolledNode) diff --git a/arena_camera/src/polled_nodelet.cpp b/arena_camera/src/polled_nodelet.cpp deleted file mode 100644 index 83008ec..0000000 --- a/arena_camera/src/polled_nodelet.cpp +++ /dev/null @@ -1,374 +0,0 @@ -/****************************************************************************** - * Software License Agreement (BSD License) - * - * 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. - *****************************************************************************/ - -#include - -#include "arena_camera/arena_camera_nodelet.h" - -namespace arena_camera { - -using sensor_msgs::CameraInfo; -using sensor_msgs::CameraInfoPtr; - -ArenaCameraPolledNodelet::ArenaCameraPolledNodelet() - : ArenaCameraNodeletBase(), grab_imgs_raw_as_() {} - -ArenaCameraPolledNodelet::~ArenaCameraPolledNodelet() {} - -// -// Nodelet::onInit function - -void ArenaCameraPolledNodelet::onInit() { - ArenaCameraNodeletBase::onInit(); - - try { - pDevice_->StartStream(); - } catch (GenICam::GenericException &e) { - NODELET_ERROR_STREAM("Error while configuring camera: \r\n" - << e.GetDescription()); - return; - } - - grab_imgs_raw_as_.reset(new GrabImagesAS( - getNodeHandle(), "grab_images_raw", - boost::bind(&ArenaCameraPolledNodelet::grabImagesRawActionExecuteCB, this, - _1), - false)); - - grab_imgs_raw_as_->start(); -} - -bool ArenaCameraPolledNodelet::sendSoftwareTrigger() { - boost::lock_guard lock(device_mutex_); - bool retval = false; - - try { - GenApi::CStringPtr pTriggerMode = - pDevice_->GetNodeMap()->GetNode("TriggerMode"); - - if (GenApi::IsWritable(pTriggerMode)) { - bool isTriggerArmed = false; - - do { - isTriggerArmed = - Arena::GetNodeValue(pDevice_->GetNodeMap(), "TriggerArmed"); - } while (isTriggerArmed == false); - Arena::ExecuteNode(pDevice_->GetNodeMap(), "TriggerSoftware"); - } - } catch (GenICam::GenericException &e) { - ; - } - - return true; -} - -bool ArenaCameraPolledNodelet::grabImage() { - boost::lock_guard lock(device_mutex_); - bool retval = false; - - try { - GenApi::CStringPtr pTriggerMode = - pDevice_->GetNodeMap()->GetNode("TriggerMode"); - - if (GenApi::IsWritable(pTriggerMode)) { - bool isTriggerArmed = false; - - do { - isTriggerArmed = - Arena::GetNodeValue(pDevice_->GetNodeMap(), "TriggerArmed"); - } while (isTriggerArmed == false); - Arena::ExecuteNode(pDevice_->GetNodeMap(), "TriggerSoftware"); - } - - Arena::IBuffer *pBuffer = pDevice_->GetBuffer(250); - - if (pBuffer->HasImageData()) { - Arena::IImage *pImage = dynamic_cast(pBuffer); - - if (pImage->IsIncomplete()) { - auto node_map = pDevice_->GetNodeMap(); - const auto payload_size = - Arena::GetNodeValue(node_map, "PayloadSize"); - if (pBuffer->DataLargerThanBuffer()) { - NODELET_WARN_STREAM("Image incomplete: data larger than buffer; " - << pBuffer->GetSizeOfBuffer() << " > " - << payload_size); - } else { - NODELET_WARN_STREAM("Image incomplete: Payload size = " - << pBuffer->GetSizeFilled() << " , buffer size = " - << pBuffer->GetSizeOfBuffer() << " , expected " - << payload_size); - } - - goto out; - } - - img_raw_msg_.header.stamp = ros::Time::now(); - - // Will return false if PixelEndiannessUnknown - img_raw_msg_.is_bigendian = - (pImage->GetPixelEndianness() == Arena::PixelEndiannessBig); - - img_raw_msg_.encoding = currentROSEncoding(); - img_raw_msg_.height = pImage->GetHeight(); - img_raw_msg_.width = pImage->GetWidth(); - - const unsigned int bytes_per_pixel = pImage->GetBitsPerPixel() / 8; - img_raw_msg_.step = img_raw_msg_.width * bytes_per_pixel; - - const unsigned int data_size = img_raw_msg_.height * img_raw_msg_.step; - - // \todo{amarburg} Compare to Buffer/Image payload size - - img_raw_msg_.data.resize(data_size); - memcpy(&img_raw_msg_.data[0], pImage->GetData(), data_size); - - retval = true; - } - - out: - pDevice_->RequeueBuffer(pBuffer); - } catch (GenICam::GenericException &e) { - ; - } - - return retval; -} - -void ArenaCameraPolledNodelet::grabImagesRawActionExecuteCB( - const camera_control_msgs::GrabImagesGoal::ConstPtr &goal) { - camera_control_msgs::GrabImagesResult result; - result = grabImagesRaw(goal, grab_imgs_raw_as_.get()); - grab_imgs_raw_as_->setSucceeded(result); -} - -camera_control_msgs::GrabImagesResult ArenaCameraPolledNodelet::grabImagesRaw( - const camera_control_msgs::GrabImagesGoal::ConstPtr &goal, - GrabImagesAS *action_server) { - camera_control_msgs::GrabImagesResult result; - camera_control_msgs::GrabImagesFeedback feedback; - -#if DEBUG - std::cout << *goal << std::endl; -#endif - - // if (goal->exposure_given && goal->exposure_times.empty()) { - // NODELET_ERROR_STREAM( - // "GrabImagesRaw action server received request and " - // << "'exposure_given' is true, but the 'exposure_times' vector is " - // << "empty! Not enough information to execute acquisition!"); - // result.success = false; - // return result; - // } - - if (goal->gain_given && goal->gain_values.empty()) { - NODELET_ERROR_STREAM( - "GrabImagesRaw action server received request and " - << "'gain_given' is true, but the 'gain_values' vector is " - << "empty! Not enough information to execute acquisition!"); - result.success = false; - return result; - } - - if (goal->brightness_given && goal->brightness_values.empty()) { - NODELET_ERROR_STREAM( - "GrabImagesRaw action server received request and " - << "'brightness_given' is true, but the 'brightness_values' vector" - << " is empty! Not enough information to execute acquisition!"); - result.success = false; - return result; - } - - if (goal->gamma_given && goal->gamma_values.empty()) { - NODELET_ERROR_STREAM( - "GrabImagesRaw action server received request and " - << "'gamma_given' is true, but the 'gamma_values' vector is " - << "empty! Not enough information to execute acquisition!"); - result.success = false; - return result; - } - - std::vector candidates; - candidates.resize(4); // gain, exposure, gamma, brightness - candidates.at(0) = goal->gain_given ? goal->gain_values.size() : 0; - candidates.at(1) = goal->exposure_given ? goal->exposure_times.size() : 0; - candidates.at(2) = - goal->brightness_given ? goal->brightness_values.size() : 0; - candidates.at(3) = goal->gamma_given ? goal->gamma_values.size() : 0; - - size_t n_images = *std::max_element(candidates.begin(), candidates.end()); - - if (goal->exposure_given && goal->exposure_times.size() != n_images) { - NODELET_ERROR_STREAM( - "Size of requested exposure times does not match to " - << "the size of the requested vaules of brightness, gain or " - << "gamma! Can't grab!"); - result.success = false; - return result; - } - - if (goal->gain_given && goal->gain_values.size() != n_images) { - NODELET_ERROR_STREAM( - "Size of requested gain values does not match to " - << "the size of the requested exposure times or the vaules of " - << "brightness or gamma! Can't grab!"); - result.success = false; - return result; - } - - if (goal->gamma_given && goal->gamma_values.size() != n_images) { - NODELET_ERROR_STREAM( - "Size of requested gamma values does not match to " - << "the size of the requested exposure times or the vaules of " - << "brightness or gain! Can't grab!"); - result.success = false; - return result; - } - - if (goal->brightness_given && goal->brightness_values.size() != n_images) { - NODELET_ERROR_STREAM( - "Size of requested brightness values does not match to " - << "the size of the requested exposure times or the vaules of gain " - "or " - << "gamma! Can't grab!"); - result.success = false; - return result; - } - - if (goal->brightness_given && !(goal->exposure_auto || goal->gain_auto)) { - NODELET_ERROR_STREAM( - "Error while executing the GrabImagesRawAction: A " - << "target brightness is provided but Exposure time AND gain are " - << "declared as fix, so its impossible to reach the brightness"); - result.success = false; - return result; - } - - result.images.resize(n_images); - result.reached_exposure_times.resize(n_images); - result.reached_gain_values.resize(n_images); - result.reached_gamma_values.resize(n_images); - result.reached_brightness_values.resize(n_images); - - result.success = true; - - boost::lock_guard lock(device_mutex_); - - float previous_exp, previous_gain, previous_gamma; - if (goal->exposure_given) { - previous_exp = - Arena::GetNodeValue(pDevice_->GetNodeMap(), "ExposureTime"); - } - if (goal->gain_given) { - previous_gain = currentGain(); - } - if (goal->gamma_given) { - previous_gamma = currentGamma(); - } - if (goal->brightness_given) { - previous_gain = currentGain(); - previous_exp = currentExposure(); - } - - for (std::size_t i = 0; i < n_images; ++i) { - if (goal->exposure_given) { - // result.success = setExposure(goal->exposure_times[i], - // result.reached_exposure_times[i]); - } - if (goal->gain_given) { - result.success = setGain(ArenaCameraPolledNodelet::AutoGainMode::Off, - goal->gain_values[i]); - result.reached_gain_values[i] = currentGain(); - } - if (goal->gamma_given) { - result.success = setGamma(goal->gamma_values[i]); - result.reached_gamma_values[i] = currentGamma(); - } - if (goal->brightness_given) { - // int reached_brightness; - // result.success = - // setBrightness(goal->brightness_values[i], reached_brightness, - // goal->exposure_auto, goal->gain_auto); - // result.reached_brightness_values[i] = - // static_cast(reached_brightness); - // result.reached_exposure_times[i] = currentExposure(); - // result.reached_gain_values[i] = currentGain(); - } - if (!result.success) { - NODELET_ERROR_STREAM( - "Error while setting one of the desired image " - << "properties in the GrabImagesRawActionCB. Aborting!"); - break; - } - - // \todo{amarburg} What is this? - // sensor_msgs::Image &img = result.images[i]; - // img.encoding = currentROSEncoding(); - // img.height = pImage_->GetHeight(); - // img.width = pImage_->GetWidth(); - // // step = full row length in bytes, img_size = (step * rows), - // // imagePixelDepth already contains the number of channels - // img_raw_msg_.step = img_raw_msg_.width * (pImage_->GetBitsPerPixel() / - // 8); - - // img.header.stamp = ros::Time::now(); - // img.header.frame_id = cameraFrame(); - feedback.curr_nr_images_taken = i + 1; - - if (action_server != nullptr) { - action_server->publishFeedback(feedback); - } - } - if (camera_info_manager_) { - sensor_msgs::CameraInfoPtr cam_info( - new sensor_msgs::CameraInfo(camera_info_manager_->getCameraInfo())); - result.cam_info = *cam_info; - } - - // restore previous settings: - float reached_val; - if (goal->exposure_given) { - // setExposure(previous_exp, reached_val); - } - if (goal->gain_given) { - // setGain(previous_gain, reached_val); - } - if (goal->gamma_given) { - // setGamma(previous_gamma, reached_val); - } - if (goal->brightness_given) { - // setGain(previous_gain, reached_val); - // setExposure(previous_exp, reached_val); - } - return result; -} - -} // namespace arena_camera - -PLUGINLIB_EXPORT_CLASS(arena_camera::ArenaCameraPolledNodelet, nodelet::Nodelet) diff --git a/arena_camera/src/streaming_node.cpp b/arena_camera/src/streaming_node.cpp new file mode 100644 index 0000000..a52f924 --- /dev/null +++ b/arena_camera/src/streaming_node.cpp @@ -0,0 +1,175 @@ +/****************************************************************************** + * 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 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. + * + * 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. + *****************************************************************************/ + +#include "arena_camera/arena_camera_nodes.h" +#include "arena_camera/encoding_conversions.h" + +namespace arena_camera { + +// using sensor_msgs::CameraInfo; +// using sensor_msgs::CameraInfoPtr; + +ArenaCameraStreamingNode::ArenaCameraStreamingNode( + const rclcpp::NodeOptions &options) + : ArenaCameraBaseNode("arena_streaming", options), + image_callback_obj_(std::bind(&ArenaCameraStreamingNode::newImageCb, this, + std::placeholders::_1)) { + pDevice_->RegisterImageCallback(&image_callback_obj_); +} + +ArenaCameraStreamingNode::~ArenaCameraStreamingNode() { + pDevice_->DeregisterImageCallback(&image_callback_obj_); +} + +// +// Nodelet::onInit function + +// void ArenaCameraStreamingNode::onInit() { +// // Call parent initiallzer +// ArenaCameraNodeletBase::onInit(); + +// try { +// startStreaming(); +// } catch (GenICam::GenericException &e) { +// NODELET_ERROR_STREAM("Error while configuring camera: \r\n" +// << e.GetDescription()); +// return; +// } + +// } + +void ArenaCameraStreamingNode::newImageCb(Arena::IImage *pImage) { + // // Is this always true if the ImageCallback is called? + // Arena::IBuffer *pBuffer = dynamic_cast(pImage); + // if (pBuffer->HasImageData()) { + // if (pImage->IsIncomplete()) { + // auto node_map = pDevice_->GetNodeMap(); + // const auto payload_size = + // Arena::GetNodeValue(node_map, "PayloadSize"); + // if (pBuffer->DataLargerThanBuffer()) { + // NODELET_WARN_STREAM("Image incomplete: data larger than buffer; " + // << pBuffer->GetSizeOfBuffer() << " > " + // << payload_size); + // } else { + // NODELET_WARN_STREAM("Image incomplete: Payload size = " + // << pBuffer->GetSizeFilled() + // << " , buffer size = " << + // pBuffer->GetSizeOfBuffer() + // << " , expected " << payload_size); + // } + + return; +} + +// img_raw_msg_.header.stamp = ros::Time::now(); + +// // Will return false if PixelEndiannessUnknown +// img_raw_msg_.is_bigendian = +// (pImage->GetPixelEndianness() == Arena::PixelEndiannessBig); + +// img_raw_msg_.encoding = currentROSEncoding(); +// img_raw_msg_.height = pImage->GetHeight(); +// img_raw_msg_.width = pImage->GetWidth(); + +// const unsigned int bytes_per_pixel = pImage->GetBitsPerPixel() / 8; +// img_raw_msg_.step = img_raw_msg_.width * bytes_per_pixel; + +// const unsigned int data_size = img_raw_msg_.height * img_raw_msg_.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()); + +// // \todo{amarburg} Validate image by comparing calculated image +// // size to actual Buffer/Image payload size +// img_raw_msg_.data.resize(data_size); +// memcpy(&img_raw_msg_.data[0], pImage->GetData(), data_size); + +// if (img_raw_pub_.getNumSubscribers() > 0) { +// // Create a new cam_info-object in every frame, because it might have +// // changed due to a 'set_camera_info'-service call +// sensor_msgs::CameraInfo cam_info = +// sensor_msgs::CameraInfo(camera_info_manager_->getCameraInfo()); +// cam_info.header.stamp = img_raw_msg_.header.stamp; + +// img_raw_pub_.publish(img_raw_msg_, cam_info); +// } +// } + +// imaging_msgs::ImagingMetadata meta_msg; +// meta_msg.header = img_raw_msg_.header; + +// meta_msg.exposure_us = currentExposure(); +// meta_msg.gain = currentGain(); + +// metadata_pub_.publish(meta_msg); + +// if (encoding_conversions::isHDR(currentROSEncoding())) { +// imaging_msgs::HdrImagingMetadata hdr_meta_msg; +// hdr_meta_msg.header = meta_msg.header; +// hdr_meta_msg.exposure_us = meta_msg.exposure_us; +// hdr_meta_msg.gain = meta_msg.gain; + +// const int num_hdr_channels = 4; +// hdr_meta_msg.hdr_exposure_us.resize(num_hdr_channels); +// hdr_meta_msg.hdr_gain.resize(num_hdr_channels); + +// for (int hdr_channel = 0; hdr_channel < num_hdr_channels; hdr_channel++) +// { +// hdr_meta_msg.hdr_exposure_us[hdr_channel] = +// currentHdrExposure(hdr_channel); +// hdr_meta_msg.hdr_gain[hdr_channel] = currentHdrGain(hdr_channel); +// } + +// hdr_metadata_pub_.publish(hdr_meta_msg); +// } +// } + +// void ArenaCameraStreamingNode::reconfigureCallback(ArenaCameraConfig &config, +// uint32_t level) { +// stopStreaming(); + +// ArenaCameraNodeletBase::reconfigureCallback(config, level); + +// startStreaming(); +// } + +} // namespace arena_camera + +// Register the component with class_loader. +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(arena_camera::ArenaCameraStreamingNode) diff --git a/arena_camera/src/streaming_nodelet.cpp b/arena_camera/src/streaming_nodelet.cpp deleted file mode 100644 index 43ed666..0000000 --- a/arena_camera/src/streaming_nodelet.cpp +++ /dev/null @@ -1,172 +0,0 @@ -/****************************************************************************** - * Software License Agreement (BSD License) - * - * Copyright (C) 2023, University of Washington. All rights reserved. - * - * based on - * - * 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 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. - * - * 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. - *****************************************************************************/ - -#include - -#include "arena_camera/arena_camera_nodelet.h" -#include "arena_camera/encoding_conversions.h" - -namespace arena_camera { - -using sensor_msgs::CameraInfo; -using sensor_msgs::CameraInfoPtr; - -ArenaCameraStreamingNodelet::ArenaCameraStreamingNodelet() - : ArenaCameraNodeletBase(), - image_callback_obj_(std::bind(&ArenaCameraStreamingNodelet::imageCallback, - this, std::placeholders::_1)) {} - -ArenaCameraStreamingNodelet::~ArenaCameraStreamingNodelet() { - pDevice_->DeregisterImageCallback(&image_callback_obj_); -} - -// -// Nodelet::onInit function - -void ArenaCameraStreamingNodelet::onInit() { - // Call parent initiallzer - ArenaCameraNodeletBase::onInit(); - - try { - startStreaming(); - } catch (GenICam::GenericException &e) { - NODELET_ERROR_STREAM("Error while configuring camera: \r\n" - << e.GetDescription()); - return; - } - - pDevice_->RegisterImageCallback(&image_callback_obj_); -} - -void ArenaCameraStreamingNodelet::imageCallback(Arena::IImage *pImage) { - // Is this always true if the ImageCallback is called? - Arena::IBuffer *pBuffer = dynamic_cast(pImage); - if (pBuffer->HasImageData()) { - if (pImage->IsIncomplete()) { - auto node_map = pDevice_->GetNodeMap(); - const auto payload_size = - Arena::GetNodeValue(node_map, "PayloadSize"); - if (pBuffer->DataLargerThanBuffer()) { - NODELET_WARN_STREAM("Image incomplete: data larger than buffer; " - << pBuffer->GetSizeOfBuffer() << " > " - << payload_size); - } else { - NODELET_WARN_STREAM("Image incomplete: Payload size = " - << pBuffer->GetSizeFilled() - << " , buffer size = " << pBuffer->GetSizeOfBuffer() - << " , expected " << payload_size); - } - - return; - } - - img_raw_msg_.header.stamp = ros::Time::now(); - - // Will return false if PixelEndiannessUnknown - img_raw_msg_.is_bigendian = - (pImage->GetPixelEndianness() == Arena::PixelEndiannessBig); - - img_raw_msg_.encoding = currentROSEncoding(); - img_raw_msg_.height = pImage->GetHeight(); - img_raw_msg_.width = pImage->GetWidth(); - - const unsigned int bytes_per_pixel = pImage->GetBitsPerPixel() / 8; - img_raw_msg_.step = img_raw_msg_.width * bytes_per_pixel; - - const unsigned int data_size = img_raw_msg_.height * img_raw_msg_.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()); - - // \todo{amarburg} Validate image by comparing calculated image - // size to actual Buffer/Image payload size - img_raw_msg_.data.resize(data_size); - memcpy(&img_raw_msg_.data[0], pImage->GetData(), data_size); - - if (img_raw_pub_.getNumSubscribers() > 0) { - // Create a new cam_info-object in every frame, because it might have - // changed due to a 'set_camera_info'-service call - sensor_msgs::CameraInfo cam_info = - sensor_msgs::CameraInfo(camera_info_manager_->getCameraInfo()); - cam_info.header.stamp = img_raw_msg_.header.stamp; - - img_raw_pub_.publish(img_raw_msg_, cam_info); - } - } - - imaging_msgs::ImagingMetadata meta_msg; - meta_msg.header = img_raw_msg_.header; - - meta_msg.exposure_us = currentExposure(); - meta_msg.gain = currentGain(); - - metadata_pub_.publish(meta_msg); - - if (encoding_conversions::isHDR(currentROSEncoding())) { - imaging_msgs::HdrImagingMetadata hdr_meta_msg; - hdr_meta_msg.header = meta_msg.header; - hdr_meta_msg.exposure_us = meta_msg.exposure_us; - hdr_meta_msg.gain = meta_msg.gain; - - const int num_hdr_channels = 4; - hdr_meta_msg.hdr_exposure_us.resize(num_hdr_channels); - hdr_meta_msg.hdr_gain.resize(num_hdr_channels); - - for (int hdr_channel = 0; hdr_channel < num_hdr_channels; hdr_channel++) { - hdr_meta_msg.hdr_exposure_us[hdr_channel] = - currentHdrExposure(hdr_channel); - hdr_meta_msg.hdr_gain[hdr_channel] = currentHdrGain(hdr_channel); - } - - hdr_metadata_pub_.publish(hdr_meta_msg); - } -} - -void ArenaCameraStreamingNodelet::reconfigureCallback(ArenaCameraConfig &config, - uint32_t level) { - stopStreaming(); - - ArenaCameraNodeletBase::reconfigureCallback(config, level); - - startStreaming(); -} - -} // namespace arena_camera - -PLUGINLIB_EXPORT_CLASS(arena_camera::ArenaCameraStreamingNodelet, - nodelet::Nodelet)