diff --git a/stereo_image_proc/CMakeLists.txt b/stereo_image_proc/CMakeLists.txt index aed4a6d1d..3c0d11cf9 100644 --- a/stereo_image_proc/CMakeLists.txt +++ b/stereo_image_proc/CMakeLists.txt @@ -1,57 +1,110 @@ -cmake_minimum_required(VERSION 2.8) +cmake_minimum_required(VERSION 3.5) project(stereo_image_proc) -find_package(catkin REQUIRED cv_bridge dynamic_reconfigure image_geometry image_proc image_transport message_filters nodelet sensor_msgs stereo_msgs) -find_package(Boost REQUIRED COMPONENTS thread) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() -if(cv_bridge_VERSION VERSION_GREATER "1.12.0") - add_compile_options(-std=c++11) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) endif() -# Dynamic reconfigure support -generate_dynamic_reconfigure_options(cfg/Disparity.cfg) +find_package(cv_bridge REQUIRED) +find_package(image_geometry REQUIRED) +find_package(image_proc REQUIRED) +find_package(image_transport REQUIRED) +find_package(message_filters REQUIRED) +find_package(OpenCV REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(stereo_msgs REQUIRED) -catkin_package( - CATKIN_DEPENDS image_geometry image_proc sensor_msgs stereo_msgs - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} +include_directories( + include + ${OpenCV_INCLUDE_DIRS} ) -include_directories(include) - -find_package(OpenCV REQUIRED) -include_directories(${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS}) - # See note in image_proc/CMakeLists.txt -add_definitions(-DOPENCV_TRAITS_ENABLE_DEPRECATED) +# add_definitions(-DOPENCV_TRAITS_ENABLE_DEPRECATED) -# Nodelet library -add_library(${PROJECT_NAME} src/libstereo_image_proc/processor.cpp src/nodelets/disparity.cpp src/nodelets/point_cloud2.cpp) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} - ${OpenCV_LIBRARIES} +add_library(${PROJECT_NAME} SHARED + src/${PROJECT_NAME}/stereo_processor.cpp + src/${PROJECT_NAME}/disparity_node.cpp + src/${PROJECT_NAME}/point_cloud_node.cpp ) -add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) -install(TARGETS ${PROJECT_NAME} - DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +target_link_libraries(${PROJECT_NAME} + ${OpenCV_LIBRARIES} ) -install(FILES nodelet_plugins.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +ament_target_dependencies(${PROJECT_NAME} + cv_bridge + image_geometry + image_proc + image_transport + message_filters + rclcpp + rclcpp_components + sensor_msgs + stereo_msgs ) -# Standalone node -add_executable(stereoimageproc_exe src/nodes/stereo_image_proc.cpp) -target_link_libraries(stereoimageproc_exe stereo_image_proc) -SET_TARGET_PROPERTIES(stereoimageproc_exe PROPERTIES OUTPUT_NAME stereo_image_proc) -install(TARGETS stereoimageproc_exe - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "stereo_image_proc::DisparityNode" + EXECUTABLE disparity_node +) +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "stereo_image_proc::PointCloudNode" + EXECUTABLE point_cloud_node +) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) -# install the launch file install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/ + DESTINATION share/${PROJECT_NAME} ) -# install the include directory -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY include/ + DESTINATION include ) + +ament_export_include_directories(include) +ament_export_libraries(${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_cmake_pytest REQUIRED) + find_package(launch_testing_ament_cmake REQUIRED) + find_package(ament_lint_auto REQUIRED) + find_package(python_cmake_module REQUIRED) + + ament_lint_auto_find_test_dependencies() + + set(_PYTHON_EXECUTABLE "${PYTHON_EXECUTABLE}") + if(WIN32 AND CMAKE_BUILD_TYPE STREQUAL "Debug") + set(PYTHON_EXECUTABLE "${PYTHON_EXECUTABLE_DEBUG}") + endif() + + # Test DisparityNode in launch test + add_launch_test("test/test_disparity_node.py" + PYTHON_EXECUTABLE "${PYTHON_EXECUTABLE}" + ) + # TODO(jacobperron): As of Eloquent, they're just plain pytests + # ament_add_pytest_test("test_disparity_node" test/test_disparity_node.py + # PYTHON_EXECUTABLE "${PYTHON_EXECUTABLE}" + # ) + + # Test PointCloudNode in launch test + add_launch_test("test/test_point_cloud_node.py" + PYTHON_EXECUTABLE "${PYTHON_EXECUTABLE}" + ) + # ament_add_pytest_test("test_point_cloud_node" test/test_point_cloud_node.py + # PYTHON_EXECUTABLE "${PYTHON_EXECUTABLE}" + # ) + + set(PYTHON_EXECUTABLE "${_PYTHON_EXECUTABLE}") +endif() + +ament_package() diff --git a/stereo_image_proc/COLCON_IGNORE b/stereo_image_proc/COLCON_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/stereo_image_proc/include/stereo_image_proc/processor.h b/stereo_image_proc/include/stereo_image_proc/processor.h deleted file mode 100644 index 22abdd4b2..000000000 --- a/stereo_image_proc/include/stereo_image_proc/processor.h +++ /dev/null @@ -1,299 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* 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 name of the Willow Garage 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. -*********************************************************************/ -#ifndef STEREO_IMAGE_PROC_PROCESSOR_H -#define STEREO_IMAGE_PROC_PROCESSOR_H - -#include -#include -#include -#include -#include - -namespace stereo_image_proc { - -struct StereoImageSet -{ - image_proc::ImageSet left; - image_proc::ImageSet right; - stereo_msgs::DisparityImage disparity; - sensor_msgs::PointCloud points; - sensor_msgs::PointCloud2 points2; -}; - -class StereoProcessor -{ -public: - - StereoProcessor() -#if CV_MAJOR_VERSION == 3 - { - block_matcher_ = cv::StereoBM::create(); - sg_block_matcher_ = cv::StereoSGBM::create(1, 1, 10); -#else - : block_matcher_(cv::StereoBM::BASIC_PRESET), - sg_block_matcher_() - { -#endif - } - - enum StereoType - { - BM, SGBM - }; - - enum { - LEFT_MONO = 1 << 0, - LEFT_RECT = 1 << 1, - LEFT_COLOR = 1 << 2, - LEFT_RECT_COLOR = 1 << 3, - RIGHT_MONO = 1 << 4, - RIGHT_RECT = 1 << 5, - RIGHT_COLOR = 1 << 6, - RIGHT_RECT_COLOR = 1 << 7, - DISPARITY = 1 << 8, - POINT_CLOUD = 1 << 9, - POINT_CLOUD2 = 1 << 10, - - LEFT_ALL = LEFT_MONO | LEFT_RECT | LEFT_COLOR | LEFT_RECT_COLOR, - RIGHT_ALL = RIGHT_MONO | RIGHT_RECT | RIGHT_COLOR | RIGHT_RECT_COLOR, - STEREO_ALL = DISPARITY | POINT_CLOUD | POINT_CLOUD2, - ALL = LEFT_ALL | RIGHT_ALL | STEREO_ALL - }; - - inline - StereoType getStereoType() const {return current_stereo_algorithm_;} - inline - void setStereoType(StereoType type) {current_stereo_algorithm_ = type;} - - int getInterpolation() const; - void setInterpolation(int interp); - - // Disparity pre-filtering parameters - - int getPreFilterSize() const; - void setPreFilterSize(int size); - - int getPreFilterCap() const; - void setPreFilterCap(int cap); - - // Disparity correlation parameters - - int getCorrelationWindowSize() const; - void setCorrelationWindowSize(int size); - - int getMinDisparity() const; - void setMinDisparity(int min_d); - - int getDisparityRange() const; - void setDisparityRange(int range); // Number of pixels to search - - // Disparity post-filtering parameters - - int getTextureThreshold() const; - void setTextureThreshold(int threshold); - - float getUniquenessRatio() const; - void setUniquenessRatio(float ratio); - - int getSpeckleSize() const; - void setSpeckleSize(int size); - - int getSpeckleRange() const; - void setSpeckleRange(int range); - - // SGBM only - int getSgbmMode() const; - void setSgbmMode(int fullDP); - - int getP1() const; - void setP1(int P1); - - int getP2() const; - void setP2(int P2); - - int getDisp12MaxDiff() const; - void setDisp12MaxDiff(int disp12MaxDiff); - - // Do all the work! - bool process(const sensor_msgs::ImageConstPtr& left_raw, - const sensor_msgs::ImageConstPtr& right_raw, - const image_geometry::StereoCameraModel& model, - StereoImageSet& output, int flags) const; - - void processDisparity(const cv::Mat& left_rect, const cv::Mat& right_rect, - const image_geometry::StereoCameraModel& model, - stereo_msgs::DisparityImage& disparity) const; - - void processPoints(const stereo_msgs::DisparityImage& disparity, - const cv::Mat& color, const std::string& encoding, - const image_geometry::StereoCameraModel& model, - sensor_msgs::PointCloud& points) const; - void processPoints2(const stereo_msgs::DisparityImage& disparity, - const cv::Mat& color, const std::string& encoding, - const image_geometry::StereoCameraModel& model, - sensor_msgs::PointCloud2& points) const; - -private: - image_proc::Processor mono_processor_; - - mutable cv::Mat_ disparity16_; // scratch buffer for 16-bit signed disparity image -#if CV_MAJOR_VERSION == 3 - mutable cv::Ptr block_matcher_; // contains scratch buffers for block matching - mutable cv::Ptr sg_block_matcher_; -#else - mutable cv::StereoBM block_matcher_; // contains scratch buffers for block matching - mutable cv::StereoSGBM sg_block_matcher_; -#endif - StereoType current_stereo_algorithm_; - // scratch buffer for dense point cloud - mutable cv::Mat_ dense_points_; -}; - - -inline int StereoProcessor::getInterpolation() const -{ - return mono_processor_.interpolation_; -} - -inline void StereoProcessor::setInterpolation(int interp) -{ - mono_processor_.interpolation_ = interp; -} - -// For once, a macro is used just to avoid errors -#define STEREO_IMAGE_PROC_OPENCV2(GET, SET, TYPE, PARAM) \ -inline TYPE StereoProcessor::GET() const \ -{ \ - if (current_stereo_algorithm_ == BM) \ - return block_matcher_.state->PARAM; \ - return sg_block_matcher_.PARAM; \ -} \ - \ -inline void StereoProcessor::SET(TYPE param) \ -{ \ - block_matcher_.state->PARAM = param; \ - sg_block_matcher_.PARAM = param; \ -} - -#define STEREO_IMAGE_PROC_OPENCV3(GET, SET, TYPE, GET_OPENCV, SET_OPENCV) \ -inline TYPE StereoProcessor::GET() const \ -{ \ - if (current_stereo_algorithm_ == BM) \ - return block_matcher_->GET_OPENCV(); \ - return sg_block_matcher_->GET_OPENCV(); \ -} \ -\ -inline void StereoProcessor::SET(TYPE param) \ -{ \ - block_matcher_->SET_OPENCV(param); \ - sg_block_matcher_->SET_OPENCV(param); \ -} - -#if CV_MAJOR_VERSION == 3 -STEREO_IMAGE_PROC_OPENCV3(getPreFilterCap, setPreFilterCap, int, getPreFilterCap, setPreFilterCap) -STEREO_IMAGE_PROC_OPENCV3(getCorrelationWindowSize, setCorrelationWindowSize, int, getBlockSize, setBlockSize) -STEREO_IMAGE_PROC_OPENCV3(getMinDisparity, setMinDisparity, int, getMinDisparity, setMinDisparity) -STEREO_IMAGE_PROC_OPENCV3(getDisparityRange, setDisparityRange, int, getNumDisparities, setNumDisparities) -STEREO_IMAGE_PROC_OPENCV3(getUniquenessRatio, setUniquenessRatio, float, getUniquenessRatio, setUniquenessRatio) -STEREO_IMAGE_PROC_OPENCV3(getSpeckleSize, setSpeckleSize, int, getSpeckleWindowSize, setSpeckleWindowSize) -STEREO_IMAGE_PROC_OPENCV3(getSpeckleRange, setSpeckleRange, int, getSpeckleRange, setSpeckleRange) -#else -STEREO_IMAGE_PROC_OPENCV2(getPreFilterCap, setPreFilterCap, int, preFilterCap) -STEREO_IMAGE_PROC_OPENCV2(getCorrelationWindowSize, setCorrelationWindowSize, int, SADWindowSize) -STEREO_IMAGE_PROC_OPENCV2(getMinDisparity, setMinDisparity, int, minDisparity) -STEREO_IMAGE_PROC_OPENCV2(getDisparityRange, setDisparityRange, int, numberOfDisparities) -STEREO_IMAGE_PROC_OPENCV2(getUniquenessRatio, setUniquenessRatio, float, uniquenessRatio) -STEREO_IMAGE_PROC_OPENCV2(getSpeckleSize, setSpeckleSize, int, speckleWindowSize) -STEREO_IMAGE_PROC_OPENCV2(getSpeckleRange, setSpeckleRange, int, speckleRange) -#endif - -#define STEREO_IMAGE_PROC_BM_ONLY_OPENCV2(GET, SET, TYPE, PARAM) \ -inline TYPE StereoProcessor::GET() const \ -{ \ - return block_matcher_.state->PARAM; \ -} \ -\ -inline void StereoProcessor::SET(TYPE param) \ -{ \ - block_matcher_.state->PARAM = param; \ -} - -#define STEREO_IMAGE_PROC_SGBM_ONLY_OPENCV2(GET, SET, TYPE, PARAM) \ -inline TYPE StereoProcessor::GET() const \ -{ \ - return sg_block_matcher_.PARAM; \ -} \ -\ -inline void StereoProcessor::SET(TYPE param) \ -{ \ - sg_block_matcher_.PARAM = param; \ -} - -#define STEREO_IMAGE_PROC_ONLY_OPENCV3(MEMBER, GET, SET, TYPE, GET_OPENCV, SET_OPENCV) \ -inline TYPE StereoProcessor::GET() const \ -{ \ - return MEMBER->GET_OPENCV(); \ -} \ -\ -inline void StereoProcessor::SET(TYPE param) \ -{ \ - MEMBER->SET_OPENCV(param); \ -} - -// BM only -#if CV_MAJOR_VERSION == 3 -STEREO_IMAGE_PROC_ONLY_OPENCV3(block_matcher_, getPreFilterSize, setPreFilterSize, int, getPreFilterSize, setPreFilterSize) -STEREO_IMAGE_PROC_ONLY_OPENCV3(block_matcher_, getTextureThreshold, setTextureThreshold, int, getTextureThreshold, setTextureThreshold) -#else -STEREO_IMAGE_PROC_BM_ONLY_OPENCV2(getPreFilterSize, setPreFilterSize, int, preFilterSize) -STEREO_IMAGE_PROC_BM_ONLY_OPENCV2(getTextureThreshold, setTextureThreshold, int, textureThreshold) -#endif - -// SGBM specific -#if CV_MAJOR_VERSION == 3 -// getSgbmMode can return MODE_SGBM = 0, MODE_HH = 1. FullDP == 1 was MODE_HH so we're good -STEREO_IMAGE_PROC_ONLY_OPENCV3(sg_block_matcher_, getSgbmMode, setSgbmMode, int, getMode, setMode) -STEREO_IMAGE_PROC_ONLY_OPENCV3(sg_block_matcher_, getP1, setP1, int, getP1, setP1) -STEREO_IMAGE_PROC_ONLY_OPENCV3(sg_block_matcher_, getP2, setP2, int, getP2, setP2) -STEREO_IMAGE_PROC_ONLY_OPENCV3(sg_block_matcher_, getDisp12MaxDiff, setDisp12MaxDiff, int, getDisp12MaxDiff, setDisp12MaxDiff) -#else -STEREO_IMAGE_PROC_SGBM_ONLY_OPENCV2(getSgbmMode, setSgbmMode, int, fullDP) -STEREO_IMAGE_PROC_SGBM_ONLY_OPENCV2(getP1, setP1, int, P1) -STEREO_IMAGE_PROC_SGBM_ONLY_OPENCV2(getP2, setP2, int, P2) -STEREO_IMAGE_PROC_SGBM_ONLY_OPENCV2(getDisp12MaxDiff, setDisp12MaxDiff, int, disp12MaxDiff) -#endif - -} //namespace stereo_image_proc - -#endif diff --git a/stereo_image_proc/include/stereo_image_proc/stereo_processor.hpp b/stereo_image_proc/include/stereo_image_proc/stereo_processor.hpp new file mode 100644 index 000000000..3cef016bf --- /dev/null +++ b/stereo_image_proc/include/stereo_image_proc/stereo_processor.hpp @@ -0,0 +1,314 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT 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. +#ifndef STEREO_IMAGE_PROC__STEREO_PROCESSOR_HPP_ +#define STEREO_IMAGE_PROC__STEREO_PROCESSOR_HPP_ + +#include +#include +#include +#include +#include + +#include + +namespace stereo_image_proc +{ + +struct StereoImageSet +{ + image_proc::ImageSet left; + image_proc::ImageSet right; + stereo_msgs::msg::DisparityImage disparity; + sensor_msgs::msg::PointCloud points; + sensor_msgs::msg::PointCloud2 points2; +}; + +class StereoProcessor +{ +public: + StereoProcessor() + { + block_matcher_ = cv::StereoBM::create(); + sg_block_matcher_ = cv::StereoSGBM::create(1, 1, 10); + } + + enum StereoType + { + BM, SGBM + }; + + enum + { + LEFT_MONO = 1 << 0, + LEFT_RECT = 1 << 1, + LEFT_COLOR = 1 << 2, + LEFT_RECT_COLOR = 1 << 3, + RIGHT_MONO = 1 << 4, + RIGHT_RECT = 1 << 5, + RIGHT_COLOR = 1 << 6, + RIGHT_RECT_COLOR = 1 << 7, + DISPARITY = 1 << 8, + POINT_CLOUD = 1 << 9, + POINT_CLOUD2 = 1 << 10, + LEFT_ALL = LEFT_MONO | LEFT_RECT | LEFT_COLOR | LEFT_RECT_COLOR, + RIGHT_ALL = RIGHT_MONO | RIGHT_RECT | RIGHT_COLOR | RIGHT_RECT_COLOR, + STEREO_ALL = DISPARITY | POINT_CLOUD | POINT_CLOUD2, + ALL = LEFT_ALL | RIGHT_ALL | STEREO_ALL + }; + + inline StereoType getStereoType() const + { + return current_stereo_algorithm_; + } + + inline void setStereoType(StereoType type) + { + current_stereo_algorithm_ = type; + } + + inline int getInterpolation() const + { + return mono_processor_.interpolation_; + } + + inline void setInterpolation(int interp) + { + mono_processor_.interpolation_ = interp; + } + + inline int getPreFilterCap() const + { + if (current_stereo_algorithm_ == BM) { + return block_matcher_->getPreFilterCap(); + } + return sg_block_matcher_->getPreFilterCap(); + } + + inline void setPreFilterCap(int param) + { + block_matcher_->setPreFilterCap(param); + sg_block_matcher_->setPreFilterCap(param); + } + + inline int getCorrelationWindowSize() const + { + if (current_stereo_algorithm_ == BM) { + return block_matcher_->getBlockSize(); + } + return sg_block_matcher_->getBlockSize(); + } + + inline void setCorrelationWindowSize(int param) + { + block_matcher_->setBlockSize(param); + sg_block_matcher_->setBlockSize(param); + } + + inline int getMinDisparity() const + { + if (current_stereo_algorithm_ == BM) { + return block_matcher_->getMinDisparity(); + } + return sg_block_matcher_->getMinDisparity(); + } + + inline void setMinDisparity(int param) + { + block_matcher_->setMinDisparity(param); + sg_block_matcher_->setMinDisparity(param); + } + + inline int getDisparityRange() const + { + if (current_stereo_algorithm_ == BM) { + return block_matcher_->getNumDisparities(); + } + return sg_block_matcher_->getNumDisparities(); + } + + inline void setDisparityRange(int param) + { + block_matcher_->setNumDisparities(param); + sg_block_matcher_->setNumDisparities(param); + } + + inline float getUniquenessRatio() const + { + if (current_stereo_algorithm_ == BM) { + return block_matcher_->getUniquenessRatio(); + } + return sg_block_matcher_->getUniquenessRatio(); + } + + inline void setUniquenessRatio(float param) + { + block_matcher_->setUniquenessRatio(param); + sg_block_matcher_->setUniquenessRatio(param); + } + + inline int getSpeckleSize() const + { + if (current_stereo_algorithm_ == BM) { + return block_matcher_->getBlockSize(); + } + return sg_block_matcher_->getBlockSize(); + } + + inline void setSpeckleSize(int param) + { + block_matcher_->setBlockSize(param); + sg_block_matcher_->setBlockSize(param); + } + + inline int getSpeckleRange() const + { + if (current_stereo_algorithm_ == BM) { + return block_matcher_->getSpeckleRange(); + } + return sg_block_matcher_->getSpeckleRange(); + } + + inline void setSpeckleRange(int param) + { + block_matcher_->setSpeckleRange(param); + sg_block_matcher_->setSpeckleRange(param); + } + + // BM only + + inline int getPreFilterSize() const + { + return block_matcher_->getPreFilterSize(); + } + + inline void setPreFilterSize(int param) + { + block_matcher_->setPreFilterSize(param); + } + + inline int getTextureThreshold() const + { + return block_matcher_->getTextureThreshold(); + } + + inline void setTextureThreshold(int param) + { + block_matcher_->setTextureThreshold(param); + } + + // SGBM specific + + // getSgbmMode can return MODE_SGBM = 0, MODE_HH = 1. FullDP == 1 was MODE_HH so we're good + inline int getSgbmMode() const + { + return sg_block_matcher_->getMode(); + } + + inline void setSgbmMode(int param) + { + sg_block_matcher_->setMode(param); + } + + inline int getP1() const + { + return sg_block_matcher_->getP1(); + } + + inline void setP1(int param) + { + sg_block_matcher_->setP1(param); + } + + inline int getP2() const + { + return sg_block_matcher_->getP2(); + } + + inline void setP2(int param) + { + sg_block_matcher_->setP2(param); + } + + inline int getDisp12MaxDiff() const + { + return sg_block_matcher_->getDisp12MaxDiff(); + } + + inline void setDisp12MaxDiff(int param) + { + sg_block_matcher_->setDisp12MaxDiff(param); + } + + // Do all the work! + bool process( + const sensor_msgs::msg::Image::ConstSharedPtr & left_raw, + const sensor_msgs::msg::Image::ConstSharedPtr & right_raw, + const image_geometry::StereoCameraModel & model, + StereoImageSet & output, + int flags) const; + + void processDisparity( + const cv::Mat & left_rect, + const cv::Mat & right_rect, + const image_geometry::StereoCameraModel & model, + stereo_msgs::msg::DisparityImage & disparity) const; + + void processPoints( + const stereo_msgs::msg::DisparityImage & disparity, + const cv::Mat & color, + const std::string & encoding, + const image_geometry::StereoCameraModel & model, + sensor_msgs::msg::PointCloud & points) const; + + void processPoints2( + const stereo_msgs::msg::DisparityImage & disparity, + const cv::Mat & color, + const std::string & encoding, + const image_geometry::StereoCameraModel & model, + sensor_msgs::msg::PointCloud2 & points) const; + +private: + image_proc::Processor mono_processor_; + + /// Scratch buffer for 16-bit signed disparity image + mutable cv::Mat_ disparity16_; + /// Contains scratch buffers for block matching. + mutable cv::Ptr block_matcher_; + mutable cv::Ptr sg_block_matcher_; + StereoType current_stereo_algorithm_; + /// Scratch buffer for dense point cloud. + mutable cv::Mat_ dense_points_; +}; + +} // namespace stereo_image_proc + +#endif // STEREO_IMAGE_PROC__STEREO_PROCESSOR_HPP_ diff --git a/stereo_image_proc/launch/stereo_image_proc.launch b/stereo_image_proc/launch/stereo_image_proc.launch deleted file mode 100644 index c82316fd3..000000000 --- a/stereo_image_proc/launch/stereo_image_proc.launch +++ /dev/null @@ -1,40 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/stereo_image_proc/launch/stereo_image_proc.launch.py b/stereo_image_proc/launch/stereo_image_proc.launch.py new file mode 100644 index 000000000..71d67dc33 --- /dev/null +++ b/stereo_image_proc/launch/stereo_image_proc.launch.py @@ -0,0 +1,64 @@ +# Copyright (c) 2019, Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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 name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT 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. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + # TODO(jacobperron): Include image_proc launch file when it exists + return LaunchDescription([ + DeclareLaunchArgument( + name='approximate_sync', default_value='False', + description='Whether to use approximate synchronization of topics. Set to true if ' + 'the left and right cameras do not produce exactly synced timestamps.' + ), + ComposableNodeContainer( + package='rclcpp_components', node_executable='component_container', + node_name='stereo_image_proc_container', node_namespace='', + composable_node_descriptions=[ + ComposableNode( + package='stereo_image_proc', + node_plugin='stereo_image_proc::DisparityNode', + parameters=[{'approximate_sync': LaunchConfiguration('approximate_sync')}] + ), + ComposableNode( + package='stereo_image_proc', + node_plugin='stereo_image_proc::PointCloudNode', + parameters=[{'approximate_sync': LaunchConfiguration('approximate_sync')}] + ), + ], + ), + ]) diff --git a/stereo_image_proc/nodelet_plugins.xml b/stereo_image_proc/nodelet_plugins.xml deleted file mode 100644 index 3512efc4e..000000000 --- a/stereo_image_proc/nodelet_plugins.xml +++ /dev/null @@ -1,11 +0,0 @@ - - - - Nodelet to perform stereo processing on a pair of rectified image streams, producing disparity images - - - - Nodelet to produce XYZRGB PointCloud2 messages - - - diff --git a/stereo_image_proc/package.xml b/stereo_image_proc/package.xml index 871b05b1b..b46f28083 100644 --- a/stereo_image_proc/package.xml +++ b/stereo_image_proc/package.xml @@ -1,6 +1,6 @@ - + stereo_image_proc - 1.12.23 + 2.0.0 Stereo and single image rectification and disparity processing. Patrick Mihelich Kurt Konolige @@ -11,32 +11,30 @@ BSD http://www.ros.org/wiki/stereo_image_proc - - - - + ament_cmake - catkin + cv_bridge + image_geometry + image_proc + image_transport + message_filters + rclcpp + rclcpp_components + sensor_msgs + stereo_msgs - rostest - - cv_bridge - dynamic_reconfigure - image_geometry - image_proc - image_transport - message_filters - nodelet - sensor_msgs - stereo_msgs + ament_cmake_pytest + ament_lint_auto + ament_lint_common + launch + launch_ros + launch_testing + launch_testing_ament_cmake + python_cmake_module + python3-opencv + rclpy - cv_bridge - dynamic_reconfigure - image_geometry - image_proc - image_transport - message_filters - nodelet - sensor_msgs - stereo_msgs + + ament_cmake + diff --git a/stereo_image_proc/rosdoc.yaml b/stereo_image_proc/rosdoc.yaml deleted file mode 100644 index 976fdf0a6..000000000 --- a/stereo_image_proc/rosdoc.yaml +++ /dev/null @@ -1,4 +0,0 @@ - - builder: doxygen - name: C++ API - output_dir: c++ - file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox' diff --git a/stereo_image_proc/src/libstereo_image_proc/processor.cpp b/stereo_image_proc/src/libstereo_image_proc/processor.cpp deleted file mode 100644 index cecbf9728..000000000 --- a/stereo_image_proc/src/libstereo_image_proc/processor.cpp +++ /dev/null @@ -1,317 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* 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 name of the Willow Garage 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 "stereo_image_proc/processor.h" -#include -#include -#include - -namespace stereo_image_proc { - -bool StereoProcessor::process(const sensor_msgs::ImageConstPtr& left_raw, - const sensor_msgs::ImageConstPtr& right_raw, - const image_geometry::StereoCameraModel& model, - StereoImageSet& output, int flags) const -{ - // Do monocular processing on left and right images - int left_flags = flags & LEFT_ALL; - int right_flags = flags & RIGHT_ALL; - if (flags & STEREO_ALL) { - // Need the rectified images for stereo processing - left_flags |= LEFT_RECT; - right_flags |= RIGHT_RECT; - } - if (flags & (POINT_CLOUD | POINT_CLOUD2)) { - flags |= DISPARITY; - // Need the color channels for the point cloud - left_flags |= LEFT_RECT_COLOR; - } - if (!mono_processor_.process(left_raw, model.left(), output.left, left_flags)) - return false; - if (!mono_processor_.process(right_raw, model.right(), output.right, right_flags >> 4)) - return false; - - // Do block matching to produce the disparity image - if (flags & DISPARITY) { - processDisparity(output.left.rect, output.right.rect, model, output.disparity); - } - - // Project disparity image to 3d point cloud - if (flags & POINT_CLOUD) { - processPoints(output.disparity, output.left.rect_color, output.left.color_encoding, model, output.points); - } - - // Project disparity image to 3d point cloud - if (flags & POINT_CLOUD2) { - processPoints2(output.disparity, output.left.rect_color, output.left.color_encoding, model, output.points2); - } - - return true; -} - -void StereoProcessor::processDisparity(const cv::Mat& left_rect, const cv::Mat& right_rect, - const image_geometry::StereoCameraModel& model, - stereo_msgs::DisparityImage& disparity) const -{ - // Fixed-point disparity is 16 times the true value: d = d_fp / 16.0 = x_l - x_r. - static const int DPP = 16; // disparities per pixel - static const double inv_dpp = 1.0 / DPP; - - // Block matcher produces 16-bit signed (fixed point) disparity image - if (current_stereo_algorithm_ == BM) -#if CV_MAJOR_VERSION == 3 - block_matcher_->compute(left_rect, right_rect, disparity16_); - else - sg_block_matcher_->compute(left_rect, right_rect, disparity16_); -#else - block_matcher_(left_rect, right_rect, disparity16_); - else - sg_block_matcher_(left_rect, right_rect, disparity16_); -#endif - - // Fill in DisparityImage image data, converting to 32-bit float - sensor_msgs::Image& dimage = disparity.image; - dimage.height = disparity16_.rows; - dimage.width = disparity16_.cols; - dimage.encoding = sensor_msgs::image_encodings::TYPE_32FC1; - dimage.step = dimage.width * sizeof(float); - dimage.data.resize(dimage.step * dimage.height); - cv::Mat_ dmat(dimage.height, dimage.width, (float*)&dimage.data[0], dimage.step); - // We convert from fixed-point to float disparity and also adjust for any x-offset between - // the principal points: d = d_fp*inv_dpp - (cx_l - cx_r) - disparity16_.convertTo(dmat, dmat.type(), inv_dpp, -(model.left().cx() - model.right().cx())); - ROS_ASSERT(dmat.data == &dimage.data[0]); - /// @todo is_bigendian? :) - - // Stereo parameters - disparity.f = model.right().fx(); - disparity.T = model.baseline(); - - /// @todo Window of (potentially) valid disparities - - // Disparity search range - disparity.min_disparity = getMinDisparity(); - disparity.max_disparity = getMinDisparity() + getDisparityRange() - 1; - disparity.delta_d = inv_dpp; -} - -inline bool isValidPoint(const cv::Vec3f& pt) -{ - // Check both for disparities explicitly marked as invalid (where OpenCV maps pt.z to MISSING_Z) - // and zero disparities (point mapped to infinity). - return pt[2] != image_geometry::StereoCameraModel::MISSING_Z && !std::isinf(pt[2]); -} - -void StereoProcessor::processPoints(const stereo_msgs::DisparityImage& disparity, - const cv::Mat& color, const std::string& encoding, - const image_geometry::StereoCameraModel& model, - sensor_msgs::PointCloud& points) const -{ - // Calculate dense point cloud - const sensor_msgs::Image& dimage = disparity.image; - const cv::Mat_ dmat(dimage.height, dimage.width, (float*)&dimage.data[0], dimage.step); - model.projectDisparityImageTo3d(dmat, dense_points_, true); - - // Fill in sparse point cloud message - points.points.resize(0); - points.channels.resize(3); - points.channels[0].name = "rgb"; - points.channels[0].values.resize(0); - points.channels[1].name = "u"; - points.channels[1].values.resize(0); - points.channels[2].name = "v"; - points.channels[2].values.resize(0); - - for (int32_t u = 0; u < dense_points_.rows; ++u) { - for (int32_t v = 0; v < dense_points_.cols; ++v) { - if (isValidPoint(dense_points_(u,v))) { - // x,y,z - geometry_msgs::Point32 pt; - pt.x = dense_points_(u,v)[0]; - pt.y = dense_points_(u,v)[1]; - pt.z = dense_points_(u,v)[2]; - points.points.push_back(pt); - // u,v - points.channels[1].values.push_back(u); - points.channels[2].values.push_back(v); - } - } - } - - // Fill in color - namespace enc = sensor_msgs::image_encodings; - points.channels[0].values.reserve(points.points.size()); - if (encoding == enc::MONO8) { - for (int32_t u = 0; u < dense_points_.rows; ++u) { - for (int32_t v = 0; v < dense_points_.cols; ++v) { - if (isValidPoint(dense_points_(u,v))) { - uint8_t g = color.at(u,v); - int32_t rgb = (g << 16) | (g << 8) | g; - points.channels[0].values.push_back(*(float*)(&rgb)); - } - } - } - } - else if (encoding == enc::RGB8) { - for (int32_t u = 0; u < dense_points_.rows; ++u) { - for (int32_t v = 0; v < dense_points_.cols; ++v) { - if (isValidPoint(dense_points_(u,v))) { - const cv::Vec3b& rgb = color.at(u,v); - int32_t rgb_packed = (rgb[0] << 16) | (rgb[1] << 8) | rgb[2]; - points.channels[0].values.push_back(*(float*)(&rgb_packed)); - } - } - } - } - else if (encoding == enc::BGR8) { - for (int32_t u = 0; u < dense_points_.rows; ++u) { - for (int32_t v = 0; v < dense_points_.cols; ++v) { - if (isValidPoint(dense_points_(u,v))) { - const cv::Vec3b& bgr = color.at(u,v); - int32_t rgb_packed = (bgr[2] << 16) | (bgr[1] << 8) | bgr[0]; - points.channels[0].values.push_back(*(float*)(&rgb_packed)); - } - } - } - } - else { - ROS_WARN("Could not fill color channel of the point cloud, unrecognized encoding '%s'", encoding.c_str()); - } -} - -void StereoProcessor::processPoints2(const stereo_msgs::DisparityImage& disparity, - const cv::Mat& color, const std::string& encoding, - const image_geometry::StereoCameraModel& model, - sensor_msgs::PointCloud2& points) const -{ - // Calculate dense point cloud - const sensor_msgs::Image& dimage = disparity.image; - const cv::Mat_ dmat(dimage.height, dimage.width, (float*)&dimage.data[0], dimage.step); - model.projectDisparityImageTo3d(dmat, dense_points_, true); - - // Fill in sparse point cloud message - points.height = dense_points_.rows; - points.width = dense_points_.cols; - points.fields.resize (4); - points.fields[0].name = "x"; - points.fields[0].offset = 0; - points.fields[0].count = 1; - points.fields[0].datatype = sensor_msgs::PointField::FLOAT32; - points.fields[1].name = "y"; - points.fields[1].offset = 4; - points.fields[1].count = 1; - points.fields[1].datatype = sensor_msgs::PointField::FLOAT32; - points.fields[2].name = "z"; - points.fields[2].offset = 8; - points.fields[2].count = 1; - points.fields[2].datatype = sensor_msgs::PointField::FLOAT32; - points.fields[3].name = "rgb"; - points.fields[3].offset = 12; - points.fields[3].count = 1; - points.fields[3].datatype = sensor_msgs::PointField::FLOAT32; - //points.is_bigendian = false; ??? - points.point_step = 16; - points.row_step = points.point_step * points.width; - points.data.resize (points.row_step * points.height); - points.is_dense = false; // there may be invalid points - - float bad_point = std::numeric_limits::quiet_NaN (); - int i = 0; - for (int32_t u = 0; u < dense_points_.rows; ++u) { - for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) { - if (isValidPoint(dense_points_(u,v))) { - // x,y,z,rgba - memcpy (&points.data[i * points.point_step + 0], &dense_points_(u,v)[0], sizeof (float)); - memcpy (&points.data[i * points.point_step + 4], &dense_points_(u,v)[1], sizeof (float)); - memcpy (&points.data[i * points.point_step + 8], &dense_points_(u,v)[2], sizeof (float)); - } - else { - memcpy (&points.data[i * points.point_step + 0], &bad_point, sizeof (float)); - memcpy (&points.data[i * points.point_step + 4], &bad_point, sizeof (float)); - memcpy (&points.data[i * points.point_step + 8], &bad_point, sizeof (float)); - } - } - } - - // Fill in color - namespace enc = sensor_msgs::image_encodings; - i = 0; - if (encoding == enc::MONO8) { - for (int32_t u = 0; u < dense_points_.rows; ++u) { - for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) { - if (isValidPoint(dense_points_(u,v))) { - uint8_t g = color.at(u,v); - int32_t rgb = (g << 16) | (g << 8) | g; - memcpy (&points.data[i * points.point_step + 12], &rgb, sizeof (int32_t)); - } - else { - memcpy (&points.data[i * points.point_step + 12], &bad_point, sizeof (float)); - } - } - } - } - else if (encoding == enc::RGB8) { - for (int32_t u = 0; u < dense_points_.rows; ++u) { - for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) { - if (isValidPoint(dense_points_(u,v))) { - const cv::Vec3b& rgb = color.at(u,v); - int32_t rgb_packed = (rgb[0] << 16) | (rgb[1] << 8) | rgb[2]; - memcpy (&points.data[i * points.point_step + 12], &rgb_packed, sizeof (int32_t)); - } - else { - memcpy (&points.data[i * points.point_step + 12], &bad_point, sizeof (float)); - } - } - } - } - else if (encoding == enc::BGR8) { - for (int32_t u = 0; u < dense_points_.rows; ++u) { - for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) { - if (isValidPoint(dense_points_(u,v))) { - const cv::Vec3b& bgr = color.at(u,v); - int32_t rgb_packed = (bgr[2] << 16) | (bgr[1] << 8) | bgr[0]; - memcpy (&points.data[i * points.point_step + 12], &rgb_packed, sizeof (int32_t)); - } - else { - memcpy (&points.data[i * points.point_step + 12], &bad_point, sizeof (float)); - } - } - } - } - else { - ROS_WARN("Could not fill color channel of the point cloud, unrecognized encoding '%s'", encoding.c_str()); - } -} - -} //namespace stereo_image_proc diff --git a/stereo_image_proc/src/nodelets/disparity.cpp b/stereo_image_proc/src/nodelets/disparity.cpp deleted file mode 100644 index 1c39f9925..000000000 --- a/stereo_image_proc/src/nodelets/disparity.cpp +++ /dev/null @@ -1,250 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* 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 name of the Willow Garage 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 -#if ((BOOST_VERSION / 100) % 1000) >= 53 -#include -#endif - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -#include -#include - -#include - -namespace stereo_image_proc { - -using namespace sensor_msgs; -using namespace stereo_msgs; -using namespace message_filters::sync_policies; - -class DisparityNodelet : public nodelet::Nodelet -{ - boost::shared_ptr it_; - - // Subscriptions - image_transport::SubscriberFilter sub_l_image_, sub_r_image_; - message_filters::Subscriber sub_l_info_, sub_r_info_; - typedef ExactTime ExactPolicy; - typedef ApproximateTime ApproximatePolicy; - typedef message_filters::Synchronizer ExactSync; - typedef message_filters::Synchronizer ApproximateSync; - boost::shared_ptr exact_sync_; - boost::shared_ptr approximate_sync_; - // Publications - boost::mutex connect_mutex_; - ros::Publisher pub_disparity_; - - // Dynamic reconfigure - boost::recursive_mutex config_mutex_; - typedef stereo_image_proc::DisparityConfig Config; - typedef dynamic_reconfigure::Server ReconfigureServer; - boost::shared_ptr reconfigure_server_; - - // Processing state (note: only safe because we're single-threaded!) - image_geometry::StereoCameraModel model_; - stereo_image_proc::StereoProcessor block_matcher_; // contains scratch buffers for block matching - - virtual void onInit(); - - void connectCb(); - - void imageCb(const ImageConstPtr& l_image_msg, const CameraInfoConstPtr& l_info_msg, - const ImageConstPtr& r_image_msg, const CameraInfoConstPtr& r_info_msg); - - void configCb(Config &config, uint32_t level); -}; - -void DisparityNodelet::onInit() -{ - ros::NodeHandle &nh = getNodeHandle(); - ros::NodeHandle &private_nh = getPrivateNodeHandle(); - - it_.reset(new image_transport::ImageTransport(nh)); - - // Synchronize inputs. Topic subscriptions happen on demand in the connection - // callback. Optionally do approximate synchronization. - int queue_size; - private_nh.param("queue_size", queue_size, 5); - bool approx; - private_nh.param("approximate_sync", approx, false); - if (approx) - { - approximate_sync_.reset( new ApproximateSync(ApproximatePolicy(queue_size), - sub_l_image_, sub_l_info_, - sub_r_image_, sub_r_info_) ); - approximate_sync_->registerCallback(boost::bind(&DisparityNodelet::imageCb, - this, _1, _2, _3, _4)); - } - else - { - exact_sync_.reset( new ExactSync(ExactPolicy(queue_size), - sub_l_image_, sub_l_info_, - sub_r_image_, sub_r_info_) ); - exact_sync_->registerCallback(boost::bind(&DisparityNodelet::imageCb, - this, _1, _2, _3, _4)); - } - - // Set up dynamic reconfiguration - ReconfigureServer::CallbackType f = boost::bind(&DisparityNodelet::configCb, - this, _1, _2); - reconfigure_server_.reset(new ReconfigureServer(config_mutex_, private_nh)); - reconfigure_server_->setCallback(f); - - // Monitor whether anyone is subscribed to the output - ros::SubscriberStatusCallback connect_cb = boost::bind(&DisparityNodelet::connectCb, this); - // Make sure we don't enter connectCb() between advertising and assigning to pub_disparity_ - boost::lock_guard lock(connect_mutex_); - pub_disparity_ = nh.advertise("disparity", 1, connect_cb, connect_cb); -} - -// Handles (un)subscribing when clients (un)subscribe -void DisparityNodelet::connectCb() -{ - boost::lock_guard lock(connect_mutex_); - if (pub_disparity_.getNumSubscribers() == 0) - { - sub_l_image_.unsubscribe(); - sub_l_info_ .unsubscribe(); - sub_r_image_.unsubscribe(); - sub_r_info_ .unsubscribe(); - } - else if (!sub_l_image_.getSubscriber()) - { - ros::NodeHandle &nh = getNodeHandle(); - // Queue size 1 should be OK; the one that matters is the synchronizer queue size. - /// @todo Allow remapping left, right? - image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle()); - sub_l_image_.subscribe(*it_, "left/image_rect", 1, hints); - sub_l_info_ .subscribe(nh, "left/camera_info", 1); - sub_r_image_.subscribe(*it_, "right/image_rect", 1, hints); - sub_r_info_ .subscribe(nh, "right/camera_info", 1); - } -} - -void DisparityNodelet::imageCb(const ImageConstPtr& l_image_msg, - const CameraInfoConstPtr& l_info_msg, - const ImageConstPtr& r_image_msg, - const CameraInfoConstPtr& r_info_msg) -{ - // Update the camera model - model_.fromCameraInfo(l_info_msg, r_info_msg); - - // Allocate new disparity image message - DisparityImagePtr disp_msg = boost::make_shared(); - disp_msg->header = l_info_msg->header; - disp_msg->image.header = l_info_msg->header; - - // Compute window of (potentially) valid disparities - int border = block_matcher_.getCorrelationWindowSize() / 2; - int left = block_matcher_.getDisparityRange() + block_matcher_.getMinDisparity() + border - 1; - int wtf = (block_matcher_.getMinDisparity() >= 0) ? border + block_matcher_.getMinDisparity() : std::max(border, -block_matcher_.getMinDisparity()); - int right = disp_msg->image.width - 1 - wtf; - int top = border; - int bottom = disp_msg->image.height - 1 - border; - disp_msg->valid_window.x_offset = left; - disp_msg->valid_window.y_offset = top; - disp_msg->valid_window.width = right - left; - disp_msg->valid_window.height = bottom - top; - - // Create cv::Mat views onto all buffers - const cv::Mat_ l_image = cv_bridge::toCvShare(l_image_msg, sensor_msgs::image_encodings::MONO8)->image; - const cv::Mat_ r_image = cv_bridge::toCvShare(r_image_msg, sensor_msgs::image_encodings::MONO8)->image; - - // Perform block matching to find the disparities - block_matcher_.processDisparity(l_image, r_image, model_, *disp_msg); - - // Adjust for any x-offset between the principal points: d' = d - (cx_l - cx_r) - double cx_l = model_.left().cx(); - double cx_r = model_.right().cx(); - if (cx_l != cx_r) { - cv::Mat_ disp_image(disp_msg->image.height, disp_msg->image.width, - reinterpret_cast(&disp_msg->image.data[0]), - disp_msg->image.step); - cv::subtract(disp_image, cv::Scalar(cx_l - cx_r), disp_image); - } - - pub_disparity_.publish(disp_msg); -} - -void DisparityNodelet::configCb(Config &config, uint32_t level) -{ - // Tweak all settings to be valid - config.prefilter_size |= 0x1; // must be odd - config.correlation_window_size |= 0x1; // must be odd - config.disparity_range = (config.disparity_range / 16) * 16; // must be multiple of 16 - - // check stereo method - // Note: With single-threaded NodeHandle, configCb and imageCb can't be called - // concurrently, so this is thread-safe. - block_matcher_.setPreFilterCap(config.prefilter_cap); - block_matcher_.setCorrelationWindowSize(config.correlation_window_size); - block_matcher_.setMinDisparity(config.min_disparity); - block_matcher_.setDisparityRange(config.disparity_range); - block_matcher_.setUniquenessRatio(config.uniqueness_ratio); - block_matcher_.setSpeckleSize(config.speckle_size); - block_matcher_.setSpeckleRange(config.speckle_range); - if (config.stereo_algorithm == stereo_image_proc::Disparity_StereoBM) { // StereoBM - block_matcher_.setStereoType(StereoProcessor::BM); - block_matcher_.setPreFilterSize(config.prefilter_size); - block_matcher_.setTextureThreshold(config.texture_threshold); - } - else if (config.stereo_algorithm == stereo_image_proc::Disparity_StereoSGBM) { // StereoSGBM - block_matcher_.setStereoType(StereoProcessor::SGBM); - block_matcher_.setSgbmMode(config.fullDP); - block_matcher_.setP1(config.P1); - block_matcher_.setP2(config.P2); - block_matcher_.setDisp12MaxDiff(config.disp12MaxDiff); - } -} - -} // namespace stereo_image_proc - -// Register nodelet -#include -PLUGINLIB_EXPORT_CLASS(stereo_image_proc::DisparityNodelet,nodelet::Nodelet) diff --git a/stereo_image_proc/src/nodelets/point_cloud2.cpp b/stereo_image_proc/src/nodelets/point_cloud2.cpp deleted file mode 100644 index fae8ba1f4..000000000 --- a/stereo_image_proc/src/nodelets/point_cloud2.cpp +++ /dev/null @@ -1,272 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* 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 name of the Willow Garage 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 -#if ((BOOST_VERSION / 100) % 1000) >= 53 -#include -#endif - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace stereo_image_proc { - -using namespace sensor_msgs; -using namespace stereo_msgs; -using namespace message_filters::sync_policies; - -class PointCloud2Nodelet : public nodelet::Nodelet -{ - boost::shared_ptr it_; - - // Subscriptions - image_transport::SubscriberFilter sub_l_image_; - message_filters::Subscriber sub_l_info_, sub_r_info_; - message_filters::Subscriber sub_disparity_; - typedef ExactTime ExactPolicy; - typedef ApproximateTime ApproximatePolicy; - typedef message_filters::Synchronizer ExactSync; - typedef message_filters::Synchronizer ApproximateSync; - boost::shared_ptr exact_sync_; - boost::shared_ptr approximate_sync_; - - // Publications - boost::mutex connect_mutex_; - ros::Publisher pub_points2_; - - // Processing state (note: only safe because we're single-threaded!) - image_geometry::StereoCameraModel model_; - cv::Mat_ points_mat_; // scratch buffer - - virtual void onInit(); - - void connectCb(); - - void imageCb(const ImageConstPtr& l_image_msg, - const CameraInfoConstPtr& l_info_msg, - const CameraInfoConstPtr& r_info_msg, - const DisparityImageConstPtr& disp_msg); -}; - -void PointCloud2Nodelet::onInit() -{ - ros::NodeHandle &nh = getNodeHandle(); - ros::NodeHandle &private_nh = getPrivateNodeHandle(); - it_.reset(new image_transport::ImageTransport(nh)); - - // Synchronize inputs. Topic subscriptions happen on demand in the connection - // callback. Optionally do approximate synchronization. - int queue_size; - private_nh.param("queue_size", queue_size, 5); - bool approx; - private_nh.param("approximate_sync", approx, false); - if (approx) - { - approximate_sync_.reset( new ApproximateSync(ApproximatePolicy(queue_size), - sub_l_image_, sub_l_info_, - sub_r_info_, sub_disparity_) ); - approximate_sync_->registerCallback(boost::bind(&PointCloud2Nodelet::imageCb, - this, _1, _2, _3, _4)); - } - else - { - exact_sync_.reset( new ExactSync(ExactPolicy(queue_size), - sub_l_image_, sub_l_info_, - sub_r_info_, sub_disparity_) ); - exact_sync_->registerCallback(boost::bind(&PointCloud2Nodelet::imageCb, - this, _1, _2, _3, _4)); - } - - // Monitor whether anyone is subscribed to the output - ros::SubscriberStatusCallback connect_cb = boost::bind(&PointCloud2Nodelet::connectCb, this); - // Make sure we don't enter connectCb() between advertising and assigning to pub_points2_ - boost::lock_guard lock(connect_mutex_); - pub_points2_ = nh.advertise("points2", 1, connect_cb, connect_cb); -} - -// Handles (un)subscribing when clients (un)subscribe -void PointCloud2Nodelet::connectCb() -{ - boost::lock_guard lock(connect_mutex_); - if (pub_points2_.getNumSubscribers() == 0) - { - sub_l_image_ .unsubscribe(); - sub_l_info_ .unsubscribe(); - sub_r_info_ .unsubscribe(); - sub_disparity_.unsubscribe(); - } - else if (!sub_l_image_.getSubscriber()) - { - ros::NodeHandle &nh = getNodeHandle(); - // Queue size 1 should be OK; the one that matters is the synchronizer queue size. - image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle()); - sub_l_image_ .subscribe(*it_, "left/image_rect_color", 1, hints); - sub_l_info_ .subscribe(nh, "left/camera_info", 1); - sub_r_info_ .subscribe(nh, "right/camera_info", 1); - sub_disparity_.subscribe(nh, "disparity", 1); - } -} - -inline bool isValidPoint(const cv::Vec3f& pt) -{ - // Check both for disparities explicitly marked as invalid (where OpenCV maps pt.z to MISSING_Z) - // and zero disparities (point mapped to infinity). - return pt[2] != image_geometry::StereoCameraModel::MISSING_Z && !std::isinf(pt[2]); -} - -void PointCloud2Nodelet::imageCb(const ImageConstPtr& l_image_msg, - const CameraInfoConstPtr& l_info_msg, - const CameraInfoConstPtr& r_info_msg, - const DisparityImageConstPtr& disp_msg) -{ - // Update the camera model - model_.fromCameraInfo(l_info_msg, r_info_msg); - - // Calculate point cloud - const Image& dimage = disp_msg->image; - const cv::Mat_ dmat(dimage.height, dimage.width, (float*)&dimage.data[0], dimage.step); - model_.projectDisparityImageTo3d(dmat, points_mat_, true); - cv::Mat_ mat = points_mat_; - - // Fill in new PointCloud2 message (2D image-like layout) - PointCloud2Ptr points_msg = boost::make_shared(); - points_msg->header = disp_msg->header; - points_msg->height = mat.rows; - points_msg->width = mat.cols; - points_msg->is_bigendian = false; - points_msg->is_dense = false; // there may be invalid points - - sensor_msgs::PointCloud2Modifier pcd_modifier(*points_msg); - pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); - - sensor_msgs::PointCloud2Iterator iter_x(*points_msg, "x"); - sensor_msgs::PointCloud2Iterator iter_y(*points_msg, "y"); - sensor_msgs::PointCloud2Iterator iter_z(*points_msg, "z"); - sensor_msgs::PointCloud2Iterator iter_r(*points_msg, "r"); - sensor_msgs::PointCloud2Iterator iter_g(*points_msg, "g"); - sensor_msgs::PointCloud2Iterator iter_b(*points_msg, "b"); - - float bad_point = std::numeric_limits::quiet_NaN (); - for (int v = 0; v < mat.rows; ++v) - { - for (int u = 0; u < mat.cols; ++u, ++iter_x, ++iter_y, ++iter_z) - { - if (isValidPoint(mat(v,u))) - { - // x,y,z - *iter_x = mat(v, u)[0]; - *iter_y = mat(v, u)[1]; - *iter_z = mat(v, u)[2]; - } - else - { - *iter_x = *iter_y = *iter_z = bad_point; - } - } - } - - // Fill in color - namespace enc = sensor_msgs::image_encodings; - const std::string& encoding = l_image_msg->encoding; - if (encoding == enc::MONO8) - { - const cv::Mat_ color(l_image_msg->height, l_image_msg->width, - (uint8_t*)&l_image_msg->data[0], - l_image_msg->step); - for (int v = 0; v < mat.rows; ++v) - { - for (int u = 0; u < mat.cols; ++u, ++iter_r, ++iter_g, ++iter_b) - { - uint8_t g = color(v,u); - *iter_r = *iter_g = *iter_b = g; - } - } - } - else if (encoding == enc::RGB8) - { - const cv::Mat_ color(l_image_msg->height, l_image_msg->width, - (cv::Vec3b*)&l_image_msg->data[0], - l_image_msg->step); - for (int v = 0; v < mat.rows; ++v) - { - for (int u = 0; u < mat.cols; ++u, ++iter_r, ++iter_g, ++iter_b) - { - const cv::Vec3b& rgb = color(v,u); - *iter_r = rgb[0]; - *iter_g = rgb[1]; - *iter_b = rgb[2]; - } - } - } - else if (encoding == enc::BGR8) - { - const cv::Mat_ color(l_image_msg->height, l_image_msg->width, - (cv::Vec3b*)&l_image_msg->data[0], - l_image_msg->step); - for (int v = 0; v < mat.rows; ++v) - { - for (int u = 0; u < mat.cols; ++u, ++iter_r, ++iter_g, ++iter_b) - { - const cv::Vec3b& bgr = color(v,u); - *iter_r = bgr[2]; - *iter_g = bgr[1]; - *iter_b = bgr[0]; - } - } - } - else - { - NODELET_WARN_THROTTLE(30, "Could not fill color channel of the point cloud, " - "unsupported encoding '%s'", encoding.c_str()); - } - - pub_points2_.publish(points_msg); -} - -} // namespace stereo_image_proc - -// Register nodelet -#include -PLUGINLIB_EXPORT_CLASS(stereo_image_proc::PointCloud2Nodelet,nodelet::Nodelet) diff --git a/stereo_image_proc/src/nodes/stereo_image_proc.cpp b/stereo_image_proc/src/nodes/stereo_image_proc.cpp deleted file mode 100644 index df8330277..000000000 --- a/stereo_image_proc/src/nodes/stereo_image_proc.cpp +++ /dev/null @@ -1,149 +0,0 @@ -/********************************************************************* -* Software License Agreement (BSD License) -* -* Copyright (c) 2008, Willow Garage, Inc. -* 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 name of the Willow Garage 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 -#include - -void loadMonocularNodelets(nodelet::Loader& manager, const std::string& side, - const XmlRpc::XmlRpcValue& rectify_params, - const nodelet::V_string& my_argv) -{ - nodelet::M_string remappings; - - // Explicitly resolve global remappings (wg-ros-pkg #5055). - // Otherwise the internal remapping 'image_raw' -> 'left/image_raw' can hide a - // global remapping from the launch file or command line. - std::string image_raw_topic = ros::names::resolve(side + "/image_raw"); - std::string image_mono_topic = ros::names::resolve(side + "/image_mono"); - std::string image_color_topic = ros::names::resolve(side + "/image_color"); - std::string image_rect_topic = ros::names::resolve(side + "/image_rect"); - std::string image_rect_color_topic = ros::names::resolve(side + "/image_rect_color"); - std::string camera_info_topic = ros::names::resolve(side + "/camera_info"); - - // Debayer nodelet: image_raw -> image_mono, image_color - remappings["image_raw"] = image_raw_topic; - remappings["image_mono"] = image_mono_topic; - remappings["image_color"] = image_color_topic; - std::string debayer_name = ros::this_node::getName() + "_debayer_" + side; - manager.load(debayer_name, "image_proc/debayer", remappings, my_argv); - - // Rectify nodelet: image_mono -> image_rect - remappings.clear(); - remappings["image_mono"] = image_mono_topic; - remappings["camera_info"] = camera_info_topic; - remappings["image_rect"] = image_rect_topic; - std::string rectify_mono_name = ros::this_node::getName() + "_rectify_mono_" + side; - if (rectify_params.valid()) - ros::param::set(rectify_mono_name, rectify_params); - manager.load(rectify_mono_name, "image_proc/rectify", remappings, my_argv); - - // Rectify nodelet: image_color -> image_rect_color - remappings.clear(); - remappings["image_mono"] = image_color_topic; - remappings["camera_info"] = camera_info_topic; - remappings["image_rect"] = image_rect_color_topic; - std::string rectify_color_name = ros::this_node::getName() + "_rectify_color_" + side; - if (rectify_params.valid()) - ros::param::set(rectify_color_name, rectify_params); - manager.load(rectify_color_name, "image_proc/rectify", remappings, my_argv); -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "stereo_image_proc"); - - // Check for common user errors - if (ros::names::remap("camera") != "camera") - { - ROS_WARN("Remapping 'camera' has no effect! Start stereo_image_proc in the " - "stereo namespace instead.\nExample command-line usage:\n" - "\t$ ROS_NAMESPACE=%s rosrun stereo_image_proc stereo_image_proc", - ros::names::remap("camera").c_str()); - } - if (ros::this_node::getNamespace() == "/") - { - ROS_WARN("Started in the global namespace! This is probably wrong. Start " - "stereo_image_proc in the stereo namespace.\nExample command-line usage:\n" - "\t$ ROS_NAMESPACE=my_stereo rosrun stereo_image_proc stereo_image_proc"); - } - - // Shared parameters to be propagated to nodelet private namespaces - ros::NodeHandle private_nh("~"); - XmlRpc::XmlRpcValue shared_params; - int queue_size; - if (private_nh.getParam("queue_size", queue_size)) - shared_params["queue_size"] = queue_size; - - nodelet::Loader manager(false); // Don't bring up the manager ROS API - nodelet::M_string remappings; - nodelet::V_string my_argv; - - // Load equivalents of image_proc for left and right cameras - loadMonocularNodelets(manager, "left", shared_params, my_argv); - loadMonocularNodelets(manager, "right", shared_params, my_argv); - - // Stereo nodelets also need to know the synchronization policy - bool approx_sync; - if (private_nh.getParam("approximate_sync", approx_sync)) - shared_params["approximate_sync"] = XmlRpc::XmlRpcValue(approx_sync); - - // Disparity nodelet - // Inputs: left/image_rect, left/camera_info, right/image_rect, right/camera_info - // Outputs: disparity - // NOTE: Using node name for the disparity nodelet because it is the only one using - // dynamic_reconfigure so far, and this makes us backwards-compatible with cturtle. - std::string disparity_name = ros::this_node::getName(); - manager.load(disparity_name, "stereo_image_proc/disparity", remappings, my_argv); - - // PointCloud2 nodelet - // Inputs: left/image_rect_color, left/camera_info, right/camera_info, disparity - // Outputs: points2 - std::string point_cloud2_name = ros::this_node::getName() + "_point_cloud2"; - if (shared_params.valid()) - ros::param::set(point_cloud2_name, shared_params); - manager.load(point_cloud2_name, "stereo_image_proc/point_cloud2", remappings, my_argv); - - // Check for only the original camera topics - ros::V_string topics; - topics.push_back(ros::names::resolve("left/image_raw")); - topics.push_back(ros::names::resolve("left/camera_info")); - topics.push_back(ros::names::resolve("right/image_raw")); - topics.push_back(ros::names::resolve("right/camera_info")); - image_proc::AdvertisementChecker check_inputs(ros::NodeHandle(), ros::this_node::getName()); - check_inputs.start(topics, 60.0); - - ros::spin(); - return 0; -} diff --git a/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp b/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp new file mode 100644 index 000000000..3f4f1822e --- /dev/null +++ b/stereo_image_proc/src/stereo_image_proc/disparity_node.cpp @@ -0,0 +1,227 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +namespace stereo_image_proc +{ + +class DisparityNode : public rclcpp::Node +{ +public: + explicit DisparityNode(const rclcpp::NodeOptions & options); + +private: + // Subscriptions + image_transport::SubscriberFilter sub_l_image_, sub_r_image_; + message_filters::Subscriber sub_l_info_, sub_r_info_; + using ExactPolicy = message_filters::sync_policies::ExactTime< + sensor_msgs::msg::Image, + sensor_msgs::msg::CameraInfo, + sensor_msgs::msg::Image, + sensor_msgs::msg::CameraInfo>; + using ApproximatePolicy = message_filters::sync_policies::ApproximateTime< + sensor_msgs::msg::Image, + sensor_msgs::msg::CameraInfo, + sensor_msgs::msg::Image, + sensor_msgs::msg::CameraInfo>; + using ExactSync = message_filters::Synchronizer; + using ApproximateSync = message_filters::Synchronizer; + std::shared_ptr exact_sync_; + std::shared_ptr approximate_sync_; + // Publications + std::shared_ptr> pub_disparity_; + + // Processing state (note: only safe because we're single-threaded!) + image_geometry::StereoCameraModel model_; + // contains scratch buffers for block matching + stereo_image_proc::StereoProcessor block_matcher_; + + void connectCb(); + + void imageCb( + const sensor_msgs::msg::Image::ConstSharedPtr & l_image_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & l_info_msg, + const sensor_msgs::msg::Image::ConstSharedPtr & r_image_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & r_info_msg); + + // void configCb(Config &config, uint32_t level); +}; + +DisparityNode::DisparityNode(const rclcpp::NodeOptions & options) +: rclcpp::Node("disparity_node", options) +{ + using namespace std::placeholders; + + // Declare/read parameters + int queue_size = this->declare_parameter("queue_size", 5); + bool approx = this->declare_parameter("approximate_sync", false); + + // Synchronize callbacks + if (approx) { + approximate_sync_.reset(new ApproximateSync( + ApproximatePolicy(queue_size), + sub_l_image_, sub_l_info_, + sub_r_image_, sub_r_info_)); + approximate_sync_->registerCallback( + std::bind(&DisparityNode::imageCb, this, _1, _2, _3, _4)); + } else { + exact_sync_.reset(new ExactSync( + ExactPolicy(queue_size), + sub_l_image_, sub_l_info_, + sub_r_image_, sub_r_info_)); + exact_sync_->registerCallback( + std::bind(&DisparityNode::imageCb, this, _1, _2, _3, _4)); + } + + // TODO(jacobperron): Setup equivalent of dynamic reconfigure with ROS 2 parameters + // See configCb + + pub_disparity_ = create_publisher("disparity", 1); + + // TODO(jacobperron): Replace this with a graph event. + // Only subscribe if there's a subscription listening to our publisher. + connectCb(); +} + +// Handles (un)subscribing when clients (un)subscribe +void DisparityNode::connectCb() +{ + // TODO(jacobperron): Add unsubscribe logic when we use graph events + image_transport::TransportHints hints(this, "raw"); + sub_l_image_.subscribe(this, "left/image_rect", hints.getTransport()); + sub_l_info_.subscribe(this, "left/camera_info"); + sub_r_image_.subscribe(this, "right/image_rect", hints.getTransport()); + sub_r_info_.subscribe(this, "right/camera_info"); +} + +void DisparityNode::imageCb( + const sensor_msgs::msg::Image::ConstSharedPtr & l_image_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & l_info_msg, + const sensor_msgs::msg::Image::ConstSharedPtr & r_image_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & r_info_msg) +{ + // If there are no subscriptions for the disparity image, do nothing + if (pub_disparity_->get_subscription_count() == 0u) { + return; + } + + // Update the camera model + model_.fromCameraInfo(l_info_msg, r_info_msg); + + // Allocate new disparity image message + auto disp_msg = std::make_shared(); + disp_msg->header = l_info_msg->header; + disp_msg->image.header = l_info_msg->header; + + // Compute window of (potentially) valid disparities + int border = block_matcher_.getCorrelationWindowSize() / 2; + int left = block_matcher_.getDisparityRange() + block_matcher_.getMinDisparity() + border - 1; + int wtf; + if (block_matcher_.getMinDisparity() >= 0) { + wtf = border + block_matcher_.getMinDisparity(); + } else { + wtf = std::max(border, -block_matcher_.getMinDisparity()); + } + // TODO(jacobperron): the message width has not been set yet! What should we do here? + int right = disp_msg->image.width - 1 - wtf; + int top = border; + int bottom = disp_msg->image.height - 1 - border; + disp_msg->valid_window.x_offset = left; + disp_msg->valid_window.y_offset = top; + disp_msg->valid_window.width = right - left; + disp_msg->valid_window.height = bottom - top; + + // Create cv::Mat views onto all buffers + const cv::Mat_ l_image = + cv_bridge::toCvShare(l_image_msg, sensor_msgs::image_encodings::MONO8)->image; + const cv::Mat_ r_image = + cv_bridge::toCvShare(r_image_msg, sensor_msgs::image_encodings::MONO8)->image; + + // Perform block matching to find the disparities + block_matcher_.processDisparity(l_image, r_image, model_, *disp_msg); + + pub_disparity_->publish(*disp_msg); +} + +// void DisparityNode::configCb(Config &config, uint32_t level) +// { +// // Tweak all settings to be valid +// config.prefilter_size |= 0x1; // must be odd +// config.correlation_window_size |= 0x1; // must be odd +// config.disparity_range = (config.disparity_range / 16) * 16; // must be multiple of 16 +// +// // check stereo method +// // Note: With single-threaded NodeHandle, configCb and imageCb can't be called +// // concurrently, so this is thread-safe. +// block_matcher_.setPreFilterCap(config.prefilter_cap); +// block_matcher_.setCorrelationWindowSize(config.correlation_window_size); +// block_matcher_.setMinDisparity(config.min_disparity); +// block_matcher_.setDisparityRange(config.disparity_range); +// block_matcher_.setUniquenessRatio(config.uniqueness_ratio); +// block_matcher_.setSpeckleSize(config.speckle_size); +// block_matcher_.setSpeckleRange(config.speckle_range); +// if (config.stereo_algorithm == stereo_image_proc::Disparity_StereoBM) { // StereoBM +// block_matcher_.setStereoType(StereoProcessor::BM); +// block_matcher_.setPreFilterSize(config.prefilter_size); +// block_matcher_.setTextureThreshold(config.texture_threshold); +// } +// else if (config.stereo_algorithm == stereo_image_proc::Disparity_StereoSGBM) { // StereoSGBM +// block_matcher_.setStereoType(StereoProcessor::SGBM); +// block_matcher_.setSgbmMode(config.fullDP); +// block_matcher_.setP1(config.P1); +// block_matcher_.setP2(config.P2); +// block_matcher_.setDisp12MaxDiff(config.disp12MaxDiff); +// } +// } + +} // namespace stereo_image_proc + +// Register component +RCLCPP_COMPONENTS_REGISTER_NODE(stereo_image_proc::DisparityNode) diff --git a/stereo_image_proc/src/stereo_image_proc/point_cloud_node.cpp b/stereo_image_proc/src/stereo_image_proc/point_cloud_node.cpp new file mode 100644 index 000000000..7f66b929b --- /dev/null +++ b/stereo_image_proc/src/stereo_image_proc/point_cloud_node.cpp @@ -0,0 +1,256 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace stereo_image_proc +{ + +class PointCloudNode : public rclcpp::Node +{ +public: + explicit PointCloudNode(const rclcpp::NodeOptions & options); + +private: + // Subscriptions + image_transport::SubscriberFilter sub_l_image_; + message_filters::Subscriber sub_l_info_, sub_r_info_; + message_filters::Subscriber sub_disparity_; + using ExactPolicy = message_filters::sync_policies::ExactTime< + sensor_msgs::msg::Image, + sensor_msgs::msg::CameraInfo, + sensor_msgs::msg::CameraInfo, + stereo_msgs::msg::DisparityImage>; + using ApproximatePolicy = message_filters::sync_policies::ApproximateTime< + sensor_msgs::msg::Image, + sensor_msgs::msg::CameraInfo, + sensor_msgs::msg::CameraInfo, + stereo_msgs::msg::DisparityImage>; + using ExactSync = message_filters::Synchronizer; + using ApproximateSync = message_filters::Synchronizer; + std::shared_ptr exact_sync_; + std::shared_ptr approximate_sync_; + + // Publications + std::shared_ptr> pub_points2_; + + // Processing state (note: only safe because we're single-threaded!) + image_geometry::StereoCameraModel model_; + cv::Mat_ points_mat_; // scratch buffer + + void connectCb(); + + void imageCb( + const sensor_msgs::msg::Image::ConstSharedPtr & l_image_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & l_info_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & r_info_msg, + const stereo_msgs::msg::DisparityImage::ConstSharedPtr & disp_msg); +}; + +PointCloudNode::PointCloudNode(const rclcpp::NodeOptions & options) +: rclcpp::Node("point_cloud_node", options) +{ + using namespace std::placeholders; + + // Declare/read parameters + int queue_size = this->declare_parameter("queue_size", 5); + bool approx = this->declare_parameter("approximate_sync", false); + + // Synchronize callbacks + if (approx) { + approximate_sync_.reset(new ApproximateSync( + ApproximatePolicy(queue_size), + sub_l_image_, sub_l_info_, + sub_r_info_, sub_disparity_)); + approximate_sync_->registerCallback( + std::bind(&PointCloudNode::imageCb, this, _1, _2, _3, _4)); + } else { + exact_sync_.reset(new ExactSync( + ExactPolicy(queue_size), + sub_l_image_, sub_l_info_, + sub_r_info_, sub_disparity_)); + exact_sync_->registerCallback( + std::bind(&PointCloudNode::imageCb, this, _1, _2, _3, _4)); + } + + pub_points2_ = create_publisher("points2", 1); + + // TODO(jacobperron): Replace this with a graph event. + // Only subscribe if there's a subscription listening to our publisher. + connectCb(); +} + +// Handles (un)subscribing when clients (un)subscribe +void PointCloudNode::connectCb() +{ + // TODO(jacobperron): Add unsubscribe logic when we use graph events + image_transport::TransportHints hints(this, "raw"); + sub_l_image_.subscribe(this, "left/image_rect_color", hints.getTransport()); + sub_l_info_.subscribe(this, "left/camera_info"); + sub_r_info_.subscribe(this, "right/camera_info"); + sub_disparity_.subscribe(this, "disparity"); +} + +inline bool isValidPoint(const cv::Vec3f & pt) +{ + // Check both for disparities explicitly marked as invalid (where OpenCV maps pt.z to MISSING_Z) + // and zero disparities (point mapped to infinity). + return pt[2] != image_geometry::StereoCameraModel::MISSING_Z && !std::isinf(pt[2]); +} + +void PointCloudNode::imageCb( + const sensor_msgs::msg::Image::ConstSharedPtr & l_image_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & l_info_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & r_info_msg, + const stereo_msgs::msg::DisparityImage::ConstSharedPtr & disp_msg) +{ + // If there are no subscriptions for the point cloud, do nothing + if (pub_points2_->get_subscription_count() == 0u) { + return; + } + + // Update the camera model + model_.fromCameraInfo(l_info_msg, r_info_msg); + + // Calculate point cloud + const sensor_msgs::msg::Image & dimage = disp_msg->image; + // The cv::Mat_ constructor doesn't accept a const data data pointer + // so we remove the constness before reinterpreting into float. + // This is "safe" since our cv::Mat is const. + float * data = reinterpret_cast(const_cast(&dimage.data[0])); + + const cv::Mat_ dmat(dimage.height, dimage.width, data, dimage.step); + model_.projectDisparityImageTo3d(dmat, points_mat_, true); + cv::Mat_ mat = points_mat_; + + // Fill in new PointCloud2 message (2D image-like layout) + auto points_msg = std::make_shared(); + points_msg->header = disp_msg->header; + points_msg->height = mat.rows; + points_msg->width = mat.cols; + points_msg->is_bigendian = false; + points_msg->is_dense = false; // there may be invalid points + + sensor_msgs::PointCloud2Modifier pcd_modifier(*points_msg); + pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); + + sensor_msgs::PointCloud2Iterator iter_x(*points_msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(*points_msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(*points_msg, "z"); + sensor_msgs::PointCloud2Iterator iter_r(*points_msg, "r"); + sensor_msgs::PointCloud2Iterator iter_g(*points_msg, "g"); + sensor_msgs::PointCloud2Iterator iter_b(*points_msg, "b"); + + float bad_point = std::numeric_limits::quiet_NaN(); + for (int v = 0; v < mat.rows; ++v) { + for (int u = 0; u < mat.cols; ++u, ++iter_x, ++iter_y, ++iter_z) { + if (isValidPoint(mat(v, u))) { + // x,y,z + *iter_x = mat(v, u)[0]; + *iter_y = mat(v, u)[1]; + *iter_z = mat(v, u)[2]; + } else { + *iter_x = *iter_y = *iter_z = bad_point; + } + } + } + + // Fill in color + namespace enc = sensor_msgs::image_encodings; + const std::string & encoding = l_image_msg->encoding; + if (encoding == enc::MONO8) { + const cv::Mat_ color( + l_image_msg->height, l_image_msg->width, + const_cast(&l_image_msg->data[0]), + l_image_msg->step); + for (int v = 0; v < mat.rows; ++v) { + for (int u = 0; u < mat.cols; ++u, ++iter_r, ++iter_g, ++iter_b) { + uint8_t g = color(v, u); + *iter_r = *iter_g = *iter_b = g; + } + } + } else if (encoding == enc::RGB8) { + const cv::Mat_ color( + l_image_msg->height, l_image_msg->width, + (cv::Vec3b *)(&l_image_msg->data[0]), + l_image_msg->step); + for (int v = 0; v < mat.rows; ++v) { + for (int u = 0; u < mat.cols; ++u, ++iter_r, ++iter_g, ++iter_b) { + const cv::Vec3b & rgb = color(v, u); + *iter_r = rgb[0]; + *iter_g = rgb[1]; + *iter_b = rgb[2]; + } + } + } else if (encoding == enc::BGR8) { + const cv::Mat_ color( + l_image_msg->height, l_image_msg->width, + (cv::Vec3b *)(&l_image_msg->data[0]), + l_image_msg->step); + for (int v = 0; v < mat.rows; ++v) { + for (int u = 0; u < mat.cols; ++u, ++iter_r, ++iter_g, ++iter_b) { + const cv::Vec3b & bgr = color(v, u); + *iter_r = bgr[2]; + *iter_g = bgr[1]; + *iter_b = bgr[0]; + } + } + } else { + // Throttle duration in milliseconds + RCUTILS_LOG_WARN_THROTTLE(RCUTILS_STEADY_TIME, 30000, + "Could not fill color channel of the point cloud, " + "unsupported encoding '%s'", encoding.c_str()); + } + + pub_points2_->publish(*points_msg); +} + +} // namespace stereo_image_proc + +// Register node +RCLCPP_COMPONENTS_REGISTER_NODE(stereo_image_proc::PointCloudNode) diff --git a/stereo_image_proc/src/stereo_image_proc/stereo_processor.cpp b/stereo_image_proc/src/stereo_image_proc/stereo_processor.cpp new file mode 100644 index 000000000..09e6cee45 --- /dev/null +++ b/stereo_image_proc/src/stereo_image_proc/stereo_processor.cpp @@ -0,0 +1,337 @@ +// Copyright (c) 2008, Willow Garage, Inc. +// All rights reserved. +// +// Software License Agreement (BSD License 2.0) +// +// 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 name of the copyright holder nor the names of its +// contributors may be used to endorse or promote products derived +// from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +// COPYRIGHT 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 "stereo_image_proc/stereo_processor.hpp" + +#include +#include + +#include +#include +#include + +// TODO(jacobperron): Remove this after it's implemented upstream +// https://github.com/ros2/rcutils/pull/112 +#ifndef RCUTILS_ASSERT +#define RCUTILS_ASSERT assert +#endif +// Uncomment below instead +// #include + +namespace stereo_image_proc +{ + +bool StereoProcessor::process( + const sensor_msgs::msg::Image::ConstSharedPtr & left_raw, + const sensor_msgs::msg::Image::ConstSharedPtr & right_raw, + const image_geometry::StereoCameraModel & model, + StereoImageSet & output, + int flags) const +{ + // Do monocular processing on left and right images + int left_flags = flags & LEFT_ALL; + int right_flags = flags & RIGHT_ALL; + if (flags & STEREO_ALL) { + // Need the rectified images for stereo processing + left_flags |= LEFT_RECT; + right_flags |= RIGHT_RECT; + } + if (flags & (POINT_CLOUD | POINT_CLOUD2)) { + flags |= DISPARITY; + // Need the color channels for the point cloud + left_flags |= LEFT_RECT_COLOR; + } + if (!mono_processor_.process(left_raw, model.left(), output.left, left_flags)) { + return false; + } + if (!mono_processor_.process(right_raw, model.right(), output.right, right_flags >> 4)) { + return false; + } + + // Do block matching to produce the disparity image + if (flags & DISPARITY) { + processDisparity(output.left.rect, output.right.rect, model, output.disparity); + } + + // Project disparity image to 3d point cloud + if (flags & POINT_CLOUD) { + processPoints( + output.disparity, output.left.rect_color, output.left.color_encoding, model, output.points); + } + + // Project disparity image to 3d point cloud + if (flags & POINT_CLOUD2) { + processPoints2( + output.disparity, output.left.rect_color, output.left.color_encoding, model, output.points2); + } + + return true; +} + +void StereoProcessor::processDisparity( + const cv::Mat & left_rect, + const cv::Mat & right_rect, + const image_geometry::StereoCameraModel & model, + stereo_msgs::msg::DisparityImage & disparity) const +{ + // Fixed-point disparity is 16 times the true value: d = d_fp / 16.0 = x_l - x_r. + static const int DPP = 16; // disparities per pixel + static const double inv_dpp = 1.0 / DPP; + + // Block matcher produces 16-bit signed (fixed point) disparity image + if (current_stereo_algorithm_ == BM) { + block_matcher_->compute(left_rect, right_rect, disparity16_); + } else { + sg_block_matcher_->compute(left_rect, right_rect, disparity16_); + } + + // Fill in DisparityImage image data, converting to 32-bit float + sensor_msgs::msg::Image & dimage = disparity.image; + dimage.height = disparity16_.rows; + dimage.width = disparity16_.cols; + dimage.encoding = sensor_msgs::image_encodings::TYPE_32FC1; + dimage.step = dimage.width * sizeof(float); + dimage.data.resize(dimage.step * dimage.height); + cv::Mat_ dmat( + dimage.height, dimage.width, reinterpret_cast(&dimage.data[0]), dimage.step); + // We convert from fixed-point to float disparity and also adjust for any x-offset between + // the principal points: d = d_fp*inv_dpp - (cx_l - cx_r) + disparity16_.convertTo(dmat, dmat.type(), inv_dpp, -(model.left().cx() - model.right().cx())); + RCUTILS_ASSERT(dmat.data == &dimage.data[0]); + // TODO(unknown): is_bigendian? + + // Stereo parameters + disparity.f = model.right().fx(); + disparity.t = model.baseline(); + + // TODO(unknown): Window of (potentially) valid disparities + + // Disparity search range + disparity.min_disparity = getMinDisparity(); + disparity.max_disparity = getMinDisparity() + getDisparityRange() - 1; + disparity.delta_d = inv_dpp; +} + +inline bool isValidPoint(const cv::Vec3f & pt) +{ + // Check both for disparities explicitly marked as invalid (where OpenCV maps pt.z to MISSING_Z) + // and zero disparities (point mapped to infinity). + return pt[2] != image_geometry::StereoCameraModel::MISSING_Z && !std::isinf(pt[2]); +} + +void StereoProcessor::processPoints( + const stereo_msgs::msg::DisparityImage & disparity, + const cv::Mat & color, + const std::string & encoding, + const image_geometry::StereoCameraModel & model, + sensor_msgs::msg::PointCloud & points) const +{ + // Calculate dense point cloud + const sensor_msgs::msg::Image & dimage = disparity.image; + // The cv::Mat_ constructor doesn't accept a const data data pointer + // so we remove the constness before reinterpreting into float. + // This is "safe" since our cv::Mat is const. + float * data = reinterpret_cast(const_cast(&dimage.data[0])); + const cv::Mat_ dmat(dimage.height, dimage.width, data, dimage.step); + model.projectDisparityImageTo3d(dmat, dense_points_, true); + + // Fill in sparse point cloud message + points.points.resize(0); + points.channels.resize(3); + points.channels[0].name = "rgb"; + points.channels[0].values.resize(0); + points.channels[1].name = "u"; + points.channels[1].values.resize(0); + points.channels[2].name = "v"; + points.channels[2].values.resize(0); + + for (int32_t u = 0; u < dense_points_.rows; ++u) { + for (int32_t v = 0; v < dense_points_.cols; ++v) { + if (isValidPoint(dense_points_(u, v))) { + // x,y,z + geometry_msgs::msg::Point32 pt; + pt.x = dense_points_(u, v)[0]; + pt.y = dense_points_(u, v)[1]; + pt.z = dense_points_(u, v)[2]; + points.points.push_back(pt); + // u,v + points.channels[1].values.push_back(u); + points.channels[2].values.push_back(v); + } + } + } + + // Fill in color + namespace enc = sensor_msgs::image_encodings; + points.channels[0].values.reserve(points.points.size()); + if (encoding == enc::MONO8) { + for (int32_t u = 0; u < dense_points_.rows; ++u) { + for (int32_t v = 0; v < dense_points_.cols; ++v) { + if (isValidPoint(dense_points_(u, v))) { + uint8_t g = color.at(u, v); + int32_t rgb = (g << 16) | (g << 8) | g; + points.channels[0].values.push_back(*reinterpret_cast(&rgb)); + } + } + } + } else if (encoding == enc::RGB8) { + for (int32_t u = 0; u < dense_points_.rows; ++u) { + for (int32_t v = 0; v < dense_points_.cols; ++v) { + if (isValidPoint(dense_points_(u, v))) { + const cv::Vec3b & rgb = color.at(u, v); + int32_t rgb_packed = (rgb[0] << 16) | (rgb[1] << 8) | rgb[2]; + points.channels[0].values.push_back(*reinterpret_cast(&rgb_packed)); + } + } + } + } else if (encoding == enc::BGR8) { + for (int32_t u = 0; u < dense_points_.rows; ++u) { + for (int32_t v = 0; v < dense_points_.cols; ++v) { + if (isValidPoint(dense_points_(u, v))) { + const cv::Vec3b & bgr = color.at(u, v); + int32_t rgb_packed = (bgr[2] << 16) | (bgr[1] << 8) | bgr[0]; + points.channels[0].values.push_back(*reinterpret_cast(&rgb_packed)); + } + } + } + } else { + RCUTILS_LOG_WARN( + "Could not fill color channel of the point cloud, unrecognized encoding '%s'", + encoding.c_str()); + } +} + +void StereoProcessor::processPoints2( + const stereo_msgs::msg::DisparityImage & disparity, + const cv::Mat & color, + const std::string & encoding, + const image_geometry::StereoCameraModel & model, + sensor_msgs::msg::PointCloud2 & points) const +{ + // Calculate dense point cloud + const sensor_msgs::msg::Image & dimage = disparity.image; + // The cv::Mat_ constructor doesn't accept a const data data pointer + // so we remove the constness before reinterpreting into float. + // This is "safe" since our cv::Mat is const. + float * data = reinterpret_cast(const_cast(&dimage.data[0])); + const cv::Mat_ dmat(dimage.height, dimage.width, data, dimage.step); + model.projectDisparityImageTo3d(dmat, dense_points_, true); + + // Fill in sparse point cloud message + points.height = dense_points_.rows; + points.width = dense_points_.cols; + points.fields.resize(4); + points.fields[0].name = "x"; + points.fields[0].offset = 0; + points.fields[0].count = 1; + points.fields[0].datatype = sensor_msgs::msg::PointField::FLOAT32; + points.fields[1].name = "y"; + points.fields[1].offset = 4; + points.fields[1].count = 1; + points.fields[1].datatype = sensor_msgs::msg::PointField::FLOAT32; + points.fields[2].name = "z"; + points.fields[2].offset = 8; + points.fields[2].count = 1; + points.fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32; + points.fields[3].name = "rgb"; + points.fields[3].offset = 12; + points.fields[3].count = 1; + points.fields[3].datatype = sensor_msgs::msg::PointField::FLOAT32; + // points.is_bigendian = false; ??? + points.point_step = 16; + points.row_step = points.point_step * points.width; + points.data.resize(points.row_step * points.height); + points.is_dense = false; // there may be invalid points + + float bad_point = std::numeric_limits::quiet_NaN(); + int i = 0; + for (int32_t u = 0; u < dense_points_.rows; ++u) { + for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) { + if (isValidPoint(dense_points_(u, v))) { + // x,y,z,rgba + memcpy(&points.data[i * points.point_step + 0], &dense_points_(u, v)[0], sizeof(float)); + memcpy(&points.data[i * points.point_step + 4], &dense_points_(u, v)[1], sizeof(float)); + memcpy(&points.data[i * points.point_step + 8], &dense_points_(u, v)[2], sizeof(float)); + } else { + memcpy(&points.data[i * points.point_step + 0], &bad_point, sizeof(float)); + memcpy(&points.data[i * points.point_step + 4], &bad_point, sizeof(float)); + memcpy(&points.data[i * points.point_step + 8], &bad_point, sizeof(float)); + } + } + } + + // Fill in color + namespace enc = sensor_msgs::image_encodings; + i = 0; + if (encoding == enc::MONO8) { + for (int32_t u = 0; u < dense_points_.rows; ++u) { + for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) { + if (isValidPoint(dense_points_(u, v))) { + uint8_t g = color.at(u, v); + int32_t rgb = (g << 16) | (g << 8) | g; + memcpy(&points.data[i * points.point_step + 12], &rgb, sizeof(int32_t)); + } else { + memcpy(&points.data[i * points.point_step + 12], &bad_point, sizeof(float)); + } + } + } + } else if (encoding == enc::RGB8) { + for (int32_t u = 0; u < dense_points_.rows; ++u) { + for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) { + if (isValidPoint(dense_points_(u, v))) { + const cv::Vec3b & rgb = color.at(u, v); + int32_t rgb_packed = (rgb[0] << 16) | (rgb[1] << 8) | rgb[2]; + memcpy(&points.data[i * points.point_step + 12], &rgb_packed, sizeof(int32_t)); + } else { + memcpy(&points.data[i * points.point_step + 12], &bad_point, sizeof(float)); + } + } + } + } else if (encoding == enc::BGR8) { + for (int32_t u = 0; u < dense_points_.rows; ++u) { + for (int32_t v = 0; v < dense_points_.cols; ++v, ++i) { + if (isValidPoint(dense_points_(u, v))) { + const cv::Vec3b & bgr = color.at(u, v); + int32_t rgb_packed = (bgr[2] << 16) | (bgr[1] << 8) | bgr[0]; + memcpy(&points.data[i * points.point_step + 12], &rgb_packed, sizeof(int32_t)); + } else { + memcpy(&points.data[i * points.point_step + 12], &bad_point, sizeof(float)); + } + } + } + } else { + RCUTILS_LOG_WARN( + "Could not fill color channel of the point cloud, unrecognized encoding '%s'", + encoding.c_str()); + } +} + +} // namespace stereo_image_proc diff --git a/stereo_image_proc/test/data/README.md b/stereo_image_proc/test/data/README.md new file mode 100644 index 000000000..83dc00ffa --- /dev/null +++ b/stereo_image_proc/test/data/README.md @@ -0,0 +1,2 @@ +The images in this directory were copied from the OpenCV project [from this location](https://github.com/opencv/opencv_extra/tree/dc0c6c1ee2cd142f936e7f957bbc595df2ce17e8/testdata/gpu/stereobm). +Images are licensed under the open-source BSD license: https://opencv.org/license/ diff --git a/stereo_image_proc/test/data/aloe-L.png b/stereo_image_proc/test/data/aloe-L.png new file mode 100644 index 000000000..47587668e Binary files /dev/null and b/stereo_image_proc/test/data/aloe-L.png differ diff --git a/stereo_image_proc/test/data/aloe-R.png b/stereo_image_proc/test/data/aloe-R.png new file mode 100644 index 000000000..5d11c57a9 Binary files /dev/null and b/stereo_image_proc/test/data/aloe-R.png differ diff --git a/stereo_image_proc/test/data/aloe-disp.png b/stereo_image_proc/test/data/aloe-disp.png new file mode 100644 index 000000000..dd4a499be Binary files /dev/null and b/stereo_image_proc/test/data/aloe-disp.png differ diff --git a/stereo_image_proc/test/fixtures/disparity_image_publisher.py b/stereo_image_proc/test/fixtures/disparity_image_publisher.py new file mode 100644 index 000000000..4af425976 --- /dev/null +++ b/stereo_image_proc/test/fixtures/disparity_image_publisher.py @@ -0,0 +1,124 @@ +# Copyright (c) 2019, Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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 name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT 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. + +import array +import sys + +import cv2 + +import numpy + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import CameraInfo +from sensor_msgs.msg import Image +from stereo_msgs.msg import DisparityImage + + +class DisparityImagePublisher(Node): + """ + Disparity image publisher test fixture. + + Publishes test data on topics expected by stereo_image_proc nodes: + + - left image + - left camera info + - right camera info + - disparity image + """ + + def __init__( + self, + left_image: numpy.ndarray, + disparity_image: numpy.ndarray, + *, + timer_period: float = 0.1 + ): + """ + Construct a stereo image publisher. + + :param: left_image The image to publish on the left topic. + :param: right_image The image to publish on the right topic. + :param: timer_period The period in seconds at which messages are published. + """ + super().__init__('image_publisher') + self.left_image_and_info = self._create_image_and_info_messages(left_image) + self.disparity_image = DisparityImage() + self.disparity_image.image.height = disparity_image.shape[0] + self.disparity_image.image.width = disparity_image.shape[1] + self.disparity_image.image.step = self.disparity_image.image.width + self.disparity_image.image.data = array.array('B', disparity_image.tobytes()) + + self.left_image_pub = self.create_publisher(Image, 'left/image_rect_color', 1) + self.left_camera_info_pub = self.create_publisher(CameraInfo, 'left/camera_info', 1) + self.right_camera_info_pub = self.create_publisher(CameraInfo, 'right/camera_info', 1) + self.disparity_image_pub = self.create_publisher(DisparityImage, 'disparity', 1) + self.timer = self.create_timer(timer_period, self.timer_callback) + + def _create_image_and_info_messages(self, image): + image_msg = Image() + image_msg.height = image.shape[0] + image_msg.width = image.shape[1] + image_msg.encoding = 'bgr8' + image_msg.step = image_msg.width * 3 + image_msg.data = array.array('B', image.tobytes()) + + camera_info_msg = CameraInfo() + camera_info_msg.height = image.shape[0] + camera_info_msg.width = image.shape[1] + + return (image_msg, camera_info_msg) + + def timer_callback(self): + now = self.get_clock().now().to_msg() + self.left_image_and_info[0].header.stamp = now + self.left_image_and_info[1].header.stamp = now + self.disparity_image.header.stamp = now + + self.left_image_pub.publish(self.left_image_and_info[0]) + self.left_camera_info_pub.publish(self.left_image_and_info[1]) + self.right_camera_info_pub.publish(self.left_image_and_info[1]) + self.disparity_image_pub.publish(self.disparity_image) + + +if __name__ == '__main__': + rclpy.init() + left_image = cv2.imread(sys.argv[1]) + disparity_image = cv2.imread(sys.argv[2], cv2.IMREAD_GRAYSCALE).astype(float) + publisher = DisparityImagePublisher(left_image, disparity_image) + try: + rclpy.spin(publisher) + except KeyboardInterrupt: + pass + finally: + publisher.destroy_node() + rclpy.shutdown() diff --git a/stereo_image_proc/test/fixtures/stereo_image_publisher.py b/stereo_image_proc/test/fixtures/stereo_image_publisher.py new file mode 100644 index 000000000..0068ba632 --- /dev/null +++ b/stereo_image_proc/test/fixtures/stereo_image_publisher.py @@ -0,0 +1,120 @@ +# Copyright (c) 2019, Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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 name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT 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. + +import array +import sys + +import cv2 + +import numpy + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import CameraInfo +from sensor_msgs.msg import Image + + +class StereoImagePublisher(Node): + """ + Stereo image publisher test fixture. + + Publishes test data on topics expected by stereo_image_proc nodes: + + - left image + - right image + - left camera info + - right camera info + """ + + def __init__( + self, + left_image: numpy.ndarray, + right_image: numpy.ndarray, + *, + timer_period: float = 0.1 + ): + """ + Construct a stereo image publisher. + + :param: left_image The image to publish on the left topic. + :param: right_image The image to publish on the right topic. + :param: timer_period The period in seconds at which messages are published. + """ + super().__init__('image_publisher') + self.left_image_and_info = self._create_image_and_info_messages(left_image) + self.right_image_and_info = self._create_image_and_info_messages(right_image) + + self.left_image_pub = self.create_publisher(Image, 'left/image_rect', 1) + self.left_camera_info_pub = self.create_publisher(CameraInfo, 'left/camera_info', 1) + self.right_image_pub = self.create_publisher(Image, 'right/image_rect', 1) + self.right_camera_info_pub = self.create_publisher(CameraInfo, 'right/camera_info', 1) + self.timer = self.create_timer(timer_period, self.timer_callback) + + def _create_image_and_info_messages(self, image): + image_msg = Image() + image_msg.height = image.shape[0] + image_msg.width = image.shape[1] + image_msg.encoding = 'bgr8' + image_msg.step = image_msg.width * 3 + image_msg.data = array.array('B', image.tobytes()) + + camera_info_msg = CameraInfo() + camera_info_msg.height = image.shape[0] + camera_info_msg.width = image.shape[1] + + return (image_msg, camera_info_msg) + + def timer_callback(self): + now = self.get_clock().now().to_msg() + self.left_image_and_info[0].header.stamp = now + self.left_image_and_info[1].header.stamp = now + self.right_image_and_info[0].header.stamp = now + self.right_image_and_info[1].header.stamp = now + + self.left_image_pub.publish(self.left_image_and_info[0]) + self.left_camera_info_pub.publish(self.left_image_and_info[1]) + self.right_image_pub.publish(self.right_image_and_info[0]) + self.right_camera_info_pub.publish(self.right_image_and_info[1]) + + +if __name__ == '__main__': + rclpy.init() + left_image = cv2.imread(sys.argv[1]) + right_image = cv2.imread(sys.argv[2]) + publisher = StereoImagePublisher(left_image, right_image) + try: + rclpy.spin(publisher) + except KeyboardInterrupt: + pass + finally: + publisher.destroy_node() + rclpy.shutdown() diff --git a/stereo_image_proc/test/test_disparity_node.py b/stereo_image_proc/test/test_disparity_node.py new file mode 100644 index 000000000..914953b27 --- /dev/null +++ b/stereo_image_proc/test/test_disparity_node.py @@ -0,0 +1,113 @@ +# Copyright (c) 2019, Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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 name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT 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. + +import os +import sys +import time +import unittest + +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from launch.actions import OpaqueFunction + +from launch_ros.actions import Node + +import pytest + +import rclpy + +from stereo_msgs.msg import DisparityImage + + +@pytest.mark.rostest +def generate_test_description(ready_fn): + + path_to_stereo_image_publisher_fixture = os.path.join( + os.path.dirname(__file__), 'fixtures', 'stereo_image_publisher.py') + path_to_test_data = os.path.join(os.path.dirname(__file__), 'data') + path_to_left_image = os.path.join(path_to_test_data, 'aloe-L.png') + path_to_right_image = os.path.join(path_to_test_data, 'aloe-R.png') + + return LaunchDescription([ + # Stereo image publisher + # TODO(jacobperron): we can use Node in Eloquent + ExecuteProcess( + cmd=[ + sys.executable, + path_to_stereo_image_publisher_fixture, + path_to_left_image, + path_to_right_image + ], + output='screen' + ), + # DisparityNode + Node( + package='stereo_image_proc', + node_executable='disparity_node', + node_name='disparity_node', + output='screen' + ), + # TODO(jacobperron): In Eloquent, use 'launch_testing.actions.ReadyToTest()' + OpaqueFunction(function=lambda context: ready_fn()), + ]) + + +class TestDisparityNode(unittest.TestCase): + + @classmethod + def setUpClass(cls): + rclpy.init() + cls.node = rclpy.create_node('test_disparity_node') + + @classmethod + def tearDownClass(cls): + cls.node.destroy_node() + rclpy.shutdown() + + def test_message_received(self): + # Expect the disparity node to publish on '/diparity' topic + msgs_received = [] + self.node.create_subscription( + DisparityImage, + 'disparity', + lambda msg: msgs_received.append(msg), + 1 + ) + + # Wait up to 10 seconds to receive message + start_time = time.time() + while len(msgs_received) == 0 and (time.time() - start_time) < 10: + rclpy.spin_once(self.node, timeout_sec=0.1) + + assert len(msgs_received) > 0 + + # TODO(jacobperron): Compare received disparity image against expected diff --git a/stereo_image_proc/test/test_point_cloud_node.py b/stereo_image_proc/test/test_point_cloud_node.py new file mode 100644 index 000000000..8b8022501 --- /dev/null +++ b/stereo_image_proc/test/test_point_cloud_node.py @@ -0,0 +1,113 @@ +# Copyright (c) 2019, Open Source Robotics Foundation, Inc. +# All rights reserved. +# +# Software License Agreement (BSD License 2.0) +# +# 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 name of the copyright holder nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT 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. + +import os +import sys +import time +import unittest + +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from launch.actions import OpaqueFunction + +from launch_ros.actions import Node + +import pytest + +import rclpy + +from sensor_msgs.msg import PointCloud2 + + +@pytest.mark.rostest +def generate_test_description(ready_fn): + + path_to_disparity_image_publisher_fixture = os.path.join( + os.path.dirname(__file__), 'fixtures', 'disparity_image_publisher.py') + path_to_test_data = os.path.join(os.path.dirname(__file__), 'data') + path_to_left_image = os.path.join(path_to_test_data, 'aloe-L.png') + path_to_disparity_image = os.path.join(path_to_test_data, 'aloe-disp.png') + + return LaunchDescription([ + # Disparity image publisher + # TODO(jacobperron): we can use Node in Eloquent + ExecuteProcess( + cmd=[ + sys.executable, + path_to_disparity_image_publisher_fixture, + path_to_left_image, + path_to_disparity_image, + ], + output='screen' + ), + # PointCloudNode + Node( + package='stereo_image_proc', + node_executable='point_cloud_node', + node_name='point_cloud_node', + output='screen' + ), + # TODO(jacobperron): In Eloquent, use 'launch_testing.actions.ReadyToTest()' + OpaqueFunction(function=lambda context: ready_fn()), + ]) + + +class TestPointCloudNode(unittest.TestCase): + + @classmethod + def setUpClass(cls): + # TODO(jacobperron): Instead of handling the init/shutdown cycle, as of Eloqeunt + # we can use the node 'launch_service.context.locals.launch_ros_node' + rclpy.init() + cls.node = rclpy.create_node('test_point_cloud_node') + + @classmethod + def tearDownClass(cls): + cls.node.destroy_node() + rclpy.shutdown() + + def test_message_received(self): + # Expect the point cloud node to publish on '/points2' topic + msgs_received = [] + self.node.create_subscription( + PointCloud2, + 'points2', + lambda msg: msgs_received.append(msg), + 1 + ) + + # Wait up to 10 seconds to receive message + start_time = time.time() + while len(msgs_received) == 0 and (time.time() - start_time) < 10: + rclpy.spin_once(self.node, timeout_sec=(0.1)) + + assert len(msgs_received) > 0