From 5a3f743dbf13243a4b3dc8f7655f51f182e2be4d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 24 Jul 2023 18:27:43 +0200 Subject: [PATCH] Added DepthCloud default plugin (#996) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero (cherry picked from commit 8f2e17e441399974ebd465a2d2ef0a3529f57f23) --- README.md | 19 +- rviz_common/CMakeLists.txt | 1 + .../include/rviz_common/depth_cloud_mld.hpp | 157 +++++ .../properties/ros_topic_property.hpp | 29 +- .../src/rviz_common/depth_cloud_mld.cpp | 601 +++++++++++++++++ .../properties/ros_topic_property.cpp | 38 +- rviz_default_plugins/CMakeLists.txt | 15 + .../depth_cloud/depth_cloud_display.hpp | 186 ++++++ .../pointcloud/point_cloud_common.hpp | 3 +- rviz_default_plugins/plugins_description.xml | 11 + .../depth_cloud/depth_cloud_display.cpp | 608 ++++++++++++++++++ .../pointcloud/point_cloud_common.cpp | 15 +- .../pointcloud/point_cloud_display.cpp | 2 +- .../depth_cloud_display_visual_test_ref.png | Bin 0 -> 5332 bytes .../depth_cloud_display_visual_test.cpp | 67 ++ .../page_objects/depth_cloud_page_object.cpp | 101 +++ .../page_objects/depth_cloud_page_object.hpp | 54 ++ .../publishers/image_publisher.hpp | 5 +- 18 files changed, 1885 insertions(+), 27 deletions(-) create mode 100644 rviz_common/include/rviz_common/depth_cloud_mld.hpp create mode 100644 rviz_common/src/rviz_common/depth_cloud_mld.cpp create mode 100644 rviz_default_plugins/include/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.hpp create mode 100644 rviz_default_plugins/src/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.cpp create mode 100644 rviz_default_plugins/test/reference_images/depth_cloud_display_visual_test_ref.png create mode 100644 rviz_default_plugins/test/rviz_default_plugins/displays/depth_cloud/depth_cloud_display_visual_test.cpp create mode 100644 rviz_default_plugins/test/rviz_default_plugins/page_objects/depth_cloud_page_object.cpp create mode 100644 rviz_default_plugins/test/rviz_default_plugins/page_objects/depth_cloud_page_object.hpp diff --git a/README.md b/README.md index 989667a47a..4367f51edf 100644 --- a/README.md +++ b/README.md @@ -16,12 +16,13 @@ For some displays, the [documentation is updated](docs/FEATURES.md). | --------------------- | ------------- | --------------------- | --------------- | | Axes | Move Camera | Orbit | Displays | | Camera | Focus Camera | XY Orbit | Help | -| Effort | Measure | First Person | Selections | -| Fluid | Select | Third Person Follower | Time | -| Grid | 2D Nav Goal | Top Down Orthographic | Tool Properties | -| Grid Cells | Publish Point | | Views | -| Illuminance | Initial Pose | -| Image | Interact | +| DepthCloud | Measure | First Person | Selections | +| Effort | Select | Third Person Follower | Time | +| Fluid | 2D Nav Goal | Top Down Orthographic | Tool Properties | +| Grid | Publish Point | | Views | +| Grid Cells | Initial Pose | +| Illuminance | Interact | +| Image | | Interactive Marker | | Laser Scan | | Map | @@ -43,12 +44,6 @@ For some displays, the [documentation is updated](docs/FEATURES.md). ### Not yet ported -These features have not been ported to `ros2/rviz` yet. - -| Displays | -| ------------- | -| DepthCloud | - Other features: - Stereo diff --git a/rviz_common/CMakeLists.txt b/rviz_common/CMakeLists.txt index f84013d3d0..98d6ea235a 100644 --- a/rviz_common/CMakeLists.txt +++ b/rviz_common/CMakeLists.txt @@ -137,6 +137,7 @@ set(rviz_common_source_files src/rviz_common/add_display_dialog.cpp src/rviz_common/bit_allocator.cpp src/rviz_common/config.cpp + src/rviz_common/depth_cloud_mld.cpp src/rviz_common/display_factory.cpp src/rviz_common/display_group.cpp src/rviz_common/display.cpp diff --git a/rviz_common/include/rviz_common/depth_cloud_mld.hpp b/rviz_common/include/rviz_common/depth_cloud_mld.hpp new file mode 100644 index 0000000000..d11f6bdd65 --- /dev/null +++ b/rviz_common/include/rviz_common/depth_cloud_mld.hpp @@ -0,0 +1,157 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2023, Open Source Robotics Foundation, 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, Inc. 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 RVIZ_COMMON__DEPTH_CLOUD_MLD_HPP_ +#define RVIZ_COMMON__DEPTH_CLOUD_MLD_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include "rviz_common/ros_integration/ros_node_abstraction_iface.hpp" +#include "rviz_common/visibility_control.hpp" + +// This is necessary because of using stl types here. It is completely safe, because +// a) the member is not accessible from the outside +// b) there are no inline functions. +#ifdef _WIN32 +# pragma warning(push) +# pragma warning(disable:4251) +#endif + +namespace rviz_common +{ +class MultiLayerDepthException : public std::exception +{ +public: + RVIZ_COMMON_PUBLIC + explicit MultiLayerDepthException(const std::string & error_msg) + : std::exception(), error_msg_(error_msg) + { + } + ~MultiLayerDepthException() throw() override + { + } + + RVIZ_COMMON_PUBLIC + const char * what() const throw() override + { + return error_msg_.c_str(); + } + +protected: + std::string error_msg_; +}; + +class RVIZ_COMMON_PUBLIC MultiLayerDepth final +{ +public: + MultiLayerDepth() + : shadow_time_out_(30.0), shadow_distance_(0.01f) {} + + virtual ~MultiLayerDepth() + { + } + + void setShadowTimeOut(double time_out); + + void enableOcclusionCompensation(bool occlusion_compensation); + + sensor_msgs::msg::PointCloud2::SharedPtr + generatePointCloudFromDepth( + const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, + const sensor_msgs::msg::Image::ConstSharedPtr & color_msg, + sensor_msgs::msg::CameraInfo::ConstSharedPtr camera_info_msg, + ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node); + + void reset(); + +private: + /** @brief Precompute projection matrix, initialize buffers */ + void initializeConversion( + const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, + sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info_msg); + + /** @brief Convert color data to RGBA format */ + template + void convertColor( + const sensor_msgs::msg::Image::ConstSharedPtr & color_msg, + std::vector & rgba_color_raw); + + /** @brief Generate single-layered depth cloud (depth only) */ + template + sensor_msgs::msg::PointCloud2::SharedPtr generatePointCloudSL( + const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, + std::vector & rgba_color_raw); + + /** @brief Generate multi-layered depth cloud (depth+shadow) */ + template + sensor_msgs::msg::PointCloud2::SharedPtr generatePointCloudML( + const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, + std::vector & rgba_color_raw, + ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node); + + // Helpers to generate pointcloud2 message + sensor_msgs::msg::PointCloud2::SharedPtr initPointCloud(); + void finalizePointCloud( + sensor_msgs::msg::PointCloud2::SharedPtr & point_cloud, + std::size_t size); + + std::vector projection_map_x_; + std::vector projection_map_y_; + + // shadow buffers + std::vector shadow_depth_; + std::vector shadow_timestamp_; + std::vector shadow_buffer_; + + // configuration + bool occlusion_compensation_; + double shadow_time_out_; + float shadow_distance_; +}; +} // namespace rviz_common + +#ifdef _WIN32 +# pragma warning(pop) +#endif + +#endif // RVIZ_COMMON__DEPTH_CLOUD_MLD_HPP_ diff --git a/rviz_common/include/rviz_common/properties/ros_topic_property.hpp b/rviz_common/include/rviz_common/properties/ros_topic_property.hpp index 9fbd08b1b6..c55303e6de 100644 --- a/rviz_common/include/rviz_common/properties/ros_topic_property.hpp +++ b/rviz_common/include/rviz_common/properties/ros_topic_property.hpp @@ -78,8 +78,35 @@ protected Q_SLOTS: QString message_type_; }; -} // end namespace properties +class RVIZ_COMMON_PUBLIC RosFilteredTopicProperty + : public rviz_common::properties::RosTopicProperty +{ + Q_OBJECT + +public: + RosFilteredTopicProperty( + const QString & name = QString(), + const QString & default_value = QString(), + const QString & message_type = QString(), + const QString & description = QString(), + const QRegExp & filter = QRegExp(), + Property * parent = 0, + const char * changed_slot = 0, + QObject * receiver = 0); + void enableFilter(bool enabled); + + QRegExp filter() const; + +protected Q_SLOTS: + void fillTopicList() override; + +private: + QRegExp filter_; + bool filter_enabled_; +}; + +} // end namespace properties } // end namespace rviz_common #endif // RVIZ_COMMON__PROPERTIES__ROS_TOPIC_PROPERTY_HPP_ diff --git a/rviz_common/src/rviz_common/depth_cloud_mld.cpp b/rviz_common/src/rviz_common/depth_cloud_mld.cpp new file mode 100644 index 0000000000..324c069432 --- /dev/null +++ b/rviz_common/src/rviz_common/depth_cloud_mld.cpp @@ -0,0 +1,601 @@ +/* + * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2023, Open Source Robotics Foundation, 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, Inc. 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 "rviz_common/depth_cloud_mld.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "rviz_common/ros_integration/ros_node_abstraction_iface.hpp" + +constexpr size_t POINT_STEP = (sizeof(float) * 4); + +namespace rviz_common +{ +// Encapsulate differences between processing float and uint16_t depths +template +struct DepthTraits +{ +}; + +template<> +struct DepthTraits +{ + static inline bool valid(float depth) + { + return depth != 0.0; + } + static inline float toMeters(uint16_t depth) + { + return depth * 0.001f; + } +}; + +template<> +struct DepthTraits +{ + static inline bool valid(float depth) + { + return std::isfinite(depth); + } + static inline float toMeters(float depth) + { + return depth; + } +}; + + +struct RGBA +{ + uint8_t red; + uint8_t green; + uint8_t blue; + uint8_t alpha; +}; + +void MultiLayerDepth::setShadowTimeOut(double time_out) +{ + this->shadow_time_out_ = time_out; +} + +void MultiLayerDepth::enableOcclusionCompensation(bool occlusion_compensation) +{ + occlusion_compensation_ = occlusion_compensation; + reset(); +} + +void MultiLayerDepth::reset() +{ + if (occlusion_compensation_) { + // reset shadow buffer + memset(&shadow_depth_[0], 0, sizeof(float) * shadow_depth_.size()); + memset(&shadow_buffer_[0], 0, sizeof(uint8_t) * shadow_buffer_.size()); + memset(&shadow_timestamp_[0], 0, sizeof(double) * shadow_timestamp_.size()); + } +} + + +void MultiLayerDepth::initializeConversion( + const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, + sensor_msgs::msg::CameraInfo::ConstSharedPtr & camera_info_msg) +{ + if (!depth_msg || !camera_info_msg) { + std::string error_msg("Waiting for CameraInfo message.."); + throw(rviz_common::MultiLayerDepthException(error_msg)); + } + + // do some sanity checks + uint32_t binning_x = camera_info_msg->binning_x > 1 ? camera_info_msg->binning_x : 1; + uint32_t binning_y = camera_info_msg->binning_y > 1 ? camera_info_msg->binning_y : 1; + + uint32_t roi_width = + camera_info_msg->roi.width > 0 ? camera_info_msg->roi.width : camera_info_msg->width; + uint32_t roi_height = + camera_info_msg->roi.height > 0 ? camera_info_msg->roi.height : camera_info_msg->height; + + uint32_t expected_width = roi_width / binning_x; + uint32_t expected_height = roi_height / binning_y; + + if (expected_width != depth_msg->width || expected_height != depth_msg->height) { + std::ostringstream s; + s << "Depth image size and camera info don't match: "; + s << depth_msg->width << " x " << depth_msg->height; + s << " vs " << expected_width << " x " << expected_height; + s << "(binning: " << binning_x << " x " << binning_y; + s << ", ROI size: " << roi_width << " x " << roi_height << ")"; + throw(rviz_common::MultiLayerDepthException(s.str())); + } + + uint32_t width = depth_msg->width; + uint32_t height = depth_msg->height; + + uint32_t size = width * height; + + if (size != static_cast(shadow_depth_.size())) { + // Allocate memory for shadow processing + shadow_depth_.resize(size, 0.0f); + shadow_timestamp_.resize(size, 0.0); + shadow_buffer_.resize(size * POINT_STEP, 0); + + // Precompute 3D projection matrix + // + // The following computation of center_x,y and fx,fy duplicates + // code in the image_geometry package, but this avoids a dependency + // on OpenCV, which simplifies releasing rviz. + + // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y) + double scale_x = camera_info_msg->binning_x > 1 ? (1.0 / camera_info_msg->binning_x) : 1.0; + double scale_y = camera_info_msg->binning_y > 1 ? (1.0 / camera_info_msg->binning_y) : 1.0; + + // Use correct principal point from calibration + float center_x = + static_cast((camera_info_msg->p[2] - camera_info_msg->roi.x_offset) * scale_x); + float center_y = + static_cast((camera_info_msg->p[6] - camera_info_msg->roi.y_offset) * scale_y); + + double fx = camera_info_msg->p[0] * scale_x; + double fy = camera_info_msg->p[5] * scale_y; + + float constant_x = static_cast(1.0f / fx); + float constant_y = static_cast(1.0f / fy); + + projection_map_x_.resize(width); + projection_map_y_.resize(height); + std::vector::iterator projX = projection_map_x_.begin(); + std::vector::iterator projY = projection_map_y_.begin(); + + // precompute 3D projection matrix + for (uint32_t v = 0; v < height; ++v, ++projY) { + *projY = (v - center_y) * constant_y; + } + + for (uint32_t u = 0; u < width; ++u, ++projX) { + *projX = (u - center_x) * constant_x; + } + + // reset shadow vectors + reset(); + } +} + + +template +sensor_msgs::msg::PointCloud2::SharedPtr +MultiLayerDepth::generatePointCloudSL( + const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, + std::vector & rgba_color_raw) +{ + sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg = initPointCloud(); + cloud_msg->data.resize(depth_msg->height * depth_msg->width * cloud_msg->point_step); + + uint32_t * color_img_ptr = nullptr; + + if (!rgba_color_raw.empty()) { + color_img_ptr = &rgba_color_raw[0]; + } + + //////////////////////////////////////////////// + // depth map to point cloud conversion + //////////////////////////////////////////////// + + float * cloud_data_ptr = reinterpret_cast(&cloud_msg->data[0]); + + std::size_t point_count = 0; + std::size_t point_idx = 0; + + const T * depth_img_ptr = reinterpret_cast(&depth_msg->data[0]); + + std::vector::iterator proj_x; + std::vector::const_iterator proj_x_end = projection_map_x_.end(); + + std::vector::iterator proj_y; + std::vector::const_iterator proj_y_end = projection_map_y_.end(); + + // iterate over projection matrix + for (proj_y = projection_map_y_.begin(); proj_y != proj_y_end; ++proj_y) { + for (proj_x = projection_map_x_.begin(); proj_x != proj_x_end; + ++proj_x, ++point_idx, ++depth_img_ptr) + { + T depth_raw = *depth_img_ptr; + if (DepthTraits::valid(depth_raw)) { + float depth = DepthTraits::toMeters(depth_raw); + + // define point color + uint32_t color; + if (color_img_ptr) { + color = *color_img_ptr; + } else { + color = ((uint32_t)255 << 16 | (uint32_t)255 << 8 | (uint32_t)255); + } + + // fill in X,Y,Z and color + *cloud_data_ptr = (*proj_x) * depth; + ++cloud_data_ptr; + *cloud_data_ptr = (*proj_y) * depth; + ++cloud_data_ptr; + *cloud_data_ptr = depth; + ++cloud_data_ptr; + *cloud_data_ptr = *reinterpret_cast(&color); + ++cloud_data_ptr; + + ++point_count; + } + + // increase color iterator pointer + if (color_img_ptr) { + ++color_img_ptr; + } + } + } + + finalizePointCloud(cloud_msg, point_count); + + return cloud_msg; +} + + +template +sensor_msgs::msg::PointCloud2::SharedPtr +MultiLayerDepth::generatePointCloudML( + const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, + std::vector & rgba_color_raw, + ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node) +{ + sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg = initPointCloud(); + cloud_msg->data.resize(depth_msg->height * depth_msg->width * cloud_msg->point_step * 2); + + uint32_t * color_img_ptr = nullptr; + + if (!rgba_color_raw.empty()) { + color_img_ptr = &rgba_color_raw[0]; + } + + //////////////////////////////////////////////// + // depth map to point cloud conversion + //////////////////////////////////////////////// + + float * cloud_data_ptr = reinterpret_cast(&cloud_msg->data[0]); + uint8_t * cloud_shadow_buffer_ptr = &shadow_buffer_[0]; + + const std::size_t point_step = cloud_msg->point_step; + + std::size_t point_count = 0; + std::size_t point_idx = 0; + + double time_now = rviz_ros_node.lock()->get_raw_node()->now().seconds(); + double time_expire = time_now - shadow_time_out_; + + const T * depth_img_ptr = reinterpret_cast(&depth_msg->data[0]); + + std::vector::iterator proj_x; + std::vector::const_iterator proj_x_end = projection_map_x_.end(); + + std::vector::iterator proj_y; + std::vector::const_iterator proj_y_end = projection_map_y_.end(); + + // iterate over projection matrix + for (proj_y = projection_map_y_.begin(); proj_y != proj_y_end; ++proj_y) { + for (proj_x = projection_map_x_.begin(); proj_x != proj_x_end; + ++proj_x, ++point_idx, ++depth_img_ptr, cloud_shadow_buffer_ptr += point_step) + { + // lookup shadow depth + float shadow_depth = shadow_depth_[point_idx]; + + // check for time-outs + if ((shadow_depth != 0.0f) && (shadow_timestamp_[point_idx] < time_expire)) { + // clear shadow pixel + shadow_depth = shadow_depth_[point_idx] = 0.0f; + } + + T depth_raw = *depth_img_ptr; + if (DepthTraits::valid(depth_raw)) { + float depth = DepthTraits::toMeters(depth_raw); + + // pointer to current point data + float * cloud_data_pixel_ptr = cloud_data_ptr; + + // define point color + uint32_t color; + if (color_img_ptr) { + color = *color_img_ptr; + } else { + color = ((uint32_t)255 << 16 | (uint32_t)255 << 8 | (uint32_t)255); + } + + // fill in X,Y,Z and color + *cloud_data_ptr = (*proj_x) * depth; + ++cloud_data_ptr; + *cloud_data_ptr = (*proj_y) * depth; + ++cloud_data_ptr; + *cloud_data_ptr = depth; + ++cloud_data_ptr; + *cloud_data_ptr = *reinterpret_cast(&color); + ++cloud_data_ptr; + + ++point_count; + + // if shadow point exists -> display it + if (depth < shadow_depth - shadow_distance_) { + // copy point data from shadow buffer to point cloud + memcpy(cloud_data_ptr, cloud_shadow_buffer_ptr, point_step); + cloud_data_ptr += 4; + ++point_count; + } else { + // save a copy of current point to shadow buffer + memcpy(cloud_shadow_buffer_ptr, cloud_data_pixel_ptr, point_step); + + // reduce color intensity in shadow buffer + RGBA * color = reinterpret_cast(cloud_shadow_buffer_ptr + sizeof(float) * 3); + color->red /= 2; + color->green /= 2; + color->blue /= 2; + + // update shadow depth & time out + shadow_depth_[point_idx] = depth; + shadow_timestamp_[point_idx] = time_now; + } + } else { + // current depth pixel is invalid -> check shadow buffer + if (shadow_depth != 0) { + // copy shadow point to point cloud + memcpy(cloud_data_ptr, cloud_shadow_buffer_ptr, point_step); + cloud_data_ptr += 4; + ++point_count; + } + } + + // increase color iterator pointer + if (color_img_ptr) { + ++color_img_ptr; + } + } + } + + finalizePointCloud(cloud_msg, point_count); + + return cloud_msg; +} + + +template +void MultiLayerDepth::convertColor( + const sensor_msgs::msg::Image::ConstSharedPtr & color_msg, + std::vector & rgba_color_raw) +{ + if (color_msg->encoding.find("rgb") == std::string::npos && + color_msg->encoding.find("bgr") == std::string::npos) + { + throw rviz_common::MultiLayerDepthException("Encoded type not supported!"); + } + + size_t i; + size_t num_pixel = color_msg->width * color_msg->height; + + // query image properties + int num_channels = sensor_msgs::image_encodings::numChannels(color_msg->encoding); + + bool rgb_encoding = false; + if (color_msg->encoding.find("rgb") != std::string::npos) { + rgb_encoding = true; + } + + bool has_alpha = sensor_msgs::image_encodings::hasAlpha(color_msg->encoding); + + // prepare output vector + rgba_color_raw.clear(); + rgba_color_raw.reserve(num_pixel); + + // pointer to most significant byte + const uint8_t * img_ptr = reinterpret_cast(&color_msg->data[sizeof(T) - 1]); + + // color conversion + switch (num_channels) { + case 1: + // grayscale image + for (i = 0; i < num_pixel; ++i) { + uint8_t gray_value = *img_ptr; + img_ptr += sizeof(T); + + rgba_color_raw.push_back( + (uint32_t)gray_value << 16 | (uint32_t)gray_value << 8 | + (uint32_t)gray_value); + } + break; + case 3: + case 4: + // rgb/bgr encoding + for (i = 0; i < num_pixel; ++i) { + uint8_t color1 = *(reinterpret_cast(img_ptr)); + img_ptr += sizeof(T); + uint8_t color2 = *(reinterpret_cast(img_ptr)); + img_ptr += sizeof(T); + uint8_t color3 = *(reinterpret_cast(img_ptr)); + img_ptr += sizeof(T); + + if (has_alpha) { + img_ptr += sizeof(T); // skip alpha values + } + if (rgb_encoding) { + // rgb encoding + rgba_color_raw.push_back( + (uint32_t)color1 << 16 | (uint32_t)color2 << 8 | (uint32_t)color3 << 0); + } else { + // bgr encoding + rgba_color_raw.push_back( + (uint32_t)color3 << 16 | (uint32_t)color2 << 8 | (uint32_t)color1 << 0); + } + } + break; + default: + break; + } +} + +sensor_msgs::msg::PointCloud2::SharedPtr +MultiLayerDepth::generatePointCloudFromDepth( + const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, + const sensor_msgs::msg::Image::ConstSharedPtr & color_msg, + sensor_msgs::msg::CameraInfo::ConstSharedPtr camera_info_msg, + ros_integration::RosNodeAbstractionIface::WeakPtr rviz_ros_node) +{ + if (!camera_info_msg) { + throw rviz_common::MultiLayerDepthException("Camera info missing!"); + } + + // Add data to multi depth image + sensor_msgs::msg::PointCloud2::SharedPtr point_cloud_out; + + // Bit depth of image encoding + int bitDepth = sensor_msgs::image_encodings::bitDepth(depth_msg->encoding); + int numChannels = sensor_msgs::image_encodings::numChannels(depth_msg->encoding); + + // precompute projection matrix and initialize shadow buffer + initializeConversion(depth_msg, camera_info_msg); + + std::vector rgba_color_raw_; + + if (color_msg) { + if (depth_msg->width != color_msg->width || depth_msg->height != color_msg->height) { + std::stringstream error_msg; + error_msg << "Depth image resolution (" << static_cast(depth_msg->width) << "x" << + static_cast(depth_msg->height) + << ") " + "does not match color image resolution (" + << static_cast(color_msg->width) << "x" + << static_cast(color_msg->height) << ")"; + throw(rviz_common::MultiLayerDepthException(error_msg.str())); + } + + // convert color coding to 8-bit rgb data + switch (sensor_msgs::image_encodings::bitDepth(color_msg->encoding)) { + case 8: + convertColor(color_msg, rgba_color_raw_); + break; + case 16: + convertColor(color_msg, rgba_color_raw_); + break; + default: + std::string error_msg("Color image has invalid bit depth"); + throw(rviz_common::MultiLayerDepthException(error_msg)); + break; + } + } + + if (!occlusion_compensation_) { + // generate single layer depth cloud + + if ((bitDepth == 32) && (numChannels == 1)) { + // floating point encoded depth map + point_cloud_out = generatePointCloudSL(depth_msg, rgba_color_raw_); + } else if ((bitDepth == 16) && (numChannels == 1)) { + // 32bit integer encoded depth map + point_cloud_out = generatePointCloudSL(depth_msg, rgba_color_raw_); + } + } else { + // generate two layered depth cloud (depth+shadow) + + if ((bitDepth == 32) && (numChannels == 1)) { + // floating point encoded depth map + point_cloud_out = generatePointCloudML(depth_msg, rgba_color_raw_, rviz_ros_node); + } else if ((bitDepth == 16) && (numChannels == 1)) { + // 32bit integer encoded depth map + point_cloud_out = generatePointCloudML(depth_msg, rgba_color_raw_, rviz_ros_node); + } + } + + if (!point_cloud_out) { + std::string error_msg("Depth image has invalid format (only 16 bit and float are supported)!"); + throw(rviz_common::MultiLayerDepthException(error_msg)); + } + + return point_cloud_out; +} + +sensor_msgs::msg::PointCloud2::SharedPtr MultiLayerDepth::initPointCloud() +{ + sensor_msgs::msg::PointCloud2::SharedPtr point_cloud_out = + std::make_shared(); + + point_cloud_out->fields.resize(4); + std::size_t point_offset = 0; + + point_cloud_out->fields[0].name = "x"; + point_cloud_out->fields[0].datatype = sensor_msgs::msg::PointField::FLOAT32; + point_cloud_out->fields[0].count = 1; + point_cloud_out->fields[0].offset = static_cast(point_offset); + point_offset += sizeof(float); + + point_cloud_out->fields[1].name = "y"; + point_cloud_out->fields[1].datatype = sensor_msgs::msg::PointField::FLOAT32; + point_cloud_out->fields[1].count = 1; + point_cloud_out->fields[1].offset = static_cast(point_offset); + point_offset += sizeof(float); + + point_cloud_out->fields[2].name = "z"; + point_cloud_out->fields[2].datatype = sensor_msgs::msg::PointField::FLOAT32; + point_cloud_out->fields[2].count = 1; + point_cloud_out->fields[2].offset = static_cast(point_offset); + point_offset += sizeof(float); + + point_cloud_out->fields[3].name = "rgb"; + point_cloud_out->fields[3].datatype = sensor_msgs::msg::PointField::FLOAT32; + point_cloud_out->fields[3].count = 1; + point_cloud_out->fields[3].offset = static_cast(point_offset); + point_offset += sizeof(float); + + point_cloud_out->point_step = static_cast(point_offset); + + point_cloud_out->is_bigendian = false; + point_cloud_out->is_dense = false; + + return point_cloud_out; +} + +void MultiLayerDepth::finalizePointCloud( + sensor_msgs::msg::PointCloud2::SharedPtr & point_cloud, + std::size_t size) +{ + point_cloud->width = static_cast(size); + point_cloud->height = static_cast(1); + point_cloud->data.resize(point_cloud->height * point_cloud->width * point_cloud->point_step); + point_cloud->row_step = point_cloud->point_step * point_cloud->width; +} + +} // namespace rviz_common diff --git a/rviz_common/src/rviz_common/properties/ros_topic_property.cpp b/rviz_common/src/rviz_common/properties/ros_topic_property.cpp index ce5deefc54..6a1fab3966 100644 --- a/rviz_common/src/rviz_common/properties/ros_topic_property.cpp +++ b/rviz_common/src/rviz_common/properties/ros_topic_property.cpp @@ -89,6 +89,42 @@ void RosTopicProperty::fillTopicList() QApplication::restoreOverrideCursor(); } -} // end namespace properties +RosFilteredTopicProperty::RosFilteredTopicProperty( + const QString & name, + const QString & default_value, + const QString & message_type, + const QString & description, + const QRegExp & filter, + Property * parent, + const char * changed_slot, + QObject * receiver) +: RosTopicProperty(name, default_value, message_type, description, parent, changed_slot, receiver) + , filter_(filter) + , filter_enabled_(true) +{ +} +void RosFilteredTopicProperty::enableFilter(bool enabled) +{ + filter_enabled_ = enabled; + fillTopicList(); +} + +QRegExp RosFilteredTopicProperty::filter() const +{ + return filter_; +} + +void RosFilteredTopicProperty::fillTopicList() +{ + QStringList filtered_strings_; + + // Obtain list of available topics + RosTopicProperty::fillTopicList(); + // Apply filter + if (filter_enabled_) { + strings_ = strings_.filter(filter_); + } +} +} // end namespace properties } // end namespace rviz_common diff --git a/rviz_default_plugins/CMakeLists.txt b/rviz_default_plugins/CMakeLists.txt index c51e5018dd..9bcde5de95 100644 --- a/rviz_default_plugins/CMakeLists.txt +++ b/rviz_default_plugins/CMakeLists.txt @@ -81,6 +81,7 @@ set(rviz_default_plugins_headers_to_moc include/rviz_default_plugins/displays/accel/accel_display.hpp include/rviz_default_plugins/displays/axes/axes_display.hpp include/rviz_default_plugins/displays/camera/camera_display.hpp + include/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.hpp include/rviz_default_plugins/displays/effort/effort_display.hpp include/rviz_default_plugins/displays/fluid_pressure/fluid_pressure_display.hpp include/rviz_default_plugins/displays/grid/grid_display.hpp @@ -138,6 +139,7 @@ set(rviz_default_plugins_source_files src/rviz_default_plugins/displays/accel/accel_display.cpp src/rviz_default_plugins/displays/axes/axes_display.cpp src/rviz_default_plugins/displays/camera/camera_display.cpp + src/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.cpp src/rviz_default_plugins/displays/effort/effort_display.cpp src/rviz_default_plugins/displays/grid/grid_display.cpp src/rviz_default_plugins/displays/grid_cells/grid_cells_display.cpp @@ -713,6 +715,19 @@ if(BUILD_TESTING) rviz_visual_testing_framework::rviz_visual_testing_framework) endif() + ament_add_gtest(depth_cloud_display_visual_test + test/rviz_default_plugins/displays/depth_cloud/depth_cloud_display_visual_test.cpp + test/rviz_default_plugins/page_objects/depth_cloud_page_object.cpp + test/rviz_default_plugins/publishers/camera_info_publisher.hpp + test/rviz_default_plugins/publishers/image_publisher.hpp + ${SKIP_VISUAL_TESTS} + TIMEOUT 180) + if(TARGET depth_cloud_display_visual_test) + target_include_directories(depth_cloud_display_visual_test PRIVATE test) + target_link_libraries(depth_cloud_display_visual_test + rviz_visual_testing_framework::rviz_visual_testing_framework) + endif() + ament_add_gtest(effort_display_visual_test test/rviz_default_plugins/displays/effort/effort_display_visual_test.cpp test/rviz_default_plugins/publishers/effort_publisher.hpp diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.hpp new file mode 100644 index 0000000000..5ae8a2f53d --- /dev/null +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/depth_cloud/depth_cloud_display.hpp @@ -0,0 +1,186 @@ +/* + * Copyright (c) 2009, 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, Inc. 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 RVIZ_DEFAULT_PLUGINS__DISPLAYS__DEPTH_CLOUD__DEPTH_CLOUD_DISPLAY_HPP_ +#define RVIZ_DEFAULT_PLUGINS__DISPLAYS__DEPTH_CLOUD__DEPTH_CLOUD_DISPLAY_HPP_ + +#ifndef Q_MOC_RUN +#include // NOLINT: cpplint cannot handle the include order here +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#endif + +#include "rviz_default_plugins/visibility_control.hpp" + +#include // NOLINT: cpplint cannot handle the include order here +#include // NOLINT: cpplint cannot handle the include order here + +namespace rviz_default_plugins +{ + +namespace displays +{ +/** + * \class DepthCloudDisplay + * + */ +class RVIZ_DEFAULT_PLUGINS_PUBLIC DepthCloudDisplay : public rviz_common::Display +{ + Q_OBJECT + +public: + DepthCloudDisplay(); + ~DepthCloudDisplay() override; + + void onInitialize() override; + + // Overrides from Display + void update(float wall_dt, float ros_dt) override; + void reset() override; + void setTopic(const QString & topic, const QString & datatype) override; + + void processDepthMessage(const sensor_msgs::msg::Image::ConstSharedPtr msg); + void processMessage( + const sensor_msgs::msg::Image::ConstSharedPtr depth_msg, + const sensor_msgs::msg::Image::ConstSharedPtr rgb_msg); + +protected Q_SLOTS: + void updateQueueSize(); + /** @brief Fill list of available and working transport options */ + void fillTransportOptionList(rviz_common::properties::EnumProperty * property); + + // Property callbacks + virtual void updateTopic(); + virtual void updateTopicFilter(); + virtual void updateUseAutoSize(); + virtual void updateAutoSizeFactor(); + virtual void updateUseOcclusionCompensation(); + virtual void updateOcclusionTimeOut(); + +protected: + void scanForTransportSubscriberPlugins(); + + void caminfoCallback(sensor_msgs::msg::CameraInfo::ConstSharedPtr msg); + + // overrides from Display + void onEnable() override; + void onDisable() override; + + void fixedFrameChanged() override; + + void subscribe(); + void unsubscribe(); + + void clear(); + + // thread-safe status updates + // add status update to global status list + void updateStatus( + rviz_common::properties::StatusProperty::Level level, + const QString & name, const QString & text); + + // use global status list to update rviz plugin status + void setStatusList(); + + uint32_t messages_received_; + + // ROS image subscription & synchronization + std::unique_ptr depthmap_it_; + std::shared_ptr depthmap_sub_; + std::shared_ptr> depthmap_tf_filter_; + std::unique_ptr rgb_it_; + std::shared_ptr rgb_sub_; + rclcpp::Subscription::SharedPtr cam_info_sub_; + sensor_msgs::msg::CameraInfo::ConstSharedPtr cam_info_; + std::mutex cam_info_mutex_; + + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::msg::Image, sensor_msgs::msg::Image> SyncPolicyDepthColor; + typedef message_filters::Synchronizer SynchronizerDepthColor; + + std::shared_ptr sync_depth_color_; + + // RVIZ properties + rviz_common::properties::Property * topic_filter_property_; + rviz_common::properties::IntProperty * queue_size_property_; + rviz_common::properties::BoolProperty * use_auto_size_property_; + rviz_common::properties::FloatProperty * auto_size_factor_property_; + rviz_common::properties::RosFilteredTopicProperty * depth_topic_property_; + rviz_common::properties::EnumProperty * depth_transport_property_; + rviz_common::properties::RosFilteredTopicProperty * color_topic_property_; + rviz_common::properties::EnumProperty * color_transport_property_; + rviz_common::properties::BoolProperty * use_occlusion_compensation_property_; + rviz_common::properties::FloatProperty * occlusion_shadow_timeout_property_; + + uint32_t queue_size_; + + std::unique_ptr ml_depth_data_; + + Ogre::Quaternion current_orientation_; + Ogre::Vector3 current_position_; + float angular_thres_; + float trans_thres_; + + std::unique_ptr pointcloud_common_; + + std::set transport_plugin_types_; +}; +} // namespace displays +} // namespace rviz_default_plugins +#endif // RVIZ_DEFAULT_PLUGINS__DISPLAYS__DEPTH_CLOUD__DEPTH_CLOUD_DISPLAY_HPP_ diff --git a/rviz_default_plugins/include/rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp b/rviz_default_plugins/include/rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp index 1b035452ad..77c5c6a11e 100644 --- a/rviz_default_plugins/include/rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp +++ b/rviz_default_plugins/include/rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp @@ -162,8 +162,7 @@ class RVIZ_DEFAULT_PLUGINS_PUBLIC PointCloudCommon : public QObject rviz_common::properties::EnumProperty * style_property_; rviz_common::properties::FloatProperty * decay_time_property_; -// TODO(anhosi): check if still needed when migrating DepthCloud -// void setAutoSize(bool auto_size); + void setAutoSize(bool auto_size); public Q_SLOTS: void causeRetransform(); diff --git a/rviz_default_plugins/plugins_description.xml b/rviz_default_plugins/plugins_description.xml index b03e0e9510..8bebf818c5 100644 --- a/rviz_default_plugins/plugins_description.xml +++ b/rviz_default_plugins/plugins_description.xml @@ -35,6 +35,17 @@ sensor_msgs/msg/CompressedImage + + + Displays point clouds based on depth maps. + + sensor_msgs/msg/Image + + +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include // NOLINT: cpplint cannot handle the include order here +#include + +#include + +#include + +#include + +#include +#include + +namespace rviz_default_plugins +{ +namespace displays +{ +DepthCloudDisplay::DepthCloudDisplay() +: rviz_common::Display() + , messages_received_(0) + , depthmap_sub_() + , rgb_sub_() + , cam_info_sub_() + , queue_size_(5) + , angular_thres_(0.5f) + , trans_thres_(0.01f) +{ + ml_depth_data_ = std::make_unique(); + // Depth map properties + QRegExp depth_filter("depth"); + depth_filter.setCaseSensitivity(Qt::CaseInsensitive); + + topic_filter_property_ = + new rviz_common::properties::Property( + "Topic Filter", true, + "List only topics with names that relate to depth and color images", this, + SLOT(updateTopicFilter())); + + depth_topic_property_ = new rviz_common::properties::RosFilteredTopicProperty( + "Depth Map Topic", "", "sensor_msgs/msg/Image", + "sensor_msgs::msg::Image topic to subscribe to.", depth_filter, this, SLOT(updateTopic())); + + depth_transport_property_ = new rviz_common::properties::EnumProperty( + "Depth Map Transport Hint", "raw", "Preferred method of sending images.", + this, SLOT(updateTopic())); + + QObject::connect( + depth_transport_property_, + SIGNAL(requestOptions(rviz_common::properties::EnumProperty*)), + this, + SLOT(fillTransportOptionList(rviz_common::properties::EnumProperty*))); + + depth_transport_property_->setStdString("raw"); + + // color image properties + QRegExp color_filter("color|rgb|bgr|gray|mono"); + color_filter.setCaseSensitivity(Qt::CaseInsensitive); + + color_topic_property_ = new rviz_common::properties::RosFilteredTopicProperty( + "Color Image Topic", "", + "sensor_msgs/msg/Image", + "sensor_msgs::msg::Image topic to subscribe to.", color_filter, this, SLOT(updateTopic())); + + color_transport_property_ = new rviz_common::properties::EnumProperty( + "Color Transport Hint", "raw", "Preferred method of sending images.", this, + SLOT(updateTopic())); + + QObject::connect( + color_transport_property_, + SIGNAL(requestOptions(rviz_common::properties::EnumProperty*)), + this, + SLOT(fillTransportOptionList(rviz_common::properties::EnumProperty*))); + + color_transport_property_->setStdString("raw"); + + // Queue size property + queue_size_property_ = + new rviz_common::properties::IntProperty( + "Queue Size", queue_size_, + "Advanced: set the size of the incoming message queue. Increasing this " + "is useful if your incoming TF data is delayed significantly from your" + " image data, but it can greatly increase memory usage if the messages " + "are big.", + this, SLOT(updateQueueSize())); + queue_size_property_->setMin(1); + + use_auto_size_property_ = new rviz_common::properties::BoolProperty( + "Auto Size", true, + "Automatically scale each point based on its depth value and the camera parameters.", this, + SLOT(updateUseAutoSize())); + + auto_size_factor_property_ = + new rviz_common::properties::FloatProperty( + "Auto Size Factor", 1, "Scaling factor to be applied to the auto size.", + use_auto_size_property_, SLOT(updateAutoSizeFactor()), this); + auto_size_factor_property_->setMin(0.0001f); + + use_occlusion_compensation_property_ = + new rviz_common::properties::BoolProperty( + "Occlusion Compensation", false, + "Keep points alive after they have been occluded by a closer point. " + "Points are removed after a timeout or when the camera frame moves.", + this, SLOT(updateUseOcclusionCompensation())); + + occlusion_shadow_timeout_property_ = new rviz_common::properties::FloatProperty( + "Occlusion Time-Out", 30.0f, + "Amount of seconds before removing occluded points from the depth cloud", + use_occlusion_compensation_property_, SLOT(updateOcclusionTimeOut()), this); +} + +void DepthCloudDisplay::onInitialize() +{ + auto rviz_ros_node_ = context_->getRosNodeAbstraction().lock(); + + depthmap_it_ = std::make_unique( + rviz_ros_node_->get_raw_node()); + rgb_it_ = std::make_unique( + rviz_ros_node_->get_raw_node()); + + // Instantiate PointCloudCommon class for displaying point clouds + pointcloud_common_ = std::make_unique(this); + + updateUseAutoSize(); + updateUseOcclusionCompensation(); + + // Scan for available transport plugins + scanForTransportSubscriberPlugins(); + + pointcloud_common_->initialize(context_, scene_node_); + pointcloud_common_->xyz_transformer_property_->hide(); + + depth_topic_property_->initialize(rviz_ros_node_); + color_topic_property_->initialize(rviz_ros_node_); +} + +DepthCloudDisplay::~DepthCloudDisplay() +{ + if (initialized()) { + unsubscribe(); + pointcloud_common_.reset(); + } +} + +void DepthCloudDisplay::setTopic(const QString & topic, const QString & datatype) +{ + if (datatype == "sensor_msgs::msgs::Image") { + depth_transport_property_->setStdString("raw"); + depth_topic_property_->setString(topic); + } else { + int index = topic.lastIndexOf("/"); + if (index == -1) { + return; + } + QString transport = topic.mid(index + 1); + QString base_topic = topic.mid(0, index); + + depth_transport_property_->setString(transport); + depth_topic_property_->setString(base_topic); + } +} + +void DepthCloudDisplay::updateQueueSize() +{ + queue_size_ = queue_size_property_->getInt(); +} + +void DepthCloudDisplay::updateUseAutoSize() +{ + bool use_auto_size = use_auto_size_property_->getBool(); + pointcloud_common_->point_world_size_property_->setReadOnly(use_auto_size); + pointcloud_common_->setAutoSize(use_auto_size); + auto_size_factor_property_->setHidden(!use_auto_size); + if (use_auto_size) { + use_auto_size_property_->expand(); + } +} + +void DepthCloudDisplay::updateAutoSizeFactor() +{ +} + +void DepthCloudDisplay::updateTopicFilter() +{ + bool enabled = topic_filter_property_->getValue().toBool(); + depth_topic_property_->enableFilter(enabled); + color_topic_property_->enableFilter(enabled); +} + +void DepthCloudDisplay::updateUseOcclusionCompensation() +{ + bool use_occlusion_compensation = use_occlusion_compensation_property_->getBool(); + occlusion_shadow_timeout_property_->setHidden(!use_occlusion_compensation); + + if (use_occlusion_compensation) { + updateOcclusionTimeOut(); + ml_depth_data_->enableOcclusionCompensation(true); + use_occlusion_compensation_property_->expand(); + } else { + ml_depth_data_->enableOcclusionCompensation(false); + } +} + +void DepthCloudDisplay::updateOcclusionTimeOut() +{ + float occlusion_timeout = occlusion_shadow_timeout_property_->getFloat(); + ml_depth_data_->setShadowTimeOut(occlusion_timeout); +} + +void DepthCloudDisplay::onEnable() +{ + subscribe(); +} + +void DepthCloudDisplay::onDisable() +{ + unsubscribe(); + + ml_depth_data_->reset(); + + clear(); +} + +void DepthCloudDisplay::subscribe() +{ + if (!isEnabled()) { + return; + } + + try { + // reset all message filters + sync_depth_color_ = std::make_shared( + SyncPolicyDepthColor(queue_size_)); + depthmap_tf_filter_.reset(); + depthmap_sub_ = std::make_shared(); + rgb_sub_ = std::make_shared(); + cam_info_sub_.reset(); + + std::string depthmap_topic = depth_topic_property_->getTopicStd(); + std::string color_topic = color_topic_property_->getTopicStd(); + + std::string depthmap_transport = depth_transport_property_->getStdString(); + std::string color_transport = color_transport_property_->getStdString(); + + auto rviz_ros_node_ = context_->getRosNodeAbstraction().lock(); + + if (!depthmap_topic.empty() && !depthmap_transport.empty()) { + // subscribe to depth map topic + depthmap_sub_->subscribe( + rviz_ros_node_->get_raw_node().get(), + depthmap_topic, + depthmap_transport); + + depthmap_tf_filter_ = + std::make_shared>( + *context_->getFrameManager()->getTransformer(), + fixed_frame_.toStdString(), + 10, + rviz_ros_node_->get_raw_node()); + + depthmap_tf_filter_->connectInput(*depthmap_sub_); + + // subscribe to CameraInfo topic + std::string info_topic = image_transport::getCameraInfoTopic(depthmap_topic); + + rclcpp::SubscriptionOptions sub_opts; + sub_opts.event_callbacks.message_lost_callback = + [&](rclcpp::QOSMessageLostInfo & info) + { + std::ostringstream sstm; + sstm << "Some messages were lost:\n>\tNumber of new lost messages: " << + info.total_count_change << " \n>\tTotal number of messages lost: " << + info.total_count; + setStatus( + rviz_common::properties::StatusProperty::Warn, + "Depth Camera Info", + QString(sstm.str().c_str())); + }; + + cam_info_sub_ = rviz_ros_node_->get_raw_node()-> + create_subscription( + info_topic, + rclcpp::SensorDataQoS(), + [this](sensor_msgs::msg::CameraInfo::ConstSharedPtr msg) { + std::lock_guard lock(cam_info_mutex_); + cam_info_ = msg; + }, sub_opts); + + if (!color_topic.empty() && !color_transport.empty()) { + // subscribe to color image topic + rgb_sub_->subscribe( + rviz_ros_node_->get_raw_node().get(), + color_topic, color_transport, rclcpp::SensorDataQoS().get_rmw_qos_profile()); + + // connect message filters to synchronizer + sync_depth_color_->connectInput(*depthmap_tf_filter_, *rgb_sub_); + sync_depth_color_->setInterMessageLowerBound(0, rclcpp::Duration(0, 0.5 * 1e+9)); + sync_depth_color_->setInterMessageLowerBound(1, rclcpp::Duration(0, 0.5 * 1e+9)); + sync_depth_color_->registerCallback( + std::bind( + &DepthCloudDisplay::processMessage, this, + std::placeholders::_1, std::placeholders::_2)); + + pointcloud_common_->color_transformer_property_->setValue("RGB8"); + } else { + depthmap_tf_filter_->registerCallback( + std::bind(&DepthCloudDisplay::processDepthMessage, this, std::placeholders::_1)); + } + } + } catch (const image_transport::TransportLoadException & e) { + setStatus( + rviz_common::properties::StatusProperty::Error, "Message", + QString("Error subscribing: ") + e.what()); + } +} + +void DepthCloudDisplay::caminfoCallback(sensor_msgs::msg::CameraInfo::ConstSharedPtr msg) +{ + std::lock_guard lock(cam_info_mutex_); + cam_info_ = std::move(msg); +} + +void DepthCloudDisplay::unsubscribe() +{ + clear(); + + sync_depth_color_.reset(new SynchronizerDepthColor(SyncPolicyDepthColor(queue_size_))); + depthmap_tf_filter_.reset(); + depthmap_sub_.reset(); + rgb_sub_.reset(); + cam_info_sub_.reset(); +} + +void DepthCloudDisplay::clear() +{ + pointcloud_common_->reset(); +} + +void DepthCloudDisplay::update(float wall_dt, float ros_dt) +{ + pointcloud_common_->update(wall_dt, ros_dt); +} + +void DepthCloudDisplay::reset() +{ + clear(); + messages_received_ = 0; + setStatus(rviz_common::properties::StatusProperty::Ok, "Depth Map", "0 depth maps received"); + setStatus(rviz_common::properties::StatusProperty::Ok, "Message", "Ok"); +} + +void DepthCloudDisplay::processDepthMessage(const sensor_msgs::msg::Image::ConstSharedPtr depth_msg) +{ + processMessage(depth_msg, sensor_msgs::msg::Image::ConstSharedPtr()); +} + +void DepthCloudDisplay::processMessage( + const sensor_msgs::msg::Image::ConstSharedPtr depth_msg, + const sensor_msgs::msg::Image::ConstSharedPtr rgb_msg) +{ + if (context_->getFrameManager()->getPause()) { + return; + } + + std::ostringstream s; + + ++messages_received_; + setStatus( + rviz_common::properties::StatusProperty::Ok, "Depth Map", + QString::number(messages_received_) + " depth maps received"); + setStatus(rviz_common::properties::StatusProperty::Ok, "Message", "Ok"); + + sensor_msgs::msg::CameraInfo::ConstSharedPtr cam_info; + { + std::lock_guard lock(cam_info_mutex_); + cam_info = cam_info_; + } + + if (!cam_info || !depth_msg) { + return; + } + + s.str(""); + s << depth_msg->width << " x " << depth_msg->height; + setStatusStd(rviz_common::properties::StatusProperty::Ok, "Depth Image Size", s.str()); + + if (rgb_msg) { + s.str(""); + s << rgb_msg->width << " x " << rgb_msg->height; + setStatusStd(rviz_common::properties::StatusProperty::Ok, "Image Size", s.str()); + + if (depth_msg->header.frame_id != rgb_msg->header.frame_id) { + std::stringstream errorMsg; + errorMsg << "Depth image frame id [" << depth_msg->header.frame_id.c_str() + << "] doesn't match color image frame id [" + << rgb_msg->header.frame_id.c_str() << "]"; + setStatusStd(rviz_common::properties::StatusProperty::Warn, "Message", errorMsg.str()); + } + } + + if (use_auto_size_property_->getBool()) { + float f = cam_info->k[0]; + float bx = cam_info->binning_x > 0 ? cam_info->binning_x : 1.0; + float s = auto_size_factor_property_->getFloat(); + pointcloud_common_->point_world_size_property_->setFloat(s / f * bx); + } + + bool use_occlusion_compensation = use_occlusion_compensation_property_->getBool(); + + if (use_occlusion_compensation) { + // reset depth cloud display if camera moves + Ogre::Quaternion orientation; + Ogre::Vector3 position; + + if (!context_->getFrameManager()->getTransform(depth_msg->header, position, orientation)) { + setStatus( + rviz_common::properties::StatusProperty::Error, "Message", + QString("Failed to transform from frame [") + depth_msg->header.frame_id.c_str() + + QString("] to frame [") + context_->getFrameManager()->getFixedFrame().c_str() + + QString("]")); + return; + } else { + Ogre::Radian angle; + Ogre::Vector3 axis; + + (current_orientation_.Inverse() * orientation).ToAngleAxis(angle, axis); + + float angle_deg = angle.valueDegrees(); + if (angle_deg >= 180.0f) { + angle_deg -= 180.0f; + } + if (angle_deg < -180.0f) { + angle_deg += 180.0f; + } + + if (trans_thres_ == 0.0 || angular_thres_ == 0.0 || + (position - current_position_).length() > trans_thres_ || angle_deg > angular_thres_) + { + // camera orientation/position changed + current_position_ = position; + current_orientation_ = orientation; + + // reset multi-layered depth image + ml_depth_data_->reset(); + } + } + } + + try { + auto rviz_ros_node_ = context_->getRosNodeAbstraction().lock(); + + sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg = + ml_depth_data_->generatePointCloudFromDepth(depth_msg, rgb_msg, cam_info, rviz_ros_node_); + + if (!cloud_msg.get()) { + std::ostringstream sstm; + sstm << "generatePointCloudFromDepth() returned zero."; + setStatus( + rviz_common::properties::StatusProperty::Warn, + "Depth Camera Info", + QString(sstm.str().c_str())); + } + cloud_msg->header = depth_msg->header; + + // add point cloud message to pointcloud_common to be visualized + pointcloud_common_->addMessage(cloud_msg); + } catch (rviz_common::MultiLayerDepthException & e) { + setStatus( + rviz_common::properties::StatusProperty::Error, "Message", + QString("Error updating depth cloud: ") + e.what()); + } +} + + +void DepthCloudDisplay::scanForTransportSubscriberPlugins() +{ + pluginlib::ClassLoader sub_loader( + "image_transport", "image_transport::SubscriberPlugin"); + + std::vector lookup_names = sub_loader.getDeclaredClasses(); + for (const auto & lookup_name : lookup_names) { + // lookup_name is formatted as "pkg/transport_sub", for instance + // "image_transport/compressed_sub" for the "compressed" + // transport. This code removes the "_sub" from the tail and + // everything up to and including the "/" from the head, leaving + // "compressed" (for example) in transport_name. + std::string transport_name = image_transport::erase_last_copy(lookup_name, "_sub"); + transport_name = transport_name.substr(lookup_name.find('/') + 1); + + // If the plugin loads without throwing an exception, add its + // transport name to the list of valid plugins, otherwise ignore + // it. + try { + std::shared_ptr sub = + sub_loader.createSharedInstance(lookup_name); + transport_plugin_types_.insert(transport_name); + } catch (const pluginlib::LibraryLoadException & /*e*/) { + } catch (const pluginlib::CreateClassException & /*e*/) { + } + } +} + +void DepthCloudDisplay::updateTopic() +{ + unsubscribe(); + reset(); + subscribe(); + context_->queueRender(); +} + +void DepthCloudDisplay::fillTransportOptionList(rviz_common::properties::EnumProperty * property) +{ + property->clearOptions(); + + std::vector choices; + + choices.push_back("raw"); + + auto rviz_ros_node_ = context_->getRosNodeAbstraction().lock(); + + std::map> published_topics = + rviz_ros_node_->get_topic_names_and_types(); + + const std::string & topic = depth_topic_property_->getStdString(); + + for (const auto &[topic_name, topic_types] : published_topics) { + if (topic_name.find(topic) == 0 && topic_name != topic && topic_name[topic.size()] == '/' && + topic_name.find('/', topic.size() + 1) == std::string::npos) + { + std::string transport_type = topic_name.substr(topic.size() + 1); + + // If the transport type string found above is in the set of + // supported transport type plugins, add it to the list. + if (transport_plugin_types_.find(transport_type) != transport_plugin_types_.end()) { + choices.push_back(transport_type); + } + } + } + + for (size_t i = 0; i < choices.size(); i++) { + property->addOptionStd(choices[i]); + } +} + +void DepthCloudDisplay::fixedFrameChanged() +{ + Display::reset(); +} + +} // namespace displays +} // namespace rviz_default_plugins + +#include // NOLINT: cpplint cannot handle the include order here + +PLUGINLIB_EXPORT_CLASS(rviz_default_plugins::displays::DepthCloudDisplay, rviz_common::Display) diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_common.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_common.cpp index 30c752d5cb..ec0fc82b13 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_common.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_common.cpp @@ -214,14 +214,13 @@ void PointCloudCommon::loadTransformer( transformers_[name] = info; } -// TODO(anhosi): check if still needed when migrating DepthCloud -// void PointCloudCommon::setAutoSize(bool auto_size) -// { -// auto_size_ = auto_size; -// for (auto const & cloud_info : cloud_infos_) { -// cloud_info->cloud_->setAutoSize(auto_size); -// } -// } +void PointCloudCommon::setAutoSize(bool auto_size) +{ + auto_size_ = auto_size; + for (auto const & cloud_info : cloud_infos_) { + cloud_info->cloud_->setAutoSize(auto_size); + } +} void PointCloudCommon::updateAlpha() { diff --git a/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_display.cpp b/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_display.cpp index 5e147ed466..c7fae2aab5 100644 --- a/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_display.cpp +++ b/rviz_default_plugins/src/rviz_default_plugins/displays/pointcloud/point_cloud_display.cpp @@ -32,7 +32,7 @@ #include #include -#include +#include #include "rviz_default_plugins/displays/pointcloud/point_cloud_common.hpp" #include "rviz_common/display_context.hpp" diff --git a/rviz_default_plugins/test/reference_images/depth_cloud_display_visual_test_ref.png b/rviz_default_plugins/test/reference_images/depth_cloud_display_visual_test_ref.png new file mode 100644 index 0000000000000000000000000000000000000000..4b2188a6e64c3f9d2f701a43edee2e02c8fbc758 GIT binary patch literal 5332 zcmeAS@N?(olHy`uVBq!ia0y~yV0q8Lz+B3~1Qc0u&*KV^;wb| z85l&`JY5_^D(1Ysxsmg*gMh2!=^Yza|M9vdHb+_~K$pLzF*D` zqzbcc4nMPI XJMp(BhfKHx)Xm`O>gTe~DWM4fknUH* literal 0 HcmV?d00001 diff --git a/rviz_default_plugins/test/rviz_default_plugins/displays/depth_cloud/depth_cloud_display_visual_test.cpp b/rviz_default_plugins/test/rviz_default_plugins/displays/depth_cloud/depth_cloud_display_visual_test.cpp new file mode 100644 index 0000000000..621092072e --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/displays/depth_cloud/depth_cloud_display_visual_test.cpp @@ -0,0 +1,67 @@ +/* + * Copyright (c) 2023, Open Source Robotics Foundation, 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, Inc. 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 "rviz_visual_testing_framework/visual_test_fixture.hpp" +#include "rviz_visual_testing_framework/visual_test_publisher.hpp" + +#include "../../publishers/camera_info_publisher.hpp" +#include "../../publishers/image_publisher.hpp" +#include "../../page_objects/depth_cloud_page_object.hpp" + +TEST_F(VisualTestFixture, test_depth_cloud_display_with_published_image) { + std::vector publishers = { + PublisherWithFrame(std::make_shared(), "image"), + PublisherWithFrame(std::make_shared("/image/image_raw"), "image_frame"), + PublisherWithFrame( + std::make_shared( + "/rgbd_camera/image"), "depthcloud_frame") + }; + auto cam_publisher = std::make_unique(publishers); + + auto depth_cloud_display = addDisplay(); + depth_cloud_display->setTopicFilter(true); + depth_cloud_display->setDepthMapTopic("/rgbd_camera/image"); + depth_cloud_display->setImageMapTopic("/image/image_raw"); + depth_cloud_display->setDepthMapTransport("raw"); + depth_cloud_display->setImageMapTransport("raw"); + depth_cloud_display->setQueueSize(10); + depth_cloud_display->setOclusionCompensation(true); + depth_cloud_display->setSelectable(false); + depth_cloud_display->setStyle("Spheres"); + depth_cloud_display->setSize(10); + depth_cloud_display->setAlpha(0.5); + depth_cloud_display->setDecay(0); + + /// Compare test screenshots with the reference ones (if in TEST mode): + assertScreenShotsIdentity(); +} diff --git a/rviz_default_plugins/test/rviz_default_plugins/page_objects/depth_cloud_page_object.cpp b/rviz_default_plugins/test/rviz_default_plugins/page_objects/depth_cloud_page_object.cpp new file mode 100644 index 0000000000..5ec6a44d05 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/page_objects/depth_cloud_page_object.cpp @@ -0,0 +1,101 @@ +/* + * Copyright (c) 2019, Martin Idel + * 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 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 "depth_cloud_page_object.hpp" + +#include +#include + +DepthCloudDisplayPageObject::DepthCloudDisplayPageObject() +: BasePageObject(0, "DepthCloud") +{} + +void DepthCloudDisplayPageObject::setDepthMapTopic(QString topic) +{ + setString("Depth Map Topic", topic); + waitForFirstMessage(); +} + +void DepthCloudDisplayPageObject::setImageMapTopic(QString topic) +{ + setString("Color Image Topic", topic); + waitForFirstMessage(); +} + +void DepthCloudDisplayPageObject::setQueueSize(int size) +{ + setInt("Queue Size", size); +} + +void DepthCloudDisplayPageObject::setDecay(int size) +{ + setInt("Decay Time", size); +} + +void DepthCloudDisplayPageObject::setSize(int size) +{ + setInt("Size (m)", size); +} + +void DepthCloudDisplayPageObject::setAlpha(float alpha) +{ + setFloat("Alpha", alpha); +} + +void DepthCloudDisplayPageObject::setStyle(QString style) +{ + setComboBox("Style", style); +} + +void DepthCloudDisplayPageObject::setDepthMapTransport(QString type) +{ + setComboBox("Depth Map Transport Hint", type); + waitForFirstMessage(); +} + +void DepthCloudDisplayPageObject::setImageMapTransport(QString type) +{ + setComboBox("Color Transport Hint", type); + waitForFirstMessage(); +} + +void DepthCloudDisplayPageObject::setSelectable(bool selectable) +{ + setBool("Selectable", selectable); +} + +void DepthCloudDisplayPageObject::setOclusionCompensation(bool visibility) +{ + setBool("Occlusion Compensation", visibility); +} + +void DepthCloudDisplayPageObject::setTopicFilter(bool visibility) +{ + setBool("Topic Filter", visibility); +} diff --git a/rviz_default_plugins/test/rviz_default_plugins/page_objects/depth_cloud_page_object.hpp b/rviz_default_plugins/test/rviz_default_plugins/page_objects/depth_cloud_page_object.hpp new file mode 100644 index 0000000000..05beedd375 --- /dev/null +++ b/rviz_default_plugins/test/rviz_default_plugins/page_objects/depth_cloud_page_object.hpp @@ -0,0 +1,54 @@ +/* + * Copyright (c) 2019, Martin Idel + * 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 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 RVIZ_DEFAULT_PLUGINS__PAGE_OBJECTS__DEPTH_CLOUD_PAGE_OBJECT_HPP_ +#define RVIZ_DEFAULT_PLUGINS__PAGE_OBJECTS__DEPTH_CLOUD_PAGE_OBJECT_HPP_ + +#include "rviz_visual_testing_framework/page_objects/base_page_object.hpp" + +class DepthCloudDisplayPageObject : public BasePageObject +{ +public: + DepthCloudDisplayPageObject(); + + void setTopicFilter(bool visibility); + void setDepthMapTopic(QString topic); + void setDepthMapTransport(QString type); + void setImageMapTopic(QString topic); + void setImageMapTransport(QString type); + void setQueueSize(int size); + void setOclusionCompensation(bool visibility); + void setSelectable(bool selectable); + void setStyle(QString points); + void setSize(int size); + void setAlpha(float size); + void setDecay(int size); +}; + +#endif // RVIZ_DEFAULT_PLUGINS__PAGE_OBJECTS__DEPTH_CLOUD_PAGE_OBJECT_HPP_ diff --git a/rviz_default_plugins/test/rviz_default_plugins/publishers/image_publisher.hpp b/rviz_default_plugins/test/rviz_default_plugins/publishers/image_publisher.hpp index 544c4c24de..db7836cbdc 100644 --- a/rviz_default_plugins/test/rviz_default_plugins/publishers/image_publisher.hpp +++ b/rviz_default_plugins/test/rviz_default_plugins/publishers/image_publisher.hpp @@ -32,6 +32,7 @@ #include #include +#include #include #include "rclcpp/rclcpp.hpp" @@ -57,10 +58,10 @@ namespace nodes class ImagePublisher : public rclcpp::Node { public: - ImagePublisher() + explicit ImagePublisher(const std::string & topic_name = "image") : Node("image_publisher") { - publisher = this->create_publisher("image", 10); + publisher = this->create_publisher(topic_name, 10); timer = this->create_wall_timer(100ms, std::bind(&ImagePublisher::timer_callback, this)); }