From fd0aafaf20796bb2a00087f9475ac677b837e900 Mon Sep 17 00:00:00 2001 From: john-maidbot Date: Sun, 18 Jun 2023 14:58:00 -0500 Subject: [PATCH 01/28] First pass at ros2 conversion --- .vscode/settings.json | 91 ++++ CHANGELOG.rst | 5 + CMakeLists.txt | 180 ++++---- PORT_TODO.md | 8 + README.md | 10 +- .../{exception.h => exception.hpp} | 0 .../{loader_fwds.h => loader_fwds.hpp} | 10 +- ...nt_cloud_codec.h => point_cloud_codec.hpp} | 48 +-- .../point_cloud_common.hpp | 44 ++ ..._transport.h => point_cloud_transport.hpp} | 92 ++-- .../{publisher.h => publisher.hpp} | 48 +-- .../point_cloud_transport/publisher_plugin.h | 184 -------- .../publisher_plugin.hpp | 127 ++++++ .../{raw_publisher.h => raw_publisher.hpp} | 36 +- .../{raw_subscriber.h => raw_subscriber.hpp} | 29 +- .../{republish.h => republish.hpp} | 20 +- .../simple_publisher_plugin.h | 405 ------------------ .../simple_publisher_plugin.hpp | 195 +++++++++ .../simple_subscriber_plugin.h | 294 ------------- .../simple_subscriber_plugin.hpp | 173 ++++++++ ...sher.h => single_subscriber_publisher.hpp} | 24 +- .../{subscriber.h => subscriber.hpp} | 39 +- ...scriber_filter.h => subscriber_filter.hpp} | 45 +- .../point_cloud_transport/subscriber_plugin.h | 261 ----------- .../subscriber_plugin.hpp | 192 +++++++++ ...{transport_hints.h => transport_hints.hpp} | 30 +- package.xml | 42 +- src/list_transports.cpp | 36 +- src/manifest.cpp | 10 +- src/point_cloud_codec.cpp | 55 ++- src/point_cloud_common.cpp | 47 ++ src/point_cloud_transport.cpp | 225 ++++------ src/point_cloud_transport/encoder.py | 2 +- src/publisher.cpp | 133 +++--- src/publisher_plugin.cpp | 42 +- src/raw_publisher.cpp | 82 ---- src/raw_subscriber.cpp | 27 +- src/republish.cpp | 59 +-- src/single_subscriber_publisher.cpp | 19 +- src/subscriber.cpp | 73 ++-- 40 files changed, 1494 insertions(+), 1948 deletions(-) create mode 100644 .vscode/settings.json create mode 100644 PORT_TODO.md rename include/point_cloud_transport/{exception.h => exception.hpp} (100%) rename include/point_cloud_transport/{loader_fwds.h => loader_fwds.hpp} (92%) rename include/point_cloud_transport/{point_cloud_codec.h => point_cloud_codec.hpp} (73%) create mode 100644 include/point_cloud_transport/point_cloud_common.hpp rename include/point_cloud_transport/{point_cloud_transport.h => point_cloud_transport.hpp} (61%) rename include/point_cloud_transport/{publisher.h => publisher.hpp} (69%) delete mode 100644 include/point_cloud_transport/publisher_plugin.h create mode 100644 include/point_cloud_transport/publisher_plugin.hpp rename include/point_cloud_transport/{raw_publisher.h => raw_publisher.hpp} (71%) rename include/point_cloud_transport/{raw_subscriber.h => raw_subscriber.hpp} (78%) rename include/point_cloud_transport/{republish.h => republish.hpp} (57%) delete mode 100644 include/point_cloud_transport/simple_publisher_plugin.h create mode 100644 include/point_cloud_transport/simple_publisher_plugin.hpp delete mode 100644 include/point_cloud_transport/simple_subscriber_plugin.h create mode 100644 include/point_cloud_transport/simple_subscriber_plugin.hpp rename include/point_cloud_transport/{single_subscriber_publisher.h => single_subscriber_publisher.hpp} (79%) rename include/point_cloud_transport/{subscriber.h => subscriber.hpp} (82%) rename include/point_cloud_transport/{subscriber_filter.h => subscriber_filter.hpp} (77%) delete mode 100644 include/point_cloud_transport/subscriber_plugin.h create mode 100644 include/point_cloud_transport/subscriber_plugin.hpp rename include/point_cloud_transport/{transport_hints.h => transport_hints.hpp} (78%) create mode 100644 src/point_cloud_common.cpp delete mode 100644 src/raw_publisher.cpp diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..878e524 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,91 @@ +{ + "files.associations": { + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "csignal": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "any": "cpp", + "array": "cpp", + "atomic": "cpp", + "hash_map": "cpp", + "hash_set": "cpp", + "strstream": "cpp", + "bit": "cpp", + "*.tcc": "cpp", + "bitset": "cpp", + "cfenv": "cpp", + "charconv": "cpp", + "chrono": "cpp", + "cinttypes": "cpp", + "codecvt": "cpp", + "compare": "cpp", + "complex": "cpp", + "concepts": "cpp", + "condition_variable": "cpp", + "coroutine": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "forward_list": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "regex": "cpp", + "source_location": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "slist": "cpp", + "fstream": "cpp", + "future": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "numbers": "cpp", + "ostream": "cpp", + "ranges": "cpp", + "scoped_allocator": "cpp", + "semaphore": "cpp", + "shared_mutex": "cpp", + "span": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "valarray": "cpp", + "variant": "cpp", + "expected": "cpp", + "*.ipp": "cpp" + } +} \ No newline at end of file diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 28f1364..a605f11 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package point_cloud_transport ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.0.12 (2023-06-18) +------------------- +* ROS2 port +* Contributors: John D'Angelo + 1.0.11 (2023-06-16) ------------------- * Fixed bad_expected_access error when pointcloud encoding fails. diff --git a/CMakeLists.txt b/CMakeLists.txt index 550bf34..a7bd971 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,117 +1,123 @@ cmake_minimum_required(VERSION 3.10.2) -project(point_cloud_transport) - -find_package(catkin REQUIRED COMPONENTS - cras_cpp_common - cras_topic_tools - dynamic_reconfigure - message_generation - nodelet - pluginlib - roscpp - sensor_msgs - topic_tools -) -find_package(Boost REQUIRED) +project(point_cloud_transport) -catkin_python_setup() +find_package(ament_cmake_ros REQUIRED) -generate_dynamic_reconfigure_options(cfg/NoConfig.cfg) +find_package(message_filters REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) -catkin_package( - INCLUDE_DIRS include - LIBRARIES ${PROJECT_NAME} raw_${PROJECT_NAME} - CATKIN_DEPENDS cras_cpp_common cras_topic_tools dynamic_reconfigure message_filters message_runtime nodelet roscpp sensor_msgs topic_tools +include_directories( + include ) -include_directories(include ${catkin_INCLUDE_DIRS}) - # Build libpoint_cloud_transport add_library(${PROJECT_NAME} - src/point_cloud_codec.cpp + # src/point_cloud_codec.cpp # TODO: Is this needed? + src/point_cloud_common.cpp src/point_cloud_transport.cpp src/publisher.cpp src/publisher_plugin.cpp src/single_subscriber_publisher.cpp src/subscriber.cpp ) -add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) -target_link_libraries(${PROJECT_NAME} PUBLIC ${Boost_LIBRARIES} ${catkin_LIBRARIES}) - -# The library is build twice. Once with symbols exported for direct use, and once with symbols hidden for use via pluginlib. -# Build libraw_point_cloud_transport -add_library(raw_${PROJECT_NAME} src/raw_publisher.cpp src/raw_subscriber.cpp) -target_link_libraries(raw_${PROJECT_NAME} ${PROJECT_NAME}) -add_dependencies(raw_${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS}) +add_library(${PROJECT_NAME}::${PROJECT_NAME} ALIAS ${PROJECT_NAME}) +target_include_directories(${PROJECT_NAME} PUBLIC + "$" + "$") +target_link_libraries(${PROJECT_NAME} PUBLIC + message_filters::message_filters + rclcpp::rclcpp + ${sensor_msgs_TARGETS}) +target_link_libraries(${PROJECT_NAME} PRIVATE + pluginlib::pluginlib) # Build libpoint_cloud_transport_plugins -add_library(${PROJECT_NAME}_plugins src/manifest.cpp src/raw_publisher.cpp src/raw_subscriber.cpp) -target_link_libraries(${PROJECT_NAME}_plugins ${PROJECT_NAME}) -add_dependencies(${PROJECT_NAME}_plugins ${${PROJECT_NAME}_EXPORTED_TARGETS}) -class_loader_hide_library_symbols(${PROJECT_NAME}_plugins) +add_library(${PROJECT_NAME}_plugins + src/manifest.cpp +) +add_library(${PROJECT_NAME}::${PROJECT_NAME}_plugins ALIAS ${PROJECT_NAME}_plugins) -add_library(${PROJECT_NAME}_republish src/republish.cpp) -target_link_libraries(${PROJECT_NAME}_republish ${PROJECT_NAME}) -cras_node_from_nodelet(${PROJECT_NAME}_republish "${PROJECT_NAME}/republish.h" ${PROJECT_NAME}::RepublishNodelet OUTPUT_NAME republish ANONYMOUS) +target_link_libraries(${PROJECT_NAME}_plugins PRIVATE + ${PROJECT_NAME} + pluginlib::pluginlib) -add_executable(list_transports src/list_transports.cpp) -target_link_libraries(list_transports ${PROJECT_NAME}) +# Build republish (TODO: there was some nodelet stuff happening here) +add_executable(republish src/republish.cpp) +target_link_libraries(republish + ${PROJECT_NAME} + pluginlib::pluginlib) -install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_plugins ${PROJECT_NAME}_republish raw_${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -) +# Build list_transports (TODO: there was some nodelet stuff happening here) +add_executable(list_transports src/list_transports.cpp) +target_link_libraries(list_transports + ${PROJECT_NAME} + pluginlib::pluginlib) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) +# Install plugin descriptions +pluginlib_export_plugin_description_file(${PROJECT_NAME} default_plugins.xml) -install(TARGETS list_transports - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_plugins + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) +install(TARGETS ${PROJECT_NAME} EXPORT export_${PROJECT_NAME}) -install(FILES default_plugins.xml nodelets.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# Install executables +install( + TARGETS list_transports republish + RUNTIME DESTINATION lib/${PROJECT_NAME} ) -if (CATKIN_ENABLE_TESTING) - find_package(roslint REQUIRED) - - # catkin_lint - checks validity of package.xml and CMakeLists.txt - # ROS buildfarm calls this without any environment and with empty rosdep cache, - # so we have problems reading the list of packages from env - # see https://github.com/ros-infrastructure/ros_buildfarm/issues/923 - if(DEFINED ENV{ROS_HOME}) - #catkin_lint: ignore_once env_var - set(ROS_HOME "$ENV{ROS_HOME}") - else() - #catkin_lint: ignore_once env_var - set(ROS_HOME "$ENV{HOME}/.ros") - endif() - #catkin_lint: ignore_once env_var - if(DEFINED ENV{ROS_ROOT} AND EXISTS "${ROS_HOME}/rosdep/sources.cache") - roslint_custom(catkin_lint "-W2" .) - endif() - - # Roslint C++ - checks formatting and some other rules for C++ files - - file(GLOB_RECURSE ROSLINT_INCLUDE include/*.h include/*.hpp) - file(GLOB_RECURSE ROSLINT_SRC src/*.cpp src/*.hpp src/*.h) - #file(GLOB_RECURSE ROSLINT_TEST test/*.cpp) - - set(ROSLINT_CPP_OPTS "--extensions=h,hpp,hh,c,cpp,cc;--linelength=120;--filter=\ - -build/header_guard,-readability/namespace,-whitespace/braces,-runtime/references,\ - -build/c++11,-readability/nolint,-readability/todo,-legal/copyright,-build/namespaces") - roslint_cpp(${ROSLINT_INCLUDE} ${ROSLINT_SRC}) +# Install include directories +install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) + +ament_export_targets(export_${PROJECT_NAME}) + +ament_export_dependencies(message_filters rclcpp sensor_msgs pluginlib) + +# TODO: Fix testing +# if (CATKIN_ENABLE_TESTING) +# find_package(roslint REQUIRED) + +# # catkin_lint - checks validity of package.xml and CMakeLists.txt +# # ROS buildfarm calls this without any environment and with empty rosdep cache, +# # so we have problems reading the list of packages from env +# # see https://github.com/ros-infrastructure/ros_buildfarm/issues/923 +# if(DEFINED ENV{ROS_HOME}) +# #catkin_lint: ignore_once env_var +# set(ROS_HOME "$ENV{ROS_HOME}") +# else() +# #catkin_lint: ignore_once env_var +# set(ROS_HOME "$ENV{HOME}/.ros") +# endif() +# #catkin_lint: ignore_once env_var +# if(DEFINED ENV{ROS_ROOT} AND EXISTS "${ROS_HOME}/rosdep/sources.cache") +# roslint_custom(catkin_lint "-W2" .) +# endif() + +# # Roslint C++ - checks formatting and some other rules for C++ files + +# file(GLOB_RECURSE ROSLINT_INCLUDE include/*.h include/*.hpp) +# file(GLOB_RECURSE ROSLINT_SRC src/*.cpp src/*.hpp src/*.h) +# #file(GLOB_RECURSE ROSLINT_TEST test/*.cpp) + +# set(ROSLINT_CPP_OPTS "--extensions=h,hpp,hh,c,cpp,cc;--linelength=120;--filter=\ +# -build/header_guard,-readability/namespace,-whitespace/braces,-runtime/references,\ +# -build/c++11,-readability/nolint,-readability/todo,-legal/copyright,-build/namespaces") +# roslint_cpp(${ROSLINT_INCLUDE} ${ROSLINT_SRC}) - # Roslint Python +# # Roslint Python + +# # Run roslint on Python sources +# file(GLOB_RECURSE python_files src/*.py) +# roslint_python("${python_files}") - # Run roslint on Python sources - file(GLOB_RECURSE python_files src/*.py) - roslint_python("${python_files}") +# roslint_add_test() +# endif() - roslint_add_test() -endif() +ament_package() diff --git a/PORT_TODO.md b/PORT_TODO.md new file mode 100644 index 0000000..ed52aa1 --- /dev/null +++ b/PORT_TODO.md @@ -0,0 +1,8 @@ +## TODO +- [] roscpp --> rclcpp conversion (IN PROGRESS) +- [] c++ codebase builds with cmake (minus tests) +- [] c++ codebase runs with test pointclouds +- [] tests build and pass +- [] rospy --> rclpy conversion +- [] python codebase runs with test pointclouds + diff --git a/README.md b/README.md index e95c040..6cef231 100644 --- a/README.md +++ b/README.md @@ -16,16 +16,16 @@ For complete examples of publishing and subscribing to point clouds using [point ### C++ Communicating PointCloud2 messages using [point_cloud_transport](https://github.com/ctu-vras/point_cloud_transport): ```cpp -#include -#include +#include +#include -void Callback(const sensor_msgs::PointCloud2ConstPtr& msg) +void Callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg) { // ... process the message } -ros::NodeHandle nh; -point_cloud_transport::PointCloudTransport pct(nh); +auto node = std::make_shared(); +point_cloud_transport::PointCloudTransport pct(node); point_cloud_transport::Subscriber sub = pct.subscribe("in_point_cloud_base_topic", 1, Callback); point_cloud_transport::Publisher pub = pct.advertise("out_point_cloud_base_topic", 1); ``` diff --git a/include/point_cloud_transport/exception.h b/include/point_cloud_transport/exception.hpp similarity index 100% rename from include/point_cloud_transport/exception.h rename to include/point_cloud_transport/exception.hpp diff --git a/include/point_cloud_transport/loader_fwds.h b/include/point_cloud_transport/loader_fwds.hpp similarity index 92% rename from include/point_cloud_transport/loader_fwds.h rename to include/point_cloud_transport/loader_fwds.hpp index 2154fc7..8954f01 100644 --- a/include/point_cloud_transport/loader_fwds.h +++ b/include/point_cloud_transport/loader_fwds.hpp @@ -40,10 +40,12 @@ #pragma once -#include +#include + + // Forward-declare some classes most users shouldn't care about so that -// point_cloud_transport.h doesn't bring them in. +// point_cloud_transport.hpp doesn't bring them in. namespace pluginlib { @@ -56,8 +58,8 @@ class PublisherPlugin; class SubscriberPlugin; typedef pluginlib::ClassLoader PubLoader; -typedef boost::shared_ptr PubLoaderPtr; +typedef std::shared_ptr PubLoaderPtr; typedef pluginlib::ClassLoader SubLoader; -typedef boost::shared_ptr SubLoaderPtr; +typedef std::shared_ptr SubLoaderPtr; } diff --git a/include/point_cloud_transport/point_cloud_codec.h b/include/point_cloud_transport/point_cloud_codec.hpp similarity index 73% rename from include/point_cloud_transport/point_cloud_codec.h rename to include/point_cloud_transport/point_cloud_codec.hpp index a32cb9c..cc95ff7 100644 --- a/include/point_cloud_transport/point_cloud_codec.h +++ b/include/point_cloud_transport/point_cloud_codec.hpp @@ -44,21 +44,15 @@ #include #include -#include -#include -#include -#include -#include - #include #include #include #include #include -#include +#include -#include -#include +#include +#include namespace point_cloud_transport { @@ -76,20 +70,20 @@ class PointCloudCodec : public cras::HasLogger //! Constructor explicit PointCloudCodec(const cras::LogHelperPtr& log = std::make_shared()); - boost::shared_ptr getEncoderByName(const std::string& name) const; + std::shared_ptr getEncoderByName(const std::string& name) const; - boost::shared_ptr getEncoderByTopic( + std::shared_ptr getEncoderByTopic( const std::string& topic, const std::string& datatype) const; - boost::shared_ptr getDecoderByName(const std::string& name) const; + std::shared_ptr getDecoderByName(const std::string& name) const; - boost::shared_ptr getDecoderByTopic( + std::shared_ptr getDecoderByTopic( const std::string& topic, const std::string& datatype) const; private: struct Impl; - typedef boost::shared_ptr ImplPtr; - typedef boost::weak_ptr ImplWPtr; + typedef std::shared_ptr ImplPtr; + typedef std::weak_ptr ImplWPtr; ImplPtr impl_; }; @@ -98,19 +92,19 @@ class PointCloudCodec : public cras::HasLogger extern "C" bool pointCloudTransportCodecsEncode( const char* codec, - sensor_msgs::PointCloud2::_height_type rawHeight, - sensor_msgs::PointCloud2::_width_type rawWidth, + sensor_msgs::msg::PointCloud2::_height_type rawHeight, + sensor_msgs::msg::PointCloud2::_width_type rawWidth, size_t rawNumFields, const char* rawFieldNames[], sensor_msgs::PointField::_offset_type rawFieldOffsets[], sensor_msgs::PointField::_datatype_type rawFieldDatatypes[], sensor_msgs::PointField::_count_type rawFieldCounts[], - sensor_msgs::PointCloud2::_is_bigendian_type rawIsBigendian, - sensor_msgs::PointCloud2::_point_step_type rawPointStep, - sensor_msgs::PointCloud2::_row_step_type rawRowStep, + sensor_msgs::msg::PointCloud2::_is_bigendian_type rawIsBigendian, + sensor_msgs::msg::PointCloud2::_point_step_type rawPointStep, + sensor_msgs::msg::PointCloud2::_row_step_type rawRowStep, size_t rawDataLength, const uint8_t rawData[], - sensor_msgs::PointCloud2::_is_dense_type rawIsDense, + sensor_msgs::msg::PointCloud2::_is_dense_type rawIsDense, cras::allocator_t compressedTypeAllocator, cras::allocator_t compressedMd5SumAllocator, cras::allocator_t compressedDataAllocator, @@ -126,18 +120,18 @@ extern "C" bool pointCloudTransportCodecsDecode( const char* compressedMd5sum, size_t compressedDataLength, const uint8_t compressedData[], - sensor_msgs::PointCloud2::_height_type& rawHeight, - sensor_msgs::PointCloud2::_width_type& rawWidth, + sensor_msgs::msg::PointCloud2::_height_type& rawHeight, + sensor_msgs::msg::PointCloud2::_width_type& rawWidth, uint32_t& rawNumFields, cras::allocator_t rawFieldNamesAllocator, cras::allocator_t rawFieldOffsetsAllocator, cras::allocator_t rawFieldDatatypesAllocator, cras::allocator_t rawFieldCountsAllocator, - sensor_msgs::PointCloud2::_is_bigendian_type& rawIsBigEndian, - sensor_msgs::PointCloud2::_point_step_type& rawPointStep, - sensor_msgs::PointCloud2::_row_step_type& rawRowStep, + sensor_msgs::msg::PointCloud2::_is_bigendian_type& rawIsBigEndian, + sensor_msgs::msg::PointCloud2::_point_step_type& rawPointStep, + sensor_msgs::msg::PointCloud2::_row_step_type& rawRowStep, cras::allocator_t rawDataAllocator, - sensor_msgs::PointCloud2::_is_dense_type& rawIsDense, + sensor_msgs::msg::PointCloud2::_is_dense_type& rawIsDense, size_t serializedConfigLength, const uint8_t serializedConfig[], cras::allocator_t errorStringAllocator, diff --git a/include/point_cloud_transport/point_cloud_common.hpp b/include/point_cloud_transport/point_cloud_common.hpp new file mode 100644 index 0000000..f803d90 --- /dev/null +++ b/include/point_cloud_transport/point_cloud_common.hpp @@ -0,0 +1,44 @@ +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include "point_cloud_transport/point_cloud_common.hpp" + +#include +#include + +namespace point_cloud_transport +{ + +/** + * \brief Replacement for uses of boost::erase_last_copy + */ +std::string erase_last_copy(const std::string & input, const std::string & search); + +} diff --git a/include/point_cloud_transport/point_cloud_transport.h b/include/point_cloud_transport/point_cloud_transport.hpp similarity index 61% rename from include/point_cloud_transport/point_cloud_transport.h rename to include/point_cloud_transport/point_cloud_transport.hpp index 8d3923d..57f8c92 100644 --- a/include/point_cloud_transport/point_cloud_transport.h +++ b/include/point_cloud_transport/point_cloud_transport.hpp @@ -40,26 +40,20 @@ #pragma once +#include #include #include #include #include -#include -#include -#include -#include -#include +#include "rclcpp/node.hpp" -#include -#include -#include -#include +#include -#include -#include -#include -#include +#include +#include +#include +#include namespace point_cloud_transport { @@ -107,78 +101,76 @@ class PointCloudTransportLoader class PointCloudTransport : public PointCloudTransportLoader { + using VoidPtr = std::shared_ptr; + public: //! Constructor - explicit PointCloudTransport(const ros::NodeHandle& nh); + explicit PointCloudTransport(rclcpp::Node::SharedPtr node); + + ~PointCloudTransport(); //! Advertise a PointCloud2 topic, simple version. point_cloud_transport::Publisher advertise(const std::string& base_topic, uint32_t queue_size, bool latch = false); //! Advertise an PointCloud2 topic with subscriber status callbacks. - point_cloud_transport::Publisher advertise(const std::string& base_topic, uint32_t queue_size, - const point_cloud_transport::SubscriberStatusCallback& connect_cb, - const point_cloud_transport::SubscriberStatusCallback& disconnect_cb = {}, - const ros::VoidPtr& tracked_object = {}, bool latch = false); + // TODO(ros2) Implement when SubscriberStatusCallback is available + // point_cloud_transport::Publisher advertise(const std::string& base_topic, uint32_t queue_size, + // const point_cloud_transport::SubscriberStatusCallback& connect_cb, + // const point_cloud_transport::SubscriberStatusCallback& disconnect_cb = {}, + // const ros::VoidPtr& tracked_object = {}, bool latch = false); - //! Subscribe to a point cloud topic, version for arbitrary boost::function object. + //! Subscribe to a point cloud topic, version for arbitrary std::function object. point_cloud_transport::Subscriber subscribe( const std::string& base_topic, uint32_t queue_size, - const boost::function& callback, - const ros::VoidPtr& tracked_object = {}, - const point_cloud_transport::TransportHints& transport_hints = {}, - bool allow_concurrent_callbacks = false); + const std::function& callback, + const VoidPtr& tracked_object = {}, + const point_cloud_transport::TransportHints* transport_hints = nullptr); //! Subscribe to a point cloud topic, version for bare function. point_cloud_transport::Subscriber subscribe(const std::string& base_topic, uint32_t queue_size, - void(* fp)(const sensor_msgs::PointCloud2ConstPtr&), - const point_cloud_transport::TransportHints& transport_hints = {}, - bool allow_concurrent_callbacks = false) + void(* fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr&), + const point_cloud_transport::TransportHints* transport_hints = nullptr) { return subscribe(base_topic, queue_size, - boost::function(fp), - ros::VoidPtr(), transport_hints, allow_concurrent_callbacks); + std::function(fp), + VoidPtr(), transport_hints); } //! Subscribe to a point cloud topic, version for class member function with bare pointer. template point_cloud_transport::Subscriber subscribe(const std::string& base_topic, uint32_t queue_size, - void(T::*fp)(const sensor_msgs::PointCloud2ConstPtr&), T* obj, - const point_cloud_transport::TransportHints& transport_hints = {}, + void(T::*fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr&), T* obj, + const point_cloud_transport::TransportHints* transport_hints = nullptr, bool allow_concurrent_callbacks = false) { - return subscribe(base_topic, queue_size, boost::bind(fp, obj, _1), ros::VoidPtr(), transport_hints, - allow_concurrent_callbacks); + return subscribe(base_topic, queue_size, std::bind(fp, obj.get(), std::placeholders::_1), VoidPtr(), transport_hints); } //! Subscribe to a point cloud topic, version for class member function with shared_ptr. template point_cloud_transport::Subscriber subscribe(const std::string& base_topic, uint32_t queue_size, - void(T::*fp)(const sensor_msgs::PointCloud2ConstPtr&), - const boost::shared_ptr& obj, - const point_cloud_transport::TransportHints& transport_hints = {}, - bool allow_concurrent_callbacks = false) + void(T::*fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr&), + const std::shared_ptr& obj, + const point_cloud_transport::TransportHints* transport_hints = nullptr) { - return subscribe(base_topic, queue_size, boost::bind(fp, obj.get(), _1), obj, transport_hints, - allow_concurrent_callbacks); + return subscribe(base_topic, queue_size, std::bind(fp, obj.get(), std::placeholders::_1), obj, transport_hints); } private: struct Impl; - typedef boost::shared_ptr ImplPtr; - typedef boost::weak_ptr ImplWPtr; - - ImplPtr impl_; + std::shared_ptr impl_; }; } -extern "C" void pointCloudTransportGetLoadableTransports( - cras::allocator_t transportAllocator, cras::allocator_t nameAllocator); +// TODO: May need these? +// extern "C" void pointCloudTransportGetLoadableTransports( +// cras::allocator_t transportAllocator, cras::allocator_t nameAllocator); -extern "C" void pointCloudTransportGetTopicsToPublish( - const char* baseTopic, cras::allocator_t transportAllocator, cras::allocator_t nameAllocator, - cras::allocator_t topicAllocator, cras::allocator_t dataTypeAllocator, cras::allocator_t configTypeAllocator); +// extern "C" void pointCloudTransportGetTopicsToPublish( +// const char* baseTopic, cras::allocator_t transportAllocator, cras::allocator_t nameAllocator, +// cras::allocator_t topicAllocator, cras::allocator_t dataTypeAllocator, cras::allocator_t configTypeAllocator); -extern "C" void pointCloudTransportGetTopicToSubscribe( - const char* baseTopic, const char* transport, cras::allocator_t nameAllocator, cras::allocator_t topicAllocator, - cras::allocator_t dataTypeAllocator, cras::allocator_t configTypeAllocator); +// extern "C" void pointCloudTransportGetTopicToSubscribe( +// const char* baseTopic, const char* transport, cras::allocator_t nameAllocator, cras::allocator_t topicAllocator, +// cras::allocator_t dataTypeAllocator, cras::allocator_t configTypeAllocator); diff --git a/include/point_cloud_transport/publisher.h b/include/point_cloud_transport/publisher.hpp similarity index 69% rename from include/point_cloud_transport/publisher.h rename to include/point_cloud_transport/publisher.hpp index ecd321e..5e773d1 100644 --- a/include/point_cloud_transport/publisher.h +++ b/include/point_cloud_transport/publisher.hpp @@ -41,16 +41,17 @@ #pragma once #include +#include -#include -#include +#include "rclcpp/macros.hpp" +#include "rclcpp/node.hpp" -#include -#include -#include +#include -#include -#include +#include "pluginlib/class_loader.hpp" + +#include +#include namespace point_cloud_transport { @@ -59,8 +60,14 @@ class Publisher { public: //! Constructor - Publisher(); - + Publisher() = default; + + Publisher( + rclcpp::Node * nh, + const std::string & base_topic, + PubLoaderPtr loader, + rmw_qos_profile_t custom_qos); + //! get total number of subscribers to all advertised topics. uint32_t getNumSubscribers() const; @@ -68,10 +75,10 @@ class Publisher std::string getTopic() const; //! Publish a point cloud on the topics associated with this Publisher. - void publish(const sensor_msgs::PointCloud2& message) const; + void publish(const sensor_msgs::msg::PointCloud2& message) const; //! Publish a point cloud on the topics associated with this Publisher. - void publish(const sensor_msgs::PointCloud2ConstPtr& message) const; + void publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& message) const; //! Shutdown the advertisements associated with this Publisher. void shutdown(); @@ -94,25 +101,8 @@ class Publisher } private: - Publisher(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, - const point_cloud_transport::SubscriberStatusCallback& connect_cb, - const point_cloud_transport::SubscriberStatusCallback& disconnect_cb, - const ros::VoidPtr& tracked_object, bool latch, - const point_cloud_transport::PubLoaderPtr& loader); - struct Impl; - typedef boost::shared_ptr ImplPtr; - typedef boost::weak_ptr ImplWPtr; - - ImplPtr impl_; - - static void weakSubscriberCb(const ImplWPtr& impl_wptr, - const point_cloud_transport::SingleSubscriberPublisher& plugin_pub, - const point_cloud_transport::SubscriberStatusCallback& user_cb); - - point_cloud_transport::SubscriberStatusCallback rebindCB( - const point_cloud_transport::SubscriberStatusCallback& user_cb); - + std::shared_ptr impl_; friend class PointCloudTransport; }; diff --git a/include/point_cloud_transport/publisher_plugin.h b/include/point_cloud_transport/publisher_plugin.h deleted file mode 100644 index eaf2345..0000000 --- a/include/point_cloud_transport/publisher_plugin.h +++ /dev/null @@ -1,184 +0,0 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. - -/* - * - * BSD 3-Clause License - * - * Copyright (c) Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - */ - -#pragma once - -#include -#include - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace point_cloud_transport -{ - -//! Base class for plugins to Publisher. -class PublisherPlugin : public cras::HasLogger, boost::noncopyable -{ -public: - // There has to be cras::ShapeShifter instead of topic_tools::ShapeShifter to avoid Melodic memory corruption issues. - //! \brief Result of cloud encoding. Either a shapeshifter holding the compressed cloud, empty value or error message. - typedef cras::expected, std::string> EncodeResult; - - explicit PublisherPlugin(const cras::LogHelperPtr& log = std::make_shared()) : - cras::HasLogger(log) - { - } - - virtual ~PublisherPlugin() = default; - - //! Get a string identifier for the transport provided by this plugin - virtual std::string getTransportName() const = 0; - - /** - * Whether the given topic and datatype match this transport. - */ - virtual bool matchesTopic(const std::string& topic, const std::string& datatype) const = 0; - - /** - * \brief Encode the given raw pointcloud into the given shapeshifter object. - * \param[in] raw The input raw pointcloud. - * \param[in] config Config of the compression (if it has any parameters). - * \return The output shapeshifter holding the compressed cloud message (if encoding succeeds), or an error message. - */ - virtual EncodeResult encode(const sensor_msgs::PointCloud2& raw, const dynamic_reconfigure::Config& config) const = 0; - - /** - * \brief Encode the given raw pointcloud into the given shapeshifter object using default configuration. - * \param[in] raw The input raw pointcloud. - * \return The output shapeshifter holding the compressed cloud message (if encoding succeeds), or an error message. - */ - EncodeResult encode(const sensor_msgs::PointCloud2& raw) const; - - /** - * \brief Encode the given raw pointcloud into the given shapeshifter object. - * \param[in] raw The input raw pointcloud. - * \param[in] config Config of the compression (if it has any parameters). Pass a XmlRpc dict. - * \return The output shapeshifter holding the compressed cloud message (if encoding succeeds), or an error message. - */ - EncodeResult encode(const sensor_msgs::PointCloud2& raw, const XmlRpc::XmlRpcValue& config) const; - - /** - * \brief Encode the given raw pointcloud into the given shapeshifter object. - * \tparam Config Type of the config object. This should be the generated dynamic_reconfigure interface of the - * corresponding point_cloud_transport publisher. - * \param[in] raw The input raw pointcloud. - * \param[in] config Config of the compression (if it has any parameters). - * \return The output shapeshifter holding the compressed cloud message (if encoding succeeds), or an error message. - */ - template - EncodeResult encode(const sensor_msgs::PointCloud2& raw, const Config& config) const - { - dynamic_reconfigure::Config configMsg; - config.__toMessage__(configMsg); - return this->encode(raw, configMsg); - } - - //! Advertise a topic, simple version. - void advertise(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, bool latch = true); - - //! Advertise a topic with subscriber status callbacks. - void advertise(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, - const point_cloud_transport::SubscriberStatusCallback& connect_cb, - const point_cloud_transport::SubscriberStatusCallback& disconnect_cb = {}, - const ros::VoidPtr& tracked_object = {}, bool latch = true); - - //! Returns the number of subscribers that are currently connected to this PublisherPlugin - virtual uint32_t getNumSubscribers() const = 0; - - //! Returns the topic that this PublisherPlugin will publish on. - virtual std::string getTopic() const = 0; - - //! Publish a point cloud using the transport associated with this PublisherPlugin. - virtual void publish(const sensor_msgs::PointCloud2& message) const = 0; - - //! Publish a point cloud using the transport associated with this PublisherPlugin. - virtual void publish(const sensor_msgs::PointCloud2ConstPtr& message) const; - - //! Shutdown any advertisements associated with this PublisherPlugin. - virtual void shutdown() = 0; - - //! Return the lookup name of the PublisherPlugin associated with a specific transport identifier. - static std::string getLookupName(const std::string& transport_name); - -protected: - //! Advertise a topic. Must be implemented by the subclass. - virtual void advertiseImpl(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, - const point_cloud_transport::SubscriberStatusCallback& connect_cb, - const point_cloud_transport::SubscriberStatusCallback& disconnect_cb, - const ros::VoidPtr& tracked_object, bool latch) = 0; -}; - -class SingleTopicPublisherPlugin : public PublisherPlugin -{ -public: - /** - * Return the communication topic name for a given base topic. - * - * Defaults to \/\. - */ - virtual std::string getTopicToAdvertise(const std::string& base_topic) const = 0; - - /** - * Return the datatype of the transported messages (as text in the form `package/Message`). - */ - virtual std::string getDataType() const = 0; - - /** - * Return the datatype of the dynamic reconfigure (as text in the form `package/Config`). - * - * Return empty string if no reconfiguration is supported. - */ - virtual std::string getConfigDataType() const = 0; -}; - -} diff --git a/include/point_cloud_transport/publisher_plugin.hpp b/include/point_cloud_transport/publisher_plugin.hpp new file mode 100644 index 0000000..ce1eb70 --- /dev/null +++ b/include/point_cloud_transport/publisher_plugin.hpp @@ -0,0 +1,127 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. + +/* + * + * BSD 3-Clause License + * + * Copyright (c) Czech Technical University in Prague + * Copyright (c) 2019, paplhjak + * 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: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include + +#include "rclcpp/node.hpp" +#include + +#include + +namespace point_cloud_transport +{ + +//! Base class for plugins to Publisher. +class PublisherPlugin +{ +public: + PublisherPlugin() = default; + PublisherPlugin(const PublisherPlugin &) = delete; + PublisherPlugin & operator=(const PublisherPlugin &) = delete; + + //! Get a string identifier for the transport provided by this plugin + virtual std::string getTransportName() const = 0; + + /** + * \brief Advertise a topic, simple version. + */ + void advertise( + rclcpp::Node * nh, + const std::string & base_topic, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default) + { + advertiseImpl(nh, base_topic, custom_qos); + } + + //! Returns the number of subscribers that are currently connected to this PublisherPlugin + virtual uint32_t getNumSubscribers() const = 0; + + //! Returns the topic that this PublisherPlugin will publish on. + virtual std::string getTopic() const = 0; + + //! Publish a point cloud using the transport associated with this PublisherPlugin. + virtual void publish(const sensor_msgs::msg::PointCloud2& message) const = 0; + + //! Publish a point cloud using the transport associated with this PublisherPlugin. + virtual void publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& message) const; + + //! Shutdown any advertisements associated with this PublisherPlugin. + virtual void shutdown() = 0; + + //! Return the lookup name of the PublisherPlugin associated with a specific transport identifier. + static std::string getLookupName(const std::string& transport_name); + +protected: + /** + * \brief Advertise a topic. Must be implemented by the subclass. + */ + virtual void advertiseImpl( + rclcpp::Node * nh, const std::string & base_topic, + rmw_qos_profile_t custom_qos) = 0; +}; + +class SingleTopicPublisherPlugin : public PublisherPlugin +{ +public: + /** + * Return the communication topic name for a given base topic. + * + * Defaults to \/\. + */ + virtual std::string getTopicToAdvertise(const std::string& base_topic) const = 0; + + /** + * Return the datatype of the transported messages (as text in the form `package/Message`). + */ + virtual std::string getDataType() const = 0; + + /** + * Return the datatype of the dynamic reconfigure (as text in the form `package/Config`). + * + * Return empty string if no reconfiguration is supported. + */ + virtual std::string getConfigDataType() const = 0; +}; + +} diff --git a/include/point_cloud_transport/raw_publisher.h b/include/point_cloud_transport/raw_publisher.hpp similarity index 71% rename from include/point_cloud_transport/raw_publisher.h rename to include/point_cloud_transport/raw_publisher.hpp index e2bcbca..d1cce0b 100644 --- a/include/point_cloud_transport/raw_publisher.h +++ b/include/point_cloud_transport/raw_publisher.hpp @@ -42,34 +42,40 @@ #include -#include +#include -#include -#include +#include namespace point_cloud_transport { //! RawPublisher is a simple wrapper for ros::Publisher, //! publishing unaltered PointCloud2 messages on the base topic. -class RawPublisher : public point_cloud_transport::SimplePublisherPlugin +class RawPublisher : public point_cloud_transport::SimplePublisherPlugin { public: - std::string getTransportName() const override; + virtual ~RawPublisher() {} - TypedEncodeResult encodeTyped( - const sensor_msgs::PointCloud2& raw, const point_cloud_transport::NoConfigConfig& config) const override; - - // Override the default implementation because publishing the message pointer allows - // the no-copy intraprocess optimization. - void publish(const sensor_msgs::PointCloud2ConstPtr& message) const override; - - bool matchesTopic(const std::string& topic, const std::string& datatype) const override; + virtual std::string getTransportName() const + { + return "raw"; + } protected: - void publish(const sensor_msgs::PointCloud2& message, const PublishFn& publish_fn) const override; + virtual void publish(const sensor_msgs::msg::PointCloud2 & message, const PublishFn & publish_fn) const + { + publish_fn(message); + } + + virtual void publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message_ptr, const PublishFn & publish_fn) const + { + publish_fn(*message_ptr); + } - std::string getTopicToAdvertise(const std::string& base_topic) const override; + virtual std::string getTopicToAdvertise(const std::string & base_topic) const + { + return base_topic; + } }; } diff --git a/include/point_cloud_transport/raw_subscriber.h b/include/point_cloud_transport/raw_subscriber.hpp similarity index 78% rename from include/point_cloud_transport/raw_subscriber.h rename to include/point_cloud_transport/raw_subscriber.hpp index abef686..af1c281 100644 --- a/include/point_cloud_transport/raw_subscriber.h +++ b/include/point_cloud_transport/raw_subscriber.hpp @@ -41,10 +41,11 @@ #pragma once #include +#include -#include +#include -#include +#include namespace point_cloud_transport { @@ -55,21 +56,29 @@ namespace point_cloud_transport * RawSubscriber is a simple wrapper for ros::Subscriber which listens for PointCloud2 messages * and passes them through to the callback. */ -class RawSubscriber : public point_cloud_transport::SimpleSubscriberPlugin +class RawSubscriber : public point_cloud_transport::SimpleSubscriberPlugin { public: - std::string getTransportName() const override; - - DecodeResult decodeTyped(const sensor_msgs::PointCloud2& compressed, const NoConfigConfig& config) const override; - DecodeResult decodeTyped(const sensor_msgs::PointCloud2ConstPtr& compressed, - const NoConfigConfig& config) const override; + virtual ~RawSubscriber() {} - bool matchesTopic(const std::string& topic, const std::string& datatype) const override; + std::string getTransportName() const override; protected: - void callback(const sensor_msgs::PointCloud2ConstPtr& message, const Callback& user_cb) override; + void internalCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& message, const Callback& user_cb) override; std::string getTopicToSubscribe(const std::string& base_topic) const override; + + using SubscriberPlugin::subscribeImpl; + + void subscribeImpl( + rclcpp::Node * node, + const std::string & base_topic, + const Callback & callback, + rmw_qos_profile_t custom_qos, + rclcpp::SubscriptionOptions options) override + { + this->subscribeImplWithOptions(node, base_topic, callback, custom_qos, options); + } }; } diff --git a/include/point_cloud_transport/republish.h b/include/point_cloud_transport/republish.hpp similarity index 57% rename from include/point_cloud_transport/republish.h rename to include/point_cloud_transport/republish.hpp index ae45396..bfbcf02 100644 --- a/include/point_cloud_transport/republish.h +++ b/include/point_cloud_transport/republish.hpp @@ -11,35 +11,31 @@ #include -#include - -#include - -#include -#include -#include -#include +#include +#include +#include +#include namespace point_cloud_transport { -//! A node(let) for republishing clouds from one transport to other transports +//! A Node (component) for republishing clouds from one transport to other transports //! //! Usage: republish in_transport in:= [out_transport] out:= //! //! Parameters: //! - `~in_queue_size` (int, default 10): Input queue size. //! - `~out_queue_size` (int, default `in_queue_size`): Output queue size. -class RepublishNodelet : public cras::Nodelet +class RepublishComponent { protected: void onInit() override; std::unique_ptr pct; point_cloud_transport::Subscriber sub; - boost::shared_ptr pub; - boost::shared_ptr pubPlugin; + std::shared_ptr pub; + std::shared_ptr pubPlugin; }; } diff --git a/include/point_cloud_transport/simple_publisher_plugin.h b/include/point_cloud_transport/simple_publisher_plugin.h deleted file mode 100644 index adbdb09..0000000 --- a/include/point_cloud_transport/simple_publisher_plugin.h +++ /dev/null @@ -1,405 +0,0 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. - -/* - * - * BSD 3-Clause License - * - * Copyright (c) Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - */ - -#pragma once - -#include -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -namespace point_cloud_transport -{ - -/** - * \brief Base class to simplify implementing most plugins to Publisher. - * - * This base class vastly simplifies implementing a PublisherPlugin in the common - * case that all communication with the matching SubscriberPlugin happens over a - * single ROS topic using a transport-specific message type. SimplePublisherPlugin - * is templated on the transport-specific message type and publisher dynamic - * reconfigure type. - * - * A subclass need implement only two methods: - * - getTransportName() from PublisherPlugin - * - encodeTyped() - * - * For access to the parameter server and name remappings, use nh(). - * - * getTopicToAdvertise() controls the name of the internal communication topic. - * It defaults to \/\. - * - * \tparam M Type of the published messages. - * \tparam Config Type of the publisher dynamic configuration. - */ -template -class SimplePublisherPlugin : public point_cloud_transport::SingleTopicPublisherPlugin -{ -public: - //! \brief Result of cloud encoding. Either the compressed cloud message, empty value, or error message. - typedef cras::expected, std::string> TypedEncodeResult; - - ~SimplePublisherPlugin() override - { - } - - uint32_t getNumSubscribers() const override - { - if (simple_impl_) - { - return simple_impl_->pub_.getNumSubscribers(); - } - return 0; - } - - std::string getTopic() const override - { - if (simple_impl_) - { - return simple_impl_->pub_.getTopic(); - } - return {}; - } - - bool matchesTopic(const std::string& topic, const std::string& datatype) const override - { - return datatype == ros::message_traits::DataType::value() && - cras::endsWith(topic, std::string("/" + getTransportName())); - } - - void publish(const sensor_msgs::PointCloud2& message) const override - { - if (!simple_impl_ || !simple_impl_->pub_) - { - ROS_ASSERT_MSG(false, "Call to publish() on an invalid point_cloud_transport::SimplePublisherPlugin"); - return; - } - - publish(message, bindInternalPublisher(simple_impl_->pub_)); - } - - void shutdown() override - { - if (simple_impl_) - { - simple_impl_->pub_.shutdown(); - } - } - - /** - * \brief Encode the given raw pointcloud into a compressed message. - * \param[in] raw The input raw pointcloud. - * \param[in] config Config of the compression (if it has any parameters). - * \return The output shapeshifter holding the compressed cloud message (if encoding succeeds), or an error message. - */ - virtual TypedEncodeResult encodeTyped( - const sensor_msgs::PointCloud2& raw, const Config& config) const = 0; - - /** - * \brief Encode the given raw pointcloud into a compressed message with default configuration. - * \param[in] raw The input raw pointcloud. - * \return The output shapeshifter holding the compressed cloud message (if encoding succeeds), or an error message. - */ - virtual TypedEncodeResult encodeTyped(const sensor_msgs::PointCloud2& raw) const - { - return this->encodeTyped(raw, Config::__getDefault__()); - } - - EncodeResult encode(const sensor_msgs::PointCloud2& raw, const dynamic_reconfigure::Config& configMsg) const override - { - Config config = Config::__getDefault__(); - // dynamic_reconfigure has a bug and generates __fromMessage__ with non-const message arg, although it is only read - if (!config.__fromMessage__(const_cast(configMsg))) - { - return cras::make_unexpected( - std::string("Wrong configuration options given to " + this->getTransportName() + " transport encoder.")); - } - - auto res = this->encodeTyped(raw, config); - if (!res) - { - return cras::make_unexpected(res.error()); - } - if (!res.value()) - { - return cras::nullopt; - } - const M& message = res.value().value(); - cras::ShapeShifter shifter; - cras::msgToShapeShifter(message, shifter); - return shifter; - } - -protected: - std::string base_topic_; - typedef dynamic_reconfigure::Server ReconfigureServer; - boost::shared_ptr reconfigure_server_; - Config config_{Config::__getDefault__()}; - - virtual void configCb(Config& config, uint32_t level) - { - config_ = config; - } - - template::value, int> = 0> - void _startDynamicReconfigureServer() - { - this->startDynamicReconfigureServer(); - } - - template::value, int> = 0> - void _startDynamicReconfigureServer() - { - // Do not start reconfigure server if there are no configuration options. - } - - virtual void startDynamicReconfigureServer() - { - // Set up reconfigure server for this topic - reconfigure_server_ = boost::make_shared(this->nh()); - typename ReconfigureServer::CallbackType f = boost::bind(&SimplePublisherPlugin::configCb, this, _1, _2); - reconfigure_server_->setCallback(f); - } - - void advertiseImpl(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, - const point_cloud_transport::SubscriberStatusCallback& user_connect_cb, - const point_cloud_transport::SubscriberStatusCallback& user_disconnect_cb, - const ros::VoidPtr& tracked_object, bool latch) override - { - base_topic_ = base_topic; - std::string transport_topic = getTopicToAdvertise(base_topic); - ros::NodeHandle param_nh(transport_topic); - simple_impl_ = std::make_unique(param_nh); - simple_impl_->pub_ = nh.advertise(transport_topic, queue_size, - bindCB(user_connect_cb, &SimplePublisherPlugin::connectCallback), - bindCB(user_disconnect_cb, &SimplePublisherPlugin::disconnectCallback), - tracked_object, latch); - - this->_startDynamicReconfigureServer(); - } - - //! Generic function for publishing the internal message type. - typedef boost::function PublishFn; - - /** - * Publish a point cloud using the specified publish function. Must be implemented by - * the subclass. - * - * The PublishFn publishes the transport-specific message type. This indirection allows - * SimpleSubscriberPlugin to use this function for both normal broadcast publishing and - * single subscriber publishing (in subscription callbacks). - */ - virtual void publish(const sensor_msgs::PointCloud2& message, const PublishFn& publish_fn) const - { - const auto res = this->encodeTyped(message, config_); - if (!res) - { - ROS_ERROR("Error encoding message by transport %s: %s.", this->getTransportName().c_str(), res.error().c_str()); - } - else if (res.value()) - { - publish_fn(res.value().value()); - } - } - - std::string getTopicToAdvertise(const std::string& base_topic) const override - { - return base_topic + "/" + getTransportName(); - } - - std::string getDataType() const override - { - return ros::message_traits::DataType::value(); - } - - template::value, int> = 0> - std::string _getConfigDataType() const - { - return cras::removeSuffix(cras::replace(cras::getTypeName(), "::", "/"), "Config"); - } - - template::value, int> = 0> - std::string _getConfigDataType() const - { - return {}; - } - - std::string getConfigDataType() const override - { - return _getConfigDataType(); - } - -protected: - /** - * Function called when a subscriber connects to the internal publisher. - * - * Defaults to noop. - */ - virtual void connectCallback(const ros::SingleSubscriberPublisher& pub) - { - } - - /** - * Function called when a subscriber disconnects from the internal publisher. - * - * Defaults to noop. - */ - virtual void disconnectCallback(const ros::SingleSubscriberPublisher& pub) - { - } - - //! Returns the ros::NodeHandle to be used for parameter lookup. - const ros::NodeHandle& nh() const - { - ROS_ASSERT(simple_impl_); - return simple_impl_->param_nh_; - } - - /** - * Returns the internal ros::Publisher. - * This really only exists so RawPublisher can implement no-copy intraprocess message - * passing easily. - */ - const ros::Publisher& getPublisher() const - { - ROS_ASSERT(simple_impl_); - return simple_impl_->pub_; - } - -private: - struct SimplePublisherPluginImpl - { - explicit SimplePublisherPluginImpl(const ros::NodeHandle& nh) - : param_nh_(nh) - { - } - - const ros::NodeHandle param_nh_; - ros::Publisher pub_; - }; - - std::unique_ptr simple_impl_; - - typedef void (SimplePublisherPlugin::*SubscriberStatusMemFn)(const ros::SingleSubscriberPublisher& pub); - - /** - * Binds the user callback to subscriberCB(), which acts as an intermediary to expose - * a publish(PointCloud2) interface to the user while publishing to an internal topic. - */ - ros::SubscriberStatusCallback bindCB(const point_cloud_transport::SubscriberStatusCallback& user_cb, - SubscriberStatusMemFn internal_cb_fn) - { - ros::SubscriberStatusCallback internal_cb = boost::bind(internal_cb_fn, this, _1); - if (user_cb) - { - return boost::bind(&SimplePublisherPlugin::subscriberCB, this, _1, user_cb, internal_cb); - } - else - { - return internal_cb; - } - } - - /** - * Forms the ros::SingleSubscriberPublisher for the internal communication topic into - * a point_cloud_transport::SingleSubscriberPublisher for PointCloud2 messages and passes it - * to the user subscriber status callback. - */ - void subscriberCB(const ros::SingleSubscriberPublisher& ros_ssp, - const point_cloud_transport::SubscriberStatusCallback& user_cb, - const ros::SubscriberStatusCallback& internal_cb) - { - // First call the internal callback (for sending setup headers, etc.) - internal_cb(ros_ssp); - - // Construct a function object for publishing sensor_msgs::PointCloud2 through the - // subclass-implemented publish() using the ros::SingleSubscriberPublisher to send - // messages of the transport-specific type. - typedef void (SimplePublisherPlugin::*PublishMemFn)(const sensor_msgs::PointCloud2&, const PublishFn&) const; - PublishMemFn pub_mem_fn = &SimplePublisherPlugin::publish; - PointCloud2PublishFn point_cloud_publish_fn = boost::bind(pub_mem_fn, this, _1, bindInternalPublisher(ros_ssp)); - - SingleSubscriberPublisher ssp(ros_ssp.getSubscriberName(), getTopic(), - boost::bind(&SimplePublisherPlugin::getNumSubscribers, this), - point_cloud_publish_fn); - user_cb(ssp); - } - - typedef boost::function PointCloud2PublishFn; - - /** - * Returns a function object for publishing the transport-specific message type - * through some ROS publisher type. - * - * @param pub An object with method void publish(const M&) - */ - template - PublishFn bindInternalPublisher(const PubT& pub) const - { - // Bind PubT::publish(const Message&) as PublishFn - typedef void (PubT::*InternalPublishMemFn)(const M&) const; - InternalPublishMemFn internal_pub_mem_fn = &PubT::publish; - return boost::bind(internal_pub_mem_fn, &pub, _1); - } -}; - -} diff --git a/include/point_cloud_transport/simple_publisher_plugin.hpp b/include/point_cloud_transport/simple_publisher_plugin.hpp new file mode 100644 index 0000000..2937151 --- /dev/null +++ b/include/point_cloud_transport/simple_publisher_plugin.hpp @@ -0,0 +1,195 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. + +/* + * + * BSD 3-Clause License + * + * Copyright (c) Czech Technical University in Prague + * Copyright (c) 2019, paplhjak + * 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: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace point_cloud_transport +{ + +/** + * \brief Base class to simplify implementing most plugins to Publisher. + * + * This base class vastly simplifies implementing a PublisherPlugin in the common + * case that all communication with the matching SubscriberPlugin happens over a + * single ROS topic using a transport-specific message type. SimplePublisherPlugin + * is templated on the transport-specific message type and publisher dynamic + * reconfigure type. + * + * A subclass need implement only two methods: + * - getTransportName() from PublisherPlugin + * - encodeTyped() + * + * For access to the parameter server and name remappings, use nh(). + * + * getTopicToAdvertise() controls the name of the internal communication topic. + * It defaults to \/\. + * + * \tparam M Type of the published messages. + * \tparam Config Type of the publisher dynamic configuration. + */ +template +class SimplePublisherPlugin : public point_cloud_transport::SingleTopicPublisherPlugin +{ +public: + ~SimplePublisherPlugin() override + { + } + + uint32_t getNumSubscribers() const override + { + if (simple_impl_) + { + return simple_impl_->pub_->get_subscription_count(); + } + return 0; + } + + std::string getTopic() const override + { + if (simple_impl_) + { + return simple_impl_->pub_->get_topic_name(); + } + return {}; + } + + void publish(const sensor_msgs::msg::PointCloud2& message) const override + { + if (!simple_impl_ || !simple_impl_->pub_) + { + auto logger = simple_impl_ ? simple_impl_->logger_ : rclcpp::get_logger("point_cloud_transport"); + RCLCPP_ERROR( + logger, + "Call to publish() on an invalid point_cloud_transport::SimplePublisherPlugin"); + return; + } + + publish(message, bindInternalPublisher(simple_impl_->pub_.get())); + } + + void shutdown() override + { + simple_impl_.reset(); + } + +protected: + virtual void advertiseImpl( + rclcpp::Node * node, const std::string & base_topic, + rmw_qos_profile_t custom_qos) + { + std::string transport_topic = getTopicToAdvertise(base_topic); + simple_impl_ = std::make_unique(node); + + RCLCPP_DEBUG(simple_impl_->logger_, "getTopicToAdvertise: %s", transport_topic.c_str()); + auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos), custom_qos); + simple_impl_->pub_ = node->create_publisher(transport_topic, qos); + } + + //! Generic function for publishing the internal message type. + typedef std::function PublishFn; + + /** + * \brief Publish an pointcloud using the specified publish function. Must be implemented by + * the subclass. + * + * The PublishFn publishes the transport-specific message type. This indirection allows + * SimpleSubscriberPlugin to use this function for both normal broadcast publishing and + * single subscriber publishing (in subscription callbacks). + */ + virtual void publish( + const sensor_msgs::msg::PointCloud2 & message, + const PublishFn & publish_fn) const = 0; + + /** + * \brief Return the communication topic name for a given base topic. + * + * Defaults to \/\. + */ + virtual std::string getTopicToAdvertise(const std::string & base_topic) const + { + return base_topic + "/" + getTransportName(); + } + +private: + struct SimplePublisherPluginImpl + { + explicit SimplePublisherPluginImpl(rclcpp::Node * node) + : node_(node), + logger_(node->get_logger()) + { + } + + rclcpp::Node * node_; + rclcpp::Logger logger_; + typename rclcpp::Publisher::SharedPtr pub_; + }; + + std::unique_ptr simple_impl_; + + typedef std::function PointCloudPublishFn; + + /** + * Returns a function object for publishing the transport-specific message type + * through some ROS publisher type. + * + * @param pub An object with method void publish(const M&) + */ + template + PublishFn bindInternalPublisher(PubT * pub) const + { + // Bind PubT::publish(const Message&) as PublishFn + typedef void (PubT::* InternalPublishMemFn)(const M &); + InternalPublishMemFn internal_pub_mem_fn = &PubT::publish; + return std::bind(internal_pub_mem_fn, pub, std::placeholders::_1); + } +}; + +} diff --git a/include/point_cloud_transport/simple_subscriber_plugin.h b/include/point_cloud_transport/simple_subscriber_plugin.h deleted file mode 100644 index a65f8e8..0000000 --- a/include/point_cloud_transport/simple_subscriber_plugin.h +++ /dev/null @@ -1,294 +0,0 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. - -/* - * - * BSD 3-Clause License - * - * Copyright (c) Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - */ - -#pragma once - -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -namespace point_cloud_transport -{ - -/** - * Base class to simplify implementing most plugins to Subscriber. - * - * The base class simplifies implementing a SubscriberPlugin in the common case that - * all communication with the matching PublisherPlugin happens over a single ROS - * topic using a transport-specific message type. SimpleSubscriberPlugin is templated - * on the transport-specific message type. - * - * A subclass need implement only two methods: - * - getTransportName() from SubscriberPlugin - * - internalCallback() - processes a message and invoked the user PointCloud2 callback if - * appropriate. - * - * For access to the parameter server and name remappings, use nh(). - * - * getTopicToSubscribe() controls the name of the internal communication topic. It - * defaults to \/\. - */ -template -class SimpleSubscriberPlugin : public SingleTopicSubscriberPlugin -{ -public: - ~SimpleSubscriberPlugin() override - { - } - - std::string getTopic() const override - { - if (simple_impl_) - { - return simple_impl_->sub_.getTopic(); - } - return {}; - } - - bool matchesTopic(const std::string& topic, const std::string& datatype) const override - { - return datatype == ros::message_traits::DataType::value() && - cras::endsWith(topic, std::string("/" + getTransportName())); - } - - uint32_t getNumPublishers() const override - { - if (simple_impl_) - { - return simple_impl_->sub_.getNumPublishers(); - } - return 0; - } - - void shutdown() override - { - reconfigure_server_.reset(); - if (simple_impl_) - { - simple_impl_->sub_.shutdown(); - } - } - - /** - * \brief Decode the given compressed pointcloud into a raw message. - * \param[in] compressed The input compressed pointcloud. - * \param[in] config Config of the decompression (if it has any parameters). - * \return The raw cloud message (if encoding succeeds), or an error message. - */ - virtual DecodeResult decodeTyped(const M& compressed, const Config& config) const = 0; - - /** - * \brief Decode the given compressed pointcloud into a raw message. - * \param[in] compressed The input compressed pointcloud. - * \param[in] config Config of the decompression (if it has any parameters). - * \return The raw cloud message (if encoding succeeds), or an error message. - */ - virtual DecodeResult decodeTyped(const typename M::ConstPtr& compressed, const Config& config) const - { - return this->decodeTyped(*compressed, config); - } - - /** - * \brief Decode the given compressed pointcloud into a raw message with default configuration. - * \param[in] compressed The input compressed pointcloud. - * \param[in] config Config of the decompression (if it has any parameters). - * \return The raw cloud message (if encoding succeeds), or an error message. - */ - virtual DecodeResult decodeTyped(const M& compressed) const - { - return this->decodeTyped(compressed, Config::__getDefault__()); - } - - DecodeResult decode(const topic_tools::ShapeShifter& compressed, - const dynamic_reconfigure::Config& configMsg) const override - { - Config config = Config::__getDefault__(); - // dynamic_reconfigure has a bug and generates __fromMessage__ with non-const message arg, although it is only read - if (!config.__fromMessage__(const_cast(configMsg))) - { - return cras::make_unexpected( - std::string("Wrong configuration options given to " + this->getTransportName() + " transport decoder.")); - } - - typename M::ConstPtr msg; - try - { - msg = compressed.instantiate(); - } - catch (const ros::Exception& e) - { - return cras::make_unexpected(cras::format("Invalid shapeshifter passed to transport decoder: %s.", e.what())); - } - - return this->decodeTyped(msg, config); - } - -protected: - std::string base_topic_; - typedef dynamic_reconfigure::Server ReconfigureServer; - boost::shared_ptr reconfigure_server_; - Config config_{Config::__getDefault__()}; - - virtual void configCb(Config& config, uint32_t level) - { - config_ = config; - } - - template::value, int> = 0> - void _startDynamicReconfigureServer() - { - this->startDynamicReconfigureServer(); - } - - template::value, int> = 0> - void _startDynamicReconfigureServer() - { - // Do not start reconfigure server if there are no configuration options. - } - - virtual void startDynamicReconfigureServer() - { - // Set up reconfigure server for this topic - reconfigure_server_ = boost::make_shared(this->nh()); - typename ReconfigureServer::CallbackType f = - boost::bind(&SimpleSubscriberPlugin::configCb, this, _1, _2); - reconfigure_server_->setCallback(f); - } - - /** - * Process a message. Must be implemented by the subclass. - */ - virtual void callback(const typename M::ConstPtr& message, const Callback& user_cb) - { - DecodeResult res = this->decodeTyped(message, config_); - if (!res) - ROS_ERROR("Error decoding message by transport %s: %s.", this->getTransportName().c_str(), res.error().c_str()); - else if (res.value()) - { - user_cb(res.value().value()); - } - } - - std::string getTopicToSubscribe(const std::string& base_topic) const override - { - return base_topic + "/" + getTransportName(); - } - - std::string getDataType() const override - { - return ros::message_traits::DataType::value(); - } - - template::value, int> = 0> - std::string _getConfigDataType() const - { - return cras::removeSuffix(cras::replace(cras::getTypeName(), "::", "/"), "Config"); - } - - template::value, int> = 0> - std::string _getConfigDataType() const - { - return {}; - } - - std::string getConfigDataType() const override - { - return _getConfigDataType(); - } - - void subscribeImpl(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, - const Callback& callback, const ros::VoidPtr& tracked_object, - const point_cloud_transport::TransportHints& transport_hints, - bool allow_concurrent_callbacks) override - { - // Push each group of transport-specific parameters into a separate sub-namespace - ros::NodeHandle param_nh(transport_hints.getParameterNH(), getTransportName()); - simple_impl_ = std::make_unique(param_nh); - - ros::SubscribeOptions ops; - ops.init(getTopicToSubscribe(base_topic), queue_size, - boost::bind(&SimpleSubscriberPlugin::callback, this, _1, callback)); - ops.tracked_object = tracked_object; - ops.transport_hints = transport_hints.getRosHints(); - ops.allow_concurrent_callbacks = allow_concurrent_callbacks; - - simple_impl_->sub_ = nh.subscribe(ops); - - this->_startDynamicReconfigureServer(); - } - - /** - * Returns the ros::NodeHandle to be used for parameter lookup. - */ - const ros::NodeHandle& nh() const - { - return simple_impl_->param_nh_; - } - -private: - struct SimpleSubscriberPluginImpl - { - explicit SimpleSubscriberPluginImpl(const ros::NodeHandle& nh) - : param_nh_(nh) - { - } - - const ros::NodeHandle param_nh_; - ros::Subscriber sub_; - }; - - std::unique_ptr simple_impl_; -}; - -} diff --git a/include/point_cloud_transport/simple_subscriber_plugin.hpp b/include/point_cloud_transport/simple_subscriber_plugin.hpp new file mode 100644 index 0000000..daeb941 --- /dev/null +++ b/include/point_cloud_transport/simple_subscriber_plugin.hpp @@ -0,0 +1,173 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. + +/* + * + * BSD 3-Clause License + * + * Copyright (c) Czech Technical University in Prague + * Copyright (c) 2019, paplhjak + * 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: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include +#include + +#include "rclcpp/subscription.hpp" + +#include + +namespace point_cloud_transport +{ + +/** + * Base class to simplify implementing most plugins to Subscriber. + * + * The base class simplifies implementing a SubscriberPlugin in the common case that + * all communication with the matching PublisherPlugin happens over a single ROS + * topic using a transport-specific message type. SimpleSubscriberPlugin is templated + * on the transport-specific message type. + * + * A subclass need implement only two methods: + * - getTransportName() from SubscriberPlugin + * - internalCallback() - processes a message and invoked the user PointCloud2 callback if + * appropriate. + * + * For access to the parameter server and name remappings, use nh(). + * + * getTopicToSubscribe() controls the name of the internal communication topic. It + * defaults to \/\. + */ +template +class SimpleSubscriberPlugin : public SingleTopicSubscriberPlugin +{ +public: + virtual ~SimpleSubscriberPlugin() + { + } + + std::string getTopic() const override + { + if (impl_) + { + return impl_->sub_->get_topic_name(); + } + return {}; + } + + uint32_t getNumPublishers() const override + { + if (impl_) + { + return impl_->sub_->get_publisher_count(); + } + return 0; + } + + void shutdown() override + { + impl_.reset(); + } + +protected: + /** + * \brief Process a message. Must be implemented by the subclass. + * + * @param message A message from the PublisherPlugin. + * @param user_cb The user PointCloud2 callback to invoke, if appropriate. + */ + + virtual void internalCallback( + const typename std::shared_ptr & message, + const Callback & user_cb) = 0; + + /** + * \brief Return the communication topic name for a given base topic. + * + * Defaults to \/\. + */ + virtual std::string getTopicToSubscribe(const std::string & base_topic) const + { + return base_topic + "/" + getTransportName(); + } + + void subscribeImpl( + rclcpp::Node * node, + const std::string & base_topic, + const Callback & callback, + rmw_qos_profile_t custom_qos) override + { + impl_ = std::make_unique(); + // Push each group of transport-specific parameters into a separate sub-namespace + // ros::NodeHandle param_nh(transport_hints.getParameterNH(), getTransportName()); + // + auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos), custom_qos); + impl_->sub_ = node->create_subscription( + getTopicToSubscribe(base_topic), qos, + [this, callback](const typename std::shared_ptr msg) { + internalCallback(msg, callback); + }); + } + + void subscribeImplWithOptions( + rclcpp::Node * node, + const std::string & base_topic, + const Callback & callback, + rmw_qos_profile_t custom_qos, + rclcpp::SubscriptionOptions options) + { + impl_ = std::make_unique(); + // Push each group of transport-specific parameters into a separate sub-namespace + // ros::NodeHandle param_nh(transport_hints.getParameterNH(), getTransportName()); + // + auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos), custom_qos); + impl_->sub_ = node->create_subscription( + getTopicToSubscribe(base_topic), qos, + [this, callback](const typename std::shared_ptr msg) { + internalCallback(msg, callback); + }, + options); + } + +private: + struct Impl + { + rclcpp::SubscriptionBase::SharedPtr sub_; + }; + + std::unique_ptr impl_; +}; + +} diff --git a/include/point_cloud_transport/single_subscriber_publisher.h b/include/point_cloud_transport/single_subscriber_publisher.hpp similarity index 79% rename from include/point_cloud_transport/single_subscriber_publisher.h rename to include/point_cloud_transport/single_subscriber_publisher.hpp index 3349100..2982fa0 100644 --- a/include/point_cloud_transport/single_subscriber_publisher.h +++ b/include/point_cloud_transport/single_subscriber_publisher.hpp @@ -40,22 +40,24 @@ #pragma once +#include #include -#include -#include - -#include +#include "rclcpp/macros.hpp" +#include namespace point_cloud_transport { //! Allows publication of a point cloud to a single subscriber. Only available inside subscriber connection callbacks. -class SingleSubscriberPublisher : boost::noncopyable +class SingleSubscriberPublisher { public: - typedef boost::function GetNumSubscribersFn; - typedef boost::function PublishFn; + typedef std::function GetNumSubscribersFn; + typedef std::function PublishFn; + + SingleSubscriberPublisher(const SingleSubscriberPublisher &) = delete; + SingleSubscriberPublisher & operator=(const SingleSubscriberPublisher &) = delete; SingleSubscriberPublisher(const std::string& caller_id, const std::string& topic, const GetNumSubscribersFn& num_subscribers_fn, @@ -67,8 +69,8 @@ class SingleSubscriberPublisher : boost::noncopyable uint32_t getNumSubscribers() const; - void publish(const sensor_msgs::PointCloud2& message) const; - void publish(const sensor_msgs::PointCloud2ConstPtr& message) const; + void publish(const sensor_msgs::msg::PointCloud2& message) const; + void publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& message) const; private: std::string caller_id_; @@ -79,6 +81,6 @@ class SingleSubscriberPublisher : boost::noncopyable friend class Publisher; // to get publish_fn_ directly }; -typedef boost::function SubscriberStatusCallback; +typedef std::function SubscriberStatusCallback; -} +} // namespace point_cloud_transport diff --git a/include/point_cloud_transport/subscriber.h b/include/point_cloud_transport/subscriber.hpp similarity index 82% rename from include/point_cloud_transport/subscriber.h rename to include/point_cloud_transport/subscriber.hpp index d2729e7..e20d340 100644 --- a/include/point_cloud_transport/subscriber.h +++ b/include/point_cloud_transport/subscriber.hpp @@ -40,18 +40,18 @@ #pragma once +#include +#include #include -#include -#include -#include +#include "rclcpp/node.hpp" -#include -#include -#include +#include -#include -#include +#include "pluginlib/class_loader.hpp" + +#include +#include namespace point_cloud_transport { @@ -73,7 +73,18 @@ namespace point_cloud_transport { class Subscriber { public: - Subscriber(); + typedef std::function Callback; + + Subscriber() = default; + + Subscriber( + rclcpp::Node * node, + const std::string & base_topic, + const Callback & callback, + SubLoaderPtr loader, + const std::string & transport, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default, + rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()); /** * Returns the base point cloud topic. @@ -116,17 +127,9 @@ class Subscriber } private: - Subscriber(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, - const boost::function& callback, - const ros::VoidPtr& tracked_object, const point_cloud_transport::TransportHints& transport_hints, - bool allow_concurrent_callbacks, const point_cloud_transport::SubLoaderPtr& loader); struct Impl; - typedef boost::shared_ptr ImplPtr; - typedef boost::weak_ptr ImplWPtr; - - ImplPtr impl_; - + std::shared_ptr impl_; friend class PointCloudTransport; }; diff --git a/include/point_cloud_transport/subscriber_filter.h b/include/point_cloud_transport/subscriber_filter.hpp similarity index 77% rename from include/point_cloud_transport/subscriber_filter.h rename to include/point_cloud_transport/subscriber_filter.hpp index e07bd63..766bc03 100644 --- a/include/point_cloud_transport/subscriber_filter.h +++ b/include/point_cloud_transport/subscriber_filter.hpp @@ -40,16 +40,14 @@ #pragma once +#include #include -#include -#include - #include -#include +#include -#include -#include +#include +#include namespace point_cloud_transport { @@ -67,24 +65,25 @@ namespace point_cloud_transport * * SubscriberFilter has no input connection. * - * The output connection for the SubscriberFilter object is the same signature as for roscpp + * The output connection for the SubscriberFilter object is the same signature as for rclcpp * subscription callbacks, ie. */ -class SubscriberFilter : public message_filters::SimpleFilter +class SubscriberFilter : public message_filters::SimpleFilter { public: /** * Constructor * - * \param pct The transport to use. + * \param nh The ros::NodeHandle to use to subscribe. * \param base_topic The topic to subscribe to. * \param queue_size The subscription queue size - * \param transport_hints The transport hints to pass along + * \param transport The transport hint to pass along */ - SubscriberFilter(PointCloudTransport& pct, const std::string& base_topic, uint32_t queue_size, - const point_cloud_transport::TransportHints& transport_hints = {}) + SubscriberFilter( + rclcpp::Node * node, const std::string & base_topic, + const std::string & transport) { - subscribe(pct, base_topic, queue_size, transport_hints); + subscribe(node, base_topic, transport); } /** @@ -104,17 +103,21 @@ class SubscriberFilter : public message_filters::SimpleFilter -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -namespace point_cloud_transport -{ - -/** - * Base class for plugins to Subscriber. - */ -class SubscriberPlugin : public cras::HasLogger, boost::noncopyable -{ -public: - typedef boost::function Callback; - - //! \brief Result of cloud decoding. Either a `sensor_msgs::PointCloud2` holding the raw message, empty value or - //! error message. - typedef cras::expected, std::string> DecodeResult; - - explicit SubscriberPlugin(const cras::LogHelperPtr& log = std::make_shared()) : - cras::HasLogger(log) - { - } - - virtual ~SubscriberPlugin() = default; - - /** - * Get a string identifier for the transport provided by - * this plugin. - */ - virtual std::string getTransportName() const = 0; - - /** - * Whether the given topic and datatype match this transport. - */ - virtual bool matchesTopic(const std::string& topic, const std::string& datatype) const = 0; - - /** - * \brief Decode the given compressed pointcloud into a raw cloud. - * \param[in] compressed The shapeshifter of the compressed pointcloud to be decoded. - * \param[in] config Config of the decompression (if it has any parameters). - * \return The decoded raw pointcloud (if decoding succeeds), or an error message. - */ - virtual DecodeResult decode(const topic_tools::ShapeShifter& compressed, - const dynamic_reconfigure::Config& config) const = 0; - - /** - * \brief Decode the given compressed pointcloud into a raw cloud using default config. - * \param[in] compressed The shapeshifter of the compressed pointcloud to be decoded. - * \return The decoded raw pointcloud (if decoding succeeds), or an error message. - */ - DecodeResult decode(const topic_tools::ShapeShifter& compressed) const - { - return this->decode(compressed, dynamic_reconfigure::Config()); - } - - /** - * \brief Decode the given compressed pointcloud into a raw cloud. - * \param[in] compressed The shapeshifter of the compressed pointcloud to be decoded. - * \param[in] config Config of the decompression (if it has any parameters). Pass a XmlRpc dict. - * \return The decoded raw pointcloud (if decoding succeeds), or an error message. - */ - DecodeResult decode(const topic_tools::ShapeShifter& compressed, const XmlRpc::XmlRpcValue& config) const - { - dynamic_reconfigure::Config configMsg; - std::list errors; - if (!cras::convert(config, configMsg, true, &errors)) - { - return cras::make_unexpected("Invalid decoder config: " + cras::join(errors, " ")); - } - return this->decode(compressed, configMsg); - } - - /** - * \brief Encode the given raw pointcloud into the given shapeshifter object. - * \tparam Config Type of the config object. This should be the generated dynamic_reconfigure interface of the - * corresponding point_cloud_transport subscriber. - * \param[in] compressed The shapeshifter of the compressed pointcloud to be decoded. - * \param[in] config Config of the decompression (if it has any parameters). - * \return The output shapeshifter holding the compressed cloud message (if encoding succeeds), or an error message. - */ - template - DecodeResult decode(const topic_tools::ShapeShifter& compressed, const Config& config) const - { - dynamic_reconfigure::Config configMsg; - config.__toMessage__(configMsg); - return this->decode(compressed, configMsg); - } - - /** - * Subscribe to a point cloud topic, version for arbitrary boost::function object. - */ - void subscribe(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, - const Callback& callback, const ros::VoidPtr& tracked_object = {}, - const point_cloud_transport::TransportHints& transport_hints = {}, - bool allow_concurrent_callbacks = false) - { - return subscribeImpl(nh, base_topic, queue_size, callback, tracked_object, transport_hints, - allow_concurrent_callbacks); - } - - /** - * Subscribe to a point cloud topic, version for bare function. - */ - void subscribe(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, - void(* fp)(const sensor_msgs::PointCloud2ConstPtr&), - const point_cloud_transport::TransportHints& transport_hints = {}, - bool allow_concurrent_callbacks = false) - { - return subscribe(nh, base_topic, queue_size, - boost::function(fp), - ros::VoidPtr(), transport_hints, allow_concurrent_callbacks); - } - - /** - * Subscribe to a point cloud topic, version for class member function with bare pointer. - */ - template - void subscribe(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, - void(T::*fp)(const sensor_msgs::PointCloud2ConstPtr&), T* obj, - const point_cloud_transport::TransportHints& transport_hints = {}, - bool allow_concurrent_callbacks = false) - { - return subscribe(nh, base_topic, queue_size, boost::bind(fp, obj, _1), ros::VoidPtr(), transport_hints, - allow_concurrent_callbacks); - } - - /** - * Subscribe to a point cloud topic, version for class member function with shared_ptr. - */ - template - void subscribe(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, - void(T::*fp)(const sensor_msgs::PointCloud2ConstPtr&), - const boost::shared_ptr& obj, - const point_cloud_transport::TransportHints& transport_hints = {}, - bool allow_concurrent_callbacks = false) - { - return subscribe(nh, base_topic, queue_size, boost::bind(fp, obj.get(), _1), obj, transport_hints, - allow_concurrent_callbacks); - } - - /** - * Get the transport-specific communication topic. - */ - virtual std::string getTopic() const = 0; - - /** - * Returns the number of publishers this subscriber is connected to. - */ - virtual uint32_t getNumPublishers() const = 0; - - /** - * Unsubscribe the callback associated with this SubscriberPlugin. - */ - virtual void shutdown() = 0; - - /** - * Return the lookup name of the SubscriberPlugin associated with a specific - * transport identifier. - */ - static std::string getLookupName(const std::string& transport_type) - { - return "point_cloud_transport/" + transport_type + "_sub"; - } - -protected: - /** - * Subscribe to a point cloud transport topic. Must be implemented by the subclass. - */ - virtual void subscribeImpl(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, - const Callback& callback, const ros::VoidPtr& tracked_object, - const point_cloud_transport::TransportHints& transport_hints, - bool allow_concurrent_callbacks) = 0; -}; - -class SingleTopicSubscriberPlugin : public SubscriberPlugin -{ -public: - /** - * Return the communication topic name for a given base topic. - * - * Defaults to \/\. - */ - virtual std::string getTopicToSubscribe(const std::string& base_topic) const = 0; - - /** - * Return the datatype of the transported messages as text. - */ - virtual std::string getDataType() const = 0; - - /** - * Return the datatype of the dynamic reconfigure (as text in the form `package/Config`). - * - * Return empty string if no reconfiguration is supported. - */ - virtual std::string getConfigDataType() const = 0; -}; - -} diff --git a/include/point_cloud_transport/subscriber_plugin.hpp b/include/point_cloud_transport/subscriber_plugin.hpp new file mode 100644 index 0000000..2832085 --- /dev/null +++ b/include/point_cloud_transport/subscriber_plugin.hpp @@ -0,0 +1,192 @@ +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. + +/* + * + * BSD 3-Clause License + * + * Copyright (c) Czech Technical University in Prague + * Copyright (c) 2019, paplhjak + * 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: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#pragma once + +#include +#include +#include + +#include "rclcpp/macros.hpp" +#include "rclcpp/node.hpp" +#include + +#include + +namespace point_cloud_transport +{ + +/** + * Base class for plugins to Subscriber. + */ +class SubscriberPlugin +{ +public: + SubscriberPlugin() = default; + SubscriberPlugin(const SubscriberPlugin &) = delete; + SubscriberPlugin & operator=(const SubscriberPlugin &) = delete; + + virtual ~SubscriberPlugin() = default; + + typedef std::function Callback; + + /** + * Get a string identifier for the transport provided by + * this plugin. + */ + virtual std::string getTransportName() const = 0; + + + /** + * \brief Subscribe to an pointcloud topic, version for arbitrary std::function object. + */ + void subscribe( + rclcpp::Node * node, const std::string & base_topic, + const Callback & callback, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default, + rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()) + { + return subscribeImpl(node, base_topic, callback, custom_qos, options); + } + + /** + * \brief Subscribe to an pointcloud topic, version for bare function. + */ + void subscribe( + rclcpp::Node * node, const std::string & base_topic, + void (* fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &), + rmw_qos_profile_t custom_qos = rmw_qos_profile_default, + rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()) + { + return subscribe( + node, base_topic, + std::function(fp), + custom_qos, options); + } + + /** + * \brief Subscribe to an pointcloud topic, version for class member function with bare pointer. + */ + template + void subscribe( + rclcpp::Node * node, const std::string & base_topic, + void (T::* fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &), T * obj, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default, + rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()) + { + return subscribe( + node, base_topic, + std::bind(fp, obj, std::placeholders::_1), custom_qos, options); + } + + /** + * Get the transport-specific communication topic. + */ + virtual std::string getTopic() const = 0; + + /** + * Returns the number of publishers this subscriber is connected to. + */ + virtual uint32_t getNumPublishers() const = 0; + + /** + * Unsubscribe the callback associated with this SubscriberPlugin. + */ + virtual void shutdown() = 0; + + /** + * Return the lookup name of the SubscriberPlugin associated with a specific + * transport identifier. + */ + static std::string getLookupName(const std::string& transport_type) + { + return "point_cloud_transport/" + transport_type + "_sub"; + } + +protected: + /** + * Subscribe to a point cloud transport topic. Must be implemented by the subclass. + */ + virtual void subscribeImpl( + rclcpp::Node * node, + const std::string & base_topic, + const Callback & callback, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default) = 0; + + virtual void subscribeImpl( + rclcpp::Node * node, + const std::string & base_topic, + const Callback & callback, + rmw_qos_profile_t custom_qos, + rclcpp::SubscriptionOptions options) + { + (void) options; + RCLCPP_ERROR( + node->get_logger(), + "SubscriberPlugin::subscribeImpl with five arguments has not been overridden"); + this->subscribeImpl(node, base_topic, callback, custom_qos); + } + +}; + +class SingleTopicSubscriberPlugin : public SubscriberPlugin +{ +public: + /** + * Return the communication topic name for a given base topic. + * + * Defaults to \/\. + */ + virtual std::string getTopicToSubscribe(const std::string& base_topic) const = 0; + + /** + * Return the datatype of the transported messages as text. + */ + virtual std::string getDataType() const = 0; + + /** + * Return the datatype of the dynamic reconfigure (as text in the form `package/Config`). + * + * Return empty string if no reconfiguration is supported. + */ + virtual std::string getConfigDataType() const = 0; +}; + +} diff --git a/include/point_cloud_transport/transport_hints.h b/include/point_cloud_transport/transport_hints.hpp similarity index 78% rename from include/point_cloud_transport/transport_hints.h rename to include/point_cloud_transport/transport_hints.hpp index 7ba4d4b..83eaefa 100644 --- a/include/point_cloud_transport/transport_hints.h +++ b/include/point_cloud_transport/transport_hints.hpp @@ -42,8 +42,7 @@ #include -#include -#include +#include "rclcpp/node.hpp" namespace point_cloud_transport { @@ -59,15 +58,18 @@ class TransportHints * name of the desired transport. By default this parameter is named "point_cloud_transport" * in the node's local namespace. For consistency across ROS applications, the * name of this parameter should not be changed without good reason. + * + * @param node Node to use when looking up the transport parameter. + * @param default_transport Preferred transport to use + * @param parameter_name The name of the transport parameter * */ - TransportHints(const std::string& default_transport = "raw", - const ros::TransportHints& ros_hints = {}, - const ros::NodeHandle& parameter_nh = ros::NodeHandle("~"), - const std::string& parameter_name = "point_cloud_transport") - : ros_hints_(ros_hints), parameter_nh_(parameter_nh) + TransportHints( + const rclcpp::Node * node, + const std::string & default_transport = "raw", + const std::string& parameter_name = "point_cloud_transport") { - parameter_nh_.param(parameter_name, transport_, default_transport); + node->get_parameter_or(parameter_name, transport_, default_transport); } const std::string& getTransport() const @@ -75,20 +77,8 @@ class TransportHints return transport_; } - const ros::TransportHints& getRosHints() const - { - return ros_hints_; - } - - const ros::NodeHandle& getParameterNH() const - { - return parameter_nh_; - } - private: std::string transport_; - ros::TransportHints ros_hints_; - ros::NodeHandle parameter_nh_; }; } diff --git a/package.xml b/package.xml index 90211c0..bc9c6a8 100644 --- a/package.xml +++ b/package.xml @@ -1,50 +1,38 @@ point_cloud_transport + 1.0.11 + Support for transporting PointCloud2 messages in compressed format and plugin interface for implementing additional PointCloud2 transports. + Jakub Paplham + Martin Pecka + John D'Angelo + BSD https://github.com/ctu-vras/point_cloud_transport https://wiki.ros.org/point_cloud_transport https://github.com/ctu-vras/point_cloud_transport/issues - catkin - python-setuptools - python3-setuptools + ament_cmake_ros - message_generation - pluginlib - - message_filters - message_runtime - - cras_py_common - message_runtime - pluginlib - - cras_cpp_common - cras_topic_tools - dynamic_reconfigure - nodelet - roscpp + message_filters + pluginlib + rclcpp_components + rclcpp sensor_msgs - topic_tools - - python-catkin-lint - python3-catkin-lint - roslint - cras_docs_common - rosdoc_lite + ament_cmake_gtest + ament_lint_auto + ament_lint_common + ament_cmake - - diff --git a/src/list_transports.cpp b/src/list_transports.cpp index 10b59c2..5e00721 100644 --- a/src/list_transports.cpp +++ b/src/list_transports.cpp @@ -42,16 +42,12 @@ #include #include -#include -#include +#include +#include -#include - -#include -#include - -using namespace point_cloud_transport; -using namespace pluginlib; +#include +#include +#include enum PluginStatus { @@ -73,26 +69,26 @@ struct TransportDesc int main(int argc, char** argv) { - ClassLoader pub_loader("point_cloud_transport", "point_cloud_transport::PublisherPlugin"); - ClassLoader sub_loader("point_cloud_transport", "point_cloud_transport::SubscriberPlugin"); + pluginlib::ClassLoader pub_loader("point_cloud_transport", "point_cloud_transport::PublisherPlugin"); + pluginlib::ClassLoader sub_loader("point_cloud_transport", "point_cloud_transport::SubscriberPlugin"); typedef std::map StatusMap; StatusMap transports; for (const auto& lookup_name : pub_loader.getDeclaredClasses()) { - std::string transport_name = boost::erase_last_copy(lookup_name, "_pub"); + std::string transport_name = point_cloud_transport::erase_last_copy(lookup_name, "_pub"); transports[transport_name].pub_name = lookup_name; transports[transport_name].package_name = pub_loader.getClassPackage(lookup_name); try { - boost::shared_ptr pub = pub_loader.createInstance(lookup_name); + auto pub = pub_loader.createSharedInstance(lookup_name); transports[transport_name].pub_status = SUCCESS; } - catch (const LibraryLoadException& e) + catch (const pluginlib::LibraryLoadException& e) { transports[transport_name].pub_status = LIB_LOAD_FAILURE; } - catch (const CreateClassException& e) + catch (const pluginlib::CreateClassException& e) { transports[transport_name].pub_status = CREATE_FAILURE; } @@ -100,19 +96,19 @@ int main(int argc, char** argv) for (const auto& lookup_name : sub_loader.getDeclaredClasses()) { - std::string transport_name = boost::erase_last_copy(lookup_name, "_sub"); + std::string transport_name = point_cloud_transport::erase_last_copy(lookup_name, "_sub"); transports[transport_name].sub_name = lookup_name; transports[transport_name].package_name = sub_loader.getClassPackage(lookup_name); try { - boost::shared_ptr sub = sub_loader.createInstance(lookup_name); + auto sub = sub_loader.createSharedInstance(lookup_name); transports[transport_name].sub_status = SUCCESS; } - catch (const LibraryLoadException& e) + catch (const pluginlib::LibraryLoadException& e) { transports[transport_name].sub_status = LIB_LOAD_FAILURE; } - catch (const CreateClassException& e) + catch (const pluginlib::CreateClassException& e) { transports[transport_name].sub_status = CREATE_FAILURE; } @@ -120,7 +116,7 @@ int main(int argc, char** argv) bool problem_package = false; printf("Declared transports:\n"); - for (const auto& value : transports) + for (const StatusMap::value_type & value : transports) { const TransportDesc& td = value.second; printf("%s", value.first.c_str()); diff --git a/src/manifest.cpp b/src/manifest.cpp index 8acf363..ce6a2b0 100644 --- a/src/manifest.cpp +++ b/src/manifest.cpp @@ -1,12 +1,12 @@ // SPDX-License-Identifier: BSD-3-Clause // SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009 -#include +#include -#include -#include -#include -#include +#include +#include +#include +#include PLUGINLIB_EXPORT_CLASS(point_cloud_transport::RawPublisher, point_cloud_transport::PublisherPlugin) PLUGINLIB_EXPORT_CLASS(point_cloud_transport::RawSubscriber, point_cloud_transport::SubscriberPlugin) diff --git a/src/point_cloud_codec.cpp b/src/point_cloud_codec.cpp index addaab8..bd2db8e 100644 --- a/src/point_cloud_codec.cpp +++ b/src/point_cloud_codec.cpp @@ -43,19 +43,12 @@ #include #include -#include +#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include +#include +#include +#include +#include namespace point_cloud_transport { @@ -68,8 +61,8 @@ struct PointCloudCodec::Impl std::unordered_map decoders_for_topics_; Impl() : - enc_loader_(boost::make_shared("point_cloud_transport", "point_cloud_transport::PublisherPlugin")), - dec_loader_(boost::make_shared("point_cloud_transport", "point_cloud_transport::SubscriberPlugin")) + enc_loader_(std::make_shared("point_cloud_transport", "point_cloud_transport::PublisherPlugin")), + dec_loader_(std::make_shared("point_cloud_transport", "point_cloud_transport::SubscriberPlugin")) { } }; @@ -97,7 +90,7 @@ bool transportNameMatches(const std::string& lookup_name, const std::string& nam return false; } -boost::shared_ptr PointCloudCodec::getEncoderByName( +std::shared_ptr PointCloudCodec::getEncoderByName( const std::string& name) const { for (const auto& lookup_name : impl_->enc_loader_->getDeclaredClasses()) @@ -114,7 +107,7 @@ boost::shared_ptr PointCloudCodec::getEn return nullptr; } -boost::shared_ptr PointCloudCodec::getEncoderByTopic( +std::shared_ptr PointCloudCodec::getEncoderByTopic( const std::string& topic, const std::string& datatype) const { if (impl_->encoders_for_topics_.find(topic) != impl_->encoders_for_topics_.end()) @@ -143,7 +136,7 @@ boost::shared_ptr PointCloudCodec::getEn return nullptr; } -boost::shared_ptr PointCloudCodec::getDecoderByName( +std::shared_ptr PointCloudCodec::getDecoderByName( const std::string& name) const { for (const auto& lookup_name : impl_->dec_loader_->getDeclaredClasses()) @@ -160,7 +153,7 @@ boost::shared_ptr PointCloudCodec::getD return nullptr; } -boost::shared_ptr PointCloudCodec::getDecoderByTopic( +std::shared_ptr PointCloudCodec::getDecoderByTopic( const std::string& topic, const std::string& datatype) const { if (impl_->decoders_for_topics_.find(topic) != impl_->decoders_for_topics_.end()) @@ -196,19 +189,19 @@ thread_local PointCloudCodec point_cloud_transport_codec_instance(globalLogger); bool pointCloudTransportCodecsEncode( const char* codec, - sensor_msgs::PointCloud2::_height_type rawHeight, - sensor_msgs::PointCloud2::_width_type rawWidth, + sensor_msgs::msg::PointCloud2::_height_type rawHeight, + sensor_msgs::msg::PointCloud2::_width_type rawWidth, size_t rawNumFields, const char* rawFieldNames[], sensor_msgs::PointField::_offset_type rawFieldOffsets[], sensor_msgs::PointField::_datatype_type rawFieldDatatypes[], sensor_msgs::PointField::_count_type rawFieldCounts[], - sensor_msgs::PointCloud2::_is_bigendian_type rawIsBigendian, - sensor_msgs::PointCloud2::_point_step_type rawPointStep, - sensor_msgs::PointCloud2::_row_step_type rawRowStep, + sensor_msgs::msg::PointCloud2::_is_bigendian_type rawIsBigendian, + sensor_msgs::msg::PointCloud2::_point_step_type rawPointStep, + sensor_msgs::msg::PointCloud2::_row_step_type rawRowStep, size_t rawDataLength, const uint8_t rawData[], - sensor_msgs::PointCloud2::_is_dense_type rawIsDense, + sensor_msgs::msg::PointCloud2::_is_dense_type rawIsDense, cras::allocator_t compressedTypeAllocator, cras::allocator_t compressedMd5SumAllocator, cras::allocator_t compressedDataAllocator, @@ -232,7 +225,7 @@ bool pointCloudTransportCodecsEncode( return false; } } - sensor_msgs::PointCloud2 raw; + sensor_msgs::msg::PointCloud2 raw; raw.height = rawHeight; raw.width = rawWidth; for (size_t i = 0; i < rawNumFields; ++i) @@ -288,18 +281,18 @@ bool pointCloudTransportCodecsDecode( const char* compressedMd5sum, size_t compressedDataLength, const uint8_t compressedData[], - sensor_msgs::PointCloud2::_height_type& rawHeight, - sensor_msgs::PointCloud2::_width_type& rawWidth, + sensor_msgs::msg::PointCloud2::_height_type& rawHeight, + sensor_msgs::msg::PointCloud2::_width_type& rawWidth, uint32_t& rawNumFields, cras::allocator_t rawFieldNamesAllocator, cras::allocator_t rawFieldOffsetsAllocator, cras::allocator_t rawFieldDatatypesAllocator, cras::allocator_t rawFieldCountsAllocator, - sensor_msgs::PointCloud2::_is_bigendian_type& rawIsBigEndian, - sensor_msgs::PointCloud2::_point_step_type& rawPointStep, - sensor_msgs::PointCloud2::_row_step_type& rawRowStep, + sensor_msgs::msg::PointCloud2::_is_bigendian_type& rawIsBigEndian, + sensor_msgs::msg::PointCloud2::_point_step_type& rawPointStep, + sensor_msgs::msg::PointCloud2::_row_step_type& rawRowStep, cras::allocator_t rawDataAllocator, - sensor_msgs::PointCloud2::_is_dense_type& rawIsDense, + sensor_msgs::msg::PointCloud2::_is_dense_type& rawIsDense, size_t serializedConfigLength, const uint8_t serializedConfig[], cras::allocator_t errorStringAllocator, diff --git a/src/point_cloud_common.cpp b/src/point_cloud_common.cpp new file mode 100644 index 0000000..b61c1a6 --- /dev/null +++ b/src/point_cloud_common.cpp @@ -0,0 +1,47 @@ +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include "point_cloud_transport/point_cloud_common.hpp" + +#include +#include + +namespace point_cloud_transport +{ + +std::string erase_last_copy(const std::string & input, const std::string & search) +{ + size_t found = input.rfind(search); + auto input_copy = input; + if (found != std::string::npos) { + input_copy.replace(found, search.length(), ""); + } + return input_copy; +} + +} \ No newline at end of file diff --git a/src/point_cloud_transport.cpp b/src/point_cloud_transport.cpp index 75fa1aa..7e68873 100644 --- a/src/point_cloud_transport.cpp +++ b/src/point_cloud_transport.cpp @@ -43,22 +43,18 @@ #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 namespace point_cloud_transport { @@ -69,17 +65,8 @@ struct PointCloudTransportLoader::Impl point_cloud_transport::SubLoaderPtr sub_loader_; Impl() : - pub_loader_(boost::make_shared("point_cloud_transport", "point_cloud_transport::PublisherPlugin")), - sub_loader_(boost::make_shared("point_cloud_transport", "point_cloud_transport::SubscriberPlugin")) - { - } -}; - -struct PointCloudTransport::Impl -{ - ros::NodeHandle nh_; - - explicit Impl(const ros::NodeHandle& nh) : nh_(nh) + pub_loader_(std::make_shared("point_cloud_transport", "point_cloud_transport::PublisherPlugin")), + sub_loader_(std::make_shared("point_cloud_transport", "point_cloud_transport::SubscriberPlugin")) { } }; @@ -93,34 +80,6 @@ PointCloudTransportLoader::~PointCloudTransportLoader() { } -PointCloudTransport::PointCloudTransport(const ros::NodeHandle& nh) - : impl_(new Impl(nh)) -{ -} - -Publisher PointCloudTransport::advertise(const std::string& base_topic, uint32_t queue_size, bool latch) -{ - return advertise(base_topic, queue_size, {}, {}, {}, latch); -} - -Publisher PointCloudTransport::advertise(const std::string& base_topic, uint32_t queue_size, - const point_cloud_transport::SubscriberStatusCallback& connect_cb, - const point_cloud_transport::SubscriberStatusCallback& disconnect_cb, - const ros::VoidPtr& tracked_object, bool latch) -{ - return {impl_->nh_, base_topic, queue_size, connect_cb, disconnect_cb, tracked_object, latch, getPublisherLoader()}; -} - -Subscriber PointCloudTransport::subscribe( - const std::string& base_topic, uint32_t queue_size, - const boost::function& callback, - const ros::VoidPtr& tracked_object, const point_cloud_transport::TransportHints& transport_hints, - bool allow_concurrent_callbacks) -{ - return {impl_->nh_, base_topic, queue_size, callback, tracked_object, transport_hints, allow_concurrent_callbacks, - getSubscriberLoader()}; -} - std::vector PointCloudTransportLoader::getDeclaredTransports() const { auto transports = impl_->sub_loader_->getDeclaredClasses(); @@ -128,7 +87,7 @@ std::vector PointCloudTransportLoader::getDeclaredTransports() cons // Remove the "_sub" at the end of each class name. for (auto& transport : transports) { - transport = boost::erase_last_copy(transport, "_sub"); + transport = erase_last_copy(transport, "_sub"); } return transports; } @@ -143,12 +102,13 @@ std::unordered_map PointCloudTransportLoader::getLoada // otherwise ignore it. try { - auto sub = impl_->sub_loader_->createInstance(transportPlugin); + auto sub = impl_->sub_loader_->createSharedInstance(transportPlugin); // Remove the "_sub" at the end of each class name. - loadableTransports[boost::erase_last_copy(transportPlugin, "_sub")] = sub->getTransportName(); - } - catch (const pluginlib::PluginlibException& e) - { + loadableTransports[erase_last_copy(transportPlugin, "_sub")] = sub->getTransportName(); + } catch (const pluginlib::LibraryLoadException & e) { + (void) e; + } catch (const pluginlib::CreateClassException & e) { + (void) e; } } @@ -167,8 +127,6 @@ SubLoaderPtr PointCloudTransportLoader::getSubscriberLoader() const thread_local std::unique_ptr loader; -} - point_cloud_transport::PointCloudTransportLoader& getLoader() { if (point_cloud_transport::loader == nullptr) @@ -176,74 +134,77 @@ point_cloud_transport::PointCloudTransportLoader& getLoader() return *point_cloud_transport::loader; } -void pointCloudTransportGetLoadableTransports(cras::allocator_t transportAllocator, cras::allocator_t nameAllocator) -{ - for (const auto& transport : getLoader().getLoadableTransports()) - { - cras::outputString(transportAllocator, transport.first); - cras::outputString(nameAllocator, transport.second); - } -} - -void pointCloudTransportGetTopicsToPublish(const char* baseTopic, - cras::allocator_t transportAllocator, - cras::allocator_t nameAllocator, - cras::allocator_t topicAllocator, - cras::allocator_t dataTypeAllocator, - cras::allocator_t configTypeAllocator) -{ - auto pubLoader = getLoader().getPublisherLoader(); - for (const auto& transportPlugin : pubLoader->getDeclaredClasses()) - { - try - { - auto pub = pubLoader->createInstance(transportPlugin); - auto singleTopicPub = boost::dynamic_pointer_cast(pub); - if (singleTopicPub == nullptr) - continue; - // Remove the "_pub" at the end of each class name. - cras::outputString(transportAllocator, boost::erase_last_copy(transportPlugin, "_pub")); - cras::outputString(nameAllocator, singleTopicPub->getTransportName()); - cras::outputString(topicAllocator, singleTopicPub->getTopicToAdvertise(baseTopic)); - cras::outputString(dataTypeAllocator, singleTopicPub->getDataType()); - cras::outputString(configTypeAllocator, singleTopicPub->getConfigDataType()); - } - catch (const pluginlib::PluginlibException& e) - { - } - } -} - -void pointCloudTransportGetTopicToSubscribe(const char* baseTopic, - const char* transport, - cras::allocator_t nameAllocator, - cras::allocator_t topicAllocator, - cras::allocator_t dataTypeAllocator, - cras::allocator_t configTypeAllocator) -{ - auto subLoader = getLoader().getSubscriberLoader(); - for (const auto& transportPlugin : subLoader->getDeclaredClasses()) - { - try - { - auto sub = subLoader->createInstance(transportPlugin); - - const auto transportClass = boost::erase_last_copy(transportPlugin, "_sub"); - if (transportClass != transport && sub->getTransportName() != transport) - continue; - - auto singleTopicSub = boost::dynamic_pointer_cast(sub); - if (singleTopicSub == nullptr) - continue; - - cras::outputString(nameAllocator, singleTopicSub->getTransportName()); - cras::outputString(topicAllocator, singleTopicSub->getTopicToSubscribe(baseTopic)); - cras::outputString(dataTypeAllocator, singleTopicSub->getDataType()); - cras::outputString(configTypeAllocator, singleTopicSub->getConfigDataType()); - return; - } - catch (const pluginlib::PluginlibException& e) - { - } - } -} +// TODO: Are these needed? +// void pointCloudTransportGetLoadableTransports(cras::allocator_t transportAllocator, cras::allocator_t nameAllocator) +// { +// for (const auto& transport : getLoader().getLoadableTransports()) +// { +// cras::outputString(transportAllocator, transport.first); +// cras::outputString(nameAllocator, transport.second); +// } +// } + +// void pointCloudTransportGetTopicsToPublish(const char* baseTopic, +// cras::allocator_t transportAllocator, +// cras::allocator_t nameAllocator, +// cras::allocator_t topicAllocator, +// cras::allocator_t dataTypeAllocator, +// cras::allocator_t configTypeAllocator) +// { +// auto pubLoader = getLoader().getPublisherLoader(); +// for (const auto& transportPlugin : pubLoader->getDeclaredClasses()) +// { +// try +// { +// auto pub = pubLoader->createSharedInstance(transportPlugin); +// auto singleTopicPub = std::dynamic_pointer_cast(pub); +// if (singleTopicPub == nullptr) +// continue; +// // Remove the "_pub" at the end of each class name. +// cras::outputString(transportAllocator, erase_last_copy(transportPlugin, "_pub")); +// cras::outputString(nameAllocator, singleTopicPub->getTransportName()); +// cras::outputString(topicAllocator, singleTopicPub->getTopicToAdvertise(baseTopic)); +// cras::outputString(dataTypeAllocator, singleTopicPub->getDataType()); +// cras::outputString(configTypeAllocator, singleTopicPub->getConfigDataType()); +// } +// catch (const pluginlib::PluginlibException& e) +// { +// } +// } +// } + +// void pointCloudTransportGetTopicToSubscribe(const char* baseTopic, +// const char* transport, +// cras::allocator_t nameAllocator, +// cras::allocator_t topicAllocator, +// cras::allocator_t dataTypeAllocator, +// cras::allocator_t configTypeAllocator) +// { +// auto subLoader = getLoader().getSubscriberLoader(); +// for (const auto& transportPlugin : subLoader->getDeclaredClasses()) +// { +// try +// { +// auto sub = subLoader->createSharedInstance(transportPlugin); + +// const auto transportClass = erase_last_copy(transportPlugin, "_sub"); +// if (transportClass != transport && sub->getTransportName() != transport) +// continue; + +// auto singleTopicSub = std::dynamic_pointer_cast(sub); +// if (singleTopicSub == nullptr) +// continue; + +// cras::outputString(nameAllocator, singleTopicSub->getTransportName()); +// cras::outputString(topicAllocator, singleTopicSub->getTopicToSubscribe(baseTopic)); +// cras::outputString(dataTypeAllocator, singleTopicSub->getDataType()); +// cras::outputString(configTypeAllocator, singleTopicSub->getConfigDataType()); +// return; +// } +// catch (const pluginlib::PluginlibException& e) +// { +// } +// } +// } + +} // namespace point_cloud_transport diff --git a/src/point_cloud_transport/encoder.py b/src/point_cloud_transport/encoder.py index ba0ecc7..ab189ea 100644 --- a/src/point_cloud_transport/encoder.py +++ b/src/point_cloud_transport/encoder.py @@ -31,7 +31,7 @@ def _get_library(): def encode(raw, topic_or_codec, config=None): - """Encode the given raw image into a compressed image with a suitable codec. + """Encode the given raw point_cloud into a compressed point_cloud with a suitable codec. :param sensor_msgs.msg.PointCloud2 raw: The raw point cloud. :param str topic_or_codec: Name of the topic where this cloud should be published or explicit name of the codec. diff --git a/src/publisher.cpp b/src/publisher.cpp index 80c2d02..1898f61 100644 --- a/src/publisher.cpp +++ b/src/publisher.cpp @@ -38,31 +38,31 @@ * */ +#include #include #include +#include #include -#include -#include -#include -#include +#include "rclcpp/expand_topic_or_service_name.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/node.hpp" +#include -#include -#include -#include -#include - -#include -#include -#include -#include +#include +#include +#include +#include +#include namespace point_cloud_transport { struct Publisher::Impl { - Impl() : unadvertised_(false) + explicit Impl(rclcpp::Node * node) + : logger_(node->get_logger()), + unadvertised_(false) { } @@ -104,54 +104,51 @@ struct Publisher::Impl const point_cloud_transport::SubscriberStatusCallback& user_cb) { point_cloud_transport::SingleSubscriberPublisher ssp( - plugin_pub.getSubscriberName(), getTopic(), boost::bind(&Publisher::Impl::getNumSubscribers, this), + plugin_pub.getSubscriberName(), getTopic(), std::bind(&Publisher::Impl::getNumSubscribers, this), plugin_pub.publish_fn_); user_cb(ssp); } + rclcpp::Logger logger_; std::string base_topic_; PubLoaderPtr loader_; - std::vector > publishers_; + std::vector> publishers_; bool unadvertised_; }; -Publisher::Publisher() = default; - -Publisher::Publisher(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, - const point_cloud_transport::SubscriberStatusCallback& connect_cb, - const point_cloud_transport::SubscriberStatusCallback& disconnect_cb, - const ros::VoidPtr& tracked_object, bool latch, - const point_cloud_transport::PubLoaderPtr& loader) - : impl_(new Impl) +Publisher::Publisher( + rclcpp::Node * node, const std::string & base_topic, + PubLoaderPtr loader, rmw_qos_profile_t custom_qos) +: impl_(std::make_shared(node)) { - // Resolve the name explicitly because otherwise the compressed topics don't remap properly - impl_->base_topic_ = nh.resolveName(base_topic); + // Resolve the name explicitly because otherwise the compressed topics don't remap + // properly (#3652). + std::string point_cloud_topic = rclcpp::expand_topic_or_service_name( + base_topic, + node->get_name(), node->get_namespace()); + impl_->base_topic_ = point_cloud_topic; impl_->loader_ = loader; - // sequence container which encapsulates dynamic size arrays std::vector blacklist_vec; - // call to parameter server - nh.getParam(impl_->base_topic_ + "/disable_pub_plugins", blacklist_vec); - + node->get_parameter(impl_->base_topic_ + "/disable_pub_plugins", blacklist_vec); // set std::set blacklist(blacklist_vec.begin(), blacklist_vec.end()); for (const auto& lookup_name : loader->getDeclaredClasses()) { - const std::string transport_name = boost::erase_last_copy(lookup_name, "_pub"); + const std::string transport_name = erase_last_copy(lookup_name, "_pub"); if (blacklist.find(transport_name) != blacklist.end()) continue; try { - auto pub = loader->createInstance(lookup_name); - impl_->publishers_.push_back(pub); - pub->advertise(nh, impl_->base_topic_, queue_size, rebindCB(connect_cb), - rebindCB(disconnect_cb), tracked_object, latch); + auto pub = loader->createUniqueInstance(lookup_name); + pub->advertise(node, point_cloud_topic, custom_qos); + impl_->publishers_.push_back(std::move(pub)); } catch (const std::runtime_error& e) { - ROS_ERROR("Failed to load plugin %s, error string: %s", lookup_name.c_str(), e.what()); + RCLCPP_ERROR(impl_->logger_, "Failed to load plugin %s, error string: %s", lookup_name.c_str(), e.what()); } } @@ -176,11 +173,13 @@ std::string Publisher::getTopic() const return {}; } -void Publisher::publish(const sensor_msgs::PointCloud2& message) const +void Publisher::publish(const sensor_msgs::msg::PointCloud2& message) const { if (!impl_ || !impl_->isValid()) { - ROS_ASSERT_MSG(false, "Call to publish() on an invalid point_cloud_transport::Publisher"); + // TODO(ros2) Switch to RCUTILS_ASSERT when ros2/rcutils#112 is merged + auto logger = impl_ ? impl_->logger_ : rclcpp::get_logger("point_cloud_transport"); + RCLCPP_FATAL(logger, "Call to publish() on an invalid point_cloud_transport::Publisher"); return; } @@ -193,11 +192,12 @@ void Publisher::publish(const sensor_msgs::PointCloud2& message) const } } -void Publisher::publish(const sensor_msgs::PointCloud2ConstPtr& message) const +void Publisher::publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& message) const { - if (!impl_ || !impl_->isValid()) - { - ROS_ASSERT_MSG(false, "Call to publish() on an invalid point_cloud_transport::Publisher"); + if (!impl_ || !impl_->isValid()) { + // TODO(ros2) Switch to RCUTILS_ASSERT when ros2/rcutils#112 is merged + auto logger = impl_ ? impl_->logger_ : rclcpp::get_logger("point_cloud_transport"); + RCLCPP_FATAL(logger, "Call to publish() on an invalid point_cloud_transport::Publisher"); return; } @@ -221,31 +221,32 @@ void Publisher::shutdown() Publisher::operator void*() const { - return (impl_ && impl_->isValid()) ? reinterpret_cast(1) : nullptr; -} - -void Publisher::weakSubscriberCb(const ImplWPtr& impl_wptr, - const point_cloud_transport::SingleSubscriberPublisher& plugin_pub, - const point_cloud_transport::SubscriberStatusCallback& user_cb) -{ - if (ImplPtr impl = impl_wptr.lock()) - { - impl->subscriberCB(plugin_pub, user_cb); - } + return (impl_ && impl_->isValid()) ? reinterpret_cast(1) : reinterpret_cast(0); } -SubscriberStatusCallback Publisher::rebindCB(const point_cloud_transport::SubscriberStatusCallback& user_cb) -{ - // Note: the subscriber callback must be bound to the internal Impl object, not - // 'this'. Due to copying behavior the Impl object may outlive the original Publisher - // instance. But it should not outlive the last Publisher, so we use a weak_ptr. - if (user_cb) - { - ImplWPtr impl_wptr(impl_); - return boost::bind(&Publisher::weakSubscriberCb, impl_wptr, _1, user_cb); - } - - return {}; -} +// TODO: fix this +// void Publisher::weakSubscriberCb(const ImplWPtr& impl_wptr, +// const point_cloud_transport::SingleSubscriberPublisher& plugin_pub, +// const point_cloud_transport::SubscriberStatusCallback& user_cb) +// { +// if (ImplPtr impl = impl_wptr.lock()) +// { +// impl->subscriberCB(plugin_pub, user_cb); +// } +// } + +// SubscriberStatusCallback Publisher::rebindCB(const point_cloud_transport::SubscriberStatusCallback& user_cb) +// { +// // Note: the subscriber callback must be bound to the internal Impl object, not +// // 'this'. Due to copying behavior the Impl object may outlive the original Publisher +// // instance. But it should not outlive the last Publisher, so we use a weak_ptr. +// if (user_cb) +// { +// ImplWPtr impl_wptr(impl_); +// return std::bind(&Publisher::weakSubscriberCb, impl_wptr, _1, user_cb); +// } + +// return {}; +// } } diff --git a/src/publisher_plugin.cpp b/src/publisher_plugin.cpp index 1d9727d..75e2edd 100644 --- a/src/publisher_plugin.cpp +++ b/src/publisher_plugin.cpp @@ -41,49 +41,13 @@ #include #include -#include -#include -#include -#include -#include -#include -#include - -#include -#include +#include +#include namespace point_cloud_transport { -PublisherPlugin::EncodeResult PublisherPlugin::encode(const sensor_msgs::PointCloud2& raw) const -{ - return this->encode(raw, dynamic_reconfigure::Config()); -} - -PublisherPlugin::EncodeResult PublisherPlugin::encode(const sensor_msgs::PointCloud2& raw, - const XmlRpc::XmlRpcValue& config) const -{ - dynamic_reconfigure::Config configMsg; - std::list errors; - if (!cras::convert(config, configMsg, true, &errors)) - return cras::make_unexpected("Invalid encoder config: " + cras::join(errors, " ")); - return this->encode(raw, configMsg); -} - -void PublisherPlugin::advertise(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, bool latch) -{ - advertiseImpl(nh, base_topic, queue_size, {}, {}, {}, latch); -} - -void PublisherPlugin::advertise(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, - const point_cloud_transport::SubscriberStatusCallback& connect_cb, - const point_cloud_transport::SubscriberStatusCallback& disconnect_cb, - const ros::VoidPtr& tracked_object, bool latch) -{ - advertiseImpl(nh, base_topic, queue_size, connect_cb, disconnect_cb, tracked_object, latch); -} - -void PublisherPlugin::publish(const sensor_msgs::PointCloud2ConstPtr& message) const +void PublisherPlugin::publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& message) const { publish(*message); } diff --git a/src/raw_publisher.cpp b/src/raw_publisher.cpp deleted file mode 100644 index 1954130..0000000 --- a/src/raw_publisher.cpp +++ /dev/null @@ -1,82 +0,0 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. - -/* - * - * BSD 3-Clause License - * - * Copyright (c) Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * - */ - -#include - -#include - -#include -#include - -namespace point_cloud_transport -{ - -RawPublisher::TypedEncodeResult RawPublisher::encodeTyped( - const sensor_msgs::PointCloud2& raw, const point_cloud_transport::NoConfigConfig&) const -{ - return raw; -} - -std::string RawPublisher::getTransportName() const -{ - return "raw"; -} - -void RawPublisher::publish(const sensor_msgs::PointCloud2ConstPtr& message) const -{ - getPublisher().publish(message); -} - -void RawPublisher::publish(const sensor_msgs::PointCloud2& message, const RawPublisher::PublishFn& publish_fn) const -{ - publish_fn(message); -} - -std::string RawPublisher::getTopicToAdvertise(const std::string& base_topic) const -{ - return base_topic; -} - -bool RawPublisher::matchesTopic(const std::string& topic, const std::string& datatype) const -{ - return datatype == ros::message_traits::DataType::value(); -} - -} diff --git a/src/raw_subscriber.cpp b/src/raw_subscriber.cpp index 22c3d6b..e5a5809 100644 --- a/src/raw_subscriber.cpp +++ b/src/raw_subscriber.cpp @@ -40,9 +40,9 @@ #include -#include +#include -#include +#include namespace point_cloud_transport { @@ -57,28 +57,9 @@ std::string RawSubscriber::getTopicToSubscribe(const std::string& base_topic) co return base_topic; } -void RawSubscriber::callback(const sensor_msgs::PointCloud2ConstPtr& message, const SubscriberPlugin::Callback& user_cb) +void RawSubscriber::internalCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& message, const SubscriberPlugin::Callback& user_cb) { user_cb(message); } -SubscriberPlugin::DecodeResult RawSubscriber::decodeTyped(const sensor_msgs::PointCloud2ConstPtr& compressed, - const NoConfigConfig&) const -{ - return compressed; -} - -SubscriberPlugin::DecodeResult RawSubscriber::decodeTyped(const sensor_msgs::PointCloud2& compressed, - const NoConfigConfig& config) const -{ - sensor_msgs::PointCloud2Ptr compressedPtr(new sensor_msgs::PointCloud2); - *compressedPtr = compressed; - return this->decodeTyped(compressedPtr, config); -} - -bool RawSubscriber::matchesTopic(const std::string& topic, const std::string& datatype) const -{ - return datatype == ros::message_traits::DataType::value(); -} - -} +} // namespace point_cloud_transport diff --git a/src/republish.cpp b/src/republish.cpp index 531ec37..e55f6cd 100644 --- a/src/republish.cpp +++ b/src/republish.cpp @@ -40,38 +40,43 @@ #include #include +#include -#include -#include +#include +#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include namespace point_cloud_transport { -void RepublishNodelet::onInit() +// TODO: Fix this +void RepublishComponent::onInit() { - if (this->getMyArgv().empty()) - { - throw std::runtime_error("Usage: republish in_transport in:= [out_transport] out:="); + auto vargv = rclcpp::init_and_remove_ros_arguments(argc, argv); + + if (vargv.size() < 2) { + printf( + "Usage: %s in_transport in:= [out_transport] out:=\n", + argv[0]); + return 0; } - std::string in_transport = this->getMyArgv()[0]; - std::string in_topic = this->getNodeHandle().resolveName("in"); - std::string out_topic = this->getNodeHandle().resolveName("out"); + auto node = rclcpp::Node::make_shared("point_cloud_republisher"); + + std::string in_topic = rclcpp::expand_topic_or_service_name( + "in", + node->get_name(), node->get_namespace()); + std::string out_topic = rclcpp::expand_topic_or_service_name( + "out", + node->get_name(), node->get_namespace()); - const auto params = this->params(this->getPrivateNodeHandle()); - const auto in_queue_size = params->getParam("in_queue_size", 10_sz, "messages"); - const auto out_queue_size = params->getParam("out_queue_size", in_queue_size, "messages"); + std::string in_transport = vargv[1]; this->pct = std::make_unique(this->getMTNodeHandle()); point_cloud_transport::TransportHints hints(in_transport, {}, this->getPrivateNodeHandle()); @@ -85,10 +90,10 @@ void RepublishNodelet::onInit() *this->pub = this->pct->advertise(out_topic, out_queue_size); // Use Publisher::publish as the subscriber callback - typedef void (point_cloud_transport::Publisher::*PublishMemFn)(const sensor_msgs::PointCloud2ConstPtr&) const; + typedef void (point_cloud_transport::Publisher::*PublishMemFn)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr&) const; PublishMemFn pub_mem_fn = &point_cloud_transport::Publisher::publish; - this->sub = this->pct->subscribe(in_topic, in_queue_size, boost::bind(pub_mem_fn, this->pub.get(), _1), this->pub, + this->sub = this->pct->subscribe(in_topic, in_queue_size, std::bind(pub_mem_fn, this->pub.get(), std::placeholders::_1), this->pub, hints); } else @@ -100,15 +105,15 @@ void RepublishNodelet::onInit() typedef point_cloud_transport::PublisherPlugin Plugin; auto loader = this->pct->getPublisherLoader(); std::string lookup_name = Plugin::getLookupName(out_transport); - this->pubPlugin = loader->createInstance(lookup_name); + this->pubPlugin = loader->createUniqueInstance(lookup_name); this->pubPlugin->advertise(this->getMTNodeHandle(), out_topic, out_queue_size, {}, {}, this->pubPlugin, false); // Use PublisherPlugin::publish as the subscriber callback - typedef void (Plugin::*PublishMemFn)(const sensor_msgs::PointCloud2ConstPtr&) const; + typedef void (Plugin::*PublishMemFn)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr&) const; PublishMemFn pub_mem_fn = &Plugin::publish; - this->sub = this->pct->subscribe(in_topic, in_queue_size, boost::bind(pub_mem_fn, this->pubPlugin.get(), _1), + this->sub = this->pct->subscribe(in_topic, in_queue_size, std::bind(pub_mem_fn, this->pubPlugin.get(), std::placeholders::_1), this->pubPlugin, in_transport); } } diff --git a/src/single_subscriber_publisher.cpp b/src/single_subscriber_publisher.cpp index fa3395e..d77eee5 100644 --- a/src/single_subscriber_publisher.cpp +++ b/src/single_subscriber_publisher.cpp @@ -40,18 +40,21 @@ #include -#include +#include -#include -#include +#include +#include namespace point_cloud_transport { SingleSubscriberPublisher::SingleSubscriberPublisher( - const std::string& caller_id, const std::string& topic, const GetNumSubscribersFn& num_subscribers_fn, - const PublishFn& publish_fn) - : caller_id_(caller_id), topic_(topic), num_subscribers_fn_(num_subscribers_fn), publish_fn_(publish_fn) + const std::string & caller_id, const std::string & topic, + const GetNumSubscribersFn & num_subscribers_fn, + const PublishFn & publish_fn) +: caller_id_(caller_id), topic_(topic), + num_subscribers_fn_(num_subscribers_fn), + publish_fn_(publish_fn) { } @@ -70,12 +73,12 @@ uint32_t SingleSubscriberPublisher::getNumSubscribers() const return num_subscribers_fn_(); } -void SingleSubscriberPublisher::publish(const sensor_msgs::PointCloud2& message) const +void SingleSubscriberPublisher::publish(const sensor_msgs::msg::PointCloud2& message) const { publish_fn_(message); } -void SingleSubscriberPublisher::publish(const sensor_msgs::PointCloud2ConstPtr& message) const +void SingleSubscriberPublisher::publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& message) const { publish_fn_(*message); } diff --git a/src/subscriber.cpp b/src/subscriber.cpp index 9950aa7..b25a480 100644 --- a/src/subscriber.cpp +++ b/src/subscriber.cpp @@ -39,31 +39,28 @@ */ #include +#include #include #include -#include -#include - -#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "rclcpp/expand_topic_or_service_name.hpp" +#include "rclcpp/logging.hpp" + +#include +#include +#include namespace point_cloud_transport { struct Subscriber::Impl { - Impl() : unsubscribed_(false) + Impl(rclcpp::Node * node, SubLoaderPtr loader) + : logger_(node->get_logger()), + loader_(loader), + unsubscribed_(false) { } @@ -87,33 +84,37 @@ struct Subscriber::Impl } } + rclcpp::Logger logger_; + std::string lookup_name_; point_cloud_transport::SubLoaderPtr loader_; - boost::shared_ptr subscriber_; + std::shared_ptr subscriber_; bool unsubscribed_; }; -Subscriber::Subscriber() = default; - -Subscriber::Subscriber(ros::NodeHandle& nh, const std::string& base_topic, uint32_t queue_size, - const boost::function& callback, - const ros::VoidPtr& tracked_object, const point_cloud_transport::TransportHints& transport_hints, - bool allow_concurrent_callbacks, const point_cloud_transport::SubLoaderPtr& loader) - : impl_(new Impl) +Subscriber::Subscriber( + rclcpp::Node * node, + const std::string & base_topic, + const Callback & callback, + SubLoaderPtr loader, + const std::string & transport, + rmw_qos_profile_t custom_qos, + rclcpp::SubscriptionOptions options) +: impl_(std::make_shared(node, loader)) { // Load the plugin for the chosen transport. - std::string lookup_name = SubscriberPlugin::getLookupName(transport_hints.getTransport()); + std::string lookup_name = SubscriberPlugin::getLookupName(transport); try { - impl_->subscriber_ = loader->createInstance(lookup_name); + impl_->subscriber_ = loader->createSharedInstance(lookup_name); } catch (pluginlib::PluginlibException& e) { - throw point_cloud_transport::TransportLoadException(transport_hints.getTransport(), e.what()); + throw point_cloud_transport::TransportLoadException(transport, e.what()); } // Hang on to the class loader so our shared library doesn't disappear from under us. impl_->loader_ = loader; // Try to catch if user passed in a transport-specific topic as base_topic. - std::string clean_topic = ros::names::clean(base_topic); + std::string clean_topic = base_topic; size_t found = clean_topic.rfind('/'); if (found != std::string::npos) { std::string transport = clean_topic.substr(found+1); @@ -121,17 +122,21 @@ Subscriber::Subscriber(ros::NodeHandle& nh, const std::string& base_topic, uint3 std::vector plugins = loader->getDeclaredClasses(); if (std::find(plugins.begin(), plugins.end(), plugin_name) != plugins.end()) { std::string real_base_topic = clean_topic.substr(0, found); - ROS_WARN("[point_cloud_transport] It looks like you are trying to subscribe directly to a " - "transport-specific point cloud topic '%s', in which case you will likely get a connection " - "error. Try subscribing to the base topic '%s' instead with parameter ~point_cloud_transport " - "set to '%s' (on the command line, _point_cloud_transport:=%s). ", - clean_topic.c_str(), real_base_topic.c_str(), transport.c_str(), transport.c_str()); + + RCLCPP_WARN( + impl_->logger_, + "[point_cloud_transport] It looks like you are trying to subscribe directly to a " + "transport-specific point_cloud topic '%s', in which case you will likely get a connection " + "error. Try subscribing to the base topic '%s' instead with parameter ~point_cloud_transport " + "set to '%s' (on the command line, _point_cloud_transport:=%s). " + "See http://ros.org/wiki/point_cloud_transport for details.", + clean_topic.c_str(), real_base_topic.c_str(), transport.c_str(), transport.c_str()); } } // Tell plugin to subscribe. - impl_->subscriber_->subscribe(nh, base_topic, queue_size, callback, tracked_object, transport_hints, - allow_concurrent_callbacks); + RCLCPP_DEBUG(impl_->logger_, "Subscribing to: %s\n", base_topic.c_str()); + impl_->subscriber_->subscribe(node, base_topic, callback, custom_qos, options); } std::string Subscriber::getTopic() const @@ -163,7 +168,7 @@ void Subscriber::shutdown() Subscriber::operator void*() const { - return (impl_ && impl_->isValid()) ? reinterpret_cast(1) : nullptr; + return (impl_ && impl_->isValid()) ? reinterpret_cast(1) : reinterpret_cast(0); } } From b24d14114a2176c8e3184997554e854d8f531ba6 Mon Sep 17 00:00:00 2001 From: john-maidbot Date: Sun, 18 Jun 2023 16:20:45 -0500 Subject: [PATCH 02/28] Progress --- .../point_cloud_transport.hpp | 32 +++++++++++++++---- include/point_cloud_transport/publisher.hpp | 2 +- .../subscriber_filter.hpp | 10 +++--- src/point_cloud_transport.cpp | 22 ++++++------- 4 files changed, 41 insertions(+), 25 deletions(-) diff --git a/include/point_cloud_transport/point_cloud_transport.hpp b/include/point_cloud_transport/point_cloud_transport.hpp index 57f8c92..3beddd5 100644 --- a/include/point_cloud_transport/point_cloud_transport.hpp +++ b/include/point_cloud_transport/point_cloud_transport.hpp @@ -110,7 +110,12 @@ class PointCloudTransport : public PointCloudTransportLoader ~PointCloudTransport(); //! Advertise a PointCloud2 topic, simple version. - point_cloud_transport::Publisher advertise(const std::string& base_topic, uint32_t queue_size, bool latch = false); + Publisher advertise( + const std::string & base_topic, + rmw_qos_profile_t custom_qos) + { + return Publisher(node_.get(), base_topic, impl_->pub_loader_, custom_qos); + } //! Advertise an PointCloud2 topic with subscriber status callbacks. // TODO(ros2) Implement when SubscriberStatusCallback is available @@ -121,10 +126,13 @@ class PointCloudTransport : public PointCloudTransportLoader //! Subscribe to a point cloud topic, version for arbitrary std::function object. point_cloud_transport::Subscriber subscribe( - const std::string& base_topic, uint32_t queue_size, - const std::function& callback, - const VoidPtr& tracked_object = {}, - const point_cloud_transport::TransportHints* transport_hints = nullptr); + const std::string &base_topic, uint32_t queue_size, + const std::function &callback, + const VoidPtr &tracked_object = {}, + const point_cloud_transport::TransportHints *transport_hints = nullptr) + { + return Subscriber(node_, base_topic, callback, impl_->sub_loader_, transport, custom_qos, options); + } //! Subscribe to a point cloud topic, version for bare function. point_cloud_transport::Subscriber subscribe(const std::string& base_topic, uint32_t queue_size, @@ -157,8 +165,20 @@ class PointCloudTransport : public PointCloudTransportLoader } private: - struct Impl; + struct Impl + { + point_cloud_transport::PubLoaderPtr pub_loader_; + point_cloud_transport::SubLoaderPtr sub_loader_; + + Impl() : + pub_loader_(std::make_shared("point_cloud_transport", "point_cloud_transport::PublisherPlugin")), + sub_loader_(std::make_shared("point_cloud_transport", "point_cloud_transport::SubscriberPlugin")) + { + } + }; + std::shared_ptr impl_; + rclcpp::Node::SharedPtr node_; }; } diff --git a/include/point_cloud_transport/publisher.hpp b/include/point_cloud_transport/publisher.hpp index 5e773d1..52093ce 100644 --- a/include/point_cloud_transport/publisher.hpp +++ b/include/point_cloud_transport/publisher.hpp @@ -63,7 +63,7 @@ class Publisher Publisher() = default; Publisher( - rclcpp::Node * nh, + rclcpp::Node * node, const std::string & base_topic, PubLoaderPtr loader, rmw_qos_profile_t custom_qos); diff --git a/include/point_cloud_transport/subscriber_filter.hpp b/include/point_cloud_transport/subscriber_filter.hpp index 766bc03..391821c 100644 --- a/include/point_cloud_transport/subscriber_filter.hpp +++ b/include/point_cloud_transport/subscriber_filter.hpp @@ -80,10 +80,11 @@ class SubscriberFilter : public message_filters::SimpleFilter("point_cloud_transport", "point_cloud_transport::PublisherPlugin")), - sub_loader_(std::make_shared("point_cloud_transport", "point_cloud_transport::SubscriberPlugin")) - { - } -}; - PointCloudTransportLoader::PointCloudTransportLoader() - : impl_(new Impl()) + : impl_(new Impl) { } @@ -134,6 +122,14 @@ point_cloud_transport::PointCloudTransportLoader& getLoader() return *point_cloud_transport::loader; } +PointCloudTransport::PointCloudTransport(rclcpp::Node::SharedPtr node) +{ + PointCloudTransportLoader(); + node_ = node; +} + + + // TODO: Are these needed? // void pointCloudTransportGetLoadableTransports(cras::allocator_t transportAllocator, cras::allocator_t nameAllocator) // { From 4caa74586b59011aeb301393bcafdccf6df399ad Mon Sep 17 00:00:00 2001 From: john-maidbot Date: Sun, 18 Jun 2023 17:38:46 -0500 Subject: [PATCH 03/28] builds without cras --- .vscode/settings.json | 88 +------------------ CMakeLists.txt | 10 +-- .../point_cloud_transport.hpp | 41 +++++---- .../publisher_plugin.hpp | 17 ---- .../simple_publisher_plugin.hpp | 2 +- .../simple_subscriber_plugin.hpp | 2 +- .../subscriber_plugin.hpp | 5 -- src/point_cloud_transport.cpp | 14 +-- 8 files changed, 38 insertions(+), 141 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 878e524..09ecae2 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,91 +1,7 @@ { "files.associations": { - "cctype": "cpp", - "clocale": "cpp", - "cmath": "cpp", - "csignal": "cpp", - "cstdarg": "cpp", - "cstddef": "cpp", - "cstdio": "cpp", - "cstdlib": "cpp", - "cstring": "cpp", - "ctime": "cpp", - "cwchar": "cpp", - "cwctype": "cpp", - "any": "cpp", - "array": "cpp", - "atomic": "cpp", - "hash_map": "cpp", - "hash_set": "cpp", - "strstream": "cpp", - "bit": "cpp", - "*.tcc": "cpp", - "bitset": "cpp", - "cfenv": "cpp", - "charconv": "cpp", - "chrono": "cpp", - "cinttypes": "cpp", - "codecvt": "cpp", - "compare": "cpp", - "complex": "cpp", - "concepts": "cpp", - "condition_variable": "cpp", - "coroutine": "cpp", - "cstdint": "cpp", - "deque": "cpp", - "forward_list": "cpp", - "list": "cpp", - "map": "cpp", - "set": "cpp", - "string": "cpp", - "unordered_map": "cpp", - "unordered_set": "cpp", - "vector": "cpp", - "exception": "cpp", - "algorithm": "cpp", - "functional": "cpp", - "iterator": "cpp", "memory": "cpp", - "memory_resource": "cpp", - "numeric": "cpp", - "optional": "cpp", - "random": "cpp", - "ratio": "cpp", - "regex": "cpp", - "source_location": "cpp", - "string_view": "cpp", - "system_error": "cpp", - "tuple": "cpp", - "type_traits": "cpp", - "utility": "cpp", - "slist": "cpp", - "fstream": "cpp", - "future": "cpp", - "initializer_list": "cpp", - "iomanip": "cpp", - "iosfwd": "cpp", - "iostream": "cpp", - "istream": "cpp", - "limits": "cpp", - "mutex": "cpp", - "new": "cpp", - "numbers": "cpp", - "ostream": "cpp", - "ranges": "cpp", - "scoped_allocator": "cpp", - "semaphore": "cpp", - "shared_mutex": "cpp", - "span": "cpp", - "sstream": "cpp", - "stdexcept": "cpp", - "stop_token": "cpp", - "streambuf": "cpp", - "thread": "cpp", - "typeindex": "cpp", - "typeinfo": "cpp", - "valarray": "cpp", - "variant": "cpp", - "expected": "cpp", - "*.ipp": "cpp" + "*.ipp": "cpp", + "system_error": "cpp" } } \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index a7bd971..cc0b27e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -46,10 +46,10 @@ target_link_libraries(${PROJECT_NAME}_plugins PRIVATE pluginlib::pluginlib) # Build republish (TODO: there was some nodelet stuff happening here) -add_executable(republish src/republish.cpp) -target_link_libraries(republish - ${PROJECT_NAME} - pluginlib::pluginlib) +# add_executable(republish src/republish.cpp) +# target_link_libraries(republish +# ${PROJECT_NAME} +# pluginlib::pluginlib) # Build list_transports (TODO: there was some nodelet stuff happening here) add_executable(list_transports src/list_transports.cpp) @@ -69,7 +69,7 @@ install(TARGETS ${PROJECT_NAME} EXPORT export_${PROJECT_NAME}) # Install executables install( - TARGETS list_transports republish + TARGETS list_transports #republish RUNTIME DESTINATION lib/${PROJECT_NAME} ) diff --git a/include/point_cloud_transport/point_cloud_transport.hpp b/include/point_cloud_transport/point_cloud_transport.hpp index 3beddd5..92eade0 100644 --- a/include/point_cloud_transport/point_cloud_transport.hpp +++ b/include/point_cloud_transport/point_cloud_transport.hpp @@ -87,11 +87,12 @@ class PointCloudTransportLoader //! The loader that can load subscriber plugins. SubLoaderPtr getSubscriberLoader() const; -private: - struct Impl; - std::unique_ptr impl_; +protected: + point_cloud_transport::PubLoaderPtr pub_loader_; + point_cloud_transport::SubLoaderPtr sub_loader_; }; + /** * Advertise and subscribe to PointCloud2 topics. * @@ -107,14 +108,26 @@ class PointCloudTransport : public PointCloudTransportLoader //! Constructor explicit PointCloudTransport(rclcpp::Node::SharedPtr node); - ~PointCloudTransport(); + ~PointCloudTransport() override = default; + + std::string getTransportOrDefault(const TransportHints * transport_hints) + { + std::string ret; + if (nullptr == transport_hints) { + TransportHints th(node_.get()); + ret = th.getTransport(); + } else { + ret = transport_hints->getTransport(); + } + return ret; + } //! Advertise a PointCloud2 topic, simple version. Publisher advertise( const std::string & base_topic, rmw_qos_profile_t custom_qos) { - return Publisher(node_.get(), base_topic, impl_->pub_loader_, custom_qos); + return Publisher(node_.get(), base_topic, pub_loader_, custom_qos); } //! Advertise an PointCloud2 topic with subscriber status callbacks. @@ -131,7 +144,10 @@ class PointCloudTransport : public PointCloudTransportLoader const VoidPtr &tracked_object = {}, const point_cloud_transport::TransportHints *transport_hints = nullptr) { - return Subscriber(node_, base_topic, callback, impl_->sub_loader_, transport, custom_qos, options); + rmw_qos_profile_t custom_qos = rmw_qos_profile_default; + custom_qos.depth = queue_size; + rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions(); + return Subscriber(node_.get(), base_topic, callback, sub_loader_, getTransportOrDefault(transport_hints), custom_qos, options); } //! Subscribe to a point cloud topic, version for bare function. @@ -165,19 +181,6 @@ class PointCloudTransport : public PointCloudTransportLoader } private: - struct Impl - { - point_cloud_transport::PubLoaderPtr pub_loader_; - point_cloud_transport::SubLoaderPtr sub_loader_; - - Impl() : - pub_loader_(std::make_shared("point_cloud_transport", "point_cloud_transport::PublisherPlugin")), - sub_loader_(std::make_shared("point_cloud_transport", "point_cloud_transport::SubscriberPlugin")) - { - } - }; - - std::shared_ptr impl_; rclcpp::Node::SharedPtr node_; }; diff --git a/include/point_cloud_transport/publisher_plugin.hpp b/include/point_cloud_transport/publisher_plugin.hpp index ce1eb70..b03a38a 100644 --- a/include/point_cloud_transport/publisher_plugin.hpp +++ b/include/point_cloud_transport/publisher_plugin.hpp @@ -104,24 +104,7 @@ class PublisherPlugin class SingleTopicPublisherPlugin : public PublisherPlugin { public: - /** - * Return the communication topic name for a given base topic. - * - * Defaults to \/\. - */ - virtual std::string getTopicToAdvertise(const std::string& base_topic) const = 0; - /** - * Return the datatype of the transported messages (as text in the form `package/Message`). - */ - virtual std::string getDataType() const = 0; - - /** - * Return the datatype of the dynamic reconfigure (as text in the form `package/Config`). - * - * Return empty string if no reconfiguration is supported. - */ - virtual std::string getConfigDataType() const = 0; }; } diff --git a/include/point_cloud_transport/simple_publisher_plugin.hpp b/include/point_cloud_transport/simple_publisher_plugin.hpp index 2937151..770e5da 100644 --- a/include/point_cloud_transport/simple_publisher_plugin.hpp +++ b/include/point_cloud_transport/simple_publisher_plugin.hpp @@ -79,7 +79,7 @@ template class SimplePublisherPlugin : public point_cloud_transport::SingleTopicPublisherPlugin { public: - ~SimplePublisherPlugin() override + ~SimplePublisherPlugin() { } diff --git a/include/point_cloud_transport/simple_subscriber_plugin.hpp b/include/point_cloud_transport/simple_subscriber_plugin.hpp index daeb941..6be6133 100644 --- a/include/point_cloud_transport/simple_subscriber_plugin.hpp +++ b/include/point_cloud_transport/simple_subscriber_plugin.hpp @@ -71,7 +71,7 @@ namespace point_cloud_transport * defaults to \/\. */ template -class SimpleSubscriberPlugin : public SingleTopicSubscriberPlugin +class SimpleSubscriberPlugin : public SubscriberPlugin { public: virtual ~SimpleSubscriberPlugin() diff --git a/include/point_cloud_transport/subscriber_plugin.hpp b/include/point_cloud_transport/subscriber_plugin.hpp index 2832085..5a42c4e 100644 --- a/include/point_cloud_transport/subscriber_plugin.hpp +++ b/include/point_cloud_transport/subscriber_plugin.hpp @@ -176,11 +176,6 @@ class SingleTopicSubscriberPlugin : public SubscriberPlugin */ virtual std::string getTopicToSubscribe(const std::string& base_topic) const = 0; - /** - * Return the datatype of the transported messages as text. - */ - virtual std::string getDataType() const = 0; - /** * Return the datatype of the dynamic reconfigure (as text in the form `package/Config`). * diff --git a/src/point_cloud_transport.cpp b/src/point_cloud_transport.cpp index 63aa2db..9d2dad3 100644 --- a/src/point_cloud_transport.cpp +++ b/src/point_cloud_transport.cpp @@ -59,8 +59,8 @@ namespace point_cloud_transport { -PointCloudTransportLoader::PointCloudTransportLoader() - : impl_(new Impl) +PointCloudTransportLoader::PointCloudTransportLoader() : pub_loader_(std::make_shared("point_cloud_transport", "point_cloud_transport::PublisherPlugin")), + sub_loader_(std::make_shared("point_cloud_transport", "point_cloud_transport::SubscriberPlugin")) { } @@ -70,7 +70,7 @@ PointCloudTransportLoader::~PointCloudTransportLoader() std::vector PointCloudTransportLoader::getDeclaredTransports() const { - auto transports = impl_->sub_loader_->getDeclaredClasses(); + auto transports = sub_loader_->getDeclaredClasses(); // Remove the "_sub" at the end of each class name. for (auto& transport : transports) @@ -84,13 +84,13 @@ std::unordered_map PointCloudTransportLoader::getLoada { std::unordered_map loadableTransports; - for (const auto& transportPlugin : impl_->sub_loader_->getDeclaredClasses()) + for (const auto& transportPlugin : sub_loader_->getDeclaredClasses()) { // If the plugin loads without throwing an exception, add its transport name to the list of valid plugins, // otherwise ignore it. try { - auto sub = impl_->sub_loader_->createSharedInstance(transportPlugin); + auto sub = sub_loader_->createSharedInstance(transportPlugin); // Remove the "_sub" at the end of each class name. loadableTransports[erase_last_copy(transportPlugin, "_sub")] = sub->getTransportName(); } catch (const pluginlib::LibraryLoadException & e) { @@ -105,12 +105,12 @@ std::unordered_map PointCloudTransportLoader::getLoada PubLoaderPtr PointCloudTransportLoader::getPublisherLoader() const { - return impl_->pub_loader_; + return pub_loader_; } SubLoaderPtr PointCloudTransportLoader::getSubscriberLoader() const { - return impl_->sub_loader_; + return sub_loader_; } thread_local std::unique_ptr loader; From 8f86c970bbb31ce7aa353bbe3c7b19040b1522dc Mon Sep 17 00:00:00 2001 From: john-maidbot Date: Mon, 19 Jun 2023 09:43:26 -0500 Subject: [PATCH 04/28] Progress --- PORT_TODO.md | 4 ++++ cfg/NoConfig.cfg | 9 --------- 2 files changed, 4 insertions(+), 9 deletions(-) delete mode 100755 cfg/NoConfig.cfg diff --git a/PORT_TODO.md b/PORT_TODO.md index ed52aa1..5fa9305 100644 --- a/PORT_TODO.md +++ b/PORT_TODO.md @@ -1,5 +1,9 @@ ## TODO - [] roscpp --> rclcpp conversion (IN PROGRESS) +--> CRAS cmake file +--> CRAS topic_tools replacement (ros2_introspection?) +--> Put CRAS stuff back into repo and get it to build + - [] c++ codebase builds with cmake (minus tests) - [] c++ codebase runs with test pointclouds - [] tests build and pass diff --git a/cfg/NoConfig.cfg b/cfg/NoConfig.cfg deleted file mode 100755 index 1943a7c..0000000 --- a/cfg/NoConfig.cfg +++ /dev/null @@ -1,9 +0,0 @@ -#! /usr/bin/env python - -PACKAGE='point_cloud_transport' - -from dynamic_reconfigure.parameter_generator_catkin import * - -gen = ParameterGenerator() - -exit(gen.generate(PACKAGE, "NoConfig", "NoConfig")) From 09fc40714aa227efe5fc9ad27a59ad4f6020df89 Mon Sep 17 00:00:00 2001 From: john-maidbot Date: Mon, 19 Jun 2023 11:21:13 -0500 Subject: [PATCH 05/28] progress --- .vscode/settings.json | 86 ++++++++++++++++++- CMakeLists.txt | 2 +- include/point_cloud_transport/c_api.h | 70 +++++++++++++++ .../point_cloud_codec.hpp | 10 +-- .../point_cloud_common.hpp | 10 ++- src/c_api.cpp | 42 +++++++++ src/point_cloud_codec.cpp | 51 ++++------- src/point_cloud_common.cpp | 43 +++++++++- 8 files changed, 266 insertions(+), 48 deletions(-) create mode 100644 include/point_cloud_transport/c_api.h create mode 100644 src/c_api.cpp diff --git a/.vscode/settings.json b/.vscode/settings.json index 09ecae2..a695773 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -2,6 +2,90 @@ "files.associations": { "memory": "cpp", "*.ipp": "cpp", - "system_error": "cpp" + "system_error": "cpp", + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "csignal": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "any": "cpp", + "array": "cpp", + "atomic": "cpp", + "hash_map": "cpp", + "hash_set": "cpp", + "strstream": "cpp", + "bit": "cpp", + "*.tcc": "cpp", + "bitset": "cpp", + "cfenv": "cpp", + "charconv": "cpp", + "chrono": "cpp", + "cinttypes": "cpp", + "codecvt": "cpp", + "compare": "cpp", + "complex": "cpp", + "concepts": "cpp", + "condition_variable": "cpp", + "coroutine": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "forward_list": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "regex": "cpp", + "source_location": "cpp", + "string_view": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "slist": "cpp", + "fstream": "cpp", + "future": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "numbers": "cpp", + "ostream": "cpp", + "ranges": "cpp", + "scoped_allocator": "cpp", + "semaphore": "cpp", + "shared_mutex": "cpp", + "span": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "valarray": "cpp", + "variant": "cpp", + "expected": "cpp" } } \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index cc0b27e..1bba423 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,7 +15,7 @@ include_directories( # Build libpoint_cloud_transport add_library(${PROJECT_NAME} - # src/point_cloud_codec.cpp # TODO: Is this needed? + # src/point_cloud_codec.cpp # TODO: This is needed for exposing the encode/decode functions without spinning an node src/point_cloud_common.cpp src/point_cloud_transport.cpp src/publisher.cpp diff --git a/include/point_cloud_transport/c_api.h b/include/point_cloud_transport/c_api.h new file mode 100644 index 0000000..5296538 --- /dev/null +++ b/include/point_cloud_transport/c_api.h @@ -0,0 +1,70 @@ +// #pragma once + +// // SPDX-License-Identifier: BSD-3-Clause +// // SPDX-FileCopyrightText: Czech Technical University in Prague + +// /** +// * \file +// * \brief Support definitions for declaration of a C API of modules. +// * \author Martin Pecka +// */ + +// #include +// #include + +// namespace cras +// { + +// /** +// * \brief Allocator function that should allocate a buffer of the given size on the caller side and return a pointer +// * to it. +// * +// * This is used throughout the C API to ease returning strings and byte buffers of dynamic size to the caller. Instead +// * of taking a writable `char*` argument or returning a `new`-allocated `const char*`, the function takes the allocator +// * as the argument. When it is about to return to the caller, it uses the allocator to get a caller-side-managed buffer +// * of the correct size and writes the string/byte buffer to this buffer. +// * +// * cras_py_common provides the `BytesAllocator` and `StringAllocator` classes that can be passed as this allocator +// * argument. Once the ctypes function call finishes, the caller can access `allocator.value` to get the string or +// * buffer returned by the function. +// */ +// typedef void* (*allocator_t)(size_t); + +// /** +// * \brief Allocate enough bytes using the given allocator and copy the given string into the buffer (including null +// * termination byte). +// * \param[in] allocator The allocator to use. +// * \param[in] string The zero-terminated string to copy into the allocated buffer. +// * \param[in] length Length of the string to copy including the null termination character. +// * \return Pointer to the allocated buffer. +// */ +// char* outputString(allocator_t allocator, const char* string, size_t length); + +// /** +// * \brief Allocate enough bytes using the given allocator and copy the given string into the buffer (including null +// * termination byte). +// * \param[in] allocator The allocator to use. +// * \param[in] string The string to copy into the allocated buffer. +// * \return Pointer to the allocated buffer. +// */ +// char* outputString(allocator_t allocator, const std::string& string); + +// /** +// * \brief Allocate enough bytes using the given allocator and copy the given bytes into the buffer. +// * \param[in] allocator The allocator to use. +// * \param[in] bytes The bytes to copy into the allocated buffer. +// * \param[in] length Length of `bytes`. +// * \return Pointer to the allocated buffer. +// */ +// uint8_t* outputByteBuffer(allocator_t allocator, const uint8_t* bytes, size_t length); + +// /** +// * \brief Allocate enough bytes using the given allocator and copy the given bytes into the buffer. +// * \param[in] allocator The allocator to use. +// * \param[in] bytes The bytes to copy into the allocated buffer. +// * \return Pointer to the allocated buffer. +// */ +// uint8_t* outputByteBuffer(allocator_t allocator, const std::vector& bytes); + + +// } diff --git a/include/point_cloud_transport/point_cloud_codec.hpp b/include/point_cloud_transport/point_cloud_codec.hpp index cc95ff7..8a806f5 100644 --- a/include/point_cloud_transport/point_cloud_codec.hpp +++ b/include/point_cloud_transport/point_cloud_codec.hpp @@ -44,13 +44,9 @@ #include #include -#include -#include -#include -#include -#include #include +#include #include #include @@ -64,11 +60,11 @@ namespace point_cloud_transport * subscribe() functions for creating advertisements and subscriptions of PointCloud2 topics. */ -class PointCloudCodec : public cras::HasLogger +class PointCloudCodec { public: //! Constructor - explicit PointCloudCodec(const cras::LogHelperPtr& log = std::make_shared()); + explicit PointCloudCodec(); std::shared_ptr getEncoderByName(const std::string& name) const; diff --git a/include/point_cloud_transport/point_cloud_common.hpp b/include/point_cloud_transport/point_cloud_common.hpp index f803d90..f2eff6d 100644 --- a/include/point_cloud_transport/point_cloud_common.hpp +++ b/include/point_cloud_transport/point_cloud_common.hpp @@ -28,8 +28,6 @@ #pragma once -#include "point_cloud_transport/point_cloud_common.hpp" - #include #include @@ -41,4 +39,12 @@ namespace point_cloud_transport */ std::string erase_last_copy(const std::string & input, const std::string & search); +std::vector split(const std::string& str, const std::string& delimiter, int maxSplits); + +// from cras::string_utils +bool endsWith(const std::string& str, const std::string& suffix); + +// from cras::string_utils +std::string removeSuffix(const std::string& str, const std::string& suffix, bool* hadSuffix); + } diff --git a/src/c_api.cpp b/src/c_api.cpp new file mode 100644 index 0000000..4545015 --- /dev/null +++ b/src/c_api.cpp @@ -0,0 +1,42 @@ +// // SPDX-License-Identifier: BSD-3-Clause +// // SPDX-FileCopyrightText: Czech Technical University in Prague + +// /** +// * \file +// * \brief Support definitions for declaration of a C API of modules +// * \author Martin Pecka +// */ + +// #include +// #include +// #include + +// #include + +// namespace cras +// { + +// char* outputString(cras::allocator_t allocator, const char* string, size_t length) +// { +// const auto buffer = static_cast(allocator(length)); +// strncpy(buffer, string, length); +// return buffer; +// } + +// char* outputString(allocator_t allocator, const std::string& string) +// { +// return outputString(allocator, string.c_str(), string.size() + 1); +// } + +// uint8_t* outputByteBuffer(allocator_t allocator, const uint8_t* bytes, size_t length) +// { +// const auto buffer = allocator(length); +// return static_cast(memcpy(buffer, bytes, length)); +// } + +// uint8_t* outputByteBuffer(allocator_t allocator, const std::vector& bytes) +// { +// return outputByteBuffer(allocator, bytes.data(), bytes.size()); +// } + +// } diff --git a/src/point_cloud_codec.cpp b/src/point_cloud_codec.cpp index bd2db8e..a525001 100644 --- a/src/point_cloud_codec.cpp +++ b/src/point_cloud_codec.cpp @@ -45,8 +45,10 @@ #include +#include #include #include +#include #include #include @@ -67,7 +69,7 @@ struct PointCloudCodec::Impl } }; -PointCloudCodec::PointCloudCodec(const cras::LogHelperPtr& log) : cras::HasLogger(log), impl_(new Impl) +PointCloudCodec::PointCloudCodec() : impl_(new Impl) { } @@ -77,12 +79,12 @@ bool transportNameMatches(const std::string& lookup_name, const std::string& nam { return true; } - const std::string transport_name = cras::removeSuffix(lookup_name, suffix); + const std::string transport_name = removeSuffix(lookup_name, suffix); if (transport_name == name) { return true; } - const auto parts = cras::split(transport_name, "/"); + const auto parts = split(transport_name, "/"); if (parts.size() == 2 && parts[1] == name) { return true; @@ -97,13 +99,12 @@ std::shared_ptr PointCloudCodec::getEnco { if (transportNameMatches(lookup_name, name, "_pub")) { - auto encoder = impl_->enc_loader_->createInstance(lookup_name); - encoder->setCrasLogger(this->log); + auto encoder = impl_->enc_loader_->createSharedInstance(lookup_name); return encoder; } } - ROS_DEBUG("Failed to find encoder %s.", name.c_str()); + // ROS_DEBUG("Failed to find encoder %s.", name.c_str()); return nullptr; } @@ -112,14 +113,13 @@ std::shared_ptr PointCloudCodec::getEnco { if (impl_->encoders_for_topics_.find(topic) != impl_->encoders_for_topics_.end()) { - auto encoder = impl_->enc_loader_->createInstance(impl_->encoders_for_topics_[topic]); - encoder->setCrasLogger(this->log); + auto encoder = impl_->enc_loader_->createSharedInstance(impl_->encoders_for_topics_[topic]); return encoder; } for (const auto& lookup_name : impl_->enc_loader_->getDeclaredClasses()) { - const auto& encoder = impl_->enc_loader_->createInstance(lookup_name); + const auto& encoder = impl_->enc_loader_->createSharedInstance(lookup_name); if (!encoder) { continue; @@ -127,12 +127,11 @@ std::shared_ptr PointCloudCodec::getEnco if (encoder->matchesTopic(topic, datatype)) { impl_->encoders_for_topics_[topic] = lookup_name; - encoder->setCrasLogger(this->log); return encoder; } } - ROS_DEBUG("Failed to find encoder for topic %s with data type %s.", topic.c_str(), datatype.c_str()); + // ROS_DEBUG("Failed to find encoder for topic %s with data type %s.", topic.c_str(), datatype.c_str()); return nullptr; } @@ -143,13 +142,12 @@ std::shared_ptr PointCloudCodec::getDec { if (transportNameMatches(lookup_name, name, "_sub")) { - auto decoder = impl_->dec_loader_->createInstance(lookup_name); - decoder->setCrasLogger(this->log); + auto decoder = impl_->dec_loader_->createSharedInstance(lookup_name); return decoder; } } - ROS_DEBUG("Failed to find decoder %s.", name.c_str()); + // ROS_DEBUG("Failed to find decoder %s.", name.c_str()); return nullptr; } @@ -158,14 +156,13 @@ std::shared_ptr PointCloudCodec::getDec { if (impl_->decoders_for_topics_.find(topic) != impl_->decoders_for_topics_.end()) { - auto decoder = impl_->dec_loader_->createInstance(impl_->decoders_for_topics_[topic]); - decoder->setCrasLogger(this->log); + auto decoder = impl_->dec_loader_->createSharedInstance(impl_->decoders_for_topics_[topic]); return decoder; } for (const auto& lookup_name : impl_->dec_loader_->getDeclaredClasses()) { - const auto& decoder = impl_->dec_loader_->createInstance(lookup_name); + const auto& decoder = impl_->dec_loader_->createSharedInstance(lookup_name); if (!decoder) { continue; @@ -173,20 +170,18 @@ std::shared_ptr PointCloudCodec::getDec if (decoder->matchesTopic(topic, datatype)) { impl_->decoders_for_topics_[topic] = lookup_name; - decoder->setCrasLogger(this->log); return decoder; } } - ROS_DEBUG("Failed to find decoder for topic %s with data type %s.", topic.c_str(), datatype.c_str()); + // ROS_DEBUG("Failed to find decoder for topic %s with data type %s.", topic.c_str(), datatype.c_str()); return nullptr; } -thread_local auto globalLogger = std::make_shared(); -thread_local PointCloudCodec point_cloud_transport_codec_instance(globalLogger); - } +// TODO: These functions seem overly complex. Can we simplify them? + bool pointCloudTransportCodecsEncode( const char* codec, sensor_msgs::msg::PointCloud2::_height_type rawHeight, @@ -244,8 +239,6 @@ bool pointCloudTransportCodecsEncode( memcpy(raw.data.data(), rawData, rawDataLength); raw.is_dense = rawIsDense; - point_cloud_transport::globalLogger->clear(); - auto encoder = point_cloud_transport::point_cloud_transport_codec_instance.getEncoderByName(codec); if (!encoder) { @@ -255,10 +248,6 @@ bool pointCloudTransportCodecsEncode( const auto compressed = encoder->encode(raw, config); - for (const auto& msg : point_cloud_transport::globalLogger->getMessages()) - cras::outputRosMessage(logMessagesAllocator, msg); - point_cloud_transport::globalLogger->clear(); - if (!compressed) { cras::outputString(errorStringAllocator, compressed.error()); @@ -319,8 +308,6 @@ bool pointCloudTransportCodecsDecode( cras::resizeBuffer(compressed, compressedDataLength); memcpy(cras::getBuffer(compressed), compressedData, compressedDataLength); - point_cloud_transport::globalLogger->clear(); - auto decoder = point_cloud_transport::point_cloud_transport_codec_instance.getDecoderByTopic( topicOrCodec, compressedType); if (!decoder) @@ -335,10 +322,6 @@ bool pointCloudTransportCodecsDecode( const auto res = decoder->decode(compressed, config); - for (const auto& msg : point_cloud_transport::globalLogger->getMessages()) - cras::outputRosMessage(logMessagesAllocator, msg); - point_cloud_transport::globalLogger->clear(); - if (!res) { cras::outputString(errorStringAllocator, res.error()); diff --git a/src/point_cloud_common.cpp b/src/point_cloud_common.cpp index b61c1a6..c0ee9fa 100644 --- a/src/point_cloud_common.cpp +++ b/src/point_cloud_common.cpp @@ -28,9 +28,6 @@ #include "point_cloud_transport/point_cloud_common.hpp" -#include -#include - namespace point_cloud_transport { @@ -44,4 +41,44 @@ std::string erase_last_copy(const std::string & input, const std::string & searc return input_copy; } +std::vector split(const std::string& str, const std::string& delimiter, int maxSplits) +{ + // inspired by https://stackoverflow.com/a/46931770/1076564, CC-BY-SA 4.0 + // renamed some variables, added the maxSplits option + size_t start{0}; + size_t end; + size_t delimiterLength{delimiter.length()}; + std::string token; + std::vector result; + + while ((end = str.find(delimiter, start)) != std::string::npos && (maxSplits == -1 || result.size() < maxSplits)) + { + token = str.substr(start, end - start); + start = end + delimiterLength; + result.push_back(token); + } + + result.push_back(str.substr(start)); + return result; +} + +// from cras::string_utils +bool endsWith(const std::string& str, const std::string& suffix) +{ + return str.size() >= suffix.size() && str.compare(str.size() - suffix.size(), suffix.size(), suffix) == 0; +} + + +// from cras::string_utils +std::string removeSuffix(const std::string& str, const std::string& suffix, bool* hadSuffix) +{ + const auto hasSuffix = endsWith(str, suffix); + if (hadSuffix != nullptr) + *hadSuffix = hasSuffix; + + return hasSuffix ? str.substr(0, str.length() - suffix.length()) : str; +} + + + } \ No newline at end of file From 0aafa65a0b233d759401a8ed0875ede5762ec336 Mon Sep 17 00:00:00 2001 From: john-maidbot Date: Mon, 19 Jun 2023 12:23:26 -0500 Subject: [PATCH 06/28] Fix build errors --- CMakeLists.txt | 2 + include/point_cloud_transport/expected.hpp | 44 + .../point_cloud_transport/raw_publisher.hpp | 6 + .../point_cloud_transport/raw_subscriber.hpp | 14 +- .../simple_publisher_plugin.hpp | 40 +- .../simple_subscriber_plugin.hpp | 35 +- .../subscriber_plugin.hpp | 6 + include/point_cloud_transport/tl/expected.hpp | 2444 +++++++++++++++++ src/raw_subscriber.cpp | 2 +- 9 files changed, 2579 insertions(+), 14 deletions(-) create mode 100644 include/point_cloud_transport/expected.hpp create mode 100644 include/point_cloud_transport/tl/expected.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 1bba423..642d23b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,5 +1,7 @@ cmake_minimum_required(VERSION 3.10.2) +set(CMAKE_CXX_STANDARD 17) + project(point_cloud_transport) find_package(ament_cmake_ros REQUIRED) diff --git a/include/point_cloud_transport/expected.hpp b/include/point_cloud_transport/expected.hpp new file mode 100644 index 0000000..c1162a9 --- /dev/null +++ b/include/point_cloud_transport/expected.hpp @@ -0,0 +1,44 @@ +#pragma once + +// SPDX-License-Identifier: BSD-3-Clause +// SPDX-FileCopyrightText: Czech Technical University in Prague + +/** + * \file + * \brief An implementation of the `std::expected` proposal. + * + * `std::expected` should be used in functions that can either succeed and return a value, or fail and return an error. + * \author Martin Pecka + */ + +#include + +namespace cras +{ + using ::tl::bad_expected_access; + using ::tl::expected; + using ::tl::in_place; + using ::tl::make_unexpected; + using ::tl::unexpect; + using ::tl::unexpected; +} + +#include + +namespace cras +{ +/** + * \brief Type trait determining whether type T is cras::expected or not. + * \tparam T The type to test. + */ +template +struct is_cras_expected : public ::std::false_type {}; + +/** + * \brief Type trait determining whether type T is std::optional or not. + * \tparam T Type of the expected value. + * \tparam E Type of the expected error. + */ +template +struct is_cras_expected<::cras::expected> : public ::std::true_type {}; +} diff --git a/include/point_cloud_transport/raw_publisher.hpp b/include/point_cloud_transport/raw_publisher.hpp index d1cce0b..7647fc8 100644 --- a/include/point_cloud_transport/raw_publisher.hpp +++ b/include/point_cloud_transport/raw_publisher.hpp @@ -61,6 +61,12 @@ class RawPublisher : public point_cloud_transport::SimplePublisherPlugin(compressed); + return this->decodeTyped(compressedPtr); + } + std::string getTransportName() const override; protected: - void internalCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& message, const Callback& user_cb) override; + void callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& message, const Callback& user_cb) override; std::string getTopicToSubscribe(const std::string& base_topic) const override; diff --git a/include/point_cloud_transport/simple_publisher_plugin.hpp b/include/point_cloud_transport/simple_publisher_plugin.hpp index 770e5da..c0f2685 100644 --- a/include/point_cloud_transport/simple_publisher_plugin.hpp +++ b/include/point_cloud_transport/simple_publisher_plugin.hpp @@ -44,10 +44,12 @@ #include #include #include +#include #include #include +#include #include #include @@ -76,13 +78,36 @@ namespace point_cloud_transport * \tparam Config Type of the publisher dynamic configuration. */ template -class SimplePublisherPlugin : public point_cloud_transport::SingleTopicPublisherPlugin +class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin { public: + //! \brief Result of cloud encoding. Either the compressed cloud message, empty value, or error message. + typedef cras::expected, std::string> TypedEncodeResult; + ~SimplePublisherPlugin() { } + rclcpp::Logger getLogger() const + { + if (simple_impl_) + { + return simple_impl_->logger_; + } + return rclcpp::get_logger("point_cloud_transport"); + } + + // template function for getting parameter of a given type + template + bool getParam(const std::string& name, T& value) const + { + if (simple_impl_) + { + return simple_impl_->node_->get_parameter(name, value); + } + return false; + } + uint32_t getNumSubscribers() const override { if (simple_impl_) @@ -101,6 +126,7 @@ class SimplePublisherPlugin : public point_cloud_transport::SingleTopicPublisher return {}; } + // TODO: Ask about this void publish(const sensor_msgs::msg::PointCloud2& message) const override { if (!simple_impl_ || !simple_impl_->pub_) @@ -120,11 +146,23 @@ class SimplePublisherPlugin : public point_cloud_transport::SingleTopicPublisher simple_impl_.reset(); } + + /** + * \brief Encode the given raw pointcloud into a compressed message. + * \param[in] raw The input raw pointcloud. + * \return The output shapeshifter holding the compressed cloud message (if encoding succeeds), or an error message. + */ + virtual TypedEncodeResult encodeTyped( + const sensor_msgs::msg::PointCloud2& raw) const = 0; + protected: + std::string base_topic_; + virtual void advertiseImpl( rclcpp::Node * node, const std::string & base_topic, rmw_qos_profile_t custom_qos) { + base_topic_ = base_topic; std::string transport_topic = getTopicToAdvertise(base_topic); simple_impl_ = std::make_unique(node); diff --git a/include/point_cloud_transport/simple_subscriber_plugin.hpp b/include/point_cloud_transport/simple_subscriber_plugin.hpp index 6be6133..ba2eae6 100644 --- a/include/point_cloud_transport/simple_subscriber_plugin.hpp +++ b/include/point_cloud_transport/simple_subscriber_plugin.hpp @@ -62,7 +62,7 @@ namespace point_cloud_transport * * A subclass need implement only two methods: * - getTransportName() from SubscriberPlugin - * - internalCallback() - processes a message and invoked the user PointCloud2 callback if + * - callback() - processes a message and invoked the user PointCloud2 callback if * appropriate. * * For access to the parameter server and name remappings, use nh(). @@ -101,17 +101,30 @@ class SimpleSubscriberPlugin : public SubscriberPlugin impl_.reset(); } -protected: /** - * \brief Process a message. Must be implemented by the subclass. - * - * @param message A message from the PublisherPlugin. - * @param user_cb The user PointCloud2 callback to invoke, if appropriate. + * \brief Decode the given compressed pointcloud into a raw message. + * \param[in] compressed The input compressed pointcloud. + * \param[in] config Config of the decompression (if it has any parameters). + * \return The raw cloud message (if encoding succeeds), or an error message. */ + virtual DecodeResult decodeTyped(const M& compressed) const = 0; + - virtual void internalCallback( - const typename std::shared_ptr & message, - const Callback & user_cb) = 0; +protected: + /** + * Process a message. Must be implemented by the subclass. + */ + virtual void callback(const typename M::ConstPtr& message, const Callback& user_cb) + { + DecodeResult res = this->decodeTyped(*message); + if (!res){ + RCLCPP_ERROR(rclcpp::get_logger("point_cloud_transport"), "Error decoding message by transport %s: %s.", this->getTransportName().c_str(), res.error().c_str()); + } + else if (res.value()) + { + user_cb(res.value().value()); + } + } /** * \brief Return the communication topic name for a given base topic. @@ -137,7 +150,7 @@ class SimpleSubscriberPlugin : public SubscriberPlugin impl_->sub_ = node->create_subscription( getTopicToSubscribe(base_topic), qos, [this, callback](const typename std::shared_ptr msg) { - internalCallback(msg, callback); + this->callback(msg, callback); }); } @@ -156,7 +169,7 @@ class SimpleSubscriberPlugin : public SubscriberPlugin impl_->sub_ = node->create_subscription( getTopicToSubscribe(base_topic), qos, [this, callback](const typename std::shared_ptr msg) { - internalCallback(msg, callback); + this->callback(msg, callback); }, options); } diff --git a/include/point_cloud_transport/subscriber_plugin.hpp b/include/point_cloud_transport/subscriber_plugin.hpp index 5a42c4e..5641ed3 100644 --- a/include/point_cloud_transport/subscriber_plugin.hpp +++ b/include/point_cloud_transport/subscriber_plugin.hpp @@ -43,11 +43,13 @@ #include #include #include +#include #include "rclcpp/macros.hpp" #include "rclcpp/node.hpp" #include +#include #include namespace point_cloud_transport @@ -59,6 +61,10 @@ namespace point_cloud_transport class SubscriberPlugin { public: + //! \brief Result of cloud decoding. Either a `sensor_msgs::msg::PointCloud2` holding the raw message, empty value or + //! error message. + typedef cras::expected, std::string> DecodeResult; + SubscriberPlugin() = default; SubscriberPlugin(const SubscriberPlugin &) = delete; SubscriberPlugin & operator=(const SubscriberPlugin &) = delete; diff --git a/include/point_cloud_transport/tl/expected.hpp b/include/point_cloud_transport/tl/expected.hpp new file mode 100644 index 0000000..7fd6467 --- /dev/null +++ b/include/point_cloud_transport/tl/expected.hpp @@ -0,0 +1,2444 @@ +/// +// expected - An implementation of std::expected with extensions +// Written in 2017 by Sy Brand (tartanllama@gmail.com, @TartanLlama) +// +// Documentation available at http://tl.tartanllama.xyz/ +// +// To the extent possible under law, the author(s) have dedicated all +// copyright and related and neighboring rights to this software to the +// public domain worldwide. This software is distributed without any warranty. +// +// You should have received a copy of the CC0 Public Domain Dedication +// along with this software. If not, see +// . +/// + +#ifndef TL_EXPECTED_HPP +#define TL_EXPECTED_HPP + +#define TL_EXPECTED_VERSION_MAJOR 1 +#define TL_EXPECTED_VERSION_MINOR 0 +#define TL_EXPECTED_VERSION_PATCH 1 + +#include +#include +#include +#include + +#if defined(__EXCEPTIONS) || defined(_CPPUNWIND) +#define TL_EXPECTED_EXCEPTIONS_ENABLED +#endif + +#if (defined(_MSC_VER) && _MSC_VER == 1900) +#define TL_EXPECTED_MSVC2015 +#define TL_EXPECTED_MSVC2015_CONSTEXPR +#else +#define TL_EXPECTED_MSVC2015_CONSTEXPR constexpr +#endif + +#if (defined(__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ <= 9 && \ + !defined(__clang__)) +#define TL_EXPECTED_GCC49 +#endif + +#if (defined(__GNUC__) && __GNUC__ == 5 && __GNUC_MINOR__ <= 4 && \ + !defined(__clang__)) +#define TL_EXPECTED_GCC54 +#endif + +#if (defined(__GNUC__) && __GNUC__ == 5 && __GNUC_MINOR__ <= 5 && \ + !defined(__clang__)) +#define TL_EXPECTED_GCC55 +#endif + +#if !defined(TL_ASSERT) +//can't have assert in constexpr in C++11 and GCC 4.9 has a compiler bug +#if (__cplusplus > 201103L) && !defined(TL_EXPECTED_GCC49) +#include +#define TL_ASSERT(x) assert(x) +#else +#define TL_ASSERT(x) +#endif +#endif + +#if (defined(__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ <= 9 && \ + !defined(__clang__)) +// GCC < 5 doesn't support overloading on const&& for member functions + +#define TL_EXPECTED_NO_CONSTRR +// GCC < 5 doesn't support some standard C++11 type traits +#define TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(T) \ + std::has_trivial_copy_constructor +#define TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(T) \ + std::has_trivial_copy_assign + +// This one will be different for GCC 5.7 if it's ever supported +#define TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(T) \ + std::is_trivially_destructible + +// GCC 5 < v < 8 has a bug in is_trivially_copy_constructible which breaks +// std::vector for non-copyable types +#elif (defined(__GNUC__) && __GNUC__ < 8 && !defined(__clang__)) +#ifndef TL_GCC_LESS_8_TRIVIALLY_COPY_CONSTRUCTIBLE_MUTEX +#define TL_GCC_LESS_8_TRIVIALLY_COPY_CONSTRUCTIBLE_MUTEX +namespace tl { +namespace detail { +template +struct is_trivially_copy_constructible + : std::is_trivially_copy_constructible {}; +#ifdef _GLIBCXX_VECTOR +template +struct is_trivially_copy_constructible> : std::false_type {}; +#endif +} // namespace detail +} // namespace tl +#endif + +#define TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(T) \ + tl::detail::is_trivially_copy_constructible +#define TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(T) \ + std::is_trivially_copy_assignable +#define TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(T) \ + std::is_trivially_destructible +#else +#define TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(T) \ + std::is_trivially_copy_constructible +#define TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(T) \ + std::is_trivially_copy_assignable +#define TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(T) \ + std::is_trivially_destructible +#endif + +#if __cplusplus > 201103L +#define TL_EXPECTED_CXX14 +#endif + +#ifdef TL_EXPECTED_GCC49 +#define TL_EXPECTED_GCC49_CONSTEXPR +#else +#define TL_EXPECTED_GCC49_CONSTEXPR constexpr +#endif + +#if (__cplusplus == 201103L || defined(TL_EXPECTED_MSVC2015) || \ + defined(TL_EXPECTED_GCC49)) +#define TL_EXPECTED_11_CONSTEXPR +#else +#define TL_EXPECTED_11_CONSTEXPR constexpr +#endif + +namespace tl { +template class expected; + +#ifndef TL_MONOSTATE_INPLACE_MUTEX +#define TL_MONOSTATE_INPLACE_MUTEX +class monostate {}; + +struct in_place_t { + explicit in_place_t() = default; +}; +static constexpr in_place_t in_place{}; +#endif + +template class unexpected { +public: + static_assert(!std::is_same::value, "E must not be void"); + + unexpected() = delete; + constexpr explicit unexpected(const E &e) : m_val(e) {} + + constexpr explicit unexpected(E &&e) : m_val(std::move(e)) {} + + template ::value>::type * = nullptr> + constexpr explicit unexpected(Args &&...args) + : m_val(std::forward(args)...) {} + template < + class U, class... Args, + typename std::enable_if &, Args &&...>::value>::type * = nullptr> + constexpr explicit unexpected(std::initializer_list l, Args &&...args) + : m_val(l, std::forward(args)...) {} + + constexpr const E &value() const & { return m_val; } + TL_EXPECTED_11_CONSTEXPR E &value() & { return m_val; } + TL_EXPECTED_11_CONSTEXPR E &&value() && { return std::move(m_val); } + constexpr const E &&value() const && { return std::move(m_val); } + +private: + E m_val; +}; + +#ifdef __cpp_deduction_guides +template unexpected(E) -> unexpected; +#endif + +template +constexpr bool operator==(const unexpected &lhs, const unexpected &rhs) { + return lhs.value() == rhs.value(); +} +template +constexpr bool operator!=(const unexpected &lhs, const unexpected &rhs) { + return lhs.value() != rhs.value(); +} +template +constexpr bool operator<(const unexpected &lhs, const unexpected &rhs) { + return lhs.value() < rhs.value(); +} +template +constexpr bool operator<=(const unexpected &lhs, const unexpected &rhs) { + return lhs.value() <= rhs.value(); +} +template +constexpr bool operator>(const unexpected &lhs, const unexpected &rhs) { + return lhs.value() > rhs.value(); +} +template +constexpr bool operator>=(const unexpected &lhs, const unexpected &rhs) { + return lhs.value() >= rhs.value(); +} + +template +unexpected::type> make_unexpected(E &&e) { + return unexpected::type>(std::forward(e)); +} + +struct unexpect_t { + unexpect_t() = default; +}; +static constexpr unexpect_t unexpect{}; + +namespace detail { +template +[[noreturn]] TL_EXPECTED_11_CONSTEXPR void throw_exception(E &&e) { +#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED + throw std::forward(e); +#else + (void)e; +#ifdef _MSC_VER + __assume(0); +#else + __builtin_unreachable(); +#endif +#endif +} + +#ifndef TL_TRAITS_MUTEX +#define TL_TRAITS_MUTEX +// C++14-style aliases for brevity +template using remove_const_t = typename std::remove_const::type; +template +using remove_reference_t = typename std::remove_reference::type; +template using decay_t = typename std::decay::type; +template +using enable_if_t = typename std::enable_if::type; +template +using conditional_t = typename std::conditional::type; + +// std::conjunction from C++17 +template struct conjunction : std::true_type {}; +template struct conjunction : B {}; +template +struct conjunction + : std::conditional, B>::type {}; + +#if defined(_LIBCPP_VERSION) && __cplusplus == 201103L +#define TL_TRAITS_LIBCXX_MEM_FN_WORKAROUND +#endif + +// In C++11 mode, there's an issue in libc++'s std::mem_fn +// which results in a hard-error when using it in a noexcept expression +// in some cases. This is a check to workaround the common failing case. +#ifdef TL_TRAITS_LIBCXX_MEM_FN_WORKAROUND +template +struct is_pointer_to_non_const_member_func : std::false_type {}; +template +struct is_pointer_to_non_const_member_func + : std::true_type {}; +template +struct is_pointer_to_non_const_member_func + : std::true_type {}; +template +struct is_pointer_to_non_const_member_func + : std::true_type {}; +template +struct is_pointer_to_non_const_member_func + : std::true_type {}; +template +struct is_pointer_to_non_const_member_func + : std::true_type {}; +template +struct is_pointer_to_non_const_member_func + : std::true_type {}; + +template struct is_const_or_const_ref : std::false_type {}; +template struct is_const_or_const_ref : std::true_type {}; +template struct is_const_or_const_ref : std::true_type {}; +#endif + +// std::invoke from C++17 +// https://stackoverflow.com/questions/38288042/c11-14-invoke-workaround +template < + typename Fn, typename... Args, +#ifdef TL_TRAITS_LIBCXX_MEM_FN_WORKAROUND + typename = enable_if_t::value && + is_const_or_const_ref::value)>, +#endif + typename = enable_if_t>::value>, int = 0> +constexpr auto invoke(Fn &&f, Args &&...args) noexcept( + noexcept(std::mem_fn(f)(std::forward(args)...))) + -> decltype(std::mem_fn(f)(std::forward(args)...)) { + return std::mem_fn(f)(std::forward(args)...); +} + +template >::value>> +constexpr auto invoke(Fn &&f, Args &&...args) noexcept( + noexcept(std::forward(f)(std::forward(args)...))) + -> decltype(std::forward(f)(std::forward(args)...)) { + return std::forward(f)(std::forward(args)...); +} + +// std::invoke_result from C++17 +template struct invoke_result_impl; + +template +struct invoke_result_impl< + F, + decltype(detail::invoke(std::declval(), std::declval()...), void()), + Us...> { + using type = + decltype(detail::invoke(std::declval(), std::declval()...)); +}; + +template +using invoke_result = invoke_result_impl; + +template +using invoke_result_t = typename invoke_result::type; + +#if defined(_MSC_VER) && _MSC_VER <= 1900 +// TODO make a version which works with MSVC 2015 +template struct is_swappable : std::true_type {}; + +template struct is_nothrow_swappable : std::true_type {}; +#else +// https://stackoverflow.com/questions/26744589/what-is-a-proper-way-to-implement-is-swappable-to-test-for-the-swappable-concept +namespace swap_adl_tests { +// if swap ADL finds this then it would call std::swap otherwise (same +// signature) +struct tag {}; + +template tag swap(T &, T &); +template tag swap(T (&a)[N], T (&b)[N]); + +// helper functions to test if an unqualified swap is possible, and if it +// becomes std::swap +template std::false_type can_swap(...) noexcept(false); +template (), std::declval()))> +std::true_type can_swap(int) noexcept(noexcept(swap(std::declval(), + std::declval()))); + +template std::false_type uses_std(...); +template +std::is_same(), std::declval())), tag> +uses_std(int); + +template +struct is_std_swap_noexcept + : std::integral_constant::value && + std::is_nothrow_move_assignable::value> {}; + +template +struct is_std_swap_noexcept : is_std_swap_noexcept {}; + +template +struct is_adl_swap_noexcept + : std::integral_constant(0))> {}; +} // namespace swap_adl_tests + +template +struct is_swappable + : std::integral_constant< + bool, + decltype(detail::swap_adl_tests::can_swap(0))::value && + (!decltype(detail::swap_adl_tests::uses_std(0))::value || + (std::is_move_assignable::value && + std::is_move_constructible::value))> {}; + +template +struct is_swappable + : std::integral_constant< + bool, + decltype(detail::swap_adl_tests::can_swap(0))::value && + (!decltype(detail::swap_adl_tests::uses_std( + 0))::value || + is_swappable::value)> {}; + +template +struct is_nothrow_swappable + : std::integral_constant< + bool, + is_swappable::value && + ((decltype(detail::swap_adl_tests::uses_std(0))::value && + detail::swap_adl_tests::is_std_swap_noexcept::value) || + (!decltype(detail::swap_adl_tests::uses_std(0))::value && + detail::swap_adl_tests::is_adl_swap_noexcept::value))> {}; +#endif +#endif + +// Trait for checking if a type is a tl::expected +template struct is_expected_impl : std::false_type {}; +template +struct is_expected_impl> : std::true_type {}; +template using is_expected = is_expected_impl>; + +template +using expected_enable_forward_value = detail::enable_if_t< + std::is_constructible::value && + !std::is_same, in_place_t>::value && + !std::is_same, detail::decay_t>::value && + !std::is_same, detail::decay_t>::value>; + +template +using expected_enable_from_other = detail::enable_if_t< + std::is_constructible::value && + std::is_constructible::value && + !std::is_constructible &>::value && + !std::is_constructible &&>::value && + !std::is_constructible &>::value && + !std::is_constructible &&>::value && + !std::is_convertible &, T>::value && + !std::is_convertible &&, T>::value && + !std::is_convertible &, T>::value && + !std::is_convertible &&, T>::value>; + +template +using is_void_or = conditional_t::value, std::true_type, U>; + +template +using is_copy_constructible_or_void = + is_void_or>; + +template +using is_move_constructible_or_void = + is_void_or>; + +template +using is_copy_assignable_or_void = is_void_or>; + +template +using is_move_assignable_or_void = is_void_or>; + +} // namespace detail + +namespace detail { +struct no_init_t {}; +static constexpr no_init_t no_init{}; + +// Implements the storage of the values, and ensures that the destructor is +// trivial if it can be. +// +// This specialization is for where neither `T` or `E` is trivially +// destructible, so the destructors must be called on destruction of the +// `expected` +template ::value, + bool = std::is_trivially_destructible::value> +struct expected_storage_base { + constexpr expected_storage_base() : m_val(T{}), m_has_val(true) {} + constexpr expected_storage_base(no_init_t) : m_no_init(), m_has_val(false) {} + + template ::value> * = + nullptr> + constexpr expected_storage_base(in_place_t, Args &&...args) + : m_val(std::forward(args)...), m_has_val(true) {} + + template &, Args &&...>::value> * = nullptr> + constexpr expected_storage_base(in_place_t, std::initializer_list il, + Args &&...args) + : m_val(il, std::forward(args)...), m_has_val(true) {} + template ::value> * = + nullptr> + constexpr explicit expected_storage_base(unexpect_t, Args &&...args) + : m_unexpect(std::forward(args)...), m_has_val(false) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected_storage_base(unexpect_t, + std::initializer_list il, + Args &&...args) + : m_unexpect(il, std::forward(args)...), m_has_val(false) {} + + ~expected_storage_base() { + if (m_has_val) { + m_val.~T(); + } else { + m_unexpect.~unexpected(); + } + } + union { + T m_val; + unexpected m_unexpect; + char m_no_init; + }; + bool m_has_val; +}; + +// This specialization is for when both `T` and `E` are trivially-destructible, +// so the destructor of the `expected` can be trivial. +template struct expected_storage_base { + constexpr expected_storage_base() : m_val(T{}), m_has_val(true) {} + constexpr expected_storage_base(no_init_t) : m_no_init(), m_has_val(false) {} + + template ::value> * = + nullptr> + constexpr expected_storage_base(in_place_t, Args &&...args) + : m_val(std::forward(args)...), m_has_val(true) {} + + template &, Args &&...>::value> * = nullptr> + constexpr expected_storage_base(in_place_t, std::initializer_list il, + Args &&...args) + : m_val(il, std::forward(args)...), m_has_val(true) {} + template ::value> * = + nullptr> + constexpr explicit expected_storage_base(unexpect_t, Args &&...args) + : m_unexpect(std::forward(args)...), m_has_val(false) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected_storage_base(unexpect_t, + std::initializer_list il, + Args &&...args) + : m_unexpect(il, std::forward(args)...), m_has_val(false) {} + + ~expected_storage_base() = default; + union { + T m_val; + unexpected m_unexpect; + char m_no_init; + }; + bool m_has_val; +}; + +// T is trivial, E is not. +template struct expected_storage_base { + constexpr expected_storage_base() : m_val(T{}), m_has_val(true) {} + TL_EXPECTED_MSVC2015_CONSTEXPR expected_storage_base(no_init_t) + : m_no_init(), m_has_val(false) {} + + template ::value> * = + nullptr> + constexpr expected_storage_base(in_place_t, Args &&...args) + : m_val(std::forward(args)...), m_has_val(true) {} + + template &, Args &&...>::value> * = nullptr> + constexpr expected_storage_base(in_place_t, std::initializer_list il, + Args &&...args) + : m_val(il, std::forward(args)...), m_has_val(true) {} + template ::value> * = + nullptr> + constexpr explicit expected_storage_base(unexpect_t, Args &&...args) + : m_unexpect(std::forward(args)...), m_has_val(false) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected_storage_base(unexpect_t, + std::initializer_list il, + Args &&...args) + : m_unexpect(il, std::forward(args)...), m_has_val(false) {} + + ~expected_storage_base() { + if (!m_has_val) { + m_unexpect.~unexpected(); + } + } + + union { + T m_val; + unexpected m_unexpect; + char m_no_init; + }; + bool m_has_val; +}; + +// E is trivial, T is not. +template struct expected_storage_base { + constexpr expected_storage_base() : m_val(T{}), m_has_val(true) {} + constexpr expected_storage_base(no_init_t) : m_no_init(), m_has_val(false) {} + + template ::value> * = + nullptr> + constexpr expected_storage_base(in_place_t, Args &&...args) + : m_val(std::forward(args)...), m_has_val(true) {} + + template &, Args &&...>::value> * = nullptr> + constexpr expected_storage_base(in_place_t, std::initializer_list il, + Args &&...args) + : m_val(il, std::forward(args)...), m_has_val(true) {} + template ::value> * = + nullptr> + constexpr explicit expected_storage_base(unexpect_t, Args &&...args) + : m_unexpect(std::forward(args)...), m_has_val(false) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected_storage_base(unexpect_t, + std::initializer_list il, + Args &&...args) + : m_unexpect(il, std::forward(args)...), m_has_val(false) {} + + ~expected_storage_base() { + if (m_has_val) { + m_val.~T(); + } + } + union { + T m_val; + unexpected m_unexpect; + char m_no_init; + }; + bool m_has_val; +}; + +// `T` is `void`, `E` is trivially-destructible +template struct expected_storage_base { + #if __GNUC__ <= 5 + //no constexpr for GCC 4/5 bug + #else + TL_EXPECTED_MSVC2015_CONSTEXPR + #endif + expected_storage_base() : m_has_val(true) {} + + constexpr expected_storage_base(no_init_t) : m_val(), m_has_val(false) {} + + constexpr expected_storage_base(in_place_t) : m_has_val(true) {} + + template ::value> * = + nullptr> + constexpr explicit expected_storage_base(unexpect_t, Args &&...args) + : m_unexpect(std::forward(args)...), m_has_val(false) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected_storage_base(unexpect_t, + std::initializer_list il, + Args &&...args) + : m_unexpect(il, std::forward(args)...), m_has_val(false) {} + + ~expected_storage_base() = default; + struct dummy {}; + union { + unexpected m_unexpect; + dummy m_val; + }; + bool m_has_val; +}; + +// `T` is `void`, `E` is not trivially-destructible +template struct expected_storage_base { + constexpr expected_storage_base() : m_dummy(), m_has_val(true) {} + constexpr expected_storage_base(no_init_t) : m_dummy(), m_has_val(false) {} + + constexpr expected_storage_base(in_place_t) : m_dummy(), m_has_val(true) {} + + template ::value> * = + nullptr> + constexpr explicit expected_storage_base(unexpect_t, Args &&...args) + : m_unexpect(std::forward(args)...), m_has_val(false) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected_storage_base(unexpect_t, + std::initializer_list il, + Args &&...args) + : m_unexpect(il, std::forward(args)...), m_has_val(false) {} + + ~expected_storage_base() { + if (!m_has_val) { + m_unexpect.~unexpected(); + } + } + + union { + unexpected m_unexpect; + char m_dummy; + }; + bool m_has_val; +}; + +// This base class provides some handy member functions which can be used in +// further derived classes +template +struct expected_operations_base : expected_storage_base { + using expected_storage_base::expected_storage_base; + + template void construct(Args &&...args) noexcept { + new (std::addressof(this->m_val)) T(std::forward(args)...); + this->m_has_val = true; + } + + template void construct_with(Rhs &&rhs) noexcept { + new (std::addressof(this->m_val)) T(std::forward(rhs).get()); + this->m_has_val = true; + } + + template void construct_error(Args &&...args) noexcept { + new (std::addressof(this->m_unexpect)) + unexpected(std::forward(args)...); + this->m_has_val = false; + } + +#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED + + // These assign overloads ensure that the most efficient assignment + // implementation is used while maintaining the strong exception guarantee. + // The problematic case is where rhs has a value, but *this does not. + // + // This overload handles the case where we can just copy-construct `T` + // directly into place without throwing. + template ::value> + * = nullptr> + void assign(const expected_operations_base &rhs) noexcept { + if (!this->m_has_val && rhs.m_has_val) { + geterr().~unexpected(); + construct(rhs.get()); + } else { + assign_common(rhs); + } + } + + // This overload handles the case where we can attempt to create a copy of + // `T`, then no-throw move it into place if the copy was successful. + template ::value && + std::is_nothrow_move_constructible::value> + * = nullptr> + void assign(const expected_operations_base &rhs) noexcept { + if (!this->m_has_val && rhs.m_has_val) { + T tmp = rhs.get(); + geterr().~unexpected(); + construct(std::move(tmp)); + } else { + assign_common(rhs); + } + } + + // This overload is the worst-case, where we have to move-construct the + // unexpected value into temporary storage, then try to copy the T into place. + // If the construction succeeds, then everything is fine, but if it throws, + // then we move the old unexpected value back into place before rethrowing the + // exception. + template ::value && + !std::is_nothrow_move_constructible::value> + * = nullptr> + void assign(const expected_operations_base &rhs) { + if (!this->m_has_val && rhs.m_has_val) { + auto tmp = std::move(geterr()); + geterr().~unexpected(); + +#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED + try { + construct(rhs.get()); + } catch (...) { + geterr() = std::move(tmp); + throw; + } +#else + construct(rhs.get()); +#endif + } else { + assign_common(rhs); + } + } + + // These overloads do the same as above, but for rvalues + template ::value> + * = nullptr> + void assign(expected_operations_base &&rhs) noexcept { + if (!this->m_has_val && rhs.m_has_val) { + geterr().~unexpected(); + construct(std::move(rhs).get()); + } else { + assign_common(std::move(rhs)); + } + } + + template ::value> + * = nullptr> + void assign(expected_operations_base &&rhs) { + if (!this->m_has_val && rhs.m_has_val) { + auto tmp = std::move(geterr()); + geterr().~unexpected(); +#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED + try { + construct(std::move(rhs).get()); + } catch (...) { + geterr() = std::move(tmp); + throw; + } +#else + construct(std::move(rhs).get()); +#endif + } else { + assign_common(std::move(rhs)); + } + } + +#else + + // If exceptions are disabled then we can just copy-construct + void assign(const expected_operations_base &rhs) noexcept { + if (!this->m_has_val && rhs.m_has_val) { + geterr().~unexpected(); + construct(rhs.get()); + } else { + assign_common(rhs); + } + } + + void assign(expected_operations_base &&rhs) noexcept { + if (!this->m_has_val && rhs.m_has_val) { + geterr().~unexpected(); + construct(std::move(rhs).get()); + } else { + assign_common(std::move(rhs)); + } + } + +#endif + + // The common part of move/copy assigning + template void assign_common(Rhs &&rhs) { + if (this->m_has_val) { + if (rhs.m_has_val) { + get() = std::forward(rhs).get(); + } else { + destroy_val(); + construct_error(std::forward(rhs).geterr()); + } + } else { + if (!rhs.m_has_val) { + geterr() = std::forward(rhs).geterr(); + } + } + } + + bool has_value() const { return this->m_has_val; } + + TL_EXPECTED_11_CONSTEXPR T &get() & { return this->m_val; } + constexpr const T &get() const & { return this->m_val; } + TL_EXPECTED_11_CONSTEXPR T &&get() && { return std::move(this->m_val); } +#ifndef TL_EXPECTED_NO_CONSTRR + constexpr const T &&get() const && { return std::move(this->m_val); } +#endif + + TL_EXPECTED_11_CONSTEXPR unexpected &geterr() & { + return this->m_unexpect; + } + constexpr const unexpected &geterr() const & { return this->m_unexpect; } + TL_EXPECTED_11_CONSTEXPR unexpected &&geterr() && { + return std::move(this->m_unexpect); + } +#ifndef TL_EXPECTED_NO_CONSTRR + constexpr const unexpected &&geterr() const && { + return std::move(this->m_unexpect); + } +#endif + + TL_EXPECTED_11_CONSTEXPR void destroy_val() { get().~T(); } +}; + +// This base class provides some handy member functions which can be used in +// further derived classes +template +struct expected_operations_base : expected_storage_base { + using expected_storage_base::expected_storage_base; + + template void construct() noexcept { this->m_has_val = true; } + + // This function doesn't use its argument, but needs it so that code in + // levels above this can work independently of whether T is void + template void construct_with(Rhs &&) noexcept { + this->m_has_val = true; + } + + template void construct_error(Args &&...args) noexcept { + new (std::addressof(this->m_unexpect)) + unexpected(std::forward(args)...); + this->m_has_val = false; + } + + template void assign(Rhs &&rhs) noexcept { + if (!this->m_has_val) { + if (rhs.m_has_val) { + geterr().~unexpected(); + construct(); + } else { + geterr() = std::forward(rhs).geterr(); + } + } else { + if (!rhs.m_has_val) { + construct_error(std::forward(rhs).geterr()); + } + } + } + + bool has_value() const { return this->m_has_val; } + + TL_EXPECTED_11_CONSTEXPR unexpected &geterr() & { + return this->m_unexpect; + } + constexpr const unexpected &geterr() const & { return this->m_unexpect; } + TL_EXPECTED_11_CONSTEXPR unexpected &&geterr() && { + return std::move(this->m_unexpect); + } +#ifndef TL_EXPECTED_NO_CONSTRR + constexpr const unexpected &&geterr() const && { + return std::move(this->m_unexpect); + } +#endif + + TL_EXPECTED_11_CONSTEXPR void destroy_val() { + // no-op + } +}; + +// This class manages conditionally having a trivial copy constructor +// This specialization is for when T and E are trivially copy constructible +template :: + value &&TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(E)::value> +struct expected_copy_base : expected_operations_base { + using expected_operations_base::expected_operations_base; +}; + +// This specialization is for when T or E are not trivially copy constructible +template +struct expected_copy_base : expected_operations_base { + using expected_operations_base::expected_operations_base; + + expected_copy_base() = default; + expected_copy_base(const expected_copy_base &rhs) + : expected_operations_base(no_init) { + if (rhs.has_value()) { + this->construct_with(rhs); + } else { + this->construct_error(rhs.geterr()); + } + } + + expected_copy_base(expected_copy_base &&rhs) = default; + expected_copy_base &operator=(const expected_copy_base &rhs) = default; + expected_copy_base &operator=(expected_copy_base &&rhs) = default; +}; + +// This class manages conditionally having a trivial move constructor +// Unfortunately there's no way to achieve this in GCC < 5 AFAIK, since it +// doesn't implement an analogue to std::is_trivially_move_constructible. We +// have to make do with a non-trivial move constructor even if T is trivially +// move constructible +#ifndef TL_EXPECTED_GCC49 +template >::value + &&std::is_trivially_move_constructible::value> +struct expected_move_base : expected_copy_base { + using expected_copy_base::expected_copy_base; +}; +#else +template struct expected_move_base; +#endif +template +struct expected_move_base : expected_copy_base { + using expected_copy_base::expected_copy_base; + + expected_move_base() = default; + expected_move_base(const expected_move_base &rhs) = default; + + expected_move_base(expected_move_base &&rhs) noexcept( + std::is_nothrow_move_constructible::value) + : expected_copy_base(no_init) { + if (rhs.has_value()) { + this->construct_with(std::move(rhs)); + } else { + this->construct_error(std::move(rhs.geterr())); + } + } + expected_move_base &operator=(const expected_move_base &rhs) = default; + expected_move_base &operator=(expected_move_base &&rhs) = default; +}; + +// This class manages conditionally having a trivial copy assignment operator +template >::value + &&TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(E)::value + &&TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(E)::value + &&TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(E)::value> +struct expected_copy_assign_base : expected_move_base { + using expected_move_base::expected_move_base; +}; + +template +struct expected_copy_assign_base : expected_move_base { + using expected_move_base::expected_move_base; + + expected_copy_assign_base() = default; + expected_copy_assign_base(const expected_copy_assign_base &rhs) = default; + + expected_copy_assign_base(expected_copy_assign_base &&rhs) = default; + expected_copy_assign_base &operator=(const expected_copy_assign_base &rhs) { + this->assign(rhs); + return *this; + } + expected_copy_assign_base & + operator=(expected_copy_assign_base &&rhs) = default; +}; + +// This class manages conditionally having a trivial move assignment operator +// Unfortunately there's no way to achieve this in GCC < 5 AFAIK, since it +// doesn't implement an analogue to std::is_trivially_move_assignable. We have +// to make do with a non-trivial move assignment operator even if T is trivially +// move assignable +#ifndef TL_EXPECTED_GCC49 +template , + std::is_trivially_move_constructible, + std::is_trivially_move_assignable>>:: + value &&std::is_trivially_destructible::value + &&std::is_trivially_move_constructible::value + &&std::is_trivially_move_assignable::value> +struct expected_move_assign_base : expected_copy_assign_base { + using expected_copy_assign_base::expected_copy_assign_base; +}; +#else +template struct expected_move_assign_base; +#endif + +template +struct expected_move_assign_base + : expected_copy_assign_base { + using expected_copy_assign_base::expected_copy_assign_base; + + expected_move_assign_base() = default; + expected_move_assign_base(const expected_move_assign_base &rhs) = default; + + expected_move_assign_base(expected_move_assign_base &&rhs) = default; + + expected_move_assign_base & + operator=(const expected_move_assign_base &rhs) = default; + + expected_move_assign_base & + operator=(expected_move_assign_base &&rhs) noexcept( + std::is_nothrow_move_constructible::value + &&std::is_nothrow_move_assignable::value) { + this->assign(std::move(rhs)); + return *this; + } +}; + +// expected_delete_ctor_base will conditionally delete copy and move +// constructors depending on whether T is copy/move constructible +template ::value && + std::is_copy_constructible::value), + bool EnableMove = (is_move_constructible_or_void::value && + std::is_move_constructible::value)> +struct expected_delete_ctor_base { + expected_delete_ctor_base() = default; + expected_delete_ctor_base(const expected_delete_ctor_base &) = default; + expected_delete_ctor_base(expected_delete_ctor_base &&) noexcept = default; + expected_delete_ctor_base & + operator=(const expected_delete_ctor_base &) = default; + expected_delete_ctor_base & + operator=(expected_delete_ctor_base &&) noexcept = default; +}; + +template +struct expected_delete_ctor_base { + expected_delete_ctor_base() = default; + expected_delete_ctor_base(const expected_delete_ctor_base &) = default; + expected_delete_ctor_base(expected_delete_ctor_base &&) noexcept = delete; + expected_delete_ctor_base & + operator=(const expected_delete_ctor_base &) = default; + expected_delete_ctor_base & + operator=(expected_delete_ctor_base &&) noexcept = default; +}; + +template +struct expected_delete_ctor_base { + expected_delete_ctor_base() = default; + expected_delete_ctor_base(const expected_delete_ctor_base &) = delete; + expected_delete_ctor_base(expected_delete_ctor_base &&) noexcept = default; + expected_delete_ctor_base & + operator=(const expected_delete_ctor_base &) = default; + expected_delete_ctor_base & + operator=(expected_delete_ctor_base &&) noexcept = default; +}; + +template +struct expected_delete_ctor_base { + expected_delete_ctor_base() = default; + expected_delete_ctor_base(const expected_delete_ctor_base &) = delete; + expected_delete_ctor_base(expected_delete_ctor_base &&) noexcept = delete; + expected_delete_ctor_base & + operator=(const expected_delete_ctor_base &) = default; + expected_delete_ctor_base & + operator=(expected_delete_ctor_base &&) noexcept = default; +}; + +// expected_delete_assign_base will conditionally delete copy and move +// constructors depending on whether T and E are copy/move constructible + +// assignable +template ::value && + std::is_copy_constructible::value && + is_copy_assignable_or_void::value && + std::is_copy_assignable::value), + bool EnableMove = (is_move_constructible_or_void::value && + std::is_move_constructible::value && + is_move_assignable_or_void::value && + std::is_move_assignable::value)> +struct expected_delete_assign_base { + expected_delete_assign_base() = default; + expected_delete_assign_base(const expected_delete_assign_base &) = default; + expected_delete_assign_base(expected_delete_assign_base &&) noexcept = + default; + expected_delete_assign_base & + operator=(const expected_delete_assign_base &) = default; + expected_delete_assign_base & + operator=(expected_delete_assign_base &&) noexcept = default; +}; + +template +struct expected_delete_assign_base { + expected_delete_assign_base() = default; + expected_delete_assign_base(const expected_delete_assign_base &) = default; + expected_delete_assign_base(expected_delete_assign_base &&) noexcept = + default; + expected_delete_assign_base & + operator=(const expected_delete_assign_base &) = default; + expected_delete_assign_base & + operator=(expected_delete_assign_base &&) noexcept = delete; +}; + +template +struct expected_delete_assign_base { + expected_delete_assign_base() = default; + expected_delete_assign_base(const expected_delete_assign_base &) = default; + expected_delete_assign_base(expected_delete_assign_base &&) noexcept = + default; + expected_delete_assign_base & + operator=(const expected_delete_assign_base &) = delete; + expected_delete_assign_base & + operator=(expected_delete_assign_base &&) noexcept = default; +}; + +template +struct expected_delete_assign_base { + expected_delete_assign_base() = default; + expected_delete_assign_base(const expected_delete_assign_base &) = default; + expected_delete_assign_base(expected_delete_assign_base &&) noexcept = + default; + expected_delete_assign_base & + operator=(const expected_delete_assign_base &) = delete; + expected_delete_assign_base & + operator=(expected_delete_assign_base &&) noexcept = delete; +}; + +// This is needed to be able to construct the expected_default_ctor_base which +// follows, while still conditionally deleting the default constructor. +struct default_constructor_tag { + explicit constexpr default_constructor_tag() = default; +}; + +// expected_default_ctor_base will ensure that expected has a deleted default +// consturctor if T is not default constructible. +// This specialization is for when T is default constructible +template ::value || std::is_void::value> +struct expected_default_ctor_base { + constexpr expected_default_ctor_base() noexcept = default; + constexpr expected_default_ctor_base( + expected_default_ctor_base const &) noexcept = default; + constexpr expected_default_ctor_base(expected_default_ctor_base &&) noexcept = + default; + expected_default_ctor_base & + operator=(expected_default_ctor_base const &) noexcept = default; + expected_default_ctor_base & + operator=(expected_default_ctor_base &&) noexcept = default; + + constexpr explicit expected_default_ctor_base(default_constructor_tag) {} +}; + +// This specialization is for when T is not default constructible +template struct expected_default_ctor_base { + constexpr expected_default_ctor_base() noexcept = delete; + constexpr expected_default_ctor_base( + expected_default_ctor_base const &) noexcept = default; + constexpr expected_default_ctor_base(expected_default_ctor_base &&) noexcept = + default; + expected_default_ctor_base & + operator=(expected_default_ctor_base const &) noexcept = default; + expected_default_ctor_base & + operator=(expected_default_ctor_base &&) noexcept = default; + + constexpr explicit expected_default_ctor_base(default_constructor_tag) {} +}; +} // namespace detail + +template class bad_expected_access : public std::exception { +public: + explicit bad_expected_access(E e) : m_val(std::move(e)) {} + + virtual const char *what() const noexcept override { + return "Bad expected access"; + } + + const E &error() const & { return m_val; } + E &error() & { return m_val; } + const E &&error() const && { return std::move(m_val); } + E &&error() && { return std::move(m_val); } + +private: + E m_val; +}; + +/// An `expected` object is an object that contains the storage for +/// another object and manages the lifetime of this contained object `T`. +/// Alternatively it could contain the storage for another unexpected object +/// `E`. The contained object may not be initialized after the expected object +/// has been initialized, and may not be destroyed before the expected object +/// has been destroyed. The initialization state of the contained object is +/// tracked by the expected object. +template +class expected : private detail::expected_move_assign_base, + private detail::expected_delete_ctor_base, + private detail::expected_delete_assign_base, + private detail::expected_default_ctor_base { + static_assert(!std::is_reference::value, "T must not be a reference"); + static_assert(!std::is_same::type>::value, + "T must not be in_place_t"); + static_assert(!std::is_same::type>::value, + "T must not be unexpect_t"); + static_assert( + !std::is_same>::type>::value, + "T must not be unexpected"); + static_assert(!std::is_reference::value, "E must not be a reference"); + + T *valptr() { return std::addressof(this->m_val); } + const T *valptr() const { return std::addressof(this->m_val); } + unexpected *errptr() { return std::addressof(this->m_unexpect); } + const unexpected *errptr() const { + return std::addressof(this->m_unexpect); + } + + template ::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR U &val() { + return this->m_val; + } + TL_EXPECTED_11_CONSTEXPR unexpected &err() { return this->m_unexpect; } + + template ::value> * = nullptr> + constexpr const U &val() const { + return this->m_val; + } + constexpr const unexpected &err() const { return this->m_unexpect; } + + using impl_base = detail::expected_move_assign_base; + using ctor_base = detail::expected_default_ctor_base; + +public: + typedef T value_type; + typedef E error_type; + typedef unexpected unexpected_type; + +#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ + !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) + template TL_EXPECTED_11_CONSTEXPR auto and_then(F &&f) & { + return and_then_impl(*this, std::forward(f)); + } + template TL_EXPECTED_11_CONSTEXPR auto and_then(F &&f) && { + return and_then_impl(std::move(*this), std::forward(f)); + } + template constexpr auto and_then(F &&f) const & { + return and_then_impl(*this, std::forward(f)); + } + +#ifndef TL_EXPECTED_NO_CONSTRR + template constexpr auto and_then(F &&f) const && { + return and_then_impl(std::move(*this), std::forward(f)); + } +#endif + +#else + template + TL_EXPECTED_11_CONSTEXPR auto + and_then(F &&f) & -> decltype(and_then_impl(std::declval(), + std::forward(f))) { + return and_then_impl(*this, std::forward(f)); + } + template + TL_EXPECTED_11_CONSTEXPR auto + and_then(F &&f) && -> decltype(and_then_impl(std::declval(), + std::forward(f))) { + return and_then_impl(std::move(*this), std::forward(f)); + } + template + constexpr auto and_then(F &&f) const & -> decltype(and_then_impl( + std::declval(), std::forward(f))) { + return and_then_impl(*this, std::forward(f)); + } + +#ifndef TL_EXPECTED_NO_CONSTRR + template + constexpr auto and_then(F &&f) const && -> decltype(and_then_impl( + std::declval(), std::forward(f))) { + return and_then_impl(std::move(*this), std::forward(f)); + } +#endif +#endif + +#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ + !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) + template TL_EXPECTED_11_CONSTEXPR auto map(F &&f) & { + return expected_map_impl(*this, std::forward(f)); + } + template TL_EXPECTED_11_CONSTEXPR auto map(F &&f) && { + return expected_map_impl(std::move(*this), std::forward(f)); + } + template constexpr auto map(F &&f) const & { + return expected_map_impl(*this, std::forward(f)); + } + template constexpr auto map(F &&f) const && { + return expected_map_impl(std::move(*this), std::forward(f)); + } +#else + template + TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl( + std::declval(), std::declval())) + map(F &&f) & { + return expected_map_impl(*this, std::forward(f)); + } + template + TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl(std::declval(), + std::declval())) + map(F &&f) && { + return expected_map_impl(std::move(*this), std::forward(f)); + } + template + constexpr decltype(expected_map_impl(std::declval(), + std::declval())) + map(F &&f) const & { + return expected_map_impl(*this, std::forward(f)); + } + +#ifndef TL_EXPECTED_NO_CONSTRR + template + constexpr decltype(expected_map_impl(std::declval(), + std::declval())) + map(F &&f) const && { + return expected_map_impl(std::move(*this), std::forward(f)); + } +#endif +#endif + +#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ + !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) + template TL_EXPECTED_11_CONSTEXPR auto transform(F &&f) & { + return expected_map_impl(*this, std::forward(f)); + } + template TL_EXPECTED_11_CONSTEXPR auto transform(F &&f) && { + return expected_map_impl(std::move(*this), std::forward(f)); + } + template constexpr auto transform(F &&f) const & { + return expected_map_impl(*this, std::forward(f)); + } + template constexpr auto transform(F &&f) const && { + return expected_map_impl(std::move(*this), std::forward(f)); + } +#else + template + TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl( + std::declval(), std::declval())) + transform(F &&f) & { + return expected_map_impl(*this, std::forward(f)); + } + template + TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl(std::declval(), + std::declval())) + transform(F &&f) && { + return expected_map_impl(std::move(*this), std::forward(f)); + } + template + constexpr decltype(expected_map_impl(std::declval(), + std::declval())) + transform(F &&f) const & { + return expected_map_impl(*this, std::forward(f)); + } + +#ifndef TL_EXPECTED_NO_CONSTRR + template + constexpr decltype(expected_map_impl(std::declval(), + std::declval())) + transform(F &&f) const && { + return expected_map_impl(std::move(*this), std::forward(f)); + } +#endif +#endif + +#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ + !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) + template TL_EXPECTED_11_CONSTEXPR auto map_error(F &&f) & { + return map_error_impl(*this, std::forward(f)); + } + template TL_EXPECTED_11_CONSTEXPR auto map_error(F &&f) && { + return map_error_impl(std::move(*this), std::forward(f)); + } + template constexpr auto map_error(F &&f) const & { + return map_error_impl(*this, std::forward(f)); + } + template constexpr auto map_error(F &&f) const && { + return map_error_impl(std::move(*this), std::forward(f)); + } +#else + template + TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl(std::declval(), + std::declval())) + map_error(F &&f) & { + return map_error_impl(*this, std::forward(f)); + } + template + TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl(std::declval(), + std::declval())) + map_error(F &&f) && { + return map_error_impl(std::move(*this), std::forward(f)); + } + template + constexpr decltype(map_error_impl(std::declval(), + std::declval())) + map_error(F &&f) const & { + return map_error_impl(*this, std::forward(f)); + } + +#ifndef TL_EXPECTED_NO_CONSTRR + template + constexpr decltype(map_error_impl(std::declval(), + std::declval())) + map_error(F &&f) const && { + return map_error_impl(std::move(*this), std::forward(f)); + } +#endif +#endif +#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ + !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) + template TL_EXPECTED_11_CONSTEXPR auto transform_error(F &&f) & { + return map_error_impl(*this, std::forward(f)); + } + template TL_EXPECTED_11_CONSTEXPR auto transform_error(F &&f) && { + return map_error_impl(std::move(*this), std::forward(f)); + } + template constexpr auto transform_error(F &&f) const & { + return map_error_impl(*this, std::forward(f)); + } + template constexpr auto transform_error(F &&f) const && { + return map_error_impl(std::move(*this), std::forward(f)); + } +#else + template + TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl(std::declval(), + std::declval())) + transform_error(F &&f) & { + return map_error_impl(*this, std::forward(f)); + } + template + TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl(std::declval(), + std::declval())) + transform_error(F &&f) && { + return map_error_impl(std::move(*this), std::forward(f)); + } + template + constexpr decltype(map_error_impl(std::declval(), + std::declval())) + transform_error(F &&f) const & { + return map_error_impl(*this, std::forward(f)); + } + +#ifndef TL_EXPECTED_NO_CONSTRR + template + constexpr decltype(map_error_impl(std::declval(), + std::declval())) + transform_error(F &&f) const && { + return map_error_impl(std::move(*this), std::forward(f)); + } +#endif +#endif + template expected TL_EXPECTED_11_CONSTEXPR or_else(F &&f) & { + return or_else_impl(*this, std::forward(f)); + } + + template expected TL_EXPECTED_11_CONSTEXPR or_else(F &&f) && { + return or_else_impl(std::move(*this), std::forward(f)); + } + + template expected constexpr or_else(F &&f) const & { + return or_else_impl(*this, std::forward(f)); + } + +#ifndef TL_EXPECTED_NO_CONSTRR + template expected constexpr or_else(F &&f) const && { + return or_else_impl(std::move(*this), std::forward(f)); + } +#endif + constexpr expected() = default; + constexpr expected(const expected &rhs) = default; + constexpr expected(expected &&rhs) = default; + expected &operator=(const expected &rhs) = default; + expected &operator=(expected &&rhs) = default; + + template ::value> * = + nullptr> + constexpr expected(in_place_t, Args &&...args) + : impl_base(in_place, std::forward(args)...), + ctor_base(detail::default_constructor_tag{}) {} + + template &, Args &&...>::value> * = nullptr> + constexpr expected(in_place_t, std::initializer_list il, Args &&...args) + : impl_base(in_place, il, std::forward(args)...), + ctor_base(detail::default_constructor_tag{}) {} + + template ::value> * = + nullptr, + detail::enable_if_t::value> * = + nullptr> + explicit constexpr expected(const unexpected &e) + : impl_base(unexpect, e.value()), + ctor_base(detail::default_constructor_tag{}) {} + + template < + class G = E, + detail::enable_if_t::value> * = + nullptr, + detail::enable_if_t::value> * = nullptr> + constexpr expected(unexpected const &e) + : impl_base(unexpect, e.value()), + ctor_base(detail::default_constructor_tag{}) {} + + template < + class G = E, + detail::enable_if_t::value> * = nullptr, + detail::enable_if_t::value> * = nullptr> + explicit constexpr expected(unexpected &&e) noexcept( + std::is_nothrow_constructible::value) + : impl_base(unexpect, std::move(e.value())), + ctor_base(detail::default_constructor_tag{}) {} + + template < + class G = E, + detail::enable_if_t::value> * = nullptr, + detail::enable_if_t::value> * = nullptr> + constexpr expected(unexpected &&e) noexcept( + std::is_nothrow_constructible::value) + : impl_base(unexpect, std::move(e.value())), + ctor_base(detail::default_constructor_tag{}) {} + + template ::value> * = + nullptr> + constexpr explicit expected(unexpect_t, Args &&...args) + : impl_base(unexpect, std::forward(args)...), + ctor_base(detail::default_constructor_tag{}) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected(unexpect_t, std::initializer_list il, + Args &&...args) + : impl_base(unexpect, il, std::forward(args)...), + ctor_base(detail::default_constructor_tag{}) {} + + template ::value && + std::is_convertible::value)> * = + nullptr, + detail::expected_enable_from_other + * = nullptr> + explicit TL_EXPECTED_11_CONSTEXPR expected(const expected &rhs) + : ctor_base(detail::default_constructor_tag{}) { + if (rhs.has_value()) { + this->construct(*rhs); + } else { + this->construct_error(rhs.error()); + } + } + + template ::value && + std::is_convertible::value)> * = + nullptr, + detail::expected_enable_from_other + * = nullptr> + TL_EXPECTED_11_CONSTEXPR expected(const expected &rhs) + : ctor_base(detail::default_constructor_tag{}) { + if (rhs.has_value()) { + this->construct(*rhs); + } else { + this->construct_error(rhs.error()); + } + } + + template < + class U, class G, + detail::enable_if_t::value && + std::is_convertible::value)> * = nullptr, + detail::expected_enable_from_other * = nullptr> + explicit TL_EXPECTED_11_CONSTEXPR expected(expected &&rhs) + : ctor_base(detail::default_constructor_tag{}) { + if (rhs.has_value()) { + this->construct(std::move(*rhs)); + } else { + this->construct_error(std::move(rhs.error())); + } + } + + template < + class U, class G, + detail::enable_if_t<(std::is_convertible::value && + std::is_convertible::value)> * = nullptr, + detail::expected_enable_from_other * = nullptr> + TL_EXPECTED_11_CONSTEXPR expected(expected &&rhs) + : ctor_base(detail::default_constructor_tag{}) { + if (rhs.has_value()) { + this->construct(std::move(*rhs)); + } else { + this->construct_error(std::move(rhs.error())); + } + } + + template < + class U = T, + detail::enable_if_t::value> * = nullptr, + detail::expected_enable_forward_value * = nullptr> + explicit TL_EXPECTED_MSVC2015_CONSTEXPR expected(U &&v) + : expected(in_place, std::forward(v)) {} + + template < + class U = T, + detail::enable_if_t::value> * = nullptr, + detail::expected_enable_forward_value * = nullptr> + TL_EXPECTED_MSVC2015_CONSTEXPR expected(U &&v) + : expected(in_place, std::forward(v)) {} + + template < + class U = T, class G = T, + detail::enable_if_t::value> * = + nullptr, + detail::enable_if_t::value> * = nullptr, + detail::enable_if_t< + (!std::is_same, detail::decay_t>::value && + !detail::conjunction, + std::is_same>>::value && + std::is_constructible::value && + std::is_assignable::value && + std::is_nothrow_move_constructible::value)> * = nullptr> + expected &operator=(U &&v) { + if (has_value()) { + val() = std::forward(v); + } else { + err().~unexpected(); + ::new (valptr()) T(std::forward(v)); + this->m_has_val = true; + } + + return *this; + } + + template < + class U = T, class G = T, + detail::enable_if_t::value> * = + nullptr, + detail::enable_if_t::value> * = nullptr, + detail::enable_if_t< + (!std::is_same, detail::decay_t>::value && + !detail::conjunction, + std::is_same>>::value && + std::is_constructible::value && + std::is_assignable::value && + std::is_nothrow_move_constructible::value)> * = nullptr> + expected &operator=(U &&v) { + if (has_value()) { + val() = std::forward(v); + } else { + auto tmp = std::move(err()); + err().~unexpected(); + +#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED + try { + ::new (valptr()) T(std::forward(v)); + this->m_has_val = true; + } catch (...) { + err() = std::move(tmp); + throw; + } +#else + ::new (valptr()) T(std::forward(v)); + this->m_has_val = true; +#endif + } + + return *this; + } + + template ::value && + std::is_assignable::value> * = nullptr> + expected &operator=(const unexpected &rhs) { + if (!has_value()) { + err() = rhs; + } else { + this->destroy_val(); + ::new (errptr()) unexpected(rhs); + this->m_has_val = false; + } + + return *this; + } + + template ::value && + std::is_move_assignable::value> * = nullptr> + expected &operator=(unexpected &&rhs) noexcept { + if (!has_value()) { + err() = std::move(rhs); + } else { + this->destroy_val(); + ::new (errptr()) unexpected(std::move(rhs)); + this->m_has_val = false; + } + + return *this; + } + + template ::value> * = nullptr> + void emplace(Args &&...args) { + if (has_value()) { + val().~T(); + } else { + err().~unexpected(); + this->m_has_val = true; + } + ::new (valptr()) T(std::forward(args)...); + } + + template ::value> * = nullptr> + void emplace(Args &&...args) { + if (has_value()) { + val().~T(); + ::new (valptr()) T(std::forward(args)...); + } else { + auto tmp = std::move(err()); + err().~unexpected(); + +#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED + try { + ::new (valptr()) T(std::forward(args)...); + this->m_has_val = true; + } catch (...) { + err() = std::move(tmp); + throw; + } +#else + ::new (valptr()) T(std::forward(args)...); + this->m_has_val = true; +#endif + } + } + + template &, Args &&...>::value> * = nullptr> + void emplace(std::initializer_list il, Args &&...args) { + if (has_value()) { + T t(il, std::forward(args)...); + val() = std::move(t); + } else { + err().~unexpected(); + ::new (valptr()) T(il, std::forward(args)...); + this->m_has_val = true; + } + } + + template &, Args &&...>::value> * = nullptr> + void emplace(std::initializer_list il, Args &&...args) { + if (has_value()) { + T t(il, std::forward(args)...); + val() = std::move(t); + } else { + auto tmp = std::move(err()); + err().~unexpected(); + +#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED + try { + ::new (valptr()) T(il, std::forward(args)...); + this->m_has_val = true; + } catch (...) { + err() = std::move(tmp); + throw; + } +#else + ::new (valptr()) T(il, std::forward(args)...); + this->m_has_val = true; +#endif + } + } + +private: + using t_is_void = std::true_type; + using t_is_not_void = std::false_type; + using t_is_nothrow_move_constructible = std::true_type; + using move_constructing_t_can_throw = std::false_type; + using e_is_nothrow_move_constructible = std::true_type; + using move_constructing_e_can_throw = std::false_type; + + void swap_where_both_have_value(expected & /*rhs*/, t_is_void) noexcept { + // swapping void is a no-op + } + + void swap_where_both_have_value(expected &rhs, t_is_not_void) { + using std::swap; + swap(val(), rhs.val()); + } + + void swap_where_only_one_has_value(expected &rhs, t_is_void) noexcept( + std::is_nothrow_move_constructible::value) { + ::new (errptr()) unexpected_type(std::move(rhs.err())); + rhs.err().~unexpected_type(); + std::swap(this->m_has_val, rhs.m_has_val); + } + + void swap_where_only_one_has_value(expected &rhs, t_is_not_void) { + swap_where_only_one_has_value_and_t_is_not_void( + rhs, typename std::is_nothrow_move_constructible::type{}, + typename std::is_nothrow_move_constructible::type{}); + } + + void swap_where_only_one_has_value_and_t_is_not_void( + expected &rhs, t_is_nothrow_move_constructible, + e_is_nothrow_move_constructible) noexcept { + auto temp = std::move(val()); + val().~T(); + ::new (errptr()) unexpected_type(std::move(rhs.err())); + rhs.err().~unexpected_type(); + ::new (rhs.valptr()) T(std::move(temp)); + std::swap(this->m_has_val, rhs.m_has_val); + } + + void swap_where_only_one_has_value_and_t_is_not_void( + expected &rhs, t_is_nothrow_move_constructible, + move_constructing_e_can_throw) { + auto temp = std::move(val()); + val().~T(); +#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED + try { + ::new (errptr()) unexpected_type(std::move(rhs.err())); + rhs.err().~unexpected_type(); + ::new (rhs.valptr()) T(std::move(temp)); + std::swap(this->m_has_val, rhs.m_has_val); + } catch (...) { + val() = std::move(temp); + throw; + } +#else + ::new (errptr()) unexpected_type(std::move(rhs.err())); + rhs.err().~unexpected_type(); + ::new (rhs.valptr()) T(std::move(temp)); + std::swap(this->m_has_val, rhs.m_has_val); +#endif + } + + void swap_where_only_one_has_value_and_t_is_not_void( + expected &rhs, move_constructing_t_can_throw, + e_is_nothrow_move_constructible) { + auto temp = std::move(rhs.err()); + rhs.err().~unexpected_type(); +#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED + try { + ::new (rhs.valptr()) T(std::move(val())); + val().~T(); + ::new (errptr()) unexpected_type(std::move(temp)); + std::swap(this->m_has_val, rhs.m_has_val); + } catch (...) { + rhs.err() = std::move(temp); + throw; + } +#else + ::new (rhs.valptr()) T(std::move(val())); + val().~T(); + ::new (errptr()) unexpected_type(std::move(temp)); + std::swap(this->m_has_val, rhs.m_has_val); +#endif + } + +public: + template + detail::enable_if_t::value && + detail::is_swappable::value && + (std::is_nothrow_move_constructible::value || + std::is_nothrow_move_constructible::value)> + swap(expected &rhs) noexcept( + std::is_nothrow_move_constructible::value + &&detail::is_nothrow_swappable::value + &&std::is_nothrow_move_constructible::value + &&detail::is_nothrow_swappable::value) { + if (has_value() && rhs.has_value()) { + swap_where_both_have_value(rhs, typename std::is_void::type{}); + } else if (!has_value() && rhs.has_value()) { + rhs.swap(*this); + } else if (has_value()) { + swap_where_only_one_has_value(rhs, typename std::is_void::type{}); + } else { + using std::swap; + swap(err(), rhs.err()); + } + } + + constexpr const T *operator->() const { + TL_ASSERT(has_value()); + return valptr(); + } + TL_EXPECTED_11_CONSTEXPR T *operator->() { + TL_ASSERT(has_value()); + return valptr(); + } + + template ::value> * = nullptr> + constexpr const U &operator*() const & { + TL_ASSERT(has_value()); + return val(); + } + template ::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR U &operator*() & { + TL_ASSERT(has_value()); + return val(); + } + template ::value> * = nullptr> + constexpr const U &&operator*() const && { + TL_ASSERT(has_value()); + return std::move(val()); + } + template ::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR U &&operator*() && { + TL_ASSERT(has_value()); + return std::move(val()); + } + + constexpr bool has_value() const noexcept { return this->m_has_val; } + constexpr explicit operator bool() const noexcept { return this->m_has_val; } + + template ::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR const U &value() const & { + if (!has_value()) + detail::throw_exception(bad_expected_access(err().value())); + return val(); + } + template ::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR U &value() & { + if (!has_value()) + detail::throw_exception(bad_expected_access(err().value())); + return val(); + } + template ::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR const U &&value() const && { + if (!has_value()) + detail::throw_exception(bad_expected_access(std::move(err()).value())); + return std::move(val()); + } + template ::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR U &&value() && { + if (!has_value()) + detail::throw_exception(bad_expected_access(std::move(err()).value())); + return std::move(val()); + } + + constexpr const E &error() const & { + TL_ASSERT(!has_value()); + return err().value(); + } + TL_EXPECTED_11_CONSTEXPR E &error() & { + TL_ASSERT(!has_value()); + return err().value(); + } + constexpr const E &&error() const && { + TL_ASSERT(!has_value()); + return std::move(err().value()); + } + TL_EXPECTED_11_CONSTEXPR E &&error() && { + TL_ASSERT(!has_value()); + return std::move(err().value()); + } + + template constexpr T value_or(U &&v) const & { + static_assert(std::is_copy_constructible::value && + std::is_convertible::value, + "T must be copy-constructible and convertible to from U&&"); + return bool(*this) ? **this : static_cast(std::forward(v)); + } + template TL_EXPECTED_11_CONSTEXPR T value_or(U &&v) && { + static_assert(std::is_move_constructible::value && + std::is_convertible::value, + "T must be move-constructible and convertible to from U&&"); + return bool(*this) ? std::move(**this) : static_cast(std::forward(v)); + } +}; + +namespace detail { +template using exp_t = typename detail::decay_t::value_type; +template using err_t = typename detail::decay_t::error_type; +template using ret_t = expected>; + +#ifdef TL_EXPECTED_CXX14 +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval(), + *std::declval()))> +constexpr auto and_then_impl(Exp &&exp, F &&f) { + static_assert(detail::is_expected::value, "F must return an expected"); + + return exp.has_value() + ? detail::invoke(std::forward(f), *std::forward(exp)) + : Ret(unexpect, std::forward(exp).error()); +} + +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval()))> +constexpr auto and_then_impl(Exp &&exp, F &&f) { + static_assert(detail::is_expected::value, "F must return an expected"); + + return exp.has_value() ? detail::invoke(std::forward(f)) + : Ret(unexpect, std::forward(exp).error()); +} +#else +template struct TC; +template (), + *std::declval())), + detail::enable_if_t>::value> * = nullptr> +auto and_then_impl(Exp &&exp, F &&f) -> Ret { + static_assert(detail::is_expected::value, "F must return an expected"); + + return exp.has_value() + ? detail::invoke(std::forward(f), *std::forward(exp)) + : Ret(unexpect, std::forward(exp).error()); +} + +template ())), + detail::enable_if_t>::value> * = nullptr> +constexpr auto and_then_impl(Exp &&exp, F &&f) -> Ret { + static_assert(detail::is_expected::value, "F must return an expected"); + + return exp.has_value() ? detail::invoke(std::forward(f)) + : Ret(unexpect, std::forward(exp).error()); +} +#endif + +#ifdef TL_EXPECTED_CXX14 +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval(), + *std::declval())), + detail::enable_if_t::value> * = nullptr> +constexpr auto expected_map_impl(Exp &&exp, F &&f) { + using result = ret_t>; + return exp.has_value() ? result(detail::invoke(std::forward(f), + *std::forward(exp))) + : result(unexpect, std::forward(exp).error()); +} + +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval(), + *std::declval())), + detail::enable_if_t::value> * = nullptr> +auto expected_map_impl(Exp &&exp, F &&f) { + using result = expected>; + if (exp.has_value()) { + detail::invoke(std::forward(f), *std::forward(exp)); + return result(); + } + + return result(unexpect, std::forward(exp).error()); +} + +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval())), + detail::enable_if_t::value> * = nullptr> +constexpr auto expected_map_impl(Exp &&exp, F &&f) { + using result = ret_t>; + return exp.has_value() ? result(detail::invoke(std::forward(f))) + : result(unexpect, std::forward(exp).error()); +} + +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval())), + detail::enable_if_t::value> * = nullptr> +auto expected_map_impl(Exp &&exp, F &&f) { + using result = expected>; + if (exp.has_value()) { + detail::invoke(std::forward(f)); + return result(); + } + + return result(unexpect, std::forward(exp).error()); +} +#else +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval(), + *std::declval())), + detail::enable_if_t::value> * = nullptr> + +constexpr auto expected_map_impl(Exp &&exp, F &&f) + -> ret_t> { + using result = ret_t>; + + return exp.has_value() ? result(detail::invoke(std::forward(f), + *std::forward(exp))) + : result(unexpect, std::forward(exp).error()); +} + +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval(), + *std::declval())), + detail::enable_if_t::value> * = nullptr> + +auto expected_map_impl(Exp &&exp, F &&f) -> expected> { + if (exp.has_value()) { + detail::invoke(std::forward(f), *std::forward(exp)); + return {}; + } + + return unexpected>(std::forward(exp).error()); +} + +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval())), + detail::enable_if_t::value> * = nullptr> + +constexpr auto expected_map_impl(Exp &&exp, F &&f) + -> ret_t> { + using result = ret_t>; + + return exp.has_value() ? result(detail::invoke(std::forward(f))) + : result(unexpect, std::forward(exp).error()); +} + +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval())), + detail::enable_if_t::value> * = nullptr> + +auto expected_map_impl(Exp &&exp, F &&f) -> expected> { + if (exp.has_value()) { + detail::invoke(std::forward(f)); + return {}; + } + + return unexpected>(std::forward(exp).error()); +} +#endif + +#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ + !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +constexpr auto map_error_impl(Exp &&exp, F &&f) { + using result = expected, detail::decay_t>; + return exp.has_value() + ? result(*std::forward(exp)) + : result(unexpect, detail::invoke(std::forward(f), + std::forward(exp).error())); +} +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +auto map_error_impl(Exp &&exp, F &&f) { + using result = expected, monostate>; + if (exp.has_value()) { + return result(*std::forward(exp)); + } + + detail::invoke(std::forward(f), std::forward(exp).error()); + return result(unexpect, monostate{}); +} +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +constexpr auto map_error_impl(Exp &&exp, F &&f) { + using result = expected, detail::decay_t>; + return exp.has_value() + ? result() + : result(unexpect, detail::invoke(std::forward(f), + std::forward(exp).error())); +} +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +auto map_error_impl(Exp &&exp, F &&f) { + using result = expected, monostate>; + if (exp.has_value()) { + return result(); + } + + detail::invoke(std::forward(f), std::forward(exp).error()); + return result(unexpect, monostate{}); +} +#else +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +constexpr auto map_error_impl(Exp &&exp, F &&f) + -> expected, detail::decay_t> { + using result = expected, detail::decay_t>; + + return exp.has_value() + ? result(*std::forward(exp)) + : result(unexpect, detail::invoke(std::forward(f), + std::forward(exp).error())); +} + +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +auto map_error_impl(Exp &&exp, F &&f) -> expected, monostate> { + using result = expected, monostate>; + if (exp.has_value()) { + return result(*std::forward(exp)); + } + + detail::invoke(std::forward(f), std::forward(exp).error()); + return result(unexpect, monostate{}); +} + +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +constexpr auto map_error_impl(Exp &&exp, F &&f) + -> expected, detail::decay_t> { + using result = expected, detail::decay_t>; + + return exp.has_value() + ? result() + : result(unexpect, detail::invoke(std::forward(f), + std::forward(exp).error())); +} + +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +auto map_error_impl(Exp &&exp, F &&f) -> expected, monostate> { + using result = expected, monostate>; + if (exp.has_value()) { + return result(); + } + + detail::invoke(std::forward(f), std::forward(exp).error()); + return result(unexpect, monostate{}); +} +#endif + +#ifdef TL_EXPECTED_CXX14 +template (), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +constexpr auto or_else_impl(Exp &&exp, F &&f) { + static_assert(detail::is_expected::value, "F must return an expected"); + return exp.has_value() ? std::forward(exp) + : detail::invoke(std::forward(f), + std::forward(exp).error()); +} + +template (), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +detail::decay_t or_else_impl(Exp &&exp, F &&f) { + return exp.has_value() ? std::forward(exp) + : (detail::invoke(std::forward(f), + std::forward(exp).error()), + std::forward(exp)); +} +#else +template (), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +auto or_else_impl(Exp &&exp, F &&f) -> Ret { + static_assert(detail::is_expected::value, "F must return an expected"); + return exp.has_value() ? std::forward(exp) + : detail::invoke(std::forward(f), + std::forward(exp).error()); +} + +template (), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +detail::decay_t or_else_impl(Exp &&exp, F &&f) { + return exp.has_value() ? std::forward(exp) + : (detail::invoke(std::forward(f), + std::forward(exp).error()), + std::forward(exp)); +} +#endif +} // namespace detail + +template +constexpr bool operator==(const expected &lhs, + const expected &rhs) { + return (lhs.has_value() != rhs.has_value()) + ? false + : (!lhs.has_value() ? lhs.error() == rhs.error() : *lhs == *rhs); +} +template +constexpr bool operator!=(const expected &lhs, + const expected &rhs) { + return (lhs.has_value() != rhs.has_value()) + ? true + : (!lhs.has_value() ? lhs.error() != rhs.error() : *lhs != *rhs); +} +template +constexpr bool operator==(const expected &lhs, + const expected &rhs) { + return (lhs.has_value() != rhs.has_value()) + ? false + : (!lhs.has_value() ? lhs.error() == rhs.error() : true); +} +template +constexpr bool operator!=(const expected &lhs, + const expected &rhs) { + return (lhs.has_value() != rhs.has_value()) + ? true + : (!lhs.has_value() ? lhs.error() == rhs.error() : false); +} + +template +constexpr bool operator==(const expected &x, const U &v) { + return x.has_value() ? *x == v : false; +} +template +constexpr bool operator==(const U &v, const expected &x) { + return x.has_value() ? *x == v : false; +} +template +constexpr bool operator!=(const expected &x, const U &v) { + return x.has_value() ? *x != v : true; +} +template +constexpr bool operator!=(const U &v, const expected &x) { + return x.has_value() ? *x != v : true; +} + +template +constexpr bool operator==(const expected &x, const unexpected &e) { + return x.has_value() ? false : x.error() == e.value(); +} +template +constexpr bool operator==(const unexpected &e, const expected &x) { + return x.has_value() ? false : x.error() == e.value(); +} +template +constexpr bool operator!=(const expected &x, const unexpected &e) { + return x.has_value() ? true : x.error() != e.value(); +} +template +constexpr bool operator!=(const unexpected &e, const expected &x) { + return x.has_value() ? true : x.error() != e.value(); +} + +template ::value || + std::is_move_constructible::value) && + detail::is_swappable::value && + std::is_move_constructible::value && + detail::is_swappable::value> * = nullptr> +void swap(expected &lhs, + expected &rhs) noexcept(noexcept(lhs.swap(rhs))) { + lhs.swap(rhs); +} +} // namespace tl + +#endif diff --git a/src/raw_subscriber.cpp b/src/raw_subscriber.cpp index e5a5809..4b70c49 100644 --- a/src/raw_subscriber.cpp +++ b/src/raw_subscriber.cpp @@ -57,7 +57,7 @@ std::string RawSubscriber::getTopicToSubscribe(const std::string& base_topic) co return base_topic; } -void RawSubscriber::internalCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& message, const SubscriberPlugin::Callback& user_cb) +void RawSubscriber::callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& message, const SubscriberPlugin::Callback& user_cb) { user_cb(message); } From 663feb1f13e0a683019ff06c4f7de00afee30a24 Mon Sep 17 00:00:00 2001 From: john-maidbot Date: Mon, 19 Jun 2023 12:28:33 -0500 Subject: [PATCH 07/28] progress --- .../simple_publisher_plugin.hpp | 17 +++++++++++++---- nodelets.xml | 7 ------- 2 files changed, 13 insertions(+), 11 deletions(-) delete mode 100644 nodelets.xml diff --git a/include/point_cloud_transport/simple_publisher_plugin.hpp b/include/point_cloud_transport/simple_publisher_plugin.hpp index c0f2685..48122f4 100644 --- a/include/point_cloud_transport/simple_publisher_plugin.hpp +++ b/include/point_cloud_transport/simple_publisher_plugin.hpp @@ -175,16 +175,25 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin typedef std::function PublishFn; /** - * \brief Publish an pointcloud using the specified publish function. Must be implemented by + * Publish a point cloud using the specified publish function. Must be implemented by * the subclass. * * The PublishFn publishes the transport-specific message type. This indirection allows * SimpleSubscriberPlugin to use this function for both normal broadcast publishing and * single subscriber publishing (in subscription callbacks). */ - virtual void publish( - const sensor_msgs::msg::PointCloud2 & message, - const PublishFn & publish_fn) const = 0; + virtual void publish(const sensor_msgs::msg::PointCloud2& message, const PublishFn& publish_fn) const + { + const auto res = this->encodeTyped(message); + if (!res) + { + RCLCPP_ERROR(rclcpp::get_logger("point_cloud_transport"), "Error encoding message by transport %s: %s.", this->getTransportName().c_str(), res.error().c_str()); + } + else if (res.value()) + { + publish_fn(res.value().value()); + } + } /** * \brief Return the communication topic name for a given base topic. diff --git a/nodelets.xml b/nodelets.xml deleted file mode 100644 index d99a447..0000000 --- a/nodelets.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - Nodelet for republishing a point cloud from one transport to other transport(s). - - - From 6d551a190dfce7fdf91da73344e2ea71c3840082 Mon Sep 17 00:00:00 2001 From: john-maidbot Date: Mon, 19 Jun 2023 12:33:29 -0500 Subject: [PATCH 08/28] Update TODO's --- PORT_TODO.md | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/PORT_TODO.md b/PORT_TODO.md index 5fa9305..d7c61bc 100644 --- a/PORT_TODO.md +++ b/PORT_TODO.md @@ -1,12 +1,18 @@ ## TODO -- [] roscpp --> rclcpp conversion (IN PROGRESS) ---> CRAS cmake file ---> CRAS topic_tools replacement (ros2_introspection?) ---> Put CRAS stuff back into repo and get it to build - +- [] start roscpp --> rclcpp conversion of transport +--> Put CRAS into this repo temporarily to get it to build +--> Do what it takes to get the rclcpp code to build without the point_cloud_codec files +--> Figure out how to update ros serialization and shape shifter usage +--> Fix the point_cloud_codec files - [] c++ codebase builds with cmake (minus tests) -- [] c++ codebase runs with test pointclouds +--> Create a node to test with simulated pointclouds +--> bag the pointcloud data +--> Test point_cloud_codec files with bagged point cloud data +- [] c++ codebase runs with test pointcloudsc +--> Fix c++ tests - [] tests build and pass - [] rospy --> rclpy conversion +--> Update python rclpy files +--> Update python bindings - [] python codebase runs with test pointclouds From 4f878c2cf833bc28d6fd4420fbff0cc2848f2a42 Mon Sep 17 00:00:00 2001 From: john-maidbot Date: Mon, 19 Jun 2023 13:42:21 -0500 Subject: [PATCH 09/28] Progress --- .vscode/settings.json | 7 +- CMakeLists.txt | 5 +- default_plugins.xml | 2 +- .../point_cloud_transport.hpp | 6 +- include/point_cloud_transport/republish.hpp | 41 ----------- .../point_cloud_transport/transport_hints.hpp | 5 ++ src/list_transports.cpp | 9 ++- src/republish.cpp | 69 ++++++++++--------- 8 files changed, 58 insertions(+), 86 deletions(-) delete mode 100644 include/point_cloud_transport/republish.hpp diff --git a/.vscode/settings.json b/.vscode/settings.json index a695773..4ad68de 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,8 +1,6 @@ { "files.associations": { - "memory": "cpp", - "*.ipp": "cpp", - "system_error": "cpp", + "functional": "cpp", "cctype": "cpp", "clocale": "cpp", "cmath": "cpp", @@ -46,8 +44,8 @@ "vector": "cpp", "exception": "cpp", "algorithm": "cpp", - "functional": "cpp", "iterator": "cpp", + "memory": "cpp", "memory_resource": "cpp", "numeric": "cpp", "optional": "cpp", @@ -56,6 +54,7 @@ "regex": "cpp", "source_location": "cpp", "string_view": "cpp", + "system_error": "cpp", "tuple": "cpp", "type_traits": "cpp", "utility": "cpp", diff --git a/CMakeLists.txt b/CMakeLists.txt index 642d23b..32c2744 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -47,13 +47,14 @@ target_link_libraries(${PROJECT_NAME}_plugins PRIVATE ${PROJECT_NAME} pluginlib::pluginlib) -# Build republish (TODO: there was some nodelet stuff happening here) +# TODO: Fix build errors +# TODO: Make this a composition # add_executable(republish src/republish.cpp) # target_link_libraries(republish # ${PROJECT_NAME} # pluginlib::pluginlib) -# Build list_transports (TODO: there was some nodelet stuff happening here) +# Build list_transports add_executable(list_transports src/list_transports.cpp) target_link_libraries(list_transports ${PROJECT_NAME} diff --git a/default_plugins.xml b/default_plugins.xml index e9a8270..8cf7b80 100644 --- a/default_plugins.xml +++ b/default_plugins.xml @@ -1,4 +1,4 @@ - + This is the default publisher. It publishes the PointCloud2 as-is on the base topic. diff --git a/include/point_cloud_transport/point_cloud_transport.hpp b/include/point_cloud_transport/point_cloud_transport.hpp index 92eade0..aa8cc73 100644 --- a/include/point_cloud_transport/point_cloud_transport.hpp +++ b/include/point_cloud_transport/point_cloud_transport.hpp @@ -139,9 +139,9 @@ class PointCloudTransport : public PointCloudTransportLoader //! Subscribe to a point cloud topic, version for arbitrary std::function object. point_cloud_transport::Subscriber subscribe( - const std::string &base_topic, uint32_t queue_size, - const std::function &callback, - const VoidPtr &tracked_object = {}, + const std::string& base_topic, uint32_t queue_size, + const std::function& callback, + const VoidPtr& tracked_object = {}, const point_cloud_transport::TransportHints *transport_hints = nullptr) { rmw_qos_profile_t custom_qos = rmw_qos_profile_default; diff --git a/include/point_cloud_transport/republish.hpp b/include/point_cloud_transport/republish.hpp deleted file mode 100644 index bfbcf02..0000000 --- a/include/point_cloud_transport/republish.hpp +++ /dev/null @@ -1,41 +0,0 @@ -#pragma once - -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague - -/** - * \file - * \brief A node(let) for republishing clouds from one transport to other transports. - * \author Martin Pecka - */ - -#include - -#include -#include -#include -#include - - -namespace point_cloud_transport -{ - -//! A Node (component) for republishing clouds from one transport to other transports -//! -//! Usage: republish in_transport in:= [out_transport] out:= -//! -//! Parameters: -//! - `~in_queue_size` (int, default 10): Input queue size. -//! - `~out_queue_size` (int, default `in_queue_size`): Output queue size. -class RepublishComponent -{ -protected: - void onInit() override; - - std::unique_ptr pct; - point_cloud_transport::Subscriber sub; - std::shared_ptr pub; - std::shared_ptr pubPlugin; -}; - -} diff --git a/include/point_cloud_transport/transport_hints.hpp b/include/point_cloud_transport/transport_hints.hpp index 83eaefa..545c038 100644 --- a/include/point_cloud_transport/transport_hints.hpp +++ b/include/point_cloud_transport/transport_hints.hpp @@ -72,6 +72,11 @@ class TransportHints node->get_parameter_or(parameter_name, transport_, default_transport); } + TransportHints( + const std::string & transport = "raw") : transport_(transport) + { + } + const std::string& getTransport() const { return transport_; diff --git a/src/list_transports.cpp b/src/list_transports.cpp index 5e00721..c07fcb0 100644 --- a/src/list_transports.cpp +++ b/src/list_transports.cpp @@ -76,21 +76,24 @@ int main(int argc, char** argv) for (const auto& lookup_name : pub_loader.getDeclaredClasses()) { + printf("Declared class: %s\n", lookup_name.c_str()); std::string transport_name = point_cloud_transport::erase_last_copy(lookup_name, "_pub"); transports[transport_name].pub_name = lookup_name; transports[transport_name].package_name = pub_loader.getClassPackage(lookup_name); try { - auto pub = pub_loader.createSharedInstance(lookup_name); + auto pub = pub_loader.createUniqueInstance(lookup_name); transports[transport_name].pub_status = SUCCESS; } catch (const pluginlib::LibraryLoadException& e) { transports[transport_name].pub_status = LIB_LOAD_FAILURE; + printf("LibraryLoadException: %s\n", e.what()); } catch (const pluginlib::CreateClassException& e) { transports[transport_name].pub_status = CREATE_FAILURE; + printf("CreateClassException: %s\n", e.what()); } } @@ -101,16 +104,18 @@ int main(int argc, char** argv) transports[transport_name].package_name = sub_loader.getClassPackage(lookup_name); try { - auto sub = sub_loader.createSharedInstance(lookup_name); + auto sub = sub_loader.createUniqueInstance(lookup_name); transports[transport_name].sub_status = SUCCESS; } catch (const pluginlib::LibraryLoadException& e) { transports[transport_name].sub_status = LIB_LOAD_FAILURE; + printf("LibraryLoadException: %s\n", e.what()); } catch (const pluginlib::CreateClassException& e) { transports[transport_name].sub_status = CREATE_FAILURE; + printf("CreateClassException: %s\n", e.what()); } } diff --git a/src/republish.cpp b/src/republish.cpp index e55f6cd..8879750 100644 --- a/src/republish.cpp +++ b/src/republish.cpp @@ -42,82 +42,85 @@ #include #include +#include "rclcpp/rclcpp.hpp" + #include #include #include #include #include -#include #include #include -namespace point_cloud_transport -{ - // TODO: Fix this -void RepublishComponent::onInit() +int main(int argc, char **argv) { auto vargv = rclcpp::init_and_remove_ros_arguments(argc, argv); - if (vargv.size() < 2) { + if (vargv.size() < 2) + { printf( - "Usage: %s in_transport in:= [out_transport] out:=\n", - argv[0]); + "Usage: %s in_transport in:= [out_transport] out:=\n", + argv[0]); return 0; } auto node = rclcpp::Node::make_shared("point_cloud_republisher"); std::string in_topic = rclcpp::expand_topic_or_service_name( - "in", - node->get_name(), node->get_namespace()); + "in", + node->get_name(), node->get_namespace()); std::string out_topic = rclcpp::expand_topic_or_service_name( - "out", - node->get_name(), node->get_namespace()); + "out", + node->get_name(), node->get_namespace()); std::string in_transport = vargv[1]; - this->pct = std::make_unique(this->getMTNodeHandle()); - point_cloud_transport::TransportHints hints(in_transport, {}, this->getPrivateNodeHandle()); + point_cloud_transport::PointCloudTransport pct(node); - // There might be exceptions thrown by the loaders. We want the propagate so that the nodelet loading fails. - - if (this->getMyArgv().size() < 2) + if (vargv.size() < 3) { // Use all available transports for output - this->pub.reset(new point_cloud_transport::Publisher); - *this->pub = this->pct->advertise(out_topic, out_queue_size); + const auto pub = std::make_shared(pct.advertise(out_topic, rmw_qos_profile_default)); // Use Publisher::publish as the subscriber callback typedef void (point_cloud_transport::Publisher::*PublishMemFn)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr&) const; PublishMemFn pub_mem_fn = &point_cloud_transport::Publisher::publish; - this->sub = this->pct->subscribe(in_topic, in_queue_size, std::bind(pub_mem_fn, this->pub.get(), std::placeholders::_1), this->pub, - hints); + const point_cloud_transport::TransportHints hint(in_transport); + auto sub = pct.subscribe( + in_topic, static_cast(1), + pub_mem_fn, pub, &hint); + // spin the node + rclcpp::spin(node); } else { // Use one specific transport for output - std::string out_transport = this->getMyArgv()[1]; + std::string out_transport = vargv[2]; // Load transport plugin typedef point_cloud_transport::PublisherPlugin Plugin; - auto loader = this->pct->getPublisherLoader(); + auto loader = pct.getPublisherLoader(); std::string lookup_name = Plugin::getLookupName(out_transport); - this->pubPlugin = loader->createUniqueInstance(lookup_name); - this->pubPlugin->advertise(this->getMTNodeHandle(), out_topic, out_queue_size, {}, {}, this->pubPlugin, false); + auto instance = loader->createUniqueInstance(lookup_name); + // DO NOT use instance after this line + const std::shared_ptr pub = std::move(instance); + pub->advertise(node.get(), out_topic); // Use PublisherPlugin::publish as the subscriber callback - typedef void (Plugin::*PublishMemFn)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr&) const; - PublishMemFn pub_mem_fn = &Plugin::publish; - - this->sub = this->pct->subscribe(in_topic, in_queue_size, std::bind(pub_mem_fn, this->pubPlugin.get(), std::placeholders::_1), - this->pubPlugin, in_transport); + typedef void (point_cloud_transport::PublisherPlugin::*PublishMemFn)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr&) const; + PublishMemFn pub_mem_fn = &point_cloud_transport::PublisherPlugin::publish; + + const point_cloud_transport::TransportHints hint(in_transport); + auto sub = pct.subscribe( + in_topic, static_cast(1), + pub_mem_fn, pub, &hint); + // spin the node + rclcpp::spin(node); } -} + return 0; } - -PLUGINLIB_EXPORT_CLASS(point_cloud_transport::RepublishNodelet, nodelet::Nodelet) From 55a86b04f5a0ef88ba88b1d6f08f7ffa893420d8 Mon Sep 17 00:00:00 2001 From: john-maidbot Date: Mon, 19 Jun 2023 13:51:01 -0500 Subject: [PATCH 10/28] More build error fixing --- CMakeLists.txt | 9 ++++----- include/point_cloud_transport/point_cloud_transport.hpp | 4 ++-- src/republish.cpp | 4 ++-- 3 files changed, 8 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 32c2744..d344032 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -47,12 +47,11 @@ target_link_libraries(${PROJECT_NAME}_plugins PRIVATE ${PROJECT_NAME} pluginlib::pluginlib) -# TODO: Fix build errors # TODO: Make this a composition -# add_executable(republish src/republish.cpp) -# target_link_libraries(republish -# ${PROJECT_NAME} -# pluginlib::pluginlib) +add_executable(republish src/republish.cpp) +target_link_libraries(republish + ${PROJECT_NAME} + pluginlib::pluginlib) # Build list_transports add_executable(list_transports src/list_transports.cpp) diff --git a/include/point_cloud_transport/point_cloud_transport.hpp b/include/point_cloud_transport/point_cloud_transport.hpp index aa8cc73..9cd54b9 100644 --- a/include/point_cloud_transport/point_cloud_transport.hpp +++ b/include/point_cloud_transport/point_cloud_transport.hpp @@ -163,7 +163,7 @@ class PointCloudTransport : public PointCloudTransportLoader //! Subscribe to a point cloud topic, version for class member function with bare pointer. template point_cloud_transport::Subscriber subscribe(const std::string& base_topic, uint32_t queue_size, - void(T::*fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr&), T* obj, + void(T::*fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr&) const, T* obj, const point_cloud_transport::TransportHints* transport_hints = nullptr, bool allow_concurrent_callbacks = false) { @@ -173,7 +173,7 @@ class PointCloudTransport : public PointCloudTransportLoader //! Subscribe to a point cloud topic, version for class member function with shared_ptr. template point_cloud_transport::Subscriber subscribe(const std::string& base_topic, uint32_t queue_size, - void(T::*fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr&), + void(T::*fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr&) const, const std::shared_ptr& obj, const point_cloud_transport::TransportHints* transport_hints = nullptr) { diff --git a/src/republish.cpp b/src/republish.cpp index 8879750..8357a35 100644 --- a/src/republish.cpp +++ b/src/republish.cpp @@ -82,7 +82,7 @@ int main(int argc, char **argv) if (vargv.size() < 3) { // Use all available transports for output - const auto pub = std::make_shared(pct.advertise(out_topic, rmw_qos_profile_default)); + auto pub = std::make_shared(pct.advertise(out_topic, rmw_qos_profile_default)); // Use Publisher::publish as the subscriber callback typedef void (point_cloud_transport::Publisher::*PublishMemFn)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr&) const; @@ -107,7 +107,7 @@ int main(int argc, char **argv) auto instance = loader->createUniqueInstance(lookup_name); // DO NOT use instance after this line - const std::shared_ptr pub = std::move(instance); + std::shared_ptr pub = std::move(instance); pub->advertise(node.get(), out_topic); // Use PublisherPlugin::publish as the subscriber callback From 4758df740d50597f0ec4d301ecc2cfd8fb9bcf7d Mon Sep 17 00:00:00 2001 From: john-maidbot Date: Mon, 19 Jun 2023 17:09:31 -0500 Subject: [PATCH 11/28] Fix raw sub --- CMakeLists.txt | 1 + src/list_transports.cpp | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d344032..2a8086b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,6 +22,7 @@ add_library(${PROJECT_NAME} src/point_cloud_transport.cpp src/publisher.cpp src/publisher_plugin.cpp + src/raw_subscriber.cpp src/single_subscriber_publisher.cpp src/subscriber.cpp ) diff --git a/src/list_transports.cpp b/src/list_transports.cpp index c07fcb0..f16fe66 100644 --- a/src/list_transports.cpp +++ b/src/list_transports.cpp @@ -76,8 +76,9 @@ int main(int argc, char** argv) for (const auto& lookup_name : pub_loader.getDeclaredClasses()) { - printf("Declared class: %s\n", lookup_name.c_str()); + printf("Lookup name: %s\n", lookup_name.c_str()); std::string transport_name = point_cloud_transport::erase_last_copy(lookup_name, "_pub"); + printf("Transport name: %s\n", transport_name.c_str()); transports[transport_name].pub_name = lookup_name; transports[transport_name].package_name = pub_loader.getClassPackage(lookup_name); try From 8db3df2a961c478d511def07dfa8c82a55a7114f Mon Sep 17 00:00:00 2001 From: john-maidbot Date: Mon, 19 Jun 2023 17:49:11 -0500 Subject: [PATCH 12/28] Temporary hardcoding --- CMakeLists.txt | 2 +- PORT_TODO.md | 3 ++- include/point_cloud_transport/point_cloud_transport.hpp | 2 +- 3 files changed, 4 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2a8086b..35a665f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -72,7 +72,7 @@ install(TARGETS ${PROJECT_NAME} EXPORT export_${PROJECT_NAME}) # Install executables install( - TARGETS list_transports #republish + TARGETS list_transports republish RUNTIME DESTINATION lib/${PROJECT_NAME} ) diff --git a/PORT_TODO.md b/PORT_TODO.md index d7c61bc..695c6e6 100644 --- a/PORT_TODO.md +++ b/PORT_TODO.md @@ -9,8 +9,9 @@ --> bag the pointcloud data --> Test point_cloud_codec files with bagged point cloud data - [] c++ codebase runs with test pointcloudsc ---> Fix c++ tests +- [] rviz integration - [] tests build and pass +--> Fix c++ tests - [] rospy --> rclpy conversion --> Update python rclpy files --> Update python bindings diff --git a/include/point_cloud_transport/point_cloud_transport.hpp b/include/point_cloud_transport/point_cloud_transport.hpp index 9cd54b9..b98a865 100644 --- a/include/point_cloud_transport/point_cloud_transport.hpp +++ b/include/point_cloud_transport/point_cloud_transport.hpp @@ -144,7 +144,7 @@ class PointCloudTransport : public PointCloudTransportLoader const VoidPtr& tracked_object = {}, const point_cloud_transport::TransportHints *transport_hints = nullptr) { - rmw_qos_profile_t custom_qos = rmw_qos_profile_default; + rmw_qos_profile_t custom_qos = rmw_qos_profile_sensor_data; custom_qos.depth = queue_size; rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions(); return Subscriber(node_.get(), base_topic, callback, sub_loader_, getTransportOrDefault(transport_hints), custom_qos, options); From 64d6f71362c75595737cd0a40c9668c8cc293dc4 Mon Sep 17 00:00:00 2001 From: john-maidbot Date: Mon, 19 Jun 2023 18:20:17 -0500 Subject: [PATCH 13/28] Update usage --- src/republish.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/republish.cpp b/src/republish.cpp index 8357a35..0fd96eb 100644 --- a/src/republish.cpp +++ b/src/republish.cpp @@ -61,7 +61,7 @@ int main(int argc, char **argv) if (vargv.size() < 2) { printf( - "Usage: %s in_transport in:= [out_transport] out:=\n", + "Usage: %s in_transport out_transport --ros-args --remap in:= --ros-args --remap out:=\n", argv[0]); return 0; } @@ -104,6 +104,7 @@ int main(int argc, char **argv) typedef point_cloud_transport::PublisherPlugin Plugin; auto loader = pct.getPublisherLoader(); std::string lookup_name = Plugin::getLookupName(out_transport); + RCLCPP_INFO(node->get_logger(), "Loading %s publisher", lookup_name.c_str()); auto instance = loader->createUniqueInstance(lookup_name); // DO NOT use instance after this line @@ -114,10 +115,14 @@ int main(int argc, char **argv) typedef void (point_cloud_transport::PublisherPlugin::*PublishMemFn)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr&) const; PublishMemFn pub_mem_fn = &point_cloud_transport::PublisherPlugin::publish; + RCLCPP_INFO(node->get_logger(), "Loading %s subscriber"); + const point_cloud_transport::TransportHints hint(in_transport); auto sub = pct.subscribe( in_topic, static_cast(1), pub_mem_fn, pub, &hint); + + RCLCPP_INFO(node->get_logger(), "Spinning node"); // spin the node rclcpp::spin(node); } From ccec8893b2a6e14e6eb810c99705a4fe963ac74a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Sun, 25 Jun 2023 22:52:20 +0200 Subject: [PATCH 14/28] Added common test (#2) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .github/workflows/build-and-test.sh | 33 + .github/workflows/ros2-ci.yml | 24 + CMakeLists.txt | 8 +- CONTRIBUTING.md | 3 + LICENSE | 40 +- doc/conf.py | 5 +- include/point_cloud_transport/c_api.h | 68 +- include/point_cloud_transport/exception.hpp | 68 +- include/point_cloud_transport/expected.hpp | 56 +- include/point_cloud_transport/loader_fwds.hpp | 72 +- .../point_cloud_codec.hpp | 156 +- .../point_cloud_common.hpp | 73 +- .../point_cloud_transport.hpp | 137 +- include/point_cloud_transport/publisher.hpp | 71 +- .../publisher_plugin.hpp | 64 +- .../point_cloud_transport/raw_publisher.hpp | 70 +- .../point_cloud_transport/raw_subscriber.hpp | 72 +- .../simple_publisher_plugin.hpp | 106 +- .../simple_subscriber_plugin.hpp | 80 +- .../single_subscriber_publisher.hpp | 76 +- include/point_cloud_transport/subscriber.hpp | 69 +- .../subscriber_filter.hpp | 75 +- .../subscriber_plugin.hpp | 71 +- include/point_cloud_transport/tl/expected.hpp | 2555 +++++++++-------- .../point_cloud_transport/transport_hints.hpp | 70 +- package.xml | 2 - setup.py | 2 +- src/c_api.cpp | 32 +- src/list_transports.cpp | 133 +- src/manifest.cpp | 37 +- src/point_cloud_codec.cpp | 313 +- src/point_cloud_common.cpp | 82 +- src/point_cloud_transport.cpp | 96 +- src/point_cloud_transport/__init__.py | 73 +- src/point_cloud_transport/common.py | 32 +- src/point_cloud_transport/decoder.py | 68 +- src/point_cloud_transport/encoder.py | 59 +- src/point_cloud_transport/publisher.py | 61 +- src/point_cloud_transport/subscriber.py | 56 +- src/publisher.cpp | 136 +- src/publisher_plugin.cpp | 65 +- src/raw_subscriber.cpp | 67 +- src/republish.cpp | 100 +- src/single_subscriber_publisher.cpp | 58 +- src/subscriber.cpp | 87 +- 45 files changed, 3135 insertions(+), 2546 deletions(-) create mode 100755 .github/workflows/build-and-test.sh create mode 100644 .github/workflows/ros2-ci.yml create mode 100644 CONTRIBUTING.md diff --git a/.github/workflows/build-and-test.sh b/.github/workflows/build-and-test.sh new file mode 100755 index 0000000..488ab20 --- /dev/null +++ b/.github/workflows/build-and-test.sh @@ -0,0 +1,33 @@ +#!/bin/bash +set -ev + +# Configuration. +export COLCON_WS=~/ws +export COLCON_WS_SRC=${COLCON_WS}/src +export DEBIAN_FRONTEND=noninteractive +export ROS_PYTHON_VERSION=3 + +apt update -qq +apt install -qq -y lsb-release wget curl build-essential + +# Dependencies. +echo "deb http://packages.ros.org/ros2-testing/ubuntu `lsb_release -cs` main" > /etc/apt/sources.list.d/ros2-testing.list +curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - +apt-get update -qq +apt-get install -y python3-colcon-common-extensions \ + python3-rosdep + +rosdep init +rosdep update +rosdep install --from-paths ./ -i -y -r --rosdistro $ROS_DISTRO $ROSDEP_ARGS + +# Build. +source /opt/ros/$ROS_DISTRO/setup.bash +mkdir -p $COLCON_WS_SRC +cp -r $GITHUB_WORKSPACE $COLCON_WS_SRC +cd $COLCON_WS +colcon build --event-handlers console_direct+ + +# Tests. +colcon test --event-handlers console_direct+ +colcon test-result diff --git a/.github/workflows/ros2-ci.yml b/.github/workflows/ros2-ci.yml new file mode 100644 index 0000000..ced53c2 --- /dev/null +++ b/.github/workflows/ros2-ci.yml @@ -0,0 +1,24 @@ +name: ROS2 CI + +on: [push, pull_request] + +jobs: + point_cloud_transport_ci: + name: point_cloud_transport CI + runs-on: ubuntu-latest + strategy: + fail-fast: false + matrix: + include: + - docker-image: "ubuntu:22.04" + ros-distro: "rolling" + container: + image: ${{ matrix.docker-image }} + steps: + - name: Checkout + uses: actions/checkout@v2 + - name: Build and Test + run: .github/workflows/build-and-test.sh + env: + DOCKER_IMAGE: ${{ matrix.docker-image }} + ROS_DISTRO: ${{ matrix.ros-distro }} diff --git a/CMakeLists.txt b/CMakeLists.txt index 35a665f..7b0756b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -83,6 +83,12 @@ ament_export_targets(export_${PROJECT_NAME}) ament_export_dependencies(message_filters rclcpp sensor_msgs pluginlib) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(AMENT_LINT_AUTO_FILE_EXCLUDE include/point_cloud_transport/tl/expected.hpp doc/conf.py) + ament_lint_auto_find_test_dependencies() +endif() + # TODO: Fix testing # if (CATKIN_ENABLE_TESTING) # find_package(roslint REQUIRED) @@ -113,7 +119,7 @@ ament_export_dependencies(message_filters rclcpp sensor_msgs pluginlib) # -build/header_guard,-readability/namespace,-whitespace/braces,-runtime/references,\ # -build/c++11,-readability/nolint,-readability/todo,-legal/copyright,-build/namespaces") # roslint_cpp(${ROSLINT_INCLUDE} ${ROSLINT_SRC}) - + # # Roslint Python # # Run roslint on Python sources diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 0000000..309be1e --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,3 @@ +Any contribution that you make to this repository will +be under the 3-Clause BSD License, as dictated by that +[license](https://opensource.org/licenses/BSD-3-Clause). diff --git a/LICENSE b/LICENSE index 3272270..574ef07 100644 --- a/LICENSE +++ b/LICENSE @@ -1,31 +1,25 @@ -BSD 3-Clause License - -Copyright (c) Czech Technical University in Prague -Copyright (c) 2019, paplhjak -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: -1. Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. -2. Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. -3. Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. + * Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +POSSIBILITY OF SUCH DAMAGE. diff --git a/doc/conf.py b/doc/conf.py index 47c00e4..ec66a0c 100644 --- a/doc/conf.py +++ b/doc/conf.py @@ -1,4 +1,5 @@ # Configuration file for the Sphinx documentation builder. import os -os.environ['CRAS_DOCS_COMMON_SPHINX_PACKAGE_PATH'] = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) -from cras_docs_common.sphinx_docs_conf import * \ No newline at end of file +os.environ['CRAS_DOCS_COMMON_SPHINX_PACKAGE_PATH'] = \ + os.path.dirname(os.path.dirname(os.path.abspath(__file__))) +from cras_docs_common.sphinx_docs_conf import * # noqa: E402, I201, F401, F403 diff --git a/include/point_cloud_transport/c_api.h b/include/point_cloud_transport/c_api.h index 5296538..2652b7a 100644 --- a/include/point_cloud_transport/c_api.h +++ b/include/point_cloud_transport/c_api.h @@ -1,7 +1,37 @@ +/* + * Copyright (c) 2023, Czech Technical University in Prague + * 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 HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ // #pragma once -// // SPDX-License-Identifier: BSD-3-Clause -// // SPDX-FileCopyrightText: Czech Technical University in Prague +#ifndef POINT_CLOUD_TRANSPORT__C_API_H_ +#define POINT_CLOUD_TRANSPORT__C_API_H_ // /** // * \file @@ -16,22 +46,29 @@ // { // /** -// * \brief Allocator function that should allocate a buffer of the given size on the caller side and return a pointer +// * \brief Allocator function that should allocate a buffer of the given size on the caller +// side and return a pointer // * to it. // * -// * This is used throughout the C API to ease returning strings and byte buffers of dynamic size to the caller. Instead -// * of taking a writable `char*` argument or returning a `new`-allocated `const char*`, the function takes the allocator -// * as the argument. When it is about to return to the caller, it uses the allocator to get a caller-side-managed buffer +// * This is used throughout the C API to ease returning strings and byte buffers of dynamic +// size to the caller. Instead +// * of taking a writable `char*` argument or returning a `new`-allocated `const char*`, +// the function takes the allocator +// * as the argument. When it is about to return to the caller, it uses the allocator to get a +// caller-side-managed buffer // * of the correct size and writes the string/byte buffer to this buffer. -// * -// * cras_py_common provides the `BytesAllocator` and `StringAllocator` classes that can be passed as this allocator -// * argument. Once the ctypes function call finishes, the caller can access `allocator.value` to get the string or +// * +// * cras_py_common provides the `BytesAllocator` and `StringAllocator` classes that can be +// passed as this allocator +// * argument. Once the ctypes function call finishes, the caller can access `allocator.value` +// to get the string or // * buffer returned by the function. // */ // typedef void* (*allocator_t)(size_t); // /** -// * \brief Allocate enough bytes using the given allocator and copy the given string into the buffer (including null +// * \brief Allocate enough bytes using the given allocator and copy the given string into +// the buffer (including null // * termination byte). // * \param[in] allocator The allocator to use. // * \param[in] string The zero-terminated string to copy into the allocated buffer. @@ -41,7 +78,8 @@ // char* outputString(allocator_t allocator, const char* string, size_t length); // /** -// * \brief Allocate enough bytes using the given allocator and copy the given string into the buffer (including null +// * \brief Allocate enough bytes using the given allocator and copy the given string into +// the buffer (including null // * termination byte). // * \param[in] allocator The allocator to use. // * \param[in] string The string to copy into the allocated buffer. @@ -50,7 +88,8 @@ // char* outputString(allocator_t allocator, const std::string& string); // /** -// * \brief Allocate enough bytes using the given allocator and copy the given bytes into the buffer. +// * \brief Allocate enough bytes using the given allocator and copy the given bytes into +// the buffer. // * \param[in] allocator The allocator to use. // * \param[in] bytes The bytes to copy into the allocated buffer. // * \param[in] length Length of `bytes`. @@ -59,7 +98,8 @@ // uint8_t* outputByteBuffer(allocator_t allocator, const uint8_t* bytes, size_t length); // /** -// * \brief Allocate enough bytes using the given allocator and copy the given bytes into the buffer. +// * \brief Allocate enough bytes using the given allocator and copy the given bytes into +// the buffer. // * \param[in] allocator The allocator to use. // * \param[in] bytes The bytes to copy into the allocated buffer. // * \return Pointer to the allocated buffer. @@ -68,3 +108,5 @@ // } + +#endif // POINT_CLOUD_TRANSPORT__C_API_H_ diff --git a/include/point_cloud_transport/exception.hpp b/include/point_cloud_transport/exception.hpp index 5bea469..67ab826 100644 --- a/include/point_cloud_transport/exception.hpp +++ b/include/point_cloud_transport/exception.hpp @@ -1,44 +1,38 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. - /* - * - * BSD 3-Clause License - * - * Copyright (c) Czech Technical University in Prague + * Copyright (c) 2023, Czech Technical University in Prague * Copyright (c) 2019, paplhjak * Copyright (c) 2009, Willow Garage, Inc. - * - * All rights reserved. + * All rights reserved. * * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. + * modification, are permitted provided that the following conditions are met: * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. + * * 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. * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. */ -#pragma once +#ifndef POINT_CLOUD_TRANSPORT__EXCEPTION_HPP_ +#define POINT_CLOUD_TRANSPORT__EXCEPTION_HPP_ #include #include @@ -50,7 +44,8 @@ namespace point_cloud_transport class Exception : public std::runtime_error { public: - explicit Exception(const std::string& message) : std::runtime_error(message) + explicit Exception(const std::string & message) + : std::runtime_error(message) { } }; @@ -59,9 +54,9 @@ class Exception : public std::runtime_error class TransportLoadException : public Exception { public: - TransportLoadException(const std::string& transport, const std::string& message) - : Exception("Unable to load plugin for transport '" + transport + "', error string:\n" + message), - transport_(transport.c_str()) + TransportLoadException(const std::string & transport, const std::string & message) + : Exception("Unable to load plugin for transport '" + transport + "', error string:\n" + message), + transport_(transport.c_str()) { } @@ -71,7 +66,8 @@ class TransportLoadException : public Exception } protected: - const char* transport_; + const char * transport_; }; -} +} // namespace point_cloud_transport +#endif // POINT_CLOUD_TRANSPORT__EXCEPTION_HPP_ diff --git a/include/point_cloud_transport/expected.hpp b/include/point_cloud_transport/expected.hpp index c1162a9..eb4fce8 100644 --- a/include/point_cloud_transport/expected.hpp +++ b/include/point_cloud_transport/expected.hpp @@ -1,4 +1,37 @@ -#pragma once +/* + * Copyright (c) 2023, Czech Technical University in Prague + * 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 HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef POINT_CLOUD_TRANSPORT__EXPECTED_HPP_ +#define POINT_CLOUD_TRANSPORT__EXPECTED_HPP_ + // SPDX-License-Identifier: BSD-3-Clause // SPDX-FileCopyrightText: Czech Technical University in Prague @@ -6,7 +39,7 @@ /** * \file * \brief An implementation of the `std::expected` proposal. - * + * * `std::expected` should be used in functions that can either succeed and return a value, or fail and return an error. * \author Martin Pecka */ @@ -15,13 +48,13 @@ namespace cras { - using ::tl::bad_expected_access; - using ::tl::expected; - using ::tl::in_place; - using ::tl::make_unexpected; - using ::tl::unexpect; - using ::tl::unexpected; -} +using ::tl::bad_expected_access; +using ::tl::expected; +using ::tl::in_place; +using ::tl::make_unexpected; +using ::tl::unexpect; +using ::tl::unexpected; +} // namespace cras #include @@ -40,5 +73,6 @@ struct is_cras_expected : public ::std::false_type {}; * \tparam E Type of the expected error. */ template -struct is_cras_expected<::cras::expected> : public ::std::true_type {}; -} +struct is_cras_expected<::cras::expected>: public ::std::true_type {}; +} // namespace cras +#endif // POINT_CLOUD_TRANSPORT__EXPECTED_HPP_ diff --git a/include/point_cloud_transport/loader_fwds.hpp b/include/point_cloud_transport/loader_fwds.hpp index 8954f01..90031f1 100644 --- a/include/point_cloud_transport/loader_fwds.hpp +++ b/include/point_cloud_transport/loader_fwds.hpp @@ -1,47 +1,41 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. - /* - * - * BSD 3-Clause License - * - * Copyright (c) Czech Technical University in Prague + * Copyright (c) 2023, Czech Technical University in Prague * Copyright (c) 2019, paplhjak * Copyright (c) 2009, Willow Garage, Inc. - * - * All rights reserved. + * All rights reserved. * * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * + * 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 HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. */ -#pragma once +#ifndef POINT_CLOUD_TRANSPORT__LOADER_FWDS_HPP_ +#define POINT_CLOUD_TRANSPORT__LOADER_FWDS_HPP_ -#include +#include // Forward-declare some classes most users shouldn't care about so that @@ -49,8 +43,9 @@ namespace pluginlib { -template class ClassLoader; -} +template +class ClassLoader; +} // namespace pluginlib namespace point_cloud_transport { @@ -62,4 +57,5 @@ typedef std::shared_ptr PubLoaderPtr; typedef pluginlib::ClassLoader SubLoader; typedef std::shared_ptr SubLoaderPtr; -} +} // namespace point_cloud_transport +#endif // POINT_CLOUD_TRANSPORT__LOADER_FWDS_HPP_ diff --git a/include/point_cloud_transport/point_cloud_codec.hpp b/include/point_cloud_transport/point_cloud_codec.hpp index 8a806f5..f92a654 100644 --- a/include/point_cloud_transport/point_cloud_codec.hpp +++ b/include/point_cloud_transport/point_cloud_codec.hpp @@ -1,44 +1,40 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. - /* - * - * BSD 3-Clause License - * - * Copyright (c) Czech Technical University in Prague + * Copyright (c) 2023, Czech Technical University in Prague * Copyright (c) 2019, paplhjak * Copyright (c) 2009, Willow Garage, Inc. - * - * All rights reserved. + * All rights reserved. * * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: + * modification, are permitted provided that the following conditions are met: * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. */ -#pragma once +#ifndef POINT_CLOUD_TRANSPORT__POINT_CLOUD_CODEC_HPP_ +#define POINT_CLOUD_TRANSPORT__POINT_CLOUD_CODEC_HPP_ + +#include #include #include @@ -46,7 +42,6 @@ #include -#include #include #include @@ -64,17 +59,19 @@ class PointCloudCodec { public: //! Constructor - explicit PointCloudCodec(); + PointCloudCodec(); - std::shared_ptr getEncoderByName(const std::string& name) const; + std::shared_ptr getEncoderByName(const std::string & name) + const; std::shared_ptr getEncoderByTopic( - const std::string& topic, const std::string& datatype) const; + const std::string & topic, const std::string & datatype) const; - std::shared_ptr getDecoderByName(const std::string& name) const; + std::shared_ptr getDecoderByName( + const std::string & name) const; std::shared_ptr getDecoderByTopic( - const std::string& topic, const std::string& datatype) const; + const std::string & topic, const std::string & datatype) const; private: struct Impl; @@ -84,52 +81,53 @@ class PointCloudCodec ImplPtr impl_; }; -} +} // namespace point_cloud_transport extern "C" bool pointCloudTransportCodecsEncode( - const char* codec, - sensor_msgs::msg::PointCloud2::_height_type rawHeight, - sensor_msgs::msg::PointCloud2::_width_type rawWidth, - size_t rawNumFields, - const char* rawFieldNames[], - sensor_msgs::PointField::_offset_type rawFieldOffsets[], - sensor_msgs::PointField::_datatype_type rawFieldDatatypes[], - sensor_msgs::PointField::_count_type rawFieldCounts[], - sensor_msgs::msg::PointCloud2::_is_bigendian_type rawIsBigendian, - sensor_msgs::msg::PointCloud2::_point_step_type rawPointStep, - sensor_msgs::msg::PointCloud2::_row_step_type rawRowStep, - size_t rawDataLength, - const uint8_t rawData[], - sensor_msgs::msg::PointCloud2::_is_dense_type rawIsDense, - cras::allocator_t compressedTypeAllocator, - cras::allocator_t compressedMd5SumAllocator, - cras::allocator_t compressedDataAllocator, - size_t serializedConfigLength, - const uint8_t serializedConfig[], - cras::allocator_t errorStringAllocator, - cras::allocator_t logMessagesAllocator + const char * codec, + sensor_msgs::msg::PointCloud2::_height_type rawHeight, + sensor_msgs::msg::PointCloud2::_width_type rawWidth, + size_t rawNumFields, + const char * rawFieldNames[], + sensor_msgs::PointField::_offset_type rawFieldOffsets[], + sensor_msgs::PointField::_datatype_type rawFieldDatatypes[], + sensor_msgs::PointField::_count_type rawFieldCounts[], + sensor_msgs::msg::PointCloud2::_is_bigendian_type rawIsBigendian, + sensor_msgs::msg::PointCloud2::_point_step_type rawPointStep, + sensor_msgs::msg::PointCloud2::_row_step_type rawRowStep, + size_t rawDataLength, + const uint8_t rawData[], + sensor_msgs::msg::PointCloud2::_is_dense_type rawIsDense, + cras::allocator_t compressedTypeAllocator, + cras::allocator_t compressedMd5SumAllocator, + cras::allocator_t compressedDataAllocator, + size_t serializedConfigLength, + const uint8_t serializedConfig[], + cras::allocator_t errorStringAllocator, + cras::allocator_t logMessagesAllocator ); extern "C" bool pointCloudTransportCodecsDecode( - const char* topicOrCodec, - const char* compressedType, - const char* compressedMd5sum, - size_t compressedDataLength, - const uint8_t compressedData[], - sensor_msgs::msg::PointCloud2::_height_type& rawHeight, - sensor_msgs::msg::PointCloud2::_width_type& rawWidth, - uint32_t& rawNumFields, - cras::allocator_t rawFieldNamesAllocator, - cras::allocator_t rawFieldOffsetsAllocator, - cras::allocator_t rawFieldDatatypesAllocator, - cras::allocator_t rawFieldCountsAllocator, - sensor_msgs::msg::PointCloud2::_is_bigendian_type& rawIsBigEndian, - sensor_msgs::msg::PointCloud2::_point_step_type& rawPointStep, - sensor_msgs::msg::PointCloud2::_row_step_type& rawRowStep, - cras::allocator_t rawDataAllocator, - sensor_msgs::msg::PointCloud2::_is_dense_type& rawIsDense, - size_t serializedConfigLength, - const uint8_t serializedConfig[], - cras::allocator_t errorStringAllocator, - cras::allocator_t logMessagesAllocator + const char * topicOrCodec, + const char * compressedType, + const char * compressedMd5sum, + size_t compressedDataLength, + const uint8_t compressedData[], + sensor_msgs::msg::PointCloud2::_height_type & rawHeight, + sensor_msgs::msg::PointCloud2::_width_type & rawWidth, + uint32_t & rawNumFields, + cras::allocator_t rawFieldNamesAllocator, + cras::allocator_t rawFieldOffsetsAllocator, + cras::allocator_t rawFieldDatatypesAllocator, + cras::allocator_t rawFieldCountsAllocator, + sensor_msgs::msg::PointCloud2::_is_bigendian_type & rawIsBigEndian, + sensor_msgs::msg::PointCloud2::_point_step_type & rawPointStep, + sensor_msgs::msg::PointCloud2::_row_step_type & rawRowStep, + cras::allocator_t rawDataAllocator, + sensor_msgs::msg::PointCloud2::_is_dense_type & rawIsDense, + size_t serializedConfigLength, + const uint8_t serializedConfig[], + cras::allocator_t errorStringAllocator, + cras::allocator_t logMessagesAllocator ); +#endif // POINT_CLOUD_TRANSPORT__POINT_CLOUD_CODEC_HPP_ diff --git a/include/point_cloud_transport/point_cloud_common.hpp b/include/point_cloud_transport/point_cloud_common.hpp index f2eff6d..49ed17f 100644 --- a/include/point_cloud_transport/point_cloud_common.hpp +++ b/include/point_cloud_transport/point_cloud_common.hpp @@ -1,32 +1,36 @@ -// Copyright (c) 2009, Willow Garage, Inc. -// -// 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 HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -#pragma once +/* + * 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 copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef POINT_CLOUD_TRANSPORT__POINT_CLOUD_COMMON_HPP_ +#define POINT_CLOUD_TRANSPORT__POINT_CLOUD_COMMON_HPP_ #include #include @@ -39,12 +43,15 @@ namespace point_cloud_transport */ std::string erase_last_copy(const std::string & input, const std::string & search); -std::vector split(const std::string& str, const std::string& delimiter, int maxSplits); +std::vector split( + const std::string & str, const std::string & delimiter, + int maxSplits); // from cras::string_utils -bool endsWith(const std::string& str, const std::string& suffix); +bool endsWith(const std::string & str, const std::string & suffix); // from cras::string_utils -std::string removeSuffix(const std::string& str, const std::string& suffix, bool* hadSuffix); +std::string removeSuffix(const std::string & str, const std::string & suffix, bool * hadSuffix); -} +} // namespace point_cloud_transport +#endif // POINT_CLOUD_TRANSPORT__POINT_CLOUD_COMMON_HPP_ diff --git a/include/point_cloud_transport/point_cloud_transport.hpp b/include/point_cloud_transport/point_cloud_transport.hpp index b98a865..89e2e74 100644 --- a/include/point_cloud_transport/point_cloud_transport.hpp +++ b/include/point_cloud_transport/point_cloud_transport.hpp @@ -1,44 +1,38 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. - /* - * - * BSD 3-Clause License - * - * Copyright (c) Czech Technical University in Prague + * Copyright (c) 2023, Czech Technical University in Prague * Copyright (c) 2019, paplhjak * Copyright (c) 2009, Willow Garage, Inc. - * - * All rights reserved. + * All rights reserved. * * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. + * modification, are permitted provided that the following conditions are met: * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. + * * 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. * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. */ -#pragma once +#ifndef POINT_CLOUD_TRANSPORT__POINT_CLOUD_TRANSPORT_HPP_ +#define POINT_CLOUD_TRANSPORT__POINT_CLOUD_TRANSPORT_HPP_ #include #include @@ -56,7 +50,8 @@ #include -namespace point_cloud_transport { +namespace point_cloud_transport +{ /** * Advertise and subscribe to PointCloud2 topics. @@ -78,7 +73,8 @@ class PointCloudTransportLoader //! transports are not necessarily built or loadable. std::vector getDeclaredTransports() const; - //! Returns the names of all transports that are loadable in the system (keys are lookup names, values are names). + //! Returns the names of all transports that are loadable in the system + // (keys are lookup names, values are names). std::unordered_map getLoadableTransports() const; //! The loader that can load publisher plugins. @@ -133,67 +129,86 @@ class PointCloudTransport : public PointCloudTransportLoader //! Advertise an PointCloud2 topic with subscriber status callbacks. // TODO(ros2) Implement when SubscriberStatusCallback is available // point_cloud_transport::Publisher advertise(const std::string& base_topic, uint32_t queue_size, - // const point_cloud_transport::SubscriberStatusCallback& connect_cb, - // const point_cloud_transport::SubscriberStatusCallback& disconnect_cb = {}, - // const ros::VoidPtr& tracked_object = {}, bool latch = false); + // const point_cloud_transport::SubscriberStatusCallback& connect_cb, + // const point_cloud_transport::SubscriberStatusCallback& disconnect_cb = {}, + // const ros::VoidPtr& tracked_object = {}, bool latch = false); //! Subscribe to a point cloud topic, version for arbitrary std::function object. point_cloud_transport::Subscriber subscribe( - const std::string& base_topic, uint32_t queue_size, - const std::function& callback, - const VoidPtr& tracked_object = {}, - const point_cloud_transport::TransportHints *transport_hints = nullptr) + const std::string & base_topic, uint32_t queue_size, + const std::function & callback, + const VoidPtr & tracked_object = {}, + const point_cloud_transport::TransportHints * transport_hints = nullptr) { rmw_qos_profile_t custom_qos = rmw_qos_profile_sensor_data; custom_qos.depth = queue_size; rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions(); - return Subscriber(node_.get(), base_topic, callback, sub_loader_, getTransportOrDefault(transport_hints), custom_qos, options); + return Subscriber( + node_.get(), base_topic, callback, sub_loader_, + getTransportOrDefault(transport_hints), custom_qos, options); } //! Subscribe to a point cloud topic, version for bare function. - point_cloud_transport::Subscriber subscribe(const std::string& base_topic, uint32_t queue_size, - void(* fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr&), - const point_cloud_transport::TransportHints* transport_hints = nullptr) + point_cloud_transport::Subscriber subscribe( + const std::string & base_topic, uint32_t queue_size, + void (* fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &), + const point_cloud_transport::TransportHints * transport_hints = nullptr) { - return subscribe(base_topic, queue_size, - std::function(fp), - VoidPtr(), transport_hints); + return subscribe( + base_topic, queue_size, + std::function(fp), + VoidPtr(), transport_hints); } //! Subscribe to a point cloud topic, version for class member function with bare pointer. template - point_cloud_transport::Subscriber subscribe(const std::string& base_topic, uint32_t queue_size, - void(T::*fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr&) const, T* obj, - const point_cloud_transport::TransportHints* transport_hints = nullptr, - bool allow_concurrent_callbacks = false) + point_cloud_transport::Subscriber subscribe( + const std::string & base_topic, uint32_t queue_size, + void (T::* fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &) const, T * obj, + const point_cloud_transport::TransportHints * transport_hints = nullptr, + bool allow_concurrent_callbacks = false) { - return subscribe(base_topic, queue_size, std::bind(fp, obj.get(), std::placeholders::_1), VoidPtr(), transport_hints); + return subscribe( + base_topic, queue_size, std::bind( + fp, + obj.get(), std::placeholders::_1), VoidPtr(), transport_hints); } //! Subscribe to a point cloud topic, version for class member function with shared_ptr. template - point_cloud_transport::Subscriber subscribe(const std::string& base_topic, uint32_t queue_size, - void(T::*fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr&) const, - const std::shared_ptr& obj, - const point_cloud_transport::TransportHints* transport_hints = nullptr) + point_cloud_transport::Subscriber subscribe( + const std::string & base_topic, uint32_t queue_size, + void (T::* fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &) const, + const std::shared_ptr & obj, + const point_cloud_transport::TransportHints * transport_hints = nullptr) { - return subscribe(base_topic, queue_size, std::bind(fp, obj.get(), std::placeholders::_1), obj, transport_hints); + return subscribe( + base_topic, queue_size, std::bind( + fp, + obj.get(), std::placeholders::_1), obj, transport_hints); } private: rclcpp::Node::SharedPtr node_; }; -} +} // namespace point_cloud_transport -// TODO: May need these? +// TODO(anyone): May need these? // extern "C" void pointCloudTransportGetLoadableTransports( // cras::allocator_t transportAllocator, cras::allocator_t nameAllocator); // extern "C" void pointCloudTransportGetTopicsToPublish( -// const char* baseTopic, cras::allocator_t transportAllocator, cras::allocator_t nameAllocator, -// cras::allocator_t topicAllocator, cras::allocator_t dataTypeAllocator, cras::allocator_t configTypeAllocator); +// const char* baseTopic, +// cras::allocator_t transportAllocator, +// cras::allocator_t nameAllocator, +// cras::allocator_t topicAllocator, +// cras::allocator_t dataTypeAllocator, cras::allocator_t configTypeAllocator); // extern "C" void pointCloudTransportGetTopicToSubscribe( -// const char* baseTopic, const char* transport, cras::allocator_t nameAllocator, cras::allocator_t topicAllocator, +// const char* baseTopic, const char* transport, cras::allocator_t nameAllocator, +// cras::allocator_t topicAllocator, // cras::allocator_t dataTypeAllocator, cras::allocator_t configTypeAllocator); + + +#endif // POINT_CLOUD_TRANSPORT__POINT_CLOUD_TRANSPORT_HPP_ diff --git a/include/point_cloud_transport/publisher.hpp b/include/point_cloud_transport/publisher.hpp index 52093ce..e689bb3 100644 --- a/include/point_cloud_transport/publisher.hpp +++ b/include/point_cloud_transport/publisher.hpp @@ -1,44 +1,38 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. - /* - * - * BSD 3-Clause License - * - * Copyright (c) Czech Technical University in Prague + * Copyright (c) 2023, Czech Technical University in Prague * Copyright (c) 2019, paplhjak * Copyright (c) 2009, Willow Garage, Inc. - * - * All rights reserved. + * All rights reserved. * * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. + * modification, are permitted provided that the following conditions are met: * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. + * * 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. * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. */ -#pragma once +#ifndef POINT_CLOUD_TRANSPORT__PUBLISHER_HPP_ +#define POINT_CLOUD_TRANSPORT__PUBLISHER_HPP_ #include #include @@ -67,7 +61,7 @@ class Publisher const std::string & base_topic, PubLoaderPtr loader, rmw_qos_profile_t custom_qos); - + //! get total number of subscribers to all advertised topics. uint32_t getNumSubscribers() const; @@ -75,27 +69,27 @@ class Publisher std::string getTopic() const; //! Publish a point cloud on the topics associated with this Publisher. - void publish(const sensor_msgs::msg::PointCloud2& message) const; + void publish(const sensor_msgs::msg::PointCloud2 & message) const; //! Publish a point cloud on the topics associated with this Publisher. - void publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& message) const; + void publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message) const; //! Shutdown the advertisements associated with this Publisher. void shutdown(); - operator void*() const; + operator void *() const; - bool operator<(const point_cloud_transport::Publisher& rhs) const + bool operator<(const point_cloud_transport::Publisher & rhs) const { return impl_ < rhs.impl_; } - bool operator!=(const point_cloud_transport::Publisher& rhs) const + bool operator!=(const point_cloud_transport::Publisher & rhs) const { return impl_ != rhs.impl_; } - bool operator==(const point_cloud_transport::Publisher& rhs) const + bool operator==(const point_cloud_transport::Publisher & rhs) const { return impl_ == rhs.impl_; } @@ -106,4 +100,5 @@ class Publisher friend class PointCloudTransport; }; -} +} // namespace point_cloud_transport +#endif // POINT_CLOUD_TRANSPORT__PUBLISHER_HPP_ diff --git a/include/point_cloud_transport/publisher_plugin.hpp b/include/point_cloud_transport/publisher_plugin.hpp index b03a38a..e513065 100644 --- a/include/point_cloud_transport/publisher_plugin.hpp +++ b/include/point_cloud_transport/publisher_plugin.hpp @@ -1,44 +1,38 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. - /* - * - * BSD 3-Clause License - * - * Copyright (c) Czech Technical University in Prague + * Copyright (c) 2023, Czech Technical University in Prague * Copyright (c) 2019, paplhjak * Copyright (c) 2009, Willow Garage, Inc. - * - * All rights reserved. + * All rights reserved. * * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. + * modification, are permitted provided that the following conditions are met: * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. + * * 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. * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. */ -#pragma once +#ifndef POINT_CLOUD_TRANSPORT__PUBLISHER_PLUGIN_HPP_ +#define POINT_CLOUD_TRANSPORT__PUBLISHER_PLUGIN_HPP_ #include #include @@ -81,16 +75,16 @@ class PublisherPlugin virtual std::string getTopic() const = 0; //! Publish a point cloud using the transport associated with this PublisherPlugin. - virtual void publish(const sensor_msgs::msg::PointCloud2& message) const = 0; + virtual void publish(const sensor_msgs::msg::PointCloud2 & message) const = 0; //! Publish a point cloud using the transport associated with this PublisherPlugin. - virtual void publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& message) const; + virtual void publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message) const; //! Shutdown any advertisements associated with this PublisherPlugin. virtual void shutdown() = 0; //! Return the lookup name of the PublisherPlugin associated with a specific transport identifier. - static std::string getLookupName(const std::string& transport_name); + static std::string getLookupName(const std::string & transport_name); protected: /** @@ -104,7 +98,7 @@ class PublisherPlugin class SingleTopicPublisherPlugin : public PublisherPlugin { public: - }; -} +} // namespace point_cloud_transport +#endif // POINT_CLOUD_TRANSPORT__PUBLISHER_PLUGIN_HPP_ diff --git a/include/point_cloud_transport/raw_publisher.hpp b/include/point_cloud_transport/raw_publisher.hpp index 7647fc8..4fefc1a 100644 --- a/include/point_cloud_transport/raw_publisher.hpp +++ b/include/point_cloud_transport/raw_publisher.hpp @@ -1,44 +1,38 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. - /* - * - * BSD 3-Clause License - * - * Copyright (c) Czech Technical University in Prague + * Copyright (c) 2023, Czech Technical University in Prague * Copyright (c) 2019, paplhjak * Copyright (c) 2009, Willow Garage, Inc. - * - * All rights reserved. + * All rights reserved. * * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. + * modification, are permitted provided that the following conditions are met: * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. + * * 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. * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. */ -#pragma once +#ifndef POINT_CLOUD_TRANSPORT__RAW_PUBLISHER_HPP_ +#define POINT_CLOUD_TRANSPORT__RAW_PUBLISHER_HPP_ #include @@ -51,7 +45,8 @@ namespace point_cloud_transport //! RawPublisher is a simple wrapper for ros::Publisher, //! publishing unaltered PointCloud2 messages on the base topic. -class RawPublisher : public point_cloud_transport::SimplePublisherPlugin +class RawPublisher + : public point_cloud_transport::SimplePublisherPlugin { public: virtual ~RawPublisher() {} @@ -62,18 +57,22 @@ class RawPublisher : public point_cloud_transport::SimplePublisherPlugin #include @@ -56,18 +51,20 @@ namespace point_cloud_transport * RawSubscriber is a simple wrapper for ros::Subscriber which listens for PointCloud2 messages * and passes them through to the callback. */ -class RawSubscriber : public point_cloud_transport::SimpleSubscriberPlugin +class RawSubscriber + : public point_cloud_transport::SimpleSubscriberPlugin { public: virtual ~RawSubscriber() {} - SubscriberPlugin::DecodeResult decodeTyped(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& compressed) const + SubscriberPlugin::DecodeResult decodeTyped( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & compressed) const { return compressed; } - SubscriberPlugin::DecodeResult decodeTyped(const sensor_msgs::msg::PointCloud2& compressed) const + SubscriberPlugin::DecodeResult decodeTyped(const sensor_msgs::msg::PointCloud2 & compressed) const { auto compressedPtr = std::make_shared(compressed); return this->decodeTyped(compressedPtr); @@ -76,9 +73,11 @@ class RawSubscriber : public point_cloud_transport::SimpleSubscriberPlugin #include @@ -81,7 +76,8 @@ template class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin { public: - //! \brief Result of cloud encoding. Either the compressed cloud message, empty value, or error message. + //! \brief Result of cloud encoding. Either the compressed cloud message, + // empty value, or error message. typedef cras::expected, std::string> TypedEncodeResult; ~SimplePublisherPlugin() @@ -90,8 +86,7 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin rclcpp::Logger getLogger() const { - if (simple_impl_) - { + if (simple_impl_) { return simple_impl_->logger_; } return rclcpp::get_logger("point_cloud_transport"); @@ -99,10 +94,9 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin // template function for getting parameter of a given type template - bool getParam(const std::string& name, T& value) const + bool getParam(const std::string & name, T & value) const { - if (simple_impl_) - { + if (simple_impl_) { return simple_impl_->node_->get_parameter(name, value); } return false; @@ -110,8 +104,7 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin uint32_t getNumSubscribers() const override { - if (simple_impl_) - { + if (simple_impl_) { return simple_impl_->pub_->get_subscription_count(); } return 0; @@ -119,19 +112,18 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin std::string getTopic() const override { - if (simple_impl_) - { + if (simple_impl_) { return simple_impl_->pub_->get_topic_name(); } return {}; } - // TODO: Ask about this - void publish(const sensor_msgs::msg::PointCloud2& message) const override + // TODO(anyone): Ask about this + void publish(const sensor_msgs::msg::PointCloud2 & message) const override { - if (!simple_impl_ || !simple_impl_->pub_) - { - auto logger = simple_impl_ ? simple_impl_->logger_ : rclcpp::get_logger("point_cloud_transport"); + if (!simple_impl_ || !simple_impl_->pub_) { + auto logger = simple_impl_ ? simple_impl_->logger_ : rclcpp::get_logger( + "point_cloud_transport"); RCLCPP_ERROR( logger, "Call to publish() on an invalid point_cloud_transport::SimplePublisherPlugin"); @@ -150,10 +142,11 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin /** * \brief Encode the given raw pointcloud into a compressed message. * \param[in] raw The input raw pointcloud. - * \return The output shapeshifter holding the compressed cloud message (if encoding succeeds), or an error message. + * \return The output shapeshifter holding the compressed cloud message + * (if encoding succeeds), or an error message. */ virtual TypedEncodeResult encodeTyped( - const sensor_msgs::msg::PointCloud2& raw) const = 0; + const sensor_msgs::msg::PointCloud2 & raw) const = 0; protected: std::string base_topic_; @@ -182,15 +175,17 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin * SimpleSubscriberPlugin to use this function for both normal broadcast publishing and * single subscriber publishing (in subscription callbacks). */ - virtual void publish(const sensor_msgs::msg::PointCloud2& message, const PublishFn& publish_fn) const + virtual void publish( + const sensor_msgs::msg::PointCloud2 & message, + const PublishFn & publish_fn) const { const auto res = this->encodeTyped(message); - if (!res) - { - RCLCPP_ERROR(rclcpp::get_logger("point_cloud_transport"), "Error encoding message by transport %s: %s.", this->getTransportName().c_str(), res.error().c_str()); - } - else if (res.value()) - { + if (!res) { + RCLCPP_ERROR( + rclcpp::get_logger( + "point_cloud_transport"), "Error encoding message by transport %s: %s.", + this->getTransportName().c_str(), res.error().c_str()); + } else if (res.value()) { publish_fn(res.value().value()); } } @@ -239,4 +234,5 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin } }; -} +} // namespace point_cloud_transport +#endif // POINT_CLOUD_TRANSPORT__SIMPLE_PUBLISHER_PLUGIN_HPP_ diff --git a/include/point_cloud_transport/simple_subscriber_plugin.hpp b/include/point_cloud_transport/simple_subscriber_plugin.hpp index ba2eae6..9754855 100644 --- a/include/point_cloud_transport/simple_subscriber_plugin.hpp +++ b/include/point_cloud_transport/simple_subscriber_plugin.hpp @@ -1,44 +1,39 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. - /* - * - * BSD 3-Clause License - * - * Copyright (c) Czech Technical University in Prague + * Copyright (c) 2023, Czech Technical University in Prague * Copyright (c) 2019, paplhjak * Copyright (c) 2009, Willow Garage, Inc. - * - * All rights reserved. + * All rights reserved. * * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: + * modification, are permitted provided that the following conditions are met: * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. */ -#pragma once +#ifndef POINT_CLOUD_TRANSPORT__SIMPLE_SUBSCRIBER_PLUGIN_HPP_ +#define POINT_CLOUD_TRANSPORT__SIMPLE_SUBSCRIBER_PLUGIN_HPP_ + #include #include @@ -80,8 +75,7 @@ class SimpleSubscriberPlugin : public SubscriberPlugin std::string getTopic() const override { - if (impl_) - { + if (impl_) { return impl_->sub_->get_topic_name(); } return {}; @@ -89,8 +83,7 @@ class SimpleSubscriberPlugin : public SubscriberPlugin uint32_t getNumPublishers() const override { - if (impl_) - { + if (impl_) { return impl_->sub_->get_publisher_count(); } return 0; @@ -107,21 +100,21 @@ class SimpleSubscriberPlugin : public SubscriberPlugin * \param[in] config Config of the decompression (if it has any parameters). * \return The raw cloud message (if encoding succeeds), or an error message. */ - virtual DecodeResult decodeTyped(const M& compressed) const = 0; - + virtual DecodeResult decodeTyped(const M & compressed) const = 0; protected: /** * Process a message. Must be implemented by the subclass. */ - virtual void callback(const typename M::ConstPtr& message, const Callback& user_cb) + virtual void callback(const typename M::ConstPtr & message, const Callback & user_cb) { DecodeResult res = this->decodeTyped(*message); - if (!res){ - RCLCPP_ERROR(rclcpp::get_logger("point_cloud_transport"), "Error decoding message by transport %s: %s.", this->getTransportName().c_str(), res.error().c_str()); - } - else if (res.value()) - { + if (!res) { + RCLCPP_ERROR( + rclcpp::get_logger( + "point_cloud_transport"), "Error decoding message by transport %s: %s.", + this->getTransportName().c_str(), res.error().c_str()); + } else if (res.value()) { user_cb(res.value().value()); } } @@ -183,4 +176,5 @@ class SimpleSubscriberPlugin : public SubscriberPlugin std::unique_ptr impl_; }; -} +} // namespace point_cloud_transport +#endif // POINT_CLOUD_TRANSPORT__SIMPLE_SUBSCRIBER_PLUGIN_HPP_ diff --git a/include/point_cloud_transport/single_subscriber_publisher.hpp b/include/point_cloud_transport/single_subscriber_publisher.hpp index 2982fa0..938db47 100644 --- a/include/point_cloud_transport/single_subscriber_publisher.hpp +++ b/include/point_cloud_transport/single_subscriber_publisher.hpp @@ -1,44 +1,38 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. - /* - * - * BSD 3-Clause License - * - * Copyright (c) Czech Technical University in Prague + * Copyright (c) 2023, Czech Technical University in Prague * Copyright (c) 2019, paplhjak * Copyright (c) 2009, Willow Garage, Inc. - * - * All rights reserved. + * All rights reserved. * * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. + * modification, are permitted provided that the following conditions are met: * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. + * * 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. * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. */ -#pragma once +#ifndef POINT_CLOUD_TRANSPORT__SINGLE_SUBSCRIBER_PUBLISHER_HPP_ +#define POINT_CLOUD_TRANSPORT__SINGLE_SUBSCRIBER_PUBLISHER_HPP_ #include #include @@ -49,19 +43,21 @@ namespace point_cloud_transport { -//! Allows publication of a point cloud to a single subscriber. Only available inside subscriber connection callbacks. +//! Allows publication of a point cloud to a single subscriber. +// Only available inside subscriber connection callbacks. class SingleSubscriberPublisher { public: typedef std::function GetNumSubscribersFn; - typedef std::function PublishFn; + typedef std::function PublishFn; SingleSubscriberPublisher(const SingleSubscriberPublisher &) = delete; SingleSubscriberPublisher & operator=(const SingleSubscriberPublisher &) = delete; - SingleSubscriberPublisher(const std::string& caller_id, const std::string& topic, - const GetNumSubscribersFn& num_subscribers_fn, - const PublishFn& publish_fn); + SingleSubscriberPublisher( + const std::string & caller_id, const std::string & topic, + const GetNumSubscribersFn & num_subscribers_fn, + const PublishFn & publish_fn); std::string getSubscriberName() const; @@ -69,8 +65,8 @@ class SingleSubscriberPublisher uint32_t getNumSubscribers() const; - void publish(const sensor_msgs::msg::PointCloud2& message) const; - void publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& message) const; + void publish(const sensor_msgs::msg::PointCloud2 & message) const; + void publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message) const; private: std::string caller_id_; @@ -81,6 +77,6 @@ class SingleSubscriberPublisher friend class Publisher; // to get publish_fn_ directly }; -typedef std::function SubscriberStatusCallback; - -} // namespace point_cloud_transport +typedef std::function SubscriberStatusCallback; +} // namespace point_cloud_transport +#endif // POINT_CLOUD_TRANSPORT__SINGLE_SUBSCRIBER_PUBLISHER_HPP_ diff --git a/include/point_cloud_transport/subscriber.hpp b/include/point_cloud_transport/subscriber.hpp index e20d340..108ff94 100644 --- a/include/point_cloud_transport/subscriber.hpp +++ b/include/point_cloud_transport/subscriber.hpp @@ -1,44 +1,38 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. - /* - * - * BSD 3-Clause License - * - * Copyright (c) Czech Technical University in Prague + * Copyright (c) 2023, Czech Technical University in Prague * Copyright (c) 2019, paplhjak * Copyright (c) 2009, Willow Garage, Inc. - * - * All rights reserved. + * All rights reserved. * * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: + * modification, are permitted provided that the following conditions are met: * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. */ -#pragma once +#ifndef POINT_CLOUD_TRANSPORT__SUBSCRIBER_HPP_ +#define POINT_CLOUD_TRANSPORT__SUBSCRIBER_HPP_ #include #include @@ -53,7 +47,8 @@ #include #include -namespace point_cloud_transport { +namespace point_cloud_transport +{ /** * Manages a subscription callback on a specific topic that can be interpreted @@ -109,28 +104,28 @@ class Subscriber */ void shutdown(); - operator void*() const; + operator void *() const; - bool operator<(const point_cloud_transport::Subscriber& rhs) const + bool operator<(const point_cloud_transport::Subscriber & rhs) const { return impl_ < rhs.impl_; } - bool operator!=(const point_cloud_transport::Subscriber& rhs) const + bool operator!=(const point_cloud_transport::Subscriber & rhs) const { return impl_ != rhs.impl_; } - bool operator==(const point_cloud_transport::Subscriber& rhs) const + bool operator==(const point_cloud_transport::Subscriber & rhs) const { return impl_ == rhs.impl_; } private: - struct Impl; std::shared_ptr impl_; friend class PointCloudTransport; }; -} +} // namespace point_cloud_transport +#endif // POINT_CLOUD_TRANSPORT__SUBSCRIBER_HPP_ diff --git a/include/point_cloud_transport/subscriber_filter.hpp b/include/point_cloud_transport/subscriber_filter.hpp index 391821c..c2bb5b2 100644 --- a/include/point_cloud_transport/subscriber_filter.hpp +++ b/include/point_cloud_transport/subscriber_filter.hpp @@ -1,49 +1,44 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. - /* - * - * BSD 3-Clause License - * - * Copyright (c) Czech Technical University in Prague + * Copyright (c) 2023, Czech Technical University in Prague * Copyright (c) 2019, paplhjak * Copyright (c) 2009, Willow Garage, Inc. - * - * All rights reserved. + * All rights reserved. * * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: + * modification, are permitted provided that the following conditions are met: * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. */ -#pragma once +#ifndef POINT_CLOUD_TRANSPORT__SUBSCRIBER_FILTER_HPP_ +#define POINT_CLOUD_TRANSPORT__SUBSCRIBER_FILTER_HPP_ + +#include #include #include -#include #include #include @@ -80,7 +75,7 @@ class SubscriberFilter : public message_filters::SimpleFilter #include @@ -61,9 +55,10 @@ namespace point_cloud_transport class SubscriberPlugin { public: - //! \brief Result of cloud decoding. Either a `sensor_msgs::msg::PointCloud2` holding the raw message, empty value or - //! error message. - typedef cras::expected, std::string> DecodeResult; + //! \brief Result of cloud decoding. Either a `sensor_msgs::msg::PointCloud2` + //! holding the raw message, empty value or error message. + typedef cras::expected, + std::string> DecodeResult; SubscriberPlugin() = default; SubscriberPlugin(const SubscriberPlugin &) = delete; @@ -141,7 +136,7 @@ class SubscriberPlugin * Return the lookup name of the SubscriberPlugin associated with a specific * transport identifier. */ - static std::string getLookupName(const std::string& transport_type) + static std::string getLookupName(const std::string & transport_type) { return "point_cloud_transport/" + transport_type + "_sub"; } @@ -169,7 +164,6 @@ class SubscriberPlugin "SubscriberPlugin::subscribeImpl with five arguments has not been overridden"); this->subscribeImpl(node, base_topic, callback, custom_qos); } - }; class SingleTopicSubscriberPlugin : public SubscriberPlugin @@ -180,14 +174,15 @@ class SingleTopicSubscriberPlugin : public SubscriberPlugin * * Defaults to \/\. */ - virtual std::string getTopicToSubscribe(const std::string& base_topic) const = 0; + virtual std::string getTopicToSubscribe(const std::string & base_topic) const = 0; /** * Return the datatype of the dynamic reconfigure (as text in the form `package/Config`). - * + * * Return empty string if no reconfiguration is supported. */ virtual std::string getConfigDataType() const = 0; }; -} +} // namespace point_cloud_transport +#endif // POINT_CLOUD_TRANSPORT__SUBSCRIBER_PLUGIN_HPP_ diff --git a/include/point_cloud_transport/tl/expected.hpp b/include/point_cloud_transport/tl/expected.hpp index 7fd6467..d5062d1 100644 --- a/include/point_cloud_transport/tl/expected.hpp +++ b/include/point_cloud_transport/tl/expected.hpp @@ -13,17 +13,19 @@ // . /// -#ifndef TL_EXPECTED_HPP -#define TL_EXPECTED_HPP +#ifndef POINT_CLOUD_TRANSPORT__TL__EXPECTED_HPP_ +#define POINT_CLOUD_TRANSPORT__TL__EXPECTED_HPP_ #define TL_EXPECTED_VERSION_MAJOR 1 #define TL_EXPECTED_VERSION_MINOR 0 #define TL_EXPECTED_VERSION_PATCH 1 +#include #include #include #include #include +#include #if defined(__EXCEPTIONS) || defined(_CPPUNWIND) #define TL_EXPECTED_EXCEPTIONS_ENABLED @@ -36,44 +38,44 @@ #define TL_EXPECTED_MSVC2015_CONSTEXPR constexpr #endif -#if (defined(__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ <= 9 && \ - !defined(__clang__)) +#if (defined(__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ <= 9 && \ + !defined(__clang__)) #define TL_EXPECTED_GCC49 #endif -#if (defined(__GNUC__) && __GNUC__ == 5 && __GNUC_MINOR__ <= 4 && \ - !defined(__clang__)) +#if (defined(__GNUC__) && __GNUC__ == 5 && __GNUC_MINOR__ <= 4 && \ + !defined(__clang__)) #define TL_EXPECTED_GCC54 #endif -#if (defined(__GNUC__) && __GNUC__ == 5 && __GNUC_MINOR__ <= 5 && \ - !defined(__clang__)) +#if (defined(__GNUC__) && __GNUC__ == 5 && __GNUC_MINOR__ <= 5 && \ + !defined(__clang__)) #define TL_EXPECTED_GCC55 #endif #if !defined(TL_ASSERT) -//can't have assert in constexpr in C++11 and GCC 4.9 has a compiler bug +// can't have assert in constexpr in C++11 and GCC 4.9 has a compiler bug #if (__cplusplus > 201103L) && !defined(TL_EXPECTED_GCC49) #include #define TL_ASSERT(x) assert(x) -#else +#else #define TL_ASSERT(x) #endif #endif -#if (defined(__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ <= 9 && \ - !defined(__clang__)) +#if (defined(__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ <= 9 && \ + !defined(__clang__)) // GCC < 5 doesn't support overloading on const&& for member functions #define TL_EXPECTED_NO_CONSTRR // GCC < 5 doesn't support some standard C++11 type traits -#define TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(T) \ +#define TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(T) \ std::has_trivial_copy_constructor -#define TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(T) \ +#define TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(T) \ std::has_trivial_copy_assign // This one will be different for GCC 5.7 if it's ever supported -#define TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(T) \ +#define TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(T) \ std::is_trivially_destructible // GCC 5 < v < 8 has a bug in is_trivially_copy_constructible which breaks @@ -81,31 +83,33 @@ #elif (defined(__GNUC__) && __GNUC__ < 8 && !defined(__clang__)) #ifndef TL_GCC_LESS_8_TRIVIALLY_COPY_CONSTRUCTIBLE_MUTEX #define TL_GCC_LESS_8_TRIVIALLY_COPY_CONSTRUCTIBLE_MUTEX -namespace tl { -namespace detail { -template +namespace tl +{ +namespace detail +{ +template struct is_trivially_copy_constructible - : std::is_trivially_copy_constructible {}; + : std::is_trivially_copy_constructible {}; #ifdef _GLIBCXX_VECTOR -template -struct is_trivially_copy_constructible> : std::false_type {}; +template +struct is_trivially_copy_constructible>: std::false_type {}; #endif -} // namespace detail -} // namespace tl +} // namespace detail +} // namespace tl #endif -#define TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(T) \ +#define TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(T) \ tl::detail::is_trivially_copy_constructible -#define TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(T) \ +#define TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(T) \ std::is_trivially_copy_assignable -#define TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(T) \ +#define TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(T) \ std::is_trivially_destructible #else -#define TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(T) \ +#define TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(T) \ std::is_trivially_copy_constructible -#define TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(T) \ +#define TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(T) \ std::is_trivially_copy_assignable -#define TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(T) \ +#define TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(T) \ std::is_trivially_destructible #endif @@ -119,97 +123,115 @@ struct is_trivially_copy_constructible> : std::false_type {}; #define TL_EXPECTED_GCC49_CONSTEXPR constexpr #endif -#if (__cplusplus == 201103L || defined(TL_EXPECTED_MSVC2015) || \ - defined(TL_EXPECTED_GCC49)) +#if (__cplusplus == 201103L || defined(TL_EXPECTED_MSVC2015) || \ + defined(TL_EXPECTED_GCC49)) #define TL_EXPECTED_11_CONSTEXPR #else #define TL_EXPECTED_11_CONSTEXPR constexpr #endif -namespace tl { -template class expected; +namespace tl +{ +template +class expected; #ifndef TL_MONOSTATE_INPLACE_MUTEX #define TL_MONOSTATE_INPLACE_MUTEX class monostate {}; -struct in_place_t { - explicit in_place_t() = default; +struct in_place_t +{ + in_place_t() = default; }; static constexpr in_place_t in_place{}; #endif -template class unexpected { +template +class unexpected +{ public: static_assert(!std::is_same::value, "E must not be void"); unexpected() = delete; - constexpr explicit unexpected(const E &e) : m_val(e) {} - - constexpr explicit unexpected(E &&e) : m_val(std::move(e)) {} - - template ::value>::type * = nullptr> - constexpr explicit unexpected(Args &&...args) - : m_val(std::forward(args)...) {} - template < - class U, class... Args, - typename std::enable_if &, Args &&...>::value>::type * = nullptr> - constexpr explicit unexpected(std::initializer_list l, Args &&...args) - : m_val(l, std::forward(args)...) {} - - constexpr const E &value() const & { return m_val; } - TL_EXPECTED_11_CONSTEXPR E &value() & { return m_val; } - TL_EXPECTED_11_CONSTEXPR E &&value() && { return std::move(m_val); } - constexpr const E &&value() const && { return std::move(m_val); } + constexpr explicit unexpected(const E & e) + : m_val(e) {} + + constexpr explicit unexpected(E && e) + : m_val(std::move(e)) {} + + template::value>::type * = nullptr> + constexpr explicit unexpected(Args &&... args) + : m_val(std::forward(args)...) {} + template< + class U, class ... Args, + typename std::enable_if &, Args &&...>::value>::type * = nullptr> + constexpr explicit unexpected(std::initializer_list l, Args &&... args) + : m_val(l, std::forward(args)...) {} + + constexpr const E & value() const & {return m_val;} + TL_EXPECTED_11_CONSTEXPR E & value() & {return m_val;} + TL_EXPECTED_11_CONSTEXPR E && value() && {return std::move(m_val);} + constexpr const E && value() const && {return std::move(m_val);} private: E m_val; }; #ifdef __cpp_deduction_guides -template unexpected(E) -> unexpected; +template +unexpected(E)->unexpected; #endif -template -constexpr bool operator==(const unexpected &lhs, const unexpected &rhs) { +template +constexpr bool operator==(const unexpected & lhs, const unexpected & rhs) +{ return lhs.value() == rhs.value(); } -template -constexpr bool operator!=(const unexpected &lhs, const unexpected &rhs) { +template +constexpr bool operator!=(const unexpected & lhs, const unexpected & rhs) +{ return lhs.value() != rhs.value(); } -template -constexpr bool operator<(const unexpected &lhs, const unexpected &rhs) { +template +constexpr bool operator<(const unexpected & lhs, const unexpected & rhs) +{ return lhs.value() < rhs.value(); } -template -constexpr bool operator<=(const unexpected &lhs, const unexpected &rhs) { +template +constexpr bool operator<=(const unexpected & lhs, const unexpected & rhs) +{ return lhs.value() <= rhs.value(); } -template -constexpr bool operator>(const unexpected &lhs, const unexpected &rhs) { +template +constexpr bool operator>(const unexpected & lhs, const unexpected & rhs) +{ return lhs.value() > rhs.value(); } -template -constexpr bool operator>=(const unexpected &lhs, const unexpected &rhs) { +template +constexpr bool operator>=(const unexpected & lhs, const unexpected & rhs) +{ return lhs.value() >= rhs.value(); } -template -unexpected::type> make_unexpected(E &&e) { +template +unexpected::type> make_unexpected(E && e) +{ return unexpected::type>(std::forward(e)); } -struct unexpect_t { +struct unexpect_t +{ unexpect_t() = default; }; static constexpr unexpect_t unexpect{}; -namespace detail { -template -[[noreturn]] TL_EXPECTED_11_CONSTEXPR void throw_exception(E &&e) { +namespace detail +{ +template +[[noreturn]] TL_EXPECTED_11_CONSTEXPR void throw_exception(E && e) +{ #ifdef TL_EXPECTED_EXCEPTIONS_ENABLED throw std::forward(e); #else @@ -225,21 +247,23 @@ template #ifndef TL_TRAITS_MUTEX #define TL_TRAITS_MUTEX // C++14-style aliases for brevity -template using remove_const_t = typename std::remove_const::type; -template +template using remove_const_t = typename std::remove_const::type; +template using remove_reference_t = typename std::remove_reference::type; -template using decay_t = typename std::decay::type; -template +template using decay_t = typename std::decay::type; +template using enable_if_t = typename std::enable_if::type; -template +template using conditional_t = typename std::conditional::type; // std::conjunction from C++17 -template struct conjunction : std::true_type {}; -template struct conjunction : B {}; -template +template +struct conjunction : std::true_type {}; +template +struct conjunction: B {}; +template struct conjunction - : std::conditional, B>::type {}; + : std::conditional, B>::type {}; #if defined(_LIBCPP_VERSION) && __cplusplus == 201103L #define TL_TRAITS_LIBCXX_MEM_FN_WORKAROUND @@ -249,191 +273,202 @@ struct conjunction // which results in a hard-error when using it in a noexcept expression // in some cases. This is a check to workaround the common failing case. #ifdef TL_TRAITS_LIBCXX_MEM_FN_WORKAROUND -template +template struct is_pointer_to_non_const_member_func : std::false_type {}; -template +template struct is_pointer_to_non_const_member_func - : std::true_type {}; -template + : std::true_type {}; +template struct is_pointer_to_non_const_member_func - : std::true_type {}; -template + : std::true_type {}; +template struct is_pointer_to_non_const_member_func - : std::true_type {}; -template + : std::true_type {}; +template struct is_pointer_to_non_const_member_func - : std::true_type {}; -template + : std::true_type {}; +template struct is_pointer_to_non_const_member_func - : std::true_type {}; -template + : std::true_type {}; +template struct is_pointer_to_non_const_member_func - : std::true_type {}; - -template struct is_const_or_const_ref : std::false_type {}; -template struct is_const_or_const_ref : std::true_type {}; -template struct is_const_or_const_ref : std::true_type {}; + : std::true_type {}; + +template +struct is_const_or_const_ref : std::false_type {}; +template +struct is_const_or_const_ref: std::true_type {}; +template +struct is_const_or_const_ref: std::true_type {}; #endif // std::invoke from C++17 // https://stackoverflow.com/questions/38288042/c11-14-invoke-workaround -template < - typename Fn, typename... Args, +template< + typename Fn, typename ... Args, #ifdef TL_TRAITS_LIBCXX_MEM_FN_WORKAROUND - typename = enable_if_t::value && - is_const_or_const_ref::value)>, + typename = enable_if_t::value && + is_const_or_const_ref::value)>, #endif - typename = enable_if_t>::value>, int = 0> -constexpr auto invoke(Fn &&f, Args &&...args) noexcept( - noexcept(std::mem_fn(f)(std::forward(args)...))) - -> decltype(std::mem_fn(f)(std::forward(args)...)) { + typename = enable_if_t>::value>, int = 0> +constexpr auto invoke(Fn && f, Args &&... args) noexcept( + noexcept(std::mem_fn(f)(std::forward(args)...))) +-> decltype(std::mem_fn(f)(std::forward(args)...)) +{ return std::mem_fn(f)(std::forward(args)...); } -template >::value>> -constexpr auto invoke(Fn &&f, Args &&...args) noexcept( - noexcept(std::forward(f)(std::forward(args)...))) - -> decltype(std::forward(f)(std::forward(args)...)) { +template>::value>> +constexpr auto invoke(Fn && f, Args &&... args) noexcept( + noexcept(std::forward(f)(std::forward(args)...))) +-> decltype(std::forward(f)(std::forward(args)...)) +{ return std::forward(f)(std::forward(args)...); } // std::invoke_result from C++17 -template struct invoke_result_impl; +template +struct invoke_result_impl; -template +template struct invoke_result_impl< - F, - decltype(detail::invoke(std::declval(), std::declval()...), void()), - Us...> { + F, + decltype(detail::invoke(std::declval(), std::declval()...), void()), + Us...> +{ using type = - decltype(detail::invoke(std::declval(), std::declval()...)); + decltype(detail::invoke(std::declval(), std::declval()...)); }; -template +template using invoke_result = invoke_result_impl; -template +template using invoke_result_t = typename invoke_result::type; #if defined(_MSC_VER) && _MSC_VER <= 1900 -// TODO make a version which works with MSVC 2015 -template struct is_swappable : std::true_type {}; +// TODO(anyone) make a version which works with MSVC 2015 +template +struct is_swappable : std::true_type {}; -template struct is_nothrow_swappable : std::true_type {}; +template +struct is_nothrow_swappable : std::true_type {}; #else // https://stackoverflow.com/questions/26744589/what-is-a-proper-way-to-implement-is-swappable-to-test-for-the-swappable-concept -namespace swap_adl_tests { +namespace swap_adl_tests +{ // if swap ADL finds this then it would call std::swap otherwise (same // signature) struct tag {}; -template tag swap(T &, T &); -template tag swap(T (&a)[N], T (&b)[N]); +template tag swap(T &, T &); +template tag swap(T(&a)[N], T(&b)[N]); // helper functions to test if an unqualified swap is possible, and if it // becomes std::swap -template std::false_type can_swap(...) noexcept(false); -template (), std::declval()))> -std::true_type can_swap(int) noexcept(noexcept(swap(std::declval(), - std::declval()))); - -template std::false_type uses_std(...); -template +template std::false_type can_swap(...) noexcept(false); +template(), std::declval()))> +std::true_type can_swap(int) noexcept(noexcept(swap(std::declval(), std::declval()))); // NOLINT + +template std::false_type uses_std(...); +template std::is_same(), std::declval())), tag> uses_std(int); -template +template struct is_std_swap_noexcept - : std::integral_constant::value && - std::is_nothrow_move_assignable::value> {}; + : std::integral_constant::value && + std::is_nothrow_move_assignable::value> {}; -template -struct is_std_swap_noexcept : is_std_swap_noexcept {}; +template +struct is_std_swap_noexcept: is_std_swap_noexcept {}; -template +template struct is_adl_swap_noexcept - : std::integral_constant(0))> {}; -} // namespace swap_adl_tests + : std::integral_constant(0))> {}; +} // namespace swap_adl_tests -template +template struct is_swappable - : std::integral_constant< - bool, - decltype(detail::swap_adl_tests::can_swap(0))::value && - (!decltype(detail::swap_adl_tests::uses_std(0))::value || - (std::is_move_assignable::value && - std::is_move_constructible::value))> {}; - -template + : std::integral_constant< + bool, + decltype(detail::swap_adl_tests::can_swap(0))::value && + (!decltype(detail::swap_adl_tests::uses_std(0))::value || + (std::is_move_assignable::value && + std::is_move_constructible::value))> {}; + +template struct is_swappable - : std::integral_constant< - bool, - decltype(detail::swap_adl_tests::can_swap(0))::value && - (!decltype(detail::swap_adl_tests::uses_std( - 0))::value || - is_swappable::value)> {}; - -template + : std::integral_constant< + bool, + decltype(detail::swap_adl_tests::can_swap(0))::value && + (!decltype(detail::swap_adl_tests::uses_std( + 0))::value || + is_swappable::value)> {}; + +template struct is_nothrow_swappable - : std::integral_constant< - bool, - is_swappable::value && - ((decltype(detail::swap_adl_tests::uses_std(0))::value && - detail::swap_adl_tests::is_std_swap_noexcept::value) || - (!decltype(detail::swap_adl_tests::uses_std(0))::value && - detail::swap_adl_tests::is_adl_swap_noexcept::value))> {}; + : std::integral_constant< + bool, + is_swappable::value && + ((decltype(detail::swap_adl_tests::uses_std(0))::value && + detail::swap_adl_tests::is_std_swap_noexcept::value) || + (!decltype(detail::swap_adl_tests::uses_std(0))::value && + detail::swap_adl_tests::is_adl_swap_noexcept::value))> {}; #endif #endif // Trait for checking if a type is a tl::expected -template struct is_expected_impl : std::false_type {}; -template -struct is_expected_impl> : std::true_type {}; -template using is_expected = is_expected_impl>; +template +struct is_expected_impl : std::false_type {}; +template +struct is_expected_impl>: std::true_type {}; +template using is_expected = is_expected_impl>; -template +template using expected_enable_forward_value = detail::enable_if_t< - std::is_constructible::value && - !std::is_same, in_place_t>::value && - !std::is_same, detail::decay_t>::value && - !std::is_same, detail::decay_t>::value>; + std::is_constructible::value && + !std::is_same, in_place_t>::value && + !std::is_same, detail::decay_t>::value && + !std::is_same, detail::decay_t>::value>; -template +template using expected_enable_from_other = detail::enable_if_t< - std::is_constructible::value && - std::is_constructible::value && - !std::is_constructible &>::value && - !std::is_constructible &&>::value && - !std::is_constructible &>::value && - !std::is_constructible &&>::value && - !std::is_convertible &, T>::value && - !std::is_convertible &&, T>::value && - !std::is_convertible &, T>::value && - !std::is_convertible &&, T>::value>; - -template + std::is_constructible::value && + std::is_constructible::value && + !std::is_constructible &>::value && + !std::is_constructible&&>::value && + !std::is_constructible &>::value && + !std::is_constructible&&>::value && + !std::is_convertible &, T>::value && + !std::is_convertible&&, T>::value && + !std::is_convertible &, T>::value && + !std::is_convertible&&, T>::value>; + +template using is_void_or = conditional_t::value, std::true_type, U>; -template +template using is_copy_constructible_or_void = - is_void_or>; + is_void_or>; -template +template using is_move_constructible_or_void = - is_void_or>; + is_void_or>; -template +template using is_copy_assignable_or_void = is_void_or>; -template +template using is_move_assignable_or_void = is_void_or>; -} // namespace detail +} // namespace detail -namespace detail { +namespace detail +{ struct no_init_t {}; static constexpr no_init_t no_init{}; @@ -443,39 +478,45 @@ static constexpr no_init_t no_init{}; // This specialization is for where neither `T` or `E` is trivially // destructible, so the destructors must be called on destruction of the // `expected` -template ::value, - bool = std::is_trivially_destructible::value> -struct expected_storage_base { - constexpr expected_storage_base() : m_val(T{}), m_has_val(true) {} - constexpr expected_storage_base(no_init_t) : m_no_init(), m_has_val(false) {} - - template ::value> * = - nullptr> - constexpr expected_storage_base(in_place_t, Args &&...args) - : m_val(std::forward(args)...), m_has_val(true) {} - - template &, Args &&...>::value> * = nullptr> - constexpr expected_storage_base(in_place_t, std::initializer_list il, - Args &&...args) - : m_val(il, std::forward(args)...), m_has_val(true) {} - template ::value> * = - nullptr> - constexpr explicit expected_storage_base(unexpect_t, Args &&...args) - : m_unexpect(std::forward(args)...), m_has_val(false) {} - - template &, Args &&...>::value> * = nullptr> - constexpr explicit expected_storage_base(unexpect_t, - std::initializer_list il, - Args &&...args) - : m_unexpect(il, std::forward(args)...), m_has_val(false) {} - - ~expected_storage_base() { +template::value, + bool = std::is_trivially_destructible::value> +struct expected_storage_base +{ + constexpr expected_storage_base() + : m_val(T{}), m_has_val(true) {} + constexpr explicit expected_storage_base(no_init_t) + : m_no_init(), m_has_val(false) {} + + template::value> * = + nullptr> + constexpr explicit expected_storage_base(in_place_t, Args &&... args) + : m_val(std::forward(args)...), m_has_val(true) {} + + template &, Args &&...>::value> * = nullptr> + constexpr expected_storage_base( + in_place_t, std::initializer_list il, + Args &&... args) + : m_val(il, std::forward(args)...), m_has_val(true) {} + template::value> * = + nullptr> + constexpr explicit expected_storage_base(unexpect_t, Args &&... args) + : m_unexpect(std::forward(args)...), m_has_val(false) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected_storage_base( + unexpect_t, + std::initializer_list il, + Args &&... args) + : m_unexpect(il, std::forward(args)...), m_has_val(false) {} + + ~expected_storage_base() + { if (m_has_val) { m_val.~T(); } else { @@ -492,35 +533,41 @@ struct expected_storage_base { // This specialization is for when both `T` and `E` are trivially-destructible, // so the destructor of the `expected` can be trivial. -template struct expected_storage_base { - constexpr expected_storage_base() : m_val(T{}), m_has_val(true) {} - constexpr expected_storage_base(no_init_t) : m_no_init(), m_has_val(false) {} - - template ::value> * = - nullptr> - constexpr expected_storage_base(in_place_t, Args &&...args) - : m_val(std::forward(args)...), m_has_val(true) {} - - template &, Args &&...>::value> * = nullptr> - constexpr expected_storage_base(in_place_t, std::initializer_list il, - Args &&...args) - : m_val(il, std::forward(args)...), m_has_val(true) {} - template ::value> * = - nullptr> - constexpr explicit expected_storage_base(unexpect_t, Args &&...args) - : m_unexpect(std::forward(args)...), m_has_val(false) {} - - template &, Args &&...>::value> * = nullptr> - constexpr explicit expected_storage_base(unexpect_t, - std::initializer_list il, - Args &&...args) - : m_unexpect(il, std::forward(args)...), m_has_val(false) {} +template +struct expected_storage_base +{ + constexpr expected_storage_base() + : m_val(T{}), m_has_val(true) {} + constexpr explicit expected_storage_base(no_init_t) + : m_no_init(), m_has_val(false) {} + + template::value> * = + nullptr> + constexpr explicit expected_storage_base(in_place_t, Args &&... args) + : m_val(std::forward(args)...), m_has_val(true) {} + + template &, Args &&...>::value> * = nullptr> + constexpr expected_storage_base( + in_place_t, std::initializer_list il, + Args &&... args) + : m_val(il, std::forward(args)...), m_has_val(true) {} + template::value> * = + nullptr> + constexpr explicit expected_storage_base(unexpect_t, Args &&... args) + : m_unexpect(std::forward(args)...), m_has_val(false) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected_storage_base( + unexpect_t, + std::initializer_list il, + Args &&... args) + : m_unexpect(il, std::forward(args)...), m_has_val(false) {} ~expected_storage_base() = default; union { @@ -532,38 +579,44 @@ template struct expected_storage_base { }; // T is trivial, E is not. -template struct expected_storage_base { - constexpr expected_storage_base() : m_val(T{}), m_has_val(true) {} +template +struct expected_storage_base +{ + constexpr expected_storage_base() + : m_val(T{}), m_has_val(true) {} TL_EXPECTED_MSVC2015_CONSTEXPR expected_storage_base(no_init_t) - : m_no_init(), m_has_val(false) {} - - template ::value> * = - nullptr> - constexpr expected_storage_base(in_place_t, Args &&...args) - : m_val(std::forward(args)...), m_has_val(true) {} - - template &, Args &&...>::value> * = nullptr> - constexpr expected_storage_base(in_place_t, std::initializer_list il, - Args &&...args) - : m_val(il, std::forward(args)...), m_has_val(true) {} - template ::value> * = - nullptr> - constexpr explicit expected_storage_base(unexpect_t, Args &&...args) - : m_unexpect(std::forward(args)...), m_has_val(false) {} - - template &, Args &&...>::value> * = nullptr> - constexpr explicit expected_storage_base(unexpect_t, - std::initializer_list il, - Args &&...args) - : m_unexpect(il, std::forward(args)...), m_has_val(false) {} - - ~expected_storage_base() { + : m_no_init(), m_has_val(false) {} + + template::value> * = + nullptr> + constexpr explicit expected_storage_base(in_place_t, Args &&... args) + : m_val(std::forward(args)...), m_has_val(true) {} + + template &, Args &&...>::value> * = nullptr> + constexpr expected_storage_base( + in_place_t, std::initializer_list il, + Args &&... args) + : m_val(il, std::forward(args)...), m_has_val(true) {} + template::value> * = + nullptr> + constexpr explicit expected_storage_base(unexpect_t, Args &&... args) + : m_unexpect(std::forward(args)...), m_has_val(false) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected_storage_base( + unexpect_t, + std::initializer_list il, + Args &&... args) + : m_unexpect(il, std::forward(args)...), m_has_val(false) {} + + ~expected_storage_base() + { if (!m_has_val) { m_unexpect.~unexpected(); } @@ -578,37 +631,44 @@ template struct expected_storage_base { }; // E is trivial, T is not. -template struct expected_storage_base { - constexpr expected_storage_base() : m_val(T{}), m_has_val(true) {} - constexpr expected_storage_base(no_init_t) : m_no_init(), m_has_val(false) {} - - template ::value> * = - nullptr> - constexpr expected_storage_base(in_place_t, Args &&...args) - : m_val(std::forward(args)...), m_has_val(true) {} - - template &, Args &&...>::value> * = nullptr> - constexpr expected_storage_base(in_place_t, std::initializer_list il, - Args &&...args) - : m_val(il, std::forward(args)...), m_has_val(true) {} - template ::value> * = - nullptr> - constexpr explicit expected_storage_base(unexpect_t, Args &&...args) - : m_unexpect(std::forward(args)...), m_has_val(false) {} - - template &, Args &&...>::value> * = nullptr> - constexpr explicit expected_storage_base(unexpect_t, - std::initializer_list il, - Args &&...args) - : m_unexpect(il, std::forward(args)...), m_has_val(false) {} - - ~expected_storage_base() { +template +struct expected_storage_base +{ + constexpr expected_storage_base() + : m_val(T{}), m_has_val(true) {} + constexpr explicit expected_storage_base(no_init_t) + : m_no_init(), m_has_val(false) {} + + template::value> * = + nullptr> + constexpr explicit expected_storage_base(in_place_t, Args &&... args) + : m_val(std::forward(args)...), m_has_val(true) {} + + template &, Args &&...>::value> * = nullptr> + constexpr expected_storage_base( + in_place_t, std::initializer_list il, + Args &&... args) + : m_val(il, std::forward(args)...), m_has_val(true) {} + template::value> * = + nullptr> + constexpr explicit expected_storage_base(unexpect_t, Args &&... args) + : m_unexpect(std::forward(args)...), m_has_val(false) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected_storage_base( + unexpect_t, + std::initializer_list il, + Args &&... args) + : m_unexpect(il, std::forward(args)...), m_has_val(false) {} + + ~expected_storage_base() + { if (m_has_val) { m_val.~T(); } @@ -622,31 +682,37 @@ template struct expected_storage_base { }; // `T` is `void`, `E` is trivially-destructible -template struct expected_storage_base { +template +struct expected_storage_base +{ #if __GNUC__ <= 5 - //no constexpr for GCC 4/5 bug + // no constexpr for GCC 4/5 bug #else TL_EXPECTED_MSVC2015_CONSTEXPR - #endif - expected_storage_base() : m_has_val(true) {} - - constexpr expected_storage_base(no_init_t) : m_val(), m_has_val(false) {} - - constexpr expected_storage_base(in_place_t) : m_has_val(true) {} - - template ::value> * = - nullptr> - constexpr explicit expected_storage_base(unexpect_t, Args &&...args) - : m_unexpect(std::forward(args)...), m_has_val(false) {} - - template &, Args &&...>::value> * = nullptr> - constexpr explicit expected_storage_base(unexpect_t, - std::initializer_list il, - Args &&...args) - : m_unexpect(il, std::forward(args)...), m_has_val(false) {} + #endif + expected_storage_base() + : m_has_val(true) {} + + constexpr explicit expected_storage_base(no_init_t) + : m_val(), m_has_val(false) {} + + constexpr explicit expected_storage_base(in_place_t) + : m_has_val(true) {} + + template::value> * = + nullptr> + constexpr explicit expected_storage_base(unexpect_t, Args &&... args) + : m_unexpect(std::forward(args)...), m_has_val(false) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected_storage_base( + unexpect_t, + std::initializer_list il, + Args &&... args) + : m_unexpect(il, std::forward(args)...), m_has_val(false) {} ~expected_storage_base() = default; struct dummy {}; @@ -658,27 +724,34 @@ template struct expected_storage_base { }; // `T` is `void`, `E` is not trivially-destructible -template struct expected_storage_base { - constexpr expected_storage_base() : m_dummy(), m_has_val(true) {} - constexpr expected_storage_base(no_init_t) : m_dummy(), m_has_val(false) {} - - constexpr expected_storage_base(in_place_t) : m_dummy(), m_has_val(true) {} - - template ::value> * = - nullptr> - constexpr explicit expected_storage_base(unexpect_t, Args &&...args) - : m_unexpect(std::forward(args)...), m_has_val(false) {} - - template &, Args &&...>::value> * = nullptr> - constexpr explicit expected_storage_base(unexpect_t, - std::initializer_list il, - Args &&...args) - : m_unexpect(il, std::forward(args)...), m_has_val(false) {} - - ~expected_storage_base() { +template +struct expected_storage_base +{ + constexpr expected_storage_base() + : m_dummy(), m_has_val(true) {} + constexpr explicit expected_storage_base(no_init_t) + : m_dummy(), m_has_val(false) {} + + constexpr explicit expected_storage_base(in_place_t) + : m_dummy(), m_has_val(true) {} + + template::value> * = + nullptr> + constexpr explicit expected_storage_base(unexpect_t, Args &&... args) + : m_unexpect(std::forward(args)...), m_has_val(false) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected_storage_base( + unexpect_t, + std::initializer_list il, + Args &&... args) + : m_unexpect(il, std::forward(args)...), m_has_val(false) {} + + ~expected_storage_base() + { if (!m_has_val) { m_unexpect.~unexpected(); } @@ -693,23 +766,27 @@ template struct expected_storage_base { // This base class provides some handy member functions which can be used in // further derived classes -template -struct expected_operations_base : expected_storage_base { +template +struct expected_operations_base : expected_storage_base +{ using expected_storage_base::expected_storage_base; - template void construct(Args &&...args) noexcept { + template void construct(Args &&... args) noexcept + { new (std::addressof(this->m_val)) T(std::forward(args)...); this->m_has_val = true; } - template void construct_with(Rhs &&rhs) noexcept { + template void construct_with(Rhs && rhs) noexcept + { new (std::addressof(this->m_val)) T(std::forward(rhs).get()); this->m_has_val = true; } - template void construct_error(Args &&...args) noexcept { + template void construct_error(Args &&... args) noexcept + { new (std::addressof(this->m_unexpect)) - unexpected(std::forward(args)...); + unexpected(std::forward(args)...); this->m_has_val = false; } @@ -721,10 +798,11 @@ struct expected_operations_base : expected_storage_base { // // This overload handles the case where we can just copy-construct `T` // directly into place without throwing. - template ::value> - * = nullptr> - void assign(const expected_operations_base &rhs) noexcept { + template::value> + * = nullptr> + void assign(const expected_operations_base & rhs) noexcept + { if (!this->m_has_val && rhs.m_has_val) { geterr().~unexpected(); construct(rhs.get()); @@ -735,11 +813,12 @@ struct expected_operations_base : expected_storage_base { // This overload handles the case where we can attempt to create a copy of // `T`, then no-throw move it into place if the copy was successful. - template ::value && - std::is_nothrow_move_constructible::value> - * = nullptr> - void assign(const expected_operations_base &rhs) noexcept { + template::value && + std::is_nothrow_move_constructible::value> + * = nullptr> + void assign(const expected_operations_base & rhs) noexcept + { if (!this->m_has_val && rhs.m_has_val) { T tmp = rhs.get(); geterr().~unexpected(); @@ -754,11 +833,12 @@ struct expected_operations_base : expected_storage_base { // If the construction succeeds, then everything is fine, but if it throws, // then we move the old unexpected value back into place before rethrowing the // exception. - template ::value && - !std::is_nothrow_move_constructible::value> - * = nullptr> - void assign(const expected_operations_base &rhs) { + template::value && + !std::is_nothrow_move_constructible::value> + * = nullptr> + void assign(const expected_operations_base & rhs) + { if (!this->m_has_val && rhs.m_has_val) { auto tmp = std::move(geterr()); geterr().~unexpected(); @@ -779,10 +859,11 @@ struct expected_operations_base : expected_storage_base { } // These overloads do the same as above, but for rvalues - template ::value> - * = nullptr> - void assign(expected_operations_base &&rhs) noexcept { + template::value> + * = nullptr> + void assign(expected_operations_base && rhs) noexcept + { if (!this->m_has_val && rhs.m_has_val) { geterr().~unexpected(); construct(std::move(rhs).get()); @@ -791,10 +872,11 @@ struct expected_operations_base : expected_storage_base { } } - template ::value> - * = nullptr> - void assign(expected_operations_base &&rhs) { + template::value> + * = nullptr> + void assign(expected_operations_base && rhs) + { if (!this->m_has_val && rhs.m_has_val) { auto tmp = std::move(geterr()); geterr().~unexpected(); @@ -816,7 +898,8 @@ struct expected_operations_base : expected_storage_base { #else // If exceptions are disabled then we can just copy-construct - void assign(const expected_operations_base &rhs) noexcept { + void assign(const expected_operations_base & rhs) noexcept + { if (!this->m_has_val && rhs.m_has_val) { geterr().~unexpected(); construct(rhs.get()); @@ -825,7 +908,8 @@ struct expected_operations_base : expected_storage_base { } } - void assign(expected_operations_base &&rhs) noexcept { + void assign(expected_operations_base && rhs) noexcept + { if (!this->m_has_val && rhs.m_has_val) { geterr().~unexpected(); construct(std::move(rhs).get()); @@ -837,7 +921,8 @@ struct expected_operations_base : expected_storage_base { #endif // The common part of move/copy assigning - template void assign_common(Rhs &&rhs) { + template void assign_common(Rhs && rhs) + { if (this->m_has_val) { if (rhs.m_has_val) { get() = std::forward(rhs).get(); @@ -852,52 +937,59 @@ struct expected_operations_base : expected_storage_base { } } - bool has_value() const { return this->m_has_val; } + bool has_value() const {return this->m_has_val;} - TL_EXPECTED_11_CONSTEXPR T &get() & { return this->m_val; } - constexpr const T &get() const & { return this->m_val; } - TL_EXPECTED_11_CONSTEXPR T &&get() && { return std::move(this->m_val); } + TL_EXPECTED_11_CONSTEXPR T & get() & {return this->m_val;} + constexpr const T & get() const & {return this->m_val;} + TL_EXPECTED_11_CONSTEXPR T && get() && {return std::move(this->m_val);} #ifndef TL_EXPECTED_NO_CONSTRR - constexpr const T &&get() const && { return std::move(this->m_val); } + constexpr const T && get() const && {return std::move(this->m_val);} #endif - TL_EXPECTED_11_CONSTEXPR unexpected &geterr() & { + TL_EXPECTED_11_CONSTEXPR unexpected & geterr() & + { return this->m_unexpect; } - constexpr const unexpected &geterr() const & { return this->m_unexpect; } - TL_EXPECTED_11_CONSTEXPR unexpected &&geterr() && { + constexpr const unexpected & geterr() const & {return this->m_unexpect;} + TL_EXPECTED_11_CONSTEXPR unexpected && geterr() && + { return std::move(this->m_unexpect); } #ifndef TL_EXPECTED_NO_CONSTRR - constexpr const unexpected &&geterr() const && { + constexpr const unexpected && geterr() const && + { return std::move(this->m_unexpect); } #endif - TL_EXPECTED_11_CONSTEXPR void destroy_val() { get().~T(); } + TL_EXPECTED_11_CONSTEXPR void destroy_val() {get().~T();} }; // This base class provides some handy member functions which can be used in // further derived classes -template -struct expected_operations_base : expected_storage_base { +template +struct expected_operations_base: expected_storage_base +{ using expected_storage_base::expected_storage_base; - template void construct() noexcept { this->m_has_val = true; } + template void construct() noexcept {this->m_has_val = true;} // This function doesn't use its argument, but needs it so that code in // levels above this can work independently of whether T is void - template void construct_with(Rhs &&) noexcept { + template void construct_with(Rhs &&) noexcept + { this->m_has_val = true; } - template void construct_error(Args &&...args) noexcept { + template void construct_error(Args &&... args) noexcept + { new (std::addressof(this->m_unexpect)) - unexpected(std::forward(args)...); + unexpected(std::forward(args)...); this->m_has_val = false; } - template void assign(Rhs &&rhs) noexcept { + template void assign(Rhs && rhs) noexcept + { if (!this->m_has_val) { if (rhs.m_has_val) { geterr().~unexpected(); @@ -912,43 +1004,50 @@ struct expected_operations_base : expected_storage_base { } } - bool has_value() const { return this->m_has_val; } + bool has_value() const {return this->m_has_val;} - TL_EXPECTED_11_CONSTEXPR unexpected &geterr() & { + TL_EXPECTED_11_CONSTEXPR unexpected & geterr() & + { return this->m_unexpect; } - constexpr const unexpected &geterr() const & { return this->m_unexpect; } - TL_EXPECTED_11_CONSTEXPR unexpected &&geterr() && { + constexpr const unexpected & geterr() const & {return this->m_unexpect;} + TL_EXPECTED_11_CONSTEXPR unexpected && geterr() && + { return std::move(this->m_unexpect); } #ifndef TL_EXPECTED_NO_CONSTRR - constexpr const unexpected &&geterr() const && { + constexpr const unexpected && geterr() const && + { return std::move(this->m_unexpect); } #endif - TL_EXPECTED_11_CONSTEXPR void destroy_val() { + TL_EXPECTED_11_CONSTEXPR void destroy_val() + { // no-op } }; // This class manages conditionally having a trivial copy constructor // This specialization is for when T and E are trivially copy constructible -template :: - value &&TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(E)::value> -struct expected_copy_base : expected_operations_base { +template:: + value && TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(E) ::value> +struct expected_copy_base : expected_operations_base +{ using expected_operations_base::expected_operations_base; }; // This specialization is for when T or E are not trivially copy constructible -template -struct expected_copy_base : expected_operations_base { +template +struct expected_copy_base: expected_operations_base +{ using expected_operations_base::expected_operations_base; expected_copy_base() = default; - expected_copy_base(const expected_copy_base &rhs) - : expected_operations_base(no_init) { + expected_copy_base(const expected_copy_base & rhs) + : expected_operations_base(no_init) + { if (rhs.has_value()) { this->construct_with(rhs); } else { @@ -956,9 +1055,9 @@ struct expected_copy_base : expected_operations_base { } } - expected_copy_base(expected_copy_base &&rhs) = default; - expected_copy_base &operator=(const expected_copy_base &rhs) = default; - expected_copy_base &operator=(expected_copy_base &&rhs) = default; + expected_copy_base(expected_copy_base && rhs) = default; + expected_copy_base & operator=(const expected_copy_base & rhs) = default; + expected_copy_base & operator=(expected_copy_base && rhs) = default; }; // This class manages conditionally having a trivial move constructor @@ -967,62 +1066,69 @@ struct expected_copy_base : expected_operations_base { // have to make do with a non-trivial move constructor even if T is trivially // move constructible #ifndef TL_EXPECTED_GCC49 -template >::value - &&std::is_trivially_move_constructible::value> -struct expected_move_base : expected_copy_base { +template>::value && + std::is_trivially_move_constructible::value> +struct expected_move_base : expected_copy_base +{ using expected_copy_base::expected_copy_base; }; #else -template struct expected_move_base; +template +struct expected_move_base; #endif -template -struct expected_move_base : expected_copy_base { +template +struct expected_move_base: expected_copy_base +{ using expected_copy_base::expected_copy_base; expected_move_base() = default; - expected_move_base(const expected_move_base &rhs) = default; + expected_move_base(const expected_move_base & rhs) = default; - expected_move_base(expected_move_base &&rhs) noexcept( - std::is_nothrow_move_constructible::value) - : expected_copy_base(no_init) { + expected_move_base(expected_move_base && rhs) noexcept( + std::is_nothrow_move_constructible::value) + : expected_copy_base(no_init) + { if (rhs.has_value()) { this->construct_with(std::move(rhs)); } else { this->construct_error(std::move(rhs.geterr())); } } - expected_move_base &operator=(const expected_move_base &rhs) = default; - expected_move_base &operator=(expected_move_base &&rhs) = default; + expected_move_base & operator=(const expected_move_base & rhs) = default; + expected_move_base & operator=(expected_move_base && rhs) = default; }; // This class manages conditionally having a trivial copy assignment operator -template >::value - &&TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(E)::value - &&TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(E)::value - &&TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(E)::value> -struct expected_copy_assign_base : expected_move_base { +template>::value && + TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(E) ::value && + TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(E) ::value && + TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(E) ::value> +struct expected_copy_assign_base : expected_move_base +{ using expected_move_base::expected_move_base; }; -template -struct expected_copy_assign_base : expected_move_base { +template +struct expected_copy_assign_base: expected_move_base +{ using expected_move_base::expected_move_base; expected_copy_assign_base() = default; - expected_copy_assign_base(const expected_copy_assign_base &rhs) = default; + expected_copy_assign_base(const expected_copy_assign_base & rhs) = default; - expected_copy_assign_base(expected_copy_assign_base &&rhs) = default; - expected_copy_assign_base &operator=(const expected_copy_assign_base &rhs) { + expected_copy_assign_base(expected_copy_assign_base && rhs) = default; + expected_copy_assign_base & operator=(const expected_copy_assign_base & rhs) + { this->assign(rhs); return *this; } expected_copy_assign_base & - operator=(expected_copy_assign_base &&rhs) = default; + operator=(expected_copy_assign_base && rhs) = default; }; // This class manages conditionally having a trivial move assignment operator @@ -1031,38 +1137,42 @@ struct expected_copy_assign_base : expected_move_base { // to make do with a non-trivial move assignment operator even if T is trivially // move assignable #ifndef TL_EXPECTED_GCC49 -template , - std::is_trivially_move_constructible, - std::is_trivially_move_assignable>>:: - value &&std::is_trivially_destructible::value - &&std::is_trivially_move_constructible::value - &&std::is_trivially_move_assignable::value> -struct expected_move_assign_base : expected_copy_assign_base { +template, + std::is_trivially_move_constructible, + std::is_trivially_move_assignable>>:: + value && std::is_trivially_destructible::value && + std::is_trivially_move_constructible::value && + std::is_trivially_move_assignable::value> +struct expected_move_assign_base : expected_copy_assign_base +{ using expected_copy_assign_base::expected_copy_assign_base; }; #else -template struct expected_move_assign_base; +template +struct expected_move_assign_base; #endif -template +template struct expected_move_assign_base - : expected_copy_assign_base { + : expected_copy_assign_base +{ using expected_copy_assign_base::expected_copy_assign_base; expected_move_assign_base() = default; - expected_move_assign_base(const expected_move_assign_base &rhs) = default; + expected_move_assign_base(const expected_move_assign_base & rhs) = default; - expected_move_assign_base(expected_move_assign_base &&rhs) = default; + expected_move_assign_base(expected_move_assign_base && rhs) = default; expected_move_assign_base & - operator=(const expected_move_assign_base &rhs) = default; + operator=(const expected_move_assign_base & rhs) = default; expected_move_assign_base & - operator=(expected_move_assign_base &&rhs) noexcept( - std::is_nothrow_move_constructible::value - &&std::is_nothrow_move_assignable::value) { + operator=(expected_move_assign_base && rhs) noexcept( + std::is_nothrow_move_constructible::value && + std::is_nothrow_move_assignable::value) + { this->assign(std::move(rhs)); return *this; } @@ -1070,12 +1180,13 @@ struct expected_move_assign_base // expected_delete_ctor_base will conditionally delete copy and move // constructors depending on whether T is copy/move constructible -template ::value && - std::is_copy_constructible::value), - bool EnableMove = (is_move_constructible_or_void::value && - std::is_move_constructible::value)> -struct expected_delete_ctor_base { +template::value && + std::is_copy_constructible::value), + bool EnableMove = (is_move_constructible_or_void::value && + std::is_move_constructible::value)> +struct expected_delete_ctor_base +{ expected_delete_ctor_base() = default; expected_delete_ctor_base(const expected_delete_ctor_base &) = default; expected_delete_ctor_base(expected_delete_ctor_base &&) noexcept = default; @@ -1085,8 +1196,9 @@ struct expected_delete_ctor_base { operator=(expected_delete_ctor_base &&) noexcept = default; }; -template -struct expected_delete_ctor_base { +template +struct expected_delete_ctor_base +{ expected_delete_ctor_base() = default; expected_delete_ctor_base(const expected_delete_ctor_base &) = default; expected_delete_ctor_base(expected_delete_ctor_base &&) noexcept = delete; @@ -1096,8 +1208,9 @@ struct expected_delete_ctor_base { operator=(expected_delete_ctor_base &&) noexcept = default; }; -template -struct expected_delete_ctor_base { +template +struct expected_delete_ctor_base +{ expected_delete_ctor_base() = default; expected_delete_ctor_base(const expected_delete_ctor_base &) = delete; expected_delete_ctor_base(expected_delete_ctor_base &&) noexcept = default; @@ -1107,8 +1220,9 @@ struct expected_delete_ctor_base { operator=(expected_delete_ctor_base &&) noexcept = default; }; -template -struct expected_delete_ctor_base { +template +struct expected_delete_ctor_base +{ expected_delete_ctor_base() = default; expected_delete_ctor_base(const expected_delete_ctor_base &) = delete; expected_delete_ctor_base(expected_delete_ctor_base &&) noexcept = delete; @@ -1121,56 +1235,60 @@ struct expected_delete_ctor_base { // expected_delete_assign_base will conditionally delete copy and move // constructors depending on whether T and E are copy/move constructible + // assignable -template ::value && - std::is_copy_constructible::value && - is_copy_assignable_or_void::value && - std::is_copy_assignable::value), - bool EnableMove = (is_move_constructible_or_void::value && - std::is_move_constructible::value && - is_move_assignable_or_void::value && - std::is_move_assignable::value)> -struct expected_delete_assign_base { +template::value && + std::is_copy_constructible::value && + is_copy_assignable_or_void::value && + std::is_copy_assignable::value), + bool EnableMove = (is_move_constructible_or_void::value && + std::is_move_constructible::value && + is_move_assignable_or_void::value && + std::is_move_assignable::value)> +struct expected_delete_assign_base +{ expected_delete_assign_base() = default; expected_delete_assign_base(const expected_delete_assign_base &) = default; expected_delete_assign_base(expected_delete_assign_base &&) noexcept = - default; + default; expected_delete_assign_base & operator=(const expected_delete_assign_base &) = default; expected_delete_assign_base & operator=(expected_delete_assign_base &&) noexcept = default; }; -template -struct expected_delete_assign_base { +template +struct expected_delete_assign_base +{ expected_delete_assign_base() = default; expected_delete_assign_base(const expected_delete_assign_base &) = default; expected_delete_assign_base(expected_delete_assign_base &&) noexcept = - default; + default; expected_delete_assign_base & operator=(const expected_delete_assign_base &) = default; expected_delete_assign_base & operator=(expected_delete_assign_base &&) noexcept = delete; }; -template -struct expected_delete_assign_base { +template +struct expected_delete_assign_base +{ expected_delete_assign_base() = default; expected_delete_assign_base(const expected_delete_assign_base &) = default; expected_delete_assign_base(expected_delete_assign_base &&) noexcept = - default; + default; expected_delete_assign_base & operator=(const expected_delete_assign_base &) = delete; expected_delete_assign_base & operator=(expected_delete_assign_base &&) noexcept = default; }; -template -struct expected_delete_assign_base { +template +struct expected_delete_assign_base +{ expected_delete_assign_base() = default; expected_delete_assign_base(const expected_delete_assign_base &) = default; expected_delete_assign_base(expected_delete_assign_base &&) noexcept = - default; + default; expected_delete_assign_base & operator=(const expected_delete_assign_base &) = delete; expected_delete_assign_base & @@ -1179,22 +1297,24 @@ struct expected_delete_assign_base { // This is needed to be able to construct the expected_default_ctor_base which // follows, while still conditionally deleting the default constructor. -struct default_constructor_tag { - explicit constexpr default_constructor_tag() = default; +struct default_constructor_tag +{ + constexpr default_constructor_tag() = default; }; // expected_default_ctor_base will ensure that expected has a deleted default // consturctor if T is not default constructible. // This specialization is for when T is default constructible -template ::value || std::is_void::value> -struct expected_default_ctor_base { +template::value || std::is_void::value> +struct expected_default_ctor_base +{ constexpr expected_default_ctor_base() noexcept = default; constexpr expected_default_ctor_base( - expected_default_ctor_base const &) noexcept = default; + expected_default_ctor_base const &) noexcept = default; constexpr expected_default_ctor_base(expected_default_ctor_base &&) noexcept = - default; + default; expected_default_ctor_base & operator=(expected_default_ctor_base const &) noexcept = default; expected_default_ctor_base & @@ -1204,12 +1324,14 @@ struct expected_default_ctor_base { }; // This specialization is for when T is not default constructible -template struct expected_default_ctor_base { +template +struct expected_default_ctor_base +{ constexpr expected_default_ctor_base() noexcept = delete; constexpr expected_default_ctor_base( - expected_default_ctor_base const &) noexcept = default; + expected_default_ctor_base const &) noexcept = default; constexpr expected_default_ctor_base(expected_default_ctor_base &&) noexcept = - default; + default; expected_default_ctor_base & operator=(expected_default_ctor_base const &) noexcept = default; expected_default_ctor_base & @@ -1217,20 +1339,24 @@ template struct expected_default_ctor_base { constexpr explicit expected_default_ctor_base(default_constructor_tag) {} }; -} // namespace detail +} // namespace detail -template class bad_expected_access : public std::exception { +template +class bad_expected_access : public std::exception +{ public: - explicit bad_expected_access(E e) : m_val(std::move(e)) {} + explicit bad_expected_access(E e) + : m_val(std::move(e)) {} - virtual const char *what() const noexcept override { + const char * what() const noexcept override + { return "Bad expected access"; } - const E &error() const & { return m_val; } - E &error() & { return m_val; } - const E &&error() const && { return std::move(m_val); } - E &&error() && { return std::move(m_val); } + const E & error() const & {return m_val;} + E & error() & {return m_val;} + const E && error() const && {return std::move(m_val);} + E && error() && {return std::move(m_val);} private: E m_val; @@ -1243,41 +1369,47 @@ template class bad_expected_access : public std::exception { /// has been initialized, and may not be destroyed before the expected object /// has been destroyed. The initialization state of the contained object is /// tracked by the expected object. -template +template class expected : private detail::expected_move_assign_base, - private detail::expected_delete_ctor_base, - private detail::expected_delete_assign_base, - private detail::expected_default_ctor_base { + private detail::expected_delete_ctor_base, + private detail::expected_delete_assign_base, + private detail::expected_default_ctor_base +{ static_assert(!std::is_reference::value, "T must not be a reference"); - static_assert(!std::is_same::type>::value, - "T must not be in_place_t"); - static_assert(!std::is_same::type>::value, - "T must not be unexpect_t"); static_assert( - !std::is_same>::type>::value, - "T must not be unexpected"); + !std::is_same::type>::value, + "T must not be in_place_t"); + static_assert( + !std::is_same::type>::value, + "T must not be unexpect_t"); + static_assert( + !std::is_same>::type>::value, + "T must not be unexpected"); static_assert(!std::is_reference::value, "E must not be a reference"); - T *valptr() { return std::addressof(this->m_val); } - const T *valptr() const { return std::addressof(this->m_val); } - unexpected *errptr() { return std::addressof(this->m_unexpect); } - const unexpected *errptr() const { + T * valptr() {return std::addressof(this->m_val);} + const T * valptr() const {return std::addressof(this->m_val);} + unexpected * errptr() {return std::addressof(this->m_unexpect);} + const unexpected * errptr() const + { return std::addressof(this->m_unexpect); } - template ::value> * = nullptr> - TL_EXPECTED_11_CONSTEXPR U &val() { + template::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR U & val() + { return this->m_val; } - TL_EXPECTED_11_CONSTEXPR unexpected &err() { return this->m_unexpect; } + TL_EXPECTED_11_CONSTEXPR unexpected & err() {return this->m_unexpect;} - template ::value> * = nullptr> - constexpr const U &val() const { + template::value> * = nullptr> + constexpr const U & val() const + { return this->m_val; } - constexpr const unexpected &err() const { return this->m_unexpect; } + constexpr const unexpected & err() const {return this->m_unexpect;} using impl_base = detail::expected_move_assign_base; using ctor_base = detail::expected_default_ctor_base; @@ -1287,322 +1419,368 @@ class expected : private detail::expected_move_assign_base, typedef E error_type; typedef unexpected unexpected_type; -#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ - !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) - template TL_EXPECTED_11_CONSTEXPR auto and_then(F &&f) & { +#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ + !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) + template TL_EXPECTED_11_CONSTEXPR auto and_then(F && f) & + { return and_then_impl(*this, std::forward(f)); } - template TL_EXPECTED_11_CONSTEXPR auto and_then(F &&f) && { + template TL_EXPECTED_11_CONSTEXPR auto and_then(F && f) && + { return and_then_impl(std::move(*this), std::forward(f)); } - template constexpr auto and_then(F &&f) const & { + template constexpr auto and_then(F && f) const & + { return and_then_impl(*this, std::forward(f)); } #ifndef TL_EXPECTED_NO_CONSTRR - template constexpr auto and_then(F &&f) const && { + template constexpr auto and_then(F && f) const && + { return and_then_impl(std::move(*this), std::forward(f)); } #endif #else - template + template TL_EXPECTED_11_CONSTEXPR auto - and_then(F &&f) & -> decltype(and_then_impl(std::declval(), - std::forward(f))) { + and_then(F && f) &->decltype(and_then_impl( + std::declval(), + std::forward(f))) + { return and_then_impl(*this, std::forward(f)); } - template + template TL_EXPECTED_11_CONSTEXPR auto - and_then(F &&f) && -> decltype(and_then_impl(std::declval(), - std::forward(f))) { + and_then(F && f) &&->decltype(and_then_impl( + std::declval(), + std::forward(f))) + { return and_then_impl(std::move(*this), std::forward(f)); } - template - constexpr auto and_then(F &&f) const & -> decltype(and_then_impl( - std::declval(), std::forward(f))) { + template + constexpr auto and_then(F && f) const & ->decltype(and_then_impl( + std::declval(), std::forward(f))) + { return and_then_impl(*this, std::forward(f)); } #ifndef TL_EXPECTED_NO_CONSTRR - template - constexpr auto and_then(F &&f) const && -> decltype(and_then_impl( - std::declval(), std::forward(f))) { + template + constexpr auto and_then(F && f) const && ->decltype(and_then_impl( + std::declval(), std::forward(f))) + { return and_then_impl(std::move(*this), std::forward(f)); } #endif #endif -#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ - !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) - template TL_EXPECTED_11_CONSTEXPR auto map(F &&f) & { +#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ + !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) + template TL_EXPECTED_11_CONSTEXPR auto map(F && f) & + { return expected_map_impl(*this, std::forward(f)); } - template TL_EXPECTED_11_CONSTEXPR auto map(F &&f) && { + template TL_EXPECTED_11_CONSTEXPR auto map(F && f) && + { return expected_map_impl(std::move(*this), std::forward(f)); } - template constexpr auto map(F &&f) const & { + template constexpr auto map(F && f) const & + { return expected_map_impl(*this, std::forward(f)); } - template constexpr auto map(F &&f) const && { + template constexpr auto map(F && f) const && + { return expected_map_impl(std::move(*this), std::forward(f)); } #else - template + template TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl( - std::declval(), std::declval())) - map(F &&f) & { + std::declval(), std::declval())) + map(F && f) & { return expected_map_impl(*this, std::forward(f)); } - template - TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl(std::declval(), - std::declval())) - map(F &&f) && { + template + TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl( + std::declval(), + std::declval())) + map(F && f) && { return expected_map_impl(std::move(*this), std::forward(f)); } - template - constexpr decltype(expected_map_impl(std::declval(), - std::declval())) - map(F &&f) const & { + template + constexpr decltype(expected_map_impl( + std::declval(), + std::declval())) + map(F && f) const & { return expected_map_impl(*this, std::forward(f)); } #ifndef TL_EXPECTED_NO_CONSTRR - template - constexpr decltype(expected_map_impl(std::declval(), - std::declval())) - map(F &&f) const && { + template + constexpr decltype(expected_map_impl( + std::declval(), + std::declval())) + map(F && f) const && { return expected_map_impl(std::move(*this), std::forward(f)); } #endif #endif -#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ - !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) - template TL_EXPECTED_11_CONSTEXPR auto transform(F &&f) & { +#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ + !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) + template TL_EXPECTED_11_CONSTEXPR auto transform(F && f) & + { return expected_map_impl(*this, std::forward(f)); } - template TL_EXPECTED_11_CONSTEXPR auto transform(F &&f) && { + template TL_EXPECTED_11_CONSTEXPR auto transform(F && f) && + { return expected_map_impl(std::move(*this), std::forward(f)); } - template constexpr auto transform(F &&f) const & { + template constexpr auto transform(F && f) const & + { return expected_map_impl(*this, std::forward(f)); } - template constexpr auto transform(F &&f) const && { + template constexpr auto transform(F && f) const && + { return expected_map_impl(std::move(*this), std::forward(f)); } #else - template + template TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl( - std::declval(), std::declval())) - transform(F &&f) & { + std::declval(), std::declval())) + transform(F && f) & { return expected_map_impl(*this, std::forward(f)); } - template - TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl(std::declval(), - std::declval())) - transform(F &&f) && { + template + TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl( + std::declval(), + std::declval())) + transform(F && f) && { return expected_map_impl(std::move(*this), std::forward(f)); } - template - constexpr decltype(expected_map_impl(std::declval(), - std::declval())) - transform(F &&f) const & { + template + constexpr decltype(expected_map_impl( + std::declval(), + std::declval())) + transform(F && f) const & { return expected_map_impl(*this, std::forward(f)); } #ifndef TL_EXPECTED_NO_CONSTRR - template - constexpr decltype(expected_map_impl(std::declval(), - std::declval())) - transform(F &&f) const && { + template + constexpr decltype(expected_map_impl( + std::declval(), + std::declval())) + transform(F && f) const && { return expected_map_impl(std::move(*this), std::forward(f)); } #endif #endif -#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ - !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) - template TL_EXPECTED_11_CONSTEXPR auto map_error(F &&f) & { +#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ + !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) + template TL_EXPECTED_11_CONSTEXPR auto map_error(F && f) & + { return map_error_impl(*this, std::forward(f)); } - template TL_EXPECTED_11_CONSTEXPR auto map_error(F &&f) && { + template TL_EXPECTED_11_CONSTEXPR auto map_error(F && f) && + { return map_error_impl(std::move(*this), std::forward(f)); } - template constexpr auto map_error(F &&f) const & { + template constexpr auto map_error(F && f) const & + { return map_error_impl(*this, std::forward(f)); } - template constexpr auto map_error(F &&f) const && { + template constexpr auto map_error(F && f) const && + { return map_error_impl(std::move(*this), std::forward(f)); } #else - template - TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl(std::declval(), - std::declval())) - map_error(F &&f) & { + template + TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl( + std::declval(), + std::declval())) + map_error(F && f) & { return map_error_impl(*this, std::forward(f)); } - template - TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl(std::declval(), - std::declval())) - map_error(F &&f) && { + template + TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl( + std::declval(), + std::declval())) + map_error(F && f) && { return map_error_impl(std::move(*this), std::forward(f)); } - template - constexpr decltype(map_error_impl(std::declval(), - std::declval())) - map_error(F &&f) const & { + template + constexpr decltype(map_error_impl( + std::declval(), + std::declval())) + map_error(F && f) const & { return map_error_impl(*this, std::forward(f)); } #ifndef TL_EXPECTED_NO_CONSTRR - template - constexpr decltype(map_error_impl(std::declval(), - std::declval())) - map_error(F &&f) const && { + template + constexpr decltype(map_error_impl( + std::declval(), + std::declval())) + map_error(F && f) const && { return map_error_impl(std::move(*this), std::forward(f)); } #endif #endif -#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ - !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) - template TL_EXPECTED_11_CONSTEXPR auto transform_error(F &&f) & { +#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ + !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) + template TL_EXPECTED_11_CONSTEXPR auto transform_error(F && f) & + { return map_error_impl(*this, std::forward(f)); } - template TL_EXPECTED_11_CONSTEXPR auto transform_error(F &&f) && { + template TL_EXPECTED_11_CONSTEXPR auto transform_error(F && f) && + { return map_error_impl(std::move(*this), std::forward(f)); } - template constexpr auto transform_error(F &&f) const & { + template constexpr auto transform_error(F && f) const & + { return map_error_impl(*this, std::forward(f)); } - template constexpr auto transform_error(F &&f) const && { + template constexpr auto transform_error(F && f) const && + { return map_error_impl(std::move(*this), std::forward(f)); } #else - template - TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl(std::declval(), - std::declval())) - transform_error(F &&f) & { + template + TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl( + std::declval(), + std::declval())) + transform_error(F && f) & { return map_error_impl(*this, std::forward(f)); } - template - TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl(std::declval(), - std::declval())) - transform_error(F &&f) && { + template + TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl( + std::declval(), + std::declval())) + transform_error(F && f) && { return map_error_impl(std::move(*this), std::forward(f)); } - template - constexpr decltype(map_error_impl(std::declval(), - std::declval())) - transform_error(F &&f) const & { + template + constexpr decltype(map_error_impl( + std::declval(), + std::declval())) + transform_error(F && f) const & { return map_error_impl(*this, std::forward(f)); } #ifndef TL_EXPECTED_NO_CONSTRR - template - constexpr decltype(map_error_impl(std::declval(), - std::declval())) - transform_error(F &&f) const && { + template + constexpr decltype(map_error_impl( + std::declval(), + std::declval())) + transform_error(F && f) const && { return map_error_impl(std::move(*this), std::forward(f)); } #endif #endif - template expected TL_EXPECTED_11_CONSTEXPR or_else(F &&f) & { + template expected TL_EXPECTED_11_CONSTEXPR or_else(F && f) & + { return or_else_impl(*this, std::forward(f)); } - template expected TL_EXPECTED_11_CONSTEXPR or_else(F &&f) && { + template expected TL_EXPECTED_11_CONSTEXPR or_else(F && f) && + { return or_else_impl(std::move(*this), std::forward(f)); } - template expected constexpr or_else(F &&f) const & { + template expected constexpr or_else(F && f) const & + { return or_else_impl(*this, std::forward(f)); } #ifndef TL_EXPECTED_NO_CONSTRR - template expected constexpr or_else(F &&f) const && { + template expected constexpr or_else(F && f) const && + { return or_else_impl(std::move(*this), std::forward(f)); } #endif constexpr expected() = default; - constexpr expected(const expected &rhs) = default; - constexpr expected(expected &&rhs) = default; - expected &operator=(const expected &rhs) = default; - expected &operator=(expected &&rhs) = default; - - template ::value> * = - nullptr> - constexpr expected(in_place_t, Args &&...args) - : impl_base(in_place, std::forward(args)...), - ctor_base(detail::default_constructor_tag{}) {} - - template &, Args &&...>::value> * = nullptr> - constexpr expected(in_place_t, std::initializer_list il, Args &&...args) - : impl_base(in_place, il, std::forward(args)...), - ctor_base(detail::default_constructor_tag{}) {} - - template ::value> * = - nullptr, - detail::enable_if_t::value> * = - nullptr> - explicit constexpr expected(const unexpected &e) - : impl_base(unexpect, e.value()), - ctor_base(detail::default_constructor_tag{}) {} - - template < - class G = E, - detail::enable_if_t::value> * = - nullptr, - detail::enable_if_t::value> * = nullptr> - constexpr expected(unexpected const &e) - : impl_base(unexpect, e.value()), - ctor_base(detail::default_constructor_tag{}) {} - - template < - class G = E, - detail::enable_if_t::value> * = nullptr, - detail::enable_if_t::value> * = nullptr> - explicit constexpr expected(unexpected &&e) noexcept( - std::is_nothrow_constructible::value) - : impl_base(unexpect, std::move(e.value())), - ctor_base(detail::default_constructor_tag{}) {} - - template < - class G = E, - detail::enable_if_t::value> * = nullptr, - detail::enable_if_t::value> * = nullptr> - constexpr expected(unexpected &&e) noexcept( - std::is_nothrow_constructible::value) - : impl_base(unexpect, std::move(e.value())), - ctor_base(detail::default_constructor_tag{}) {} - - template ::value> * = - nullptr> - constexpr explicit expected(unexpect_t, Args &&...args) - : impl_base(unexpect, std::forward(args)...), - ctor_base(detail::default_constructor_tag{}) {} - - template &, Args &&...>::value> * = nullptr> - constexpr explicit expected(unexpect_t, std::initializer_list il, - Args &&...args) - : impl_base(unexpect, il, std::forward(args)...), - ctor_base(detail::default_constructor_tag{}) {} - - template ::value && - std::is_convertible::value)> * = - nullptr, - detail::expected_enable_from_other - * = nullptr> - explicit TL_EXPECTED_11_CONSTEXPR expected(const expected &rhs) - : ctor_base(detail::default_constructor_tag{}) { + constexpr expected(const expected & rhs) = default; + constexpr expected(expected && rhs) = default; + expected & operator=(const expected & rhs) = default; + expected & operator=(expected && rhs) = default; + + template::value> * = + nullptr> + constexpr explicit expected(in_place_t, Args &&... args) + : impl_base(in_place, std::forward(args)...), + ctor_base(detail::default_constructor_tag{}) {} + + template &, Args &&...>::value> * = nullptr> + constexpr expected(in_place_t, std::initializer_list il, Args &&... args) + : impl_base(in_place, il, std::forward(args)...), + ctor_base(detail::default_constructor_tag{}) {} + + template::value> * = + nullptr, + detail::enable_if_t::value> * = + nullptr> + explicit constexpr expected(const unexpected & e) + : impl_base(unexpect, e.value()), + ctor_base(detail::default_constructor_tag{}) {} + + template< + class G = E, + detail::enable_if_t::value> * = + nullptr, + detail::enable_if_t::value> * = nullptr> + constexpr explicit expected(unexpected const & e) + : impl_base(unexpect, e.value()), + ctor_base(detail::default_constructor_tag{}) {} + + template< + class G = E, + detail::enable_if_t::value> * = nullptr, + detail::enable_if_t::value> * = nullptr> + explicit constexpr expected(unexpected && e) noexcept( + std::is_nothrow_constructible::value) + : impl_base(unexpect, std::move(e.value())), + ctor_base(detail::default_constructor_tag{}) {} + + template< + class G = E, + detail::enable_if_t::value> * = nullptr, + detail::enable_if_t::value> * = nullptr> + constexpr explicit expected(unexpected && e) noexcept( + std::is_nothrow_constructible::value) + : impl_base(unexpect, std::move(e.value())), + ctor_base(detail::default_constructor_tag{}) {} + + template::value> * = + nullptr> + constexpr explicit expected(unexpect_t, Args &&... args) + : impl_base(unexpect, std::forward(args)...), + ctor_base(detail::default_constructor_tag{}) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected( + unexpect_t, std::initializer_list il, + Args &&... args) + : impl_base(unexpect, il, std::forward(args)...), + ctor_base(detail::default_constructor_tag{}) {} + + template::value && + std::is_convertible::value)> * = + nullptr, + detail::expected_enable_from_other + * = nullptr> + explicit TL_EXPECTED_11_CONSTEXPR expected(const expected & rhs) + : ctor_base(detail::default_constructor_tag{}) + { if (rhs.has_value()) { this->construct(*rhs); } else { @@ -1610,14 +1788,15 @@ class expected : private detail::expected_move_assign_base, } } - template ::value && - std::is_convertible::value)> * = - nullptr, - detail::expected_enable_from_other - * = nullptr> - TL_EXPECTED_11_CONSTEXPR expected(const expected &rhs) - : ctor_base(detail::default_constructor_tag{}) { + template::value && + std::is_convertible::value)> * = + nullptr, + detail::expected_enable_from_other + * = nullptr> + TL_EXPECTED_11_CONSTEXPR expected(const expected & rhs) + : ctor_base(detail::default_constructor_tag{}) + { if (rhs.has_value()) { this->construct(*rhs); } else { @@ -1625,13 +1804,14 @@ class expected : private detail::expected_move_assign_base, } } - template < - class U, class G, - detail::enable_if_t::value && - std::is_convertible::value)> * = nullptr, - detail::expected_enable_from_other * = nullptr> - explicit TL_EXPECTED_11_CONSTEXPR expected(expected &&rhs) - : ctor_base(detail::default_constructor_tag{}) { + template< + class U, class G, + detail::enable_if_t::value && + std::is_convertible::value)> * = nullptr, + detail::expected_enable_from_other * = nullptr> + explicit TL_EXPECTED_11_CONSTEXPR expected(expected && rhs) + : ctor_base(detail::default_constructor_tag{}) + { if (rhs.has_value()) { this->construct(std::move(*rhs)); } else { @@ -1639,13 +1819,14 @@ class expected : private detail::expected_move_assign_base, } } - template < - class U, class G, - detail::enable_if_t<(std::is_convertible::value && - std::is_convertible::value)> * = nullptr, - detail::expected_enable_from_other * = nullptr> - TL_EXPECTED_11_CONSTEXPR expected(expected &&rhs) - : ctor_base(detail::default_constructor_tag{}) { + template< + class U, class G, + detail::enable_if_t<(std::is_convertible::value && + std::is_convertible::value)> * = nullptr, + detail::expected_enable_from_other * = nullptr> + TL_EXPECTED_11_CONSTEXPR expected(expected && rhs) + : ctor_base(detail::default_constructor_tag{}) + { if (rhs.has_value()) { this->construct(std::move(*rhs)); } else { @@ -1653,33 +1834,34 @@ class expected : private detail::expected_move_assign_base, } } - template < - class U = T, - detail::enable_if_t::value> * = nullptr, - detail::expected_enable_forward_value * = nullptr> - explicit TL_EXPECTED_MSVC2015_CONSTEXPR expected(U &&v) - : expected(in_place, std::forward(v)) {} - - template < - class U = T, - detail::enable_if_t::value> * = nullptr, - detail::expected_enable_forward_value * = nullptr> - TL_EXPECTED_MSVC2015_CONSTEXPR expected(U &&v) - : expected(in_place, std::forward(v)) {} - - template < - class U = T, class G = T, - detail::enable_if_t::value> * = - nullptr, - detail::enable_if_t::value> * = nullptr, - detail::enable_if_t< - (!std::is_same, detail::decay_t>::value && - !detail::conjunction, - std::is_same>>::value && - std::is_constructible::value && - std::is_assignable::value && - std::is_nothrow_move_constructible::value)> * = nullptr> - expected &operator=(U &&v) { + template< + class U = T, + detail::enable_if_t::value> * = nullptr, + detail::expected_enable_forward_value * = nullptr> + explicit TL_EXPECTED_MSVC2015_CONSTEXPR expected(U && v) + : expected(in_place, std::forward(v)) {} + + template< + class U = T, + detail::enable_if_t::value> * = nullptr, + detail::expected_enable_forward_value * = nullptr> + TL_EXPECTED_MSVC2015_CONSTEXPR expected(U && v) + : expected(in_place, std::forward(v)) {} + + template< + class U = T, class G = T, + detail::enable_if_t::value> * = + nullptr, + detail::enable_if_t::value> * = nullptr, + detail::enable_if_t< + (!std::is_same, detail::decay_t>::value && + !detail::conjunction, + std::is_same>>::value && + std::is_constructible::value && + std::is_assignable::value && + std::is_nothrow_move_constructible::value)> * = nullptr> + expected & operator=(U && v) + { if (has_value()) { val() = std::forward(v); } else { @@ -1691,19 +1873,20 @@ class expected : private detail::expected_move_assign_base, return *this; } - template < - class U = T, class G = T, - detail::enable_if_t::value> * = - nullptr, - detail::enable_if_t::value> * = nullptr, - detail::enable_if_t< - (!std::is_same, detail::decay_t>::value && - !detail::conjunction, - std::is_same>>::value && - std::is_constructible::value && - std::is_assignable::value && - std::is_nothrow_move_constructible::value)> * = nullptr> - expected &operator=(U &&v) { + template< + class U = T, class G = T, + detail::enable_if_t::value> * = + nullptr, + detail::enable_if_t::value> * = nullptr, + detail::enable_if_t< + (!std::is_same, detail::decay_t>::value && + !detail::conjunction, + std::is_same>>::value && + std::is_constructible::value && + std::is_assignable::value && + std::is_nothrow_move_constructible::value)> * = nullptr> + expected & operator=(U && v) + { if (has_value()) { val() = std::forward(v); } else { @@ -1727,10 +1910,11 @@ class expected : private detail::expected_move_assign_base, return *this; } - template ::value && - std::is_assignable::value> * = nullptr> - expected &operator=(const unexpected &rhs) { + template::value && + std::is_assignable::value> * = nullptr> + expected & operator=(const unexpected & rhs) + { if (!has_value()) { err() = rhs; } else { @@ -1742,10 +1926,11 @@ class expected : private detail::expected_move_assign_base, return *this; } - template ::value && - std::is_move_assignable::value> * = nullptr> - expected &operator=(unexpected &&rhs) noexcept { + template::value && + std::is_move_assignable::value> * = nullptr> + expected & operator=(unexpected && rhs) noexcept + { if (!has_value()) { err() = std::move(rhs); } else { @@ -1757,9 +1942,10 @@ class expected : private detail::expected_move_assign_base, return *this; } - template ::value> * = nullptr> - void emplace(Args &&...args) { + template::value> * = nullptr> + void emplace(Args &&... args) + { if (has_value()) { val().~T(); } else { @@ -1769,9 +1955,10 @@ class expected : private detail::expected_move_assign_base, ::new (valptr()) T(std::forward(args)...); } - template ::value> * = nullptr> - void emplace(Args &&...args) { + template::value> * = nullptr> + void emplace(Args &&... args) + { if (has_value()) { val().~T(); ::new (valptr()) T(std::forward(args)...); @@ -1794,10 +1981,11 @@ class expected : private detail::expected_move_assign_base, } } - template &, Args &&...>::value> * = nullptr> - void emplace(std::initializer_list il, Args &&...args) { + template &, Args &&...>::value> * = nullptr> + void emplace(std::initializer_list il, Args &&... args) + { if (has_value()) { T t(il, std::forward(args)...); val() = std::move(t); @@ -1808,10 +1996,11 @@ class expected : private detail::expected_move_assign_base, } } - template &, Args &&...>::value> * = nullptr> - void emplace(std::initializer_list il, Args &&...args) { + template &, Args &&...>::value> * = nullptr> + void emplace(std::initializer_list il, Args &&... args) + { if (has_value()) { T t(il, std::forward(args)...); val() = std::move(t); @@ -1842,31 +2031,36 @@ class expected : private detail::expected_move_assign_base, using e_is_nothrow_move_constructible = std::true_type; using move_constructing_e_can_throw = std::false_type; - void swap_where_both_have_value(expected & /*rhs*/, t_is_void) noexcept { + void swap_where_both_have_value(expected & /*rhs*/, t_is_void) noexcept + { // swapping void is a no-op } - void swap_where_both_have_value(expected &rhs, t_is_not_void) { + void swap_where_both_have_value(expected & rhs, t_is_not_void) + { using std::swap; swap(val(), rhs.val()); } - void swap_where_only_one_has_value(expected &rhs, t_is_void) noexcept( - std::is_nothrow_move_constructible::value) { + void swap_where_only_one_has_value(expected & rhs, t_is_void) noexcept( + std::is_nothrow_move_constructible::value) + { ::new (errptr()) unexpected_type(std::move(rhs.err())); rhs.err().~unexpected_type(); std::swap(this->m_has_val, rhs.m_has_val); } - void swap_where_only_one_has_value(expected &rhs, t_is_not_void) { + void swap_where_only_one_has_value(expected & rhs, t_is_not_void) + { swap_where_only_one_has_value_and_t_is_not_void( - rhs, typename std::is_nothrow_move_constructible::type{}, - typename std::is_nothrow_move_constructible::type{}); + rhs, typename std::is_nothrow_move_constructible::type{}, + typename std::is_nothrow_move_constructible::type{}); } void swap_where_only_one_has_value_and_t_is_not_void( - expected &rhs, t_is_nothrow_move_constructible, - e_is_nothrow_move_constructible) noexcept { + expected & rhs, t_is_nothrow_move_constructible, + e_is_nothrow_move_constructible) noexcept + { auto temp = std::move(val()); val().~T(); ::new (errptr()) unexpected_type(std::move(rhs.err())); @@ -1876,8 +2070,9 @@ class expected : private detail::expected_move_assign_base, } void swap_where_only_one_has_value_and_t_is_not_void( - expected &rhs, t_is_nothrow_move_constructible, - move_constructing_e_can_throw) { + expected & rhs, t_is_nothrow_move_constructible, + move_constructing_e_can_throw) + { auto temp = std::move(val()); val().~T(); #ifdef TL_EXPECTED_EXCEPTIONS_ENABLED @@ -1899,8 +2094,9 @@ class expected : private detail::expected_move_assign_base, } void swap_where_only_one_has_value_and_t_is_not_void( - expected &rhs, move_constructing_t_can_throw, - e_is_nothrow_move_constructible) { + expected & rhs, move_constructing_t_can_throw, + e_is_nothrow_move_constructible) + { auto temp = std::move(rhs.err()); rhs.err().~unexpected_type(); #ifdef TL_EXPECTED_EXCEPTIONS_ENABLED @@ -1922,16 +2118,17 @@ class expected : private detail::expected_move_assign_base, } public: - template + template detail::enable_if_t::value && - detail::is_swappable::value && - (std::is_nothrow_move_constructible::value || - std::is_nothrow_move_constructible::value)> - swap(expected &rhs) noexcept( - std::is_nothrow_move_constructible::value - &&detail::is_nothrow_swappable::value - &&std::is_nothrow_move_constructible::value - &&detail::is_nothrow_swappable::value) { + detail::is_swappable::value && + (std::is_nothrow_move_constructible::value || + std::is_nothrow_move_constructible::value)> + swap(expected & rhs) noexcept( + std::is_nothrow_move_constructible::value && + detail::is_nothrow_swappable::value && + std::is_nothrow_move_constructible::value && + detail::is_nothrow_swappable::value) + { if (has_value() && rhs.has_value()) { swap_where_both_have_value(rhs, typename std::is_void::type{}); } else if (!has_value() && rhs.has_value()) { @@ -1944,174 +2141,206 @@ class expected : private detail::expected_move_assign_base, } } - constexpr const T *operator->() const { + constexpr const T * operator->() const + { TL_ASSERT(has_value()); return valptr(); } - TL_EXPECTED_11_CONSTEXPR T *operator->() { + TL_EXPECTED_11_CONSTEXPR T * operator->() + { TL_ASSERT(has_value()); return valptr(); } - template ::value> * = nullptr> - constexpr const U &operator*() const & { + template::value> * = nullptr> + constexpr const U & operator*() const & + { TL_ASSERT(has_value()); return val(); } - template ::value> * = nullptr> - TL_EXPECTED_11_CONSTEXPR U &operator*() & { + template::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR U & operator*() & + { TL_ASSERT(has_value()); return val(); } - template ::value> * = nullptr> - constexpr const U &&operator*() const && { + template::value> * = nullptr> + constexpr const U && operator*() const && { TL_ASSERT(has_value()); return std::move(val()); } - template ::value> * = nullptr> - TL_EXPECTED_11_CONSTEXPR U &&operator*() && { + template::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR U && operator*() && { TL_ASSERT(has_value()); return std::move(val()); } - constexpr bool has_value() const noexcept { return this->m_has_val; } - constexpr explicit operator bool() const noexcept { return this->m_has_val; } + constexpr bool has_value() const noexcept {return this->m_has_val;} + constexpr explicit operator bool() const noexcept {return this->m_has_val;} - template ::value> * = nullptr> - TL_EXPECTED_11_CONSTEXPR const U &value() const & { - if (!has_value()) + template::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR const U & value() const & + { + if (!has_value()) { detail::throw_exception(bad_expected_access(err().value())); + } return val(); } - template ::value> * = nullptr> - TL_EXPECTED_11_CONSTEXPR U &value() & { - if (!has_value()) + template::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR U & value() & + { + if (!has_value()) { detail::throw_exception(bad_expected_access(err().value())); + } return val(); } - template ::value> * = nullptr> - TL_EXPECTED_11_CONSTEXPR const U &&value() const && { - if (!has_value()) + template::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR const U && value() const && { + if (!has_value()) { detail::throw_exception(bad_expected_access(std::move(err()).value())); + } return std::move(val()); } - template ::value> * = nullptr> - TL_EXPECTED_11_CONSTEXPR U &&value() && { - if (!has_value()) + template::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR U && value() && { + if (!has_value()) { detail::throw_exception(bad_expected_access(std::move(err()).value())); + } return std::move(val()); } - constexpr const E &error() const & { + constexpr const E & error() const & + { TL_ASSERT(!has_value()); return err().value(); } - TL_EXPECTED_11_CONSTEXPR E &error() & { + TL_EXPECTED_11_CONSTEXPR E & error() & + { TL_ASSERT(!has_value()); return err().value(); } - constexpr const E &&error() const && { + constexpr const E && error() const && + { TL_ASSERT(!has_value()); return std::move(err().value()); } - TL_EXPECTED_11_CONSTEXPR E &&error() && { + TL_EXPECTED_11_CONSTEXPR E && error() && + { TL_ASSERT(!has_value()); return std::move(err().value()); } - template constexpr T value_or(U &&v) const & { - static_assert(std::is_copy_constructible::value && - std::is_convertible::value, - "T must be copy-constructible and convertible to from U&&"); - return bool(*this) ? **this : static_cast(std::forward(v)); + template constexpr T value_or(U && v) const & + { + static_assert( + std::is_copy_constructible::value && + std::is_convertible::value, + "T must be copy-constructible and convertible to from U&&"); + return static_cast(*this) ? **this : static_cast(std::forward(v)); } - template TL_EXPECTED_11_CONSTEXPR T value_or(U &&v) && { - static_assert(std::is_move_constructible::value && - std::is_convertible::value, - "T must be move-constructible and convertible to from U&&"); - return bool(*this) ? std::move(**this) : static_cast(std::forward(v)); + template TL_EXPECTED_11_CONSTEXPR T value_or(U && v) && + { + static_assert( + std::is_move_constructible::value && + std::is_convertible::value, + "T must be move-constructible and convertible to from U&&"); + return static_cast(*this) ? std::move(**this) : static_cast(std::forward(v)); } }; -namespace detail { -template using exp_t = typename detail::decay_t::value_type; -template using err_t = typename detail::decay_t::error_type; -template using ret_t = expected>; +namespace detail +{ +template using exp_t = typename detail::decay_t::value_type; +template using err_t = typename detail::decay_t::error_type; +template using ret_t = expected>; #ifdef TL_EXPECTED_CXX14 -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval(), - *std::declval()))> -constexpr auto and_then_impl(Exp &&exp, F &&f) { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke( + std::declval(), + *std::declval()))> +constexpr auto and_then_impl(Exp && exp, F && f) +{ static_assert(detail::is_expected::value, "F must return an expected"); - return exp.has_value() - ? detail::invoke(std::forward(f), *std::forward(exp)) - : Ret(unexpect, std::forward(exp).error()); + return exp.has_value() ? + detail::invoke(std::forward(f), *std::forward(exp)) : + Ret(unexpect, std::forward(exp).error()); } -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval()))> -constexpr auto and_then_impl(Exp &&exp, F &&f) { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval()))> +constexpr auto and_then_impl(Exp && exp, F && f) +{ static_assert(detail::is_expected::value, "F must return an expected"); - return exp.has_value() ? detail::invoke(std::forward(f)) - : Ret(unexpect, std::forward(exp).error()); + return exp.has_value() ? detail::invoke(std::forward(f)) : + Ret(unexpect, std::forward(exp).error()); } #else -template struct TC; -template (), - *std::declval())), - detail::enable_if_t>::value> * = nullptr> -auto and_then_impl(Exp &&exp, F &&f) -> Ret { +template +struct TC; +template(), + *std::declval())), + detail::enable_if_t>::value> * = nullptr> +auto and_then_impl(Exp && exp, F && f) -> Ret +{ static_assert(detail::is_expected::value, "F must return an expected"); - return exp.has_value() - ? detail::invoke(std::forward(f), *std::forward(exp)) - : Ret(unexpect, std::forward(exp).error()); + return exp.has_value() ? + detail::invoke(std::forward(f), *std::forward(exp)) : + Ret(unexpect, std::forward(exp).error()); } -template ())), - detail::enable_if_t>::value> * = nullptr> -constexpr auto and_then_impl(Exp &&exp, F &&f) -> Ret { +template())), + detail::enable_if_t>::value> * = nullptr> +constexpr auto and_then_impl(Exp && exp, F && f) -> Ret +{ static_assert(detail::is_expected::value, "F must return an expected"); - return exp.has_value() ? detail::invoke(std::forward(f)) - : Ret(unexpect, std::forward(exp).error()); + return exp.has_value() ? detail::invoke(std::forward(f)) : + Ret(unexpect, std::forward(exp).error()); } #endif #ifdef TL_EXPECTED_CXX14 -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval(), - *std::declval())), - detail::enable_if_t::value> * = nullptr> -constexpr auto expected_map_impl(Exp &&exp, F &&f) { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke( + std::declval(), + *std::declval())), + detail::enable_if_t::value> * = nullptr> +constexpr auto expected_map_impl(Exp && exp, F && f) +{ using result = ret_t>; - return exp.has_value() ? result(detail::invoke(std::forward(f), - *std::forward(exp))) - : result(unexpect, std::forward(exp).error()); + return exp.has_value() ? result( + detail::invoke( + std::forward(f), + *std::forward(exp))) : + result(unexpect, std::forward(exp).error()); } -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval(), - *std::declval())), - detail::enable_if_t::value> * = nullptr> -auto expected_map_impl(Exp &&exp, F &&f) { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke( + std::declval(), + *std::declval())), + detail::enable_if_t::value> * = nullptr> +auto expected_map_impl(Exp && exp, F && f) +{ using result = expected>; if (exp.has_value()) { detail::invoke(std::forward(f), *std::forward(exp)); @@ -2121,21 +2350,23 @@ auto expected_map_impl(Exp &&exp, F &&f) { return result(unexpect, std::forward(exp).error()); } -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval())), - detail::enable_if_t::value> * = nullptr> -constexpr auto expected_map_impl(Exp &&exp, F &&f) { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval())), + detail::enable_if_t::value> * = nullptr> +constexpr auto expected_map_impl(Exp && exp, F && f) +{ using result = ret_t>; - return exp.has_value() ? result(detail::invoke(std::forward(f))) - : result(unexpect, std::forward(exp).error()); + return exp.has_value() ? result(detail::invoke(std::forward(f))) : + result(unexpect, std::forward(exp).error()); } -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval())), - detail::enable_if_t::value> * = nullptr> -auto expected_map_impl(Exp &&exp, F &&f) { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval())), + detail::enable_if_t::value> * = nullptr> +auto expected_map_impl(Exp && exp, F && f) +{ using result = expected>; if (exp.has_value()) { detail::invoke(std::forward(f)); @@ -2145,28 +2376,34 @@ auto expected_map_impl(Exp &&exp, F &&f) { return result(unexpect, std::forward(exp).error()); } #else -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval(), - *std::declval())), - detail::enable_if_t::value> * = nullptr> - -constexpr auto expected_map_impl(Exp &&exp, F &&f) - -> ret_t> { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke( + std::declval(), + *std::declval())), + detail::enable_if_t::value> * = nullptr> + +constexpr auto expected_map_impl(Exp && exp, F && f) +-> ret_t> +{ using result = ret_t>; - return exp.has_value() ? result(detail::invoke(std::forward(f), - *std::forward(exp))) - : result(unexpect, std::forward(exp).error()); + return exp.has_value() ? result( + detail::invoke( + std::forward(f), + *std::forward(exp))) : + result(unexpect, std::forward(exp).error()); } -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval(), - *std::declval())), - detail::enable_if_t::value> * = nullptr> +template>::value> * = nullptr, + class Ret = decltype(detail::invoke( + std::declval(), + *std::declval())), + detail::enable_if_t::value> * = nullptr> -auto expected_map_impl(Exp &&exp, F &&f) -> expected> { +auto expected_map_impl(Exp && exp, F && f) -> expected> +{ if (exp.has_value()) { detail::invoke(std::forward(f), *std::forward(exp)); return {}; @@ -2175,25 +2412,27 @@ auto expected_map_impl(Exp &&exp, F &&f) -> expected> { return unexpected>(std::forward(exp).error()); } -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval())), - detail::enable_if_t::value> * = nullptr> +template>::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval())), + detail::enable_if_t::value> * = nullptr> -constexpr auto expected_map_impl(Exp &&exp, F &&f) - -> ret_t> { +constexpr auto expected_map_impl(Exp && exp, F && f) +-> ret_t> +{ using result = ret_t>; - return exp.has_value() ? result(detail::invoke(std::forward(f))) - : result(unexpect, std::forward(exp).error()); + return exp.has_value() ? result(detail::invoke(std::forward(f))) : + result(unexpect, std::forward(exp).error()); } -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval())), - detail::enable_if_t::value> * = nullptr> +template>::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval())), + detail::enable_if_t::value> * = nullptr> -auto expected_map_impl(Exp &&exp, F &&f) -> expected> { +auto expected_map_impl(Exp && exp, F && f) -> expected> +{ if (exp.has_value()) { detail::invoke(std::forward(f)); return {}; @@ -2203,26 +2442,32 @@ auto expected_map_impl(Exp &&exp, F &&f) -> expected> { } #endif -#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ - !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -constexpr auto map_error_impl(Exp &&exp, F &&f) { +#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ + !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) +template>::value> * = nullptr, + class Ret = decltype(detail::invoke( + std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +constexpr auto map_error_impl(Exp && exp, F && f) +{ using result = expected, detail::decay_t>; - return exp.has_value() - ? result(*std::forward(exp)) - : result(unexpect, detail::invoke(std::forward(f), - std::forward(exp).error())); + return exp.has_value() ? + result(*std::forward(exp)) : + result( + unexpect, detail::invoke( + std::forward(f), + std::forward(exp).error())); } -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -auto map_error_impl(Exp &&exp, F &&f) { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke( + std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +auto map_error_impl(Exp && exp, F && f) +{ using result = expected, monostate>; if (exp.has_value()) { return result(*std::forward(exp)); @@ -2231,24 +2476,30 @@ auto map_error_impl(Exp &&exp, F &&f) { detail::invoke(std::forward(f), std::forward(exp).error()); return result(unexpect, monostate{}); } -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -constexpr auto map_error_impl(Exp &&exp, F &&f) { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke( + std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +constexpr auto map_error_impl(Exp && exp, F && f) +{ using result = expected, detail::decay_t>; - return exp.has_value() - ? result() - : result(unexpect, detail::invoke(std::forward(f), - std::forward(exp).error())); + return exp.has_value() ? + result() : + result( + unexpect, detail::invoke( + std::forward(f), + std::forward(exp).error())); } -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -auto map_error_impl(Exp &&exp, F &&f) { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke( + std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +auto map_error_impl(Exp && exp, F && f) +{ using result = expected, monostate>; if (exp.has_value()) { return result(); @@ -2258,27 +2509,33 @@ auto map_error_impl(Exp &&exp, F &&f) { return result(unexpect, monostate{}); } #else -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -constexpr auto map_error_impl(Exp &&exp, F &&f) - -> expected, detail::decay_t> { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke( + std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +constexpr auto map_error_impl(Exp && exp, F && f) +-> expected, detail::decay_t> +{ using result = expected, detail::decay_t>; - return exp.has_value() - ? result(*std::forward(exp)) - : result(unexpect, detail::invoke(std::forward(f), - std::forward(exp).error())); + return exp.has_value() ? + result(*std::forward(exp)) : + result( + unexpect, detail::invoke( + std::forward(f), + std::forward(exp).error())); } -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -auto map_error_impl(Exp &&exp, F &&f) -> expected, monostate> { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke( + std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +auto map_error_impl(Exp && exp, F && f) -> expected, monostate> +{ using result = expected, monostate>; if (exp.has_value()) { return result(*std::forward(exp)); @@ -2288,27 +2545,33 @@ auto map_error_impl(Exp &&exp, F &&f) -> expected, monostate> { return result(unexpect, monostate{}); } -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -constexpr auto map_error_impl(Exp &&exp, F &&f) - -> expected, detail::decay_t> { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke( + std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +constexpr auto map_error_impl(Exp && exp, F && f) +-> expected, detail::decay_t> +{ using result = expected, detail::decay_t>; - return exp.has_value() - ? result() - : result(unexpect, detail::invoke(std::forward(f), - std::forward(exp).error())); + return exp.has_value() ? + result() : + result( + unexpect, detail::invoke( + std::forward(f), + std::forward(exp).error())); } -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -auto map_error_impl(Exp &&exp, F &&f) -> expected, monostate> { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke( + std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +auto map_error_impl(Exp && exp, F && f) -> expected, monostate> +{ using result = expected, monostate>; if (exp.has_value()) { return result(); @@ -2320,125 +2583,155 @@ auto map_error_impl(Exp &&exp, F &&f) -> expected, monostate> { #endif #ifdef TL_EXPECTED_CXX14 -template (), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -constexpr auto or_else_impl(Exp &&exp, F &&f) { +template(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +constexpr auto or_else_impl(Exp && exp, F && f) +{ static_assert(detail::is_expected::value, "F must return an expected"); - return exp.has_value() ? std::forward(exp) - : detail::invoke(std::forward(f), - std::forward(exp).error()); + return exp.has_value() ? std::forward(exp) : + detail::invoke( + std::forward(f), + std::forward(exp).error()); } -template (), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -detail::decay_t or_else_impl(Exp &&exp, F &&f) { - return exp.has_value() ? std::forward(exp) - : (detail::invoke(std::forward(f), - std::forward(exp).error()), - std::forward(exp)); +template(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +detail::decay_t or_else_impl(Exp && exp, F && f) +{ + return exp.has_value() ? std::forward(exp) : + (detail::invoke( + std::forward(f), + std::forward(exp).error()), + std::forward(exp)); } #else -template (), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -auto or_else_impl(Exp &&exp, F &&f) -> Ret { +template(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +auto or_else_impl(Exp && exp, F && f) -> Ret +{ static_assert(detail::is_expected::value, "F must return an expected"); - return exp.has_value() ? std::forward(exp) - : detail::invoke(std::forward(f), - std::forward(exp).error()); + return exp.has_value() ? std::forward(exp) : + detail::invoke( + std::forward(f), + std::forward(exp).error()); } -template (), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -detail::decay_t or_else_impl(Exp &&exp, F &&f) { - return exp.has_value() ? std::forward(exp) - : (detail::invoke(std::forward(f), - std::forward(exp).error()), - std::forward(exp)); +template(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +detail::decay_t or_else_impl(Exp && exp, F && f) +{ + return exp.has_value() ? std::forward(exp) : + (detail::invoke( + std::forward(f), + std::forward(exp).error()), + std::forward(exp)); } #endif -} // namespace detail - -template -constexpr bool operator==(const expected &lhs, - const expected &rhs) { - return (lhs.has_value() != rhs.has_value()) - ? false - : (!lhs.has_value() ? lhs.error() == rhs.error() : *lhs == *rhs); +} // namespace detail + +template +constexpr bool operator==( + const expected & lhs, + const expected & rhs) +{ + return (lhs.has_value() != rhs.has_value()) ? + false : + (!lhs.has_value() ? lhs.error() == rhs.error() : *lhs == *rhs); } -template -constexpr bool operator!=(const expected &lhs, - const expected &rhs) { - return (lhs.has_value() != rhs.has_value()) - ? true - : (!lhs.has_value() ? lhs.error() != rhs.error() : *lhs != *rhs); +template +constexpr bool operator!=( + const expected & lhs, + const expected & rhs) +{ + return (lhs.has_value() != rhs.has_value()) ? + true : + (!lhs.has_value() ? lhs.error() != rhs.error() : *lhs != *rhs); } -template -constexpr bool operator==(const expected &lhs, - const expected &rhs) { - return (lhs.has_value() != rhs.has_value()) - ? false - : (!lhs.has_value() ? lhs.error() == rhs.error() : true); +template +constexpr bool operator==( + const expected & lhs, + const expected & rhs) +{ + return (lhs.has_value() != rhs.has_value()) ? + false : + (!lhs.has_value() ? lhs.error() == rhs.error() : true); } -template -constexpr bool operator!=(const expected &lhs, - const expected &rhs) { - return (lhs.has_value() != rhs.has_value()) - ? true - : (!lhs.has_value() ? lhs.error() == rhs.error() : false); +template +constexpr bool operator!=( + const expected & lhs, + const expected & rhs) +{ + return (lhs.has_value() != rhs.has_value()) ? + true : + (!lhs.has_value() ? lhs.error() == rhs.error() : false); } -template -constexpr bool operator==(const expected &x, const U &v) { +template +constexpr bool operator==(const expected & x, const U & v) +{ return x.has_value() ? *x == v : false; } -template -constexpr bool operator==(const U &v, const expected &x) { +template +constexpr bool operator==(const U & v, const expected & x) +{ return x.has_value() ? *x == v : false; } -template -constexpr bool operator!=(const expected &x, const U &v) { +template +constexpr bool operator!=(const expected & x, const U & v) +{ return x.has_value() ? *x != v : true; } -template -constexpr bool operator!=(const U &v, const expected &x) { +template +constexpr bool operator!=(const U & v, const expected & x) +{ return x.has_value() ? *x != v : true; } -template -constexpr bool operator==(const expected &x, const unexpected &e) { +template +constexpr bool operator==(const expected & x, const unexpected & e) +{ return x.has_value() ? false : x.error() == e.value(); } -template -constexpr bool operator==(const unexpected &e, const expected &x) { +template +constexpr bool operator==(const unexpected & e, const expected & x) +{ return x.has_value() ? false : x.error() == e.value(); } -template -constexpr bool operator!=(const expected &x, const unexpected &e) { +template +constexpr bool operator!=(const expected & x, const unexpected & e) +{ return x.has_value() ? true : x.error() != e.value(); } -template -constexpr bool operator!=(const unexpected &e, const expected &x) { +template +constexpr bool operator!=(const unexpected & e, const expected & x) +{ return x.has_value() ? true : x.error() != e.value(); } -template ::value || - std::is_move_constructible::value) && - detail::is_swappable::value && - std::is_move_constructible::value && - detail::is_swappable::value> * = nullptr> -void swap(expected &lhs, - expected &rhs) noexcept(noexcept(lhs.swap(rhs))) { +template::value || + std::is_move_constructible::value) && + detail::is_swappable::value && + std::is_move_constructible::value && + detail::is_swappable::value> * = nullptr> +void swap( + expected & lhs, + expected & rhs) noexcept(noexcept(lhs.swap(rhs))) +{ lhs.swap(rhs); } -} // namespace tl +} // namespace tl -#endif +#endif // POINT_CLOUD_TRANSPORT__TL__EXPECTED_HPP_ diff --git a/include/point_cloud_transport/transport_hints.hpp b/include/point_cloud_transport/transport_hints.hpp index 545c038..e059a14 100644 --- a/include/point_cloud_transport/transport_hints.hpp +++ b/include/point_cloud_transport/transport_hints.hpp @@ -1,44 +1,38 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. - /* - * - * BSD 3-Clause License - * - * Copyright (c) Czech Technical University in Prague + * Copyright (c) 2023, Czech Technical University in Prague * Copyright (c) 2019, paplhjak * Copyright (c) 2009, Willow Garage, Inc. - * - * All rights reserved. + * All rights reserved. * * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. + * modification, are permitted provided that the following conditions are met: * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. + * * 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. * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. */ -#pragma once +#ifndef POINT_CLOUD_TRANSPORT__TRANSPORT_HINTS_HPP_ +#define POINT_CLOUD_TRANSPORT__TRANSPORT_HINTS_HPP_ #include @@ -58,26 +52,27 @@ class TransportHints * name of the desired transport. By default this parameter is named "point_cloud_transport" * in the node's local namespace. For consistency across ROS applications, the * name of this parameter should not be changed without good reason. - * + * * @param node Node to use when looking up the transport parameter. * @param default_transport Preferred transport to use * @param parameter_name The name of the transport parameter * */ TransportHints( - const rclcpp::Node * node, - const std::string & default_transport = "raw", - const std::string& parameter_name = "point_cloud_transport") + const rclcpp::Node * node, + const std::string & default_transport = "raw", + const std::string & parameter_name = "point_cloud_transport") { node->get_parameter_or(parameter_name, transport_, default_transport); } TransportHints( - const std::string & transport = "raw") : transport_(transport) + const std::string & transport = "raw") + : transport_(transport) { } - const std::string& getTransport() const + const std::string & getTransport() const { return transport_; } @@ -86,4 +81,5 @@ class TransportHints std::string transport_; }; -} +} // namespace point_cloud_transport +#endif // POINT_CLOUD_TRANSPORT__TRANSPORT_HINTS_HPP_ diff --git a/package.xml b/package.xml index bc9c6a8..c2548f7 100644 --- a/package.xml +++ b/package.xml @@ -26,13 +26,11 @@ rclcpp sensor_msgs - ament_cmake_gtest ament_lint_auto ament_lint_common ament_cmake - diff --git a/setup.py b/setup.py index 029c179..5dd19f7 100644 --- a/setup.py +++ b/setup.py @@ -1,8 +1,8 @@ # SPDX-License-Identifier: BSD-3-Clause # SPDX-FileCopyrightText: Czech Technical University in Prague -from setuptools import setup from catkin_pkg.python_setup import generate_distutils_setup +from setuptools import setup setup_args = generate_distutils_setup( packages=['point_cloud_transport'], diff --git a/src/c_api.cpp b/src/c_api.cpp index 4545015..03e80b4 100644 --- a/src/c_api.cpp +++ b/src/c_api.cpp @@ -1,5 +1,33 @@ -// // SPDX-License-Identifier: BSD-3-Clause -// // SPDX-FileCopyrightText: Czech Technical University in Prague +/* + * Copyright (c) 2023, Czech Technical University in Prague + * 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 HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ // /** // * \file diff --git a/src/list_transports.cpp b/src/list_transports.cpp index f16fe66..21282f9 100644 --- a/src/list_transports.cpp +++ b/src/list_transports.cpp @@ -1,41 +1,34 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. - /* - * - * BSD 3-Clause License - * - * Copyright (c) Czech Technical University in Prague + * Copyright (c) 2023, Czech Technical University in Prague * Copyright (c) 2019, paplhjak * Copyright (c) 2009, Willow Garage, Inc. - * - * All rights reserved. + * All rights reserved. * * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: + * modification, are permitted provided that the following conditions are met: * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. */ #include @@ -56,7 +49,8 @@ enum PluginStatus struct TransportDesc { - TransportDesc() : pub_status(DOES_NOT_EXIST), sub_status(DOES_NOT_EXIST) + TransportDesc() + : pub_status(DOES_NOT_EXIST), sub_status(DOES_NOT_EXIST) { } @@ -67,54 +61,46 @@ struct TransportDesc PluginStatus sub_status; }; -int main(int argc, char** argv) +int main(int argc, char ** argv) { - pluginlib::ClassLoader pub_loader("point_cloud_transport", "point_cloud_transport::PublisherPlugin"); - pluginlib::ClassLoader sub_loader("point_cloud_transport", "point_cloud_transport::SubscriberPlugin"); + pluginlib::ClassLoader pub_loader( + "point_cloud_transport", + "point_cloud_transport::PublisherPlugin"); + pluginlib::ClassLoader sub_loader( + "point_cloud_transport", + "point_cloud_transport::SubscriberPlugin"); typedef std::map StatusMap; StatusMap transports; - for (const auto& lookup_name : pub_loader.getDeclaredClasses()) - { + for (const auto & lookup_name : pub_loader.getDeclaredClasses()) { printf("Lookup name: %s\n", lookup_name.c_str()); std::string transport_name = point_cloud_transport::erase_last_copy(lookup_name, "_pub"); printf("Transport name: %s\n", transport_name.c_str()); transports[transport_name].pub_name = lookup_name; transports[transport_name].package_name = pub_loader.getClassPackage(lookup_name); - try - { + try { auto pub = pub_loader.createUniqueInstance(lookup_name); transports[transport_name].pub_status = SUCCESS; - } - catch (const pluginlib::LibraryLoadException& e) - { + } catch (const pluginlib::LibraryLoadException & e) { transports[transport_name].pub_status = LIB_LOAD_FAILURE; printf("LibraryLoadException: %s\n", e.what()); - } - catch (const pluginlib::CreateClassException& e) - { + } catch (const pluginlib::CreateClassException & e) { transports[transport_name].pub_status = CREATE_FAILURE; printf("CreateClassException: %s\n", e.what()); } } - for (const auto& lookup_name : sub_loader.getDeclaredClasses()) - { + for (const auto & lookup_name : sub_loader.getDeclaredClasses()) { std::string transport_name = point_cloud_transport::erase_last_copy(lookup_name, "_sub"); transports[transport_name].sub_name = lookup_name; transports[transport_name].package_name = sub_loader.getClassPackage(lookup_name); - try - { + try { auto sub = sub_loader.createUniqueInstance(lookup_name); transports[transport_name].sub_status = SUCCESS; - } - catch (const pluginlib::LibraryLoadException& e) - { + } catch (const pluginlib::LibraryLoadException & e) { transports[transport_name].sub_status = LIB_LOAD_FAILURE; printf("LibraryLoadException: %s\n", e.what()); - } - catch (const pluginlib::CreateClassException& e) - { + } catch (const pluginlib::CreateClassException & e) { transports[transport_name].sub_status = CREATE_FAILURE; printf("CreateClassException: %s\n", e.what()); } @@ -122,12 +108,11 @@ int main(int argc, char** argv) bool problem_package = false; printf("Declared transports:\n"); - for (const StatusMap::value_type & value : transports) - { - const TransportDesc& td = value.second; + for (const StatusMap::value_type & value : transports) { + const TransportDesc & td = value.second; printf("%s", value.first.c_str()); if ((td.pub_status == CREATE_FAILURE || td.pub_status == LIB_LOAD_FAILURE) || - (td.sub_status == CREATE_FAILURE || td.sub_status == LIB_LOAD_FAILURE)) + (td.sub_status == CREATE_FAILURE || td.sub_status == LIB_LOAD_FAILURE)) { printf(" (*): Not available. Try 'catkin_make --pkg %s'.", td.package_name.c_str()); problem_package = true; @@ -135,41 +120,31 @@ int main(int argc, char** argv) printf("\n"); } - if (problem_package) - { + if (problem_package) { printf("(*) \n"); } printf("\nDetails:\n"); - for (const auto& value : transports) - { - const TransportDesc& td = value.second; + for (const auto & value : transports) { + const TransportDesc & td = value.second; printf("----------\n"); printf("\"%s\"\n", value.first.c_str()); - if (td.pub_status == CREATE_FAILURE || td.sub_status == CREATE_FAILURE) - { - printf("*** Plugins are built, but could not be loaded. The package may need to be rebuilt or may not be " - "compatible with this release of point_cloud_common. ***\n"); - } - else if (td.pub_status == LIB_LOAD_FAILURE || td.sub_status == LIB_LOAD_FAILURE) - { + if (td.pub_status == CREATE_FAILURE || td.sub_status == CREATE_FAILURE) { + printf( + "*** Plugins are built, but could not be loaded. The package may need to be rebuilt " + "or may not be compatible with this release of point_cloud_common. ***\n"); + } else if (td.pub_status == LIB_LOAD_FAILURE || td.sub_status == LIB_LOAD_FAILURE) { printf("*** Plugins are not built. ***\n"); } printf(" - Provided by package: %s\n", td.package_name.c_str()); - if (td.pub_status == DOES_NOT_EXIST) - { + if (td.pub_status == DOES_NOT_EXIST) { printf(" - No publisher provided\n"); - } - else - { + } else { printf(" - Publisher: %s\n", pub_loader.getClassDescription(td.pub_name).c_str()); } - if (td.sub_status == DOES_NOT_EXIST) - { + if (td.sub_status == DOES_NOT_EXIST) { printf(" - No subscriber provided\n"); - } - else - { + } else { printf(" - Subscriber: %s\n", sub_loader.getClassDescription(td.sub_name).c_str()); } } diff --git a/src/manifest.cpp b/src/manifest.cpp index ce6a2b0..2907751 100644 --- a/src/manifest.cpp +++ b/src/manifest.cpp @@ -1,5 +1,34 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009 +/* + * Copyright (c) 2023, Czech Technical University in Prague + * Copyright (c) 2019, paplhjak + * 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 HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ #include @@ -9,4 +38,6 @@ #include PLUGINLIB_EXPORT_CLASS(point_cloud_transport::RawPublisher, point_cloud_transport::PublisherPlugin) -PLUGINLIB_EXPORT_CLASS(point_cloud_transport::RawSubscriber, point_cloud_transport::SubscriberPlugin) +PLUGINLIB_EXPORT_CLASS( + point_cloud_transport::RawSubscriber, + point_cloud_transport::SubscriberPlugin) diff --git a/src/point_cloud_codec.cpp b/src/point_cloud_codec.cpp index a525001..423ade5 100644 --- a/src/point_cloud_codec.cpp +++ b/src/point_cloud_codec.cpp @@ -1,43 +1,38 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. - /* - * - * BSD 3-Clause License - * - * Copyright (c) Czech Technical University in Prague + * Copyright (c) 2023, Czech Technical University in Prague * Copyright (c) 2019, paplhjak * Copyright (c) 2009, Willow Garage, Inc. - * - * All rights reserved. + * All rights reserved. * * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: + * modification, are permitted provided that the following conditions are met: * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. */ + #include + #include #include #include @@ -45,7 +40,6 @@ #include -#include #include #include #include @@ -62,43 +56,45 @@ struct PointCloudCodec::Impl std::unordered_map encoders_for_topics_; std::unordered_map decoders_for_topics_; - Impl() : - enc_loader_(std::make_shared("point_cloud_transport", "point_cloud_transport::PublisherPlugin")), - dec_loader_(std::make_shared("point_cloud_transport", "point_cloud_transport::SubscriberPlugin")) + Impl() + : enc_loader_(std::make_shared( + "point_cloud_transport", + "point_cloud_transport::PublisherPlugin")), + dec_loader_(std::make_shared( + "point_cloud_transport", + "point_cloud_transport::SubscriberPlugin")) { } }; -PointCloudCodec::PointCloudCodec() : impl_(new Impl) +PointCloudCodec::PointCloudCodec() +: impl_(new Impl) { } -bool transportNameMatches(const std::string& lookup_name, const std::string& name, const std::string& suffix) +bool transportNameMatches( + const std::string & lookup_name, const std::string & name, + const std::string & suffix) { - if (lookup_name == name) - { + if (lookup_name == name) { return true; } const std::string transport_name = removeSuffix(lookup_name, suffix); - if (transport_name == name) - { + if (transport_name == name) { return true; } const auto parts = split(transport_name, "/"); - if (parts.size() == 2 && parts[1] == name) - { + if (parts.size() == 2 && parts[1] == name) { return true; } return false; } std::shared_ptr PointCloudCodec::getEncoderByName( - const std::string& name) const + const std::string & name) const { - for (const auto& lookup_name : impl_->enc_loader_->getDeclaredClasses()) - { - if (transportNameMatches(lookup_name, name, "_pub")) - { + for (const auto & lookup_name : impl_->enc_loader_->getDeclaredClasses()) { + if (transportNameMatches(lookup_name, name, "_pub")) { auto encoder = impl_->enc_loader_->createSharedInstance(lookup_name); return encoder; } @@ -109,39 +105,34 @@ std::shared_ptr PointCloudCodec::getEnco } std::shared_ptr PointCloudCodec::getEncoderByTopic( - const std::string& topic, const std::string& datatype) const + const std::string & topic, const std::string & datatype) const { - if (impl_->encoders_for_topics_.find(topic) != impl_->encoders_for_topics_.end()) - { + if (impl_->encoders_for_topics_.find(topic) != impl_->encoders_for_topics_.end()) { auto encoder = impl_->enc_loader_->createSharedInstance(impl_->encoders_for_topics_[topic]); return encoder; } - for (const auto& lookup_name : impl_->enc_loader_->getDeclaredClasses()) - { - const auto& encoder = impl_->enc_loader_->createSharedInstance(lookup_name); - if (!encoder) - { + for (const auto & lookup_name : impl_->enc_loader_->getDeclaredClasses()) { + const auto & encoder = impl_->enc_loader_->createSharedInstance(lookup_name); + if (!encoder) { continue; } - if (encoder->matchesTopic(topic, datatype)) - { + if (encoder->matchesTopic(topic, datatype)) { impl_->encoders_for_topics_[topic] = lookup_name; return encoder; } } - // ROS_DEBUG("Failed to find encoder for topic %s with data type %s.", topic.c_str(), datatype.c_str()); + // ROS_DEBUG("Failed to find encoder for topic %s with data type %s.", t + // opic.c_str(), datatype.c_str()); return nullptr; } std::shared_ptr PointCloudCodec::getDecoderByName( - const std::string& name) const + const std::string & name) const { - for (const auto& lookup_name : impl_->dec_loader_->getDeclaredClasses()) - { - if (transportNameMatches(lookup_name, name, "_sub")) - { + for (const auto & lookup_name : impl_->dec_loader_->getDeclaredClasses()) { + if (transportNameMatches(lookup_name, name, "_sub")) { auto decoder = impl_->dec_loader_->createSharedInstance(lookup_name); return decoder; } @@ -152,79 +143,74 @@ std::shared_ptr PointCloudCodec::getDec } std::shared_ptr PointCloudCodec::getDecoderByTopic( - const std::string& topic, const std::string& datatype) const + const std::string & topic, const std::string & datatype) const { - if (impl_->decoders_for_topics_.find(topic) != impl_->decoders_for_topics_.end()) - { + if (impl_->decoders_for_topics_.find(topic) != impl_->decoders_for_topics_.end()) { auto decoder = impl_->dec_loader_->createSharedInstance(impl_->decoders_for_topics_[topic]); return decoder; } - for (const auto& lookup_name : impl_->dec_loader_->getDeclaredClasses()) - { - const auto& decoder = impl_->dec_loader_->createSharedInstance(lookup_name); - if (!decoder) - { + for (const auto & lookup_name : impl_->dec_loader_->getDeclaredClasses()) { + const auto & decoder = impl_->dec_loader_->createSharedInstance(lookup_name); + if (!decoder) { continue; } - if (decoder->matchesTopic(topic, datatype)) - { + if (decoder->matchesTopic(topic, datatype)) { impl_->decoders_for_topics_[topic] = lookup_name; return decoder; } } - // ROS_DEBUG("Failed to find decoder for topic %s with data type %s.", topic.c_str(), datatype.c_str()); + // ROS_DEBUG("Failed to find decoder for topic %s with data type %s.", + // topic.c_str(), datatype.c_str()); return nullptr; } -} +} // namespace point_cloud_transport -// TODO: These functions seem overly complex. Can we simplify them? +// TODO(anyone): These functions seem overly complex. Can we simplify them? bool pointCloudTransportCodecsEncode( - const char* codec, - sensor_msgs::msg::PointCloud2::_height_type rawHeight, - sensor_msgs::msg::PointCloud2::_width_type rawWidth, - size_t rawNumFields, - const char* rawFieldNames[], - sensor_msgs::PointField::_offset_type rawFieldOffsets[], - sensor_msgs::PointField::_datatype_type rawFieldDatatypes[], - sensor_msgs::PointField::_count_type rawFieldCounts[], - sensor_msgs::msg::PointCloud2::_is_bigendian_type rawIsBigendian, - sensor_msgs::msg::PointCloud2::_point_step_type rawPointStep, - sensor_msgs::msg::PointCloud2::_row_step_type rawRowStep, - size_t rawDataLength, - const uint8_t rawData[], - sensor_msgs::msg::PointCloud2::_is_dense_type rawIsDense, - cras::allocator_t compressedTypeAllocator, - cras::allocator_t compressedMd5SumAllocator, - cras::allocator_t compressedDataAllocator, - size_t serializedConfigLength, - const uint8_t serializedConfig[], - cras::allocator_t errorStringAllocator, - cras::allocator_t logMessagesAllocator + const char * codec, + sensor_msgs::msg::PointCloud2::_height_type rawHeight, + sensor_msgs::msg::PointCloud2::_width_type rawWidth, + size_t rawNumFields, + const char * rawFieldNames[], + sensor_msgs::PointField::_offset_type rawFieldOffsets[], + sensor_msgs::PointField::_datatype_type rawFieldDatatypes[], + sensor_msgs::PointField::_count_type rawFieldCounts[], + sensor_msgs::msg::PointCloud2::_is_bigendian_type rawIsBigendian, + sensor_msgs::msg::PointCloud2::_point_step_type rawPointStep, + sensor_msgs::msg::PointCloud2::_row_step_type rawRowStep, + size_t rawDataLength, + const uint8_t rawData[], + sensor_msgs::msg::PointCloud2::_is_dense_type rawIsDense, + cras::allocator_t compressedTypeAllocator, + cras::allocator_t compressedMd5SumAllocator, + cras::allocator_t compressedDataAllocator, + size_t serializedConfigLength, + const uint8_t serializedConfig[], + cras::allocator_t errorStringAllocator, + cras::allocator_t logMessagesAllocator ) { dynamic_reconfigure::Config config; - if (serializedConfigLength > 0) - { - ros::serialization::IStream data(const_cast(serializedConfig), serializedConfigLength); - try - { + if (serializedConfigLength > 0) { + ros::serialization::IStream data(const_cast(serializedConfig), + serializedConfigLength); + try { ros::serialization::deserialize(data, config); - } - catch (const ros::Exception& e) - { - cras::outputString(errorStringAllocator, cras::format("Could not deserialize encoder config: %s.", e.what())); + } catch (const ros::Exception & e) { + cras::outputString( + errorStringAllocator, + cras::format("Could not deserialize encoder config: %s.", e.what())); return false; } } sensor_msgs::msg::PointCloud2 raw; raw.height = rawHeight; raw.width = rawWidth; - for (size_t i = 0; i < rawNumFields; ++i) - { + for (size_t i = 0; i < rawNumFields; ++i) { sensor_msgs::PointField field; field.name = rawFieldNames[i]; field.offset = rawFieldOffsets[i]; @@ -239,66 +225,65 @@ bool pointCloudTransportCodecsEncode( memcpy(raw.data.data(), rawData, rawDataLength); raw.is_dense = rawIsDense; - auto encoder = point_cloud_transport::point_cloud_transport_codec_instance.getEncoderByName(codec); - if (!encoder) - { + auto encoder = + point_cloud_transport::point_cloud_transport_codec_instance.getEncoderByName(codec); + if (!encoder) { cras::outputString(errorStringAllocator, std::string("Could not find encoder for ") + codec); return false; } const auto compressed = encoder->encode(raw, config); - if (!compressed) - { + if (!compressed) { cras::outputString(errorStringAllocator, compressed.error()); return false; } - if (!compressed.value()) - { + if (!compressed.value()) { return true; } cras::outputString(compressedTypeAllocator, compressed.value()->getDataType()); cras::outputString(compressedMd5SumAllocator, compressed.value()->getMD5Sum()); - cras::outputByteBuffer(compressedDataAllocator, cras::getBuffer(compressed->value()), compressed.value()->size()); + cras::outputByteBuffer( + compressedDataAllocator, cras::getBuffer( + compressed->value()), compressed.value()->size()); return true; } bool pointCloudTransportCodecsDecode( - const char* topicOrCodec, - const char* compressedType, - const char* compressedMd5sum, - size_t compressedDataLength, - const uint8_t compressedData[], - sensor_msgs::msg::PointCloud2::_height_type& rawHeight, - sensor_msgs::msg::PointCloud2::_width_type& rawWidth, - uint32_t& rawNumFields, - cras::allocator_t rawFieldNamesAllocator, - cras::allocator_t rawFieldOffsetsAllocator, - cras::allocator_t rawFieldDatatypesAllocator, - cras::allocator_t rawFieldCountsAllocator, - sensor_msgs::msg::PointCloud2::_is_bigendian_type& rawIsBigEndian, - sensor_msgs::msg::PointCloud2::_point_step_type& rawPointStep, - sensor_msgs::msg::PointCloud2::_row_step_type& rawRowStep, - cras::allocator_t rawDataAllocator, - sensor_msgs::msg::PointCloud2::_is_dense_type& rawIsDense, - size_t serializedConfigLength, - const uint8_t serializedConfig[], - cras::allocator_t errorStringAllocator, - cras::allocator_t logMessagesAllocator + const char * topicOrCodec, + const char * compressedType, + const char * compressedMd5sum, + size_t compressedDataLength, + const uint8_t compressedData[], + sensor_msgs::msg::PointCloud2::_height_type & rawHeight, + sensor_msgs::msg::PointCloud2::_width_type & rawWidth, + uint32_t & rawNumFields, + cras::allocator_t rawFieldNamesAllocator, + cras::allocator_t rawFieldOffsetsAllocator, + cras::allocator_t rawFieldDatatypesAllocator, + cras::allocator_t rawFieldCountsAllocator, + sensor_msgs::msg::PointCloud2::_is_bigendian_type & rawIsBigEndian, + sensor_msgs::msg::PointCloud2::_point_step_type & rawPointStep, + sensor_msgs::msg::PointCloud2::_row_step_type & rawRowStep, + cras::allocator_t rawDataAllocator, + sensor_msgs::msg::PointCloud2::_is_dense_type & rawIsDense, + size_t serializedConfigLength, + const uint8_t serializedConfig[], + cras::allocator_t errorStringAllocator, + cras::allocator_t logMessagesAllocator ) { dynamic_reconfigure::Config config; - if (serializedConfigLength > 0) - { - ros::serialization::IStream data(const_cast(serializedConfig), serializedConfigLength); - try - { + if (serializedConfigLength > 0) { + ros::serialization::IStream data(const_cast(serializedConfig), + serializedConfigLength); + try { ros::serialization::deserialize(data, config); - } - catch (const ros::Exception& e) - { - cras::outputString(errorStringAllocator, cras::format("Could not deserialize decoder config: %s.", e.what())); + } catch (const ros::Exception & e) { + cras::outputString( + errorStringAllocator, + cras::format("Could not deserialize decoder config: %s.", e.what())); return false; } } @@ -309,41 +294,45 @@ bool pointCloudTransportCodecsDecode( memcpy(cras::getBuffer(compressed), compressedData, compressedDataLength); auto decoder = point_cloud_transport::point_cloud_transport_codec_instance.getDecoderByTopic( - topicOrCodec, compressedType); - if (!decoder) - { - decoder = point_cloud_transport::point_cloud_transport_codec_instance.getDecoderByName(topicOrCodec); + topicOrCodec, compressedType); + if (!decoder) { + decoder = point_cloud_transport::point_cloud_transport_codec_instance.getDecoderByName( + topicOrCodec); } - if (!decoder) - { - cras::outputString(errorStringAllocator, std::string("Could not find decoder for ") + topicOrCodec); + if (!decoder) { + cras::outputString( + errorStringAllocator, std::string( + "Could not find decoder for ") + topicOrCodec); return false; } const auto res = decoder->decode(compressed, config); - if (!res) - { + if (!res) { cras::outputString(errorStringAllocator, res.error()); return false; } - if (!res.value()) - { + if (!res.value()) { return true; } - const auto& raw = res->value(); + const auto & raw = res->value(); rawHeight = raw->height; rawWidth = raw->width; rawNumFields = raw->fields.size(); - for (size_t i = 0; i < rawNumFields; ++i) - { + for (size_t i = 0; i < rawNumFields; ++i) { cras::outputString(rawFieldNamesAllocator, raw->fields[i].name); - cras::outputByteBuffer(rawFieldOffsetsAllocator, reinterpret_cast(&raw->fields[i].offset), 4); - cras::outputByteBuffer(rawFieldDatatypesAllocator, reinterpret_cast(&raw->fields[i].datatype), 1); - cras::outputByteBuffer(rawFieldCountsAllocator, reinterpret_cast(&raw->fields[i].count), 4); + cras::outputByteBuffer( + rawFieldOffsetsAllocator, + reinterpret_cast(&raw->fields[i].offset), 4); + cras::outputByteBuffer( + rawFieldDatatypesAllocator, + reinterpret_cast(&raw->fields[i].datatype), 1); + cras::outputByteBuffer( + rawFieldCountsAllocator, + reinterpret_cast(&raw->fields[i].count), 4); } rawIsBigEndian = raw->is_bigendian; rawPointStep = raw->point_step; diff --git a/src/point_cloud_common.cpp b/src/point_cloud_common.cpp index c0ee9fa..1aef5eb 100644 --- a/src/point_cloud_common.cpp +++ b/src/point_cloud_common.cpp @@ -1,30 +1,33 @@ -// Copyright (c) 2009, Willow Garage, Inc. -// -// 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 HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. +/* + * 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 copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ #include "point_cloud_transport/point_cloud_common.hpp" @@ -41,7 +44,9 @@ std::string erase_last_copy(const std::string & input, const std::string & searc return input_copy; } -std::vector split(const std::string& str, const std::string& delimiter, int maxSplits) +std::vector split( + const std::string & str, const std::string & delimiter, + int maxSplits) { // inspired by https://stackoverflow.com/a/46931770/1076564, CC-BY-SA 4.0 // renamed some variables, added the maxSplits option @@ -51,7 +56,10 @@ std::vector split(const std::string& str, const std::string& delimi std::string token; std::vector result; - while ((end = str.find(delimiter, start)) != std::string::npos && (maxSplits == -1 || result.size() < maxSplits)) + while ((end = + str.find( + delimiter, + start)) != std::string::npos && (maxSplits == -1 || result.size() < maxSplits)) { token = str.substr(start, end - start); start = end + delimiterLength; @@ -63,22 +71,22 @@ std::vector split(const std::string& str, const std::string& delimi } // from cras::string_utils -bool endsWith(const std::string& str, const std::string& suffix) +bool endsWith(const std::string & str, const std::string & suffix) { - return str.size() >= suffix.size() && str.compare(str.size() - suffix.size(), suffix.size(), suffix) == 0; + return str.size() >= suffix.size() && str.compare( + str.size() - suffix.size(), + suffix.size(), suffix) == 0; } // from cras::string_utils -std::string removeSuffix(const std::string& str, const std::string& suffix, bool* hadSuffix) +std::string removeSuffix(const std::string & str, const std::string & suffix, bool * hadSuffix) { const auto hasSuffix = endsWith(str, suffix); - if (hadSuffix != nullptr) + if (hadSuffix != nullptr) { *hadSuffix = hasSuffix; + } return hasSuffix ? str.substr(0, str.length() - suffix.length()) : str; } - - - -} \ No newline at end of file +} // namespace point_cloud_transport diff --git a/src/point_cloud_transport.cpp b/src/point_cloud_transport.cpp index 9d2dad3..62978d0 100644 --- a/src/point_cloud_transport.cpp +++ b/src/point_cloud_transport.cpp @@ -1,41 +1,34 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. - /* - * - * BSD 3-Clause License - * - * Copyright (c) Czech Technical University in Prague + * Copyright (c) 2023, Czech Technical University in Prague * Copyright (c) 2019, paplhjak * Copyright (c) 2009, Willow Garage, Inc. - * - * All rights reserved. + * All rights reserved. * * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: + * modification, are permitted provided that the following conditions are met: * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. */ #include @@ -59,8 +52,13 @@ namespace point_cloud_transport { -PointCloudTransportLoader::PointCloudTransportLoader() : pub_loader_(std::make_shared("point_cloud_transport", "point_cloud_transport::PublisherPlugin")), - sub_loader_(std::make_shared("point_cloud_transport", "point_cloud_transport::SubscriberPlugin")) +PointCloudTransportLoader::PointCloudTransportLoader() +: pub_loader_(std::make_shared( + "point_cloud_transport", + "point_cloud_transport::PublisherPlugin")), + sub_loader_(std::make_shared( + "point_cloud_transport", + "point_cloud_transport::SubscriberPlugin")) { } @@ -73,23 +71,21 @@ std::vector PointCloudTransportLoader::getDeclaredTransports() cons auto transports = sub_loader_->getDeclaredClasses(); // Remove the "_sub" at the end of each class name. - for (auto& transport : transports) - { + for (auto & transport : transports) { transport = erase_last_copy(transport, "_sub"); } return transports; } -std::unordered_map PointCloudTransportLoader::getLoadableTransports() const +std::unordered_map PointCloudTransportLoader::getLoadableTransports() const { std::unordered_map loadableTransports; - for (const auto& transportPlugin : sub_loader_->getDeclaredClasses()) - { - // If the plugin loads without throwing an exception, add its transport name to the list of valid plugins, - // otherwise ignore it. - try - { + for (const auto & transportPlugin : sub_loader_->getDeclaredClasses()) { + // If the plugin loads without throwing an exception, add its transport name to the list of + // valid plugins, otherwise ignore it. + try { auto sub = sub_loader_->createSharedInstance(transportPlugin); // Remove the "_sub" at the end of each class name. loadableTransports[erase_last_copy(transportPlugin, "_sub")] = sub->getTransportName(); @@ -115,10 +111,12 @@ SubLoaderPtr PointCloudTransportLoader::getSubscriberLoader() const thread_local std::unique_ptr loader; -point_cloud_transport::PointCloudTransportLoader& getLoader() +point_cloud_transport::PointCloudTransportLoader & getLoader() { - if (point_cloud_transport::loader == nullptr) - point_cloud_transport::loader = std::make_unique(); + if (point_cloud_transport::loader == nullptr) { + point_cloud_transport::loader = + std::make_unique(); + } return *point_cloud_transport::loader; } @@ -129,9 +127,9 @@ PointCloudTransport::PointCloudTransport(rclcpp::Node::SharedPtr node) } - -// TODO: Are these needed? -// void pointCloudTransportGetLoadableTransports(cras::allocator_t transportAllocator, cras::allocator_t nameAllocator) +// TODO(anyone): Are these needed? +// void pointCloudTransportGetLoadableTransports( +// cras::allocator_t transportAllocator, cras::allocator_t nameAllocator) // { // for (const auto& transport : getLoader().getLoadableTransports()) // { @@ -153,7 +151,8 @@ PointCloudTransport::PointCloudTransport(rclcpp::Node::SharedPtr node) // try // { // auto pub = pubLoader->createSharedInstance(transportPlugin); -// auto singleTopicPub = std::dynamic_pointer_cast(pub); +// auto singleTopicPub = +// std::dynamic_pointer_cast(pub); // if (singleTopicPub == nullptr) // continue; // // Remove the "_pub" at the end of each class name. @@ -187,7 +186,8 @@ PointCloudTransport::PointCloudTransport(rclcpp::Node::SharedPtr node) // if (transportClass != transport && sub->getTransportName() != transport) // continue; -// auto singleTopicSub = std::dynamic_pointer_cast(sub); +// auto singleTopicSub = +// std::dynamic_pointer_cast(sub); // if (singleTopicSub == nullptr) // continue; diff --git a/src/point_cloud_transport/__init__.py b/src/point_cloud_transport/__init__.py index d837d2e..c74f4df 100644 --- a/src/point_cloud_transport/__init__.py +++ b/src/point_cloud_transport/__init__.py @@ -1,7 +1,35 @@ -# SPDX-License-Identifier: BSD-3-Clause -# SPDX-FileCopyrightText: Czech Technical University in Prague +# Copyright (c) 2023, Czech Technical University in Prague +# 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 TU Darmstadt nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. -"""Encoding and decoding of point clouds compressed with any point cloud transport. +""" +Encoding and decoding of point clouds compressed with any point cloud transport. Example usage: @@ -12,14 +40,14 @@ raw = PointCloud2() ... # fill the cloud - compressed, err = encode(raw, "draco") + compressed, err = encode(raw, 'draco') if compressed is None: rospy.logerr("Error encoding point cloud: " + err) return False # work with the compressed instance in variable compressed # beware, for decoding, we do not specify "raw", but the codec used for encoding - raw2, err = decode(compressed, "draco") + raw2, err = decode(compressed, 'draco') if raw2 is None: rospy.logerr("Error decoding point cloud: " + err) return False @@ -40,38 +68,40 @@ def main(): raw = PointCloud2() raw.header.stamp = rospy.Time(10) - raw.header.frame_id = "test" + raw.header.frame_id = 'test' raw.width = raw.height = 2 raw.is_bigendian = False raw.is_dense = True raw.point_step = 8 raw.row_step = raw.point_step * 2 raw.fields = [ - PointField(name="x", offset=0, datatype=PointField.INT32, count=1), - PointField(name="y", offset=0, datatype=PointField.INT32, count=1), + PointField(name='x', offset=0, datatype=PointField.INT32, count=1), + PointField(name='y', offset=0, datatype=PointField.INT32, count=1), ] import struct - data = struct.pack('i', 0) + struct.pack('i', 1000) + struct.pack('i', 5000) + struct.pack('i', 9999) - data += struct.pack('i', 0) + struct.pack('i', -1000) + struct.pack('i', -5000) + struct.pack('i', -9999) + data = struct.pack('i', 0) + struct.pack('i', 1000) + \ + struct.pack('i', 5000) + struct.pack('i', 9999) + data += struct.pack('i', 0) + struct.pack('i', -1000) + \ + struct.pack('i', -5000) + struct.pack('i', -9999) import sys if sys.version_info[0] == 2: raw.data = map(ord, data) - rospy.init_node("test_node") - rospy.loginfo("start") + rospy.init_node('test_node') + rospy.loginfo('start') time.sleep(1) start = time.time() for i in range(1): - compressed, err = encode(raw, "draco", {"encode_speed": 1}) - # compressed, err = encode(raw, "draco") + compressed, err = encode(raw, 'draco', {'encode_speed': 1}) + # compressed, err = encode(raw, 'draco') if compressed is None: print(err) break - # raw2, err = decode(compressed, "draco", {"SkipDequantizationPOSITION": True}) - raw2, err = decode(compressed, "draco") + # raw2, err = decode(compressed, 'draco', {"SkipDequantizationPOSITION": True}) + raw2, err = decode(compressed, 'draco') end = time.time() print(end - start) @@ -87,10 +117,13 @@ def cb(msg): def cb_ros(msg): print(msg == compressed) - sub = Subscriber("test", cb, default_transport="draco", queue_size=1) - sub2 = rospy.Subscriber("test", PointCloud2, cb, queue_size=1) - sub3 = rospy.Subscriber("test/draco", type(compressed), cb_ros, queue_size=1) - pub = Publisher("test", queue_size=10) + sub = Subscriber('test', cb, default_transport='draco', queue_size=1) + sub # prevent unused variable warning + sub2 = rospy.Subscriber('test', PointCloud2, cb, queue_size=1) + sub2 # prevent unused variable warning + sub3 = rospy.Subscriber('test/draco', type(compressed), cb_ros, queue_size=1) + sub3 # prevent unused variable warning + pub = Publisher('test', queue_size=10) time.sleep(0.2) pub.publish(raw) rospy.spin() diff --git a/src/point_cloud_transport/common.py b/src/point_cloud_transport/common.py index b7bdc33..1d78d0d 100644 --- a/src/point_cloud_transport/common.py +++ b/src/point_cloud_transport/common.py @@ -1,5 +1,32 @@ -# SPDX-License-Identifier: BSD-3-Clause -# SPDX-FileCopyrightText: Czech Technical University in Prague +# Copyright (c) 2023, Czech Technical University in Prague +# 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 TU Darmstadt nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. """Common definitions.""" @@ -22,6 +49,7 @@ def _get_base_library(): class _TransportInfo(object): + def __init__(self, name, topic, data_type, config_data_type=None): self.name = name self.topic = topic diff --git a/src/point_cloud_transport/decoder.py b/src/point_cloud_transport/decoder.py index 36fe188..15c1a09 100644 --- a/src/point_cloud_transport/decoder.py +++ b/src/point_cloud_transport/decoder.py @@ -1,17 +1,44 @@ -# SPDX-License-Identifier: BSD-3-Clause -# SPDX-FileCopyrightText: Czech Technical University in Prague +# Copyright (c) 2023, Czech Technical University in Prague +# 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 TU Darmstadt nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. """Encoding and decoding of point clouds compressed with any point cloud transport.""" -from ctypes import c_bool, c_uint8, c_uint32, c_char_p, c_size_t, POINTER, byref, sizeof +from ctypes import byref, c_bool, c_char_p, c_size_t, c_uint32, c_uint8, POINTER -from sensor_msgs.msg import PointCloud2, PointField - -from cras.ctypes_utils import Allocator, StringAllocator, BytesAllocator, LogMessagesAllocator, ScalarAllocator, \ - get_ro_c_buffer +from cras.ctypes_utils import Allocator, BytesAllocator, LogMessagesAllocator +from cras.ctypes_utils import get_ro_c_buffer, ScalarAllocator, StringAllocator from cras.message_utils import dict_to_dynamic_config_msg from cras.string_utils import BufferStringIO +from sensor_msgs.msg import PointCloud2, PointField + from .common import _get_base_library @@ -24,7 +51,8 @@ def _get_library(): c_char_p, c_char_p, c_char_p, c_size_t, POINTER(c_uint8), POINTER(c_uint32), POINTER(c_uint32), - POINTER(c_size_t), Allocator.ALLOCATOR, Allocator.ALLOCATOR, Allocator.ALLOCATOR, Allocator.ALLOCATOR, + POINTER(c_size_t), Allocator.ALLOCATOR, + Allocator.ALLOCATOR, Allocator.ALLOCATOR, Allocator.ALLOCATOR, POINTER(c_uint8), POINTER(c_uint32), POINTER(c_uint32), Allocator.ALLOCATOR, POINTER(c_uint8), @@ -36,18 +64,21 @@ def _get_library(): def decode(compressed, topic_or_codec, config=None): - """Decode the given compressed point cloud encoded with any codec into a raw point cloud. + """ + Decode the given compressed point cloud encoded with any codec into a raw point cloud. :param genpy.Message compressed: The compressed point cloud. - :param str topic_or_codec: Name of the topic this cloud comes from or explicit name of the codec. + :param str topic_or_codec: Name of the topic this cloud comes from or explicit name + of the codec. :param config: Configuration of the decoding process. :type config: dict or dynamic_reconfigure.msg.Config or None - :return: Tuple of raw cloud and error string. If the decoding fails, cloud is `None` and error string is filled. + :return: Tuple of raw cloud and error string. If the decoding fails, cloud is `None` + and error string is filled. :rtype: (sensor_msgs.msg.PointCloud2 or None, str) """ codec = _get_library() if codec is None: - return None, "Could not load the codec library." + return None, 'Could not load the codec library.' field_names_allocator = StringAllocator() field_offset_allocator = ScalarAllocator(c_uint32) @@ -77,13 +108,16 @@ def decode(compressed, topic_or_codec, config=None): config_buf.seek(0) args = [ - topic_or_codec.encode("utf-8"), - compressed._type.encode("utf-8"), compressed._md5sum.encode("utf-8"), compressed_buf_len, + topic_or_codec.encode('utf-8'), + compressed._type.encode('utf-8'), compressed._md5sum.encode('utf-8'), + compressed_buf_len, get_ro_c_buffer(compressed_buf), byref(raw_height), byref(raw_width), - byref(raw_num_fields), field_names_allocator.get_cfunc(), field_offset_allocator.get_cfunc(), + byref(raw_num_fields), field_names_allocator.get_cfunc(), + field_offset_allocator.get_cfunc(), field_datatype_allocator.get_cfunc(), field_count_allocator.get_cfunc(), - byref(raw_is_big_endian), byref(raw_point_step), byref(raw_row_step), data_allocator.get_cfunc(), + byref(raw_is_big_endian), byref(raw_point_step), + byref(raw_row_step), data_allocator.get_cfunc(), byref(raw_is_dense), c_size_t(config_buf_len), get_ro_c_buffer(config_buf), error_allocator.get_cfunc(), log_allocator.get_cfunc(), @@ -112,5 +146,5 @@ def decode(compressed, topic_or_codec, config=None): if sys.version_info[0] == 2: raw.data = map(ord, raw.data) raw.is_dense = bool(raw_is_dense.value) - return raw, "" + return raw, '' return None, error_allocator.value diff --git a/src/point_cloud_transport/encoder.py b/src/point_cloud_transport/encoder.py index ab189ea..9243336 100644 --- a/src/point_cloud_transport/encoder.py +++ b/src/point_cloud_transport/encoder.py @@ -1,12 +1,40 @@ -# SPDX-License-Identifier: BSD-3-Clause -# SPDX-FileCopyrightText: Czech Technical University in Prague +# Copyright (c) 2023, Czech Technical University in Prague +# 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 TU Darmstadt nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. """Encoding and decoding of point clouds compressed with any point cloud transport.""" -from ctypes import c_bool, c_uint8, c_uint32, c_char_p, c_size_t, POINTER +from ctypes import c_bool, c_char_p, c_size_t, c_uint32, c_uint8, POINTER from cras import get_msg_type -from cras.ctypes_utils import Allocator, StringAllocator, BytesAllocator, LogMessagesAllocator, get_ro_c_buffer, c_array +from cras.ctypes_utils import Allocator, BytesAllocator, LogMessagesAllocator, StringAllocator +from cras.ctypes_utils import c_array, get_ro_c_buffer from cras.message_utils import dict_to_dynamic_config_msg from cras.string_utils import BufferStringIO @@ -20,7 +48,8 @@ def _get_library(): library.pointCloudTransportCodecsEncode.restype = c_bool library.pointCloudTransportCodecsEncode.argtypes = [ c_char_p, - c_uint32, c_uint32, c_size_t, POINTER(c_char_p), POINTER(c_uint32), POINTER(c_uint8), POINTER(c_uint32), + c_uint32, c_uint32, c_size_t, POINTER(c_char_p), POINTER(c_uint32), + POINTER(c_uint8), POINTER(c_uint32), c_uint8, c_uint32, c_uint32, c_size_t, POINTER(c_uint8), c_uint8, Allocator.ALLOCATOR, Allocator.ALLOCATOR, Allocator.ALLOCATOR, c_size_t, POINTER(c_uint8), @@ -31,19 +60,21 @@ def _get_library(): def encode(raw, topic_or_codec, config=None): - """Encode the given raw point_cloud into a compressed point_cloud with a suitable codec. + """ + Encode the given raw point_cloud into a compressed point_cloud with a suitable codec. :param sensor_msgs.msg.PointCloud2 raw: The raw point cloud. - :param str topic_or_codec: Name of the topic where this cloud should be published or explicit name of the codec. + :param str topic_or_codec: Name of the topic where this cloud should be published or explicit + name of the codec. :param config: Configuration of the encoding process. :type config: dict or dynamic_reconfigure.msg.Config or None - :return: Tuple of compressed cloud and error string. If the compression fails, cloud is `None` and error string - is filled. + :return: Tuple of compressed cloud and error string. If the compression fails, cloud is `None` + and error string is filled. :rtype: (genpy.Message or None, str) """ codec = _get_library() if codec is None: - return None, "Could not load the codec library." + return None, 'Could not load the codec library.' config = dict_to_dynamic_config_msg(config) config_buf = BufferStringIO() @@ -58,10 +89,10 @@ def encode(raw, topic_or_codec, config=None): log_allocator = LogMessagesAllocator() args = [ - topic_or_codec.encode("utf-8"), + topic_or_codec.encode('utf-8'), raw.height, raw.width, len(raw.fields), - c_array([f.name.encode("utf-8") for f in raw.fields], c_char_p), + c_array([f.name.encode('utf-8') for f in raw.fields], c_char_p), c_array([f.offset for f in raw.fields], c_uint32), c_array([f.datatype for f in raw.fields], c_uint8), c_array([f.count for f in raw.fields], c_uint32), @@ -79,9 +110,9 @@ def encode(raw, topic_or_codec, config=None): msg_type = get_msg_type(type_allocator.value) compressed = msg_type() if md5sum_allocator.value != compressed._md5sum: - return None, "MD5 sum mismatch for %s: %s vs %s" % ( + return None, 'MD5 sum mismatch for %s: %s vs %s' % ( type_allocator.value, md5sum_allocator.value, compressed._md5sum) compressed.deserialize(data_allocator.value) compressed.header = raw.header - return compressed, "" + return compressed, '' return None, error_allocator.value diff --git a/src/point_cloud_transport/publisher.py b/src/point_cloud_transport/publisher.py index ede00e8..a61f9ed 100644 --- a/src/point_cloud_transport/publisher.py +++ b/src/point_cloud_transport/publisher.py @@ -1,16 +1,42 @@ -# SPDX-License-Identifier: BSD-3-Clause -# SPDX-FileCopyrightText: Czech Technical University in Prague - +# Copyright (c) 2023, Czech Technical University in Prague +# 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 TU Darmstadt nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. """Publisher that automatically publishes to all declared transports.""" from ctypes import c_char_p +from cras import get_cfg_module, get_msg_type +from cras.ctypes_utils import Allocator, StringAllocator + import dynamic_reconfigure.server import rospy -from cras import get_msg_type, get_cfg_module -from cras.ctypes_utils import Allocator, StringAllocator - from .common import _get_base_library, _TransportInfo from .encoder import encode @@ -26,7 +52,9 @@ def _get_library(): library.pointCloudTransportGetTopicsToPublish.restype = None library.pointCloudTransportGetTopicsToPublish.argtypes = [ c_char_p, - Allocator.ALLOCATOR, Allocator.ALLOCATOR, Allocator.ALLOCATOR, Allocator.ALLOCATOR, Allocator.ALLOCATOR, + Allocator.ALLOCATOR, Allocator.ALLOCATOR, + Allocator.ALLOCATOR, Allocator.ALLOCATOR, + Allocator.ALLOCATOR, ] return library @@ -41,8 +69,9 @@ def _get_topics_to_publish(base_topic): pct = _get_library() pct.pointCloudTransportGetTopicsToPublish( - base_topic.encode("utf-8"), transport_allocator.get_cfunc(), name_allocator.get_cfunc(), - topic_allocator.get_cfunc(), data_type_allocator.get_cfunc(), config_type_allocator.get_cfunc()) + base_topic.encode('utf-8'), transport_allocator.get_cfunc(), name_allocator.get_cfunc(), + topic_allocator.get_cfunc(), data_type_allocator.get_cfunc(), + config_type_allocator.get_cfunc()) topics = {} @@ -51,19 +80,21 @@ def _get_topics_to_publish(base_topic): data_type = get_msg_type(data_type_allocator.values[i]) config_type = get_cfg_module(config_type_allocator.values[i]) topics[transport_allocator.values[i]] = \ - _TransportInfo(name_allocator.values[i], topic_allocator.values[i], data_type, config_type) + _TransportInfo(name_allocator.values[i], topic_allocator.values[i], + data_type, config_type) except ImportError as e: - rospy.logerr("Import error: " + str(e)) + rospy.logerr('Import error: ' + str(e)) return topics class Publisher(object): + def __init__(self, base_topic, *args, **kwargs): self.base_topic = rospy.names.resolve_name(base_topic) self.transports = _get_topics_to_publish(self.base_topic) - blacklist = set(rospy.get_param(self.base_topic + "/disable_pub_plugins", [])) + blacklist = set(rospy.get_param(self.base_topic + '/disable_pub_plugins', [])) self.publishers = {} self.config_servers = {} @@ -75,7 +106,8 @@ def __init__(self, base_topic, *args, **kwargs): topic_to_publish.topic, topic_to_publish.data_type, *args, **kwargs) if topic_to_publish.config_data_type is not None: self.config_servers[transport] = dynamic_reconfigure.server.Server( - topic_to_publish.config_data_type, lambda conf, _: conf, namespace=topic_to_publish.topic) + topic_to_publish.config_data_type, lambda conf, _: conf, + namespace=topic_to_publish.topic) def get_num_subscribers(self): sum([p.get_num_connections() for p in self.publishers.values()]) @@ -85,7 +117,8 @@ def get_topic(self): def publish(self, raw): for transport, transport_info in self.transports.items(): - config = self.config_servers[transport].config if transport in self.config_servers else None + config = (self.config_servers[transport].config if transport in + self.config_servers else None) compressed, err = encode(raw, transport_info.name, config) if compressed is not None: self.publishers[transport].publish(compressed) diff --git a/src/point_cloud_transport/subscriber.py b/src/point_cloud_transport/subscriber.py index 63ee2a7..39aed02 100644 --- a/src/point_cloud_transport/subscriber.py +++ b/src/point_cloud_transport/subscriber.py @@ -1,16 +1,43 @@ -# SPDX-License-Identifier: BSD-3-Clause -# SPDX-FileCopyrightText: Czech Technical University in Prague +# Copyright (c) 2023, Czech Technical University in Prague +# 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 TU Darmstadt nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. """Subscriber automatically converting from any transport to raw.""" from ctypes import c_char_p +from cras import get_cfg_module, get_msg_type +from cras.ctypes_utils import Allocator, StringAllocator + import dynamic_reconfigure.server import rospy -from cras import get_msg_type, get_cfg_module -from cras.ctypes_utils import Allocator, StringAllocator - from .common import _get_base_library, _TransportInfo from .decoder import decode @@ -26,7 +53,9 @@ def _get_library(): library.pointCloudTransportGetTopicToSubscribe.restype = None library.pointCloudTransportGetTopicToSubscribe.argtypes = [ - c_char_p, c_char_p, Allocator.ALLOCATOR, Allocator.ALLOCATOR, Allocator.ALLOCATOR, Allocator.ALLOCATOR, + c_char_p, c_char_p, + Allocator.ALLOCATOR, Allocator.ALLOCATOR, + Allocator.ALLOCATOR, Allocator.ALLOCATOR, ] return library @@ -36,7 +65,8 @@ def _get_loadable_transports(): transport_allocator = StringAllocator() name_allocator = StringAllocator() pct = _get_library() - pct.pointCloudTransportGetLoadableTransports(transport_allocator.get_cfunc(), name_allocator.get_cfunc()) + pct.pointCloudTransportGetLoadableTransports( + transport_allocator.get_cfunc(), name_allocator.get_cfunc()) return dict(zip(transport_allocator.values, name_allocator.values)) @@ -48,7 +78,8 @@ def _get_topic_to_subscribe(base_topic, transport): pct = _get_library() pct.pointCloudTransportGetTopicToSubscribe( - base_topic.encode("utf-8"), transport.encode("utf-8"), name_allocator.get_cfunc(), topic_allocator.get_cfunc(), + base_topic.encode('utf-8'), transport.encode('utf-8'), + name_allocator.get_cfunc(), topic_allocator.get_cfunc(), data_type_allocator.get_cfunc(), config_type_allocator.get_cfunc()) if len(data_type_allocator.values) == 0: @@ -59,17 +90,18 @@ def _get_topic_to_subscribe(base_topic, transport): config_type = get_cfg_module(config_type_allocator.value) return _TransportInfo(name_allocator.value, topic_allocator.value, data_type, config_type) except ImportError as e: - rospy.logerr("Import error: " + str(e)) + rospy.logerr('Import error: ' + str(e)) return None class Subscriber(object): - def __init__(self, base_topic, callback, callback_args=None, default_transport="raw", - parameter_namespace="~", parameter_name="point_cloud_transport", *args, **kwargs): + def __init__(self, base_topic, callback, callback_args=None, default_transport='raw', + parameter_namespace='~', parameter_name='point_cloud_transport', *args, **kwargs): self.base_topic = rospy.names.resolve_name(base_topic) self.callback = callback self.callback_args = callback_args - self.transport = rospy.get_param(rospy.names.ns_join(parameter_namespace, parameter_name), default_transport) + self.transport = rospy.get_param( + rospy.names.ns_join(parameter_namespace, parameter_name), default_transport) transports = _get_loadable_transports() if self.transport not in transports and self.transport not in transports.values(): diff --git a/src/publisher.cpp b/src/publisher.cpp index 1898f61..b2c7189 100644 --- a/src/publisher.cpp +++ b/src/publisher.cpp @@ -1,41 +1,34 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. - /* - * - * BSD 3-Clause License - * - * Copyright (c) Czech Technical University in Prague + * Copyright (c) 2023, Czech Technical University in Prague * Copyright (c) 2019, paplhjak * Copyright (c) 2009, Willow Garage, Inc. - * - * All rights reserved. + * All rights reserved. * * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. + * modification, are permitted provided that the following conditions are met: * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. + * * 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. * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. */ #include @@ -74,8 +67,9 @@ struct Publisher::Impl uint32_t getNumSubscribers() const { uint32_t count = 0; - for (const auto& pub : publishers_) + for (const auto & pub : publishers_) { count += pub->getNumSubscribers(); + } return count; } @@ -91,21 +85,23 @@ struct Publisher::Impl void shutdown() { - if (!unadvertised_) - { + if (!unadvertised_) { unadvertised_ = true; - for (auto& pub : publishers_) + for (auto & pub : publishers_) { pub->shutdown(); + } publishers_.clear(); } } - void subscriberCB(const point_cloud_transport::SingleSubscriberPublisher& plugin_pub, - const point_cloud_transport::SubscriberStatusCallback& user_cb) + void subscriberCB( + const point_cloud_transport::SingleSubscriberPublisher & plugin_pub, + const point_cloud_transport::SubscriberStatusCallback & user_cb) { point_cloud_transport::SingleSubscriberPublisher ssp( - plugin_pub.getSubscriberName(), getTopic(), std::bind(&Publisher::Impl::getNumSubscribers, this), - plugin_pub.publish_fn_); + plugin_pub.getSubscriberName(), getTopic(), + std::bind(&Publisher::Impl::getNumSubscribers, this), + plugin_pub.publish_fn_); user_cb(ssp); } @@ -134,65 +130,63 @@ Publisher::Publisher( // set std::set blacklist(blacklist_vec.begin(), blacklist_vec.end()); - for (const auto& lookup_name : loader->getDeclaredClasses()) - { + for (const auto & lookup_name : loader->getDeclaredClasses()) { const std::string transport_name = erase_last_copy(lookup_name, "_pub"); - if (blacklist.find(transport_name) != blacklist.end()) + if (blacklist.find(transport_name) != blacklist.end()) { continue; + } - try - { + try { auto pub = loader->createUniqueInstance(lookup_name); pub->advertise(node, point_cloud_topic, custom_qos); impl_->publishers_.push_back(std::move(pub)); - } - catch (const std::runtime_error& e) - { - RCLCPP_ERROR(impl_->logger_, "Failed to load plugin %s, error string: %s", lookup_name.c_str(), e.what()); + } catch (const std::runtime_error & e) { + RCLCPP_ERROR( + impl_->logger_, "Failed to load plugin %s, error string: %s", + lookup_name.c_str(), e.what()); } } - if (impl_->publishers_.empty()) - { - throw point_cloud_transport::Exception("No plugins found! Does `rospack plugins --attrib=plugin " - "point_cloud_transport` find any packages?"); + if (impl_->publishers_.empty()) { + throw point_cloud_transport::Exception( + "No plugins found! Does `rospack plugins --attrib=plugin " + "point_cloud_transport` find any packages?"); } } uint32_t Publisher::getNumSubscribers() const { - if (impl_ && impl_->isValid()) + if (impl_ && impl_->isValid()) { return impl_->getNumSubscribers(); + } return 0; } std::string Publisher::getTopic() const { - if (impl_) + if (impl_) { return impl_->getTopic(); + } return {}; } -void Publisher::publish(const sensor_msgs::msg::PointCloud2& message) const +void Publisher::publish(const sensor_msgs::msg::PointCloud2 & message) const { - if (!impl_ || !impl_->isValid()) - { + if (!impl_ || !impl_->isValid()) { // TODO(ros2) Switch to RCUTILS_ASSERT when ros2/rcutils#112 is merged auto logger = impl_ ? impl_->logger_ : rclcpp::get_logger("point_cloud_transport"); RCLCPP_FATAL(logger, "Call to publish() on an invalid point_cloud_transport::Publisher"); return; } - for (const auto& pub : impl_->publishers_) - { - if (pub->getNumSubscribers() > 0) - { + for (const auto & pub : impl_->publishers_) { + if (pub->getNumSubscribers() > 0) { pub->publish(message); } } } -void Publisher::publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& message) const +void Publisher::publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message) const { if (!impl_ || !impl_->isValid()) { // TODO(ros2) Switch to RCUTILS_ASSERT when ros2/rcutils#112 is merged @@ -201,10 +195,8 @@ void Publisher::publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& mes return; } - for (const auto& pub : impl_->publishers_) - { - if (pub->getNumSubscribers() > 0) - { + for (const auto & pub : impl_->publishers_) { + if (pub->getNumSubscribers() > 0) { pub->publish(message); } } @@ -212,22 +204,21 @@ void Publisher::publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& mes void Publisher::shutdown() { - if (impl_) - { + if (impl_) { impl_->shutdown(); impl_.reset(); } } -Publisher::operator void*() const +Publisher::operator void *() const { return (impl_ && impl_->isValid()) ? reinterpret_cast(1) : reinterpret_cast(0); } -// TODO: fix this +// TODO(anyone): fix this // void Publisher::weakSubscriberCb(const ImplWPtr& impl_wptr, -// const point_cloud_transport::SingleSubscriberPublisher& plugin_pub, -// const point_cloud_transport::SubscriberStatusCallback& user_cb) +// const point_cloud_transport::SingleSubscriberPublisher& plugin_pub, +// const point_cloud_transport::SubscriberStatusCallback& user_cb) // { // if (ImplPtr impl = impl_wptr.lock()) // { @@ -235,7 +226,8 @@ Publisher::operator void*() const // } // } -// SubscriberStatusCallback Publisher::rebindCB(const point_cloud_transport::SubscriberStatusCallback& user_cb) +// SubscriberStatusCallback Publisher::rebindCB( +// const point_cloud_transport::SubscriberStatusCallback& user_cb) // { // // Note: the subscriber callback must be bound to the internal Impl object, not // // 'this'. Due to copying behavior the Impl object may outlive the original Publisher @@ -249,4 +241,4 @@ Publisher::operator void*() const // return {}; // } -} +} // namespace point_cloud_transport diff --git a/src/publisher_plugin.cpp b/src/publisher_plugin.cpp index 75e2edd..cf7725d 100644 --- a/src/publisher_plugin.cpp +++ b/src/publisher_plugin.cpp @@ -1,41 +1,34 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. - /* - * - * BSD 3-Clause License - * - * Copyright (c) Czech Technical University in Prague + * Copyright (c) 2023, Czech Technical University in Prague * Copyright (c) 2019, paplhjak * Copyright (c) 2009, Willow Garage, Inc. - * - * All rights reserved. + * All rights reserved. * * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * + * 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 HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. */ #include @@ -47,14 +40,14 @@ namespace point_cloud_transport { -void PublisherPlugin::publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& message) const +void PublisherPlugin::publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message) const { publish(*message); } -std::string PublisherPlugin::getLookupName(const std::string& transport_name) +std::string PublisherPlugin::getLookupName(const std::string & transport_name) { return "point_cloud_transport/" + transport_name + "_pub"; } -} +} // namespace point_cloud_transport diff --git a/src/raw_subscriber.cpp b/src/raw_subscriber.cpp index 4b70c49..cf989e3 100644 --- a/src/raw_subscriber.cpp +++ b/src/raw_subscriber.cpp @@ -1,41 +1,34 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. - /* - * - * BSD 3-Clause License - * - * Copyright (c) Czech Technical University in Prague + * Copyright (c) 2023, Czech Technical University in Prague * Copyright (c) 2019, paplhjak * Copyright (c) 2009, Willow Garage, Inc. - * - * All rights reserved. + * All rights reserved. * * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - * + * 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 HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. */ #include @@ -52,14 +45,16 @@ std::string RawSubscriber::getTransportName() const return "raw"; } -std::string RawSubscriber::getTopicToSubscribe(const std::string& base_topic) const +std::string RawSubscriber::getTopicToSubscribe(const std::string & base_topic) const { return base_topic; } -void RawSubscriber::callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& message, const SubscriberPlugin::Callback& user_cb) +void RawSubscriber::callback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message, + const SubscriberPlugin::Callback & user_cb) { user_cb(message); } -} // namespace point_cloud_transport +} // namespace point_cloud_transport diff --git a/src/republish.cpp b/src/republish.cpp index 0fd96eb..043d919 100644 --- a/src/republish.cpp +++ b/src/republish.cpp @@ -1,41 +1,34 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. - /* - * - * BSD 3-Clause License - * - * Copyright (c) Czech Technical University in Prague + * Copyright (c) 2023, Czech Technical University in Prague * Copyright (c) 2019, paplhjak * Copyright (c) 2009, Willow Garage, Inc. - * - * All rights reserved. + * All rights reserved. * * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. + * modification, are permitted provided that the following conditions are met: * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. + * * 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. * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. */ #include @@ -53,50 +46,53 @@ #include #include -// TODO: Fix this -int main(int argc, char **argv) +// TODO(anyone): Fix this +int main(int argc, char ** argv) { auto vargv = rclcpp::init_and_remove_ros_arguments(argc, argv); - if (vargv.size() < 2) - { + if (vargv.size() < 2) { printf( - "Usage: %s in_transport out_transport --ros-args --remap in:= --ros-args --remap out:=\n", - argv[0]); + "Usage: %s in_transport out_transport --ros-args --remap in:= --ros-args " + "--remap out:=\n", + argv[0]); return 0; } auto node = rclcpp::Node::make_shared("point_cloud_republisher"); std::string in_topic = rclcpp::expand_topic_or_service_name( - "in", - node->get_name(), node->get_namespace()); + "in", + node->get_name(), node->get_namespace()); std::string out_topic = rclcpp::expand_topic_or_service_name( - "out", - node->get_name(), node->get_namespace()); + "out", + node->get_name(), node->get_namespace()); std::string in_transport = vargv[1]; point_cloud_transport::PointCloudTransport pct(node); - if (vargv.size() < 3) - { + if (vargv.size() < 3) { // Use all available transports for output - auto pub = std::make_shared(pct.advertise(out_topic, rmw_qos_profile_default)); + auto pub = + std::make_shared( + pct.advertise( + out_topic, + rmw_qos_profile_default)); // Use Publisher::publish as the subscriber callback - typedef void (point_cloud_transport::Publisher::*PublishMemFn)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr&) const; + typedef void (point_cloud_transport::Publisher::* PublishMemFn)( + const sensor_msgs::msg:: + PointCloud2::ConstSharedPtr &) const; PublishMemFn pub_mem_fn = &point_cloud_transport::Publisher::publish; const point_cloud_transport::TransportHints hint(in_transport); auto sub = pct.subscribe( - in_topic, static_cast(1), - pub_mem_fn, pub, &hint); + in_topic, static_cast(1), + pub_mem_fn, pub, &hint); // spin the node rclcpp::spin(node); - } - else - { + } else { // Use one specific transport for output std::string out_transport = vargv[2]; @@ -112,15 +108,17 @@ int main(int argc, char **argv) pub->advertise(node.get(), out_topic); // Use PublisherPlugin::publish as the subscriber callback - typedef void (point_cloud_transport::PublisherPlugin::*PublishMemFn)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr&) const; + typedef void (point_cloud_transport::PublisherPlugin::* PublishMemFn)( + const sensor_msgs::msg:: + PointCloud2::ConstSharedPtr &) const; PublishMemFn pub_mem_fn = &point_cloud_transport::PublisherPlugin::publish; RCLCPP_INFO(node->get_logger(), "Loading %s subscriber"); const point_cloud_transport::TransportHints hint(in_transport); auto sub = pct.subscribe( - in_topic, static_cast(1), - pub_mem_fn, pub, &hint); + in_topic, static_cast(1), + pub_mem_fn, pub, &hint); RCLCPP_INFO(node->get_logger(), "Spinning node"); // spin the node diff --git a/src/single_subscriber_publisher.cpp b/src/single_subscriber_publisher.cpp index d77eee5..6711a0f 100644 --- a/src/single_subscriber_publisher.cpp +++ b/src/single_subscriber_publisher.cpp @@ -1,41 +1,34 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. - /* - * - * BSD 3-Clause License - * - * Copyright (c) Czech Technical University in Prague + * Copyright (c) 2023, Czech Technical University in Prague * Copyright (c) 2019, paplhjak * Copyright (c) 2009, Willow Garage, Inc. - * - * All rights reserved. + * All rights reserved. * * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. + * modification, are permitted provided that the following conditions are met: * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. + * * 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. * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. */ #include @@ -73,14 +66,15 @@ uint32_t SingleSubscriberPublisher::getNumSubscribers() const return num_subscribers_fn_(); } -void SingleSubscriberPublisher::publish(const sensor_msgs::msg::PointCloud2& message) const +void SingleSubscriberPublisher::publish(const sensor_msgs::msg::PointCloud2 & message) const { publish_fn_(message); } -void SingleSubscriberPublisher::publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& message) const +void SingleSubscriberPublisher::publish( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message) const { publish_fn_(*message); } -} +} // namespace point_cloud_transport diff --git a/src/subscriber.cpp b/src/subscriber.cpp index b25a480..249141f 100644 --- a/src/subscriber.cpp +++ b/src/subscriber.cpp @@ -1,41 +1,34 @@ -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague .. 2019, paplhjak .. 2009, Willow Garage, Inc. - /* - * - * BSD 3-Clause License - * - * Copyright (c) Czech Technical University in Prague + * Copyright (c) 2023, Czech Technical University in Prague * Copyright (c) 2019, paplhjak * Copyright (c) 2009, Willow Garage, Inc. - * - * All rights reserved. + * All rights reserved. * * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. + * modification, are permitted provided that the following conditions are met: * - * 2. Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. * - * 3. Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. + * * 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. * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. */ #include @@ -76,11 +69,11 @@ struct Subscriber::Impl void shutdown() { - if (!unsubscribed_) - { + if (!unsubscribed_) { unsubscribed_ = true; - if (subscriber_) + if (subscriber_) { subscriber_->shutdown(); + } } } @@ -103,11 +96,9 @@ Subscriber::Subscriber( { // Load the plugin for the chosen transport. std::string lookup_name = SubscriberPlugin::getLookupName(transport); - try - { + try { impl_->subscriber_ = loader->createSharedInstance(lookup_name); - } - catch (pluginlib::PluginlibException& e) { + } catch (pluginlib::PluginlibException & e) { throw point_cloud_transport::TransportLoadException(transport, e.what()); } // Hang on to the class loader so our shared library doesn't disappear from under us. @@ -117,7 +108,7 @@ Subscriber::Subscriber( std::string clean_topic = base_topic; size_t found = clean_topic.rfind('/'); if (found != std::string::npos) { - std::string transport = clean_topic.substr(found+1); + std::string transport = clean_topic.substr(found + 1); std::string plugin_name = SubscriberPlugin::getLookupName(transport); std::vector plugins = loader->getDeclaredClasses(); if (std::find(plugins.begin(), plugins.end(), plugin_name) != plugins.end()) { @@ -126,9 +117,9 @@ Subscriber::Subscriber( RCLCPP_WARN( impl_->logger_, "[point_cloud_transport] It looks like you are trying to subscribe directly to a " - "transport-specific point_cloud topic '%s', in which case you will likely get a connection " - "error. Try subscribing to the base topic '%s' instead with parameter ~point_cloud_transport " - "set to '%s' (on the command line, _point_cloud_transport:=%s). " + "transport-specific point_cloud topic '%s', in which case you will likely get a " + "connection error. Try subscribing to the base topic '%s' instead with parameter " + "~point_cloud_transport set to '%s' (on the command line, _point_cloud_transport:=%s). " "See http://ros.org/wiki/point_cloud_transport for details.", clean_topic.c_str(), real_base_topic.c_str(), transport.c_str(), transport.c_str()); } @@ -141,34 +132,38 @@ Subscriber::Subscriber( std::string Subscriber::getTopic() const { - if (impl_) + if (impl_) { return impl_->subscriber_->getTopic(); + } return {}; } uint32_t Subscriber::getNumPublishers() const { - if (impl_) + if (impl_) { return impl_->subscriber_->getNumPublishers(); + } return 0; } std::string Subscriber::getTransport() const { - if (impl_) + if (impl_) { return impl_->subscriber_->getTransportName(); + } return {}; } void Subscriber::shutdown() { - if (impl_) + if (impl_) { impl_->shutdown(); + } } -Subscriber::operator void*() const +Subscriber::operator void *() const { return (impl_ && impl_->isValid()) ? reinterpret_cast(1) : reinterpret_cast(0); } -} +} // namespace point_cloud_transport From a3ea8ebaba3adf4ca60524767fa75fca751d6b5a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 10 Jul 2023 13:19:34 +0200 Subject: [PATCH 15/28] Refactor republisher (#4) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- CMakeLists.txt | 32 ++++- README.md | 29 ++-- .../point_cloud_transport.hpp | 7 +- include/point_cloud_transport/publisher.hpp | 2 +- .../publisher_plugin.hpp | 4 +- .../point_cloud_transport/raw_subscriber.hpp | 2 +- include/point_cloud_transport/republish.hpp | 66 +++++++++ .../simple_publisher_plugin.hpp | 6 +- .../simple_subscriber_plugin.hpp | 4 +- include/point_cloud_transport/subscriber.hpp | 2 +- .../subscriber_filter.hpp | 4 +- .../subscriber_plugin.hpp | 10 +- .../point_cloud_transport/transport_hints.hpp | 8 +- .../visibility_control.h | 58 ++++++++ package.xml | 2 +- src/list_transports.cpp | 48 ++++--- src/publisher.cpp | 5 +- src/republish.cpp | 133 ++++++++++++------ src/republish_program.cpp | 74 ++++++++++ src/subscriber.cpp | 4 +- src/utilities/utilities.cpp | 45 ++++++ src/utilities/utilities.hpp | 35 +++++ 22 files changed, 467 insertions(+), 113 deletions(-) create mode 100644 include/point_cloud_transport/republish.hpp create mode 100644 include/point_cloud_transport/visibility_control.h create mode 100644 src/republish_program.cpp create mode 100644 src/utilities/utilities.cpp create mode 100644 src/utilities/utilities.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 7b0756b..5aac17b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,6 +9,7 @@ find_package(ament_cmake_ros REQUIRED) find_package(message_filters REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(sensor_msgs REQUIRED) include_directories( @@ -48,11 +49,34 @@ target_link_libraries(${PROJECT_NAME}_plugins PRIVATE ${PROJECT_NAME} pluginlib::pluginlib) -# TODO: Make this a composition -add_executable(republish src/republish.cpp) +add_library(republish_node SHARED src/republish.cpp) +target_link_libraries(republish_node + ${PROJECT_NAME} +) +ament_target_dependencies(republish_node + pluginlib + rclcpp_components + rclcpp +) +rclcpp_components_register_nodes(republish_node "point_cloud_transport::Republisher") + +add_executable(republish + src/republish_program.cpp + src/utilities/utilities.cpp +) target_link_libraries(republish ${PROJECT_NAME} - pluginlib::pluginlib) + republish_node +) +ament_target_dependencies(republish + pluginlib + rclcpp_components + rclcpp +) + +target_include_directories(${PROJECT_NAME} PUBLIC + "$" + "$") # Build list_transports add_executable(list_transports src/list_transports.cpp) @@ -63,7 +87,7 @@ target_link_libraries(list_transports # Install plugin descriptions pluginlib_export_plugin_description_file(${PROJECT_NAME} default_plugins.xml) -install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_plugins +install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_plugins republish_node ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin diff --git a/README.md b/README.md index 6cef231..087e294 100644 --- a/README.md +++ b/README.md @@ -2,19 +2,19 @@ ## Description -[point_cloud_transport](https://github.com/ctu-vras/point_cloud_transport) is a [ROS](https://www.ros.org/) package for subscribing to and publishing [PointCloud2](http://docs.ros.org/latest/api/sensor_msgs/html/msg/PointCloud2.html) messages via different transport layers. +[point_cloud_transport](https://github.com/john-maidbot/point_cloud_transport) is a [ROS 2](https://www.ros.org/) package for subscribing to and publishing [PointCloud2](http://docs.ros.org/latest/api/sensor_msgs/html/msg/PointCloud2.html) messages via different transport layers. E.g. it can provide support for transporting point clouds in low-bandwidth environment using [Draco](https://github.com/google/draco) compression library. -[point_cloud_transport](https://github.com/ctu-vras/point_cloud_transport) is released as C++ source code and binary packages via ROS buildfarm. +[point_cloud_transport](https://github.com/john-maidbot/point_cloud_transport) is **NOT yet** released as C++ source code and binary packages via ROS buildfarm. ## Usage -[point_cloud_transport](https://github.com/ctu-vras/point_cloud_transport) can be used to publish and subscribe to [PointCloud2](http://docs.ros.org/latest/api/sensor_msgs/html/msg/PointCloud2.html) messages. At this level of usage, it is similar to using ROS Publishers and Subscribers. Using [point_cloud_transport](https://github.com/ctu-vras/point_cloud_transport) instead of the ROS primitives, however, gives the user much greater flexibility in how point clouds are communicated between nodes. +[point_cloud_transport](https://github.com/john-maidbot/point_cloud_transport) can be used to publish and subscribe to [PointCloud2](http://docs.ros.org/latest/api/sensor_msgs/html/msg/PointCloud2.html) messages. At this level of usage, it is similar to using ROS 2 Publishers and Subscribers. Using [point_cloud_transport](https://github.com/john-maidbot/point_cloud_transport) instead of the ROS 2 primitives, however, gives the user much greater flexibility in how point clouds are communicated between nodes. -For complete examples of publishing and subscribing to point clouds using [point_cloud_transport](https://github.com/ctu-vras/point_cloud_transport) , see [Tutorial](https://github.com/ctu-vras/point_cloud_transport_tutorial). +For complete examples of publishing and subscribing to point clouds using [point_cloud_transport](https://github.com/john-maidbot/point_cloud_transport), see [Tutorial](https://github.com/ahcorde/point_cloud_transport_tutorial). ### C++ -Communicating PointCloud2 messages using [point_cloud_transport](https://github.com/ctu-vras/point_cloud_transport): +Communicating PointCloud2 messages using [point_cloud_transport](https://github.com/john-maidbot/point_cloud_transport): ```cpp #include #include @@ -30,13 +30,10 @@ point_cloud_transport::Subscriber sub = pct.subscribe("in_point_cloud_base_topic point_cloud_transport::Publisher pub = pct.advertise("out_point_cloud_base_topic", 1); ``` -### Republish node(let) +### Republish rclcpp component -Similar to image_transport, this package provides a node(let) called `republish` that can convert between different transports. It can be used in a launch file in the following way: +TODO -```xml - -``` ## Known transports @@ -45,13 +42,20 @@ Similar to image_transport, this package provides a node(let) called `republish` ## Authors +### ROS 1 Version: + * **Jakub Paplhám** - *Initial work* - [paplhjak](https://github.com/paplhjak) * **Ing. Tomáš Petříček, Ph.D.** - *Supervisor* - [tpet](https://github.com/tpet) * **Martin Pecka** - *Maintainer* - [peci1](https://github.com/peci1) +### ROS 2 Version: + + * **John D'Angelo** - *Port to ROS 2 and Maintainer* - [john-maidbot](https://github.com/john-maidbot) + * **Alejandro Hernández** - *Port to ROS 2 and Maintainer* - [ahcorde](https://github.com/ahcorde) + ## License -This project is licensed under the BSD License - see the [LICENSE](https://github.com/ctu-vras/point_cloud_transport/blob/master/LICENSE) file for details. +This project is licensed under the BSD License - see the [LICENSE](https://github.com/john-maidbot/point_cloud_transport/blob/master/LICENSE) file for details. ## Acknowledgments @@ -63,8 +67,7 @@ Support For questions/comments please email the maintainer mentioned in `package.xml`. -If you have found an error in this package, please file an issue at: [https://github.com/ctu-vras/point_cloud_transport/issues]() +If you have found an error in this package, please file an issue at: [https://github.com/john-maidbot/point_cloud_transport/issues]() Patches are encouraged, and may be submitted by forking this project and submitting a pull request through GitHub. Any help is further development of the project is much appreciated. - diff --git a/include/point_cloud_transport/point_cloud_transport.hpp b/include/point_cloud_transport/point_cloud_transport.hpp index 89e2e74..e12eac8 100644 --- a/include/point_cloud_transport/point_cloud_transport.hpp +++ b/include/point_cloud_transport/point_cloud_transport.hpp @@ -88,7 +88,6 @@ class PointCloudTransportLoader point_cloud_transport::SubLoaderPtr sub_loader_; }; - /** * Advertise and subscribe to PointCloud2 topics. * @@ -110,7 +109,7 @@ class PointCloudTransport : public PointCloudTransportLoader { std::string ret; if (nullptr == transport_hints) { - TransportHints th(node_.get()); + TransportHints th(node_); ret = th.getTransport(); } else { ret = transport_hints->getTransport(); @@ -123,7 +122,7 @@ class PointCloudTransport : public PointCloudTransportLoader const std::string & base_topic, rmw_qos_profile_t custom_qos) { - return Publisher(node_.get(), base_topic, pub_loader_, custom_qos); + return Publisher(node_, base_topic, pub_loader_, custom_qos); } //! Advertise an PointCloud2 topic with subscriber status callbacks. @@ -144,7 +143,7 @@ class PointCloudTransport : public PointCloudTransportLoader custom_qos.depth = queue_size; rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions(); return Subscriber( - node_.get(), base_topic, callback, sub_loader_, + node_, base_topic, callback, sub_loader_, getTransportOrDefault(transport_hints), custom_qos, options); } diff --git a/include/point_cloud_transport/publisher.hpp b/include/point_cloud_transport/publisher.hpp index e689bb3..3f909e9 100644 --- a/include/point_cloud_transport/publisher.hpp +++ b/include/point_cloud_transport/publisher.hpp @@ -57,7 +57,7 @@ class Publisher Publisher() = default; Publisher( - rclcpp::Node * node, + std::shared_ptr node, const std::string & base_topic, PubLoaderPtr loader, rmw_qos_profile_t custom_qos); diff --git a/include/point_cloud_transport/publisher_plugin.hpp b/include/point_cloud_transport/publisher_plugin.hpp index e513065..296acb8 100644 --- a/include/point_cloud_transport/publisher_plugin.hpp +++ b/include/point_cloud_transport/publisher_plugin.hpp @@ -61,7 +61,7 @@ class PublisherPlugin * \brief Advertise a topic, simple version. */ void advertise( - rclcpp::Node * nh, + std::shared_ptr nh, const std::string & base_topic, rmw_qos_profile_t custom_qos = rmw_qos_profile_default) { @@ -91,7 +91,7 @@ class PublisherPlugin * \brief Advertise a topic. Must be implemented by the subclass. */ virtual void advertiseImpl( - rclcpp::Node * nh, const std::string & base_topic, + std::shared_ptr nh, const std::string & base_topic, rmw_qos_profile_t custom_qos) = 0; }; diff --git a/include/point_cloud_transport/raw_subscriber.hpp b/include/point_cloud_transport/raw_subscriber.hpp index 772fa75..13e2824 100644 --- a/include/point_cloud_transport/raw_subscriber.hpp +++ b/include/point_cloud_transport/raw_subscriber.hpp @@ -82,7 +82,7 @@ class RawSubscriber using SubscriberPlugin::subscribeImpl; void subscribeImpl( - rclcpp::Node * node, + std::shared_ptr node, const std::string & base_topic, const Callback & callback, rmw_qos_profile_t custom_qos, diff --git a/include/point_cloud_transport/republish.hpp b/include/point_cloud_transport/republish.hpp new file mode 100644 index 0000000..7f82991 --- /dev/null +++ b/include/point_cloud_transport/republish.hpp @@ -0,0 +1,66 @@ +/* + * Copyright (c) 2023, Czech Technical University in Prague + * Copyright (c) 2019, paplhjak + * 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 copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef POINT_CLOUD_TRANSPORT__REPUBLISH_HPP_ +#define POINT_CLOUD_TRANSPORT__REPUBLISH_HPP_ + +#include + +#include + +#include +#include "point_cloud_transport/visibility_control.h" + +namespace point_cloud_transport +{ + +class Republisher : public rclcpp::Node +{ +public: + //! Constructor + POINT_CLOUD_TRANSPORT_PUBLIC + explicit Republisher(const rclcpp::NodeOptions & options); + +private: + void initialize(); + + std::shared_ptr pct; + rclcpp::TimerBase::SharedPtr timer_; + bool initialized_{false}; + point_cloud_transport::Subscriber sub; + std::shared_ptr pub; + std::shared_ptr simple_pub; +}; + +} // namespace point_cloud_transport +#endif // POINT_CLOUD_TRANSPORT__REPUBLISH_HPP_ diff --git a/include/point_cloud_transport/simple_publisher_plugin.hpp b/include/point_cloud_transport/simple_publisher_plugin.hpp index d4f0546..bb6f2e8 100644 --- a/include/point_cloud_transport/simple_publisher_plugin.hpp +++ b/include/point_cloud_transport/simple_publisher_plugin.hpp @@ -152,7 +152,7 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin std::string base_topic_; virtual void advertiseImpl( - rclcpp::Node * node, const std::string & base_topic, + std::shared_ptr node, const std::string & base_topic, rmw_qos_profile_t custom_qos) { base_topic_ = base_topic; @@ -203,13 +203,13 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin private: struct SimplePublisherPluginImpl { - explicit SimplePublisherPluginImpl(rclcpp::Node * node) + explicit SimplePublisherPluginImpl(std::shared_ptr node) : node_(node), logger_(node->get_logger()) { } - rclcpp::Node * node_; + std::shared_ptr node_; rclcpp::Logger logger_; typename rclcpp::Publisher::SharedPtr pub_; }; diff --git a/include/point_cloud_transport/simple_subscriber_plugin.hpp b/include/point_cloud_transport/simple_subscriber_plugin.hpp index 9754855..9274be0 100644 --- a/include/point_cloud_transport/simple_subscriber_plugin.hpp +++ b/include/point_cloud_transport/simple_subscriber_plugin.hpp @@ -130,7 +130,7 @@ class SimpleSubscriberPlugin : public SubscriberPlugin } void subscribeImpl( - rclcpp::Node * node, + std::shared_ptr node, const std::string & base_topic, const Callback & callback, rmw_qos_profile_t custom_qos) override @@ -148,7 +148,7 @@ class SimpleSubscriberPlugin : public SubscriberPlugin } void subscribeImplWithOptions( - rclcpp::Node * node, + std::shared_ptr node, const std::string & base_topic, const Callback & callback, rmw_qos_profile_t custom_qos, diff --git a/include/point_cloud_transport/subscriber.hpp b/include/point_cloud_transport/subscriber.hpp index 108ff94..b4f1333 100644 --- a/include/point_cloud_transport/subscriber.hpp +++ b/include/point_cloud_transport/subscriber.hpp @@ -73,7 +73,7 @@ class Subscriber Subscriber() = default; Subscriber( - rclcpp::Node * node, + std::shared_ptr node, const std::string & base_topic, const Callback & callback, SubLoaderPtr loader, diff --git a/include/point_cloud_transport/subscriber_filter.hpp b/include/point_cloud_transport/subscriber_filter.hpp index c2bb5b2..e47b158 100644 --- a/include/point_cloud_transport/subscriber_filter.hpp +++ b/include/point_cloud_transport/subscriber_filter.hpp @@ -76,7 +76,7 @@ class SubscriberFilter : public message_filters::SimpleFilter node, const std::string & base_topic, const std::string & transport) { subscribe(pct, node, base_topic, transport); @@ -104,7 +104,7 @@ class SubscriberFilter : public message_filters::SimpleFilter node, const std::string & base_topic, const std::string & transport, rmw_qos_profile_t custom_qos = rmw_qos_profile_default, diff --git a/include/point_cloud_transport/subscriber_plugin.hpp b/include/point_cloud_transport/subscriber_plugin.hpp index f184e7c..42cfdbf 100644 --- a/include/point_cloud_transport/subscriber_plugin.hpp +++ b/include/point_cloud_transport/subscriber_plugin.hpp @@ -79,7 +79,7 @@ class SubscriberPlugin * \brief Subscribe to an pointcloud topic, version for arbitrary std::function object. */ void subscribe( - rclcpp::Node * node, const std::string & base_topic, + std::shared_ptr node, const std::string & base_topic, const Callback & callback, rmw_qos_profile_t custom_qos = rmw_qos_profile_default, rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()) @@ -91,7 +91,7 @@ class SubscriberPlugin * \brief Subscribe to an pointcloud topic, version for bare function. */ void subscribe( - rclcpp::Node * node, const std::string & base_topic, + std::shared_ptr node, const std::string & base_topic, void (* fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &), rmw_qos_profile_t custom_qos = rmw_qos_profile_default, rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()) @@ -107,7 +107,7 @@ class SubscriberPlugin */ template void subscribe( - rclcpp::Node * node, const std::string & base_topic, + std::shared_ptr node, const std::string & base_topic, void (T::* fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &), T * obj, rmw_qos_profile_t custom_qos = rmw_qos_profile_default, rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()) @@ -146,13 +146,13 @@ class SubscriberPlugin * Subscribe to a point cloud transport topic. Must be implemented by the subclass. */ virtual void subscribeImpl( - rclcpp::Node * node, + std::shared_ptr node, const std::string & base_topic, const Callback & callback, rmw_qos_profile_t custom_qos = rmw_qos_profile_default) = 0; virtual void subscribeImpl( - rclcpp::Node * node, + std::shared_ptr node, const std::string & base_topic, const Callback & callback, rmw_qos_profile_t custom_qos, diff --git a/include/point_cloud_transport/transport_hints.hpp b/include/point_cloud_transport/transport_hints.hpp index e059a14..9d2a0a3 100644 --- a/include/point_cloud_transport/transport_hints.hpp +++ b/include/point_cloud_transport/transport_hints.hpp @@ -34,9 +34,10 @@ #ifndef POINT_CLOUD_TRANSPORT__TRANSPORT_HINTS_HPP_ #define POINT_CLOUD_TRANSPORT__TRANSPORT_HINTS_HPP_ +#include #include -#include "rclcpp/node.hpp" +#include namespace point_cloud_transport { @@ -59,10 +60,11 @@ class TransportHints * */ TransportHints( - const rclcpp::Node * node, + const std::shared_ptr node, const std::string & default_transport = "raw", const std::string & parameter_name = "point_cloud_transport") { + node->declare_parameter(parameter_name, transport_); node->get_parameter_or(parameter_name, transport_, default_transport); } @@ -78,7 +80,7 @@ class TransportHints } private: - std::string transport_; + std::string transport_{"raw"}; }; } // namespace point_cloud_transport diff --git a/include/point_cloud_transport/visibility_control.h b/include/point_cloud_transport/visibility_control.h new file mode 100644 index 0000000..aa68c84 --- /dev/null +++ b/include/point_cloud_transport/visibility_control.h @@ -0,0 +1,58 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef POINT_CLOUD_TRANSPORT__VISIBILITY_CONTROL_H_ +#define POINT_CLOUD_TRANSPORT__VISIBILITY_CONTROL_H_ + +#ifdef __cplusplus +extern "C" +{ +#endif + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define POINT_CLOUD_TRANSPORT_EXPORT __attribute__ ((dllexport)) + #define POINT_CLOUD_TRANSPORT_IMPORT __attribute__ ((dllimport)) + #else + #define POINT_CLOUD_TRANSPORT_EXPORT __declspec(dllexport) + #define POINT_CLOUD_TRANSPORT_IMPORT __declspec(dllimport) + #endif + #ifdef POINT_CLOUD_TRANSPORT_BUILDING_DLL + #define POINT_CLOUD_TRANSPORT_PUBLIC POINT_CLOUD_TRANSPORT_EXPORT + #else + #define POINT_CLOUD_TRANSPORT_PUBLIC POINT_CLOUD_TRANSPORT_IMPORT + #endif + #define POINT_CLOUD_TRANSPORT_PUBLIC_TYPE POINT_CLOUD_TRANSPORT_PUBLIC + #define POINT_CLOUD_TRANSPORT_LOCAL +#else + #define POINT_CLOUD_TRANSPORT_EXPORT __attribute__ ((visibility("default"))) + #define POINT_CLOUD_TRANSPORT_IMPORT + #if __GNUC__ >= 4 + #define POINT_CLOUD_TRANSPORT_PUBLIC __attribute__ ((visibility("default"))) + #define POINT_CLOUD_TRANSPORT_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define POINT_CLOUD_TRANSPORT_PUBLIC + #define POINT_CLOUD_TRANSPORT_LOCAL + #endif + #define POINT_CLOUD_TRANSPORT_PUBLIC_TYPE +#endif + +#ifdef __cplusplus +} +#endif + +#endif // POINT_CLOUD_TRANSPORT__VISIBILITY_CONTROL_H_ diff --git a/package.xml b/package.xml index c2548f7..78cf1fd 100644 --- a/package.xml +++ b/package.xml @@ -9,7 +9,7 @@ Jakub Paplham - Martin Pecka + Alejandro Hernández John D'Angelo BSD diff --git a/src/list_transports.cpp b/src/list_transports.cpp index 21282f9..64a18a6 100644 --- a/src/list_transports.cpp +++ b/src/list_transports.cpp @@ -31,6 +31,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include #include #include #include @@ -73,9 +74,9 @@ int main(int argc, char ** argv) StatusMap transports; for (const auto & lookup_name : pub_loader.getDeclaredClasses()) { - printf("Lookup name: %s\n", lookup_name.c_str()); + std::cout << "Lookup name: " << lookup_name.c_str() << std::endl; std::string transport_name = point_cloud_transport::erase_last_copy(lookup_name, "_pub"); - printf("Transport name: %s\n", transport_name.c_str()); + std::cout << "Transport name: " << transport_name.c_str() << std::endl; transports[transport_name].pub_name = lookup_name; transports[transport_name].package_name = pub_loader.getClassPackage(lookup_name); try { @@ -83,10 +84,10 @@ int main(int argc, char ** argv) transports[transport_name].pub_status = SUCCESS; } catch (const pluginlib::LibraryLoadException & e) { transports[transport_name].pub_status = LIB_LOAD_FAILURE; - printf("LibraryLoadException: %s\n", e.what()); + std::cout << "LibraryLoadException: " << e.what() << std::endl; } catch (const pluginlib::CreateClassException & e) { transports[transport_name].pub_status = CREATE_FAILURE; - printf("CreateClassException: %s\n", e.what()); + std::cout << "CreateClassException: " << e.what() << std::endl; } } @@ -99,53 +100,56 @@ int main(int argc, char ** argv) transports[transport_name].sub_status = SUCCESS; } catch (const pluginlib::LibraryLoadException & e) { transports[transport_name].sub_status = LIB_LOAD_FAILURE; - printf("LibraryLoadException: %s\n", e.what()); + std::cout << "LibraryLoadException: " << e.what() << std::endl; } catch (const pluginlib::CreateClassException & e) { transports[transport_name].sub_status = CREATE_FAILURE; - printf("CreateClassException: %s\n", e.what()); + std::cout << "CreateClassException: " << e.what() << std::endl; } } bool problem_package = false; - printf("Declared transports:\n"); + std::cout << "Declared transports:" << std::endl; for (const StatusMap::value_type & value : transports) { const TransportDesc & td = value.second; - printf("%s", value.first.c_str()); + std::cout << value.first.c_str() << std::endl; if ((td.pub_status == CREATE_FAILURE || td.pub_status == LIB_LOAD_FAILURE) || (td.sub_status == CREATE_FAILURE || td.sub_status == LIB_LOAD_FAILURE)) { - printf(" (*): Not available. Try 'catkin_make --pkg %s'.", td.package_name.c_str()); + std::cout << " (*): Not available. Try 'colcon build --packages-select " + << td.package_name.c_str() << "'." << std::endl; problem_package = true; } - printf("\n"); + std::cout << std::endl; } if (problem_package) { - printf("(*) \n"); + std::cout << "(*) " << std::endl; } - printf("\nDetails:\n"); + std::cout << std::endl << "Details:" << std::endl; for (const auto & value : transports) { const TransportDesc & td = value.second; - printf("----------\n"); - printf("\"%s\"\n", value.first.c_str()); + std::cout << "----------" << std::endl; + std::cout << "\"" << value.first.c_str() << "\"" << std::endl; if (td.pub_status == CREATE_FAILURE || td.sub_status == CREATE_FAILURE) { - printf( + std::cout << "*** Plugins are built, but could not be loaded. The package may need to be rebuilt " - "or may not be compatible with this release of point_cloud_common. ***\n"); + "or may not be compatible with this release of point_cloud_common. ***" << std::endl; } else if (td.pub_status == LIB_LOAD_FAILURE || td.sub_status == LIB_LOAD_FAILURE) { - printf("*** Plugins are not built. ***\n"); + std::cout << "*** Plugins are not built. ***" << std::endl; } - printf(" - Provided by package: %s\n", td.package_name.c_str()); + std::cout << " - Provided by package: " << td.package_name.c_str() << std::endl; if (td.pub_status == DOES_NOT_EXIST) { - printf(" - No publisher provided\n"); + std::cout << " - No publisher provided" << std::endl; } else { - printf(" - Publisher: %s\n", pub_loader.getClassDescription(td.pub_name).c_str()); + std::cout << " - Publisher: " << pub_loader.getClassDescription(td.pub_name).c_str() + << std::endl; } if (td.sub_status == DOES_NOT_EXIST) { - printf(" - No subscriber provided\n"); + std::cout << " - No subscriber provided" << std::endl; } else { - printf(" - Subscriber: %s\n", sub_loader.getClassDescription(td.sub_name).c_str()); + std::cout << " - Subscriber: " << sub_loader.getClassDescription(td.sub_name).c_str() + << std::endl; } } diff --git a/src/publisher.cpp b/src/publisher.cpp index b2c7189..3d0a6e9 100644 --- a/src/publisher.cpp +++ b/src/publisher.cpp @@ -53,7 +53,7 @@ namespace point_cloud_transport struct Publisher::Impl { - explicit Impl(rclcpp::Node * node) + explicit Impl(std::shared_ptr node) : logger_(node->get_logger()), unadvertised_(false) { @@ -113,7 +113,7 @@ struct Publisher::Impl }; Publisher::Publisher( - rclcpp::Node * node, const std::string & base_topic, + std::shared_ptr node, const std::string & base_topic, PubLoaderPtr loader, rmw_qos_profile_t custom_qos) : impl_(std::make_shared(node)) { @@ -139,6 +139,7 @@ Publisher::Publisher( try { auto pub = loader->createUniqueInstance(lookup_name); pub->advertise(node, point_cloud_topic, custom_qos); + impl_->base_topic_ = pub->getTopic(); impl_->publishers_.push_back(std::move(pub)); } catch (const std::runtime_error & e) { RCLCPP_ERROR( diff --git a/src/republish.cpp b/src/republish.cpp index 043d919..f32768d 100644 --- a/src/republish.cpp +++ b/src/republish.cpp @@ -35,51 +35,90 @@ #include #include -#include "rclcpp/rclcpp.hpp" +#include #include #include -#include -#include -#include -#include -#include +#include "point_cloud_transport/exception.hpp" +#include "point_cloud_transport/point_cloud_transport.hpp" +#include "point_cloud_transport/publisher.hpp" +#include "point_cloud_transport/publisher_plugin.hpp" +#include "point_cloud_transport/republish.hpp" +#include "point_cloud_transport/subscriber.hpp" -// TODO(anyone): Fix this -int main(int argc, char ** argv) -{ - auto vargv = rclcpp::init_and_remove_ros_arguments(argc, argv); - - if (vargv.size() < 2) { - printf( - "Usage: %s in_transport out_transport --ros-args --remap in:= --ros-args " - "--remap out:=\n", - argv[0]); - return 0; - } +using namespace std::chrono_literals; - auto node = rclcpp::Node::make_shared("point_cloud_republisher"); +namespace point_cloud_transport +{ +Republisher::Republisher(const rclcpp::NodeOptions & options) +: Node("point_cloud_republisher", options) +{ + // Initialize Republishercomponent after construction + // shared_from_this can't be used in the constructor + this->timer_ = create_wall_timer( + 1ms, [this]() { + if (initialized_) { + timer_->cancel(); + } else { + this->initialize(); + initialized_ = true; + } + }); +} +void Republisher::initialize() +{ std::string in_topic = rclcpp::expand_topic_or_service_name( "in", - node->get_name(), node->get_namespace()); + this->get_name(), this->get_namespace()); + std::string out_topic = rclcpp::expand_topic_or_service_name( "out", - node->get_name(), node->get_namespace()); + this->get_name(), this->get_namespace()); + + std::string in_transport = "raw"; + this->declare_parameter("in_transport", in_transport); + if (!this->get_parameter( + "in_transport", in_transport)) + { + RCLCPP_WARN_STREAM( + this->get_logger(), + "The 'in_transport' parameter was not defined." << in_transport); + } else { + RCLCPP_INFO_STREAM( + this->get_logger(), + "The 'in_transport' parameter is set to: " << in_transport); + } - std::string in_transport = vargv[1]; + std::string out_transport = ""; + this->declare_parameter("out_transport", out_transport); + if (!this->get_parameter( + "out_transport", out_transport)) + { + RCLCPP_WARN_STREAM( + this->get_logger(), + "The parameter 'out_transport' was not defined." << out_transport); + } else { + RCLCPP_INFO_STREAM( + this->get_logger(), + "The 'out_transport' parameter is set to: " << out_transport); + } - point_cloud_transport::PointCloudTransport pct(node); + pct = std::make_shared(this->shared_from_this()); - if (vargv.size() < 3) { + if (out_transport.empty()) { // Use all available transports for output - auto pub = + this->simple_pub = std::make_shared( - pct.advertise( + pct->advertise( out_topic, rmw_qos_profile_default)); + RCLCPP_INFO_STREAM( + this->get_logger(), + "out topic1: " << this->simple_pub->getTopic()); + // Use Publisher::publish as the subscriber callback typedef void (point_cloud_transport::Publisher::* PublishMemFn)( const sensor_msgs::msg:: @@ -87,25 +126,24 @@ int main(int argc, char ** argv) PublishMemFn pub_mem_fn = &point_cloud_transport::Publisher::publish; const point_cloud_transport::TransportHints hint(in_transport); - auto sub = pct.subscribe( + this->sub = pct->subscribe( in_topic, static_cast(1), - pub_mem_fn, pub, &hint); - // spin the node - rclcpp::spin(node); + pub_mem_fn, this->simple_pub, &hint); } else { - // Use one specific transport for output - std::string out_transport = vargv[2]; - // Load transport plugin typedef point_cloud_transport::PublisherPlugin Plugin; - auto loader = pct.getPublisherLoader(); + auto loader = pct->getPublisherLoader(); std::string lookup_name = Plugin::getLookupName(out_transport); - RCLCPP_INFO(node->get_logger(), "Loading %s publisher", lookup_name.c_str()); + RCLCPP_INFO(this->get_logger(), "Loading %s publisher", lookup_name.c_str()); auto instance = loader->createUniqueInstance(lookup_name); // DO NOT use instance after this line - std::shared_ptr pub = std::move(instance); - pub->advertise(node.get(), out_topic); + this->pub = std::move(instance); + pub->advertise(this->shared_from_this(), out_topic); + + RCLCPP_INFO_STREAM( + this->get_logger(), + "out topic2: " << this->pub->getTopic()); // Use PublisherPlugin::publish as the subscriber callback typedef void (point_cloud_transport::PublisherPlugin::* PublishMemFn)( @@ -113,17 +151,22 @@ int main(int argc, char ** argv) PointCloud2::ConstSharedPtr &) const; PublishMemFn pub_mem_fn = &point_cloud_transport::PublisherPlugin::publish; - RCLCPP_INFO(node->get_logger(), "Loading %s subscriber"); + RCLCPP_INFO(this->get_logger(), "Loading %s subscriber", in_topic.c_str()); const point_cloud_transport::TransportHints hint(in_transport); - auto sub = pct.subscribe( + this->sub = pct->subscribe( in_topic, static_cast(1), pub_mem_fn, pub, &hint); - - RCLCPP_INFO(node->get_logger(), "Spinning node"); - // spin the node - rclcpp::spin(node); } - - return 0; + RCLCPP_INFO_STREAM( + this->get_logger(), + "in topic: " << this->sub.getTopic()); } +} // namespace point_cloud_transport + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(point_cloud_transport::Republisher) diff --git a/src/republish_program.cpp b/src/republish_program.cpp new file mode 100644 index 0000000..d10e961 --- /dev/null +++ b/src/republish_program.cpp @@ -0,0 +1,74 @@ +/* + * Copyright (c) 2023, Czech Technical University in Prague + * Copyright (c) 2019, paplhjak + * 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 copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "point_cloud_transport/republish.hpp" + +#include + +#include "utilities/utilities.hpp" + +int main(int argc, char ** argv) +{ + std::vector args = rclcpp::init_and_remove_ros_arguments(argc, argv); + + // remove program name + args.erase(args.begin()); + + std::string in_transport{"raw"}; + std::string out_transport{""}; + + if (point_cloud_transport::has_option(args, "--in_transport")) { + in_transport = point_cloud_transport::get_option(args, "--in_transport"); + } + if (point_cloud_transport::has_option(args, "--out_transport")) { + out_transport = point_cloud_transport::get_option(args, "--out_transport"); + } + + rclcpp::NodeOptions options; + // override default parameters with the desired transform + options.parameter_overrides( + { + {"in_transport", in_transport}, + {"out_transport", out_transport}, + }); + + std::shared_ptr node; + + node = std::make_shared(options); + + rclcpp::spin(node); + + rclcpp::shutdown(); + + return 0; +} diff --git a/src/subscriber.cpp b/src/subscriber.cpp index 249141f..f90fbd5 100644 --- a/src/subscriber.cpp +++ b/src/subscriber.cpp @@ -50,7 +50,7 @@ namespace point_cloud_transport struct Subscriber::Impl { - Impl(rclcpp::Node * node, SubLoaderPtr loader) + Impl(std::shared_ptr node, SubLoaderPtr loader) : logger_(node->get_logger()), loader_(loader), unsubscribed_(false) @@ -85,7 +85,7 @@ struct Subscriber::Impl }; Subscriber::Subscriber( - rclcpp::Node * node, + std::shared_ptr node, const std::string & base_topic, const Callback & callback, SubLoaderPtr loader, diff --git a/src/utilities/utilities.cpp b/src/utilities/utilities.cpp new file mode 100644 index 0000000..895f151 --- /dev/null +++ b/src/utilities/utilities.cpp @@ -0,0 +1,45 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "utilities.hpp" + +#include +#include + +namespace point_cloud_transport +{ +std::string get_option(const std::vector & args, const std::string & option_name) +{ + for (auto it = args.begin(), end = args.end(); it != end; ++it) { + if (*it == option_name) { + if (it + 1 != end) { + return *(it + 1); + } + } + } + + return ""; +} + +bool has_option(const std::vector & args, const std::string & option_name) +{ + for (auto it = args.begin(), end = args.end(); it != end; ++it) { + if (*it == option_name) { + return true; + } + } + + return false; +} +} // namespace point_cloud_transport diff --git a/src/utilities/utilities.hpp b/src/utilities/utilities.hpp new file mode 100644 index 0000000..b2a5318 --- /dev/null +++ b/src/utilities/utilities.hpp @@ -0,0 +1,35 @@ +// Copyright 2023 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UTILITIES__UTILITIES_HPP_ +#define UTILITIES__UTILITIES_HPP_ + +#include +#include + +namespace point_cloud_transport +{ +/// Get the option from a list of arguments +/// param[in] args List of arguments +/// param[in] option name to extract +/// return option value +std::string get_option(const std::vector & args, const std::string & option_name); + +/// Is the option available in the list of arguments +/// param[in] args List of arguments +/// param[in] option name to extract +/// return true if the option exists or false otherwise +bool has_option(const std::vector & args, const std::string & option_name); +} // namespace point_cloud_transport +#endif // UTILITIES__UTILITIES_HPP_ From 6fa159e8c4b2a613e7e4e5d1886e12256a5a23b5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 10 Jul 2023 13:27:31 +0200 Subject: [PATCH 16/28] Install tl/expected in Cmake - tag a version (#5) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- CMakeLists.txt | 14 +- include/point_cloud_transport/expected.hpp | 2 +- include/point_cloud_transport/tl/expected.hpp | 2737 ----------------- package.xml | 1 + 4 files changed, 14 insertions(+), 2740 deletions(-) delete mode 100644 include/point_cloud_transport/tl/expected.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 5aac17b..315bbac 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,15 @@ include_directories( include ) +include(FetchContent) +fetchcontent_declare( + expected + GIT_REPOSITORY https://github.com/TartanLlama/expected + GIT_TAG v1.1.0 +) + +fetchcontent_makeavailable(expected) + # Build libpoint_cloud_transport add_library(${PROJECT_NAME} # src/point_cloud_codec.cpp # TODO: This is needed for exposing the encode/decode functions without spinning an node @@ -31,7 +40,8 @@ add_library(${PROJECT_NAME} add_library(${PROJECT_NAME}::${PROJECT_NAME} ALIAS ${PROJECT_NAME}) target_include_directories(${PROJECT_NAME} PUBLIC "$" - "$") + "$" + "$") target_link_libraries(${PROJECT_NAME} PUBLIC message_filters::message_filters rclcpp::rclcpp @@ -109,7 +119,7 @@ ament_export_dependencies(message_filters rclcpp sensor_msgs pluginlib) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - set(AMENT_LINT_AUTO_FILE_EXCLUDE include/point_cloud_transport/tl/expected.hpp doc/conf.py) + set(AMENT_LINT_AUTO_FILE_EXCLUDE doc/conf.py) ament_lint_auto_find_test_dependencies() endif() diff --git a/include/point_cloud_transport/expected.hpp b/include/point_cloud_transport/expected.hpp index eb4fce8..b6ccb28 100644 --- a/include/point_cloud_transport/expected.hpp +++ b/include/point_cloud_transport/expected.hpp @@ -44,7 +44,7 @@ * \author Martin Pecka */ -#include +#include namespace cras { diff --git a/include/point_cloud_transport/tl/expected.hpp b/include/point_cloud_transport/tl/expected.hpp deleted file mode 100644 index d5062d1..0000000 --- a/include/point_cloud_transport/tl/expected.hpp +++ /dev/null @@ -1,2737 +0,0 @@ -/// -// expected - An implementation of std::expected with extensions -// Written in 2017 by Sy Brand (tartanllama@gmail.com, @TartanLlama) -// -// Documentation available at http://tl.tartanllama.xyz/ -// -// To the extent possible under law, the author(s) have dedicated all -// copyright and related and neighboring rights to this software to the -// public domain worldwide. This software is distributed without any warranty. -// -// You should have received a copy of the CC0 Public Domain Dedication -// along with this software. If not, see -// . -/// - -#ifndef POINT_CLOUD_TRANSPORT__TL__EXPECTED_HPP_ -#define POINT_CLOUD_TRANSPORT__TL__EXPECTED_HPP_ - -#define TL_EXPECTED_VERSION_MAJOR 1 -#define TL_EXPECTED_VERSION_MINOR 0 -#define TL_EXPECTED_VERSION_PATCH 1 - -#include -#include -#include -#include -#include -#include - -#if defined(__EXCEPTIONS) || defined(_CPPUNWIND) -#define TL_EXPECTED_EXCEPTIONS_ENABLED -#endif - -#if (defined(_MSC_VER) && _MSC_VER == 1900) -#define TL_EXPECTED_MSVC2015 -#define TL_EXPECTED_MSVC2015_CONSTEXPR -#else -#define TL_EXPECTED_MSVC2015_CONSTEXPR constexpr -#endif - -#if (defined(__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ <= 9 && \ - !defined(__clang__)) -#define TL_EXPECTED_GCC49 -#endif - -#if (defined(__GNUC__) && __GNUC__ == 5 && __GNUC_MINOR__ <= 4 && \ - !defined(__clang__)) -#define TL_EXPECTED_GCC54 -#endif - -#if (defined(__GNUC__) && __GNUC__ == 5 && __GNUC_MINOR__ <= 5 && \ - !defined(__clang__)) -#define TL_EXPECTED_GCC55 -#endif - -#if !defined(TL_ASSERT) -// can't have assert in constexpr in C++11 and GCC 4.9 has a compiler bug -#if (__cplusplus > 201103L) && !defined(TL_EXPECTED_GCC49) -#include -#define TL_ASSERT(x) assert(x) -#else -#define TL_ASSERT(x) -#endif -#endif - -#if (defined(__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ <= 9 && \ - !defined(__clang__)) -// GCC < 5 doesn't support overloading on const&& for member functions - -#define TL_EXPECTED_NO_CONSTRR -// GCC < 5 doesn't support some standard C++11 type traits -#define TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(T) \ - std::has_trivial_copy_constructor -#define TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(T) \ - std::has_trivial_copy_assign - -// This one will be different for GCC 5.7 if it's ever supported -#define TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(T) \ - std::is_trivially_destructible - -// GCC 5 < v < 8 has a bug in is_trivially_copy_constructible which breaks -// std::vector for non-copyable types -#elif (defined(__GNUC__) && __GNUC__ < 8 && !defined(__clang__)) -#ifndef TL_GCC_LESS_8_TRIVIALLY_COPY_CONSTRUCTIBLE_MUTEX -#define TL_GCC_LESS_8_TRIVIALLY_COPY_CONSTRUCTIBLE_MUTEX -namespace tl -{ -namespace detail -{ -template -struct is_trivially_copy_constructible - : std::is_trivially_copy_constructible {}; -#ifdef _GLIBCXX_VECTOR -template -struct is_trivially_copy_constructible>: std::false_type {}; -#endif -} // namespace detail -} // namespace tl -#endif - -#define TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(T) \ - tl::detail::is_trivially_copy_constructible -#define TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(T) \ - std::is_trivially_copy_assignable -#define TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(T) \ - std::is_trivially_destructible -#else -#define TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(T) \ - std::is_trivially_copy_constructible -#define TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(T) \ - std::is_trivially_copy_assignable -#define TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(T) \ - std::is_trivially_destructible -#endif - -#if __cplusplus > 201103L -#define TL_EXPECTED_CXX14 -#endif - -#ifdef TL_EXPECTED_GCC49 -#define TL_EXPECTED_GCC49_CONSTEXPR -#else -#define TL_EXPECTED_GCC49_CONSTEXPR constexpr -#endif - -#if (__cplusplus == 201103L || defined(TL_EXPECTED_MSVC2015) || \ - defined(TL_EXPECTED_GCC49)) -#define TL_EXPECTED_11_CONSTEXPR -#else -#define TL_EXPECTED_11_CONSTEXPR constexpr -#endif - -namespace tl -{ -template -class expected; - -#ifndef TL_MONOSTATE_INPLACE_MUTEX -#define TL_MONOSTATE_INPLACE_MUTEX -class monostate {}; - -struct in_place_t -{ - in_place_t() = default; -}; -static constexpr in_place_t in_place{}; -#endif - -template -class unexpected -{ -public: - static_assert(!std::is_same::value, "E must not be void"); - - unexpected() = delete; - constexpr explicit unexpected(const E & e) - : m_val(e) {} - - constexpr explicit unexpected(E && e) - : m_val(std::move(e)) {} - - template::value>::type * = nullptr> - constexpr explicit unexpected(Args &&... args) - : m_val(std::forward(args)...) {} - template< - class U, class ... Args, - typename std::enable_if &, Args &&...>::value>::type * = nullptr> - constexpr explicit unexpected(std::initializer_list l, Args &&... args) - : m_val(l, std::forward(args)...) {} - - constexpr const E & value() const & {return m_val;} - TL_EXPECTED_11_CONSTEXPR E & value() & {return m_val;} - TL_EXPECTED_11_CONSTEXPR E && value() && {return std::move(m_val);} - constexpr const E && value() const && {return std::move(m_val);} - -private: - E m_val; -}; - -#ifdef __cpp_deduction_guides -template -unexpected(E)->unexpected; -#endif - -template -constexpr bool operator==(const unexpected & lhs, const unexpected & rhs) -{ - return lhs.value() == rhs.value(); -} -template -constexpr bool operator!=(const unexpected & lhs, const unexpected & rhs) -{ - return lhs.value() != rhs.value(); -} -template -constexpr bool operator<(const unexpected & lhs, const unexpected & rhs) -{ - return lhs.value() < rhs.value(); -} -template -constexpr bool operator<=(const unexpected & lhs, const unexpected & rhs) -{ - return lhs.value() <= rhs.value(); -} -template -constexpr bool operator>(const unexpected & lhs, const unexpected & rhs) -{ - return lhs.value() > rhs.value(); -} -template -constexpr bool operator>=(const unexpected & lhs, const unexpected & rhs) -{ - return lhs.value() >= rhs.value(); -} - -template -unexpected::type> make_unexpected(E && e) -{ - return unexpected::type>(std::forward(e)); -} - -struct unexpect_t -{ - unexpect_t() = default; -}; -static constexpr unexpect_t unexpect{}; - -namespace detail -{ -template -[[noreturn]] TL_EXPECTED_11_CONSTEXPR void throw_exception(E && e) -{ -#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED - throw std::forward(e); -#else - (void)e; -#ifdef _MSC_VER - __assume(0); -#else - __builtin_unreachable(); -#endif -#endif -} - -#ifndef TL_TRAITS_MUTEX -#define TL_TRAITS_MUTEX -// C++14-style aliases for brevity -template using remove_const_t = typename std::remove_const::type; -template -using remove_reference_t = typename std::remove_reference::type; -template using decay_t = typename std::decay::type; -template -using enable_if_t = typename std::enable_if::type; -template -using conditional_t = typename std::conditional::type; - -// std::conjunction from C++17 -template -struct conjunction : std::true_type {}; -template -struct conjunction: B {}; -template -struct conjunction - : std::conditional, B>::type {}; - -#if defined(_LIBCPP_VERSION) && __cplusplus == 201103L -#define TL_TRAITS_LIBCXX_MEM_FN_WORKAROUND -#endif - -// In C++11 mode, there's an issue in libc++'s std::mem_fn -// which results in a hard-error when using it in a noexcept expression -// in some cases. This is a check to workaround the common failing case. -#ifdef TL_TRAITS_LIBCXX_MEM_FN_WORKAROUND -template -struct is_pointer_to_non_const_member_func : std::false_type {}; -template -struct is_pointer_to_non_const_member_func - : std::true_type {}; -template -struct is_pointer_to_non_const_member_func - : std::true_type {}; -template -struct is_pointer_to_non_const_member_func - : std::true_type {}; -template -struct is_pointer_to_non_const_member_func - : std::true_type {}; -template -struct is_pointer_to_non_const_member_func - : std::true_type {}; -template -struct is_pointer_to_non_const_member_func - : std::true_type {}; - -template -struct is_const_or_const_ref : std::false_type {}; -template -struct is_const_or_const_ref: std::true_type {}; -template -struct is_const_or_const_ref: std::true_type {}; -#endif - -// std::invoke from C++17 -// https://stackoverflow.com/questions/38288042/c11-14-invoke-workaround -template< - typename Fn, typename ... Args, -#ifdef TL_TRAITS_LIBCXX_MEM_FN_WORKAROUND - typename = enable_if_t::value && - is_const_or_const_ref::value)>, -#endif - typename = enable_if_t>::value>, int = 0> -constexpr auto invoke(Fn && f, Args &&... args) noexcept( - noexcept(std::mem_fn(f)(std::forward(args)...))) --> decltype(std::mem_fn(f)(std::forward(args)...)) -{ - return std::mem_fn(f)(std::forward(args)...); -} - -template>::value>> -constexpr auto invoke(Fn && f, Args &&... args) noexcept( - noexcept(std::forward(f)(std::forward(args)...))) --> decltype(std::forward(f)(std::forward(args)...)) -{ - return std::forward(f)(std::forward(args)...); -} - -// std::invoke_result from C++17 -template -struct invoke_result_impl; - -template -struct invoke_result_impl< - F, - decltype(detail::invoke(std::declval(), std::declval()...), void()), - Us...> -{ - using type = - decltype(detail::invoke(std::declval(), std::declval()...)); -}; - -template -using invoke_result = invoke_result_impl; - -template -using invoke_result_t = typename invoke_result::type; - -#if defined(_MSC_VER) && _MSC_VER <= 1900 -// TODO(anyone) make a version which works with MSVC 2015 -template -struct is_swappable : std::true_type {}; - -template -struct is_nothrow_swappable : std::true_type {}; -#else -// https://stackoverflow.com/questions/26744589/what-is-a-proper-way-to-implement-is-swappable-to-test-for-the-swappable-concept -namespace swap_adl_tests -{ -// if swap ADL finds this then it would call std::swap otherwise (same -// signature) -struct tag {}; - -template tag swap(T &, T &); -template tag swap(T(&a)[N], T(&b)[N]); - -// helper functions to test if an unqualified swap is possible, and if it -// becomes std::swap -template std::false_type can_swap(...) noexcept(false); -template(), std::declval()))> -std::true_type can_swap(int) noexcept(noexcept(swap(std::declval(), std::declval()))); // NOLINT - -template std::false_type uses_std(...); -template -std::is_same(), std::declval())), tag> -uses_std(int); - -template -struct is_std_swap_noexcept - : std::integral_constant::value && - std::is_nothrow_move_assignable::value> {}; - -template -struct is_std_swap_noexcept: is_std_swap_noexcept {}; - -template -struct is_adl_swap_noexcept - : std::integral_constant(0))> {}; -} // namespace swap_adl_tests - -template -struct is_swappable - : std::integral_constant< - bool, - decltype(detail::swap_adl_tests::can_swap(0))::value && - (!decltype(detail::swap_adl_tests::uses_std(0))::value || - (std::is_move_assignable::value && - std::is_move_constructible::value))> {}; - -template -struct is_swappable - : std::integral_constant< - bool, - decltype(detail::swap_adl_tests::can_swap(0))::value && - (!decltype(detail::swap_adl_tests::uses_std( - 0))::value || - is_swappable::value)> {}; - -template -struct is_nothrow_swappable - : std::integral_constant< - bool, - is_swappable::value && - ((decltype(detail::swap_adl_tests::uses_std(0))::value && - detail::swap_adl_tests::is_std_swap_noexcept::value) || - (!decltype(detail::swap_adl_tests::uses_std(0))::value && - detail::swap_adl_tests::is_adl_swap_noexcept::value))> {}; -#endif -#endif - -// Trait for checking if a type is a tl::expected -template -struct is_expected_impl : std::false_type {}; -template -struct is_expected_impl>: std::true_type {}; -template using is_expected = is_expected_impl>; - -template -using expected_enable_forward_value = detail::enable_if_t< - std::is_constructible::value && - !std::is_same, in_place_t>::value && - !std::is_same, detail::decay_t>::value && - !std::is_same, detail::decay_t>::value>; - -template -using expected_enable_from_other = detail::enable_if_t< - std::is_constructible::value && - std::is_constructible::value && - !std::is_constructible &>::value && - !std::is_constructible&&>::value && - !std::is_constructible &>::value && - !std::is_constructible&&>::value && - !std::is_convertible &, T>::value && - !std::is_convertible&&, T>::value && - !std::is_convertible &, T>::value && - !std::is_convertible&&, T>::value>; - -template -using is_void_or = conditional_t::value, std::true_type, U>; - -template -using is_copy_constructible_or_void = - is_void_or>; - -template -using is_move_constructible_or_void = - is_void_or>; - -template -using is_copy_assignable_or_void = is_void_or>; - -template -using is_move_assignable_or_void = is_void_or>; - -} // namespace detail - -namespace detail -{ -struct no_init_t {}; -static constexpr no_init_t no_init{}; - -// Implements the storage of the values, and ensures that the destructor is -// trivial if it can be. -// -// This specialization is for where neither `T` or `E` is trivially -// destructible, so the destructors must be called on destruction of the -// `expected` -template::value, - bool = std::is_trivially_destructible::value> -struct expected_storage_base -{ - constexpr expected_storage_base() - : m_val(T{}), m_has_val(true) {} - constexpr explicit expected_storage_base(no_init_t) - : m_no_init(), m_has_val(false) {} - - template::value> * = - nullptr> - constexpr explicit expected_storage_base(in_place_t, Args &&... args) - : m_val(std::forward(args)...), m_has_val(true) {} - - template &, Args &&...>::value> * = nullptr> - constexpr expected_storage_base( - in_place_t, std::initializer_list il, - Args &&... args) - : m_val(il, std::forward(args)...), m_has_val(true) {} - template::value> * = - nullptr> - constexpr explicit expected_storage_base(unexpect_t, Args &&... args) - : m_unexpect(std::forward(args)...), m_has_val(false) {} - - template &, Args &&...>::value> * = nullptr> - constexpr explicit expected_storage_base( - unexpect_t, - std::initializer_list il, - Args &&... args) - : m_unexpect(il, std::forward(args)...), m_has_val(false) {} - - ~expected_storage_base() - { - if (m_has_val) { - m_val.~T(); - } else { - m_unexpect.~unexpected(); - } - } - union { - T m_val; - unexpected m_unexpect; - char m_no_init; - }; - bool m_has_val; -}; - -// This specialization is for when both `T` and `E` are trivially-destructible, -// so the destructor of the `expected` can be trivial. -template -struct expected_storage_base -{ - constexpr expected_storage_base() - : m_val(T{}), m_has_val(true) {} - constexpr explicit expected_storage_base(no_init_t) - : m_no_init(), m_has_val(false) {} - - template::value> * = - nullptr> - constexpr explicit expected_storage_base(in_place_t, Args &&... args) - : m_val(std::forward(args)...), m_has_val(true) {} - - template &, Args &&...>::value> * = nullptr> - constexpr expected_storage_base( - in_place_t, std::initializer_list il, - Args &&... args) - : m_val(il, std::forward(args)...), m_has_val(true) {} - template::value> * = - nullptr> - constexpr explicit expected_storage_base(unexpect_t, Args &&... args) - : m_unexpect(std::forward(args)...), m_has_val(false) {} - - template &, Args &&...>::value> * = nullptr> - constexpr explicit expected_storage_base( - unexpect_t, - std::initializer_list il, - Args &&... args) - : m_unexpect(il, std::forward(args)...), m_has_val(false) {} - - ~expected_storage_base() = default; - union { - T m_val; - unexpected m_unexpect; - char m_no_init; - }; - bool m_has_val; -}; - -// T is trivial, E is not. -template -struct expected_storage_base -{ - constexpr expected_storage_base() - : m_val(T{}), m_has_val(true) {} - TL_EXPECTED_MSVC2015_CONSTEXPR expected_storage_base(no_init_t) - : m_no_init(), m_has_val(false) {} - - template::value> * = - nullptr> - constexpr explicit expected_storage_base(in_place_t, Args &&... args) - : m_val(std::forward(args)...), m_has_val(true) {} - - template &, Args &&...>::value> * = nullptr> - constexpr expected_storage_base( - in_place_t, std::initializer_list il, - Args &&... args) - : m_val(il, std::forward(args)...), m_has_val(true) {} - template::value> * = - nullptr> - constexpr explicit expected_storage_base(unexpect_t, Args &&... args) - : m_unexpect(std::forward(args)...), m_has_val(false) {} - - template &, Args &&...>::value> * = nullptr> - constexpr explicit expected_storage_base( - unexpect_t, - std::initializer_list il, - Args &&... args) - : m_unexpect(il, std::forward(args)...), m_has_val(false) {} - - ~expected_storage_base() - { - if (!m_has_val) { - m_unexpect.~unexpected(); - } - } - - union { - T m_val; - unexpected m_unexpect; - char m_no_init; - }; - bool m_has_val; -}; - -// E is trivial, T is not. -template -struct expected_storage_base -{ - constexpr expected_storage_base() - : m_val(T{}), m_has_val(true) {} - constexpr explicit expected_storage_base(no_init_t) - : m_no_init(), m_has_val(false) {} - - template::value> * = - nullptr> - constexpr explicit expected_storage_base(in_place_t, Args &&... args) - : m_val(std::forward(args)...), m_has_val(true) {} - - template &, Args &&...>::value> * = nullptr> - constexpr expected_storage_base( - in_place_t, std::initializer_list il, - Args &&... args) - : m_val(il, std::forward(args)...), m_has_val(true) {} - template::value> * = - nullptr> - constexpr explicit expected_storage_base(unexpect_t, Args &&... args) - : m_unexpect(std::forward(args)...), m_has_val(false) {} - - template &, Args &&...>::value> * = nullptr> - constexpr explicit expected_storage_base( - unexpect_t, - std::initializer_list il, - Args &&... args) - : m_unexpect(il, std::forward(args)...), m_has_val(false) {} - - ~expected_storage_base() - { - if (m_has_val) { - m_val.~T(); - } - } - union { - T m_val; - unexpected m_unexpect; - char m_no_init; - }; - bool m_has_val; -}; - -// `T` is `void`, `E` is trivially-destructible -template -struct expected_storage_base -{ - #if __GNUC__ <= 5 - // no constexpr for GCC 4/5 bug - #else - TL_EXPECTED_MSVC2015_CONSTEXPR - #endif - expected_storage_base() - : m_has_val(true) {} - - constexpr explicit expected_storage_base(no_init_t) - : m_val(), m_has_val(false) {} - - constexpr explicit expected_storage_base(in_place_t) - : m_has_val(true) {} - - template::value> * = - nullptr> - constexpr explicit expected_storage_base(unexpect_t, Args &&... args) - : m_unexpect(std::forward(args)...), m_has_val(false) {} - - template &, Args &&...>::value> * = nullptr> - constexpr explicit expected_storage_base( - unexpect_t, - std::initializer_list il, - Args &&... args) - : m_unexpect(il, std::forward(args)...), m_has_val(false) {} - - ~expected_storage_base() = default; - struct dummy {}; - union { - unexpected m_unexpect; - dummy m_val; - }; - bool m_has_val; -}; - -// `T` is `void`, `E` is not trivially-destructible -template -struct expected_storage_base -{ - constexpr expected_storage_base() - : m_dummy(), m_has_val(true) {} - constexpr explicit expected_storage_base(no_init_t) - : m_dummy(), m_has_val(false) {} - - constexpr explicit expected_storage_base(in_place_t) - : m_dummy(), m_has_val(true) {} - - template::value> * = - nullptr> - constexpr explicit expected_storage_base(unexpect_t, Args &&... args) - : m_unexpect(std::forward(args)...), m_has_val(false) {} - - template &, Args &&...>::value> * = nullptr> - constexpr explicit expected_storage_base( - unexpect_t, - std::initializer_list il, - Args &&... args) - : m_unexpect(il, std::forward(args)...), m_has_val(false) {} - - ~expected_storage_base() - { - if (!m_has_val) { - m_unexpect.~unexpected(); - } - } - - union { - unexpected m_unexpect; - char m_dummy; - }; - bool m_has_val; -}; - -// This base class provides some handy member functions which can be used in -// further derived classes -template -struct expected_operations_base : expected_storage_base -{ - using expected_storage_base::expected_storage_base; - - template void construct(Args &&... args) noexcept - { - new (std::addressof(this->m_val)) T(std::forward(args)...); - this->m_has_val = true; - } - - template void construct_with(Rhs && rhs) noexcept - { - new (std::addressof(this->m_val)) T(std::forward(rhs).get()); - this->m_has_val = true; - } - - template void construct_error(Args &&... args) noexcept - { - new (std::addressof(this->m_unexpect)) - unexpected(std::forward(args)...); - this->m_has_val = false; - } - -#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED - - // These assign overloads ensure that the most efficient assignment - // implementation is used while maintaining the strong exception guarantee. - // The problematic case is where rhs has a value, but *this does not. - // - // This overload handles the case where we can just copy-construct `T` - // directly into place without throwing. - template::value> - * = nullptr> - void assign(const expected_operations_base & rhs) noexcept - { - if (!this->m_has_val && rhs.m_has_val) { - geterr().~unexpected(); - construct(rhs.get()); - } else { - assign_common(rhs); - } - } - - // This overload handles the case where we can attempt to create a copy of - // `T`, then no-throw move it into place if the copy was successful. - template::value && - std::is_nothrow_move_constructible::value> - * = nullptr> - void assign(const expected_operations_base & rhs) noexcept - { - if (!this->m_has_val && rhs.m_has_val) { - T tmp = rhs.get(); - geterr().~unexpected(); - construct(std::move(tmp)); - } else { - assign_common(rhs); - } - } - - // This overload is the worst-case, where we have to move-construct the - // unexpected value into temporary storage, then try to copy the T into place. - // If the construction succeeds, then everything is fine, but if it throws, - // then we move the old unexpected value back into place before rethrowing the - // exception. - template::value && - !std::is_nothrow_move_constructible::value> - * = nullptr> - void assign(const expected_operations_base & rhs) - { - if (!this->m_has_val && rhs.m_has_val) { - auto tmp = std::move(geterr()); - geterr().~unexpected(); - -#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED - try { - construct(rhs.get()); - } catch (...) { - geterr() = std::move(tmp); - throw; - } -#else - construct(rhs.get()); -#endif - } else { - assign_common(rhs); - } - } - - // These overloads do the same as above, but for rvalues - template::value> - * = nullptr> - void assign(expected_operations_base && rhs) noexcept - { - if (!this->m_has_val && rhs.m_has_val) { - geterr().~unexpected(); - construct(std::move(rhs).get()); - } else { - assign_common(std::move(rhs)); - } - } - - template::value> - * = nullptr> - void assign(expected_operations_base && rhs) - { - if (!this->m_has_val && rhs.m_has_val) { - auto tmp = std::move(geterr()); - geterr().~unexpected(); -#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED - try { - construct(std::move(rhs).get()); - } catch (...) { - geterr() = std::move(tmp); - throw; - } -#else - construct(std::move(rhs).get()); -#endif - } else { - assign_common(std::move(rhs)); - } - } - -#else - - // If exceptions are disabled then we can just copy-construct - void assign(const expected_operations_base & rhs) noexcept - { - if (!this->m_has_val && rhs.m_has_val) { - geterr().~unexpected(); - construct(rhs.get()); - } else { - assign_common(rhs); - } - } - - void assign(expected_operations_base && rhs) noexcept - { - if (!this->m_has_val && rhs.m_has_val) { - geterr().~unexpected(); - construct(std::move(rhs).get()); - } else { - assign_common(std::move(rhs)); - } - } - -#endif - - // The common part of move/copy assigning - template void assign_common(Rhs && rhs) - { - if (this->m_has_val) { - if (rhs.m_has_val) { - get() = std::forward(rhs).get(); - } else { - destroy_val(); - construct_error(std::forward(rhs).geterr()); - } - } else { - if (!rhs.m_has_val) { - geterr() = std::forward(rhs).geterr(); - } - } - } - - bool has_value() const {return this->m_has_val;} - - TL_EXPECTED_11_CONSTEXPR T & get() & {return this->m_val;} - constexpr const T & get() const & {return this->m_val;} - TL_EXPECTED_11_CONSTEXPR T && get() && {return std::move(this->m_val);} -#ifndef TL_EXPECTED_NO_CONSTRR - constexpr const T && get() const && {return std::move(this->m_val);} -#endif - - TL_EXPECTED_11_CONSTEXPR unexpected & geterr() & - { - return this->m_unexpect; - } - constexpr const unexpected & geterr() const & {return this->m_unexpect;} - TL_EXPECTED_11_CONSTEXPR unexpected && geterr() && - { - return std::move(this->m_unexpect); - } -#ifndef TL_EXPECTED_NO_CONSTRR - constexpr const unexpected && geterr() const && - { - return std::move(this->m_unexpect); - } -#endif - - TL_EXPECTED_11_CONSTEXPR void destroy_val() {get().~T();} -}; - -// This base class provides some handy member functions which can be used in -// further derived classes -template -struct expected_operations_base: expected_storage_base -{ - using expected_storage_base::expected_storage_base; - - template void construct() noexcept {this->m_has_val = true;} - - // This function doesn't use its argument, but needs it so that code in - // levels above this can work independently of whether T is void - template void construct_with(Rhs &&) noexcept - { - this->m_has_val = true; - } - - template void construct_error(Args &&... args) noexcept - { - new (std::addressof(this->m_unexpect)) - unexpected(std::forward(args)...); - this->m_has_val = false; - } - - template void assign(Rhs && rhs) noexcept - { - if (!this->m_has_val) { - if (rhs.m_has_val) { - geterr().~unexpected(); - construct(); - } else { - geterr() = std::forward(rhs).geterr(); - } - } else { - if (!rhs.m_has_val) { - construct_error(std::forward(rhs).geterr()); - } - } - } - - bool has_value() const {return this->m_has_val;} - - TL_EXPECTED_11_CONSTEXPR unexpected & geterr() & - { - return this->m_unexpect; - } - constexpr const unexpected & geterr() const & {return this->m_unexpect;} - TL_EXPECTED_11_CONSTEXPR unexpected && geterr() && - { - return std::move(this->m_unexpect); - } -#ifndef TL_EXPECTED_NO_CONSTRR - constexpr const unexpected && geterr() const && - { - return std::move(this->m_unexpect); - } -#endif - - TL_EXPECTED_11_CONSTEXPR void destroy_val() - { - // no-op - } -}; - -// This class manages conditionally having a trivial copy constructor -// This specialization is for when T and E are trivially copy constructible -template:: - value && TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(E) ::value> -struct expected_copy_base : expected_operations_base -{ - using expected_operations_base::expected_operations_base; -}; - -// This specialization is for when T or E are not trivially copy constructible -template -struct expected_copy_base: expected_operations_base -{ - using expected_operations_base::expected_operations_base; - - expected_copy_base() = default; - expected_copy_base(const expected_copy_base & rhs) - : expected_operations_base(no_init) - { - if (rhs.has_value()) { - this->construct_with(rhs); - } else { - this->construct_error(rhs.geterr()); - } - } - - expected_copy_base(expected_copy_base && rhs) = default; - expected_copy_base & operator=(const expected_copy_base & rhs) = default; - expected_copy_base & operator=(expected_copy_base && rhs) = default; -}; - -// This class manages conditionally having a trivial move constructor -// Unfortunately there's no way to achieve this in GCC < 5 AFAIK, since it -// doesn't implement an analogue to std::is_trivially_move_constructible. We -// have to make do with a non-trivial move constructor even if T is trivially -// move constructible -#ifndef TL_EXPECTED_GCC49 -template>::value && - std::is_trivially_move_constructible::value> -struct expected_move_base : expected_copy_base -{ - using expected_copy_base::expected_copy_base; -}; -#else -template -struct expected_move_base; -#endif -template -struct expected_move_base: expected_copy_base -{ - using expected_copy_base::expected_copy_base; - - expected_move_base() = default; - expected_move_base(const expected_move_base & rhs) = default; - - expected_move_base(expected_move_base && rhs) noexcept( - std::is_nothrow_move_constructible::value) - : expected_copy_base(no_init) - { - if (rhs.has_value()) { - this->construct_with(std::move(rhs)); - } else { - this->construct_error(std::move(rhs.geterr())); - } - } - expected_move_base & operator=(const expected_move_base & rhs) = default; - expected_move_base & operator=(expected_move_base && rhs) = default; -}; - -// This class manages conditionally having a trivial copy assignment operator -template>::value && - TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(E) ::value && - TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(E) ::value && - TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(E) ::value> -struct expected_copy_assign_base : expected_move_base -{ - using expected_move_base::expected_move_base; -}; - -template -struct expected_copy_assign_base: expected_move_base -{ - using expected_move_base::expected_move_base; - - expected_copy_assign_base() = default; - expected_copy_assign_base(const expected_copy_assign_base & rhs) = default; - - expected_copy_assign_base(expected_copy_assign_base && rhs) = default; - expected_copy_assign_base & operator=(const expected_copy_assign_base & rhs) - { - this->assign(rhs); - return *this; - } - expected_copy_assign_base & - operator=(expected_copy_assign_base && rhs) = default; -}; - -// This class manages conditionally having a trivial move assignment operator -// Unfortunately there's no way to achieve this in GCC < 5 AFAIK, since it -// doesn't implement an analogue to std::is_trivially_move_assignable. We have -// to make do with a non-trivial move assignment operator even if T is trivially -// move assignable -#ifndef TL_EXPECTED_GCC49 -template, - std::is_trivially_move_constructible, - std::is_trivially_move_assignable>>:: - value && std::is_trivially_destructible::value && - std::is_trivially_move_constructible::value && - std::is_trivially_move_assignable::value> -struct expected_move_assign_base : expected_copy_assign_base -{ - using expected_copy_assign_base::expected_copy_assign_base; -}; -#else -template -struct expected_move_assign_base; -#endif - -template -struct expected_move_assign_base - : expected_copy_assign_base -{ - using expected_copy_assign_base::expected_copy_assign_base; - - expected_move_assign_base() = default; - expected_move_assign_base(const expected_move_assign_base & rhs) = default; - - expected_move_assign_base(expected_move_assign_base && rhs) = default; - - expected_move_assign_base & - operator=(const expected_move_assign_base & rhs) = default; - - expected_move_assign_base & - operator=(expected_move_assign_base && rhs) noexcept( - std::is_nothrow_move_constructible::value && - std::is_nothrow_move_assignable::value) - { - this->assign(std::move(rhs)); - return *this; - } -}; - -// expected_delete_ctor_base will conditionally delete copy and move -// constructors depending on whether T is copy/move constructible -template::value && - std::is_copy_constructible::value), - bool EnableMove = (is_move_constructible_or_void::value && - std::is_move_constructible::value)> -struct expected_delete_ctor_base -{ - expected_delete_ctor_base() = default; - expected_delete_ctor_base(const expected_delete_ctor_base &) = default; - expected_delete_ctor_base(expected_delete_ctor_base &&) noexcept = default; - expected_delete_ctor_base & - operator=(const expected_delete_ctor_base &) = default; - expected_delete_ctor_base & - operator=(expected_delete_ctor_base &&) noexcept = default; -}; - -template -struct expected_delete_ctor_base -{ - expected_delete_ctor_base() = default; - expected_delete_ctor_base(const expected_delete_ctor_base &) = default; - expected_delete_ctor_base(expected_delete_ctor_base &&) noexcept = delete; - expected_delete_ctor_base & - operator=(const expected_delete_ctor_base &) = default; - expected_delete_ctor_base & - operator=(expected_delete_ctor_base &&) noexcept = default; -}; - -template -struct expected_delete_ctor_base -{ - expected_delete_ctor_base() = default; - expected_delete_ctor_base(const expected_delete_ctor_base &) = delete; - expected_delete_ctor_base(expected_delete_ctor_base &&) noexcept = default; - expected_delete_ctor_base & - operator=(const expected_delete_ctor_base &) = default; - expected_delete_ctor_base & - operator=(expected_delete_ctor_base &&) noexcept = default; -}; - -template -struct expected_delete_ctor_base -{ - expected_delete_ctor_base() = default; - expected_delete_ctor_base(const expected_delete_ctor_base &) = delete; - expected_delete_ctor_base(expected_delete_ctor_base &&) noexcept = delete; - expected_delete_ctor_base & - operator=(const expected_delete_ctor_base &) = default; - expected_delete_ctor_base & - operator=(expected_delete_ctor_base &&) noexcept = default; -}; - -// expected_delete_assign_base will conditionally delete copy and move -// constructors depending on whether T and E are copy/move constructible + -// assignable -template::value && - std::is_copy_constructible::value && - is_copy_assignable_or_void::value && - std::is_copy_assignable::value), - bool EnableMove = (is_move_constructible_or_void::value && - std::is_move_constructible::value && - is_move_assignable_or_void::value && - std::is_move_assignable::value)> -struct expected_delete_assign_base -{ - expected_delete_assign_base() = default; - expected_delete_assign_base(const expected_delete_assign_base &) = default; - expected_delete_assign_base(expected_delete_assign_base &&) noexcept = - default; - expected_delete_assign_base & - operator=(const expected_delete_assign_base &) = default; - expected_delete_assign_base & - operator=(expected_delete_assign_base &&) noexcept = default; -}; - -template -struct expected_delete_assign_base -{ - expected_delete_assign_base() = default; - expected_delete_assign_base(const expected_delete_assign_base &) = default; - expected_delete_assign_base(expected_delete_assign_base &&) noexcept = - default; - expected_delete_assign_base & - operator=(const expected_delete_assign_base &) = default; - expected_delete_assign_base & - operator=(expected_delete_assign_base &&) noexcept = delete; -}; - -template -struct expected_delete_assign_base -{ - expected_delete_assign_base() = default; - expected_delete_assign_base(const expected_delete_assign_base &) = default; - expected_delete_assign_base(expected_delete_assign_base &&) noexcept = - default; - expected_delete_assign_base & - operator=(const expected_delete_assign_base &) = delete; - expected_delete_assign_base & - operator=(expected_delete_assign_base &&) noexcept = default; -}; - -template -struct expected_delete_assign_base -{ - expected_delete_assign_base() = default; - expected_delete_assign_base(const expected_delete_assign_base &) = default; - expected_delete_assign_base(expected_delete_assign_base &&) noexcept = - default; - expected_delete_assign_base & - operator=(const expected_delete_assign_base &) = delete; - expected_delete_assign_base & - operator=(expected_delete_assign_base &&) noexcept = delete; -}; - -// This is needed to be able to construct the expected_default_ctor_base which -// follows, while still conditionally deleting the default constructor. -struct default_constructor_tag -{ - constexpr default_constructor_tag() = default; -}; - -// expected_default_ctor_base will ensure that expected has a deleted default -// consturctor if T is not default constructible. -// This specialization is for when T is default constructible -template::value || std::is_void::value> -struct expected_default_ctor_base -{ - constexpr expected_default_ctor_base() noexcept = default; - constexpr expected_default_ctor_base( - expected_default_ctor_base const &) noexcept = default; - constexpr expected_default_ctor_base(expected_default_ctor_base &&) noexcept = - default; - expected_default_ctor_base & - operator=(expected_default_ctor_base const &) noexcept = default; - expected_default_ctor_base & - operator=(expected_default_ctor_base &&) noexcept = default; - - constexpr explicit expected_default_ctor_base(default_constructor_tag) {} -}; - -// This specialization is for when T is not default constructible -template -struct expected_default_ctor_base -{ - constexpr expected_default_ctor_base() noexcept = delete; - constexpr expected_default_ctor_base( - expected_default_ctor_base const &) noexcept = default; - constexpr expected_default_ctor_base(expected_default_ctor_base &&) noexcept = - default; - expected_default_ctor_base & - operator=(expected_default_ctor_base const &) noexcept = default; - expected_default_ctor_base & - operator=(expected_default_ctor_base &&) noexcept = default; - - constexpr explicit expected_default_ctor_base(default_constructor_tag) {} -}; -} // namespace detail - -template -class bad_expected_access : public std::exception -{ -public: - explicit bad_expected_access(E e) - : m_val(std::move(e)) {} - - const char * what() const noexcept override - { - return "Bad expected access"; - } - - const E & error() const & {return m_val;} - E & error() & {return m_val;} - const E && error() const && {return std::move(m_val);} - E && error() && {return std::move(m_val);} - -private: - E m_val; -}; - -/// An `expected` object is an object that contains the storage for -/// another object and manages the lifetime of this contained object `T`. -/// Alternatively it could contain the storage for another unexpected object -/// `E`. The contained object may not be initialized after the expected object -/// has been initialized, and may not be destroyed before the expected object -/// has been destroyed. The initialization state of the contained object is -/// tracked by the expected object. -template -class expected : private detail::expected_move_assign_base, - private detail::expected_delete_ctor_base, - private detail::expected_delete_assign_base, - private detail::expected_default_ctor_base -{ - static_assert(!std::is_reference::value, "T must not be a reference"); - static_assert( - !std::is_same::type>::value, - "T must not be in_place_t"); - static_assert( - !std::is_same::type>::value, - "T must not be unexpect_t"); - static_assert( - !std::is_same>::type>::value, - "T must not be unexpected"); - static_assert(!std::is_reference::value, "E must not be a reference"); - - T * valptr() {return std::addressof(this->m_val);} - const T * valptr() const {return std::addressof(this->m_val);} - unexpected * errptr() {return std::addressof(this->m_unexpect);} - const unexpected * errptr() const - { - return std::addressof(this->m_unexpect); - } - - template::value> * = nullptr> - TL_EXPECTED_11_CONSTEXPR U & val() - { - return this->m_val; - } - TL_EXPECTED_11_CONSTEXPR unexpected & err() {return this->m_unexpect;} - - template::value> * = nullptr> - constexpr const U & val() const - { - return this->m_val; - } - constexpr const unexpected & err() const {return this->m_unexpect;} - - using impl_base = detail::expected_move_assign_base; - using ctor_base = detail::expected_default_ctor_base; - -public: - typedef T value_type; - typedef E error_type; - typedef unexpected unexpected_type; - -#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ - !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) - template TL_EXPECTED_11_CONSTEXPR auto and_then(F && f) & - { - return and_then_impl(*this, std::forward(f)); - } - template TL_EXPECTED_11_CONSTEXPR auto and_then(F && f) && - { - return and_then_impl(std::move(*this), std::forward(f)); - } - template constexpr auto and_then(F && f) const & - { - return and_then_impl(*this, std::forward(f)); - } - -#ifndef TL_EXPECTED_NO_CONSTRR - template constexpr auto and_then(F && f) const && - { - return and_then_impl(std::move(*this), std::forward(f)); - } -#endif - -#else - template - TL_EXPECTED_11_CONSTEXPR auto - and_then(F && f) &->decltype(and_then_impl( - std::declval(), - std::forward(f))) - { - return and_then_impl(*this, std::forward(f)); - } - template - TL_EXPECTED_11_CONSTEXPR auto - and_then(F && f) &&->decltype(and_then_impl( - std::declval(), - std::forward(f))) - { - return and_then_impl(std::move(*this), std::forward(f)); - } - template - constexpr auto and_then(F && f) const & ->decltype(and_then_impl( - std::declval(), std::forward(f))) - { - return and_then_impl(*this, std::forward(f)); - } - -#ifndef TL_EXPECTED_NO_CONSTRR - template - constexpr auto and_then(F && f) const && ->decltype(and_then_impl( - std::declval(), std::forward(f))) - { - return and_then_impl(std::move(*this), std::forward(f)); - } -#endif -#endif - -#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ - !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) - template TL_EXPECTED_11_CONSTEXPR auto map(F && f) & - { - return expected_map_impl(*this, std::forward(f)); - } - template TL_EXPECTED_11_CONSTEXPR auto map(F && f) && - { - return expected_map_impl(std::move(*this), std::forward(f)); - } - template constexpr auto map(F && f) const & - { - return expected_map_impl(*this, std::forward(f)); - } - template constexpr auto map(F && f) const && - { - return expected_map_impl(std::move(*this), std::forward(f)); - } -#else - template - TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl( - std::declval(), std::declval())) - map(F && f) & { - return expected_map_impl(*this, std::forward(f)); - } - template - TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl( - std::declval(), - std::declval())) - map(F && f) && { - return expected_map_impl(std::move(*this), std::forward(f)); - } - template - constexpr decltype(expected_map_impl( - std::declval(), - std::declval())) - map(F && f) const & { - return expected_map_impl(*this, std::forward(f)); - } - -#ifndef TL_EXPECTED_NO_CONSTRR - template - constexpr decltype(expected_map_impl( - std::declval(), - std::declval())) - map(F && f) const && { - return expected_map_impl(std::move(*this), std::forward(f)); - } -#endif -#endif - -#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ - !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) - template TL_EXPECTED_11_CONSTEXPR auto transform(F && f) & - { - return expected_map_impl(*this, std::forward(f)); - } - template TL_EXPECTED_11_CONSTEXPR auto transform(F && f) && - { - return expected_map_impl(std::move(*this), std::forward(f)); - } - template constexpr auto transform(F && f) const & - { - return expected_map_impl(*this, std::forward(f)); - } - template constexpr auto transform(F && f) const && - { - return expected_map_impl(std::move(*this), std::forward(f)); - } -#else - template - TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl( - std::declval(), std::declval())) - transform(F && f) & { - return expected_map_impl(*this, std::forward(f)); - } - template - TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl( - std::declval(), - std::declval())) - transform(F && f) && { - return expected_map_impl(std::move(*this), std::forward(f)); - } - template - constexpr decltype(expected_map_impl( - std::declval(), - std::declval())) - transform(F && f) const & { - return expected_map_impl(*this, std::forward(f)); - } - -#ifndef TL_EXPECTED_NO_CONSTRR - template - constexpr decltype(expected_map_impl( - std::declval(), - std::declval())) - transform(F && f) const && { - return expected_map_impl(std::move(*this), std::forward(f)); - } -#endif -#endif - -#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ - !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) - template TL_EXPECTED_11_CONSTEXPR auto map_error(F && f) & - { - return map_error_impl(*this, std::forward(f)); - } - template TL_EXPECTED_11_CONSTEXPR auto map_error(F && f) && - { - return map_error_impl(std::move(*this), std::forward(f)); - } - template constexpr auto map_error(F && f) const & - { - return map_error_impl(*this, std::forward(f)); - } - template constexpr auto map_error(F && f) const && - { - return map_error_impl(std::move(*this), std::forward(f)); - } -#else - template - TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl( - std::declval(), - std::declval())) - map_error(F && f) & { - return map_error_impl(*this, std::forward(f)); - } - template - TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl( - std::declval(), - std::declval())) - map_error(F && f) && { - return map_error_impl(std::move(*this), std::forward(f)); - } - template - constexpr decltype(map_error_impl( - std::declval(), - std::declval())) - map_error(F && f) const & { - return map_error_impl(*this, std::forward(f)); - } - -#ifndef TL_EXPECTED_NO_CONSTRR - template - constexpr decltype(map_error_impl( - std::declval(), - std::declval())) - map_error(F && f) const && { - return map_error_impl(std::move(*this), std::forward(f)); - } -#endif -#endif -#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ - !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) - template TL_EXPECTED_11_CONSTEXPR auto transform_error(F && f) & - { - return map_error_impl(*this, std::forward(f)); - } - template TL_EXPECTED_11_CONSTEXPR auto transform_error(F && f) && - { - return map_error_impl(std::move(*this), std::forward(f)); - } - template constexpr auto transform_error(F && f) const & - { - return map_error_impl(*this, std::forward(f)); - } - template constexpr auto transform_error(F && f) const && - { - return map_error_impl(std::move(*this), std::forward(f)); - } -#else - template - TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl( - std::declval(), - std::declval())) - transform_error(F && f) & { - return map_error_impl(*this, std::forward(f)); - } - template - TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl( - std::declval(), - std::declval())) - transform_error(F && f) && { - return map_error_impl(std::move(*this), std::forward(f)); - } - template - constexpr decltype(map_error_impl( - std::declval(), - std::declval())) - transform_error(F && f) const & { - return map_error_impl(*this, std::forward(f)); - } - -#ifndef TL_EXPECTED_NO_CONSTRR - template - constexpr decltype(map_error_impl( - std::declval(), - std::declval())) - transform_error(F && f) const && { - return map_error_impl(std::move(*this), std::forward(f)); - } -#endif -#endif - template expected TL_EXPECTED_11_CONSTEXPR or_else(F && f) & - { - return or_else_impl(*this, std::forward(f)); - } - - template expected TL_EXPECTED_11_CONSTEXPR or_else(F && f) && - { - return or_else_impl(std::move(*this), std::forward(f)); - } - - template expected constexpr or_else(F && f) const & - { - return or_else_impl(*this, std::forward(f)); - } - -#ifndef TL_EXPECTED_NO_CONSTRR - template expected constexpr or_else(F && f) const && - { - return or_else_impl(std::move(*this), std::forward(f)); - } -#endif - constexpr expected() = default; - constexpr expected(const expected & rhs) = default; - constexpr expected(expected && rhs) = default; - expected & operator=(const expected & rhs) = default; - expected & operator=(expected && rhs) = default; - - template::value> * = - nullptr> - constexpr explicit expected(in_place_t, Args &&... args) - : impl_base(in_place, std::forward(args)...), - ctor_base(detail::default_constructor_tag{}) {} - - template &, Args &&...>::value> * = nullptr> - constexpr expected(in_place_t, std::initializer_list il, Args &&... args) - : impl_base(in_place, il, std::forward(args)...), - ctor_base(detail::default_constructor_tag{}) {} - - template::value> * = - nullptr, - detail::enable_if_t::value> * = - nullptr> - explicit constexpr expected(const unexpected & e) - : impl_base(unexpect, e.value()), - ctor_base(detail::default_constructor_tag{}) {} - - template< - class G = E, - detail::enable_if_t::value> * = - nullptr, - detail::enable_if_t::value> * = nullptr> - constexpr explicit expected(unexpected const & e) - : impl_base(unexpect, e.value()), - ctor_base(detail::default_constructor_tag{}) {} - - template< - class G = E, - detail::enable_if_t::value> * = nullptr, - detail::enable_if_t::value> * = nullptr> - explicit constexpr expected(unexpected && e) noexcept( - std::is_nothrow_constructible::value) - : impl_base(unexpect, std::move(e.value())), - ctor_base(detail::default_constructor_tag{}) {} - - template< - class G = E, - detail::enable_if_t::value> * = nullptr, - detail::enable_if_t::value> * = nullptr> - constexpr explicit expected(unexpected && e) noexcept( - std::is_nothrow_constructible::value) - : impl_base(unexpect, std::move(e.value())), - ctor_base(detail::default_constructor_tag{}) {} - - template::value> * = - nullptr> - constexpr explicit expected(unexpect_t, Args &&... args) - : impl_base(unexpect, std::forward(args)...), - ctor_base(detail::default_constructor_tag{}) {} - - template &, Args &&...>::value> * = nullptr> - constexpr explicit expected( - unexpect_t, std::initializer_list il, - Args &&... args) - : impl_base(unexpect, il, std::forward(args)...), - ctor_base(detail::default_constructor_tag{}) {} - - template::value && - std::is_convertible::value)> * = - nullptr, - detail::expected_enable_from_other - * = nullptr> - explicit TL_EXPECTED_11_CONSTEXPR expected(const expected & rhs) - : ctor_base(detail::default_constructor_tag{}) - { - if (rhs.has_value()) { - this->construct(*rhs); - } else { - this->construct_error(rhs.error()); - } - } - - template::value && - std::is_convertible::value)> * = - nullptr, - detail::expected_enable_from_other - * = nullptr> - TL_EXPECTED_11_CONSTEXPR expected(const expected & rhs) - : ctor_base(detail::default_constructor_tag{}) - { - if (rhs.has_value()) { - this->construct(*rhs); - } else { - this->construct_error(rhs.error()); - } - } - - template< - class U, class G, - detail::enable_if_t::value && - std::is_convertible::value)> * = nullptr, - detail::expected_enable_from_other * = nullptr> - explicit TL_EXPECTED_11_CONSTEXPR expected(expected && rhs) - : ctor_base(detail::default_constructor_tag{}) - { - if (rhs.has_value()) { - this->construct(std::move(*rhs)); - } else { - this->construct_error(std::move(rhs.error())); - } - } - - template< - class U, class G, - detail::enable_if_t<(std::is_convertible::value && - std::is_convertible::value)> * = nullptr, - detail::expected_enable_from_other * = nullptr> - TL_EXPECTED_11_CONSTEXPR expected(expected && rhs) - : ctor_base(detail::default_constructor_tag{}) - { - if (rhs.has_value()) { - this->construct(std::move(*rhs)); - } else { - this->construct_error(std::move(rhs.error())); - } - } - - template< - class U = T, - detail::enable_if_t::value> * = nullptr, - detail::expected_enable_forward_value * = nullptr> - explicit TL_EXPECTED_MSVC2015_CONSTEXPR expected(U && v) - : expected(in_place, std::forward(v)) {} - - template< - class U = T, - detail::enable_if_t::value> * = nullptr, - detail::expected_enable_forward_value * = nullptr> - TL_EXPECTED_MSVC2015_CONSTEXPR expected(U && v) - : expected(in_place, std::forward(v)) {} - - template< - class U = T, class G = T, - detail::enable_if_t::value> * = - nullptr, - detail::enable_if_t::value> * = nullptr, - detail::enable_if_t< - (!std::is_same, detail::decay_t>::value && - !detail::conjunction, - std::is_same>>::value && - std::is_constructible::value && - std::is_assignable::value && - std::is_nothrow_move_constructible::value)> * = nullptr> - expected & operator=(U && v) - { - if (has_value()) { - val() = std::forward(v); - } else { - err().~unexpected(); - ::new (valptr()) T(std::forward(v)); - this->m_has_val = true; - } - - return *this; - } - - template< - class U = T, class G = T, - detail::enable_if_t::value> * = - nullptr, - detail::enable_if_t::value> * = nullptr, - detail::enable_if_t< - (!std::is_same, detail::decay_t>::value && - !detail::conjunction, - std::is_same>>::value && - std::is_constructible::value && - std::is_assignable::value && - std::is_nothrow_move_constructible::value)> * = nullptr> - expected & operator=(U && v) - { - if (has_value()) { - val() = std::forward(v); - } else { - auto tmp = std::move(err()); - err().~unexpected(); - -#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED - try { - ::new (valptr()) T(std::forward(v)); - this->m_has_val = true; - } catch (...) { - err() = std::move(tmp); - throw; - } -#else - ::new (valptr()) T(std::forward(v)); - this->m_has_val = true; -#endif - } - - return *this; - } - - template::value && - std::is_assignable::value> * = nullptr> - expected & operator=(const unexpected & rhs) - { - if (!has_value()) { - err() = rhs; - } else { - this->destroy_val(); - ::new (errptr()) unexpected(rhs); - this->m_has_val = false; - } - - return *this; - } - - template::value && - std::is_move_assignable::value> * = nullptr> - expected & operator=(unexpected && rhs) noexcept - { - if (!has_value()) { - err() = std::move(rhs); - } else { - this->destroy_val(); - ::new (errptr()) unexpected(std::move(rhs)); - this->m_has_val = false; - } - - return *this; - } - - template::value> * = nullptr> - void emplace(Args &&... args) - { - if (has_value()) { - val().~T(); - } else { - err().~unexpected(); - this->m_has_val = true; - } - ::new (valptr()) T(std::forward(args)...); - } - - template::value> * = nullptr> - void emplace(Args &&... args) - { - if (has_value()) { - val().~T(); - ::new (valptr()) T(std::forward(args)...); - } else { - auto tmp = std::move(err()); - err().~unexpected(); - -#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED - try { - ::new (valptr()) T(std::forward(args)...); - this->m_has_val = true; - } catch (...) { - err() = std::move(tmp); - throw; - } -#else - ::new (valptr()) T(std::forward(args)...); - this->m_has_val = true; -#endif - } - } - - template &, Args &&...>::value> * = nullptr> - void emplace(std::initializer_list il, Args &&... args) - { - if (has_value()) { - T t(il, std::forward(args)...); - val() = std::move(t); - } else { - err().~unexpected(); - ::new (valptr()) T(il, std::forward(args)...); - this->m_has_val = true; - } - } - - template &, Args &&...>::value> * = nullptr> - void emplace(std::initializer_list il, Args &&... args) - { - if (has_value()) { - T t(il, std::forward(args)...); - val() = std::move(t); - } else { - auto tmp = std::move(err()); - err().~unexpected(); - -#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED - try { - ::new (valptr()) T(il, std::forward(args)...); - this->m_has_val = true; - } catch (...) { - err() = std::move(tmp); - throw; - } -#else - ::new (valptr()) T(il, std::forward(args)...); - this->m_has_val = true; -#endif - } - } - -private: - using t_is_void = std::true_type; - using t_is_not_void = std::false_type; - using t_is_nothrow_move_constructible = std::true_type; - using move_constructing_t_can_throw = std::false_type; - using e_is_nothrow_move_constructible = std::true_type; - using move_constructing_e_can_throw = std::false_type; - - void swap_where_both_have_value(expected & /*rhs*/, t_is_void) noexcept - { - // swapping void is a no-op - } - - void swap_where_both_have_value(expected & rhs, t_is_not_void) - { - using std::swap; - swap(val(), rhs.val()); - } - - void swap_where_only_one_has_value(expected & rhs, t_is_void) noexcept( - std::is_nothrow_move_constructible::value) - { - ::new (errptr()) unexpected_type(std::move(rhs.err())); - rhs.err().~unexpected_type(); - std::swap(this->m_has_val, rhs.m_has_val); - } - - void swap_where_only_one_has_value(expected & rhs, t_is_not_void) - { - swap_where_only_one_has_value_and_t_is_not_void( - rhs, typename std::is_nothrow_move_constructible::type{}, - typename std::is_nothrow_move_constructible::type{}); - } - - void swap_where_only_one_has_value_and_t_is_not_void( - expected & rhs, t_is_nothrow_move_constructible, - e_is_nothrow_move_constructible) noexcept - { - auto temp = std::move(val()); - val().~T(); - ::new (errptr()) unexpected_type(std::move(rhs.err())); - rhs.err().~unexpected_type(); - ::new (rhs.valptr()) T(std::move(temp)); - std::swap(this->m_has_val, rhs.m_has_val); - } - - void swap_where_only_one_has_value_and_t_is_not_void( - expected & rhs, t_is_nothrow_move_constructible, - move_constructing_e_can_throw) - { - auto temp = std::move(val()); - val().~T(); -#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED - try { - ::new (errptr()) unexpected_type(std::move(rhs.err())); - rhs.err().~unexpected_type(); - ::new (rhs.valptr()) T(std::move(temp)); - std::swap(this->m_has_val, rhs.m_has_val); - } catch (...) { - val() = std::move(temp); - throw; - } -#else - ::new (errptr()) unexpected_type(std::move(rhs.err())); - rhs.err().~unexpected_type(); - ::new (rhs.valptr()) T(std::move(temp)); - std::swap(this->m_has_val, rhs.m_has_val); -#endif - } - - void swap_where_only_one_has_value_and_t_is_not_void( - expected & rhs, move_constructing_t_can_throw, - e_is_nothrow_move_constructible) - { - auto temp = std::move(rhs.err()); - rhs.err().~unexpected_type(); -#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED - try { - ::new (rhs.valptr()) T(std::move(val())); - val().~T(); - ::new (errptr()) unexpected_type(std::move(temp)); - std::swap(this->m_has_val, rhs.m_has_val); - } catch (...) { - rhs.err() = std::move(temp); - throw; - } -#else - ::new (rhs.valptr()) T(std::move(val())); - val().~T(); - ::new (errptr()) unexpected_type(std::move(temp)); - std::swap(this->m_has_val, rhs.m_has_val); -#endif - } - -public: - template - detail::enable_if_t::value && - detail::is_swappable::value && - (std::is_nothrow_move_constructible::value || - std::is_nothrow_move_constructible::value)> - swap(expected & rhs) noexcept( - std::is_nothrow_move_constructible::value && - detail::is_nothrow_swappable::value && - std::is_nothrow_move_constructible::value && - detail::is_nothrow_swappable::value) - { - if (has_value() && rhs.has_value()) { - swap_where_both_have_value(rhs, typename std::is_void::type{}); - } else if (!has_value() && rhs.has_value()) { - rhs.swap(*this); - } else if (has_value()) { - swap_where_only_one_has_value(rhs, typename std::is_void::type{}); - } else { - using std::swap; - swap(err(), rhs.err()); - } - } - - constexpr const T * operator->() const - { - TL_ASSERT(has_value()); - return valptr(); - } - TL_EXPECTED_11_CONSTEXPR T * operator->() - { - TL_ASSERT(has_value()); - return valptr(); - } - - template::value> * = nullptr> - constexpr const U & operator*() const & - { - TL_ASSERT(has_value()); - return val(); - } - template::value> * = nullptr> - TL_EXPECTED_11_CONSTEXPR U & operator*() & - { - TL_ASSERT(has_value()); - return val(); - } - template::value> * = nullptr> - constexpr const U && operator*() const && { - TL_ASSERT(has_value()); - return std::move(val()); - } - template::value> * = nullptr> - TL_EXPECTED_11_CONSTEXPR U && operator*() && { - TL_ASSERT(has_value()); - return std::move(val()); - } - - constexpr bool has_value() const noexcept {return this->m_has_val;} - constexpr explicit operator bool() const noexcept {return this->m_has_val;} - - template::value> * = nullptr> - TL_EXPECTED_11_CONSTEXPR const U & value() const & - { - if (!has_value()) { - detail::throw_exception(bad_expected_access(err().value())); - } - return val(); - } - template::value> * = nullptr> - TL_EXPECTED_11_CONSTEXPR U & value() & - { - if (!has_value()) { - detail::throw_exception(bad_expected_access(err().value())); - } - return val(); - } - template::value> * = nullptr> - TL_EXPECTED_11_CONSTEXPR const U && value() const && { - if (!has_value()) { - detail::throw_exception(bad_expected_access(std::move(err()).value())); - } - return std::move(val()); - } - template::value> * = nullptr> - TL_EXPECTED_11_CONSTEXPR U && value() && { - if (!has_value()) { - detail::throw_exception(bad_expected_access(std::move(err()).value())); - } - return std::move(val()); - } - - constexpr const E & error() const & - { - TL_ASSERT(!has_value()); - return err().value(); - } - TL_EXPECTED_11_CONSTEXPR E & error() & - { - TL_ASSERT(!has_value()); - return err().value(); - } - constexpr const E && error() const && - { - TL_ASSERT(!has_value()); - return std::move(err().value()); - } - TL_EXPECTED_11_CONSTEXPR E && error() && - { - TL_ASSERT(!has_value()); - return std::move(err().value()); - } - - template constexpr T value_or(U && v) const & - { - static_assert( - std::is_copy_constructible::value && - std::is_convertible::value, - "T must be copy-constructible and convertible to from U&&"); - return static_cast(*this) ? **this : static_cast(std::forward(v)); - } - template TL_EXPECTED_11_CONSTEXPR T value_or(U && v) && - { - static_assert( - std::is_move_constructible::value && - std::is_convertible::value, - "T must be move-constructible and convertible to from U&&"); - return static_cast(*this) ? std::move(**this) : static_cast(std::forward(v)); - } -}; - -namespace detail -{ -template using exp_t = typename detail::decay_t::value_type; -template using err_t = typename detail::decay_t::error_type; -template using ret_t = expected>; - -#ifdef TL_EXPECTED_CXX14 -template>::value> * = nullptr, - class Ret = decltype(detail::invoke( - std::declval(), - *std::declval()))> -constexpr auto and_then_impl(Exp && exp, F && f) -{ - static_assert(detail::is_expected::value, "F must return an expected"); - - return exp.has_value() ? - detail::invoke(std::forward(f), *std::forward(exp)) : - Ret(unexpect, std::forward(exp).error()); -} - -template>::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval()))> -constexpr auto and_then_impl(Exp && exp, F && f) -{ - static_assert(detail::is_expected::value, "F must return an expected"); - - return exp.has_value() ? detail::invoke(std::forward(f)) : - Ret(unexpect, std::forward(exp).error()); -} -#else -template -struct TC; -template(), - *std::declval())), - detail::enable_if_t>::value> * = nullptr> -auto and_then_impl(Exp && exp, F && f) -> Ret -{ - static_assert(detail::is_expected::value, "F must return an expected"); - - return exp.has_value() ? - detail::invoke(std::forward(f), *std::forward(exp)) : - Ret(unexpect, std::forward(exp).error()); -} - -template())), - detail::enable_if_t>::value> * = nullptr> -constexpr auto and_then_impl(Exp && exp, F && f) -> Ret -{ - static_assert(detail::is_expected::value, "F must return an expected"); - - return exp.has_value() ? detail::invoke(std::forward(f)) : - Ret(unexpect, std::forward(exp).error()); -} -#endif - -#ifdef TL_EXPECTED_CXX14 -template>::value> * = nullptr, - class Ret = decltype(detail::invoke( - std::declval(), - *std::declval())), - detail::enable_if_t::value> * = nullptr> -constexpr auto expected_map_impl(Exp && exp, F && f) -{ - using result = ret_t>; - return exp.has_value() ? result( - detail::invoke( - std::forward(f), - *std::forward(exp))) : - result(unexpect, std::forward(exp).error()); -} - -template>::value> * = nullptr, - class Ret = decltype(detail::invoke( - std::declval(), - *std::declval())), - detail::enable_if_t::value> * = nullptr> -auto expected_map_impl(Exp && exp, F && f) -{ - using result = expected>; - if (exp.has_value()) { - detail::invoke(std::forward(f), *std::forward(exp)); - return result(); - } - - return result(unexpect, std::forward(exp).error()); -} - -template>::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval())), - detail::enable_if_t::value> * = nullptr> -constexpr auto expected_map_impl(Exp && exp, F && f) -{ - using result = ret_t>; - return exp.has_value() ? result(detail::invoke(std::forward(f))) : - result(unexpect, std::forward(exp).error()); -} - -template>::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval())), - detail::enable_if_t::value> * = nullptr> -auto expected_map_impl(Exp && exp, F && f) -{ - using result = expected>; - if (exp.has_value()) { - detail::invoke(std::forward(f)); - return result(); - } - - return result(unexpect, std::forward(exp).error()); -} -#else -template>::value> * = nullptr, - class Ret = decltype(detail::invoke( - std::declval(), - *std::declval())), - detail::enable_if_t::value> * = nullptr> - -constexpr auto expected_map_impl(Exp && exp, F && f) --> ret_t> -{ - using result = ret_t>; - - return exp.has_value() ? result( - detail::invoke( - std::forward(f), - *std::forward(exp))) : - result(unexpect, std::forward(exp).error()); -} - -template>::value> * = nullptr, - class Ret = decltype(detail::invoke( - std::declval(), - *std::declval())), - detail::enable_if_t::value> * = nullptr> - -auto expected_map_impl(Exp && exp, F && f) -> expected> -{ - if (exp.has_value()) { - detail::invoke(std::forward(f), *std::forward(exp)); - return {}; - } - - return unexpected>(std::forward(exp).error()); -} - -template>::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval())), - detail::enable_if_t::value> * = nullptr> - -constexpr auto expected_map_impl(Exp && exp, F && f) --> ret_t> -{ - using result = ret_t>; - - return exp.has_value() ? result(detail::invoke(std::forward(f))) : - result(unexpect, std::forward(exp).error()); -} - -template>::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval())), - detail::enable_if_t::value> * = nullptr> - -auto expected_map_impl(Exp && exp, F && f) -> expected> -{ - if (exp.has_value()) { - detail::invoke(std::forward(f)); - return {}; - } - - return unexpected>(std::forward(exp).error()); -} -#endif - -#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ - !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) -template>::value> * = nullptr, - class Ret = decltype(detail::invoke( - std::declval(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -constexpr auto map_error_impl(Exp && exp, F && f) -{ - using result = expected, detail::decay_t>; - return exp.has_value() ? - result(*std::forward(exp)) : - result( - unexpect, detail::invoke( - std::forward(f), - std::forward(exp).error())); -} -template>::value> * = nullptr, - class Ret = decltype(detail::invoke( - std::declval(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -auto map_error_impl(Exp && exp, F && f) -{ - using result = expected, monostate>; - if (exp.has_value()) { - return result(*std::forward(exp)); - } - - detail::invoke(std::forward(f), std::forward(exp).error()); - return result(unexpect, monostate{}); -} -template>::value> * = nullptr, - class Ret = decltype(detail::invoke( - std::declval(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -constexpr auto map_error_impl(Exp && exp, F && f) -{ - using result = expected, detail::decay_t>; - return exp.has_value() ? - result() : - result( - unexpect, detail::invoke( - std::forward(f), - std::forward(exp).error())); -} -template>::value> * = nullptr, - class Ret = decltype(detail::invoke( - std::declval(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -auto map_error_impl(Exp && exp, F && f) -{ - using result = expected, monostate>; - if (exp.has_value()) { - return result(); - } - - detail::invoke(std::forward(f), std::forward(exp).error()); - return result(unexpect, monostate{}); -} -#else -template>::value> * = nullptr, - class Ret = decltype(detail::invoke( - std::declval(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -constexpr auto map_error_impl(Exp && exp, F && f) --> expected, detail::decay_t> -{ - using result = expected, detail::decay_t>; - - return exp.has_value() ? - result(*std::forward(exp)) : - result( - unexpect, detail::invoke( - std::forward(f), - std::forward(exp).error())); -} - -template>::value> * = nullptr, - class Ret = decltype(detail::invoke( - std::declval(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -auto map_error_impl(Exp && exp, F && f) -> expected, monostate> -{ - using result = expected, monostate>; - if (exp.has_value()) { - return result(*std::forward(exp)); - } - - detail::invoke(std::forward(f), std::forward(exp).error()); - return result(unexpect, monostate{}); -} - -template>::value> * = nullptr, - class Ret = decltype(detail::invoke( - std::declval(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -constexpr auto map_error_impl(Exp && exp, F && f) --> expected, detail::decay_t> -{ - using result = expected, detail::decay_t>; - - return exp.has_value() ? - result() : - result( - unexpect, detail::invoke( - std::forward(f), - std::forward(exp).error())); -} - -template>::value> * = nullptr, - class Ret = decltype(detail::invoke( - std::declval(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -auto map_error_impl(Exp && exp, F && f) -> expected, monostate> -{ - using result = expected, monostate>; - if (exp.has_value()) { - return result(); - } - - detail::invoke(std::forward(f), std::forward(exp).error()); - return result(unexpect, monostate{}); -} -#endif - -#ifdef TL_EXPECTED_CXX14 -template(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -constexpr auto or_else_impl(Exp && exp, F && f) -{ - static_assert(detail::is_expected::value, "F must return an expected"); - return exp.has_value() ? std::forward(exp) : - detail::invoke( - std::forward(f), - std::forward(exp).error()); -} - -template(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -detail::decay_t or_else_impl(Exp && exp, F && f) -{ - return exp.has_value() ? std::forward(exp) : - (detail::invoke( - std::forward(f), - std::forward(exp).error()), - std::forward(exp)); -} -#else -template(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -auto or_else_impl(Exp && exp, F && f) -> Ret -{ - static_assert(detail::is_expected::value, "F must return an expected"); - return exp.has_value() ? std::forward(exp) : - detail::invoke( - std::forward(f), - std::forward(exp).error()); -} - -template(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -detail::decay_t or_else_impl(Exp && exp, F && f) -{ - return exp.has_value() ? std::forward(exp) : - (detail::invoke( - std::forward(f), - std::forward(exp).error()), - std::forward(exp)); -} -#endif -} // namespace detail - -template -constexpr bool operator==( - const expected & lhs, - const expected & rhs) -{ - return (lhs.has_value() != rhs.has_value()) ? - false : - (!lhs.has_value() ? lhs.error() == rhs.error() : *lhs == *rhs); -} -template -constexpr bool operator!=( - const expected & lhs, - const expected & rhs) -{ - return (lhs.has_value() != rhs.has_value()) ? - true : - (!lhs.has_value() ? lhs.error() != rhs.error() : *lhs != *rhs); -} -template -constexpr bool operator==( - const expected & lhs, - const expected & rhs) -{ - return (lhs.has_value() != rhs.has_value()) ? - false : - (!lhs.has_value() ? lhs.error() == rhs.error() : true); -} -template -constexpr bool operator!=( - const expected & lhs, - const expected & rhs) -{ - return (lhs.has_value() != rhs.has_value()) ? - true : - (!lhs.has_value() ? lhs.error() == rhs.error() : false); -} - -template -constexpr bool operator==(const expected & x, const U & v) -{ - return x.has_value() ? *x == v : false; -} -template -constexpr bool operator==(const U & v, const expected & x) -{ - return x.has_value() ? *x == v : false; -} -template -constexpr bool operator!=(const expected & x, const U & v) -{ - return x.has_value() ? *x != v : true; -} -template -constexpr bool operator!=(const U & v, const expected & x) -{ - return x.has_value() ? *x != v : true; -} - -template -constexpr bool operator==(const expected & x, const unexpected & e) -{ - return x.has_value() ? false : x.error() == e.value(); -} -template -constexpr bool operator==(const unexpected & e, const expected & x) -{ - return x.has_value() ? false : x.error() == e.value(); -} -template -constexpr bool operator!=(const expected & x, const unexpected & e) -{ - return x.has_value() ? true : x.error() != e.value(); -} -template -constexpr bool operator!=(const unexpected & e, const expected & x) -{ - return x.has_value() ? true : x.error() != e.value(); -} - -template::value || - std::is_move_constructible::value) && - detail::is_swappable::value && - std::is_move_constructible::value && - detail::is_swappable::value> * = nullptr> -void swap( - expected & lhs, - expected & rhs) noexcept(noexcept(lhs.swap(rhs))) -{ - lhs.swap(rhs); -} -} // namespace tl - -#endif // POINT_CLOUD_TRANSPORT__TL__EXPECTED_HPP_ diff --git a/package.xml b/package.xml index 78cf1fd..915b239 100644 --- a/package.xml +++ b/package.xml @@ -25,6 +25,7 @@ rclcpp_components rclcpp sensor_msgs + git ament_lint_auto ament_lint_common From a659059ffa849e31873909d1261a991582182ea1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 10 Jul 2023 13:45:16 +0200 Subject: [PATCH 17/28] Added dynamic parameters (#6) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- README.md | 6 ++++ .../publisher_plugin.hpp | 3 ++ .../simple_publisher_plugin.hpp | 34 ++++++++++++++++++- .../simple_subscriber_plugin.hpp | 30 ++++++++++++++++ .../subscriber_plugin.hpp | 5 +++ src/publisher.cpp | 2 ++ src/subscriber.cpp | 2 +- 7 files changed, 80 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 087e294..384bda2 100644 --- a/README.md +++ b/README.md @@ -35,6 +35,12 @@ point_cloud_transport::Publisher pub = pct.advertise("out_point_cloud_base_topic TODO +### Blacklist point cloud transport + +```bash +ros2 run point_cloud_transport_tutorial my_publisher --ros-args -p /pct/point_cloud/disable_pub_plugins:=["point_cloud_transport/raw"] +``` + ## Known transports - [draco_point_cloud_transport](https://wiki.ros.org/draco_point_cloud_transport): Lossy compression via Google Draco library. diff --git a/include/point_cloud_transport/publisher_plugin.hpp b/include/point_cloud_transport/publisher_plugin.hpp index 296acb8..93c8385 100644 --- a/include/point_cloud_transport/publisher_plugin.hpp +++ b/include/point_cloud_transport/publisher_plugin.hpp @@ -83,6 +83,9 @@ class PublisherPlugin //! Shutdown any advertisements associated with this PublisherPlugin. virtual void shutdown() = 0; + //! Declare parameter with this PublisherPlugin. + virtual void declareParameters(const std::string & base_topic) = 0; + //! Return the lookup name of the PublisherPlugin associated with a specific transport identifier. static std::string getLookupName(const std::string & transport_name); diff --git a/include/point_cloud_transport/simple_publisher_plugin.hpp b/include/point_cloud_transport/simple_publisher_plugin.hpp index bb6f2e8..3bf6e27 100644 --- a/include/point_cloud_transport/simple_publisher_plugin.hpp +++ b/include/point_cloud_transport/simple_publisher_plugin.hpp @@ -102,6 +102,26 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin return false; } + template + bool declareParam(const std::string parameter_name, const T value) + { + if (simple_impl_) { + simple_impl_->node_->template declare_parameter(parameter_name, value); + return true; + } + return false; + } + + void setParamCallback( + rclcpp::node_interfaces::NodeParametersInterface::OnSetParametersCallbackType + param_change_callback) + { + if (simple_impl_) { + simple_impl_->on_set_parameters_callback_handle_ = + simple_impl_->node_->add_on_set_parameters_callback(param_change_callback); + } + } + uint32_t getNumSubscribers() const override { if (simple_impl_) { @@ -138,6 +158,14 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin simple_impl_.reset(); } + void declareParameters(const std::string & base_topic) override + { + auto logger = simple_impl_ ? simple_impl_->logger_ : rclcpp::get_logger( + "point_cloud_transport"); + RCLCPP_WARN( + logger, + "declareParameter method not implemented for %s transport", this->getTransportName().c_str()); + } /** * \brief Encode the given raw pointcloud into a compressed message. @@ -155,13 +183,16 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin std::shared_ptr node, const std::string & base_topic, rmw_qos_profile_t custom_qos) { - base_topic_ = base_topic; std::string transport_topic = getTopicToAdvertise(base_topic); simple_impl_ = std::make_unique(node); RCLCPP_DEBUG(simple_impl_->logger_, "getTopicToAdvertise: %s", transport_topic.c_str()); auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos), custom_qos); simple_impl_->pub_ = node->create_publisher(transport_topic, qos); + + base_topic_ = simple_impl_->pub_->get_topic_name(); + + this->declareParameters(base_topic_); } //! Generic function for publishing the internal message type. @@ -210,6 +241,7 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin } std::shared_ptr node_; + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_handle_; rclcpp::Logger logger_; typename rclcpp::Publisher::SharedPtr pub_; }; diff --git a/include/point_cloud_transport/simple_subscriber_plugin.hpp b/include/point_cloud_transport/simple_subscriber_plugin.hpp index 9274be0..0f4a61a 100644 --- a/include/point_cloud_transport/simple_subscriber_plugin.hpp +++ b/include/point_cloud_transport/simple_subscriber_plugin.hpp @@ -81,6 +81,26 @@ class SimpleSubscriberPlugin : public SubscriberPlugin return {}; } + template + bool declareParam(const std::string parameter_name, const T value) + { + if (impl_) { + impl_->node_->template declare_parameter(parameter_name, value); + return true; + } + return false; + } + + void setParamCallback( + rclcpp::node_interfaces::NodeParametersInterface::OnSetParametersCallbackType + param_change_callback) + { + if (impl_) { + impl_->on_set_parameters_callback_handle_ = + impl_->node_->add_on_set_parameters_callback(param_change_callback); + } + } + uint32_t getNumPublishers() const override { if (impl_) { @@ -94,6 +114,10 @@ class SimpleSubscriberPlugin : public SubscriberPlugin impl_.reset(); } + void declareParameters() override + { + } + /** * \brief Decode the given compressed pointcloud into a raw message. * \param[in] compressed The input compressed pointcloud. @@ -140,11 +164,13 @@ class SimpleSubscriberPlugin : public SubscriberPlugin // ros::NodeHandle param_nh(transport_hints.getParameterNH(), getTransportName()); // auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos), custom_qos); + impl_->node_ = node; impl_->sub_ = node->create_subscription( getTopicToSubscribe(base_topic), qos, [this, callback](const typename std::shared_ptr msg) { this->callback(msg, callback); }); + this->declareParameters(); } void subscribeImplWithOptions( @@ -158,6 +184,7 @@ class SimpleSubscriberPlugin : public SubscriberPlugin // Push each group of transport-specific parameters into a separate sub-namespace // ros::NodeHandle param_nh(transport_hints.getParameterNH(), getTransportName()); // + impl_->node_ = node; auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos), custom_qos); impl_->sub_ = node->create_subscription( getTopicToSubscribe(base_topic), qos, @@ -165,12 +192,15 @@ class SimpleSubscriberPlugin : public SubscriberPlugin this->callback(msg, callback); }, options); + this->declareParameters(); } private: struct Impl { rclcpp::SubscriptionBase::SharedPtr sub_; + std::shared_ptr node_; + rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_handle_; }; std::unique_ptr impl_; diff --git a/include/point_cloud_transport/subscriber_plugin.hpp b/include/point_cloud_transport/subscriber_plugin.hpp index 42cfdbf..1cd21f7 100644 --- a/include/point_cloud_transport/subscriber_plugin.hpp +++ b/include/point_cloud_transport/subscriber_plugin.hpp @@ -132,6 +132,11 @@ class SubscriberPlugin */ virtual void shutdown() = 0; + /** + * Declare parameter with this SubscriberPlugin. + */ + virtual void declareParameters() = 0; + /** * Return the lookup name of the SubscriberPlugin associated with a specific * transport identifier. diff --git a/src/publisher.cpp b/src/publisher.cpp index 3d0a6e9..cc6e71d 100644 --- a/src/publisher.cpp +++ b/src/publisher.cpp @@ -126,6 +126,8 @@ Publisher::Publisher( impl_->loader_ = loader; std::vector blacklist_vec; + node->declare_parameter>( + impl_->base_topic_ + "/disable_pub_plugins", blacklist_vec); node->get_parameter(impl_->base_topic_ + "/disable_pub_plugins", blacklist_vec); // set std::set blacklist(blacklist_vec.begin(), blacklist_vec.end()); diff --git a/src/subscriber.cpp b/src/subscriber.cpp index f90fbd5..ee8b3bc 100644 --- a/src/subscriber.cpp +++ b/src/subscriber.cpp @@ -126,8 +126,8 @@ Subscriber::Subscriber( } // Tell plugin to subscribe. - RCLCPP_DEBUG(impl_->logger_, "Subscribing to: %s\n", base_topic.c_str()); impl_->subscriber_->subscribe(node, base_topic, callback, custom_qos, options); + RCLCPP_INFO(impl_->logger_, "Subscribing to: %s\n", base_topic.c_str()); } std::string Subscriber::getTopic() const From 7833f947e4cb3eabd8b4ac165400b4f3340dbff6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 21 Jul 2023 09:33:11 +0200 Subject: [PATCH 18/28] Added QoS override and tests (#7) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- CMakeLists.txt | 95 +++++----- .../point_cloud_transport.hpp | 28 ++- include/point_cloud_transport/publisher.hpp | 3 +- .../publisher_plugin.hpp | 8 +- .../simple_publisher_plugin.hpp | 5 +- src/point_cloud_transport.cpp | 34 ++++ src/publisher.cpp | 5 +- test/test_message_passing.cpp | 117 +++++++++++++ test/test_point_cloud_common.cpp | 41 +++++ test/test_publisher.cpp | 73 ++++++++ test/test_qos_override.cpp | 163 ++++++++++++++++++ test/test_remapping.cpp | 130 ++++++++++++++ test/test_single_subscriber_publisher.cpp | 86 +++++++++ test/test_subscriber.cpp | 80 +++++++++ test/utils.hpp | 71 ++++++++ 15 files changed, 889 insertions(+), 50 deletions(-) create mode 100644 test/test_message_passing.cpp create mode 100644 test/test_point_cloud_common.cpp create mode 100644 test/test_publisher.cpp create mode 100644 test/test_qos_override.cpp create mode 100644 test/test_remapping.cpp create mode 100644 test/test_single_subscriber_publisher.cpp create mode 100644 test/test_subscriber.cpp create mode 100644 test/utils.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 315bbac..1b4915b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -121,46 +121,61 @@ if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) set(AMENT_LINT_AUTO_FILE_EXCLUDE doc/conf.py) ament_lint_auto_find_test_dependencies() -endif() -# TODO: Fix testing -# if (CATKIN_ENABLE_TESTING) -# find_package(roslint REQUIRED) - -# # catkin_lint - checks validity of package.xml and CMakeLists.txt -# # ROS buildfarm calls this without any environment and with empty rosdep cache, -# # so we have problems reading the list of packages from env -# # see https://github.com/ros-infrastructure/ros_buildfarm/issues/923 -# if(DEFINED ENV{ROS_HOME}) -# #catkin_lint: ignore_once env_var -# set(ROS_HOME "$ENV{ROS_HOME}") -# else() -# #catkin_lint: ignore_once env_var -# set(ROS_HOME "$ENV{HOME}/.ros") -# endif() -# #catkin_lint: ignore_once env_var -# if(DEFINED ENV{ROS_ROOT} AND EXISTS "${ROS_HOME}/rosdep/sources.cache") -# roslint_custom(catkin_lint "-W2" .) -# endif() - -# # Roslint C++ - checks formatting and some other rules for C++ files - -# file(GLOB_RECURSE ROSLINT_INCLUDE include/*.h include/*.hpp) -# file(GLOB_RECURSE ROSLINT_SRC src/*.cpp src/*.hpp src/*.h) -# #file(GLOB_RECURSE ROSLINT_TEST test/*.cpp) - -# set(ROSLINT_CPP_OPTS "--extensions=h,hpp,hh,c,cpp,cc;--linelength=120;--filter=\ -# -build/header_guard,-readability/namespace,-whitespace/braces,-runtime/references,\ -# -build/c++11,-readability/nolint,-readability/todo,-legal/copyright,-build/namespaces") -# roslint_cpp(${ROSLINT_INCLUDE} ${ROSLINT_SRC}) - -# # Roslint Python - -# # Run roslint on Python sources -# file(GLOB_RECURSE python_files src/*.py) -# roslint_python("${python_files}") - -# roslint_add_test() -# endif() + find_package(ament_cmake_gtest) + + ament_add_gtest(${PROJECT_NAME}-point_cloud_common test/test_point_cloud_common.cpp) + if(TARGET ${PROJECT_NAME}-point_cloud_common) + target_link_libraries(${PROJECT_NAME}-point_cloud_common ${PROJECT_NAME}) + endif() + + ament_add_gtest(${PROJECT_NAME}-publisher test/test_publisher.cpp) + if(TARGET ${PROJECT_NAME}-publisher) + target_link_libraries(${PROJECT_NAME}-publisher ${PROJECT_NAME}) + ament_target_dependencies(${PROJECT_NAME}-publisher + pluginlib + ) + endif() + + ament_add_gtest(${PROJECT_NAME}-subscriber test/test_subscriber.cpp) + if(TARGET ${PROJECT_NAME}-subscriber) + target_link_libraries(${PROJECT_NAME}-subscriber ${PROJECT_NAME}) + ament_target_dependencies(${PROJECT_NAME}-subscriber + pluginlib + ) + endif() + + ament_add_gtest(${PROJECT_NAME}-message_passing test/test_message_passing.cpp) + if(TARGET ${PROJECT_NAME}-message_passing) + target_link_libraries(${PROJECT_NAME}-message_passing ${PROJECT_NAME}) + ament_target_dependencies(${PROJECT_NAME}-message_passing + pluginlib + ) + endif() + + ament_add_gtest(${PROJECT_NAME}-qos_override test/test_qos_override.cpp) + if(TARGET ${PROJECT_NAME}-qos_override) + target_link_libraries(${PROJECT_NAME}-qos_override ${PROJECT_NAME}) + ament_target_dependencies(${PROJECT_NAME}-qos_override + pluginlib + ) + endif() + + ament_add_gtest(${PROJECT_NAME}-remapping test/test_remapping.cpp) + if(TARGET ${PROJECT_NAME}-remapping) + target_link_libraries(${PROJECT_NAME}-remapping ${PROJECT_NAME}) + ament_target_dependencies(${PROJECT_NAME}-remapping + pluginlib + ) + endif() + + ament_add_gtest(${PROJECT_NAME}-single_subscriber_publisher test/test_single_subscriber_publisher.cpp) + if(TARGET ${PROJECT_NAME}-single_subscriber_publisher) + target_link_libraries(${PROJECT_NAME}-single_subscriber_publisher ${PROJECT_NAME}) + ament_target_dependencies(${PROJECT_NAME}-single_subscriber_publisher + pluginlib + ) + endif() +endif() ament_package() diff --git a/include/point_cloud_transport/point_cloud_transport.hpp b/include/point_cloud_transport/point_cloud_transport.hpp index e12eac8..f960f75 100644 --- a/include/point_cloud_transport/point_cloud_transport.hpp +++ b/include/point_cloud_transport/point_cloud_transport.hpp @@ -83,11 +83,34 @@ class PointCloudTransportLoader //! The loader that can load subscriber plugins. SubLoaderPtr getSubscriberLoader() const; + point_cloud_transport::PubLoaderPtr getPubLoader(); + point_cloud_transport::SubLoaderPtr getSubLoader(); + protected: point_cloud_transport::PubLoaderPtr pub_loader_; point_cloud_transport::SubLoaderPtr sub_loader_; }; +/*! + * \brief Advertise an image topic, free function version. + */ +Publisher create_publisher( + std::shared_ptr node, + const std::string & base_topic, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default, + const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions()); + +/** + * \brief Subscribe to an image topic, free function version. + */ +Subscriber create_subscription( + std::shared_ptr node, + const std::string & base_topic, + const Subscriber::Callback & callback, + const std::string & transport, + rmw_qos_profile_t custom_qos = rmw_qos_profile_default, + rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()); + /** * Advertise and subscribe to PointCloud2 topics. * @@ -120,9 +143,10 @@ class PointCloudTransport : public PointCloudTransportLoader //! Advertise a PointCloud2 topic, simple version. Publisher advertise( const std::string & base_topic, - rmw_qos_profile_t custom_qos) + rmw_qos_profile_t custom_qos, + const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions()) { - return Publisher(node_, base_topic, pub_loader_, custom_qos); + return Publisher(node_, base_topic, pub_loader_, custom_qos, options); } //! Advertise an PointCloud2 topic with subscriber status callbacks. diff --git a/include/point_cloud_transport/publisher.hpp b/include/point_cloud_transport/publisher.hpp index 3f909e9..a80f6a5 100644 --- a/include/point_cloud_transport/publisher.hpp +++ b/include/point_cloud_transport/publisher.hpp @@ -60,7 +60,8 @@ class Publisher std::shared_ptr node, const std::string & base_topic, PubLoaderPtr loader, - rmw_qos_profile_t custom_qos); + rmw_qos_profile_t custom_qos, + const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions()); //! get total number of subscribers to all advertised topics. uint32_t getNumSubscribers() const; diff --git a/include/point_cloud_transport/publisher_plugin.hpp b/include/point_cloud_transport/publisher_plugin.hpp index 93c8385..0d5071f 100644 --- a/include/point_cloud_transport/publisher_plugin.hpp +++ b/include/point_cloud_transport/publisher_plugin.hpp @@ -63,9 +63,10 @@ class PublisherPlugin void advertise( std::shared_ptr nh, const std::string & base_topic, - rmw_qos_profile_t custom_qos = rmw_qos_profile_default) + rmw_qos_profile_t custom_qos = rmw_qos_profile_default, + const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions()) { - advertiseImpl(nh, base_topic, custom_qos); + advertiseImpl(nh, base_topic, custom_qos, options); } //! Returns the number of subscribers that are currently connected to this PublisherPlugin @@ -95,7 +96,8 @@ class PublisherPlugin */ virtual void advertiseImpl( std::shared_ptr nh, const std::string & base_topic, - rmw_qos_profile_t custom_qos) = 0; + rmw_qos_profile_t custom_qos, + const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions()) = 0; }; class SingleTopicPublisherPlugin : public PublisherPlugin diff --git a/include/point_cloud_transport/simple_publisher_plugin.hpp b/include/point_cloud_transport/simple_publisher_plugin.hpp index 3bf6e27..86a4a0b 100644 --- a/include/point_cloud_transport/simple_publisher_plugin.hpp +++ b/include/point_cloud_transport/simple_publisher_plugin.hpp @@ -181,14 +181,15 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin virtual void advertiseImpl( std::shared_ptr node, const std::string & base_topic, - rmw_qos_profile_t custom_qos) + rmw_qos_profile_t custom_qos, + const rclcpp::PublisherOptions & options) { std::string transport_topic = getTopicToAdvertise(base_topic); simple_impl_ = std::make_unique(node); RCLCPP_DEBUG(simple_impl_->logger_, "getTopicToAdvertise: %s", transport_topic.c_str()); auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos), custom_qos); - simple_impl_->pub_ = node->create_publisher(transport_topic, qos); + simple_impl_->pub_ = node->create_publisher(transport_topic, qos, options); base_topic_ = simple_impl_->pub_->get_topic_name(); diff --git a/src/point_cloud_transport.cpp b/src/point_cloud_transport.cpp index 62978d0..ad5018a 100644 --- a/src/point_cloud_transport.cpp +++ b/src/point_cloud_transport.cpp @@ -62,10 +62,44 @@ PointCloudTransportLoader::PointCloudTransportLoader() { } +point_cloud_transport::PubLoaderPtr PointCloudTransportLoader::getPubLoader() +{ + return pub_loader_; +} + +point_cloud_transport::SubLoaderPtr PointCloudTransportLoader::getSubLoader() +{ + return sub_loader_; +} + PointCloudTransportLoader::~PointCloudTransportLoader() { } +static PointCloudTransportLoader * kImpl = new PointCloudTransportLoader(); + +Publisher create_publisher( + std::shared_ptr node, + const std::string & base_topic, + rmw_qos_profile_t custom_qos, + const rclcpp::PublisherOptions & options) +{ + return Publisher(node, base_topic, kImpl->getPubLoader(), custom_qos, options); +} + +Subscriber create_subscription( + std::shared_ptr node, + const std::string & base_topic, + const Subscriber::Callback & callback, + const std::string & transport, + rmw_qos_profile_t custom_qos, + rclcpp::SubscriptionOptions options) +{ + return Subscriber( + node, base_topic, callback, + kImpl->getSubLoader(), transport, custom_qos, options); +} + std::vector PointCloudTransportLoader::getDeclaredTransports() const { auto transports = sub_loader_->getDeclaredClasses(); diff --git a/src/publisher.cpp b/src/publisher.cpp index cc6e71d..94bbb99 100644 --- a/src/publisher.cpp +++ b/src/publisher.cpp @@ -114,7 +114,8 @@ struct Publisher::Impl Publisher::Publisher( std::shared_ptr node, const std::string & base_topic, - PubLoaderPtr loader, rmw_qos_profile_t custom_qos) + PubLoaderPtr loader, rmw_qos_profile_t custom_qos, + const rclcpp::PublisherOptions & options) : impl_(std::make_shared(node)) { // Resolve the name explicitly because otherwise the compressed topics don't remap @@ -140,7 +141,7 @@ Publisher::Publisher( try { auto pub = loader->createUniqueInstance(lookup_name); - pub->advertise(node, point_cloud_topic, custom_qos); + pub->advertise(node, point_cloud_topic, custom_qos, options); impl_->base_topic_ = pub->getTopic(); impl_->publishers_.push_back(std::move(pub)); } catch (const std::runtime_error & e) { diff --git a/test/test_message_passing.cpp b/test/test_message_passing.cpp new file mode 100644 index 0000000..c2df7e0 --- /dev/null +++ b/test/test_message_passing.cpp @@ -0,0 +1,117 @@ +// Copyright (c) 2023 Open Source Robotics Foundation, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include +#include + +#include +#include + +#include +#include + +#include "utils.hpp" + +using namespace std::chrono_literals; + +int total_pointclouds_received = 0; + +class MessagePassingTesting : public ::testing::Test +{ +public: + sensor_msgs::msg::PointCloud2::UniquePtr generate_random_cloudpoint() + { + auto pointcloud = std::make_unique(); + return pointcloud; + } + +protected: + void SetUp() + { + node_ = rclcpp::Node::make_shared("test_message_passing"); + total_pointclouds_received = 0; + } + + rclcpp::Node::SharedPtr node_; +}; + +void pointcloudCallback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & msg) +{ + (void) msg; + total_pointclouds_received++; +} + +TEST_F(MessagePassingTesting, one_message_passing) +{ + const size_t max_retries = 3; + const size_t max_loops = 200; + const std::chrono::milliseconds sleep_per_loop = std::chrono::milliseconds(10); + + rclcpp::executors::SingleThreadedExecutor executor; + + auto pub = point_cloud_transport::create_publisher(node_, "camera/pointcloud"); + auto sub = + point_cloud_transport::create_subscription( + node_, "camera/pointcloud", pointcloudCallback, + "raw"); + + test_rclcpp::wait_for_subscriber(node_, sub.getTopic()); + + ASSERT_EQ(0, total_pointclouds_received); + ASSERT_EQ(1u, pub.getNumSubscribers()); + ASSERT_EQ(1u, sub.getNumPublishers()); + + executor.spin_node_some(node_); + ASSERT_EQ(0, total_pointclouds_received); + + size_t retry = 0; + while (retry < max_retries && total_pointclouds_received == 0) { + // generate random pointcloud and publish it + pub.publish(generate_random_cloudpoint()); + + executor.spin_node_some(node_); + size_t loop = 0; + while ((total_pointclouds_received != 1) && (loop++ < max_loops)) { + std::this_thread::sleep_for(sleep_per_loop); + executor.spin_node_some(node_); + } + } + + ASSERT_EQ(1, total_pointclouds_received); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; +} diff --git a/test/test_point_cloud_common.cpp b/test/test_point_cloud_common.cpp new file mode 100644 index 0000000..7372043 --- /dev/null +++ b/test/test_point_cloud_common.cpp @@ -0,0 +1,41 @@ +// Copyright (c) 2023 Open Source Robotics Foundation, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include "point_cloud_transport/point_cloud_common.hpp" + +TEST(PointCloudCommon, erase_last_copy) { + EXPECT_EQ("pointcloud", point_cloud_transport::erase_last_copy("pointcloud_pub", "_pub")); + EXPECT_EQ( + "/pointcloud_pub/pointcloud", + point_cloud_transport::erase_last_copy("/pointcloud_pub/pointcloud_pub", "_pub")); + EXPECT_EQ( + "/pointcloud/pointcloud", + point_cloud_transport::erase_last_copy("/pointcloud_pub/pointcloud", "_pub")); +} diff --git a/test/test_publisher.cpp b/test/test_publisher.cpp new file mode 100644 index 0000000..0daff5e --- /dev/null +++ b/test/test_publisher.cpp @@ -0,0 +1,73 @@ +// Copyright (c) 2023 Open Source Robotics Foundation, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include +#include + +#include + +#include "point_cloud_transport/point_cloud_transport.hpp" + +class TestPublisher : public ::testing::Test +{ +protected: + void SetUp() + { + node_ = rclcpp::Node::make_shared("test_publisher"); + } + + rclcpp::Node::SharedPtr node_; +}; + +TEST_F(TestPublisher, publisher) +{ + auto pub = point_cloud_transport::create_publisher(node_, "camera/point_cloud"); + EXPECT_EQ(node_->get_node_graph_interface()->count_publishers("camera/point_cloud"), 1u); + pub.shutdown(); + EXPECT_EQ(node_->get_node_graph_interface()->count_publishers("camera/point_cloud"), 0u); + // coverage tests: invalid publisher should fail but not crash + pub.publish(sensor_msgs::msg::PointCloud2()); + pub.publish(sensor_msgs::msg::PointCloud2::ConstSharedPtr()); +} + +TEST_F(TestPublisher, point_cloud_transport_publisher) +{ + point_cloud_transport::PointCloudTransport it(node_); + auto pub = it.advertise("camera/point_cloud", rmw_qos_profile_sensor_data); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; +} diff --git a/test/test_qos_override.cpp b/test/test_qos_override.cpp new file mode 100644 index 0000000..eebb653 --- /dev/null +++ b/test/test_qos_override.cpp @@ -0,0 +1,163 @@ +// Copyright (c) 2023 Open Source Robotics Foundation, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include +#include + +#include + +#include "point_cloud_transport/point_cloud_transport.hpp" + +class TestQosOverride : public ::testing::Test +{ +protected: + void SetUp() + { + pub_node_ = rclcpp::Node::make_shared("test_publisher"); + qos_override_pub_node_ = rclcpp::Node::make_shared( + "test_qos_override_publisher", rclcpp::NodeOptions().parameter_overrides( + { + rclcpp::Parameter( + "qos_overrides./camera/pointcloud.publisher.reliability", "best_effort"), + })); + sub_node_ = rclcpp::Node::make_shared("test_subscriber"); + qos_override_sub_node_ = rclcpp::Node::make_shared( + "test_qos_override_subscriber", rclcpp::NodeOptions().parameter_overrides( + { + rclcpp::Parameter( + "qos_overrides./camera/pointcloud.subscription.reliability", "best_effort"), + })); + } + + rclcpp::Node::SharedPtr pub_node_; + rclcpp::Node::SharedPtr qos_override_pub_node_; + rclcpp::Node::SharedPtr sub_node_; + rclcpp::Node::SharedPtr qos_override_sub_node_; +}; + +TEST_F(TestQosOverride, qos_override_publisher_without_options) { + auto pub = point_cloud_transport::create_publisher( + pub_node_, "camera/pointcloud", + rmw_qos_profile_default); + auto endpoint_info_vec = pub_node_->get_publishers_info_by_topic("camera/pointcloud"); + EXPECT_EQ(endpoint_info_vec[0].qos_profile().reliability(), rclcpp::ReliabilityPolicy::Reliable); + pub.shutdown(); + + pub = point_cloud_transport::create_publisher( + qos_override_pub_node_, "camera/pointcloud", rmw_qos_profile_default); + + endpoint_info_vec = qos_override_pub_node_->get_publishers_info_by_topic("camera/pointcloud"); + EXPECT_EQ( + endpoint_info_vec[0].qos_profile().reliability(), + rclcpp::ReliabilityPolicy::Reliable); + pub.shutdown(); +} + +TEST_F(TestQosOverride, qos_override_publisher_with_options) { + rclcpp::PublisherOptions options; + options.qos_overriding_options = rclcpp::QosOverridingOptions( + { + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::Durability, + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Reliability, + }); + + auto pub = point_cloud_transport::create_publisher( + pub_node_, "camera/pointcloud", rmw_qos_profile_default, options); + auto endpoint_info_vec = pub_node_->get_publishers_info_by_topic("camera/pointcloud"); + EXPECT_EQ(endpoint_info_vec[0].qos_profile().reliability(), rclcpp::ReliabilityPolicy::Reliable); + pub.shutdown(); + + pub = point_cloud_transport::create_publisher( + qos_override_pub_node_, "camera/pointcloud", rmw_qos_profile_default, options); + + endpoint_info_vec = qos_override_pub_node_->get_publishers_info_by_topic("camera/pointcloud"); + EXPECT_EQ( + endpoint_info_vec[0].qos_profile().reliability(), + rclcpp::ReliabilityPolicy::BestEffort); + pub.shutdown(); +} + +TEST_F(TestQosOverride, qos_override_subscriber_without_options) { + std::function fcn = + [](const auto & msg) {(void)msg;}; + + auto sub = point_cloud_transport::create_subscription( + sub_node_, "camera/pointcloud", fcn, "raw", rmw_qos_profile_default); + auto endpoint_info_vec = sub_node_->get_subscriptions_info_by_topic("camera/pointcloud"); + EXPECT_EQ(endpoint_info_vec[0].qos_profile().reliability(), rclcpp::ReliabilityPolicy::Reliable); + sub.shutdown(); + + sub = point_cloud_transport::create_subscription( + qos_override_sub_node_, "camera/pointcloud", fcn, "raw", rmw_qos_profile_default); + + endpoint_info_vec = qos_override_sub_node_->get_subscriptions_info_by_topic("camera/pointcloud"); + EXPECT_EQ( + endpoint_info_vec[0].qos_profile().reliability(), + rclcpp::ReliabilityPolicy::Reliable); +} + +TEST_F(TestQosOverride, qos_override_subscriber_with_options) { + std::function fcn = + [](const auto & msg) {(void)msg;}; + + rclcpp::SubscriptionOptions options; + options.qos_overriding_options = rclcpp::QosOverridingOptions( + { + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::Durability, + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Reliability, + }); + + auto sub = point_cloud_transport::create_subscription( + sub_node_, "camera/pointcloud", fcn, "raw", rmw_qos_profile_default, options); + auto endpoint_info_vec = sub_node_->get_subscriptions_info_by_topic("camera/pointcloud"); + EXPECT_EQ(endpoint_info_vec[0].qos_profile().reliability(), rclcpp::ReliabilityPolicy::Reliable); + sub.shutdown(); + + sub = point_cloud_transport::create_subscription( + qos_override_sub_node_, "camera/pointcloud", fcn, "raw", rmw_qos_profile_default, options); + + endpoint_info_vec = qos_override_sub_node_->get_subscriptions_info_by_topic("camera/pointcloud"); + EXPECT_EQ( + endpoint_info_vec[0].qos_profile().reliability(), + rclcpp::ReliabilityPolicy::BestEffort); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; +} diff --git a/test/test_remapping.cpp b/test/test_remapping.cpp new file mode 100644 index 0000000..2b38b95 --- /dev/null +++ b/test/test_remapping.cpp @@ -0,0 +1,130 @@ +// Copyright (c) 2023 Open Source Robotics Foundation, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include +#include +#include +#include + +#include +#include +#include "utils.hpp" + +#include "point_cloud_transport/point_cloud_transport.hpp" + +class TestPublisher : public ::testing::Test +{ +protected: + void SetUp() + { + node_ = rclcpp::Node::make_shared("node", "namespace"); + std::vector arguments{ + "--ros-args", "-r", "old_topic:=new_topic" + }; + rclcpp::NodeOptions node_options; + node_options.arguments(arguments); + + node_remap_ = rclcpp::Node::make_shared( + "node_remap", + "namespace", + node_options + ); + } + + rclcpp::Node::SharedPtr node_; + rclcpp::Node::SharedPtr node_remap_; +}; + +TEST_F(TestPublisher, RemappedPublisher) { + const size_t max_retries = 3; + const size_t max_loops = 200; + const std::chrono::milliseconds sleep_per_loop = std::chrono::milliseconds(10); + + rclcpp::executors::SingleThreadedExecutor executor; + auto pointcloud = std::make_shared(); + + // Subscribe + bool received{false}; + auto sub = point_cloud_transport::create_subscription( + node_remap_, "old_topic", + [&received](const sensor_msgs::msg::PointCloud2::ConstSharedPtr & msg) { + (void)msg; + received = true; + }, "raw"); + + // Publish + auto pub = point_cloud_transport::create_publisher(node_, "new_topic"); + + ASSERT_EQ("/namespace/new_topic", sub.getTopic()); + ASSERT_EQ("/namespace/new_topic", pub.getTopic()); + + test_rclcpp::wait_for_subscriber(node_, sub.getTopic()); + + ASSERT_FALSE(received); + + size_t retry = 0; + size_t nSub = 0; + size_t nPub = 0; + while (retry < max_retries && nPub == 0 && nSub == 0) { + nSub = pub.getNumSubscribers(); + nPub = sub.getNumPublishers(); + std::this_thread::sleep_for(sleep_per_loop); + } + + executor.spin_node_some(node_); + executor.spin_node_some(node_remap_); + + retry = 0; + while (retry < max_retries && !received) { + // generate random pointcloud and publish it + pub.publish(pointcloud); + + executor.spin_node_some(node_); + executor.spin_node_some(node_remap_); + + size_t loop = 0; + while ((!received) && (loop++ < max_loops)) { + std::this_thread::sleep_for(sleep_per_loop); + executor.spin_node_some(node_); + executor.spin_node_some(node_remap_); + } + } + + EXPECT_TRUE(received); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; +} diff --git a/test/test_single_subscriber_publisher.cpp b/test/test_single_subscriber_publisher.cpp new file mode 100644 index 0000000..b7d9146 --- /dev/null +++ b/test/test_single_subscriber_publisher.cpp @@ -0,0 +1,86 @@ +// Copyright (c) 2023 Open Source Robotics Foundation, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include +#include +#include + +#include "gtest/gtest.h" + +#include +#include + +#include "point_cloud_transport/single_subscriber_publisher.hpp" + +class TestPublisher : public ::testing::Test +{ +protected: + static constexpr const char * caller_id = "node"; + static constexpr const char * topic = "/topic"; + + + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } + + void SetUp() + { + node = std::make_shared("point_cloud_transport", "/ns"); + } + + void TearDown() + { + node.reset(); + } + + rclcpp::Node::SharedPtr node; +}; + +TEST_F(TestPublisher, construction_and_destruction) { + auto get_num_subscribers = []() -> size_t {return 0;}; + auto publish_fn = [](const sensor_msgs::msg::PointCloud2 & /*pointcloud*/) {}; + + point_cloud_transport::SingleSubscriberPublisher ssp(caller_id, topic, + get_num_subscribers, publish_fn); +} + +TEST_F(TestPublisher, getNumSubscribers) { + size_t nSub = 0; + + auto get_num_subscribers = [&nSub]() -> size_t {return nSub;}; + auto publish_fn = [](const sensor_msgs::msg::PointCloud2 & /*pointcloud*/) {}; + + point_cloud_transport::SingleSubscriberPublisher ssp(caller_id, topic, + get_num_subscribers, publish_fn); + + nSub = 0; + ASSERT_EQ(ssp.getNumSubscribers(), 0u); + nSub = 1; + ASSERT_EQ(ssp.getNumSubscribers(), 1u); +} diff --git a/test/test_subscriber.cpp b/test/test_subscriber.cpp new file mode 100644 index 0000000..8494f1b --- /dev/null +++ b/test/test_subscriber.cpp @@ -0,0 +1,80 @@ +// Copyright (c) 2023 Open Source Robotics Foundation, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include +#include + +#include + +#include "point_cloud_transport/point_cloud_transport.hpp" + +class TestSubscriber : public ::testing::Test +{ +protected: + void SetUp() + { + node_ = rclcpp::Node::make_shared("test_subscriber"); + } + + rclcpp::Node::SharedPtr node_; +}; + +TEST_F(TestSubscriber, construction_and_destruction) +{ + std::function fcn = + [](const auto & msg) { + (void)msg; + }; + + auto sub = point_cloud_transport::create_subscription(node_, "camera/pointcloud", fcn, "raw"); + + rclcpp::executors::SingleThreadedExecutor executor; + executor.spin_node_some(node_); +} + +TEST_F(TestSubscriber, shutdown) +{ + std::function fcn = + [](const auto & msg) {(void)msg;}; + + auto sub = point_cloud_transport::create_subscription(node_, "camera/pointcloud", fcn, "raw"); + EXPECT_EQ(node_->get_node_graph_interface()->count_subscribers("camera/pointcloud"), 1u); + sub.shutdown(); + EXPECT_EQ(node_->get_node_graph_interface()->count_subscribers("camera/pointcloud"), 0u); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; +} diff --git a/test/utils.hpp b/test/utils.hpp new file mode 100644 index 0000000..aee9d9c --- /dev/null +++ b/test/utils.hpp @@ -0,0 +1,71 @@ +// Copyright 2016 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef UTILS_HPP_ +#define UTILS_HPP_ + +#include +#include +#include +#include + +#include "gtest/gtest.h" + +#include "rclcpp/rclcpp.hpp" + +namespace test_rclcpp +{ + +// Sleep for timeout ms or until a subscriber has registered for the topic +// if to_be_available is true, then it will wait for the number of +// subscribers to be > 0, if false it will wait for the number of +// subscribers to be 0 +void wait_for_subscriber( + std::shared_ptr node, + const std::string & topic_name, + bool to_be_available = true, + std::chrono::milliseconds timeout = std::chrono::milliseconds(1), + std::chrono::microseconds sleep_period = std::chrono::seconds(1)) +{ + using std::chrono::duration_cast; + using std::chrono::microseconds; + using std::chrono::steady_clock; + auto start = steady_clock::now(); + microseconds time_slept(0); + auto predicate = [&node, &topic_name, &to_be_available]() -> bool { + if (to_be_available) { + // the subscriber is available if the count is gt 0 + return node->count_subscribers(topic_name) > 0; + } else { + // the subscriber is no longer available when the count is 0 + return node->count_subscribers(topic_name) == 0; + } + }; + while (!predicate() && time_slept < duration_cast(timeout)) { + rclcpp::Event::SharedPtr graph_event = node->get_graph_event(); + node->wait_for_graph_change(graph_event, sleep_period); + time_slept = duration_cast(steady_clock::now() - start); + } + int64_t time_slept_count = + std::chrono::duration_cast(time_slept).count(); + printf( + "Waited %" PRId64 " microseconds for the subscriber to %s topic '%s'\n", + time_slept_count, + to_be_available ? "connect to" : "disconnect from", + topic_name.c_str()); +} + +} // namespace test_rclcpp + +#endif // UTILS_HPP_ From a6564c68be0cb79f469459afff386efe49c4048c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 21 Jul 2023 09:38:39 +0200 Subject: [PATCH 19/28] Allow to include param description (#9) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- include/point_cloud_transport/simple_publisher_plugin.hpp | 8 ++++++-- .../point_cloud_transport/simple_subscriber_plugin.hpp | 2 +- src/subscriber.cpp | 2 +- 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/include/point_cloud_transport/simple_publisher_plugin.hpp b/include/point_cloud_transport/simple_publisher_plugin.hpp index 86a4a0b..2fb90b4 100644 --- a/include/point_cloud_transport/simple_publisher_plugin.hpp +++ b/include/point_cloud_transport/simple_publisher_plugin.hpp @@ -103,10 +103,14 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin } template - bool declareParam(const std::string parameter_name, const T value) + bool declareParam( + const std::string parameter_name, const T value, + const rcl_interfaces::msg::ParameterDescriptor & parameter_descriptor = + rcl_interfaces::msg::ParameterDescriptor()) { if (simple_impl_) { - simple_impl_->node_->template declare_parameter(parameter_name, value); + simple_impl_->node_->template declare_parameter( + parameter_name, value, parameter_descriptor); return true; } return false; diff --git a/include/point_cloud_transport/simple_subscriber_plugin.hpp b/include/point_cloud_transport/simple_subscriber_plugin.hpp index 0f4a61a..b173050 100644 --- a/include/point_cloud_transport/simple_subscriber_plugin.hpp +++ b/include/point_cloud_transport/simple_subscriber_plugin.hpp @@ -130,7 +130,7 @@ class SimpleSubscriberPlugin : public SubscriberPlugin /** * Process a message. Must be implemented by the subclass. */ - virtual void callback(const typename M::ConstPtr & message, const Callback & user_cb) + virtual void callback(const typename std::shared_ptr & message, const Callback & user_cb) { DecodeResult res = this->decodeTyped(*message); if (!res) { diff --git a/src/subscriber.cpp b/src/subscriber.cpp index ee8b3bc..90f41c1 100644 --- a/src/subscriber.cpp +++ b/src/subscriber.cpp @@ -127,7 +127,7 @@ Subscriber::Subscriber( // Tell plugin to subscribe. impl_->subscriber_->subscribe(node, base_topic, callback, custom_qos, options); - RCLCPP_INFO(impl_->logger_, "Subscribing to: %s\n", base_topic.c_str()); + RCLCPP_INFO(impl_->logger_, "Subscribing to: %s\n", impl_->subscriber_->getTopic().c_str()); } std::string Subscriber::getTopic() const From 4f8149d6fb239f33e12639214d103e935a00dfb2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 21 Jul 2023 09:42:10 +0200 Subject: [PATCH 20/28] Fixed subscriber filters (#10) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../subscriber_filter.hpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/include/point_cloud_transport/subscriber_filter.hpp b/include/point_cloud_transport/subscriber_filter.hpp index e47b158..659a551 100644 --- a/include/point_cloud_transport/subscriber_filter.hpp +++ b/include/point_cloud_transport/subscriber_filter.hpp @@ -69,17 +69,16 @@ class SubscriberFilter : public message_filters::SimpleFilter node, const std::string & base_topic, const std::string & transport) { - subscribe(pct, node, base_topic, transport); + subscribe(node, base_topic, transport); } /** @@ -99,11 +98,13 @@ class SubscriberFilter : public message_filters::SimpleFilter node, const std::string & base_topic, const std::string & transport, @@ -111,11 +112,10 @@ class SubscriberFilter : public message_filters::SimpleFilter Date: Fri, 21 Jul 2023 13:59:47 +0200 Subject: [PATCH 21/28] Windows support (#8) Signed-off-by: Alejandro Hernandez Cordero --- CMakeLists.txt | 31 ++++++++++++++----- .../point_cloud_common.hpp | 6 ++++ .../point_cloud_transport.hpp | 18 +++++++++++ include/point_cloud_transport/publisher.hpp | 13 ++++++++ .../publisher_plugin.hpp | 11 ++++--- .../point_cloud_transport/raw_publisher.hpp | 2 ++ .../point_cloud_transport/raw_subscriber.hpp | 4 +++ include/point_cloud_transport/republish.hpp | 5 +-- .../simple_publisher_plugin.hpp | 4 ++- .../simple_subscriber_plugin.hpp | 2 +- .../single_subscriber_publisher.hpp | 9 ++++++ include/point_cloud_transport/subscriber.hpp | 11 +++++++ .../subscriber_filter.hpp | 11 +++++++ .../subscriber_plugin.hpp | 4 ++- .../point_cloud_transport/transport_hints.hpp | 5 +++ ...ility_control.h => visibility_control.hpp} | 6 ++-- 16 files changed, 121 insertions(+), 21 deletions(-) rename include/point_cloud_transport/{visibility_control.h => visibility_control.hpp} (91%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1b4915b..94354e9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,9 +1,17 @@ cmake_minimum_required(VERSION 3.10.2) -set(CMAKE_CXX_STANDARD 17) - project(point_cloud_transport) +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + find_package(ament_cmake_ros REQUIRED) find_package(message_filters REQUIRED) @@ -49,6 +57,8 @@ target_link_libraries(${PROJECT_NAME} PUBLIC target_link_libraries(${PROJECT_NAME} PRIVATE pluginlib::pluginlib) +target_compile_definitions(${PROJECT_NAME} PRIVATE "POINT_CLOUD_TRANSPORT_BUILDING_DLL") + # Build libpoint_cloud_transport_plugins add_library(${PROJECT_NAME}_plugins src/manifest.cpp @@ -63,11 +73,16 @@ add_library(republish_node SHARED src/republish.cpp) target_link_libraries(republish_node ${PROJECT_NAME} ) +target_compile_definitions(republish_node PRIVATE "POINT_CLOUD_TRANSPORT_BUILDING_DLL") ament_target_dependencies(republish_node pluginlib rclcpp_components rclcpp ) +target_include_directories(republish_node PUBLIC + "$" + "$" + "$") rclcpp_components_register_nodes(republish_node "point_cloud_transport::Republisher") add_executable(republish @@ -84,10 +99,6 @@ ament_target_dependencies(republish rclcpp ) -target_include_directories(${PROJECT_NAME} PUBLIC - "$" - "$") - # Build list_transports add_executable(list_transports src/list_transports.cpp) target_link_libraries(list_transports @@ -106,12 +117,16 @@ install(TARGETS ${PROJECT_NAME} EXPORT export_${PROJECT_NAME}) # Install executables install( - TARGETS list_transports republish + TARGETS + list_transports + republish RUNTIME DESTINATION lib/${PROJECT_NAME} ) # Install include directories -install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) +install(DIRECTORY include/ $ DESTINATION include/${PROJECT_NAME}) + +ament_export_include_directories("include/${PROJECT_NAME}") ament_export_targets(export_${PROJECT_NAME}) diff --git a/include/point_cloud_transport/point_cloud_common.hpp b/include/point_cloud_transport/point_cloud_common.hpp index 49ed17f..a33b435 100644 --- a/include/point_cloud_transport/point_cloud_common.hpp +++ b/include/point_cloud_transport/point_cloud_common.hpp @@ -35,22 +35,28 @@ #include #include +#include "point_cloud_transport/visibility_control.hpp" + namespace point_cloud_transport { /** * \brief Replacement for uses of boost::erase_last_copy */ +POINT_CLOUD_TRANSPORT_PUBLIC std::string erase_last_copy(const std::string & input, const std::string & search); +POINT_CLOUD_TRANSPORT_PUBLIC std::vector split( const std::string & str, const std::string & delimiter, int maxSplits); // from cras::string_utils +POINT_CLOUD_TRANSPORT_PUBLIC bool endsWith(const std::string & str, const std::string & suffix); // from cras::string_utils +POINT_CLOUD_TRANSPORT_PUBLIC std::string removeSuffix(const std::string & str, const std::string & suffix, bool * hadSuffix); } // namespace point_cloud_transport diff --git a/include/point_cloud_transport/point_cloud_transport.hpp b/include/point_cloud_transport/point_cloud_transport.hpp index f960f75..83dbfba 100644 --- a/include/point_cloud_transport/point_cloud_transport.hpp +++ b/include/point_cloud_transport/point_cloud_transport.hpp @@ -49,6 +49,7 @@ #include #include +#include "point_cloud_transport/visibility_control.hpp" namespace point_cloud_transport { @@ -64,26 +65,35 @@ class PointCloudTransportLoader { public: //! Constructor + POINT_CLOUD_TRANSPORT_PUBLIC PointCloudTransportLoader(); //! Destructor + POINT_CLOUD_TRANSPORT_PUBLIC virtual ~PointCloudTransportLoader(); //! Returns the names of all transports declared in the system. Declared //! transports are not necessarily built or loadable. + POINT_CLOUD_TRANSPORT_PUBLIC std::vector getDeclaredTransports() const; //! Returns the names of all transports that are loadable in the system // (keys are lookup names, values are names). + POINT_CLOUD_TRANSPORT_PUBLIC std::unordered_map getLoadableTransports() const; //! The loader that can load publisher plugins. + POINT_CLOUD_TRANSPORT_PUBLIC PubLoaderPtr getPublisherLoader() const; //! The loader that can load subscriber plugins. + POINT_CLOUD_TRANSPORT_PUBLIC SubLoaderPtr getSubscriberLoader() const; + POINT_CLOUD_TRANSPORT_PUBLIC point_cloud_transport::PubLoaderPtr getPubLoader(); + + POINT_CLOUD_TRANSPORT_PUBLIC point_cloud_transport::SubLoaderPtr getSubLoader(); protected: @@ -94,6 +104,7 @@ class PointCloudTransportLoader /*! * \brief Advertise an image topic, free function version. */ +POINT_CLOUD_TRANSPORT_PUBLIC Publisher create_publisher( std::shared_ptr node, const std::string & base_topic, @@ -103,6 +114,7 @@ Publisher create_publisher( /** * \brief Subscribe to an image topic, free function version. */ +POINT_CLOUD_TRANSPORT_PUBLIC Subscriber create_subscription( std::shared_ptr node, const std::string & base_topic, @@ -124,10 +136,13 @@ class PointCloudTransport : public PointCloudTransportLoader public: //! Constructor + POINT_CLOUD_TRANSPORT_PUBLIC explicit PointCloudTransport(rclcpp::Node::SharedPtr node); + POINT_CLOUD_TRANSPORT_PUBLIC ~PointCloudTransport() override = default; + POINT_CLOUD_TRANSPORT_PUBLIC std::string getTransportOrDefault(const TransportHints * transport_hints) { std::string ret; @@ -141,6 +156,7 @@ class PointCloudTransport : public PointCloudTransportLoader } //! Advertise a PointCloud2 topic, simple version. + POINT_CLOUD_TRANSPORT_PUBLIC Publisher advertise( const std::string & base_topic, rmw_qos_profile_t custom_qos, @@ -157,6 +173,7 @@ class PointCloudTransport : public PointCloudTransportLoader // const ros::VoidPtr& tracked_object = {}, bool latch = false); //! Subscribe to a point cloud topic, version for arbitrary std::function object. + POINT_CLOUD_TRANSPORT_PUBLIC point_cloud_transport::Subscriber subscribe( const std::string & base_topic, uint32_t queue_size, const std::function & callback, @@ -172,6 +189,7 @@ class PointCloudTransport : public PointCloudTransportLoader } //! Subscribe to a point cloud topic, version for bare function. + POINT_CLOUD_TRANSPORT_PUBLIC point_cloud_transport::Subscriber subscribe( const std::string & base_topic, uint32_t queue_size, void (* fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &), diff --git a/include/point_cloud_transport/publisher.hpp b/include/point_cloud_transport/publisher.hpp index a80f6a5..21f2de1 100644 --- a/include/point_cloud_transport/publisher.hpp +++ b/include/point_cloud_transport/publisher.hpp @@ -47,6 +47,8 @@ #include #include +#include "point_cloud_transport/visibility_control.hpp" + namespace point_cloud_transport { @@ -54,8 +56,10 @@ class Publisher { public: //! Constructor + POINT_CLOUD_TRANSPORT_PUBLIC Publisher() = default; + POINT_CLOUD_TRANSPORT_PUBLIC Publisher( std::shared_ptr node, const std::string & base_topic, @@ -64,32 +68,41 @@ class Publisher const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions()); //! get total number of subscribers to all advertised topics. + POINT_CLOUD_TRANSPORT_PUBLIC uint32_t getNumSubscribers() const; //! get base topic of this Publisher + POINT_CLOUD_TRANSPORT_PUBLIC std::string getTopic() const; //! Publish a point cloud on the topics associated with this Publisher. + POINT_CLOUD_TRANSPORT_PUBLIC void publish(const sensor_msgs::msg::PointCloud2 & message) const; //! Publish a point cloud on the topics associated with this Publisher. + POINT_CLOUD_TRANSPORT_PUBLIC void publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message) const; //! Shutdown the advertisements associated with this Publisher. + POINT_CLOUD_TRANSPORT_PUBLIC void shutdown(); + POINT_CLOUD_TRANSPORT_PUBLIC operator void *() const; + POINT_CLOUD_TRANSPORT_PUBLIC bool operator<(const point_cloud_transport::Publisher & rhs) const { return impl_ < rhs.impl_; } + POINT_CLOUD_TRANSPORT_PUBLIC bool operator!=(const point_cloud_transport::Publisher & rhs) const { return impl_ != rhs.impl_; } + POINT_CLOUD_TRANSPORT_PUBLIC bool operator==(const point_cloud_transport::Publisher & rhs) const { return impl_ == rhs.impl_; diff --git a/include/point_cloud_transport/publisher_plugin.hpp b/include/point_cloud_transport/publisher_plugin.hpp index 0d5071f..1200f00 100644 --- a/include/point_cloud_transport/publisher_plugin.hpp +++ b/include/point_cloud_transport/publisher_plugin.hpp @@ -42,12 +42,13 @@ #include #include +#include "point_cloud_transport/visibility_control.hpp" namespace point_cloud_transport { //! Base class for plugins to Publisher. -class PublisherPlugin +class POINT_CLOUD_TRANSPORT_PUBLIC PublisherPlugin { public: PublisherPlugin() = default; @@ -100,10 +101,10 @@ class PublisherPlugin const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions()) = 0; }; -class SingleTopicPublisherPlugin : public PublisherPlugin -{ -public: -}; +// class SingleTopicPublisherPlugin : public PublisherPlugin +// { +// public: +// }; } // namespace point_cloud_transport #endif // POINT_CLOUD_TRANSPORT__PUBLISHER_PLUGIN_HPP_ diff --git a/include/point_cloud_transport/raw_publisher.hpp b/include/point_cloud_transport/raw_publisher.hpp index 4fefc1a..e268068 100644 --- a/include/point_cloud_transport/raw_publisher.hpp +++ b/include/point_cloud_transport/raw_publisher.hpp @@ -40,6 +40,8 @@ #include +#include "point_cloud_transport/visibility_control.hpp" + namespace point_cloud_transport { diff --git a/include/point_cloud_transport/raw_subscriber.hpp b/include/point_cloud_transport/raw_subscriber.hpp index 13e2824..9818881 100644 --- a/include/point_cloud_transport/raw_subscriber.hpp +++ b/include/point_cloud_transport/raw_subscriber.hpp @@ -41,6 +41,7 @@ #include #include +#include "point_cloud_transport/visibility_control.hpp" namespace point_cloud_transport { @@ -70,13 +71,16 @@ class RawSubscriber return this->decodeTyped(compressedPtr); } + POINT_CLOUD_TRANSPORT_PUBLIC std::string getTransportName() const override; protected: + POINT_CLOUD_TRANSPORT_PUBLIC void callback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message, const Callback & user_cb) override; + POINT_CLOUD_TRANSPORT_PUBLIC std::string getTopicToSubscribe(const std::string & base_topic) const override; using SubscriberPlugin::subscribeImpl; diff --git a/include/point_cloud_transport/republish.hpp b/include/point_cloud_transport/republish.hpp index 7f82991..3103fc8 100644 --- a/include/point_cloud_transport/republish.hpp +++ b/include/point_cloud_transport/republish.hpp @@ -36,10 +36,11 @@ #include -#include +#include "point_cloud_transport/visibility_control.hpp" #include -#include "point_cloud_transport/visibility_control.h" + +#include namespace point_cloud_transport { diff --git a/include/point_cloud_transport/simple_publisher_plugin.hpp b/include/point_cloud_transport/simple_publisher_plugin.hpp index 2fb90b4..7232993 100644 --- a/include/point_cloud_transport/simple_publisher_plugin.hpp +++ b/include/point_cloud_transport/simple_publisher_plugin.hpp @@ -47,6 +47,7 @@ #include #include #include +#include "point_cloud_transport/visibility_control.hpp" namespace point_cloud_transport { @@ -129,7 +130,7 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin uint32_t getNumSubscribers() const override { if (simple_impl_) { - return simple_impl_->pub_->get_subscription_count(); + return static_cast(simple_impl_->pub_->get_subscription_count()); } return 0; } @@ -177,6 +178,7 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin * \return The output shapeshifter holding the compressed cloud message * (if encoding succeeds), or an error message. */ + POINT_CLOUD_TRANSPORT_PUBLIC virtual TypedEncodeResult encodeTyped( const sensor_msgs::msg::PointCloud2 & raw) const = 0; diff --git a/include/point_cloud_transport/simple_subscriber_plugin.hpp b/include/point_cloud_transport/simple_subscriber_plugin.hpp index b173050..aa56419 100644 --- a/include/point_cloud_transport/simple_subscriber_plugin.hpp +++ b/include/point_cloud_transport/simple_subscriber_plugin.hpp @@ -104,7 +104,7 @@ class SimpleSubscriberPlugin : public SubscriberPlugin uint32_t getNumPublishers() const override { if (impl_) { - return impl_->sub_->get_publisher_count(); + return static_cast(impl_->sub_->get_publisher_count()); } return 0; } diff --git a/include/point_cloud_transport/single_subscriber_publisher.hpp b/include/point_cloud_transport/single_subscriber_publisher.hpp index 938db47..ae3e2a4 100644 --- a/include/point_cloud_transport/single_subscriber_publisher.hpp +++ b/include/point_cloud_transport/single_subscriber_publisher.hpp @@ -40,6 +40,8 @@ #include "rclcpp/macros.hpp" #include +#include "point_cloud_transport/visibility_control.hpp" + namespace point_cloud_transport { @@ -54,18 +56,25 @@ class SingleSubscriberPublisher SingleSubscriberPublisher(const SingleSubscriberPublisher &) = delete; SingleSubscriberPublisher & operator=(const SingleSubscriberPublisher &) = delete; + POINT_CLOUD_TRANSPORT_PUBLIC SingleSubscriberPublisher( const std::string & caller_id, const std::string & topic, const GetNumSubscribersFn & num_subscribers_fn, const PublishFn & publish_fn); + POINT_CLOUD_TRANSPORT_PUBLIC std::string getSubscriberName() const; + POINT_CLOUD_TRANSPORT_PUBLIC std::string getTopic() const; + POINT_CLOUD_TRANSPORT_PUBLIC uint32_t getNumSubscribers() const; + POINT_CLOUD_TRANSPORT_PUBLIC void publish(const sensor_msgs::msg::PointCloud2 & message) const; + + POINT_CLOUD_TRANSPORT_PUBLIC void publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message) const; private: diff --git a/include/point_cloud_transport/subscriber.hpp b/include/point_cloud_transport/subscriber.hpp index b4f1333..ecf9c73 100644 --- a/include/point_cloud_transport/subscriber.hpp +++ b/include/point_cloud_transport/subscriber.hpp @@ -47,6 +47,8 @@ #include #include +#include "point_cloud_transport/visibility_control.hpp" + namespace point_cloud_transport { @@ -70,8 +72,10 @@ class Subscriber public: typedef std::function Callback; + POINT_CLOUD_TRANSPORT_PUBLIC Subscriber() = default; + POINT_CLOUD_TRANSPORT_PUBLIC Subscriber( std::shared_ptr node, const std::string & base_topic, @@ -87,35 +91,42 @@ class Subscriber * The Subscriber may actually be subscribed to some transport-specific topic that * differs from the base topic. */ + POINT_CLOUD_TRANSPORT_PUBLIC std::string getTopic() const; /** * Returns the number of publishers this subscriber is connected to. */ + POINT_CLOUD_TRANSPORT_PUBLIC uint32_t getNumPublishers() const; /** * Returns the name of the transport being used. */ + POINT_CLOUD_TRANSPORT_PUBLIC std::string getTransport() const; /** * Unsubscribe the callback associated with this Subscriber. */ + POINT_CLOUD_TRANSPORT_PUBLIC void shutdown(); operator void *() const; + POINT_CLOUD_TRANSPORT_PUBLIC bool operator<(const point_cloud_transport::Subscriber & rhs) const { return impl_ < rhs.impl_; } + POINT_CLOUD_TRANSPORT_PUBLIC bool operator!=(const point_cloud_transport::Subscriber & rhs) const { return impl_ != rhs.impl_; } + POINT_CLOUD_TRANSPORT_PUBLIC bool operator==(const point_cloud_transport::Subscriber & rhs) const { return impl_ == rhs.impl_; diff --git a/include/point_cloud_transport/subscriber_filter.hpp b/include/point_cloud_transport/subscriber_filter.hpp index 659a551..77a6718 100644 --- a/include/point_cloud_transport/subscriber_filter.hpp +++ b/include/point_cloud_transport/subscriber_filter.hpp @@ -44,6 +44,8 @@ #include #include +#include "point_cloud_transport/visibility_control.hpp" + namespace point_cloud_transport { @@ -74,6 +76,7 @@ class SubscriberFilter : public message_filters::SimpleFilter node, const std::string & base_topic, const std::string & transport) @@ -84,10 +87,12 @@ class SubscriberFilter : public message_filters::SimpleFilter node, const std::string & base_topic, @@ -121,11 +127,13 @@ class SubscriberFilter : public message_filters::SimpleFilter #include +#include "point_cloud_transport/visibility_control.hpp" + namespace point_cloud_transport { /** * Base class for plugins to Subscriber. */ -class SubscriberPlugin +class POINT_CLOUD_TRANSPORT_PUBLIC SubscriberPlugin { public: //! \brief Result of cloud decoding. Either a `sensor_msgs::msg::PointCloud2` diff --git a/include/point_cloud_transport/transport_hints.hpp b/include/point_cloud_transport/transport_hints.hpp index 9d2a0a3..baf5886 100644 --- a/include/point_cloud_transport/transport_hints.hpp +++ b/include/point_cloud_transport/transport_hints.hpp @@ -39,6 +39,8 @@ #include +#include "point_cloud_transport/visibility_control.hpp" + namespace point_cloud_transport { @@ -59,6 +61,7 @@ class TransportHints * @param parameter_name The name of the transport parameter * */ + POINT_CLOUD_TRANSPORT_PUBLIC TransportHints( const std::shared_ptr node, const std::string & default_transport = "raw", @@ -68,12 +71,14 @@ class TransportHints node->get_parameter_or(parameter_name, transport_, default_transport); } + POINT_CLOUD_TRANSPORT_PUBLIC TransportHints( const std::string & transport = "raw") : transport_(transport) { } + POINT_CLOUD_TRANSPORT_PUBLIC const std::string & getTransport() const { return transport_; diff --git a/include/point_cloud_transport/visibility_control.h b/include/point_cloud_transport/visibility_control.hpp similarity index 91% rename from include/point_cloud_transport/visibility_control.h rename to include/point_cloud_transport/visibility_control.hpp index aa68c84..f3f308d 100644 --- a/include/point_cloud_transport/visibility_control.h +++ b/include/point_cloud_transport/visibility_control.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef POINT_CLOUD_TRANSPORT__VISIBILITY_CONTROL_H_ -#define POINT_CLOUD_TRANSPORT__VISIBILITY_CONTROL_H_ +#ifndef POINT_CLOUD_TRANSPORT__VISIBILITY_CONTROL_HPP_ +#define POINT_CLOUD_TRANSPORT__VISIBILITY_CONTROL_HPP_ #ifdef __cplusplus extern "C" @@ -55,4 +55,4 @@ extern "C" } #endif -#endif // POINT_CLOUD_TRANSPORT__VISIBILITY_CONTROL_H_ +#endif // POINT_CLOUD_TRANSPORT__VISIBILITY_CONTROL_HPP_ From ce50faf446d2e376748070ae5a440911fda4df4a Mon Sep 17 00:00:00 2001 From: john-maidbot <78750993+john-maidbot@users.noreply.github.com> Date: Tue, 25 Jul 2023 16:24:06 -0500 Subject: [PATCH 22/28] Fix pointcloud-codec and python bindings (#3) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Update topic / transport name introspection * Fix some build errors * Replacing shapeshifter with rclcpp::serializedmessage * remove c_api * consolidate bindable code into codec * Update tests * Get pybind working * remove stale comments * Fix python nodes * doc update * Add pybind11_vendor * Update include/point_cloud_transport/point_cloud_codec.hpp * Add license to pybind_codec * param docs + standardize \param * remove references to ros wiki * make flake8 happy * cpplint --------- Signed-off-by: Alejandro Hernández Cordero Co-authored-by: Alejandro Hernández Cordero --- CMakeLists.txt | 43 +- PORT_TODO.md | 19 - README.md | 43 +- doc/index.rst | 2 +- include/point_cloud_transport/c_api.h | 112 ----- .../point_cloud_codec.hpp | 204 ++++++--- .../point_cloud_common.hpp | 12 +- .../point_cloud_transport.hpp | 21 +- .../publisher_plugin.hpp | 36 +- .../point_cloud_transport/raw_publisher.hpp | 8 +- .../point_cloud_transport/raw_subscriber.hpp | 14 +- .../simple_publisher_plugin.hpp | 30 +- .../simple_subscriber_plugin.hpp | 20 +- .../subscriber_plugin.hpp | 35 +- .../point_cloud_transport/transport_hints.hpp | 6 +- package.xml | 5 +- point_cloud_transport/__init__.py | 0 .../common.py | 43 +- point_cloud_transport/publisher.py | 116 ++++++ point_cloud_transport/subscriber.py | 105 +++++ src/c_api.cpp | 70 ---- src/point_cloud_codec.cpp | 392 +++++++----------- src/point_cloud_common.cpp | 26 +- src/point_cloud_transport.cpp | 86 ---- src/point_cloud_transport/__init__.py | 131 ------ src/point_cloud_transport/decoder.py | 150 ------- src/point_cloud_transport/encoder.py | 118 ------ src/point_cloud_transport/publisher.py | 130 ------ src/point_cloud_transport/subscriber.py | 141 ------- src/publisher_plugin.cpp | 15 + src/pybind_codec.cpp | 156 +++++++ src/raw_subscriber.cpp | 21 +- src/subscriber.cpp | 3 +- test/test_point_cloud_codec.cpp | 212 ++++++++++ 34 files changed, 1163 insertions(+), 1362 deletions(-) delete mode 100644 PORT_TODO.md delete mode 100644 include/point_cloud_transport/c_api.h create mode 100755 point_cloud_transport/__init__.py rename {src/point_cloud_transport => point_cloud_transport}/common.py (64%) mode change 100644 => 100755 create mode 100755 point_cloud_transport/publisher.py create mode 100755 point_cloud_transport/subscriber.py delete mode 100644 src/c_api.cpp delete mode 100644 src/point_cloud_transport/__init__.py delete mode 100644 src/point_cloud_transport/decoder.py delete mode 100644 src/point_cloud_transport/encoder.py delete mode 100644 src/point_cloud_transport/publisher.py delete mode 100644 src/point_cloud_transport/subscriber.py create mode 100644 src/pybind_codec.cpp create mode 100644 test/test_point_cloud_codec.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 94354e9..08569fb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,11 +13,17 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake_ros REQUIRED) +find_package(ament_cmake_python REQUIRED) find_package(message_filters REQUIRED) find_package(pluginlib REQUIRED) +find_package(pybind11 REQUIRED) +find_package(pybind11_vendor REQUIRED) +find_package(python_cmake_module REQUIRED) +find_package(Python3 REQUIRED COMPONENTS Interpreter Development) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) +find_package(rclpy REQUIRED) find_package(sensor_msgs REQUIRED) include_directories( @@ -35,7 +41,7 @@ fetchcontent_makeavailable(expected) # Build libpoint_cloud_transport add_library(${PROJECT_NAME} - # src/point_cloud_codec.cpp # TODO: This is needed for exposing the encode/decode functions without spinning an node + src/point_cloud_codec.cpp src/point_cloud_common.cpp src/point_cloud_transport.cpp src/publisher.cpp @@ -105,6 +111,33 @@ target_link_libraries(list_transports ${PROJECT_NAME} pluginlib::pluginlib) + +# Install Python modules +ament_python_install_package(${PROJECT_NAME}) + +# Python bindings +pybind11_add_module(_codec SHARED + src/pybind_codec.cpp +) +target_link_libraries(_codec PRIVATE + ${PROJECT_NAME} + pluginlib::pluginlib +) + +# Install cython modules as sub-modules of the project +install( + TARGETS + _codec + DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}" +) + +# Install Python executables +install(PROGRAMS + point_cloud_transport/publisher.py + point_cloud_transport/subscriber.py + DESTINATION lib/${PROJECT_NAME} +) + # Install plugin descriptions pluginlib_export_plugin_description_file(${PROJECT_NAME} default_plugins.xml) @@ -152,6 +185,14 @@ if(BUILD_TESTING) ) endif() + ament_add_gtest(${PROJECT_NAME}-codec test/test_point_cloud_codec.cpp) + if(TARGET ${PROJECT_NAME}-codec) + target_link_libraries(${PROJECT_NAME}-codec ${PROJECT_NAME}) + ament_target_dependencies(${PROJECT_NAME}-codec + pluginlib + ) + endif() + ament_add_gtest(${PROJECT_NAME}-subscriber test/test_subscriber.cpp) if(TARGET ${PROJECT_NAME}-subscriber) target_link_libraries(${PROJECT_NAME}-subscriber ${PROJECT_NAME}) diff --git a/PORT_TODO.md b/PORT_TODO.md deleted file mode 100644 index 695c6e6..0000000 --- a/PORT_TODO.md +++ /dev/null @@ -1,19 +0,0 @@ -## TODO -- [] start roscpp --> rclcpp conversion of transport ---> Put CRAS into this repo temporarily to get it to build ---> Do what it takes to get the rclcpp code to build without the point_cloud_codec files ---> Figure out how to update ros serialization and shape shifter usage ---> Fix the point_cloud_codec files -- [] c++ codebase builds with cmake (minus tests) ---> Create a node to test with simulated pointclouds ---> bag the pointcloud data ---> Test point_cloud_codec files with bagged point cloud data -- [] c++ codebase runs with test pointcloudsc -- [] rviz integration -- [] tests build and pass ---> Fix c++ tests -- [] rospy --> rclpy conversion ---> Update python rclpy files ---> Update python bindings -- [] python codebase runs with test pointclouds - diff --git a/README.md b/README.md index 384bda2..3acf206 100644 --- a/README.md +++ b/README.md @@ -30,20 +30,57 @@ point_cloud_transport::Subscriber sub = pct.subscribe("in_point_cloud_base_topic point_cloud_transport::Publisher pub = pct.advertise("out_point_cloud_base_topic", 1); ``` +Alternatively, you can use point_cloud_transport outside of ROS2. + +```cpp +#include + +#include + +point_cloud_transport::PointCloudCodec codec; + +sensor_msgs::msg::PointCloud2 msg; +// ... do some cool pointcloud generation stuff ... + +// untyped version (outputs an rclcpp::SerializedMessage) +rclcpp::SerializedMessage serialized_msg; +bool success = codec.encode("draco", msg, serialized_msg); + +// OR + +// typed version (outputs whatever message your selected transport returns, +// for draco that is a point_cloud_interfaces::msg::CompressedPointCloud2) +point_cloud_interfaces::msg::CompressedPointCloud2 compressed_msg; +bool success = codec.encode("draco", msg, compressed_msg); +``` + ### Republish rclcpp component -TODO +We provide a process to republish any topic speaking in a given transport to a different topic speaking a different transport. +e.g. have it subscribe to a topic you recorded encoded using draco and publish it as a raw, decoded message + +```bash +ros2 run point_cloud_transport republish --in_transport draco --out_transport raw --ros-args --remap in:=input_topic_name --remap out:=ouput_topic_name +``` + +### Python +The functionality of `point_cloud_transport` is also exposed to python via `pybind11` and `rclpy` serialization. + +Please see [point_cloud_transport/publisher.py](point_cloud_transport/publisher.py) and [point_cloud_transport/subscriber.py](point_cloud_transport/subscriber.py) for example usage. ### Blacklist point cloud transport +This allows you to specify plugins you do not want to load (a.k.a. blacklist them). + ```bash ros2 run point_cloud_transport_tutorial my_publisher --ros-args -p /pct/point_cloud/disable_pub_plugins:=["point_cloud_transport/raw"] ``` ## Known transports -- [draco_point_cloud_transport](https://wiki.ros.org/draco_point_cloud_transport): Lossy compression via Google Draco library. +- [draco_point_cloud_transport](https://github.com/john-maidbot/point_cloud_transport_plugins/tree/ros2/draco_point_cloud_transport): Lossy compression via Google +- [zlib_point_cloud_transport](https://github.com/john-maidbot/point_cloud_transport_plugins/tree/ros2/zlib_point_cloud_transport): Lossless compression via Zlib compression. - Did you write one? Don't hesitate and send a pull request adding it to this list! ## Authors @@ -65,7 +102,7 @@ This project is licensed under the BSD License - see the [LICENSE](https://githu ## Acknowledgments -* [image_transport](http://wiki.ros.org/image_transport) - Provided template of plugin interface +* [image_transport](https://github.com/ros-perception/image_common) - Provided template of plugin interface * [Draco](https://github.com/google/draco) - Provided compression functionality Support diff --git a/doc/index.rst b/doc/index.rst index 60ed97d..00dcb9c 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -16,7 +16,7 @@ Python API ROS API ------- -See |wiki| for the ROS API description. +See |ros2_documentation| for the ROS API description. C and C++ API ------------- diff --git a/include/point_cloud_transport/c_api.h b/include/point_cloud_transport/c_api.h deleted file mode 100644 index 2652b7a..0000000 --- a/include/point_cloud_transport/c_api.h +++ /dev/null @@ -1,112 +0,0 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * 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 HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ -// #pragma once - -#ifndef POINT_CLOUD_TRANSPORT__C_API_H_ -#define POINT_CLOUD_TRANSPORT__C_API_H_ - -// /** -// * \file -// * \brief Support definitions for declaration of a C API of modules. -// * \author Martin Pecka -// */ - -// #include -// #include - -// namespace cras -// { - -// /** -// * \brief Allocator function that should allocate a buffer of the given size on the caller -// side and return a pointer -// * to it. -// * -// * This is used throughout the C API to ease returning strings and byte buffers of dynamic -// size to the caller. Instead -// * of taking a writable `char*` argument or returning a `new`-allocated `const char*`, -// the function takes the allocator -// * as the argument. When it is about to return to the caller, it uses the allocator to get a -// caller-side-managed buffer -// * of the correct size and writes the string/byte buffer to this buffer. -// * -// * cras_py_common provides the `BytesAllocator` and `StringAllocator` classes that can be -// passed as this allocator -// * argument. Once the ctypes function call finishes, the caller can access `allocator.value` -// to get the string or -// * buffer returned by the function. -// */ -// typedef void* (*allocator_t)(size_t); - -// /** -// * \brief Allocate enough bytes using the given allocator and copy the given string into -// the buffer (including null -// * termination byte). -// * \param[in] allocator The allocator to use. -// * \param[in] string The zero-terminated string to copy into the allocated buffer. -// * \param[in] length Length of the string to copy including the null termination character. -// * \return Pointer to the allocated buffer. -// */ -// char* outputString(allocator_t allocator, const char* string, size_t length); - -// /** -// * \brief Allocate enough bytes using the given allocator and copy the given string into -// the buffer (including null -// * termination byte). -// * \param[in] allocator The allocator to use. -// * \param[in] string The string to copy into the allocated buffer. -// * \return Pointer to the allocated buffer. -// */ -// char* outputString(allocator_t allocator, const std::string& string); - -// /** -// * \brief Allocate enough bytes using the given allocator and copy the given bytes into -// the buffer. -// * \param[in] allocator The allocator to use. -// * \param[in] bytes The bytes to copy into the allocated buffer. -// * \param[in] length Length of `bytes`. -// * \return Pointer to the allocated buffer. -// */ -// uint8_t* outputByteBuffer(allocator_t allocator, const uint8_t* bytes, size_t length); - -// /** -// * \brief Allocate enough bytes using the given allocator and copy the given bytes into -// the buffer. -// * \param[in] allocator The allocator to use. -// * \param[in] bytes The bytes to copy into the allocated buffer. -// * \return Pointer to the allocated buffer. -// */ -// uint8_t* outputByteBuffer(allocator_t allocator, const std::vector& bytes); - - -// } - -#endif // POINT_CLOUD_TRANSPORT__C_API_H_ diff --git a/include/point_cloud_transport/point_cloud_codec.hpp b/include/point_cloud_transport/point_cloud_codec.hpp index f92a654..32c9d55 100644 --- a/include/point_cloud_transport/point_cloud_codec.hpp +++ b/include/point_cloud_transport/point_cloud_codec.hpp @@ -34,14 +34,16 @@ #ifndef POINT_CLOUD_TRANSPORT__POINT_CLOUD_CODEC_HPP_ #define POINT_CLOUD_TRANSPORT__POINT_CLOUD_CODEC_HPP_ -#include - #include #include #include +#include +#include + #include +#include #include #include @@ -49,11 +51,8 @@ namespace point_cloud_transport { /** -* Advertise and subscribe to PointCloud2 topics. -* -* PointCloudTransport is analogous to ros::NodeHandle in that it contains advertise() and -* subscribe() functions for creating advertisements and subscriptions of PointCloud2 topics. -*/ + * Class to expose all the functionality of pointcloud transport (encode/decode msgs) without needing to spin a node. + */ class PointCloudCodec { @@ -61,73 +60,140 @@ class PointCloudCodec //! Constructor PointCloudCodec(); - std::shared_ptr getEncoderByName(const std::string & name) - const; - - std::shared_ptr getEncoderByTopic( - const std::string & topic, const std::string & datatype) const; - + //! Destructor + virtual ~PointCloudCodec(); + + /** + * Get a shared pointer to an instance of a publisher plugin given its transport name (publishers encode messages). + * e.g. if you want the raw encoder, call getEncoderByName("raw"). + * + * \param name The name of the transport to load. + * \returns A shared pointer to the publisher plugin. + */ + std::shared_ptr getEncoderByName( + const std::string & name); + + /** + * Get a shared pointer to an instance of a publisher plugin given its transport name (subscribers decode messages). + * e.g. if you want the raw decoder, call getDecoderByName("raw"). + * + * \param name The name of the transport to load. + * \returns A shared pointer to the subscriber plugin. + */ std::shared_ptr getDecoderByName( - const std::string & name) const; - - std::shared_ptr getDecoderByTopic( - const std::string & topic, const std::string & datatype) const; + const std::string & name); + + /** + * Get a list of all the transports that can be loaded. + * + * \param[out] transports Vector of the loadable transport plugins. + * \param[out] names Vector of string identifieries for the transport provided by each plugin + */ + void getLoadableTransports( + std::vector & transports, + std::vector & names); + + /** + * Get a list of all the transport plugins, topics, transport names, and their data types that can be loaded. + * + * \param[in] baseTopic The base topic to use for the transport. + * \param[out] transports Vector of the loadable transport plugins. + * \param[out] topics Vector of the topics that can be published. + * \param[out] names Vector of string identifieries for the transport provided by each plugin + * \param[out] dataTypes Vector of the data types the transports encode a PointCloud2 into + */ + void getTopicsToPublish( + const std::string & baseTopic, + std::vector & transports, + std::vector & topics, + std::vector & names, + std::vector & dataTypes); + + /** + * Get the topic, transport name, and data type that a given topic is published on for a particular transport plugin. + * + * \param[in] baseTopic The base topic to use for the transport. + * \param[in] transport The transport plugin to load. + * \param[out] topic The topic that should be subscribed to. + * \param[out] name String identifier for the transport provided by the plugin + * \param[out] dataType The data type the transport encodes a PointCloud2 into + */ + void getTopicToSubscribe( + const std::string & baseTopic, + const std::string & transport, + std::string & topic, + std::string & name, + std::string & dataType); + + /** + * Encode a PointCloud2 message into a serialized message + * using the specified transport plugin. The underlying type + * of the serialized message is determined by the transport plugin, + * but doesnt need to be known by this function. + * + * \param[in] transport_name The name of the transport plugin to use. + * \param[in] msg The message to encode. + * \param[out] serialized_msg The serialized message to store the encoded message in. + * \returns True if the message was successfully encoded, false otherwise. + */ + bool encode( + const std::string & transport_name, + const sensor_msgs::msg::PointCloud2 & msg, + rclcpp::SerializedMessage & serialized_msg); + + /** + * Encode a PointCloud2 message into some compressed message type + * using the specified transport plugin. The compressed message type + * is determined by the transport plugin. + * + * \param[in] transport_name The name of the transport plugin to use. + * \param[in] msg The message to encode. + * \param[out] compressed_msg The compressed message to store the encoded message in. + * \returns True if the message was successfully encoded, false otherwise. + */ + template + bool encodeTyped( + const std::string & transport_name, + const sensor_msgs::msg::PointCloud2 & msg, + M & compressed_msg); + + /** + * Dencode a serialized message into a PointCloud2 + * using the specified transport plugin. The underlying type + * of the serialized message is determined by the transport plugin, + * but doesnt need to be known by this function. + * + * \param[in] transport_name The name of the transport plugin to use. + * \param[in] serialized_msg The serialized message to decode. + * \param[out] msg The message to store the decoded output in. + * \returns True if the message was successfully decoded, false otherwise. + */ + bool decode( + const std::string & transport_name, + const rclcpp::SerializedMessage & serialized_msg, + sensor_msgs::msg::PointCloud2 & msg); + + /** + * Decode some compressed message type + * into a PointCloud2 based on the specified transport plugin. The compressed message type + * is determined by the transport plugin. + * + * \param[in] transport_name The name of the transport plugin to use. + * \param[in] compressed_msg The compressed message to decode. + * \param[out] msg The message to store the decoded output in. + * \returns True if the message was successfully decoded, false otherwise. + */ + template + bool decodeTyped( + const std::string & transport_name, + const M & compressed_msg, + sensor_msgs::msg::PointCloud2 & msg); private: - struct Impl; - typedef std::shared_ptr ImplPtr; - typedef std::weak_ptr ImplWPtr; - - ImplPtr impl_; + point_cloud_transport::PubLoaderPtr enc_loader_; + point_cloud_transport::SubLoaderPtr dec_loader_; }; } // namespace point_cloud_transport -extern "C" bool pointCloudTransportCodecsEncode( - const char * codec, - sensor_msgs::msg::PointCloud2::_height_type rawHeight, - sensor_msgs::msg::PointCloud2::_width_type rawWidth, - size_t rawNumFields, - const char * rawFieldNames[], - sensor_msgs::PointField::_offset_type rawFieldOffsets[], - sensor_msgs::PointField::_datatype_type rawFieldDatatypes[], - sensor_msgs::PointField::_count_type rawFieldCounts[], - sensor_msgs::msg::PointCloud2::_is_bigendian_type rawIsBigendian, - sensor_msgs::msg::PointCloud2::_point_step_type rawPointStep, - sensor_msgs::msg::PointCloud2::_row_step_type rawRowStep, - size_t rawDataLength, - const uint8_t rawData[], - sensor_msgs::msg::PointCloud2::_is_dense_type rawIsDense, - cras::allocator_t compressedTypeAllocator, - cras::allocator_t compressedMd5SumAllocator, - cras::allocator_t compressedDataAllocator, - size_t serializedConfigLength, - const uint8_t serializedConfig[], - cras::allocator_t errorStringAllocator, - cras::allocator_t logMessagesAllocator -); - -extern "C" bool pointCloudTransportCodecsDecode( - const char * topicOrCodec, - const char * compressedType, - const char * compressedMd5sum, - size_t compressedDataLength, - const uint8_t compressedData[], - sensor_msgs::msg::PointCloud2::_height_type & rawHeight, - sensor_msgs::msg::PointCloud2::_width_type & rawWidth, - uint32_t & rawNumFields, - cras::allocator_t rawFieldNamesAllocator, - cras::allocator_t rawFieldOffsetsAllocator, - cras::allocator_t rawFieldDatatypesAllocator, - cras::allocator_t rawFieldCountsAllocator, - sensor_msgs::msg::PointCloud2::_is_bigendian_type & rawIsBigEndian, - sensor_msgs::msg::PointCloud2::_point_step_type & rawPointStep, - sensor_msgs::msg::PointCloud2::_row_step_type & rawRowStep, - cras::allocator_t rawDataAllocator, - sensor_msgs::msg::PointCloud2::_is_dense_type & rawIsDense, - size_t serializedConfigLength, - const uint8_t serializedConfig[], - cras::allocator_t errorStringAllocator, - cras::allocator_t logMessagesAllocator -); #endif // POINT_CLOUD_TRANSPORT__POINT_CLOUD_CODEC_HPP_ diff --git a/include/point_cloud_transport/point_cloud_common.hpp b/include/point_cloud_transport/point_cloud_common.hpp index a33b435..9f54d16 100644 --- a/include/point_cloud_transport/point_cloud_common.hpp +++ b/include/point_cloud_transport/point_cloud_common.hpp @@ -49,7 +49,7 @@ std::string erase_last_copy(const std::string & input, const std::string & searc POINT_CLOUD_TRANSPORT_PUBLIC std::vector split( const std::string & str, const std::string & delimiter, - int maxSplits); + int maxSplits = -1); // from cras::string_utils POINT_CLOUD_TRANSPORT_PUBLIC @@ -57,7 +57,15 @@ bool endsWith(const std::string & str, const std::string & suffix); // from cras::string_utils POINT_CLOUD_TRANSPORT_PUBLIC -std::string removeSuffix(const std::string & str, const std::string & suffix, bool * hadSuffix); +std::string removeSuffix( + const std::string & str, const std::string & suffix, + bool * hadSuffix = nullptr); + +POINT_CLOUD_TRANSPORT_PUBLIC +bool transportNameMatches( + const std::string & lookup_name, + const std::string & name, const std::string & suffix); + } // namespace point_cloud_transport #endif // POINT_CLOUD_TRANSPORT__POINT_CLOUD_COMMON_HPP_ diff --git a/include/point_cloud_transport/point_cloud_transport.hpp b/include/point_cloud_transport/point_cloud_transport.hpp index 83dbfba..561f161 100644 --- a/include/point_cloud_transport/point_cloud_transport.hpp +++ b/include/point_cloud_transport/point_cloud_transport.hpp @@ -126,8 +126,8 @@ Subscriber create_subscription( /** * Advertise and subscribe to PointCloud2 topics. * -* PointCloudTransport is analogous to ros::NodeHandle in that it contains advertise() and -* subscribe() functions for creating advertisements and subscriptions of PointCloud2 topics. +* PointCloudTransport is analogous to rclcpp::Node in that it contains create_publisher() and +* create_subscription() functions for creating publishers and subscriptions of PointCloud2 topics. */ class PointCloudTransport : public PointCloudTransportLoader @@ -235,21 +235,4 @@ class PointCloudTransport : public PointCloudTransportLoader } // namespace point_cloud_transport -// TODO(anyone): May need these? -// extern "C" void pointCloudTransportGetLoadableTransports( -// cras::allocator_t transportAllocator, cras::allocator_t nameAllocator); - -// extern "C" void pointCloudTransportGetTopicsToPublish( -// const char* baseTopic, -// cras::allocator_t transportAllocator, -// cras::allocator_t nameAllocator, -// cras::allocator_t topicAllocator, -// cras::allocator_t dataTypeAllocator, cras::allocator_t configTypeAllocator); - -// extern "C" void pointCloudTransportGetTopicToSubscribe( -// const char* baseTopic, const char* transport, cras::allocator_t nameAllocator, -// cras::allocator_t topicAllocator, -// cras::allocator_t dataTypeAllocator, cras::allocator_t configTypeAllocator); - - #endif // POINT_CLOUD_TRANSPORT__POINT_CLOUD_TRANSPORT_HPP_ diff --git a/include/point_cloud_transport/publisher_plugin.hpp b/include/point_cloud_transport/publisher_plugin.hpp index 1200f00..993920f 100644 --- a/include/point_cloud_transport/publisher_plugin.hpp +++ b/include/point_cloud_transport/publisher_plugin.hpp @@ -41,6 +41,7 @@ #include "rclcpp/node.hpp" #include +#include #include #include "point_cloud_transport/visibility_control.hpp" @@ -51,6 +52,11 @@ namespace point_cloud_transport class POINT_CLOUD_TRANSPORT_PUBLIC PublisherPlugin { public: + //! \brief Result of cloud encoding. Either an holding the compressed cloud, + // empty value or error message. + typedef cras::expected>, + std::string> EncodeResult; + PublisherPlugin() = default; PublisherPlugin(const PublisherPlugin &) = delete; PublisherPlugin & operator=(const PublisherPlugin &) = delete; @@ -58,17 +64,12 @@ class POINT_CLOUD_TRANSPORT_PUBLIC PublisherPlugin //! Get a string identifier for the transport provided by this plugin virtual std::string getTransportName() const = 0; - /** - * \brief Advertise a topic, simple version. - */ + //! \brief Advertise a topic, simple version. void advertise( std::shared_ptr nh, const std::string & base_topic, rmw_qos_profile_t custom_qos = rmw_qos_profile_default, - const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions()) - { - advertiseImpl(nh, base_topic, custom_qos, options); - } + const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions()); //! Returns the number of subscribers that are currently connected to this PublisherPlugin virtual uint32_t getNumSubscribers() const = 0; @@ -76,6 +77,16 @@ class POINT_CLOUD_TRANSPORT_PUBLIC PublisherPlugin //! Returns the topic that this PublisherPlugin will publish on. virtual std::string getTopic() const = 0; + //! Return the datatype of the transported messages (as text in the form `package/Message`). + virtual std::string getDataType() const = 0; + + /** + * \brief Encode the given raw pointcloud into EncodeResult + * \param[in] raw The input raw pointcloud. + * \return The output EncodeResult holding the compressed cloud message (if encoding succeeds), or an error message. + */ + virtual EncodeResult encode(const sensor_msgs::msg::PointCloud2 & raw) const; + //! Publish a point cloud using the transport associated with this PublisherPlugin. virtual void publish(const sensor_msgs::msg::PointCloud2 & message) const = 0; @@ -88,23 +99,18 @@ class POINT_CLOUD_TRANSPORT_PUBLIC PublisherPlugin //! Declare parameter with this PublisherPlugin. virtual void declareParameters(const std::string & base_topic) = 0; + virtual std::string getTopicToAdvertise(const std::string & base_topic) const = 0; + //! Return the lookup name of the PublisherPlugin associated with a specific transport identifier. static std::string getLookupName(const std::string & transport_name); protected: - /** - * \brief Advertise a topic. Must be implemented by the subclass. - */ + //! Advertise a topic. Must be implemented by the subclass. virtual void advertiseImpl( std::shared_ptr nh, const std::string & base_topic, rmw_qos_profile_t custom_qos, const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions()) = 0; }; -// class SingleTopicPublisherPlugin : public PublisherPlugin -// { -// public: -// }; - } // namespace point_cloud_transport #endif // POINT_CLOUD_TRANSPORT__PUBLISHER_PLUGIN_HPP_ diff --git a/include/point_cloud_transport/raw_publisher.hpp b/include/point_cloud_transport/raw_publisher.hpp index e268068..61772d3 100644 --- a/include/point_cloud_transport/raw_publisher.hpp +++ b/include/point_cloud_transport/raw_publisher.hpp @@ -58,8 +58,12 @@ class RawPublisher return "raw"; } - RawPublisher::TypedEncodeResult encodeTyped( - const sensor_msgs::msg::PointCloud2 & raw) const + std::string getDataType() const override + { + return "sensor_msgs/msg/PointCloud2"; + } + + RawPublisher::TypedEncodeResult encodeTyped(const sensor_msgs::msg::PointCloud2 & raw) const { return raw; } diff --git a/include/point_cloud_transport/raw_subscriber.hpp b/include/point_cloud_transport/raw_subscriber.hpp index 9818881..a990d4f 100644 --- a/include/point_cloud_transport/raw_subscriber.hpp +++ b/include/point_cloud_transport/raw_subscriber.hpp @@ -58,20 +58,14 @@ class RawSubscriber public: virtual ~RawSubscriber() {} + SubscriberPlugin::DecodeResult decodeTyped( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & compressed) const; SubscriberPlugin::DecodeResult decodeTyped( - const sensor_msgs::msg::PointCloud2::ConstSharedPtr & compressed) const - { - return compressed; - } + const sensor_msgs::msg::PointCloud2 & compressed) const; - SubscriberPlugin::DecodeResult decodeTyped(const sensor_msgs::msg::PointCloud2 & compressed) const - { - auto compressedPtr = std::make_shared(compressed); - return this->decodeTyped(compressedPtr); - } + std::string getDataType() const override; - POINT_CLOUD_TRANSPORT_PUBLIC std::string getTransportName() const override; protected: diff --git a/include/point_cloud_transport/simple_publisher_plugin.hpp b/include/point_cloud_transport/simple_publisher_plugin.hpp index 7232993..db2da32 100644 --- a/include/point_cloud_transport/simple_publisher_plugin.hpp +++ b/include/point_cloud_transport/simple_publisher_plugin.hpp @@ -42,9 +42,11 @@ #include #include +#include "rclcpp/serialization.hpp" #include #include +#include #include #include #include "point_cloud_transport/visibility_control.hpp" @@ -71,7 +73,6 @@ namespace point_cloud_transport * It defaults to \/\. * * \tparam M Type of the published messages. - * \tparam Config Type of the publisher dynamic configuration. */ template class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin @@ -143,7 +144,6 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin return {}; } - // TODO(anyone): Ask about this void publish(const sensor_msgs::msg::PointCloud2 & message) const override { if (!simple_impl_ || !simple_impl_->pub_) { @@ -175,13 +175,31 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin /** * \brief Encode the given raw pointcloud into a compressed message. * \param[in] raw The input raw pointcloud. - * \return The output shapeshifter holding the compressed cloud message + * \return The output rmw serialized msg holding the compressed cloud message * (if encoding succeeds), or an error message. */ POINT_CLOUD_TRANSPORT_PUBLIC virtual TypedEncodeResult encodeTyped( const sensor_msgs::msg::PointCloud2 & raw) const = 0; + EncodeResult encode(const sensor_msgs::msg::PointCloud2 & raw) const override + { + // encode the message using the expected transport method + auto res = this->encodeTyped(raw); + if (!res) { + return cras::make_unexpected(res.error()); + } + if (!res.value()) { + return std::nullopt; + } + + // publish the message (of some unknown type) as a serialized message + auto serialized_msg_ptr = std::make_shared(); + static rclcpp::Serialization serializer; + serializer.serialize_message(&(res.value().value()), serialized_msg_ptr.get()); + return serialized_msg_ptr; + } + protected: std::string base_topic_; @@ -210,7 +228,7 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin * the subclass. * * The PublishFn publishes the transport-specific message type. This indirection allows - * SimpleSubscriberPlugin to use this function for both normal broadcast publishing and + * SimplePublisherPlugin to use this function for both normal broadcast publishing and * single subscriber publishing (in subscription callbacks). */ virtual void publish( @@ -233,7 +251,7 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin * * Defaults to \/\. */ - virtual std::string getTopicToAdvertise(const std::string & base_topic) const + std::string getTopicToAdvertise(const std::string & base_topic) const override { return base_topic + "/" + getTransportName(); } @@ -261,7 +279,7 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin * Returns a function object for publishing the transport-specific message type * through some ROS publisher type. * - * @param pub An object with method void publish(const M&) + * \param pub An object with method void publish(const M&) */ template PublishFn bindInternalPublisher(PubT * pub) const diff --git a/include/point_cloud_transport/simple_subscriber_plugin.hpp b/include/point_cloud_transport/simple_subscriber_plugin.hpp index aa56419..9778483 100644 --- a/include/point_cloud_transport/simple_subscriber_plugin.hpp +++ b/include/point_cloud_transport/simple_subscriber_plugin.hpp @@ -40,8 +40,10 @@ #include #include +#include "rclcpp/serialization.hpp" #include "rclcpp/subscription.hpp" +#include #include namespace point_cloud_transport @@ -121,11 +123,25 @@ class SimpleSubscriberPlugin : public SubscriberPlugin /** * \brief Decode the given compressed pointcloud into a raw message. * \param[in] compressed The input compressed pointcloud. - * \param[in] config Config of the decompression (if it has any parameters). * \return The raw cloud message (if encoding succeeds), or an error message. */ virtual DecodeResult decodeTyped(const M & compressed) const = 0; + DecodeResult decode(const std::shared_ptr & compressed) const override + { + auto msg = std::make_shared(); + try { + auto serializer = rclcpp::Serialization(); + serializer.deserialize_message(compressed.get(), msg.get()); + } catch (const std::exception & e) { + return cras::make_unexpected( + "Error deserializing message for transport decoder: " + std::string( + e.what()) + "."); + } + + return this->decodeTyped(*msg); + } + protected: /** * Process a message. Must be implemented by the subclass. @@ -148,7 +164,7 @@ class SimpleSubscriberPlugin : public SubscriberPlugin * * Defaults to \/\. */ - virtual std::string getTopicToSubscribe(const std::string & base_topic) const + std::string getTopicToSubscribe(const std::string & base_topic) const override { return base_topic + "/" + getTransportName(); } diff --git a/include/point_cloud_transport/subscriber_plugin.hpp b/include/point_cloud_transport/subscriber_plugin.hpp index 36d997f..41ec0d3 100644 --- a/include/point_cloud_transport/subscriber_plugin.hpp +++ b/include/point_cloud_transport/subscriber_plugin.hpp @@ -76,6 +76,13 @@ class POINT_CLOUD_TRANSPORT_PUBLIC SubscriberPlugin */ virtual std::string getTransportName() const = 0; + /** + * \brief Decode the given compressed pointcloud into a raw cloud. + * \param[in] compressed The rclcpp::SerializedMessage of the compressed pointcloud to be decoded. + * \return The decoded raw pointcloud (if decoding succeeds), or an error message. + */ + virtual DecodeResult decode(const std::shared_ptr & compressed) const = + 0; /** * \brief Subscribe to an pointcloud topic, version for arbitrary std::function object. @@ -134,11 +141,21 @@ class POINT_CLOUD_TRANSPORT_PUBLIC SubscriberPlugin */ virtual void shutdown() = 0; + /** + * Return the datatype of the transported messages (as text in the form `package/Message`). + */ + virtual std::string getDataType() const = 0; + /** * Declare parameter with this SubscriberPlugin. */ virtual void declareParameters() = 0; + /** + * Get the name of the topic that this SubscriberPlugin will subscribe to. + */ + virtual std::string getTopicToSubscribe(const std::string & base_topic) const = 0; + /** * Return the lookup name of the SubscriberPlugin associated with a specific * transport identifier. @@ -173,23 +190,5 @@ class POINT_CLOUD_TRANSPORT_PUBLIC SubscriberPlugin } }; -class SingleTopicSubscriberPlugin : public SubscriberPlugin -{ -public: - /** - * Return the communication topic name for a given base topic. - * - * Defaults to \/\. - */ - virtual std::string getTopicToSubscribe(const std::string & base_topic) const = 0; - - /** - * Return the datatype of the dynamic reconfigure (as text in the form `package/Config`). - * - * Return empty string if no reconfiguration is supported. - */ - virtual std::string getConfigDataType() const = 0; -}; - } // namespace point_cloud_transport #endif // POINT_CLOUD_TRANSPORT__SUBSCRIBER_PLUGIN_HPP_ diff --git a/include/point_cloud_transport/transport_hints.hpp b/include/point_cloud_transport/transport_hints.hpp index baf5886..f3dc8a4 100644 --- a/include/point_cloud_transport/transport_hints.hpp +++ b/include/point_cloud_transport/transport_hints.hpp @@ -56,9 +56,9 @@ class TransportHints * in the node's local namespace. For consistency across ROS applications, the * name of this parameter should not be changed without good reason. * - * @param node Node to use when looking up the transport parameter. - * @param default_transport Preferred transport to use - * @param parameter_name The name of the transport parameter + * \param node Node to use when looking up the transport parameter. + * \param default_transport Preferred transport to use + * \param parameter_name The name of the transport parameter * */ POINT_CLOUD_TRANSPORT_PUBLIC diff --git a/package.xml b/package.xml index 915b239..f5b727c 100644 --- a/package.xml +++ b/package.xml @@ -15,15 +15,18 @@ BSD https://github.com/ctu-vras/point_cloud_transport - https://wiki.ros.org/point_cloud_transport https://github.com/ctu-vras/point_cloud_transport/issues ament_cmake_ros + ament_cmake_python message_filters + pybind11 + pybind11_vendor pluginlib rclcpp_components rclcpp + rclpy sensor_msgs git diff --git a/point_cloud_transport/__init__.py b/point_cloud_transport/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/src/point_cloud_transport/common.py b/point_cloud_transport/common.py old mode 100644 new mode 100755 similarity index 64% rename from src/point_cloud_transport/common.py rename to point_cloud_transport/common.py index 1d78d0d..18ee00f --- a/src/point_cloud_transport/common.py +++ b/point_cloud_transport/common.py @@ -1,3 +1,4 @@ +# Copyright (c) 2023, John D'Angelo # Copyright (c) 2023, Czech Technical University in Prague # All rights reserved. # @@ -30,28 +31,38 @@ """Common definitions.""" -from ctypes import RTLD_GLOBAL +from rclpy.serialization import serialize_message, deserialize_message +from sensor_msgs.msg import PointCloud2 +from importlib import import_module -from cras.ctypes_utils import load_library +class TransportInfo(object): -__library = None + def __init__(self, name: str, topic: str, data_type: str): + self.name = name + self.topic = topic + self.data_type = data_type -def _get_base_library(): - global __library - if __library is None: - __library = load_library('point_cloud_transport', mode=RTLD_GLOBAL) - if __library is None: - return None +def stringToPointCloud2(buffer: str): + cloud = deserialize_message(buffer, "sensor_msgs/msg/PointCloud2") + return cloud - return __library +def pointCloud2ToString(msg: PointCloud2): + buffer = serialize_message(msg) + return buffer -class _TransportInfo(object): - def __init__(self, name, topic, data_type, config_data_type=None): - self.name = name - self.topic = topic - self.data_type = data_type - self.config_data_type = config_data_type +def stringToMsgType(message_type_str): + try: + # Dynamically import the message type + package_name, message_type = message_type_str.replace( + "/", ".").rsplit(".", 1) + module = import_module(package_name) + message_class = getattr(module, message_type) + # Return the subscription object + return message_class + except Exception as e: + print(f"Error creating subscription: {e}") + return None diff --git a/point_cloud_transport/publisher.py b/point_cloud_transport/publisher.py new file mode 100755 index 0000000..d989f9f --- /dev/null +++ b/point_cloud_transport/publisher.py @@ -0,0 +1,116 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2023, John D'Angelo +# Copyright (c) 2023, Czech Technical University in Prague +# 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 TU Darmstadt nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""Publisher that automatically publishes to all declared transports.""" + +from rclpy.node import Node +from sensor_msgs.msg import PointCloud2 + +from point_cloud_transport.common import TransportInfo, pointCloud2ToString, stringToMsgType +from point_cloud_transport._codec import PointCloudCodec, VectorString + + +def _get_topics_to_publish(codec, base_topic, logger): + transports = VectorString() + names = VectorString() + topics = VectorString() + data_types = VectorString() + + codec.getTopicsToPublish( + base_topic, transports, + topics, names, data_types) + + topics_to_publish = {} + + for i in range(len(transports)): + try: + topics_to_publish[transports[i]] = \ + TransportInfo(names[i], topics[i], data_types[i]) + except ImportError as e: + print('Import error: ' + str(e)) + + return topics_to_publish + + +class Publisher(Node): + + def __init__(self): + node_name = "point_cloud_transport_publisher" + super().__init__(node_name) + + self.codec = PointCloudCodec() + print("Codec created") + self.topics_to_publish = _get_topics_to_publish( + self.codec, "point_cloud", self.get_logger()) + print("Topics to publish: \n", self.topics_to_publish) + + blacklist = set(self.get_parameter_or('disable_pub_plugins', [])) + print("Blacklist: \n" + str(blacklist)) + + self.transport_publishers = {} + for transport in self.topics_to_publish: + if transport in blacklist: + continue + topic_to_publish = self.topics_to_publish[transport] + self.transport_publishers[transport] = self.create_publisher( + stringToMsgType(topic_to_publish.data_type), topic_to_publish.topic, 1) + + def publish(self, raw: PointCloud2): + for transport, transport_info in self.topics_to_publish.items(): + compressed_buffer = self.codec.encode( + transport_info.name, pointCloud2ToString(raw)) + if compressed_buffer: + # rclpy is smart and will publish the correct type even though we + # are giving it a serialized array of bytes + self.transport_publishers[transport].publish(compressed_buffer) + else: + self.get_logger().error('Error encoding message!') + + +if __name__ == "__main__": + import rclpy + import sys + + rclpy.init(args=sys.argv) + + publisher_node = None + try: + publisher_node = Publisher() + rclpy.spin(publisher_node) + except Exception as e: + print("Error in publisher node!") + print(e) + finally: + if publisher_node is not None: + publisher_node.destroy_node() + rclpy.shutdown() diff --git a/point_cloud_transport/subscriber.py b/point_cloud_transport/subscriber.py new file mode 100755 index 0000000..e4a1c8c --- /dev/null +++ b/point_cloud_transport/subscriber.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2023, John D'Angelo +# Copyright (c) 2023, Czech Technical University in Prague +# 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 TU Darmstadt nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +"""Subscriber automatically converting from any transport to raw.""" + +from rclpy.node import Node + +from point_cloud_transport.common import TransportInfo, stringToPointCloud2, stringToMsgType +from point_cloud_transport._codec import PointCloudCodec, VectorString + + +def _get_loadable_transports(codec: PointCloudCodec): + transports = VectorString() + names = VectorString() + codec.getLoadableTransports(transports, names) + return dict(zip(transports, names)) + + +def _get_topic_to_subscribe(codec, base_topic, transport_name): + (topic, name, data_type) = codec.getTopicToSubscribe( + base_topic, transport_name) + + if len(data_type) == 0: + return None + + return TransportInfo(name, topic, data_type) + + +class Subscriber(Node): + def __init__(self): + node_name = "point_cloud_transport_subscriber" + super().__init__(node_name) + + self.base_topic = "point_cloud" + self.transport = self.get_parameter_or("transport", "raw") + self.codec = PointCloudCodec() + + transports = _get_loadable_transports(self.codec) + if self.transport not in transports and self.transport not in transports.values(): + raise RuntimeError( + "Point cloud transport '%s' not found." % (self.transport,)) + + self.transport_info = _get_topic_to_subscribe( + self.codec, self.base_topic, self.transport) + if self.transport_info is None: + raise RuntimeError( + "Point cloud transport '%s' not found." % (self.transport,)) + + # subscribe to compressed, serialized msg + self.subscriber = self.create_subscription(stringToMsgType( + self.transport_info.data_type), self.transport_info.topic, self.cb, 1, raw=True) + + def cb(self, serialized_buffer): + cloud_buffer = self.codec.decode( + self.transport_info.name, serialized_buffer) + cloud = stringToPointCloud2(cloud_buffer) + print(cloud) + + +if __name__ == "__main__": + import rclpy + import sys + + rclpy.init(args=sys.argv) + + subscriber_node = None + try: + subscriber_node = Subscriber() + rclpy.spin(subscriber_node) + except Exception as e: + print(e) + finally: + if subscriber_node is not None: + subscriber_node.destroy_node() + rclpy.shutdown() diff --git a/src/c_api.cpp b/src/c_api.cpp deleted file mode 100644 index 03e80b4..0000000 --- a/src/c_api.cpp +++ /dev/null @@ -1,70 +0,0 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * 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 HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -// /** -// * \file -// * \brief Support definitions for declaration of a C API of modules -// * \author Martin Pecka -// */ - -// #include -// #include -// #include - -// #include - -// namespace cras -// { - -// char* outputString(cras::allocator_t allocator, const char* string, size_t length) -// { -// const auto buffer = static_cast(allocator(length)); -// strncpy(buffer, string, length); -// return buffer; -// } - -// char* outputString(allocator_t allocator, const std::string& string) -// { -// return outputString(allocator, string.c_str(), string.size() + 1); -// } - -// uint8_t* outputByteBuffer(allocator_t allocator, const uint8_t* bytes, size_t length) -// { -// const auto buffer = allocator(length); -// return static_cast(memcpy(buffer, bytes, length)); -// } - -// uint8_t* outputByteBuffer(allocator_t allocator, const std::vector& bytes) -// { -// return outputByteBuffer(allocator, bytes.data(), bytes.size()); -// } - -// } diff --git a/src/point_cloud_codec.cpp b/src/point_cloud_codec.cpp index 423ade5..eb76346 100644 --- a/src/point_cloud_codec.cpp +++ b/src/point_cloud_codec.cpp @@ -31,313 +31,241 @@ * POSSIBILITY OF SUCH DAMAGE. */ - #include - #include #include #include #include +#include #include -#include #include +#include +#include #include -#include -#include namespace point_cloud_transport { -struct PointCloudCodec::Impl -{ - point_cloud_transport::PubLoaderPtr enc_loader_; - point_cloud_transport::SubLoaderPtr dec_loader_; - std::unordered_map encoders_for_topics_; - std::unordered_map decoders_for_topics_; - - Impl() - : enc_loader_(std::make_shared( - "point_cloud_transport", - "point_cloud_transport::PublisherPlugin")), - dec_loader_(std::make_shared( - "point_cloud_transport", - "point_cloud_transport::SubscriberPlugin")) - { - } -}; - PointCloudCodec::PointCloudCodec() -: impl_(new Impl) -{ -} +: enc_loader_(std::make_shared( + "point_cloud_transport", "point_cloud_transport::PublisherPlugin")), + dec_loader_(std::make_shared( + "point_cloud_transport", + "point_cloud_transport::SubscriberPlugin")) {} -bool transportNameMatches( - const std::string & lookup_name, const std::string & name, - const std::string & suffix) -{ - if (lookup_name == name) { - return true; - } - const std::string transport_name = removeSuffix(lookup_name, suffix); - if (transport_name == name) { - return true; - } - const auto parts = split(transport_name, "/"); - if (parts.size() == 2 && parts[1] == name) { - return true; - } - return false; -} +PointCloudCodec::~PointCloudCodec() {} -std::shared_ptr PointCloudCodec::getEncoderByName( - const std::string & name) const +std::shared_ptr +PointCloudCodec::getEncoderByName(const std::string & name) { - for (const auto & lookup_name : impl_->enc_loader_->getDeclaredClasses()) { + for (const auto & lookup_name : enc_loader_->getDeclaredClasses()) { if (transportNameMatches(lookup_name, name, "_pub")) { - auto encoder = impl_->enc_loader_->createSharedInstance(lookup_name); + auto encoder = enc_loader_->createSharedInstance(lookup_name); return encoder; } } - // ROS_DEBUG("Failed to find encoder %s.", name.c_str()); + RCLCPP_DEBUG( + rclcpp::get_logger("point_cloud_transport"), + "Failed to find encoder %s.", name.c_str()); return nullptr; } -std::shared_ptr PointCloudCodec::getEncoderByTopic( - const std::string & topic, const std::string & datatype) const +std::shared_ptr +PointCloudCodec::getDecoderByName(const std::string & name) { - if (impl_->encoders_for_topics_.find(topic) != impl_->encoders_for_topics_.end()) { - auto encoder = impl_->enc_loader_->createSharedInstance(impl_->encoders_for_topics_[topic]); - return encoder; - } - - for (const auto & lookup_name : impl_->enc_loader_->getDeclaredClasses()) { - const auto & encoder = impl_->enc_loader_->createSharedInstance(lookup_name); - if (!encoder) { - continue; - } - if (encoder->matchesTopic(topic, datatype)) { - impl_->encoders_for_topics_[topic] = lookup_name; - return encoder; + for (const auto & lookup_name : dec_loader_->getDeclaredClasses()) { + if (transportNameMatches(lookup_name, name, "_sub")) { + auto decoder = dec_loader_->createSharedInstance(lookup_name); + return decoder; } } - // ROS_DEBUG("Failed to find encoder for topic %s with data type %s.", t - // opic.c_str(), datatype.c_str()); + RCLCPP_DEBUG( + rclcpp::get_logger("point_cloud_transport"), + "Failed to find decoder %s.", name.c_str()); return nullptr; } -std::shared_ptr PointCloudCodec::getDecoderByName( - const std::string & name) const +void PointCloudCodec::getLoadableTransports( + std::vector & transports, + std::vector & names) { - for (const auto & lookup_name : impl_->dec_loader_->getDeclaredClasses()) { - if (transportNameMatches(lookup_name, name, "_sub")) { - auto decoder = impl_->dec_loader_->createSharedInstance(lookup_name); - return decoder; + for (const auto & transportPlugin : dec_loader_->getDeclaredClasses()) { + // If the plugin loads without throwing an exception, add its transport name to the list of + // valid plugins, otherwise ignore it. + try { + auto sub = dec_loader_->createSharedInstance(transportPlugin); + // Remove the "_sub" at the end of each class name. + transports.push_back(erase_last_copy(transportPlugin, "_sub")); + names.push_back(sub->getTransportName()); + } catch (const pluginlib::LibraryLoadException & e) { + (void)e; + } catch (const pluginlib::CreateClassException & e) { + (void)e; } } - - // ROS_DEBUG("Failed to find decoder %s.", name.c_str()); - return nullptr; } -std::shared_ptr PointCloudCodec::getDecoderByTopic( - const std::string & topic, const std::string & datatype) const +void PointCloudCodec::getTopicsToPublish( + const std::string & baseTopic, + std::vector & transports, + std::vector & topics, + std::vector & names, + std::vector & dataTypes) { - if (impl_->decoders_for_topics_.find(topic) != impl_->decoders_for_topics_.end()) { - auto decoder = impl_->dec_loader_->createSharedInstance(impl_->decoders_for_topics_[topic]); - return decoder; - } - - for (const auto & lookup_name : impl_->dec_loader_->getDeclaredClasses()) { - const auto & decoder = impl_->dec_loader_->createSharedInstance(lookup_name); - if (!decoder) { - continue; - } - if (decoder->matchesTopic(topic, datatype)) { - impl_->decoders_for_topics_[topic] = lookup_name; - return decoder; + for (const auto & transportPlugin : enc_loader_->getDeclaredClasses()) { + try { + auto pub = enc_loader_->createSharedInstance(transportPlugin); + + if (pub == nullptr) { + continue; + } + + // Remove the "_pub" at the end of each class name. + transports.push_back(erase_last_copy(transportPlugin, "_pub")); + names.push_back(pub->getTransportName()); + topics.push_back(pub->getTopicToAdvertise(baseTopic)); + dataTypes.push_back(pub->getDataType()); + } catch (const pluginlib::PluginlibException & e) { + std::cout << "pointCloudTransportGetTopicsToPublish: " << e.what() << "\n" + << std::endl; } } - - // ROS_DEBUG("Failed to find decoder for topic %s with data type %s.", - // topic.c_str(), datatype.c_str()); - return nullptr; } -} // namespace point_cloud_transport - -// TODO(anyone): These functions seem overly complex. Can we simplify them? - -bool pointCloudTransportCodecsEncode( - const char * codec, - sensor_msgs::msg::PointCloud2::_height_type rawHeight, - sensor_msgs::msg::PointCloud2::_width_type rawWidth, - size_t rawNumFields, - const char * rawFieldNames[], - sensor_msgs::PointField::_offset_type rawFieldOffsets[], - sensor_msgs::PointField::_datatype_type rawFieldDatatypes[], - sensor_msgs::PointField::_count_type rawFieldCounts[], - sensor_msgs::msg::PointCloud2::_is_bigendian_type rawIsBigendian, - sensor_msgs::msg::PointCloud2::_point_step_type rawPointStep, - sensor_msgs::msg::PointCloud2::_row_step_type rawRowStep, - size_t rawDataLength, - const uint8_t rawData[], - sensor_msgs::msg::PointCloud2::_is_dense_type rawIsDense, - cras::allocator_t compressedTypeAllocator, - cras::allocator_t compressedMd5SumAllocator, - cras::allocator_t compressedDataAllocator, - size_t serializedConfigLength, - const uint8_t serializedConfig[], - cras::allocator_t errorStringAllocator, - cras::allocator_t logMessagesAllocator -) +void PointCloudCodec::getTopicToSubscribe( + const std::string & baseTopic, + const std::string & transport, + std::string & topic, + std::string & name, + std::string & dataType) { - dynamic_reconfigure::Config config; - if (serializedConfigLength > 0) { - ros::serialization::IStream data(const_cast(serializedConfig), - serializedConfigLength); + for (const auto & transportPlugin : dec_loader_->getDeclaredClasses()) { try { - ros::serialization::deserialize(data, config); - } catch (const ros::Exception & e) { - cras::outputString( - errorStringAllocator, - cras::format("Could not deserialize encoder config: %s.", e.what())); - return false; + auto sub = dec_loader_->createSharedInstance(transportPlugin); + + const auto transportClass = erase_last_copy(transportPlugin, "_sub"); + if (transportClass != transport && sub->getTransportName() != transport) { + continue; + } + + if (sub == nullptr) { + continue; + } + + topic = sub->getTopicToSubscribe(baseTopic); + name = sub->getTransportName(); + dataType = sub->getDataType(); + return; + } catch (const pluginlib::PluginlibException & e) { + std::cout << "pointCloudTransportGetTopicToSubscribe: " << e.what() << "\n" + << std::endl; } } - sensor_msgs::msg::PointCloud2 raw; - raw.height = rawHeight; - raw.width = rawWidth; - for (size_t i = 0; i < rawNumFields; ++i) { - sensor_msgs::PointField field; - field.name = rawFieldNames[i]; - field.offset = rawFieldOffsets[i]; - field.datatype = rawFieldDatatypes[i]; - field.count = rawFieldCounts[i]; - raw.fields.push_back(field); - } - raw.is_bigendian = rawIsBigendian; - raw.point_step = rawPointStep; - raw.row_step = rawRowStep; - raw.data.resize(rawDataLength); - memcpy(raw.data.data(), rawData, rawDataLength); - raw.is_dense = rawIsDense; - - auto encoder = - point_cloud_transport::point_cloud_transport_codec_instance.getEncoderByName(codec); +} + +bool PointCloudCodec::encode( + const std::string & transport_name, const sensor_msgs::msg::PointCloud2 & msg, + rclcpp::SerializedMessage & serialized_msg) +{ + auto encoder = getEncoderByName(transport_name); if (!encoder) { - cras::outputString(errorStringAllocator, std::string("Could not find encoder for ") + codec); return false; } - const auto compressed = encoder->encode(raw, config); + const auto compressed = encoder->encode(msg); if (!compressed) { - cras::outputString(errorStringAllocator, compressed.error()); return false; } if (!compressed.value()) { - return true; + return false; } - cras::outputString(compressedTypeAllocator, compressed.value()->getDataType()); - cras::outputString(compressedMd5SumAllocator, compressed.value()->getMD5Sum()); - cras::outputByteBuffer( - compressedDataAllocator, cras::getBuffer( - compressed->value()), compressed.value()->size()); + serialized_msg = *(compressed.value()->get()); return true; } -bool pointCloudTransportCodecsDecode( - const char * topicOrCodec, - const char * compressedType, - const char * compressedMd5sum, - size_t compressedDataLength, - const uint8_t compressedData[], - sensor_msgs::msg::PointCloud2::_height_type & rawHeight, - sensor_msgs::msg::PointCloud2::_width_type & rawWidth, - uint32_t & rawNumFields, - cras::allocator_t rawFieldNamesAllocator, - cras::allocator_t rawFieldOffsetsAllocator, - cras::allocator_t rawFieldDatatypesAllocator, - cras::allocator_t rawFieldCountsAllocator, - sensor_msgs::msg::PointCloud2::_is_bigendian_type & rawIsBigEndian, - sensor_msgs::msg::PointCloud2::_point_step_type & rawPointStep, - sensor_msgs::msg::PointCloud2::_row_step_type & rawRowStep, - cras::allocator_t rawDataAllocator, - sensor_msgs::msg::PointCloud2::_is_dense_type & rawIsDense, - size_t serializedConfigLength, - const uint8_t serializedConfig[], - cras::allocator_t errorStringAllocator, - cras::allocator_t logMessagesAllocator -) +template +bool PointCloudCodec::encodeTyped( + const std::string & transport_name, const sensor_msgs::msg::PointCloud2 & msg, + M & compressed_msg) { - dynamic_reconfigure::Config config; - if (serializedConfigLength > 0) { - ros::serialization::IStream data(const_cast(serializedConfig), - serializedConfigLength); - try { - ros::serialization::deserialize(data, config); - } catch (const ros::Exception & e) { - cras::outputString( - errorStringAllocator, - cras::format("Could not deserialize decoder config: %s.", e.what())); - return false; - } + auto encoder = getEncoderByName(transport_name); + if (!encoder) { + return false; } - topic_tools::ShapeShifter compressed; - compressed.morph(compressedMd5sum, compressedType, "", ""); - cras::resizeBuffer(compressed, compressedDataLength); - memcpy(cras::getBuffer(compressed), compressedData, compressedDataLength); + auto typed_encoder = + reinterpret_cast *>(encoder.get()); + const auto compressed = typed_encoder->encodeTyped(msg); - auto decoder = point_cloud_transport::point_cloud_transport_codec_instance.getDecoderByTopic( - topicOrCodec, compressedType); - if (!decoder) { - decoder = point_cloud_transport::point_cloud_transport_codec_instance.getDecoderByName( - topicOrCodec); + if (!compressed) { + return false; } + if (!compressed.value()) { + return false; + } + + compressed_msg = *(compressed.value()->get()); + return true; +} + +bool PointCloudCodec::decode( + const std::string & transport_name, + const rclcpp::SerializedMessage & serialized_msg, + sensor_msgs::msg::PointCloud2 & msg) +{ + // decode the serialized msg into a pointcloud + auto decoder = getDecoderByName(transport_name); + if (!decoder) { - cras::outputString( - errorStringAllocator, std::string( - "Could not find decoder for ") + topicOrCodec); return false; } - const auto res = decoder->decode(compressed, config); + const auto decompressed = decoder->decode( + std::make_shared(serialized_msg)); - if (!res) { - cras::outputString(errorStringAllocator, res.error()); + if (!decompressed) { return false; } + if (!decompressed.value()) { + return false; + } + + msg = *(decompressed.value()->get()); + return true; +} - if (!res.value()) { - return true; +template +bool PointCloudCodec::decodeTyped( + const std::string & transport_name, + const M & compressed_msg, + sensor_msgs::msg::PointCloud2 & msg) +{ + // decode the serialized msg into a pointcloud + auto decoder = getDecoderByName(transport_name); + + if (!decoder) { + return false; } - const auto & raw = res->value(); - - rawHeight = raw->height; - rawWidth = raw->width; - rawNumFields = raw->fields.size(); - for (size_t i = 0; i < rawNumFields; ++i) { - cras::outputString(rawFieldNamesAllocator, raw->fields[i].name); - cras::outputByteBuffer( - rawFieldOffsetsAllocator, - reinterpret_cast(&raw->fields[i].offset), 4); - cras::outputByteBuffer( - rawFieldDatatypesAllocator, - reinterpret_cast(&raw->fields[i].datatype), 1); - cras::outputByteBuffer( - rawFieldCountsAllocator, - reinterpret_cast(&raw->fields[i].count), 4); + auto typed_decoder = + reinterpret_cast *>(decoder.get()); + + const auto decompressed = typed_decoder->decodeTyped(compressed_msg); + + if (!decompressed) { + return false; + } + if (!decompressed.value()) { + return false; } - rawIsBigEndian = raw->is_bigendian; - rawPointStep = raw->point_step; - rawRowStep = raw->row_step; - cras::outputByteBuffer(rawDataAllocator, raw->data); - rawIsDense = raw->is_dense; + + msg = *(decompressed.value()->get()); return true; } + +} // namespace point_cloud_transport diff --git a/src/point_cloud_common.cpp b/src/point_cloud_common.cpp index 1aef5eb..27ff72a 100644 --- a/src/point_cloud_common.cpp +++ b/src/point_cloud_common.cpp @@ -29,6 +29,9 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include +#include + #include "point_cloud_transport/point_cloud_common.hpp" namespace point_cloud_transport @@ -56,10 +59,12 @@ std::vector split( std::string token; std::vector result; + size_t split_limit = maxSplits >= 0 ? maxSplits : std::numeric_limits::max(); + while ((end = str.find( delimiter, - start)) != std::string::npos && (maxSplits == -1 || result.size() < maxSplits)) + start)) != std::string::npos && (result.size() < split_limit)) { token = str.substr(start, end - start); start = end + delimiterLength; @@ -89,4 +94,23 @@ std::string removeSuffix(const std::string & str, const std::string & suffix, bo return hasSuffix ? str.substr(0, str.length() - suffix.length()) : str; } + +bool transportNameMatches( + const std::string & lookup_name, + const std::string & name, const std::string & suffix) +{ + if (lookup_name == name) { + return true; + } + const std::string transport_name = removeSuffix(lookup_name, suffix); + if (transport_name == name) { + return true; + } + const auto parts = split(transport_name, "/"); + if (parts.size() == 2 && parts[1] == name) { + return true; + } + return false; +} + } // namespace point_cloud_transport diff --git a/src/point_cloud_transport.cpp b/src/point_cloud_transport.cpp index ad5018a..f4d692c 100644 --- a/src/point_cloud_transport.cpp +++ b/src/point_cloud_transport.cpp @@ -145,96 +145,10 @@ SubLoaderPtr PointCloudTransportLoader::getSubscriberLoader() const thread_local std::unique_ptr loader; -point_cloud_transport::PointCloudTransportLoader & getLoader() -{ - if (point_cloud_transport::loader == nullptr) { - point_cloud_transport::loader = - std::make_unique(); - } - return *point_cloud_transport::loader; -} - PointCloudTransport::PointCloudTransport(rclcpp::Node::SharedPtr node) { PointCloudTransportLoader(); node_ = node; } - -// TODO(anyone): Are these needed? -// void pointCloudTransportGetLoadableTransports( -// cras::allocator_t transportAllocator, cras::allocator_t nameAllocator) -// { -// for (const auto& transport : getLoader().getLoadableTransports()) -// { -// cras::outputString(transportAllocator, transport.first); -// cras::outputString(nameAllocator, transport.second); -// } -// } - -// void pointCloudTransportGetTopicsToPublish(const char* baseTopic, -// cras::allocator_t transportAllocator, -// cras::allocator_t nameAllocator, -// cras::allocator_t topicAllocator, -// cras::allocator_t dataTypeAllocator, -// cras::allocator_t configTypeAllocator) -// { -// auto pubLoader = getLoader().getPublisherLoader(); -// for (const auto& transportPlugin : pubLoader->getDeclaredClasses()) -// { -// try -// { -// auto pub = pubLoader->createSharedInstance(transportPlugin); -// auto singleTopicPub = -// std::dynamic_pointer_cast(pub); -// if (singleTopicPub == nullptr) -// continue; -// // Remove the "_pub" at the end of each class name. -// cras::outputString(transportAllocator, erase_last_copy(transportPlugin, "_pub")); -// cras::outputString(nameAllocator, singleTopicPub->getTransportName()); -// cras::outputString(topicAllocator, singleTopicPub->getTopicToAdvertise(baseTopic)); -// cras::outputString(dataTypeAllocator, singleTopicPub->getDataType()); -// cras::outputString(configTypeAllocator, singleTopicPub->getConfigDataType()); -// } -// catch (const pluginlib::PluginlibException& e) -// { -// } -// } -// } - -// void pointCloudTransportGetTopicToSubscribe(const char* baseTopic, -// const char* transport, -// cras::allocator_t nameAllocator, -// cras::allocator_t topicAllocator, -// cras::allocator_t dataTypeAllocator, -// cras::allocator_t configTypeAllocator) -// { -// auto subLoader = getLoader().getSubscriberLoader(); -// for (const auto& transportPlugin : subLoader->getDeclaredClasses()) -// { -// try -// { -// auto sub = subLoader->createSharedInstance(transportPlugin); - -// const auto transportClass = erase_last_copy(transportPlugin, "_sub"); -// if (transportClass != transport && sub->getTransportName() != transport) -// continue; - -// auto singleTopicSub = -// std::dynamic_pointer_cast(sub); -// if (singleTopicSub == nullptr) -// continue; - -// cras::outputString(nameAllocator, singleTopicSub->getTransportName()); -// cras::outputString(topicAllocator, singleTopicSub->getTopicToSubscribe(baseTopic)); -// cras::outputString(dataTypeAllocator, singleTopicSub->getDataType()); -// cras::outputString(configTypeAllocator, singleTopicSub->getConfigDataType()); -// return; -// } -// catch (const pluginlib::PluginlibException& e) -// { -// } -// } -// } - } // namespace point_cloud_transport diff --git a/src/point_cloud_transport/__init__.py b/src/point_cloud_transport/__init__.py deleted file mode 100644 index c74f4df..0000000 --- a/src/point_cloud_transport/__init__.py +++ /dev/null @@ -1,131 +0,0 @@ -# Copyright (c) 2023, Czech Technical University in Prague -# 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 TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -""" -Encoding and decoding of point clouds compressed with any point cloud transport. - -Example usage: - -.. code-block:: python - - from point_cloud_transport import decode, encode - from sensor_msgs.msg import PointCloud2 - - raw = PointCloud2() - ... # fill the cloud - compressed, err = encode(raw, 'draco') - if compressed is None: - rospy.logerr("Error encoding point cloud: " + err) - return False - # work with the compressed instance in variable compressed - - # beware, for decoding, we do not specify "raw", but the codec used for encoding - raw2, err = decode(compressed, 'draco') - if raw2 is None: - rospy.logerr("Error decoding point cloud: " + err) - return False - # work with the PointCloud2 instance in variable raw2 -""" - -from point_cloud_transport.decoder import decode -from point_cloud_transport.encoder import encode -from point_cloud_transport.publisher import Publisher -from point_cloud_transport.subscriber import Subscriber - - -if __name__ == '__main__': - def main(): - import rospy - import time - from sensor_msgs.msg import PointCloud2, PointField - - raw = PointCloud2() - raw.header.stamp = rospy.Time(10) - raw.header.frame_id = 'test' - raw.width = raw.height = 2 - raw.is_bigendian = False - raw.is_dense = True - raw.point_step = 8 - raw.row_step = raw.point_step * 2 - raw.fields = [ - PointField(name='x', offset=0, datatype=PointField.INT32, count=1), - PointField(name='y', offset=0, datatype=PointField.INT32, count=1), - ] - - import struct - data = struct.pack('i', 0) + struct.pack('i', 1000) + \ - struct.pack('i', 5000) + struct.pack('i', 9999) - data += struct.pack('i', 0) + struct.pack('i', -1000) + \ - struct.pack('i', -5000) + struct.pack('i', -9999) - import sys - if sys.version_info[0] == 2: - raw.data = map(ord, data) - - rospy.init_node('test_node') - rospy.loginfo('start') - time.sleep(1) - - start = time.time() - for i in range(1): - compressed, err = encode(raw, 'draco', {'encode_speed': 1}) - # compressed, err = encode(raw, 'draco') - if compressed is None: - print(err) - break - - # raw2, err = decode(compressed, 'draco', {"SkipDequantizationPOSITION": True}) - raw2, err = decode(compressed, 'draco') - - end = time.time() - print(end - start) - print(bool(raw)) - print(bool(compressed)) - print(err) - print(raw.fields == raw2.fields) - print(raw == raw2) - - def cb(msg): - print(msg == raw) # they are not equal exactly... - - def cb_ros(msg): - print(msg == compressed) - - sub = Subscriber('test', cb, default_transport='draco', queue_size=1) - sub # prevent unused variable warning - sub2 = rospy.Subscriber('test', PointCloud2, cb, queue_size=1) - sub2 # prevent unused variable warning - sub3 = rospy.Subscriber('test/draco', type(compressed), cb_ros, queue_size=1) - sub3 # prevent unused variable warning - pub = Publisher('test', queue_size=10) - time.sleep(0.2) - pub.publish(raw) - rospy.spin() - - main() diff --git a/src/point_cloud_transport/decoder.py b/src/point_cloud_transport/decoder.py deleted file mode 100644 index 15c1a09..0000000 --- a/src/point_cloud_transport/decoder.py +++ /dev/null @@ -1,150 +0,0 @@ -# Copyright (c) 2023, Czech Technical University in Prague -# 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 TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -"""Encoding and decoding of point clouds compressed with any point cloud transport.""" - -from ctypes import byref, c_bool, c_char_p, c_size_t, c_uint32, c_uint8, POINTER - -from cras.ctypes_utils import Allocator, BytesAllocator, LogMessagesAllocator -from cras.ctypes_utils import get_ro_c_buffer, ScalarAllocator, StringAllocator -from cras.message_utils import dict_to_dynamic_config_msg -from cras.string_utils import BufferStringIO - -from sensor_msgs.msg import PointCloud2, PointField - -from .common import _get_base_library - - -def _get_library(): - library = _get_base_library() - # Add function signatures - - library.pointCloudTransportCodecsDecode.restype = c_bool - library.pointCloudTransportCodecsDecode.argtypes = [ - c_char_p, - c_char_p, c_char_p, c_size_t, POINTER(c_uint8), - POINTER(c_uint32), POINTER(c_uint32), - POINTER(c_size_t), Allocator.ALLOCATOR, - Allocator.ALLOCATOR, Allocator.ALLOCATOR, Allocator.ALLOCATOR, - POINTER(c_uint8), POINTER(c_uint32), POINTER(c_uint32), - Allocator.ALLOCATOR, - POINTER(c_uint8), - c_size_t, POINTER(c_uint8), - Allocator.ALLOCATOR, Allocator.ALLOCATOR, - ] - - return library - - -def decode(compressed, topic_or_codec, config=None): - """ - Decode the given compressed point cloud encoded with any codec into a raw point cloud. - - :param genpy.Message compressed: The compressed point cloud. - :param str topic_or_codec: Name of the topic this cloud comes from or explicit name - of the codec. - :param config: Configuration of the decoding process. - :type config: dict or dynamic_reconfigure.msg.Config or None - :return: Tuple of raw cloud and error string. If the decoding fails, cloud is `None` - and error string is filled. - :rtype: (sensor_msgs.msg.PointCloud2 or None, str) - """ - codec = _get_library() - if codec is None: - return None, 'Could not load the codec library.' - - field_names_allocator = StringAllocator() - field_offset_allocator = ScalarAllocator(c_uint32) - field_datatype_allocator = ScalarAllocator(c_uint8) - field_count_allocator = ScalarAllocator(c_uint32) - data_allocator = BytesAllocator() - error_allocator = StringAllocator() - log_allocator = LogMessagesAllocator() - - raw_height = c_uint32() - raw_width = c_uint32() - raw_is_big_endian = c_uint8() - raw_num_fields = c_size_t() - raw_point_step = c_uint32() - raw_row_step = c_uint32() - raw_is_dense = c_uint8() - - compressed_buf = BufferStringIO() - compressed.serialize(compressed_buf) - compressed_buf_len = compressed_buf.tell() - compressed_buf.seek(0) - - config = dict_to_dynamic_config_msg(config) - config_buf = BufferStringIO() - config.serialize(config_buf) - config_buf_len = config_buf.tell() - config_buf.seek(0) - - args = [ - topic_or_codec.encode('utf-8'), - compressed._type.encode('utf-8'), compressed._md5sum.encode('utf-8'), - compressed_buf_len, - get_ro_c_buffer(compressed_buf), - byref(raw_height), byref(raw_width), - byref(raw_num_fields), field_names_allocator.get_cfunc(), - field_offset_allocator.get_cfunc(), - field_datatype_allocator.get_cfunc(), field_count_allocator.get_cfunc(), - byref(raw_is_big_endian), byref(raw_point_step), - byref(raw_row_step), data_allocator.get_cfunc(), - byref(raw_is_dense), - c_size_t(config_buf_len), get_ro_c_buffer(config_buf), - error_allocator.get_cfunc(), log_allocator.get_cfunc(), - ] - ret = codec.pointCloudTransportCodecsDecode(*args) - - log_allocator.print_log_messages() - if ret: - raw = PointCloud2() - if hasattr(compressed, 'header'): - raw.header = compressed.header - raw.height = raw_height.value - raw.width = raw_width.value - for i in range(raw_num_fields.value): - f = PointField() - f.name = field_names_allocator.values[i] - f.offset = field_offset_allocator.values[i] - f.datatype = field_datatype_allocator.values[i] - f.count = field_count_allocator.values[i] - raw.fields.append(f) - raw.is_bigendian = bool(raw_is_big_endian.value) - raw.point_step = raw_point_step.value - raw.row_step = raw_row_step.value - raw.data = data_allocator.value - import sys - if sys.version_info[0] == 2: - raw.data = map(ord, raw.data) - raw.is_dense = bool(raw_is_dense.value) - return raw, '' - return None, error_allocator.value diff --git a/src/point_cloud_transport/encoder.py b/src/point_cloud_transport/encoder.py deleted file mode 100644 index 9243336..0000000 --- a/src/point_cloud_transport/encoder.py +++ /dev/null @@ -1,118 +0,0 @@ -# Copyright (c) 2023, Czech Technical University in Prague -# 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 TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -"""Encoding and decoding of point clouds compressed with any point cloud transport.""" - -from ctypes import c_bool, c_char_p, c_size_t, c_uint32, c_uint8, POINTER - -from cras import get_msg_type -from cras.ctypes_utils import Allocator, BytesAllocator, LogMessagesAllocator, StringAllocator -from cras.ctypes_utils import c_array, get_ro_c_buffer -from cras.message_utils import dict_to_dynamic_config_msg -from cras.string_utils import BufferStringIO - -from .common import _get_base_library - - -def _get_library(): - library = _get_base_library() - # Add function signatures - - library.pointCloudTransportCodecsEncode.restype = c_bool - library.pointCloudTransportCodecsEncode.argtypes = [ - c_char_p, - c_uint32, c_uint32, c_size_t, POINTER(c_char_p), POINTER(c_uint32), - POINTER(c_uint8), POINTER(c_uint32), - c_uint8, c_uint32, c_uint32, c_size_t, POINTER(c_uint8), c_uint8, - Allocator.ALLOCATOR, Allocator.ALLOCATOR, Allocator.ALLOCATOR, - c_size_t, POINTER(c_uint8), - Allocator.ALLOCATOR, Allocator.ALLOCATOR, - ] - - return library - - -def encode(raw, topic_or_codec, config=None): - """ - Encode the given raw point_cloud into a compressed point_cloud with a suitable codec. - - :param sensor_msgs.msg.PointCloud2 raw: The raw point cloud. - :param str topic_or_codec: Name of the topic where this cloud should be published or explicit - name of the codec. - :param config: Configuration of the encoding process. - :type config: dict or dynamic_reconfigure.msg.Config or None - :return: Tuple of compressed cloud and error string. If the compression fails, cloud is `None` - and error string is filled. - :rtype: (genpy.Message or None, str) - """ - codec = _get_library() - if codec is None: - return None, 'Could not load the codec library.' - - config = dict_to_dynamic_config_msg(config) - config_buf = BufferStringIO() - config.serialize(config_buf) - config_buf_len = config_buf.tell() - config_buf.seek(0) - - type_allocator = StringAllocator() - md5sum_allocator = StringAllocator() - data_allocator = BytesAllocator() - error_allocator = StringAllocator() - log_allocator = LogMessagesAllocator() - - args = [ - topic_or_codec.encode('utf-8'), - raw.height, raw.width, - len(raw.fields), - c_array([f.name.encode('utf-8') for f in raw.fields], c_char_p), - c_array([f.offset for f in raw.fields], c_uint32), - c_array([f.datatype for f in raw.fields], c_uint8), - c_array([f.count for f in raw.fields], c_uint32), - raw.is_bigendian, raw.point_step, raw.row_step, - len(raw.data), get_ro_c_buffer(raw.data), raw.is_dense, - type_allocator.get_cfunc(), md5sum_allocator.get_cfunc(), data_allocator.get_cfunc(), - c_size_t(config_buf_len), get_ro_c_buffer(config_buf), - error_allocator.get_cfunc(), log_allocator.get_cfunc(), - ] - - ret = codec.pointCloudTransportCodecsEncode(*args) - - log_allocator.print_log_messages() - if ret: - msg_type = get_msg_type(type_allocator.value) - compressed = msg_type() - if md5sum_allocator.value != compressed._md5sum: - return None, 'MD5 sum mismatch for %s: %s vs %s' % ( - type_allocator.value, md5sum_allocator.value, compressed._md5sum) - compressed.deserialize(data_allocator.value) - compressed.header = raw.header - return compressed, '' - return None, error_allocator.value diff --git a/src/point_cloud_transport/publisher.py b/src/point_cloud_transport/publisher.py deleted file mode 100644 index a61f9ed..0000000 --- a/src/point_cloud_transport/publisher.py +++ /dev/null @@ -1,130 +0,0 @@ -# Copyright (c) 2023, Czech Technical University in Prague -# 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 TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -"""Publisher that automatically publishes to all declared transports.""" - -from ctypes import c_char_p - -from cras import get_cfg_module, get_msg_type -from cras.ctypes_utils import Allocator, StringAllocator - -import dynamic_reconfigure.server -import rospy - -from .common import _get_base_library, _TransportInfo -from .encoder import encode - - -def _get_library(): - library = _get_base_library() - - library.pointCloudTransportGetLoadableTransports.restype = None - library.pointCloudTransportGetLoadableTransports.argtypes = [ - Allocator.ALLOCATOR, - ] - - library.pointCloudTransportGetTopicsToPublish.restype = None - library.pointCloudTransportGetTopicsToPublish.argtypes = [ - c_char_p, - Allocator.ALLOCATOR, Allocator.ALLOCATOR, - Allocator.ALLOCATOR, Allocator.ALLOCATOR, - Allocator.ALLOCATOR, - ] - - return library - - -def _get_topics_to_publish(base_topic): - transport_allocator = StringAllocator() - name_allocator = StringAllocator() - topic_allocator = StringAllocator() - data_type_allocator = StringAllocator() - config_type_allocator = StringAllocator() - pct = _get_library() - - pct.pointCloudTransportGetTopicsToPublish( - base_topic.encode('utf-8'), transport_allocator.get_cfunc(), name_allocator.get_cfunc(), - topic_allocator.get_cfunc(), data_type_allocator.get_cfunc(), - config_type_allocator.get_cfunc()) - - topics = {} - - for i in range(len(transport_allocator.values)): - try: - data_type = get_msg_type(data_type_allocator.values[i]) - config_type = get_cfg_module(config_type_allocator.values[i]) - topics[transport_allocator.values[i]] = \ - _TransportInfo(name_allocator.values[i], topic_allocator.values[i], - data_type, config_type) - except ImportError as e: - rospy.logerr('Import error: ' + str(e)) - - return topics - - -class Publisher(object): - - def __init__(self, base_topic, *args, **kwargs): - self.base_topic = rospy.names.resolve_name(base_topic) - self.transports = _get_topics_to_publish(self.base_topic) - - blacklist = set(rospy.get_param(self.base_topic + '/disable_pub_plugins', [])) - - self.publishers = {} - self.config_servers = {} - for transport in self.transports: - if transport in blacklist: - continue - topic_to_publish = self.transports[transport] - self.publishers[transport] = rospy.Publisher( - topic_to_publish.topic, topic_to_publish.data_type, *args, **kwargs) - if topic_to_publish.config_data_type is not None: - self.config_servers[transport] = dynamic_reconfigure.server.Server( - topic_to_publish.config_data_type, lambda conf, _: conf, - namespace=topic_to_publish.topic) - - def get_num_subscribers(self): - sum([p.get_num_connections() for p in self.publishers.values()]) - - def get_topic(self): - return self.base_topic - - def publish(self, raw): - for transport, transport_info in self.transports.items(): - config = (self.config_servers[transport].config if transport in - self.config_servers else None) - compressed, err = encode(raw, transport_info.name, config) - if compressed is not None: - self.publishers[transport].publish(compressed) - else: - rospy.logerr(err) - - def shutdown(self): - for publisher in self.publishers.values(): - publisher.unregister() diff --git a/src/point_cloud_transport/subscriber.py b/src/point_cloud_transport/subscriber.py deleted file mode 100644 index 39aed02..0000000 --- a/src/point_cloud_transport/subscriber.py +++ /dev/null @@ -1,141 +0,0 @@ -# Copyright (c) 2023, Czech Technical University in Prague -# 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 TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -"""Subscriber automatically converting from any transport to raw.""" - -from ctypes import c_char_p - -from cras import get_cfg_module, get_msg_type -from cras.ctypes_utils import Allocator, StringAllocator - -import dynamic_reconfigure.server -import rospy - -from .common import _get_base_library, _TransportInfo -from .decoder import decode - - -def _get_library(): - library = _get_base_library() - # Add function signatures - - library.pointCloudTransportGetLoadableTransports.restype = None - library.pointCloudTransportGetLoadableTransports.argtypes = [ - Allocator.ALLOCATOR, - ] - - library.pointCloudTransportGetTopicToSubscribe.restype = None - library.pointCloudTransportGetTopicToSubscribe.argtypes = [ - c_char_p, c_char_p, - Allocator.ALLOCATOR, Allocator.ALLOCATOR, - Allocator.ALLOCATOR, Allocator.ALLOCATOR, - ] - - return library - - -def _get_loadable_transports(): - transport_allocator = StringAllocator() - name_allocator = StringAllocator() - pct = _get_library() - pct.pointCloudTransportGetLoadableTransports( - transport_allocator.get_cfunc(), name_allocator.get_cfunc()) - return dict(zip(transport_allocator.values, name_allocator.values)) - - -def _get_topic_to_subscribe(base_topic, transport): - name_allocator = StringAllocator() - topic_allocator = StringAllocator() - data_type_allocator = StringAllocator() - config_type_allocator = StringAllocator() - pct = _get_library() - - pct.pointCloudTransportGetTopicToSubscribe( - base_topic.encode('utf-8'), transport.encode('utf-8'), - name_allocator.get_cfunc(), topic_allocator.get_cfunc(), - data_type_allocator.get_cfunc(), config_type_allocator.get_cfunc()) - - if len(data_type_allocator.values) == 0: - return None - - try: - data_type = get_msg_type(data_type_allocator.value) - config_type = get_cfg_module(config_type_allocator.value) - return _TransportInfo(name_allocator.value, topic_allocator.value, data_type, config_type) - except ImportError as e: - rospy.logerr('Import error: ' + str(e)) - return None - - -class Subscriber(object): - def __init__(self, base_topic, callback, callback_args=None, default_transport='raw', - parameter_namespace='~', parameter_name='point_cloud_transport', *args, **kwargs): - self.base_topic = rospy.names.resolve_name(base_topic) - self.callback = callback - self.callback_args = callback_args - self.transport = rospy.get_param( - rospy.names.ns_join(parameter_namespace, parameter_name), default_transport) - - transports = _get_loadable_transports() - if self.transport not in transports and self.transport not in transports.values(): - raise RuntimeError("Point cloud transport '%s' not found." % (self.transport,)) - - self.transport_info = _get_topic_to_subscribe(self.base_topic, self.transport) - if self.transport_info is None: - raise RuntimeError("Point cloud transport '%s' not found." % (self.transport,)) - - self.subscriber = rospy.Subscriber( - self.transport_info.topic, self.transport_info.data_type, self.cb, *args, **kwargs) - if self.transport_info.config_data_type is not None: - self.config_server = dynamic_reconfigure.server.Server( - self.transport_info.config_data_type, lambda conf, _: conf, - namespace=rospy.names.ns_join(parameter_namespace, self.transport_info.name)) - else: - self.config_server = None - - def cb(self, msg): - config = self.config_server.config if self.config_server is not None else None - raw, err = decode(msg, self.transport_info.name, config) - if raw is not None: - if self.callback_args is None: - self.callback(raw) - else: - self.callback(raw, self.callback_args) - else: - rospy.logerr(err) - - def get_num_publishers(self): - return self.subscriber.get_num_connections() - - def get_topic(self): - return self.base_topic - - def shutdown(self): - self.subscriber.unregister() diff --git a/src/publisher_plugin.cpp b/src/publisher_plugin.cpp index cf7725d..a4d8c5b 100644 --- a/src/publisher_plugin.cpp +++ b/src/publisher_plugin.cpp @@ -40,6 +40,21 @@ namespace point_cloud_transport { +PublisherPlugin::EncodeResult PublisherPlugin::encode(const sensor_msgs::msg::PointCloud2 & raw) +const +{ + return this->encode(raw); +} + +void PublisherPlugin::advertise( + std::shared_ptr nh, + const std::string & base_topic, + rmw_qos_profile_t custom_qos, + const rclcpp::PublisherOptions & options) +{ + advertiseImpl(nh, base_topic, custom_qos, options); +} + void PublisherPlugin::publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message) const { publish(*message); diff --git a/src/pybind_codec.cpp b/src/pybind_codec.cpp new file mode 100644 index 0000000..4ca6391 --- /dev/null +++ b/src/pybind_codec.cpp @@ -0,0 +1,156 @@ +/* + * Copyright (c) 2023, John D'Angelo + * 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 HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +namespace py = pybind11; +using namespace point_cloud_transport; // NOLINT + +// Utilities for handling the transfer of information to/from python +namespace point_cloud_transport +{ +void stringToSerializedMsg(const std::string & buffer, rclcpp::SerializedMessage & serial_msg) +{ + rcl_serialized_message_t raw_serialized_msg = rmw_get_zero_initialized_serialized_message(); + + // Allocate memory for the serialized message + raw_serialized_msg.buffer_capacity = buffer.size(); + raw_serialized_msg.buffer_length = buffer.size(); + raw_serialized_msg.buffer = static_cast(malloc(raw_serialized_msg.buffer_capacity)); + + if (!raw_serialized_msg.buffer) { + // Handle memory allocation error + // For simplicity, we just throw an exception here + throw std::runtime_error("Failed to allocate memory for serialized message."); + } + + // Copy the string data into the serialized message buffer + memcpy(raw_serialized_msg.buffer, buffer.c_str(), raw_serialized_msg.buffer_length); + + serial_msg = rclcpp::SerializedMessage(raw_serialized_msg); +} + +void serializedMsgToString(const rclcpp::SerializedMessage & serial_msg, std::string & buffer) +{ + buffer = std::string( + reinterpret_cast(serial_msg.get_rcl_serialized_message().buffer), + serial_msg.get_rcl_serialized_message().buffer_length); +} + +void stringToPointCloud2(const std::string & buffer, sensor_msgs::msg::PointCloud2 & cloud) +{ + // serialize the pointcloud2 msg and use stringToSerialziedMsg + auto serialized_msg_ptr = std::make_shared(); + stringToSerializedMsg(buffer, *serialized_msg_ptr); + + rclcpp::Serialization deserializer; + deserializer.deserialize_message(serialized_msg_ptr.get(), &(cloud)); +} + +void pointCloud2ToString(const sensor_msgs::msg::PointCloud2 & cloud, std::string & buffer) +{ + // serialize the pointcloud2 msg and use stringToSerialziedMsg + auto serialized_msg_ptr = std::make_shared(); + rclcpp::Serialization serializer; + serializer.serialize_message(&(cloud), serialized_msg_ptr.get()); + serializedMsgToString(*serialized_msg_ptr, buffer); +} +} // namespace point_cloud_transport + +// Bindings for STL vector of strings +PYBIND11_MAKE_OPAQUE(std::vector); + +// Bindings for the PointCloudCodec class +PYBIND11_MODULE(_codec, m) +{ + py::bind_vector>(m, "VectorString"); + + py::class_(m, "PointCloudCodec") + .def(pybind11::init()) + .def("getLoadableTransports", &PointCloudCodec::getLoadableTransports) + .def("getTopicsToPublish", &PointCloudCodec::getTopicsToPublish) + .def( + "getTopicToSubscribe", + [](PointCloudCodec & codec, const std::string & baseTopic, const std::string & transport) + { + std::string topic; + std::string name; + std::string dataType; + codec.getTopicToSubscribe(baseTopic, transport, topic, name, dataType); + return std::make_tuple(topic, name, dataType); + }) + .def( + "encode", + [](PointCloudCodec & codec, const std::string & transport_name, + const std::string & pointcloud_buffer) + { + sensor_msgs::msg::PointCloud2 msg; + stringToPointCloud2(pointcloud_buffer, msg); + + rclcpp::SerializedMessage serialized_msg; + bool success = codec.encode(transport_name, msg, serialized_msg); + + std::string serialized_buffer; + if (success) { + serializedMsgToString(serialized_msg, serialized_buffer); + } + return serialized_buffer; + } + ) + .def( + "decode", + [](PointCloudCodec & codec, const std::string & transport_name, + const std::string & serialized_buffer) + { + rclcpp::SerializedMessage serialized_msg; + stringToSerializedMsg(serialized_buffer, serialized_msg); + + sensor_msgs::msg::PointCloud2 msg; + bool success = codec.decode(transport_name, serialized_msg, msg); + + std::string buffer; + if (success) { + pointCloud2ToString(msg, buffer); + } + return buffer; + } + ); +} diff --git a/src/raw_subscriber.cpp b/src/raw_subscriber.cpp index cf989e3..644fa44 100644 --- a/src/raw_subscriber.cpp +++ b/src/raw_subscriber.cpp @@ -33,8 +33,6 @@ #include -#include - #include namespace point_cloud_transport @@ -50,6 +48,25 @@ std::string RawSubscriber::getTopicToSubscribe(const std::string & base_topic) c return base_topic; } +std::string RawSubscriber::getDataType() const +{ + return "sensor_msgs/msg/PointCloud2"; +} + +SubscriberPlugin::DecodeResult RawSubscriber::decodeTyped( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & compressed) const +{ + return compressed; +} + +SubscriberPlugin::DecodeResult RawSubscriber::decodeTyped( + const sensor_msgs::msg::PointCloud2 & compressed) const +{ + auto compressedPtr = std::make_shared(compressed); + return this->decodeTyped(compressedPtr); +} + + void RawSubscriber::callback( const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message, const SubscriberPlugin::Callback & user_cb) diff --git a/src/subscriber.cpp b/src/subscriber.cpp index 90f41c1..13d7cd9 100644 --- a/src/subscriber.cpp +++ b/src/subscriber.cpp @@ -119,8 +119,7 @@ Subscriber::Subscriber( "[point_cloud_transport] It looks like you are trying to subscribe directly to a " "transport-specific point_cloud topic '%s', in which case you will likely get a " "connection error. Try subscribing to the base topic '%s' instead with parameter " - "~point_cloud_transport set to '%s' (on the command line, _point_cloud_transport:=%s). " - "See http://ros.org/wiki/point_cloud_transport for details.", + "~point_cloud_transport set to '%s' (on the command line, _point_cloud_transport:=%s). ", clean_topic.c_str(), real_base_topic.c_str(), transport.c_str(), transport.c_str()); } } diff --git a/test/test_point_cloud_codec.cpp b/test/test_point_cloud_codec.cpp new file mode 100644 index 0000000..ef7590e --- /dev/null +++ b/test/test_point_cloud_codec.cpp @@ -0,0 +1,212 @@ +// Copyright (c) 2023 John D'Angelo +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include +#include +#include + +#include "gtest/gtest.h" + +#include +#include +#include + +#include "point_cloud_transport/point_cloud_codec.hpp" + +class TestCodec : public ::testing::Test +{ +public: + // dont do this in real code + point_cloud_transport::PointCloudCodec codec; +}; + +// utility function for verifying the raw encode/decode process works +// (doesnt really make sense to use this with a lossy compression algo though, so its only +// applicable to test raw or zlib) +bool arePointCloudsEqual( + const sensor_msgs::msg::PointCloud2 & msg1, + const sensor_msgs::msg::PointCloud2 & msg2) +{ + if (msg1.width != msg2.width || msg1.height != msg2.height) { + return false; + } + + // Compare fields + if (msg1.fields.size() != msg2.fields.size()) { + return false; + } + + for (size_t i = 0; i < msg1.fields.size(); ++i) { + if (msg1.fields[i].name != msg2.fields[i].name || + msg1.fields[i].offset != msg2.fields[i].offset || + msg1.fields[i].datatype != msg2.fields[i].datatype || + msg1.fields[i].count != msg2.fields[i].count) + { + return false; + } + } + + // Compare data + for (const std::string field_name : {"r", "g", "b"}) { + sensor_msgs::PointCloud2ConstIterator iter1(msg1, field_name); + sensor_msgs::PointCloud2ConstIterator iter2(msg2, field_name); + + for (; iter1 != iter1.end() && iter2 != iter2.end(); ++iter1, ++iter2) { + if (*iter1 != *iter2) { + return false; + } + } + } + + for (const std::string field_name : {"x", "y", "z"}) { + sensor_msgs::PointCloud2ConstIterator iter1(msg1, field_name); + sensor_msgs::PointCloud2ConstIterator iter2(msg2, field_name); + + for (; iter1 != iter1.end() && iter2 != iter2.end(); ++iter1, ++iter2) { + if (*iter1 != *iter2) { + return false; + } + } + } + + return true; +} + +TEST_F(TestCodec, listTransports) { + std::vector transports; + std::vector names; + codec.getLoadableTransports(transports, names); + for (size_t i = 0; i < transports.size(); ++i) { + std::cout << "Transport: " << transports[i] << std::endl; + std::cout << "Name: " << names[i] << std::endl; + } + EXPECT_TRUE(true); +} + +TEST_F(TestCodec, listSubscribedTopics) { + std::vector transports; + std::vector names; + codec.getLoadableTransports(transports, names); + const std::string base_topic = "point_cloud"; + for (const auto & transport : transports) { + std::string topic, name, dataType; + codec.getTopicToSubscribe(base_topic, transport, topic, name, dataType); + std::cout << "Transport: " << transport << std::endl; + std::cout << "Topic: " << topic << std::endl; + std::cout << "Name: " << name << std::endl; + std::cout << "DataType: " << dataType << std::endl; + } + EXPECT_TRUE(true); +} + +TEST_F(TestCodec, listPublishedTopisc) { + std::vector transports; + std::vector topics; + std::vector names; + std::vector dataTypes; + + const std::string base_topic = "point_cloud"; + codec.getTopicsToPublish(base_topic, transports, topics, names, dataTypes); + for (size_t i = 0; i < transports.size(); ++i) { + std::cout << "Transport: " << transports[i] << std::endl; + std::cout << "Topic: " << topics[i] << std::endl; + std::cout << "Name: " << names[i] << std::endl; + std::cout << "DataType: " << dataTypes[i] << std::endl; + } + + EXPECT_TRUE(true); +} + +TEST_F(TestCodec, encodeMesssage) { + auto msg = std::make_shared(); + rclcpp::SerializedMessage serialized_msg; + + // populate the pointcloud with some random data + + // Set the PointCloud2 header + msg->header.frame_id = "base_link"; // Replace with the appropriate frame ID + + // Set the PointCloud2 dimensions and fields + msg->height = 1; + msg->width = 4; + msg->is_dense = true; + + sensor_msgs::PointCloud2Modifier modifier(*msg); + modifier.setPointCloud2FieldsByString(2, "xyz", "rgb"); + + // Fill the PointCloud2 data + sensor_msgs::PointCloud2Iterator iter_x(*msg, "x"); + sensor_msgs::PointCloud2Iterator iter_y(*msg, "y"); + sensor_msgs::PointCloud2Iterator iter_z(*msg, "z"); + sensor_msgs::PointCloud2Iterator iter_r(*msg, "r"); + sensor_msgs::PointCloud2Iterator iter_g(*msg, "g"); + sensor_msgs::PointCloud2Iterator iter_b(*msg, "b"); + + // Sample data points + float points[] = {1.0, 2.0, 3.0, 4.0}; + uint8_t colors[] = {255, 0, 0, 0, 255, 0, 0, 0, 255, 255, 255, 0}; + + for (int i = 0; i < 4; ++i) { + *iter_x = points[i]; + *iter_y = points[i] * 2; + *iter_z = points[i] * 3; + + *iter_r = colors[i * 3]; + *iter_g = colors[i * 3 + 1]; + *iter_b = colors[i * 3 + 2]; + + ++iter_x; + ++iter_y; + ++iter_z; + ++iter_r; + ++iter_g; + ++iter_b; + } + + // encode that data + std::cout << "Encoding message" << std::endl; + bool success = codec.encode("raw", *msg, serialized_msg); + EXPECT_TRUE(success); + + std::cout << "Decoding message" << std::endl; + auto new_msg = std::make_shared(); + success = codec.decode("raw", serialized_msg, *new_msg); + EXPECT_TRUE(success); + + // check that the data is the same + std::cout << "Check for equality" << std::endl; + success = arePointCloudsEqual(*msg, *new_msg); + EXPECT_TRUE(success); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + return ret; +} From de25c8ad544c658ad3a93f58c731ddbd168773c0 Mon Sep 17 00:00:00 2001 From: john-maidbot <78750993+john-maidbot@users.noreply.github.com> Date: Wed, 26 Jul 2023 19:44:07 -0500 Subject: [PATCH 23/28] Add ThirdParty folder to support building offline without FetchContent (#12) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Update package to support building offline * Exclude thirdparty folder from ament_lint * Suggestions to #12 - Fixed linters (#14) * Suyggestions to #12 - Fixed linters Signed-off-by: Alejandro Hernández Cordero * missing dependency Signed-off-by: Alejandro Hernández Cordero --------- Signed-off-by: Alejandro Hernández Cordero --------- Signed-off-by: Alejandro Hernández Cordero Co-authored-by: Alejandro Hernández Cordero --- CMakeLists.txt | 42 +- ThirdParty/expected/COPYING | 121 + ThirdParty/expected/README.md | 76 + ThirdParty/expected/include/tl/expected.hpp | 2444 +++++++++++++++++++ package.xml | 11 +- point_cloud_transport/common.py | 11 +- point_cloud_transport/publisher.py | 20 +- point_cloud_transport/subscriber.py | 19 +- 8 files changed, 2707 insertions(+), 37 deletions(-) create mode 100644 ThirdParty/expected/COPYING create mode 100644 ThirdParty/expected/README.md create mode 100644 ThirdParty/expected/include/tl/expected.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 08569fb..f8a0f97 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,14 +30,8 @@ include_directories( include ) -include(FetchContent) -fetchcontent_declare( - expected - GIT_REPOSITORY https://github.com/TartanLlama/expected - GIT_TAG v1.1.0 -) - -fetchcontent_makeavailable(expected) +# Tell CMake where to find the ThirdParty expected dependency +set(expected_SOURCE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/ThirdParty/expected") # Build libpoint_cloud_transport add_library(${PROJECT_NAME} @@ -166,9 +160,35 @@ ament_export_targets(export_${PROJECT_NAME}) ament_export_dependencies(message_filters rclcpp sensor_msgs pluginlib) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - set(AMENT_LINT_AUTO_FILE_EXCLUDE doc/conf.py) - ament_lint_auto_find_test_dependencies() + set( + _linter_excludes + doc/conf.py + ${expected_SOURCE_DIR}/include/tl/expected.hpp + ) + + find_package(ament_cmake_copyright REQUIRED) + find_package(ament_cmake_cppcheck REQUIRED) + find_package(ament_cmake_cpplint REQUIRED) + find_package(ament_cmake_flake8 REQUIRED) + find_package(ament_cmake_lint_cmake REQUIRED) + find_package(ament_cmake_pep257 REQUIRED) + find_package(ament_cmake_uncrustify REQUIRED) + find_package(ament_cmake_xmllint REQUIRED) + + ament_copyright(EXCLUDE ${_linter_excludes}) + ament_cppcheck( + EXCLUDE ${_linter_excludes} + LANGUAGE c++ + ) + ament_cpplint(EXCLUDE ${_linter_excludes}) + ament_flake8(EXCLUDE ${_linter_excludes}) + ament_lint_cmake() + ament_pep257(EXCLUDE ${_linter_excludes}) + ament_uncrustify( + EXCLUDE ${_linter_excludes} + LANGUAGE c++ + ) + ament_xmllint() find_package(ament_cmake_gtest) diff --git a/ThirdParty/expected/COPYING b/ThirdParty/expected/COPYING new file mode 100644 index 0000000..0e259d4 --- /dev/null +++ b/ThirdParty/expected/COPYING @@ -0,0 +1,121 @@ +Creative Commons Legal Code + +CC0 1.0 Universal + + CREATIVE COMMONS CORPORATION IS NOT A LAW FIRM AND DOES NOT PROVIDE + LEGAL SERVICES. DISTRIBUTION OF THIS DOCUMENT DOES NOT CREATE AN + ATTORNEY-CLIENT RELATIONSHIP. CREATIVE COMMONS PROVIDES THIS + INFORMATION ON AN "AS-IS" BASIS. CREATIVE COMMONS MAKES NO WARRANTIES + REGARDING THE USE OF THIS DOCUMENT OR THE INFORMATION OR WORKS + PROVIDED HEREUNDER, AND DISCLAIMS LIABILITY FOR DAMAGES RESULTING FROM + THE USE OF THIS DOCUMENT OR THE INFORMATION OR WORKS PROVIDED + HEREUNDER. + +Statement of Purpose + +The laws of most jurisdictions throughout the world automatically confer +exclusive Copyright and Related Rights (defined below) upon the creator +and subsequent owner(s) (each and all, an "owner") of an original work of +authorship and/or a database (each, a "Work"). + +Certain owners wish to permanently relinquish those rights to a Work for +the purpose of contributing to a commons of creative, cultural and +scientific works ("Commons") that the public can reliably and without fear +of later claims of infringement build upon, modify, incorporate in other +works, reuse and redistribute as freely as possible in any form whatsoever +and for any purposes, including without limitation commercial purposes. +These owners may contribute to the Commons to promote the ideal of a free +culture and the further production of creative, cultural and scientific +works, or to gain reputation or greater distribution for their Work in +part through the use and efforts of others. + +For these and/or other purposes and motivations, and without any +expectation of additional consideration or compensation, the person +associating CC0 with a Work (the "Affirmer"), to the extent that he or she +is an owner of Copyright and Related Rights in the Work, voluntarily +elects to apply CC0 to the Work and publicly distribute the Work under its +terms, with knowledge of his or her Copyright and Related Rights in the +Work and the meaning and intended legal effect of CC0 on those rights. + +1. Copyright and Related Rights. A Work made available under CC0 may be +protected by copyright and related or neighboring rights ("Copyright and +Related Rights"). Copyright and Related Rights include, but are not +limited to, the following: + + i. the right to reproduce, adapt, distribute, perform, display, + communicate, and translate a Work; + ii. moral rights retained by the original author(s) and/or performer(s); +iii. publicity and privacy rights pertaining to a person's image or + likeness depicted in a Work; + iv. rights protecting against unfair competition in regards to a Work, + subject to the limitations in paragraph 4(a), below; + v. rights protecting the extraction, dissemination, use and reuse of data + in a Work; + vi. database rights (such as those arising under Directive 96/9/EC of the + European Parliament and of the Council of 11 March 1996 on the legal + protection of databases, and under any national implementation + thereof, including any amended or successor version of such + directive); and +vii. other similar, equivalent or corresponding rights throughout the + world based on applicable law or treaty, and any national + implementations thereof. + +2. Waiver. To the greatest extent permitted by, but not in contravention +of, applicable law, Affirmer hereby overtly, fully, permanently, +irrevocably and unconditionally waives, abandons, and surrenders all of +Affirmer's Copyright and Related Rights and associated claims and causes +of action, whether now known or unknown (including existing as well as +future claims and causes of action), in the Work (i) in all territories +worldwide, (ii) for the maximum duration provided by applicable law or +treaty (including future time extensions), (iii) in any current or future +medium and for any number of copies, and (iv) for any purpose whatsoever, +including without limitation commercial, advertising or promotional +purposes (the "Waiver"). Affirmer makes the Waiver for the benefit of each +member of the public at large and to the detriment of Affirmer's heirs and +successors, fully intending that such Waiver shall not be subject to +revocation, rescission, cancellation, termination, or any other legal or +equitable action to disrupt the quiet enjoyment of the Work by the public +as contemplated by Affirmer's express Statement of Purpose. + +3. Public License Fallback. Should any part of the Waiver for any reason +be judged legally invalid or ineffective under applicable law, then the +Waiver shall be preserved to the maximum extent permitted taking into +account Affirmer's express Statement of Purpose. In addition, to the +extent the Waiver is so judged Affirmer hereby grants to each affected +person a royalty-free, non transferable, non sublicensable, non exclusive, +irrevocable and unconditional license to exercise Affirmer's Copyright and +Related Rights in the Work (i) in all territories worldwide, (ii) for the +maximum duration provided by applicable law or treaty (including future +time extensions), (iii) in any current or future medium and for any number +of copies, and (iv) for any purpose whatsoever, including without +limitation commercial, advertising or promotional purposes (the +"License"). The License shall be deemed effective as of the date CC0 was +applied by Affirmer to the Work. Should any part of the License for any +reason be judged legally invalid or ineffective under applicable law, such +partial invalidity or ineffectiveness shall not invalidate the remainder +of the License, and in such case Affirmer hereby affirms that he or she +will not (i) exercise any of his or her remaining Copyright and Related +Rights in the Work or (ii) assert any associated claims and causes of +action with respect to the Work, in either case contrary to Affirmer's +express Statement of Purpose. + +4. Limitations and Disclaimers. + + a. No trademark or patent rights held by Affirmer are waived, abandoned, + surrendered, licensed or otherwise affected by this document. + b. Affirmer offers the Work as-is and makes no representations or + warranties of any kind concerning the Work, express, implied, + statutory or otherwise, including without limitation warranties of + title, merchantability, fitness for a particular purpose, non + infringement, or the absence of latent or other defects, accuracy, or + the present or absence of errors, whether or not discoverable, all to + the greatest extent permissible under applicable law. + c. Affirmer disclaims responsibility for clearing rights of other persons + that may apply to the Work or any use thereof, including without + limitation any person's Copyright and Related Rights in the Work. + Further, Affirmer disclaims responsibility for obtaining any necessary + consents, permissions or other rights required for any use of the + Work. + d. Affirmer understands and acknowledges that Creative Commons is not a + party to this document and has no duty or obligation with respect to + this CC0 or use of the Work. diff --git a/ThirdParty/expected/README.md b/ThirdParty/expected/README.md new file mode 100644 index 0000000..a73133f --- /dev/null +++ b/ThirdParty/expected/README.md @@ -0,0 +1,76 @@ +# expected (commit hash: 3f0ca7b19253129700a073abfa6d8638d9f7c80c) +Single header implementation of `std::expected` with functional-style extensions. + +[![Documentation Status](https://readthedocs.org/projects/tl-docs/badge/?version=latest)](https://tl.tartanllama.xyz/en/latest/?badge=latest) +Clang + GCC: [![Linux Build Status](https://github.com/TartanLlama/expected/actions/workflows/cmake.yml/badge.svg)](https://github.com/TartanLlama/expected/actions/workflows/cmake.yml) +MSVC: [![Windows Build Status](https://ci.appveyor.com/api/projects/status/k5x00xa11y3s5wsg?svg=true)](https://ci.appveyor.com/project/TartanLlama/expected) + +Available on [Vcpkg](https://github.com/microsoft/vcpkg/tree/master/ports/tl-expected) and [Conan](https://github.com/yipdw/conan-tl-expected). + +[`std::expected`](http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2017/p0323r3.pdf) is proposed as the preferred way to represent object which will either have an expected value, or an unexpected value giving information about why something failed. Unfortunately, chaining together many computations which may fail can be verbose, as error-checking code will be mixed in with the actual programming logic. This implementation provides a number of utilities to make coding with `expected` cleaner. + +For example, instead of writing this code: + +```cpp +std::expected get_cute_cat (const image& img) { + auto cropped = crop_to_cat(img); + if (!cropped) { + return cropped; + } + + auto with_tie = add_bow_tie(*cropped); + if (!with_tie) { + return with_tie; + } + + auto with_sparkles = make_eyes_sparkle(*with_tie); + if (!with_sparkles) { + return with_sparkles; + } + + return add_rainbow(make_smaller(*with_sparkles)); +} +``` + +You can do this: + +```cpp +tl::expected get_cute_cat (const image& img) { + return crop_to_cat(img) + .and_then(add_bow_tie) + .and_then(make_eyes_sparkle) + .map(make_smaller) + .map(add_rainbow); +} +``` + +The interface is the same as `std::expected` as proposed in [p0323r3](http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2017/p0323r3.pdf), but the following member functions are also defined. Explicit types are for clarity. + +- `map`: carries out some operation on the stored object if there is one. + * `tl::expected s = exp_string.map(&std::string::size);` +- `map_error`: carries out some operation on the unexpected object if there is one. + * `my_error_code translate_error (std::error_code);` + * `tl::expected s = exp_int.map_error(translate_error);` +- `and_then`: like `map`, but for operations which return a `tl::expected`. + * `tl::expected parse (const std::string& s);` + * `tl::expected exp_ast = exp_string.and_then(parse);` +- `or_else`: calls some function if there is no value stored. + * `exp.or_else([] { throw std::runtime_error{"oh no"}; });` + +p0323r3 specifies calling `.error()` on an expected value, or using the `*` or `->` operators on an unexpected value, to be undefined behaviour. In this implementation it causes an assertion failure. The implementation of assertions can be overridden by defining the macro `TL_ASSERT(boolean_condition)` before #including ; by default, `assert(boolean_condition)` from the `` header is used. Note that correct code would not rely on these assertions. + +### Compiler support + +Tested on: + +- Linux + * clang++ 3.5, 3.6, 3.7, 3.8, 3.9, 4, 5, 6, 7, 8, 9, 10, 11 + * g++ 4.8, 4.9, 5.5, 6.4, 7.5, 8, 9, 10 +- Windows + * MSVC 2015, 2017, 2019, 2022 + +---------- + +[![CC0](http://i.creativecommons.org/p/zero/1.0/88x31.png)]("http://creativecommons.org/publicdomain/zero/1.0/") + +To the extent possible under law, [Sy Brand](https://twitter.com/TartanLlama) has waived all copyright and related or neighboring rights to the `expected` library. This work is published from: United Kingdom. diff --git a/ThirdParty/expected/include/tl/expected.hpp b/ThirdParty/expected/include/tl/expected.hpp new file mode 100644 index 0000000..afee404 --- /dev/null +++ b/ThirdParty/expected/include/tl/expected.hpp @@ -0,0 +1,2444 @@ +/// +// expected - An implementation of std::expected with extensions +// Written in 2017 by Sy Brand (tartanllama@gmail.com, @TartanLlama) +// +// Documentation available at http://tl.tartanllama.xyz/ +// +// To the extent possible under law, the author(s) have dedicated all +// copyright and related and neighboring rights to this software to the +// public domain worldwide. This software is distributed without any warranty. +// +// You should have received a copy of the CC0 Public Domain Dedication +// along with this software. If not, see +// . +/// + +#ifndef TL_EXPECTED_HPP +#define TL_EXPECTED_HPP + +#define TL_EXPECTED_VERSION_MAJOR 1 +#define TL_EXPECTED_VERSION_MINOR 1 +#define TL_EXPECTED_VERSION_PATCH 0 + +#include +#include +#include +#include + +#if defined(__EXCEPTIONS) || defined(_CPPUNWIND) +#define TL_EXPECTED_EXCEPTIONS_ENABLED +#endif + +#if (defined(_MSC_VER) && _MSC_VER == 1900) +#define TL_EXPECTED_MSVC2015 +#define TL_EXPECTED_MSVC2015_CONSTEXPR +#else +#define TL_EXPECTED_MSVC2015_CONSTEXPR constexpr +#endif + +#if (defined(__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ <= 9 && \ + !defined(__clang__)) +#define TL_EXPECTED_GCC49 +#endif + +#if (defined(__GNUC__) && __GNUC__ == 5 && __GNUC_MINOR__ <= 4 && \ + !defined(__clang__)) +#define TL_EXPECTED_GCC54 +#endif + +#if (defined(__GNUC__) && __GNUC__ == 5 && __GNUC_MINOR__ <= 5 && \ + !defined(__clang__)) +#define TL_EXPECTED_GCC55 +#endif + +#if !defined(TL_ASSERT) +//can't have assert in constexpr in C++11 and GCC 4.9 has a compiler bug +#if (__cplusplus > 201103L) && !defined(TL_EXPECTED_GCC49) +#include +#define TL_ASSERT(x) assert(x) +#else +#define TL_ASSERT(x) +#endif +#endif + +#if (defined(__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ <= 9 && \ + !defined(__clang__)) +// GCC < 5 doesn't support overloading on const&& for member functions + +#define TL_EXPECTED_NO_CONSTRR +// GCC < 5 doesn't support some standard C++11 type traits +#define TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(T) \ + std::has_trivial_copy_constructor +#define TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(T) \ + std::has_trivial_copy_assign + +// This one will be different for GCC 5.7 if it's ever supported +#define TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(T) \ + std::is_trivially_destructible + +// GCC 5 < v < 8 has a bug in is_trivially_copy_constructible which breaks +// std::vector for non-copyable types +#elif (defined(__GNUC__) && __GNUC__ < 8 && !defined(__clang__)) +#ifndef TL_GCC_LESS_8_TRIVIALLY_COPY_CONSTRUCTIBLE_MUTEX +#define TL_GCC_LESS_8_TRIVIALLY_COPY_CONSTRUCTIBLE_MUTEX +namespace tl { +namespace detail { +template +struct is_trivially_copy_constructible + : std::is_trivially_copy_constructible {}; +#ifdef _GLIBCXX_VECTOR +template +struct is_trivially_copy_constructible> : std::false_type {}; +#endif +} // namespace detail +} // namespace tl +#endif + +#define TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(T) \ + tl::detail::is_trivially_copy_constructible +#define TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(T) \ + std::is_trivially_copy_assignable +#define TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(T) \ + std::is_trivially_destructible +#else +#define TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(T) \ + std::is_trivially_copy_constructible +#define TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(T) \ + std::is_trivially_copy_assignable +#define TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(T) \ + std::is_trivially_destructible +#endif + +#if __cplusplus > 201103L +#define TL_EXPECTED_CXX14 +#endif + +#ifdef TL_EXPECTED_GCC49 +#define TL_EXPECTED_GCC49_CONSTEXPR +#else +#define TL_EXPECTED_GCC49_CONSTEXPR constexpr +#endif + +#if (__cplusplus == 201103L || defined(TL_EXPECTED_MSVC2015) || \ + defined(TL_EXPECTED_GCC49)) +#define TL_EXPECTED_11_CONSTEXPR +#else +#define TL_EXPECTED_11_CONSTEXPR constexpr +#endif + +namespace tl { +template class expected; + +#ifndef TL_MONOSTATE_INPLACE_MUTEX +#define TL_MONOSTATE_INPLACE_MUTEX +class monostate {}; + +struct in_place_t { + explicit in_place_t() = default; +}; +static constexpr in_place_t in_place{}; +#endif + +template class unexpected { +public: + static_assert(!std::is_same::value, "E must not be void"); + + unexpected() = delete; + constexpr explicit unexpected(const E &e) : m_val(e) {} + + constexpr explicit unexpected(E &&e) : m_val(std::move(e)) {} + + template ::value>::type * = nullptr> + constexpr explicit unexpected(Args &&...args) + : m_val(std::forward(args)...) {} + template < + class U, class... Args, + typename std::enable_if &, Args &&...>::value>::type * = nullptr> + constexpr explicit unexpected(std::initializer_list l, Args &&...args) + : m_val(l, std::forward(args)...) {} + + constexpr const E &value() const & { return m_val; } + TL_EXPECTED_11_CONSTEXPR E &value() & { return m_val; } + TL_EXPECTED_11_CONSTEXPR E &&value() && { return std::move(m_val); } + constexpr const E &&value() const && { return std::move(m_val); } + +private: + E m_val; +}; + +#ifdef __cpp_deduction_guides +template unexpected(E) -> unexpected; +#endif + +template +constexpr bool operator==(const unexpected &lhs, const unexpected &rhs) { + return lhs.value() == rhs.value(); +} +template +constexpr bool operator!=(const unexpected &lhs, const unexpected &rhs) { + return lhs.value() != rhs.value(); +} +template +constexpr bool operator<(const unexpected &lhs, const unexpected &rhs) { + return lhs.value() < rhs.value(); +} +template +constexpr bool operator<=(const unexpected &lhs, const unexpected &rhs) { + return lhs.value() <= rhs.value(); +} +template +constexpr bool operator>(const unexpected &lhs, const unexpected &rhs) { + return lhs.value() > rhs.value(); +} +template +constexpr bool operator>=(const unexpected &lhs, const unexpected &rhs) { + return lhs.value() >= rhs.value(); +} + +template +unexpected::type> make_unexpected(E &&e) { + return unexpected::type>(std::forward(e)); +} + +struct unexpect_t { + unexpect_t() = default; +}; +static constexpr unexpect_t unexpect{}; + +namespace detail { +template +[[noreturn]] TL_EXPECTED_11_CONSTEXPR void throw_exception(E &&e) { +#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED + throw std::forward(e); +#else + (void)e; +#ifdef _MSC_VER + __assume(0); +#else + __builtin_unreachable(); +#endif +#endif +} + +#ifndef TL_TRAITS_MUTEX +#define TL_TRAITS_MUTEX +// C++14-style aliases for brevity +template using remove_const_t = typename std::remove_const::type; +template +using remove_reference_t = typename std::remove_reference::type; +template using decay_t = typename std::decay::type; +template +using enable_if_t = typename std::enable_if::type; +template +using conditional_t = typename std::conditional::type; + +// std::conjunction from C++17 +template struct conjunction : std::true_type {}; +template struct conjunction : B {}; +template +struct conjunction + : std::conditional, B>::type {}; + +#if defined(_LIBCPP_VERSION) && __cplusplus == 201103L +#define TL_TRAITS_LIBCXX_MEM_FN_WORKAROUND +#endif + +// In C++11 mode, there's an issue in libc++'s std::mem_fn +// which results in a hard-error when using it in a noexcept expression +// in some cases. This is a check to workaround the common failing case. +#ifdef TL_TRAITS_LIBCXX_MEM_FN_WORKAROUND +template +struct is_pointer_to_non_const_member_func : std::false_type {}; +template +struct is_pointer_to_non_const_member_func + : std::true_type {}; +template +struct is_pointer_to_non_const_member_func + : std::true_type {}; +template +struct is_pointer_to_non_const_member_func + : std::true_type {}; +template +struct is_pointer_to_non_const_member_func + : std::true_type {}; +template +struct is_pointer_to_non_const_member_func + : std::true_type {}; +template +struct is_pointer_to_non_const_member_func + : std::true_type {}; + +template struct is_const_or_const_ref : std::false_type {}; +template struct is_const_or_const_ref : std::true_type {}; +template struct is_const_or_const_ref : std::true_type {}; +#endif + +// std::invoke from C++17 +// https://stackoverflow.com/questions/38288042/c11-14-invoke-workaround +template < + typename Fn, typename... Args, +#ifdef TL_TRAITS_LIBCXX_MEM_FN_WORKAROUND + typename = enable_if_t::value && + is_const_or_const_ref::value)>, +#endif + typename = enable_if_t>::value>, int = 0> +constexpr auto invoke(Fn &&f, Args &&...args) noexcept( + noexcept(std::mem_fn(f)(std::forward(args)...))) + -> decltype(std::mem_fn(f)(std::forward(args)...)) { + return std::mem_fn(f)(std::forward(args)...); +} + +template >::value>> +constexpr auto invoke(Fn &&f, Args &&...args) noexcept( + noexcept(std::forward(f)(std::forward(args)...))) + -> decltype(std::forward(f)(std::forward(args)...)) { + return std::forward(f)(std::forward(args)...); +} + +// std::invoke_result from C++17 +template struct invoke_result_impl; + +template +struct invoke_result_impl< + F, + decltype(detail::invoke(std::declval(), std::declval()...), void()), + Us...> { + using type = + decltype(detail::invoke(std::declval(), std::declval()...)); +}; + +template +using invoke_result = invoke_result_impl; + +template +using invoke_result_t = typename invoke_result::type; + +#if defined(_MSC_VER) && _MSC_VER <= 1900 +// TODO make a version which works with MSVC 2015 +template struct is_swappable : std::true_type {}; + +template struct is_nothrow_swappable : std::true_type {}; +#else +// https://stackoverflow.com/questions/26744589/what-is-a-proper-way-to-implement-is-swappable-to-test-for-the-swappable-concept +namespace swap_adl_tests { +// if swap ADL finds this then it would call std::swap otherwise (same +// signature) +struct tag {}; + +template tag swap(T &, T &); +template tag swap(T (&a)[N], T (&b)[N]); + +// helper functions to test if an unqualified swap is possible, and if it +// becomes std::swap +template std::false_type can_swap(...) noexcept(false); +template (), std::declval()))> +std::true_type can_swap(int) noexcept(noexcept(swap(std::declval(), + std::declval()))); + +template std::false_type uses_std(...); +template +std::is_same(), std::declval())), tag> +uses_std(int); + +template +struct is_std_swap_noexcept + : std::integral_constant::value && + std::is_nothrow_move_assignable::value> {}; + +template +struct is_std_swap_noexcept : is_std_swap_noexcept {}; + +template +struct is_adl_swap_noexcept + : std::integral_constant(0))> {}; +} // namespace swap_adl_tests + +template +struct is_swappable + : std::integral_constant< + bool, + decltype(detail::swap_adl_tests::can_swap(0))::value && + (!decltype(detail::swap_adl_tests::uses_std(0))::value || + (std::is_move_assignable::value && + std::is_move_constructible::value))> {}; + +template +struct is_swappable + : std::integral_constant< + bool, + decltype(detail::swap_adl_tests::can_swap(0))::value && + (!decltype(detail::swap_adl_tests::uses_std( + 0))::value || + is_swappable::value)> {}; + +template +struct is_nothrow_swappable + : std::integral_constant< + bool, + is_swappable::value && + ((decltype(detail::swap_adl_tests::uses_std(0))::value && + detail::swap_adl_tests::is_std_swap_noexcept::value) || + (!decltype(detail::swap_adl_tests::uses_std(0))::value && + detail::swap_adl_tests::is_adl_swap_noexcept::value))> {}; +#endif +#endif + +// Trait for checking if a type is a tl::expected +template struct is_expected_impl : std::false_type {}; +template +struct is_expected_impl> : std::true_type {}; +template using is_expected = is_expected_impl>; + +template +using expected_enable_forward_value = detail::enable_if_t< + std::is_constructible::value && + !std::is_same, in_place_t>::value && + !std::is_same, detail::decay_t>::value && + !std::is_same, detail::decay_t>::value>; + +template +using expected_enable_from_other = detail::enable_if_t< + std::is_constructible::value && + std::is_constructible::value && + !std::is_constructible &>::value && + !std::is_constructible &&>::value && + !std::is_constructible &>::value && + !std::is_constructible &&>::value && + !std::is_convertible &, T>::value && + !std::is_convertible &&, T>::value && + !std::is_convertible &, T>::value && + !std::is_convertible &&, T>::value>; + +template +using is_void_or = conditional_t::value, std::true_type, U>; + +template +using is_copy_constructible_or_void = + is_void_or>; + +template +using is_move_constructible_or_void = + is_void_or>; + +template +using is_copy_assignable_or_void = is_void_or>; + +template +using is_move_assignable_or_void = is_void_or>; + +} // namespace detail + +namespace detail { +struct no_init_t {}; +static constexpr no_init_t no_init{}; + +// Implements the storage of the values, and ensures that the destructor is +// trivial if it can be. +// +// This specialization is for where neither `T` or `E` is trivially +// destructible, so the destructors must be called on destruction of the +// `expected` +template ::value, + bool = std::is_trivially_destructible::value> +struct expected_storage_base { + constexpr expected_storage_base() : m_val(T{}), m_has_val(true) {} + constexpr expected_storage_base(no_init_t) : m_no_init(), m_has_val(false) {} + + template ::value> * = + nullptr> + constexpr expected_storage_base(in_place_t, Args &&...args) + : m_val(std::forward(args)...), m_has_val(true) {} + + template &, Args &&...>::value> * = nullptr> + constexpr expected_storage_base(in_place_t, std::initializer_list il, + Args &&...args) + : m_val(il, std::forward(args)...), m_has_val(true) {} + template ::value> * = + nullptr> + constexpr explicit expected_storage_base(unexpect_t, Args &&...args) + : m_unexpect(std::forward(args)...), m_has_val(false) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected_storage_base(unexpect_t, + std::initializer_list il, + Args &&...args) + : m_unexpect(il, std::forward(args)...), m_has_val(false) {} + + ~expected_storage_base() { + if (m_has_val) { + m_val.~T(); + } else { + m_unexpect.~unexpected(); + } + } + union { + T m_val; + unexpected m_unexpect; + char m_no_init; + }; + bool m_has_val; +}; + +// This specialization is for when both `T` and `E` are trivially-destructible, +// so the destructor of the `expected` can be trivial. +template struct expected_storage_base { + constexpr expected_storage_base() : m_val(T{}), m_has_val(true) {} + constexpr expected_storage_base(no_init_t) : m_no_init(), m_has_val(false) {} + + template ::value> * = + nullptr> + constexpr expected_storage_base(in_place_t, Args &&...args) + : m_val(std::forward(args)...), m_has_val(true) {} + + template &, Args &&...>::value> * = nullptr> + constexpr expected_storage_base(in_place_t, std::initializer_list il, + Args &&...args) + : m_val(il, std::forward(args)...), m_has_val(true) {} + template ::value> * = + nullptr> + constexpr explicit expected_storage_base(unexpect_t, Args &&...args) + : m_unexpect(std::forward(args)...), m_has_val(false) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected_storage_base(unexpect_t, + std::initializer_list il, + Args &&...args) + : m_unexpect(il, std::forward(args)...), m_has_val(false) {} + + ~expected_storage_base() = default; + union { + T m_val; + unexpected m_unexpect; + char m_no_init; + }; + bool m_has_val; +}; + +// T is trivial, E is not. +template struct expected_storage_base { + constexpr expected_storage_base() : m_val(T{}), m_has_val(true) {} + TL_EXPECTED_MSVC2015_CONSTEXPR expected_storage_base(no_init_t) + : m_no_init(), m_has_val(false) {} + + template ::value> * = + nullptr> + constexpr expected_storage_base(in_place_t, Args &&...args) + : m_val(std::forward(args)...), m_has_val(true) {} + + template &, Args &&...>::value> * = nullptr> + constexpr expected_storage_base(in_place_t, std::initializer_list il, + Args &&...args) + : m_val(il, std::forward(args)...), m_has_val(true) {} + template ::value> * = + nullptr> + constexpr explicit expected_storage_base(unexpect_t, Args &&...args) + : m_unexpect(std::forward(args)...), m_has_val(false) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected_storage_base(unexpect_t, + std::initializer_list il, + Args &&...args) + : m_unexpect(il, std::forward(args)...), m_has_val(false) {} + + ~expected_storage_base() { + if (!m_has_val) { + m_unexpect.~unexpected(); + } + } + + union { + T m_val; + unexpected m_unexpect; + char m_no_init; + }; + bool m_has_val; +}; + +// E is trivial, T is not. +template struct expected_storage_base { + constexpr expected_storage_base() : m_val(T{}), m_has_val(true) {} + constexpr expected_storage_base(no_init_t) : m_no_init(), m_has_val(false) {} + + template ::value> * = + nullptr> + constexpr expected_storage_base(in_place_t, Args &&...args) + : m_val(std::forward(args)...), m_has_val(true) {} + + template &, Args &&...>::value> * = nullptr> + constexpr expected_storage_base(in_place_t, std::initializer_list il, + Args &&...args) + : m_val(il, std::forward(args)...), m_has_val(true) {} + template ::value> * = + nullptr> + constexpr explicit expected_storage_base(unexpect_t, Args &&...args) + : m_unexpect(std::forward(args)...), m_has_val(false) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected_storage_base(unexpect_t, + std::initializer_list il, + Args &&...args) + : m_unexpect(il, std::forward(args)...), m_has_val(false) {} + + ~expected_storage_base() { + if (m_has_val) { + m_val.~T(); + } + } + union { + T m_val; + unexpected m_unexpect; + char m_no_init; + }; + bool m_has_val; +}; + +// `T` is `void`, `E` is trivially-destructible +template struct expected_storage_base { + #if __GNUC__ <= 5 + //no constexpr for GCC 4/5 bug + #else + TL_EXPECTED_MSVC2015_CONSTEXPR + #endif + expected_storage_base() : m_has_val(true) {} + + constexpr expected_storage_base(no_init_t) : m_val(), m_has_val(false) {} + + constexpr expected_storage_base(in_place_t) : m_has_val(true) {} + + template ::value> * = + nullptr> + constexpr explicit expected_storage_base(unexpect_t, Args &&...args) + : m_unexpect(std::forward(args)...), m_has_val(false) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected_storage_base(unexpect_t, + std::initializer_list il, + Args &&...args) + : m_unexpect(il, std::forward(args)...), m_has_val(false) {} + + ~expected_storage_base() = default; + struct dummy {}; + union { + unexpected m_unexpect; + dummy m_val; + }; + bool m_has_val; +}; + +// `T` is `void`, `E` is not trivially-destructible +template struct expected_storage_base { + constexpr expected_storage_base() : m_dummy(), m_has_val(true) {} + constexpr expected_storage_base(no_init_t) : m_dummy(), m_has_val(false) {} + + constexpr expected_storage_base(in_place_t) : m_dummy(), m_has_val(true) {} + + template ::value> * = + nullptr> + constexpr explicit expected_storage_base(unexpect_t, Args &&...args) + : m_unexpect(std::forward(args)...), m_has_val(false) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected_storage_base(unexpect_t, + std::initializer_list il, + Args &&...args) + : m_unexpect(il, std::forward(args)...), m_has_val(false) {} + + ~expected_storage_base() { + if (!m_has_val) { + m_unexpect.~unexpected(); + } + } + + union { + unexpected m_unexpect; + char m_dummy; + }; + bool m_has_val; +}; + +// This base class provides some handy member functions which can be used in +// further derived classes +template +struct expected_operations_base : expected_storage_base { + using expected_storage_base::expected_storage_base; + + template void construct(Args &&...args) noexcept { + new (std::addressof(this->m_val)) T(std::forward(args)...); + this->m_has_val = true; + } + + template void construct_with(Rhs &&rhs) noexcept { + new (std::addressof(this->m_val)) T(std::forward(rhs).get()); + this->m_has_val = true; + } + + template void construct_error(Args &&...args) noexcept { + new (std::addressof(this->m_unexpect)) + unexpected(std::forward(args)...); + this->m_has_val = false; + } + +#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED + + // These assign overloads ensure that the most efficient assignment + // implementation is used while maintaining the strong exception guarantee. + // The problematic case is where rhs has a value, but *this does not. + // + // This overload handles the case where we can just copy-construct `T` + // directly into place without throwing. + template ::value> + * = nullptr> + void assign(const expected_operations_base &rhs) noexcept { + if (!this->m_has_val && rhs.m_has_val) { + geterr().~unexpected(); + construct(rhs.get()); + } else { + assign_common(rhs); + } + } + + // This overload handles the case where we can attempt to create a copy of + // `T`, then no-throw move it into place if the copy was successful. + template ::value && + std::is_nothrow_move_constructible::value> + * = nullptr> + void assign(const expected_operations_base &rhs) noexcept { + if (!this->m_has_val && rhs.m_has_val) { + T tmp = rhs.get(); + geterr().~unexpected(); + construct(std::move(tmp)); + } else { + assign_common(rhs); + } + } + + // This overload is the worst-case, where we have to move-construct the + // unexpected value into temporary storage, then try to copy the T into place. + // If the construction succeeds, then everything is fine, but if it throws, + // then we move the old unexpected value back into place before rethrowing the + // exception. + template ::value && + !std::is_nothrow_move_constructible::value> + * = nullptr> + void assign(const expected_operations_base &rhs) { + if (!this->m_has_val && rhs.m_has_val) { + auto tmp = std::move(geterr()); + geterr().~unexpected(); + +#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED + try { + construct(rhs.get()); + } catch (...) { + geterr() = std::move(tmp); + throw; + } +#else + construct(rhs.get()); +#endif + } else { + assign_common(rhs); + } + } + + // These overloads do the same as above, but for rvalues + template ::value> + * = nullptr> + void assign(expected_operations_base &&rhs) noexcept { + if (!this->m_has_val && rhs.m_has_val) { + geterr().~unexpected(); + construct(std::move(rhs).get()); + } else { + assign_common(std::move(rhs)); + } + } + + template ::value> + * = nullptr> + void assign(expected_operations_base &&rhs) { + if (!this->m_has_val && rhs.m_has_val) { + auto tmp = std::move(geterr()); + geterr().~unexpected(); +#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED + try { + construct(std::move(rhs).get()); + } catch (...) { + geterr() = std::move(tmp); + throw; + } +#else + construct(std::move(rhs).get()); +#endif + } else { + assign_common(std::move(rhs)); + } + } + +#else + + // If exceptions are disabled then we can just copy-construct + void assign(const expected_operations_base &rhs) noexcept { + if (!this->m_has_val && rhs.m_has_val) { + geterr().~unexpected(); + construct(rhs.get()); + } else { + assign_common(rhs); + } + } + + void assign(expected_operations_base &&rhs) noexcept { + if (!this->m_has_val && rhs.m_has_val) { + geterr().~unexpected(); + construct(std::move(rhs).get()); + } else { + assign_common(std::move(rhs)); + } + } + +#endif + + // The common part of move/copy assigning + template void assign_common(Rhs &&rhs) { + if (this->m_has_val) { + if (rhs.m_has_val) { + get() = std::forward(rhs).get(); + } else { + destroy_val(); + construct_error(std::forward(rhs).geterr()); + } + } else { + if (!rhs.m_has_val) { + geterr() = std::forward(rhs).geterr(); + } + } + } + + bool has_value() const { return this->m_has_val; } + + TL_EXPECTED_11_CONSTEXPR T &get() & { return this->m_val; } + constexpr const T &get() const & { return this->m_val; } + TL_EXPECTED_11_CONSTEXPR T &&get() && { return std::move(this->m_val); } +#ifndef TL_EXPECTED_NO_CONSTRR + constexpr const T &&get() const && { return std::move(this->m_val); } +#endif + + TL_EXPECTED_11_CONSTEXPR unexpected &geterr() & { + return this->m_unexpect; + } + constexpr const unexpected &geterr() const & { return this->m_unexpect; } + TL_EXPECTED_11_CONSTEXPR unexpected &&geterr() && { + return std::move(this->m_unexpect); + } +#ifndef TL_EXPECTED_NO_CONSTRR + constexpr const unexpected &&geterr() const && { + return std::move(this->m_unexpect); + } +#endif + + TL_EXPECTED_11_CONSTEXPR void destroy_val() { get().~T(); } +}; + +// This base class provides some handy member functions which can be used in +// further derived classes +template +struct expected_operations_base : expected_storage_base { + using expected_storage_base::expected_storage_base; + + template void construct() noexcept { this->m_has_val = true; } + + // This function doesn't use its argument, but needs it so that code in + // levels above this can work independently of whether T is void + template void construct_with(Rhs &&) noexcept { + this->m_has_val = true; + } + + template void construct_error(Args &&...args) noexcept { + new (std::addressof(this->m_unexpect)) + unexpected(std::forward(args)...); + this->m_has_val = false; + } + + template void assign(Rhs &&rhs) noexcept { + if (!this->m_has_val) { + if (rhs.m_has_val) { + geterr().~unexpected(); + construct(); + } else { + geterr() = std::forward(rhs).geterr(); + } + } else { + if (!rhs.m_has_val) { + construct_error(std::forward(rhs).geterr()); + } + } + } + + bool has_value() const { return this->m_has_val; } + + TL_EXPECTED_11_CONSTEXPR unexpected &geterr() & { + return this->m_unexpect; + } + constexpr const unexpected &geterr() const & { return this->m_unexpect; } + TL_EXPECTED_11_CONSTEXPR unexpected &&geterr() && { + return std::move(this->m_unexpect); + } +#ifndef TL_EXPECTED_NO_CONSTRR + constexpr const unexpected &&geterr() const && { + return std::move(this->m_unexpect); + } +#endif + + TL_EXPECTED_11_CONSTEXPR void destroy_val() { + // no-op + } +}; + +// This class manages conditionally having a trivial copy constructor +// This specialization is for when T and E are trivially copy constructible +template :: + value &&TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(E)::value> +struct expected_copy_base : expected_operations_base { + using expected_operations_base::expected_operations_base; +}; + +// This specialization is for when T or E are not trivially copy constructible +template +struct expected_copy_base : expected_operations_base { + using expected_operations_base::expected_operations_base; + + expected_copy_base() = default; + expected_copy_base(const expected_copy_base &rhs) + : expected_operations_base(no_init) { + if (rhs.has_value()) { + this->construct_with(rhs); + } else { + this->construct_error(rhs.geterr()); + } + } + + expected_copy_base(expected_copy_base &&rhs) = default; + expected_copy_base &operator=(const expected_copy_base &rhs) = default; + expected_copy_base &operator=(expected_copy_base &&rhs) = default; +}; + +// This class manages conditionally having a trivial move constructor +// Unfortunately there's no way to achieve this in GCC < 5 AFAIK, since it +// doesn't implement an analogue to std::is_trivially_move_constructible. We +// have to make do with a non-trivial move constructor even if T is trivially +// move constructible +#ifndef TL_EXPECTED_GCC49 +template >::value + &&std::is_trivially_move_constructible::value> +struct expected_move_base : expected_copy_base { + using expected_copy_base::expected_copy_base; +}; +#else +template struct expected_move_base; +#endif +template +struct expected_move_base : expected_copy_base { + using expected_copy_base::expected_copy_base; + + expected_move_base() = default; + expected_move_base(const expected_move_base &rhs) = default; + + expected_move_base(expected_move_base &&rhs) noexcept( + std::is_nothrow_move_constructible::value) + : expected_copy_base(no_init) { + if (rhs.has_value()) { + this->construct_with(std::move(rhs)); + } else { + this->construct_error(std::move(rhs.geterr())); + } + } + expected_move_base &operator=(const expected_move_base &rhs) = default; + expected_move_base &operator=(expected_move_base &&rhs) = default; +}; + +// This class manages conditionally having a trivial copy assignment operator +template >::value + &&TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(E)::value + &&TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(E)::value + &&TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(E)::value> +struct expected_copy_assign_base : expected_move_base { + using expected_move_base::expected_move_base; +}; + +template +struct expected_copy_assign_base : expected_move_base { + using expected_move_base::expected_move_base; + + expected_copy_assign_base() = default; + expected_copy_assign_base(const expected_copy_assign_base &rhs) = default; + + expected_copy_assign_base(expected_copy_assign_base &&rhs) = default; + expected_copy_assign_base &operator=(const expected_copy_assign_base &rhs) { + this->assign(rhs); + return *this; + } + expected_copy_assign_base & + operator=(expected_copy_assign_base &&rhs) = default; +}; + +// This class manages conditionally having a trivial move assignment operator +// Unfortunately there's no way to achieve this in GCC < 5 AFAIK, since it +// doesn't implement an analogue to std::is_trivially_move_assignable. We have +// to make do with a non-trivial move assignment operator even if T is trivially +// move assignable +#ifndef TL_EXPECTED_GCC49 +template , + std::is_trivially_move_constructible, + std::is_trivially_move_assignable>>:: + value &&std::is_trivially_destructible::value + &&std::is_trivially_move_constructible::value + &&std::is_trivially_move_assignable::value> +struct expected_move_assign_base : expected_copy_assign_base { + using expected_copy_assign_base::expected_copy_assign_base; +}; +#else +template struct expected_move_assign_base; +#endif + +template +struct expected_move_assign_base + : expected_copy_assign_base { + using expected_copy_assign_base::expected_copy_assign_base; + + expected_move_assign_base() = default; + expected_move_assign_base(const expected_move_assign_base &rhs) = default; + + expected_move_assign_base(expected_move_assign_base &&rhs) = default; + + expected_move_assign_base & + operator=(const expected_move_assign_base &rhs) = default; + + expected_move_assign_base & + operator=(expected_move_assign_base &&rhs) noexcept( + std::is_nothrow_move_constructible::value + &&std::is_nothrow_move_assignable::value) { + this->assign(std::move(rhs)); + return *this; + } +}; + +// expected_delete_ctor_base will conditionally delete copy and move +// constructors depending on whether T is copy/move constructible +template ::value && + std::is_copy_constructible::value), + bool EnableMove = (is_move_constructible_or_void::value && + std::is_move_constructible::value)> +struct expected_delete_ctor_base { + expected_delete_ctor_base() = default; + expected_delete_ctor_base(const expected_delete_ctor_base &) = default; + expected_delete_ctor_base(expected_delete_ctor_base &&) noexcept = default; + expected_delete_ctor_base & + operator=(const expected_delete_ctor_base &) = default; + expected_delete_ctor_base & + operator=(expected_delete_ctor_base &&) noexcept = default; +}; + +template +struct expected_delete_ctor_base { + expected_delete_ctor_base() = default; + expected_delete_ctor_base(const expected_delete_ctor_base &) = default; + expected_delete_ctor_base(expected_delete_ctor_base &&) noexcept = delete; + expected_delete_ctor_base & + operator=(const expected_delete_ctor_base &) = default; + expected_delete_ctor_base & + operator=(expected_delete_ctor_base &&) noexcept = default; +}; + +template +struct expected_delete_ctor_base { + expected_delete_ctor_base() = default; + expected_delete_ctor_base(const expected_delete_ctor_base &) = delete; + expected_delete_ctor_base(expected_delete_ctor_base &&) noexcept = default; + expected_delete_ctor_base & + operator=(const expected_delete_ctor_base &) = default; + expected_delete_ctor_base & + operator=(expected_delete_ctor_base &&) noexcept = default; +}; + +template +struct expected_delete_ctor_base { + expected_delete_ctor_base() = default; + expected_delete_ctor_base(const expected_delete_ctor_base &) = delete; + expected_delete_ctor_base(expected_delete_ctor_base &&) noexcept = delete; + expected_delete_ctor_base & + operator=(const expected_delete_ctor_base &) = default; + expected_delete_ctor_base & + operator=(expected_delete_ctor_base &&) noexcept = default; +}; + +// expected_delete_assign_base will conditionally delete copy and move +// constructors depending on whether T and E are copy/move constructible + +// assignable +template ::value && + std::is_copy_constructible::value && + is_copy_assignable_or_void::value && + std::is_copy_assignable::value), + bool EnableMove = (is_move_constructible_or_void::value && + std::is_move_constructible::value && + is_move_assignable_or_void::value && + std::is_move_assignable::value)> +struct expected_delete_assign_base { + expected_delete_assign_base() = default; + expected_delete_assign_base(const expected_delete_assign_base &) = default; + expected_delete_assign_base(expected_delete_assign_base &&) noexcept = + default; + expected_delete_assign_base & + operator=(const expected_delete_assign_base &) = default; + expected_delete_assign_base & + operator=(expected_delete_assign_base &&) noexcept = default; +}; + +template +struct expected_delete_assign_base { + expected_delete_assign_base() = default; + expected_delete_assign_base(const expected_delete_assign_base &) = default; + expected_delete_assign_base(expected_delete_assign_base &&) noexcept = + default; + expected_delete_assign_base & + operator=(const expected_delete_assign_base &) = default; + expected_delete_assign_base & + operator=(expected_delete_assign_base &&) noexcept = delete; +}; + +template +struct expected_delete_assign_base { + expected_delete_assign_base() = default; + expected_delete_assign_base(const expected_delete_assign_base &) = default; + expected_delete_assign_base(expected_delete_assign_base &&) noexcept = + default; + expected_delete_assign_base & + operator=(const expected_delete_assign_base &) = delete; + expected_delete_assign_base & + operator=(expected_delete_assign_base &&) noexcept = default; +}; + +template +struct expected_delete_assign_base { + expected_delete_assign_base() = default; + expected_delete_assign_base(const expected_delete_assign_base &) = default; + expected_delete_assign_base(expected_delete_assign_base &&) noexcept = + default; + expected_delete_assign_base & + operator=(const expected_delete_assign_base &) = delete; + expected_delete_assign_base & + operator=(expected_delete_assign_base &&) noexcept = delete; +}; + +// This is needed to be able to construct the expected_default_ctor_base which +// follows, while still conditionally deleting the default constructor. +struct default_constructor_tag { + explicit constexpr default_constructor_tag() = default; +}; + +// expected_default_ctor_base will ensure that expected has a deleted default +// consturctor if T is not default constructible. +// This specialization is for when T is default constructible +template ::value || std::is_void::value> +struct expected_default_ctor_base { + constexpr expected_default_ctor_base() noexcept = default; + constexpr expected_default_ctor_base( + expected_default_ctor_base const &) noexcept = default; + constexpr expected_default_ctor_base(expected_default_ctor_base &&) noexcept = + default; + expected_default_ctor_base & + operator=(expected_default_ctor_base const &) noexcept = default; + expected_default_ctor_base & + operator=(expected_default_ctor_base &&) noexcept = default; + + constexpr explicit expected_default_ctor_base(default_constructor_tag) {} +}; + +// This specialization is for when T is not default constructible +template struct expected_default_ctor_base { + constexpr expected_default_ctor_base() noexcept = delete; + constexpr expected_default_ctor_base( + expected_default_ctor_base const &) noexcept = default; + constexpr expected_default_ctor_base(expected_default_ctor_base &&) noexcept = + default; + expected_default_ctor_base & + operator=(expected_default_ctor_base const &) noexcept = default; + expected_default_ctor_base & + operator=(expected_default_ctor_base &&) noexcept = default; + + constexpr explicit expected_default_ctor_base(default_constructor_tag) {} +}; +} // namespace detail + +template class bad_expected_access : public std::exception { +public: + explicit bad_expected_access(E e) : m_val(std::move(e)) {} + + virtual const char *what() const noexcept override { + return "Bad expected access"; + } + + const E &error() const & { return m_val; } + E &error() & { return m_val; } + const E &&error() const && { return std::move(m_val); } + E &&error() && { return std::move(m_val); } + +private: + E m_val; +}; + +/// An `expected` object is an object that contains the storage for +/// another object and manages the lifetime of this contained object `T`. +/// Alternatively it could contain the storage for another unexpected object +/// `E`. The contained object may not be initialized after the expected object +/// has been initialized, and may not be destroyed before the expected object +/// has been destroyed. The initialization state of the contained object is +/// tracked by the expected object. +template +class expected : private detail::expected_move_assign_base, + private detail::expected_delete_ctor_base, + private detail::expected_delete_assign_base, + private detail::expected_default_ctor_base { + static_assert(!std::is_reference::value, "T must not be a reference"); + static_assert(!std::is_same::type>::value, + "T must not be in_place_t"); + static_assert(!std::is_same::type>::value, + "T must not be unexpect_t"); + static_assert( + !std::is_same>::type>::value, + "T must not be unexpected"); + static_assert(!std::is_reference::value, "E must not be a reference"); + + T *valptr() { return std::addressof(this->m_val); } + const T *valptr() const { return std::addressof(this->m_val); } + unexpected *errptr() { return std::addressof(this->m_unexpect); } + const unexpected *errptr() const { + return std::addressof(this->m_unexpect); + } + + template ::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR U &val() { + return this->m_val; + } + TL_EXPECTED_11_CONSTEXPR unexpected &err() { return this->m_unexpect; } + + template ::value> * = nullptr> + constexpr const U &val() const { + return this->m_val; + } + constexpr const unexpected &err() const { return this->m_unexpect; } + + using impl_base = detail::expected_move_assign_base; + using ctor_base = detail::expected_default_ctor_base; + +public: + typedef T value_type; + typedef E error_type; + typedef unexpected unexpected_type; + +#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ + !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) + template TL_EXPECTED_11_CONSTEXPR auto and_then(F &&f) & { + return and_then_impl(*this, std::forward(f)); + } + template TL_EXPECTED_11_CONSTEXPR auto and_then(F &&f) && { + return and_then_impl(std::move(*this), std::forward(f)); + } + template constexpr auto and_then(F &&f) const & { + return and_then_impl(*this, std::forward(f)); + } + +#ifndef TL_EXPECTED_NO_CONSTRR + template constexpr auto and_then(F &&f) const && { + return and_then_impl(std::move(*this), std::forward(f)); + } +#endif + +#else + template + TL_EXPECTED_11_CONSTEXPR auto + and_then(F &&f) & -> decltype(and_then_impl(std::declval(), + std::forward(f))) { + return and_then_impl(*this, std::forward(f)); + } + template + TL_EXPECTED_11_CONSTEXPR auto + and_then(F &&f) && -> decltype(and_then_impl(std::declval(), + std::forward(f))) { + return and_then_impl(std::move(*this), std::forward(f)); + } + template + constexpr auto and_then(F &&f) const & -> decltype(and_then_impl( + std::declval(), std::forward(f))) { + return and_then_impl(*this, std::forward(f)); + } + +#ifndef TL_EXPECTED_NO_CONSTRR + template + constexpr auto and_then(F &&f) const && -> decltype(and_then_impl( + std::declval(), std::forward(f))) { + return and_then_impl(std::move(*this), std::forward(f)); + } +#endif +#endif + +#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ + !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) + template TL_EXPECTED_11_CONSTEXPR auto map(F &&f) & { + return expected_map_impl(*this, std::forward(f)); + } + template TL_EXPECTED_11_CONSTEXPR auto map(F &&f) && { + return expected_map_impl(std::move(*this), std::forward(f)); + } + template constexpr auto map(F &&f) const & { + return expected_map_impl(*this, std::forward(f)); + } + template constexpr auto map(F &&f) const && { + return expected_map_impl(std::move(*this), std::forward(f)); + } +#else + template + TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl( + std::declval(), std::declval())) + map(F &&f) & { + return expected_map_impl(*this, std::forward(f)); + } + template + TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl(std::declval(), + std::declval())) + map(F &&f) && { + return expected_map_impl(std::move(*this), std::forward(f)); + } + template + constexpr decltype(expected_map_impl(std::declval(), + std::declval())) + map(F &&f) const & { + return expected_map_impl(*this, std::forward(f)); + } + +#ifndef TL_EXPECTED_NO_CONSTRR + template + constexpr decltype(expected_map_impl(std::declval(), + std::declval())) + map(F &&f) const && { + return expected_map_impl(std::move(*this), std::forward(f)); + } +#endif +#endif + +#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ + !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) + template TL_EXPECTED_11_CONSTEXPR auto transform(F &&f) & { + return expected_map_impl(*this, std::forward(f)); + } + template TL_EXPECTED_11_CONSTEXPR auto transform(F &&f) && { + return expected_map_impl(std::move(*this), std::forward(f)); + } + template constexpr auto transform(F &&f) const & { + return expected_map_impl(*this, std::forward(f)); + } + template constexpr auto transform(F &&f) const && { + return expected_map_impl(std::move(*this), std::forward(f)); + } +#else + template + TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl( + std::declval(), std::declval())) + transform(F &&f) & { + return expected_map_impl(*this, std::forward(f)); + } + template + TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl(std::declval(), + std::declval())) + transform(F &&f) && { + return expected_map_impl(std::move(*this), std::forward(f)); + } + template + constexpr decltype(expected_map_impl(std::declval(), + std::declval())) + transform(F &&f) const & { + return expected_map_impl(*this, std::forward(f)); + } + +#ifndef TL_EXPECTED_NO_CONSTRR + template + constexpr decltype(expected_map_impl(std::declval(), + std::declval())) + transform(F &&f) const && { + return expected_map_impl(std::move(*this), std::forward(f)); + } +#endif +#endif + +#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ + !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) + template TL_EXPECTED_11_CONSTEXPR auto map_error(F &&f) & { + return map_error_impl(*this, std::forward(f)); + } + template TL_EXPECTED_11_CONSTEXPR auto map_error(F &&f) && { + return map_error_impl(std::move(*this), std::forward(f)); + } + template constexpr auto map_error(F &&f) const & { + return map_error_impl(*this, std::forward(f)); + } + template constexpr auto map_error(F &&f) const && { + return map_error_impl(std::move(*this), std::forward(f)); + } +#else + template + TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl(std::declval(), + std::declval())) + map_error(F &&f) & { + return map_error_impl(*this, std::forward(f)); + } + template + TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl(std::declval(), + std::declval())) + map_error(F &&f) && { + return map_error_impl(std::move(*this), std::forward(f)); + } + template + constexpr decltype(map_error_impl(std::declval(), + std::declval())) + map_error(F &&f) const & { + return map_error_impl(*this, std::forward(f)); + } + +#ifndef TL_EXPECTED_NO_CONSTRR + template + constexpr decltype(map_error_impl(std::declval(), + std::declval())) + map_error(F &&f) const && { + return map_error_impl(std::move(*this), std::forward(f)); + } +#endif +#endif +#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ + !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) + template TL_EXPECTED_11_CONSTEXPR auto transform_error(F &&f) & { + return map_error_impl(*this, std::forward(f)); + } + template TL_EXPECTED_11_CONSTEXPR auto transform_error(F &&f) && { + return map_error_impl(std::move(*this), std::forward(f)); + } + template constexpr auto transform_error(F &&f) const & { + return map_error_impl(*this, std::forward(f)); + } + template constexpr auto transform_error(F &&f) const && { + return map_error_impl(std::move(*this), std::forward(f)); + } +#else + template + TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl(std::declval(), + std::declval())) + transform_error(F &&f) & { + return map_error_impl(*this, std::forward(f)); + } + template + TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl(std::declval(), + std::declval())) + transform_error(F &&f) && { + return map_error_impl(std::move(*this), std::forward(f)); + } + template + constexpr decltype(map_error_impl(std::declval(), + std::declval())) + transform_error(F &&f) const & { + return map_error_impl(*this, std::forward(f)); + } + +#ifndef TL_EXPECTED_NO_CONSTRR + template + constexpr decltype(map_error_impl(std::declval(), + std::declval())) + transform_error(F &&f) const && { + return map_error_impl(std::move(*this), std::forward(f)); + } +#endif +#endif + template expected TL_EXPECTED_11_CONSTEXPR or_else(F &&f) & { + return or_else_impl(*this, std::forward(f)); + } + + template expected TL_EXPECTED_11_CONSTEXPR or_else(F &&f) && { + return or_else_impl(std::move(*this), std::forward(f)); + } + + template expected constexpr or_else(F &&f) const & { + return or_else_impl(*this, std::forward(f)); + } + +#ifndef TL_EXPECTED_NO_CONSTRR + template expected constexpr or_else(F &&f) const && { + return or_else_impl(std::move(*this), std::forward(f)); + } +#endif + constexpr expected() = default; + constexpr expected(const expected &rhs) = default; + constexpr expected(expected &&rhs) = default; + expected &operator=(const expected &rhs) = default; + expected &operator=(expected &&rhs) = default; + + template ::value> * = + nullptr> + constexpr expected(in_place_t, Args &&...args) + : impl_base(in_place, std::forward(args)...), + ctor_base(detail::default_constructor_tag{}) {} + + template &, Args &&...>::value> * = nullptr> + constexpr expected(in_place_t, std::initializer_list il, Args &&...args) + : impl_base(in_place, il, std::forward(args)...), + ctor_base(detail::default_constructor_tag{}) {} + + template ::value> * = + nullptr, + detail::enable_if_t::value> * = + nullptr> + explicit constexpr expected(const unexpected &e) + : impl_base(unexpect, e.value()), + ctor_base(detail::default_constructor_tag{}) {} + + template < + class G = E, + detail::enable_if_t::value> * = + nullptr, + detail::enable_if_t::value> * = nullptr> + constexpr expected(unexpected const &e) + : impl_base(unexpect, e.value()), + ctor_base(detail::default_constructor_tag{}) {} + + template < + class G = E, + detail::enable_if_t::value> * = nullptr, + detail::enable_if_t::value> * = nullptr> + explicit constexpr expected(unexpected &&e) noexcept( + std::is_nothrow_constructible::value) + : impl_base(unexpect, std::move(e.value())), + ctor_base(detail::default_constructor_tag{}) {} + + template < + class G = E, + detail::enable_if_t::value> * = nullptr, + detail::enable_if_t::value> * = nullptr> + constexpr expected(unexpected &&e) noexcept( + std::is_nothrow_constructible::value) + : impl_base(unexpect, std::move(e.value())), + ctor_base(detail::default_constructor_tag{}) {} + + template ::value> * = + nullptr> + constexpr explicit expected(unexpect_t, Args &&...args) + : impl_base(unexpect, std::forward(args)...), + ctor_base(detail::default_constructor_tag{}) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected(unexpect_t, std::initializer_list il, + Args &&...args) + : impl_base(unexpect, il, std::forward(args)...), + ctor_base(detail::default_constructor_tag{}) {} + + template ::value && + std::is_convertible::value)> * = + nullptr, + detail::expected_enable_from_other + * = nullptr> + explicit TL_EXPECTED_11_CONSTEXPR expected(const expected &rhs) + : ctor_base(detail::default_constructor_tag{}) { + if (rhs.has_value()) { + this->construct(*rhs); + } else { + this->construct_error(rhs.error()); + } + } + + template ::value && + std::is_convertible::value)> * = + nullptr, + detail::expected_enable_from_other + * = nullptr> + TL_EXPECTED_11_CONSTEXPR expected(const expected &rhs) + : ctor_base(detail::default_constructor_tag{}) { + if (rhs.has_value()) { + this->construct(*rhs); + } else { + this->construct_error(rhs.error()); + } + } + + template < + class U, class G, + detail::enable_if_t::value && + std::is_convertible::value)> * = nullptr, + detail::expected_enable_from_other * = nullptr> + explicit TL_EXPECTED_11_CONSTEXPR expected(expected &&rhs) + : ctor_base(detail::default_constructor_tag{}) { + if (rhs.has_value()) { + this->construct(std::move(*rhs)); + } else { + this->construct_error(std::move(rhs.error())); + } + } + + template < + class U, class G, + detail::enable_if_t<(std::is_convertible::value && + std::is_convertible::value)> * = nullptr, + detail::expected_enable_from_other * = nullptr> + TL_EXPECTED_11_CONSTEXPR expected(expected &&rhs) + : ctor_base(detail::default_constructor_tag{}) { + if (rhs.has_value()) { + this->construct(std::move(*rhs)); + } else { + this->construct_error(std::move(rhs.error())); + } + } + + template < + class U = T, + detail::enable_if_t::value> * = nullptr, + detail::expected_enable_forward_value * = nullptr> + explicit TL_EXPECTED_MSVC2015_CONSTEXPR expected(U &&v) + : expected(in_place, std::forward(v)) {} + + template < + class U = T, + detail::enable_if_t::value> * = nullptr, + detail::expected_enable_forward_value * = nullptr> + TL_EXPECTED_MSVC2015_CONSTEXPR expected(U &&v) + : expected(in_place, std::forward(v)) {} + + template < + class U = T, class G = T, + detail::enable_if_t::value> * = + nullptr, + detail::enable_if_t::value> * = nullptr, + detail::enable_if_t< + (!std::is_same, detail::decay_t>::value && + !detail::conjunction, + std::is_same>>::value && + std::is_constructible::value && + std::is_assignable::value && + std::is_nothrow_move_constructible::value)> * = nullptr> + expected &operator=(U &&v) { + if (has_value()) { + val() = std::forward(v); + } else { + err().~unexpected(); + ::new (valptr()) T(std::forward(v)); + this->m_has_val = true; + } + + return *this; + } + + template < + class U = T, class G = T, + detail::enable_if_t::value> * = + nullptr, + detail::enable_if_t::value> * = nullptr, + detail::enable_if_t< + (!std::is_same, detail::decay_t>::value && + !detail::conjunction, + std::is_same>>::value && + std::is_constructible::value && + std::is_assignable::value && + std::is_nothrow_move_constructible::value)> * = nullptr> + expected &operator=(U &&v) { + if (has_value()) { + val() = std::forward(v); + } else { + auto tmp = std::move(err()); + err().~unexpected(); + +#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED + try { + ::new (valptr()) T(std::forward(v)); + this->m_has_val = true; + } catch (...) { + err() = std::move(tmp); + throw; + } +#else + ::new (valptr()) T(std::forward(v)); + this->m_has_val = true; +#endif + } + + return *this; + } + + template ::value && + std::is_assignable::value> * = nullptr> + expected &operator=(const unexpected &rhs) { + if (!has_value()) { + err() = rhs; + } else { + this->destroy_val(); + ::new (errptr()) unexpected(rhs); + this->m_has_val = false; + } + + return *this; + } + + template ::value && + std::is_move_assignable::value> * = nullptr> + expected &operator=(unexpected &&rhs) noexcept { + if (!has_value()) { + err() = std::move(rhs); + } else { + this->destroy_val(); + ::new (errptr()) unexpected(std::move(rhs)); + this->m_has_val = false; + } + + return *this; + } + + template ::value> * = nullptr> + void emplace(Args &&...args) { + if (has_value()) { + val().~T(); + } else { + err().~unexpected(); + this->m_has_val = true; + } + ::new (valptr()) T(std::forward(args)...); + } + + template ::value> * = nullptr> + void emplace(Args &&...args) { + if (has_value()) { + val().~T(); + ::new (valptr()) T(std::forward(args)...); + } else { + auto tmp = std::move(err()); + err().~unexpected(); + +#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED + try { + ::new (valptr()) T(std::forward(args)...); + this->m_has_val = true; + } catch (...) { + err() = std::move(tmp); + throw; + } +#else + ::new (valptr()) T(std::forward(args)...); + this->m_has_val = true; +#endif + } + } + + template &, Args &&...>::value> * = nullptr> + void emplace(std::initializer_list il, Args &&...args) { + if (has_value()) { + T t(il, std::forward(args)...); + val() = std::move(t); + } else { + err().~unexpected(); + ::new (valptr()) T(il, std::forward(args)...); + this->m_has_val = true; + } + } + + template &, Args &&...>::value> * = nullptr> + void emplace(std::initializer_list il, Args &&...args) { + if (has_value()) { + T t(il, std::forward(args)...); + val() = std::move(t); + } else { + auto tmp = std::move(err()); + err().~unexpected(); + +#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED + try { + ::new (valptr()) T(il, std::forward(args)...); + this->m_has_val = true; + } catch (...) { + err() = std::move(tmp); + throw; + } +#else + ::new (valptr()) T(il, std::forward(args)...); + this->m_has_val = true; +#endif + } + } + +private: + using t_is_void = std::true_type; + using t_is_not_void = std::false_type; + using t_is_nothrow_move_constructible = std::true_type; + using move_constructing_t_can_throw = std::false_type; + using e_is_nothrow_move_constructible = std::true_type; + using move_constructing_e_can_throw = std::false_type; + + void swap_where_both_have_value(expected & /*rhs*/, t_is_void) noexcept { + // swapping void is a no-op + } + + void swap_where_both_have_value(expected &rhs, t_is_not_void) { + using std::swap; + swap(val(), rhs.val()); + } + + void swap_where_only_one_has_value(expected &rhs, t_is_void) noexcept( + std::is_nothrow_move_constructible::value) { + ::new (errptr()) unexpected_type(std::move(rhs.err())); + rhs.err().~unexpected_type(); + std::swap(this->m_has_val, rhs.m_has_val); + } + + void swap_where_only_one_has_value(expected &rhs, t_is_not_void) { + swap_where_only_one_has_value_and_t_is_not_void( + rhs, typename std::is_nothrow_move_constructible::type{}, + typename std::is_nothrow_move_constructible::type{}); + } + + void swap_where_only_one_has_value_and_t_is_not_void( + expected &rhs, t_is_nothrow_move_constructible, + e_is_nothrow_move_constructible) noexcept { + auto temp = std::move(val()); + val().~T(); + ::new (errptr()) unexpected_type(std::move(rhs.err())); + rhs.err().~unexpected_type(); + ::new (rhs.valptr()) T(std::move(temp)); + std::swap(this->m_has_val, rhs.m_has_val); + } + + void swap_where_only_one_has_value_and_t_is_not_void( + expected &rhs, t_is_nothrow_move_constructible, + move_constructing_e_can_throw) { + auto temp = std::move(val()); + val().~T(); +#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED + try { + ::new (errptr()) unexpected_type(std::move(rhs.err())); + rhs.err().~unexpected_type(); + ::new (rhs.valptr()) T(std::move(temp)); + std::swap(this->m_has_val, rhs.m_has_val); + } catch (...) { + val() = std::move(temp); + throw; + } +#else + ::new (errptr()) unexpected_type(std::move(rhs.err())); + rhs.err().~unexpected_type(); + ::new (rhs.valptr()) T(std::move(temp)); + std::swap(this->m_has_val, rhs.m_has_val); +#endif + } + + void swap_where_only_one_has_value_and_t_is_not_void( + expected &rhs, move_constructing_t_can_throw, + e_is_nothrow_move_constructible) { + auto temp = std::move(rhs.err()); + rhs.err().~unexpected_type(); +#ifdef TL_EXPECTED_EXCEPTIONS_ENABLED + try { + ::new (rhs.valptr()) T(std::move(val())); + val().~T(); + ::new (errptr()) unexpected_type(std::move(temp)); + std::swap(this->m_has_val, rhs.m_has_val); + } catch (...) { + rhs.err() = std::move(temp); + throw; + } +#else + ::new (rhs.valptr()) T(std::move(val())); + val().~T(); + ::new (errptr()) unexpected_type(std::move(temp)); + std::swap(this->m_has_val, rhs.m_has_val); +#endif + } + +public: + template + detail::enable_if_t::value && + detail::is_swappable::value && + (std::is_nothrow_move_constructible::value || + std::is_nothrow_move_constructible::value)> + swap(expected &rhs) noexcept( + std::is_nothrow_move_constructible::value + &&detail::is_nothrow_swappable::value + &&std::is_nothrow_move_constructible::value + &&detail::is_nothrow_swappable::value) { + if (has_value() && rhs.has_value()) { + swap_where_both_have_value(rhs, typename std::is_void::type{}); + } else if (!has_value() && rhs.has_value()) { + rhs.swap(*this); + } else if (has_value()) { + swap_where_only_one_has_value(rhs, typename std::is_void::type{}); + } else { + using std::swap; + swap(err(), rhs.err()); + } + } + + constexpr const T *operator->() const { + TL_ASSERT(has_value()); + return valptr(); + } + TL_EXPECTED_11_CONSTEXPR T *operator->() { + TL_ASSERT(has_value()); + return valptr(); + } + + template ::value> * = nullptr> + constexpr const U &operator*() const & { + TL_ASSERT(has_value()); + return val(); + } + template ::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR U &operator*() & { + TL_ASSERT(has_value()); + return val(); + } + template ::value> * = nullptr> + constexpr const U &&operator*() const && { + TL_ASSERT(has_value()); + return std::move(val()); + } + template ::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR U &&operator*() && { + TL_ASSERT(has_value()); + return std::move(val()); + } + + constexpr bool has_value() const noexcept { return this->m_has_val; } + constexpr explicit operator bool() const noexcept { return this->m_has_val; } + + template ::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR const U &value() const & { + if (!has_value()) + detail::throw_exception(bad_expected_access(err().value())); + return val(); + } + template ::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR U &value() & { + if (!has_value()) + detail::throw_exception(bad_expected_access(err().value())); + return val(); + } + template ::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR const U &&value() const && { + if (!has_value()) + detail::throw_exception(bad_expected_access(std::move(err()).value())); + return std::move(val()); + } + template ::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR U &&value() && { + if (!has_value()) + detail::throw_exception(bad_expected_access(std::move(err()).value())); + return std::move(val()); + } + + constexpr const E &error() const & { + TL_ASSERT(!has_value()); + return err().value(); + } + TL_EXPECTED_11_CONSTEXPR E &error() & { + TL_ASSERT(!has_value()); + return err().value(); + } + constexpr const E &&error() const && { + TL_ASSERT(!has_value()); + return std::move(err().value()); + } + TL_EXPECTED_11_CONSTEXPR E &&error() && { + TL_ASSERT(!has_value()); + return std::move(err().value()); + } + + template constexpr T value_or(U &&v) const & { + static_assert(std::is_copy_constructible::value && + std::is_convertible::value, + "T must be copy-constructible and convertible to from U&&"); + return bool(*this) ? **this : static_cast(std::forward(v)); + } + template TL_EXPECTED_11_CONSTEXPR T value_or(U &&v) && { + static_assert(std::is_move_constructible::value && + std::is_convertible::value, + "T must be move-constructible and convertible to from U&&"); + return bool(*this) ? std::move(**this) : static_cast(std::forward(v)); + } +}; + +namespace detail { +template using exp_t = typename detail::decay_t::value_type; +template using err_t = typename detail::decay_t::error_type; +template using ret_t = expected>; + +#ifdef TL_EXPECTED_CXX14 +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval(), + *std::declval()))> +constexpr auto and_then_impl(Exp &&exp, F &&f) { + static_assert(detail::is_expected::value, "F must return an expected"); + + return exp.has_value() + ? detail::invoke(std::forward(f), *std::forward(exp)) + : Ret(unexpect, std::forward(exp).error()); +} + +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval()))> +constexpr auto and_then_impl(Exp &&exp, F &&f) { + static_assert(detail::is_expected::value, "F must return an expected"); + + return exp.has_value() ? detail::invoke(std::forward(f)) + : Ret(unexpect, std::forward(exp).error()); +} +#else +template struct TC; +template (), + *std::declval())), + detail::enable_if_t>::value> * = nullptr> +auto and_then_impl(Exp &&exp, F &&f) -> Ret { + static_assert(detail::is_expected::value, "F must return an expected"); + + return exp.has_value() + ? detail::invoke(std::forward(f), *std::forward(exp)) + : Ret(unexpect, std::forward(exp).error()); +} + +template ())), + detail::enable_if_t>::value> * = nullptr> +constexpr auto and_then_impl(Exp &&exp, F &&f) -> Ret { + static_assert(detail::is_expected::value, "F must return an expected"); + + return exp.has_value() ? detail::invoke(std::forward(f)) + : Ret(unexpect, std::forward(exp).error()); +} +#endif + +#ifdef TL_EXPECTED_CXX14 +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval(), + *std::declval())), + detail::enable_if_t::value> * = nullptr> +constexpr auto expected_map_impl(Exp &&exp, F &&f) { + using result = ret_t>; + return exp.has_value() ? result(detail::invoke(std::forward(f), + *std::forward(exp))) + : result(unexpect, std::forward(exp).error()); +} + +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval(), + *std::declval())), + detail::enable_if_t::value> * = nullptr> +auto expected_map_impl(Exp &&exp, F &&f) { + using result = expected>; + if (exp.has_value()) { + detail::invoke(std::forward(f), *std::forward(exp)); + return result(); + } + + return result(unexpect, std::forward(exp).error()); +} + +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval())), + detail::enable_if_t::value> * = nullptr> +constexpr auto expected_map_impl(Exp &&exp, F &&f) { + using result = ret_t>; + return exp.has_value() ? result(detail::invoke(std::forward(f))) + : result(unexpect, std::forward(exp).error()); +} + +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval())), + detail::enable_if_t::value> * = nullptr> +auto expected_map_impl(Exp &&exp, F &&f) { + using result = expected>; + if (exp.has_value()) { + detail::invoke(std::forward(f)); + return result(); + } + + return result(unexpect, std::forward(exp).error()); +} +#else +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval(), + *std::declval())), + detail::enable_if_t::value> * = nullptr> + +constexpr auto expected_map_impl(Exp &&exp, F &&f) + -> ret_t> { + using result = ret_t>; + + return exp.has_value() ? result(detail::invoke(std::forward(f), + *std::forward(exp))) + : result(unexpect, std::forward(exp).error()); +} + +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval(), + *std::declval())), + detail::enable_if_t::value> * = nullptr> + +auto expected_map_impl(Exp &&exp, F &&f) -> expected> { + if (exp.has_value()) { + detail::invoke(std::forward(f), *std::forward(exp)); + return {}; + } + + return unexpected>(std::forward(exp).error()); +} + +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval())), + detail::enable_if_t::value> * = nullptr> + +constexpr auto expected_map_impl(Exp &&exp, F &&f) + -> ret_t> { + using result = ret_t>; + + return exp.has_value() ? result(detail::invoke(std::forward(f))) + : result(unexpect, std::forward(exp).error()); +} + +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval())), + detail::enable_if_t::value> * = nullptr> + +auto expected_map_impl(Exp &&exp, F &&f) -> expected> { + if (exp.has_value()) { + detail::invoke(std::forward(f)); + return {}; + } + + return unexpected>(std::forward(exp).error()); +} +#endif + +#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ + !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +constexpr auto map_error_impl(Exp &&exp, F &&f) { + using result = expected, detail::decay_t>; + return exp.has_value() + ? result(*std::forward(exp)) + : result(unexpect, detail::invoke(std::forward(f), + std::forward(exp).error())); +} +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +auto map_error_impl(Exp &&exp, F &&f) { + using result = expected, monostate>; + if (exp.has_value()) { + return result(*std::forward(exp)); + } + + detail::invoke(std::forward(f), std::forward(exp).error()); + return result(unexpect, monostate{}); +} +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +constexpr auto map_error_impl(Exp &&exp, F &&f) { + using result = expected, detail::decay_t>; + return exp.has_value() + ? result() + : result(unexpect, detail::invoke(std::forward(f), + std::forward(exp).error())); +} +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +auto map_error_impl(Exp &&exp, F &&f) { + using result = expected, monostate>; + if (exp.has_value()) { + return result(); + } + + detail::invoke(std::forward(f), std::forward(exp).error()); + return result(unexpect, monostate{}); +} +#else +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +constexpr auto map_error_impl(Exp &&exp, F &&f) + -> expected, detail::decay_t> { + using result = expected, detail::decay_t>; + + return exp.has_value() + ? result(*std::forward(exp)) + : result(unexpect, detail::invoke(std::forward(f), + std::forward(exp).error())); +} + +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +auto map_error_impl(Exp &&exp, F &&f) -> expected, monostate> { + using result = expected, monostate>; + if (exp.has_value()) { + return result(*std::forward(exp)); + } + + detail::invoke(std::forward(f), std::forward(exp).error()); + return result(unexpect, monostate{}); +} + +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +constexpr auto map_error_impl(Exp &&exp, F &&f) + -> expected, detail::decay_t> { + using result = expected, detail::decay_t>; + + return exp.has_value() + ? result() + : result(unexpect, detail::invoke(std::forward(f), + std::forward(exp).error())); +} + +template >::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +auto map_error_impl(Exp &&exp, F &&f) -> expected, monostate> { + using result = expected, monostate>; + if (exp.has_value()) { + return result(); + } + + detail::invoke(std::forward(f), std::forward(exp).error()); + return result(unexpect, monostate{}); +} +#endif + +#ifdef TL_EXPECTED_CXX14 +template (), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +constexpr auto or_else_impl(Exp &&exp, F &&f) { + static_assert(detail::is_expected::value, "F must return an expected"); + return exp.has_value() ? std::forward(exp) + : detail::invoke(std::forward(f), + std::forward(exp).error()); +} + +template (), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +detail::decay_t or_else_impl(Exp &&exp, F &&f) { + return exp.has_value() ? std::forward(exp) + : (detail::invoke(std::forward(f), + std::forward(exp).error()), + std::forward(exp)); +} +#else +template (), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +auto or_else_impl(Exp &&exp, F &&f) -> Ret { + static_assert(detail::is_expected::value, "F must return an expected"); + return exp.has_value() ? std::forward(exp) + : detail::invoke(std::forward(f), + std::forward(exp).error()); +} + +template (), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +detail::decay_t or_else_impl(Exp &&exp, F &&f) { + return exp.has_value() ? std::forward(exp) + : (detail::invoke(std::forward(f), + std::forward(exp).error()), + std::forward(exp)); +} +#endif +} // namespace detail + +template +constexpr bool operator==(const expected &lhs, + const expected &rhs) { + return (lhs.has_value() != rhs.has_value()) + ? false + : (!lhs.has_value() ? lhs.error() == rhs.error() : *lhs == *rhs); +} +template +constexpr bool operator!=(const expected &lhs, + const expected &rhs) { + return (lhs.has_value() != rhs.has_value()) + ? true + : (!lhs.has_value() ? lhs.error() != rhs.error() : *lhs != *rhs); +} +template +constexpr bool operator==(const expected &lhs, + const expected &rhs) { + return (lhs.has_value() != rhs.has_value()) + ? false + : (!lhs.has_value() ? lhs.error() == rhs.error() : true); +} +template +constexpr bool operator!=(const expected &lhs, + const expected &rhs) { + return (lhs.has_value() != rhs.has_value()) + ? true + : (!lhs.has_value() ? lhs.error() == rhs.error() : false); +} + +template +constexpr bool operator==(const expected &x, const U &v) { + return x.has_value() ? *x == v : false; +} +template +constexpr bool operator==(const U &v, const expected &x) { + return x.has_value() ? *x == v : false; +} +template +constexpr bool operator!=(const expected &x, const U &v) { + return x.has_value() ? *x != v : true; +} +template +constexpr bool operator!=(const U &v, const expected &x) { + return x.has_value() ? *x != v : true; +} + +template +constexpr bool operator==(const expected &x, const unexpected &e) { + return x.has_value() ? false : x.error() == e.value(); +} +template +constexpr bool operator==(const unexpected &e, const expected &x) { + return x.has_value() ? false : x.error() == e.value(); +} +template +constexpr bool operator!=(const expected &x, const unexpected &e) { + return x.has_value() ? true : x.error() != e.value(); +} +template +constexpr bool operator!=(const unexpected &e, const expected &x) { + return x.has_value() ? true : x.error() != e.value(); +} + +template ::value || + std::is_move_constructible::value) && + detail::is_swappable::value && + std::is_move_constructible::value && + detail::is_swappable::value> * = nullptr> +void swap(expected &lhs, + expected &rhs) noexcept(noexcept(lhs.swap(rhs))) { + lhs.swap(rhs); +} +} // namespace tl + +#endif diff --git a/package.xml b/package.xml index f5b727c..b737951 100644 --- a/package.xml +++ b/package.xml @@ -30,8 +30,15 @@ sensor_msgs git - ament_lint_auto - ament_lint_common + ament_cmake_gtest + ament_cmake_copyright + ament_cmake_cppcheck + ament_cmake_cpplint + ament_cmake_flake8 + ament_cmake_lint_cmake + ament_cmake_pep257 + ament_cmake_uncrustify + ament_cmake_xmllint ament_cmake diff --git a/point_cloud_transport/common.py b/point_cloud_transport/common.py index 18ee00f..0d860e8 100755 --- a/point_cloud_transport/common.py +++ b/point_cloud_transport/common.py @@ -31,10 +31,11 @@ """Common definitions.""" -from rclpy.serialization import serialize_message, deserialize_message -from sensor_msgs.msg import PointCloud2 from importlib import import_module +from rclpy.serialization import deserialize_message, serialize_message +from sensor_msgs.msg import PointCloud2 + class TransportInfo(object): @@ -45,7 +46,7 @@ def __init__(self, name: str, topic: str, data_type: str): def stringToPointCloud2(buffer: str): - cloud = deserialize_message(buffer, "sensor_msgs/msg/PointCloud2") + cloud = deserialize_message(buffer, 'sensor_msgs/msg/PointCloud2') return cloud @@ -58,11 +59,11 @@ def stringToMsgType(message_type_str): try: # Dynamically import the message type package_name, message_type = message_type_str.replace( - "/", ".").rsplit(".", 1) + '/', '.').rsplit('.', 1) module = import_module(package_name) message_class = getattr(module, message_type) # Return the subscription object return message_class except Exception as e: - print(f"Error creating subscription: {e}") + print(f'Error creating subscription: {e}') return None diff --git a/point_cloud_transport/publisher.py b/point_cloud_transport/publisher.py index d989f9f..56f0235 100755 --- a/point_cloud_transport/publisher.py +++ b/point_cloud_transport/publisher.py @@ -33,12 +33,12 @@ """Publisher that automatically publishes to all declared transports.""" +from point_cloud_transport._codec import PointCloudCodec, VectorString +from point_cloud_transport.common import pointCloud2ToString, stringToMsgType, TransportInfo + from rclpy.node import Node from sensor_msgs.msg import PointCloud2 -from point_cloud_transport.common import TransportInfo, pointCloud2ToString, stringToMsgType -from point_cloud_transport._codec import PointCloudCodec, VectorString - def _get_topics_to_publish(codec, base_topic, logger): transports = VectorString() @@ -65,17 +65,17 @@ def _get_topics_to_publish(codec, base_topic, logger): class Publisher(Node): def __init__(self): - node_name = "point_cloud_transport_publisher" + node_name = 'point_cloud_transport_publisher' super().__init__(node_name) self.codec = PointCloudCodec() - print("Codec created") + print('Codec created') self.topics_to_publish = _get_topics_to_publish( - self.codec, "point_cloud", self.get_logger()) - print("Topics to publish: \n", self.topics_to_publish) + self.codec, 'point_cloud', self.get_logger()) + print('Topics to publish: \n', self.topics_to_publish) blacklist = set(self.get_parameter_or('disable_pub_plugins', [])) - print("Blacklist: \n" + str(blacklist)) + print('Blacklist: \n' + str(blacklist)) self.transport_publishers = {} for transport in self.topics_to_publish: @@ -97,7 +97,7 @@ def publish(self, raw: PointCloud2): self.get_logger().error('Error encoding message!') -if __name__ == "__main__": +if __name__ == '__main__': import rclpy import sys @@ -108,7 +108,7 @@ def publish(self, raw: PointCloud2): publisher_node = Publisher() rclpy.spin(publisher_node) except Exception as e: - print("Error in publisher node!") + print('Error in publisher node!') print(e) finally: if publisher_node is not None: diff --git a/point_cloud_transport/subscriber.py b/point_cloud_transport/subscriber.py index e4a1c8c..78a846c 100755 --- a/point_cloud_transport/subscriber.py +++ b/point_cloud_transport/subscriber.py @@ -33,10 +33,10 @@ """Subscriber automatically converting from any transport to raw.""" -from rclpy.node import Node - -from point_cloud_transport.common import TransportInfo, stringToPointCloud2, stringToMsgType from point_cloud_transport._codec import PointCloudCodec, VectorString +from point_cloud_transport.common import stringToMsgType, stringToPointCloud2, TransportInfo + +from rclpy.node import Node def _get_loadable_transports(codec: PointCloudCodec): @@ -57,24 +57,25 @@ def _get_topic_to_subscribe(codec, base_topic, transport_name): class Subscriber(Node): + def __init__(self): - node_name = "point_cloud_transport_subscriber" + node_name = 'point_cloud_transport_subscriber' super().__init__(node_name) - self.base_topic = "point_cloud" - self.transport = self.get_parameter_or("transport", "raw") + self.base_topic = 'point_cloud' + self.transport = self.get_parameter_or('transport', 'raw') self.codec = PointCloudCodec() transports = _get_loadable_transports(self.codec) if self.transport not in transports and self.transport not in transports.values(): raise RuntimeError( - "Point cloud transport '%s' not found." % (self.transport,)) + 'Point cloud transport "%s" not found.' % (self.transport,)) self.transport_info = _get_topic_to_subscribe( self.codec, self.base_topic, self.transport) if self.transport_info is None: raise RuntimeError( - "Point cloud transport '%s' not found." % (self.transport,)) + 'Point cloud transport "%s" not found.' % (self.transport,)) # subscribe to compressed, serialized msg self.subscriber = self.create_subscription(stringToMsgType( @@ -87,7 +88,7 @@ def cb(self, serialized_buffer): print(cloud) -if __name__ == "__main__": +if __name__ == '__main__': import rclpy import sys From 95bd22852bf08d1aa10e472894e4eab4a92461a1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 27 Jul 2023 12:48:02 +0200 Subject: [PATCH 24/28] Removed warnings (#15) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- include/point_cloud_transport/point_cloud_transport.hpp | 1 + include/point_cloud_transport/simple_publisher_plugin.hpp | 2 +- src/list_transports.cpp | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/include/point_cloud_transport/point_cloud_transport.hpp b/include/point_cloud_transport/point_cloud_transport.hpp index 561f161..e733341 100644 --- a/include/point_cloud_transport/point_cloud_transport.hpp +++ b/include/point_cloud_transport/point_cloud_transport.hpp @@ -180,6 +180,7 @@ class PointCloudTransport : public PointCloudTransportLoader const VoidPtr & tracked_object = {}, const point_cloud_transport::TransportHints * transport_hints = nullptr) { + (void)tracked_object; rmw_qos_profile_t custom_qos = rmw_qos_profile_sensor_data; custom_qos.depth = queue_size; rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions(); diff --git a/include/point_cloud_transport/simple_publisher_plugin.hpp b/include/point_cloud_transport/simple_publisher_plugin.hpp index db2da32..6ca3292 100644 --- a/include/point_cloud_transport/simple_publisher_plugin.hpp +++ b/include/point_cloud_transport/simple_publisher_plugin.hpp @@ -163,7 +163,7 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin simple_impl_.reset(); } - void declareParameters(const std::string & base_topic) override + void declareParameters(const std::string & /*base_topic*/) override { auto logger = simple_impl_ ? simple_impl_->logger_ : rclcpp::get_logger( "point_cloud_transport"); diff --git a/src/list_transports.cpp b/src/list_transports.cpp index 64a18a6..328d530 100644 --- a/src/list_transports.cpp +++ b/src/list_transports.cpp @@ -62,7 +62,7 @@ struct TransportDesc PluginStatus sub_status; }; -int main(int argc, char ** argv) +int main(int /*argc*/, char ** /*argv*/) { pluginlib::ClassLoader pub_loader( "point_cloud_transport", From f476edb026850906378464b18729d23c20e7dc25 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 28 Jul 2023 14:22:19 +0200 Subject: [PATCH 25/28] Use whitelist instead of blacklist (#13) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Use whitelist instead of blacklist Signed-off-by: Alejandro Hernández Cordero * Feedback Signed-off-by: Alejandro Hernández Cordero * feedback Signed-off-by: Alejandro Hernández Cordero * Updates to param namespacing (#16) --------- Signed-off-by: Alejandro Hernández Cordero Co-authored-by: john-maidbot <78750993+john-maidbot@users.noreply.github.com> --- README.md | 8 +++--- point_cloud_transport/publisher.py | 7 ++--- src/publisher.cpp | 41 ++++++++++++++++++++++-------- 3 files changed, 38 insertions(+), 18 deletions(-) diff --git a/README.md b/README.md index 3acf206..acdb670 100644 --- a/README.md +++ b/README.md @@ -48,7 +48,7 @@ bool success = codec.encode("draco", msg, serialized_msg); // OR -// typed version (outputs whatever message your selected transport returns, +// typed version (outputs whatever message your selected transport returns, // for draco that is a point_cloud_interfaces::msg::CompressedPointCloud2) point_cloud_interfaces::msg::CompressedPointCloud2 compressed_msg; bool success = codec.encode("draco", msg, compressed_msg); @@ -69,12 +69,12 @@ The functionality of `point_cloud_transport` is also exposed to python via `pybi Please see [point_cloud_transport/publisher.py](point_cloud_transport/publisher.py) and [point_cloud_transport/subscriber.py](point_cloud_transport/subscriber.py) for example usage. -### Blacklist point cloud transport +### Whitelist point cloud transport -This allows you to specify plugins you do not want to load (a.k.a. blacklist them). +This allows you to specify plugins you do want to load (a.k.a. whitelist them). ```bash -ros2 run point_cloud_transport_tutorial my_publisher --ros-args -p /pct/point_cloud/disable_pub_plugins:=["point_cloud_transport/raw"] +ros2 run point_cloud_transport_tutorial my_publisher --ros-args -p pct.point_cloud.enable_pub_plugins:=["point_cloud_transport/zlib"] ``` ## Known transports diff --git a/point_cloud_transport/publisher.py b/point_cloud_transport/publisher.py index 56f0235..5d6541c 100755 --- a/point_cloud_transport/publisher.py +++ b/point_cloud_transport/publisher.py @@ -37,6 +37,7 @@ from point_cloud_transport.common import pointCloud2ToString, stringToMsgType, TransportInfo from rclpy.node import Node +from rclpy.parameter import Parameter from sensor_msgs.msg import PointCloud2 @@ -74,12 +75,12 @@ def __init__(self): self.codec, 'point_cloud', self.get_logger()) print('Topics to publish: \n', self.topics_to_publish) - blacklist = set(self.get_parameter_or('disable_pub_plugins', [])) - print('Blacklist: \n' + str(blacklist)) + self.declare_parameter('enable_pub_plugins', Parameter.Type.STRING_ARRAY) + whitelist = self.get_parameter('enable_pub_plugins') self.transport_publishers = {} for transport in self.topics_to_publish: - if transport in blacklist: + if transport not in whitelist.value: continue topic_to_publish = self.topics_to_publish[transport] self.transport_publishers[transport] = self.create_publisher( diff --git a/src/publisher.cpp b/src/publisher.cpp index 94bbb99..2feb8e4 100644 --- a/src/publisher.cpp +++ b/src/publisher.cpp @@ -126,23 +126,42 @@ Publisher::Publisher( impl_->base_topic_ = point_cloud_topic; impl_->loader_ = loader; - std::vector blacklist_vec; - node->declare_parameter>( - impl_->base_topic_ + "/disable_pub_plugins", blacklist_vec); - node->get_parameter(impl_->base_topic_ + "/disable_pub_plugins", blacklist_vec); - // set - std::set blacklist(blacklist_vec.begin(), blacklist_vec.end()); + auto ns_len = node->get_effective_namespace().length(); + std::string param_base_name = point_cloud_topic.substr(ns_len); + std::replace(param_base_name.begin(), param_base_name.end(), '/', '.'); + if (param_base_name.front() == '.') { + param_base_name = param_base_name.substr(1); + } + std::vector whitelist_vec; + std::vector all_transport_names; for (const auto & lookup_name : loader->getDeclaredClasses()) { - const std::string transport_name = erase_last_copy(lookup_name, "_pub"); - if (blacklist.find(transport_name) != blacklist.end()) { - continue; - } + all_transport_names.emplace_back(erase_last_copy(lookup_name, "_pub")); + } + + try { + whitelist_vec = node->declare_parameter>( + param_base_name + ".enable_pub_plugins", all_transport_names); + } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &) { + RCLCPP_DEBUG_STREAM( + node->get_logger(), param_base_name << ".enable_pub_plugins" << " was previously declared" + ); + whitelist_vec = + node->get_parameter( + param_base_name + + ".enable_pub_plugins").get_value>(); + } + + std::set whitelist; + for (size_t i = 0; i < whitelist_vec.size(); ++i) { + whitelist.insert(whitelist_vec[i]); + } + for (const auto & transport_name : whitelist) { + const auto & lookup_name = transport_name + "_pub"; try { auto pub = loader->createUniqueInstance(lookup_name); pub->advertise(node, point_cloud_topic, custom_qos, options); - impl_->base_topic_ = pub->getTopic(); impl_->publishers_.push_back(std::move(pub)); } catch (const std::runtime_error & e) { RCLCPP_ERROR( From b77c9bdede8f89b146d058e425903c387c545fe3 Mon Sep 17 00:00:00 2001 From: john-maidbot <78750993+john-maidbot@users.noreply.github.com> Date: Tue, 1 Aug 2023 10:28:32 -0500 Subject: [PATCH 26/28] Updates to fix build errors (#17) --- include/point_cloud_transport/publisher.hpp | 2 -- .../simple_subscriber_plugin.hpp | 27 ++++++++++++------- include/point_cloud_transport/subscriber.hpp | 2 -- src/publisher.cpp | 2 ++ src/subscriber.cpp | 1 + 5 files changed, 20 insertions(+), 14 deletions(-) diff --git a/include/point_cloud_transport/publisher.hpp b/include/point_cloud_transport/publisher.hpp index 21f2de1..307bae4 100644 --- a/include/point_cloud_transport/publisher.hpp +++ b/include/point_cloud_transport/publisher.hpp @@ -42,8 +42,6 @@ #include -#include "pluginlib/class_loader.hpp" - #include #include diff --git a/include/point_cloud_transport/simple_subscriber_plugin.hpp b/include/point_cloud_transport/simple_subscriber_plugin.hpp index 9778483..626e211 100644 --- a/include/point_cloud_transport/simple_subscriber_plugin.hpp +++ b/include/point_cloud_transport/simple_subscriber_plugin.hpp @@ -75,6 +75,14 @@ class SimpleSubscriberPlugin : public SubscriberPlugin { } + rclcpp::Logger getLogger() const + { + if (impl_) { + return impl_->logger_; + } + return rclcpp::get_logger("point_cloud_transport"); + } + std::string getTopic() const override { if (impl_) { @@ -175,12 +183,8 @@ class SimpleSubscriberPlugin : public SubscriberPlugin const Callback & callback, rmw_qos_profile_t custom_qos) override { - impl_ = std::make_unique(); - // Push each group of transport-specific parameters into a separate sub-namespace - // ros::NodeHandle param_nh(transport_hints.getParameterNH(), getTransportName()); - // + impl_ = std::make_unique(node); auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos), custom_qos); - impl_->node_ = node; impl_->sub_ = node->create_subscription( getTopicToSubscribe(base_topic), qos, [this, callback](const typename std::shared_ptr msg) { @@ -196,11 +200,7 @@ class SimpleSubscriberPlugin : public SubscriberPlugin rmw_qos_profile_t custom_qos, rclcpp::SubscriptionOptions options) { - impl_ = std::make_unique(); - // Push each group of transport-specific parameters into a separate sub-namespace - // ros::NodeHandle param_nh(transport_hints.getParameterNH(), getTransportName()); - // - impl_->node_ = node; + impl_ = std::make_unique(node); auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos), custom_qos); impl_->sub_ = node->create_subscription( getTopicToSubscribe(base_topic), qos, @@ -214,8 +214,15 @@ class SimpleSubscriberPlugin : public SubscriberPlugin private: struct Impl { + explicit Impl(std::shared_ptr node) + : node_(node), + logger_(node->get_logger()) + { + } + rclcpp::SubscriptionBase::SharedPtr sub_; std::shared_ptr node_; + rclcpp::Logger logger_; rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_handle_; }; diff --git a/include/point_cloud_transport/subscriber.hpp b/include/point_cloud_transport/subscriber.hpp index ecf9c73..85e575a 100644 --- a/include/point_cloud_transport/subscriber.hpp +++ b/include/point_cloud_transport/subscriber.hpp @@ -42,8 +42,6 @@ #include -#include "pluginlib/class_loader.hpp" - #include #include diff --git a/src/publisher.cpp b/src/publisher.cpp index 2feb8e4..391f8c8 100644 --- a/src/publisher.cpp +++ b/src/publisher.cpp @@ -37,6 +37,8 @@ #include #include +#include "pluginlib/class_loader.hpp" + #include "rclcpp/expand_topic_or_service_name.hpp" #include "rclcpp/logging.hpp" #include "rclcpp/node.hpp" diff --git a/src/subscriber.cpp b/src/subscriber.cpp index 13d7cd9..ed48b96 100644 --- a/src/subscriber.cpp +++ b/src/subscriber.cpp @@ -36,6 +36,7 @@ #include #include +#include #include #include "rclcpp/expand_topic_or_service_name.hpp" From 406559f4247e0e214d472c40594a8e63af1d999b Mon Sep 17 00:00:00 2001 From: john-maidbot <78750993+john-maidbot@users.noreply.github.com> Date: Sat, 5 Aug 2023 19:28:57 -0500 Subject: [PATCH 27/28] Bug fixes from porting tutorials (#18) * Define declareParameters for raw pub/sub * Add queue_size version of advertise * Fix copyright linting --- include/point_cloud_transport/exception.hpp | 62 ++++++++-------- include/point_cloud_transport/expected.hpp | 58 +++++++-------- include/point_cloud_transport/loader_fwds.hpp | 62 ++++++++-------- .../point_cloud_codec.hpp | 62 ++++++++-------- .../point_cloud_common.hpp | 59 ++++++++------- .../point_cloud_transport.hpp | 73 ++++++++++-------- include/point_cloud_transport/publisher.hpp | 62 ++++++++-------- .../publisher_plugin.hpp | 62 ++++++++-------- .../point_cloud_transport/raw_publisher.hpp | 66 +++++++++-------- .../point_cloud_transport/raw_subscriber.hpp | 74 ++++++++----------- include/point_cloud_transport/republish.hpp | 62 ++++++++-------- .../simple_publisher_plugin.hpp | 71 ++++++++---------- .../simple_subscriber_plugin.hpp | 70 ++++++++---------- .../single_subscriber_publisher.hpp | 62 ++++++++-------- include/point_cloud_transport/subscriber.hpp | 62 ++++++++-------- .../subscriber_filter.hpp | 62 ++++++++-------- .../subscriber_plugin.hpp | 71 ++++++++---------- .../point_cloud_transport/transport_hints.hpp | 62 ++++++++-------- point_cloud_transport/common.py | 1 - point_cloud_transport/publisher.py | 1 - point_cloud_transport/subscriber.py | 1 - src/list_transports.cpp | 62 ++++++++-------- src/manifest.cpp | 59 +++++++-------- src/point_cloud_codec.cpp | 62 ++++++++-------- src/point_cloud_common.cpp | 58 +++++++-------- src/point_cloud_transport.cpp | 62 ++++++++-------- src/publisher.cpp | 62 ++++++++-------- src/publisher_plugin.cpp | 62 ++++++++-------- src/pybind_codec.cpp | 58 +++++++-------- src/raw_subscriber.cpp | 66 +++++++++-------- src/republish.cpp | 62 ++++++++-------- src/republish_program.cpp | 62 ++++++++-------- src/single_subscriber_publisher.cpp | 62 ++++++++-------- src/subscriber.cpp | 62 ++++++++-------- 34 files changed, 945 insertions(+), 1019 deletions(-) diff --git a/include/point_cloud_transport/exception.hpp b/include/point_cloud_transport/exception.hpp index 67ab826..4dc2adb 100644 --- a/include/point_cloud_transport/exception.hpp +++ b/include/point_cloud_transport/exception.hpp @@ -1,35 +1,33 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// Copyright (c) 2019, paplhjak +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #ifndef POINT_CLOUD_TRANSPORT__EXCEPTION_HPP_ #define POINT_CLOUD_TRANSPORT__EXCEPTION_HPP_ diff --git a/include/point_cloud_transport/expected.hpp b/include/point_cloud_transport/expected.hpp index b6ccb28..1dca329 100644 --- a/include/point_cloud_transport/expected.hpp +++ b/include/point_cloud_transport/expected.hpp @@ -1,33 +1,31 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * 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 HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #ifndef POINT_CLOUD_TRANSPORT__EXPECTED_HPP_ #define POINT_CLOUD_TRANSPORT__EXPECTED_HPP_ diff --git a/include/point_cloud_transport/loader_fwds.hpp b/include/point_cloud_transport/loader_fwds.hpp index 90031f1..b84f536 100644 --- a/include/point_cloud_transport/loader_fwds.hpp +++ b/include/point_cloud_transport/loader_fwds.hpp @@ -1,35 +1,33 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// Copyright (c) 2019, paplhjak +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #ifndef POINT_CLOUD_TRANSPORT__LOADER_FWDS_HPP_ #define POINT_CLOUD_TRANSPORT__LOADER_FWDS_HPP_ diff --git a/include/point_cloud_transport/point_cloud_codec.hpp b/include/point_cloud_transport/point_cloud_codec.hpp index 32c9d55..7607eb1 100644 --- a/include/point_cloud_transport/point_cloud_codec.hpp +++ b/include/point_cloud_transport/point_cloud_codec.hpp @@ -1,35 +1,33 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// Copyright (c) 2019, paplhjak +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #ifndef POINT_CLOUD_TRANSPORT__POINT_CLOUD_CODEC_HPP_ #define POINT_CLOUD_TRANSPORT__POINT_CLOUD_CODEC_HPP_ diff --git a/include/point_cloud_transport/point_cloud_common.hpp b/include/point_cloud_transport/point_cloud_common.hpp index 9f54d16..24bd5ed 100644 --- a/include/point_cloud_transport/point_cloud_common.hpp +++ b/include/point_cloud_transport/point_cloud_common.hpp @@ -1,33 +1,32 @@ -/* - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, John D'Angelo +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #ifndef POINT_CLOUD_TRANSPORT__POINT_CLOUD_COMMON_HPP_ #define POINT_CLOUD_TRANSPORT__POINT_CLOUD_COMMON_HPP_ diff --git a/include/point_cloud_transport/point_cloud_transport.hpp b/include/point_cloud_transport/point_cloud_transport.hpp index e733341..2abb706 100644 --- a/include/point_cloud_transport/point_cloud_transport.hpp +++ b/include/point_cloud_transport/point_cloud_transport.hpp @@ -1,35 +1,33 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// Copyright (c) 2019, paplhjak +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #ifndef POINT_CLOUD_TRANSPORT__POINT_CLOUD_TRANSPORT_HPP_ #define POINT_CLOUD_TRANSPORT__POINT_CLOUD_TRANSPORT_HPP_ @@ -156,6 +154,17 @@ class PointCloudTransport : public PointCloudTransportLoader } //! Advertise a PointCloud2 topic, simple version. + POINT_CLOUD_TRANSPORT_PUBLIC + Publisher advertise( + const std::string & base_topic, + uint32_t queue_size, + const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions()) + { + rmw_qos_profile_t custom_qos = rmw_qos_profile_sensor_data; + custom_qos.depth = queue_size; + return Publisher(node_, base_topic, pub_loader_, custom_qos, options); + } + POINT_CLOUD_TRANSPORT_PUBLIC Publisher advertise( const std::string & base_topic, diff --git a/include/point_cloud_transport/publisher.hpp b/include/point_cloud_transport/publisher.hpp index 307bae4..5eeb2e1 100644 --- a/include/point_cloud_transport/publisher.hpp +++ b/include/point_cloud_transport/publisher.hpp @@ -1,35 +1,33 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// Copyright (c) 2019, paplhjak +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #ifndef POINT_CLOUD_TRANSPORT__PUBLISHER_HPP_ #define POINT_CLOUD_TRANSPORT__PUBLISHER_HPP_ diff --git a/include/point_cloud_transport/publisher_plugin.hpp b/include/point_cloud_transport/publisher_plugin.hpp index 993920f..c98f5ea 100644 --- a/include/point_cloud_transport/publisher_plugin.hpp +++ b/include/point_cloud_transport/publisher_plugin.hpp @@ -1,35 +1,33 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// Copyright (c) 2019, paplhjak +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #ifndef POINT_CLOUD_TRANSPORT__PUBLISHER_PLUGIN_HPP_ #define POINT_CLOUD_TRANSPORT__PUBLISHER_PLUGIN_HPP_ diff --git a/include/point_cloud_transport/raw_publisher.hpp b/include/point_cloud_transport/raw_publisher.hpp index 61772d3..db2c4dc 100644 --- a/include/point_cloud_transport/raw_publisher.hpp +++ b/include/point_cloud_transport/raw_publisher.hpp @@ -1,35 +1,33 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// Copyright (c) 2019, paplhjak +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #ifndef POINT_CLOUD_TRANSPORT__RAW_PUBLISHER_HPP_ #define POINT_CLOUD_TRANSPORT__RAW_PUBLISHER_HPP_ @@ -63,6 +61,10 @@ class RawPublisher return "sensor_msgs/msg/PointCloud2"; } + void declareParameters(const std::string & /*base_topic*/) override + { + } + RawPublisher::TypedEncodeResult encodeTyped(const sensor_msgs::msg::PointCloud2 & raw) const { return raw; diff --git a/include/point_cloud_transport/raw_subscriber.hpp b/include/point_cloud_transport/raw_subscriber.hpp index a990d4f..5e53128 100644 --- a/include/point_cloud_transport/raw_subscriber.hpp +++ b/include/point_cloud_transport/raw_subscriber.hpp @@ -1,35 +1,33 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// Copyright (c) 2019, paplhjak +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #ifndef POINT_CLOUD_TRANSPORT__RAW_SUBSCRIBER_HPP_ #define POINT_CLOUD_TRANSPORT__RAW_SUBSCRIBER_HPP_ @@ -66,6 +64,8 @@ class RawSubscriber std::string getDataType() const override; + void declareParameters() override; + std::string getTransportName() const override; protected: @@ -78,16 +78,6 @@ class RawSubscriber std::string getTopicToSubscribe(const std::string & base_topic) const override; using SubscriberPlugin::subscribeImpl; - - void subscribeImpl( - std::shared_ptr node, - const std::string & base_topic, - const Callback & callback, - rmw_qos_profile_t custom_qos, - rclcpp::SubscriptionOptions options) override - { - this->subscribeImplWithOptions(node, base_topic, callback, custom_qos, options); - } }; } // namespace point_cloud_transport diff --git a/include/point_cloud_transport/republish.hpp b/include/point_cloud_transport/republish.hpp index 3103fc8..11e416b 100644 --- a/include/point_cloud_transport/republish.hpp +++ b/include/point_cloud_transport/republish.hpp @@ -1,35 +1,33 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// Copyright (c) 2019, paplhjak +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #ifndef POINT_CLOUD_TRANSPORT__REPUBLISH_HPP_ #define POINT_CLOUD_TRANSPORT__REPUBLISH_HPP_ diff --git a/include/point_cloud_transport/simple_publisher_plugin.hpp b/include/point_cloud_transport/simple_publisher_plugin.hpp index 6ca3292..da2daa1 100644 --- a/include/point_cloud_transport/simple_publisher_plugin.hpp +++ b/include/point_cloud_transport/simple_publisher_plugin.hpp @@ -1,35 +1,33 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// Copyright (c) 2019, paplhjak +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #ifndef POINT_CLOUD_TRANSPORT__SIMPLE_PUBLISHER_PLUGIN_HPP_ #define POINT_CLOUD_TRANSPORT__SIMPLE_PUBLISHER_PLUGIN_HPP_ @@ -163,15 +161,6 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin simple_impl_.reset(); } - void declareParameters(const std::string & /*base_topic*/) override - { - auto logger = simple_impl_ ? simple_impl_->logger_ : rclcpp::get_logger( - "point_cloud_transport"); - RCLCPP_WARN( - logger, - "declareParameter method not implemented for %s transport", this->getTransportName().c_str()); - } - /** * \brief Encode the given raw pointcloud into a compressed message. * \param[in] raw The input raw pointcloud. diff --git a/include/point_cloud_transport/simple_subscriber_plugin.hpp b/include/point_cloud_transport/simple_subscriber_plugin.hpp index 626e211..19f2a83 100644 --- a/include/point_cloud_transport/simple_subscriber_plugin.hpp +++ b/include/point_cloud_transport/simple_subscriber_plugin.hpp @@ -1,35 +1,33 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// Copyright (c) 2019, paplhjak +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #ifndef POINT_CLOUD_TRANSPORT__SIMPLE_SUBSCRIBER_PLUGIN_HPP_ #define POINT_CLOUD_TRANSPORT__SIMPLE_SUBSCRIBER_PLUGIN_HPP_ @@ -124,10 +122,6 @@ class SimpleSubscriberPlugin : public SubscriberPlugin impl_.reset(); } - void declareParameters() override - { - } - /** * \brief Decode the given compressed pointcloud into a raw message. * \param[in] compressed The input compressed pointcloud. @@ -193,12 +187,12 @@ class SimpleSubscriberPlugin : public SubscriberPlugin this->declareParameters(); } - void subscribeImplWithOptions( + void subscribeImpl( std::shared_ptr node, const std::string & base_topic, const Callback & callback, rmw_qos_profile_t custom_qos, - rclcpp::SubscriptionOptions options) + rclcpp::SubscriptionOptions options) override { impl_ = std::make_unique(node); auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos), custom_qos); diff --git a/include/point_cloud_transport/single_subscriber_publisher.hpp b/include/point_cloud_transport/single_subscriber_publisher.hpp index ae3e2a4..7fdbe3b 100644 --- a/include/point_cloud_transport/single_subscriber_publisher.hpp +++ b/include/point_cloud_transport/single_subscriber_publisher.hpp @@ -1,35 +1,33 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// Copyright (c) 2019, paplhjak +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #ifndef POINT_CLOUD_TRANSPORT__SINGLE_SUBSCRIBER_PUBLISHER_HPP_ #define POINT_CLOUD_TRANSPORT__SINGLE_SUBSCRIBER_PUBLISHER_HPP_ diff --git a/include/point_cloud_transport/subscriber.hpp b/include/point_cloud_transport/subscriber.hpp index 85e575a..0fe1fa8 100644 --- a/include/point_cloud_transport/subscriber.hpp +++ b/include/point_cloud_transport/subscriber.hpp @@ -1,35 +1,33 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// Copyright (c) 2019, paplhjak +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #ifndef POINT_CLOUD_TRANSPORT__SUBSCRIBER_HPP_ #define POINT_CLOUD_TRANSPORT__SUBSCRIBER_HPP_ diff --git a/include/point_cloud_transport/subscriber_filter.hpp b/include/point_cloud_transport/subscriber_filter.hpp index 77a6718..8b9f42a 100644 --- a/include/point_cloud_transport/subscriber_filter.hpp +++ b/include/point_cloud_transport/subscriber_filter.hpp @@ -1,35 +1,33 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// Copyright (c) 2019, paplhjak +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #ifndef POINT_CLOUD_TRANSPORT__SUBSCRIBER_FILTER_HPP_ #define POINT_CLOUD_TRANSPORT__SUBSCRIBER_FILTER_HPP_ diff --git a/include/point_cloud_transport/subscriber_plugin.hpp b/include/point_cloud_transport/subscriber_plugin.hpp index 41ec0d3..06f974d 100644 --- a/include/point_cloud_transport/subscriber_plugin.hpp +++ b/include/point_cloud_transport/subscriber_plugin.hpp @@ -1,35 +1,33 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// Copyright (c) 2019, paplhjak +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #ifndef POINT_CLOUD_TRANSPORT__SUBSCRIBER_PLUGIN_HPP_ #define POINT_CLOUD_TRANSPORT__SUBSCRIBER_PLUGIN_HPP_ @@ -180,14 +178,7 @@ class POINT_CLOUD_TRANSPORT_PUBLIC SubscriberPlugin const std::string & base_topic, const Callback & callback, rmw_qos_profile_t custom_qos, - rclcpp::SubscriptionOptions options) - { - (void) options; - RCLCPP_ERROR( - node->get_logger(), - "SubscriberPlugin::subscribeImpl with five arguments has not been overridden"); - this->subscribeImpl(node, base_topic, callback, custom_qos); - } + rclcpp::SubscriptionOptions options) = 0; }; } // namespace point_cloud_transport diff --git a/include/point_cloud_transport/transport_hints.hpp b/include/point_cloud_transport/transport_hints.hpp index f3dc8a4..be533f5 100644 --- a/include/point_cloud_transport/transport_hints.hpp +++ b/include/point_cloud_transport/transport_hints.hpp @@ -1,35 +1,33 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// Copyright (c) 2019, paplhjak +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #ifndef POINT_CLOUD_TRANSPORT__TRANSPORT_HINTS_HPP_ #define POINT_CLOUD_TRANSPORT__TRANSPORT_HINTS_HPP_ diff --git a/point_cloud_transport/common.py b/point_cloud_transport/common.py index 0d860e8..6485848 100755 --- a/point_cloud_transport/common.py +++ b/point_cloud_transport/common.py @@ -1,6 +1,5 @@ # Copyright (c) 2023, John D'Angelo # Copyright (c) 2023, Czech Technical University in Prague -# All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions diff --git a/point_cloud_transport/publisher.py b/point_cloud_transport/publisher.py index 5d6541c..3d2c4cc 100755 --- a/point_cloud_transport/publisher.py +++ b/point_cloud_transport/publisher.py @@ -2,7 +2,6 @@ # Copyright (c) 2023, John D'Angelo # Copyright (c) 2023, Czech Technical University in Prague -# All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions diff --git a/point_cloud_transport/subscriber.py b/point_cloud_transport/subscriber.py index 78a846c..f1484cd 100755 --- a/point_cloud_transport/subscriber.py +++ b/point_cloud_transport/subscriber.py @@ -2,7 +2,6 @@ # Copyright (c) 2023, John D'Angelo # Copyright (c) 2023, Czech Technical University in Prague -# All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions diff --git a/src/list_transports.cpp b/src/list_transports.cpp index 328d530..a2aac22 100644 --- a/src/list_transports.cpp +++ b/src/list_transports.cpp @@ -1,35 +1,33 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// Copyright (c) 2019, paplhjak +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #include #include diff --git a/src/manifest.cpp b/src/manifest.cpp index 2907751..f15d520 100644 --- a/src/manifest.cpp +++ b/src/manifest.cpp @@ -1,34 +1,31 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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 HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// Copyright (c) 2019, paplhjak +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #include diff --git a/src/point_cloud_codec.cpp b/src/point_cloud_codec.cpp index eb76346..76883df 100644 --- a/src/point_cloud_codec.cpp +++ b/src/point_cloud_codec.cpp @@ -1,35 +1,33 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// Copyright (c) 2019, paplhjak +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #include #include diff --git a/src/point_cloud_common.cpp b/src/point_cloud_common.cpp index 27ff72a..673016a 100644 --- a/src/point_cloud_common.cpp +++ b/src/point_cloud_common.cpp @@ -1,33 +1,31 @@ -/* - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #include #include diff --git a/src/point_cloud_transport.cpp b/src/point_cloud_transport.cpp index f4d692c..d2502ab 100644 --- a/src/point_cloud_transport.cpp +++ b/src/point_cloud_transport.cpp @@ -1,35 +1,33 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// Copyright (c) 2019, paplhjak +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #include #include diff --git a/src/publisher.cpp b/src/publisher.cpp index 391f8c8..2b3f554 100644 --- a/src/publisher.cpp +++ b/src/publisher.cpp @@ -1,35 +1,33 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// Copyright (c) 2019, paplhjak +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #include #include diff --git a/src/publisher_plugin.cpp b/src/publisher_plugin.cpp index a4d8c5b..adb580b 100644 --- a/src/publisher_plugin.cpp +++ b/src/publisher_plugin.cpp @@ -1,35 +1,33 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// Copyright (c) 2019, paplhjak +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #include #include diff --git a/src/pybind_codec.cpp b/src/pybind_codec.cpp index 4ca6391..68168fb 100644 --- a/src/pybind_codec.cpp +++ b/src/pybind_codec.cpp @@ -1,33 +1,31 @@ -/* - * Copyright (c) 2023, John D'Angelo - * 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 HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, John D'Angelo +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #include #include diff --git a/src/raw_subscriber.cpp b/src/raw_subscriber.cpp index 644fa44..337dda0 100644 --- a/src/raw_subscriber.cpp +++ b/src/raw_subscriber.cpp @@ -1,35 +1,33 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// Copyright (c) 2019, paplhjak +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #include @@ -53,6 +51,10 @@ std::string RawSubscriber::getDataType() const return "sensor_msgs/msg/PointCloud2"; } +void RawSubscriber::declareParameters() +{ +} + SubscriberPlugin::DecodeResult RawSubscriber::decodeTyped( const sensor_msgs::msg::PointCloud2::ConstSharedPtr & compressed) const { diff --git a/src/republish.cpp b/src/republish.cpp index f32768d..b7e8666 100644 --- a/src/republish.cpp +++ b/src/republish.cpp @@ -1,35 +1,33 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// Copyright (c) 2019, paplhjak +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #include #include diff --git a/src/republish_program.cpp b/src/republish_program.cpp index d10e961..bc1574b 100644 --- a/src/republish_program.cpp +++ b/src/republish_program.cpp @@ -1,35 +1,33 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// Copyright (c) 2019, paplhjak +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #include "point_cloud_transport/republish.hpp" diff --git a/src/single_subscriber_publisher.cpp b/src/single_subscriber_publisher.cpp index 6711a0f..25b4bf0 100644 --- a/src/single_subscriber_publisher.cpp +++ b/src/single_subscriber_publisher.cpp @@ -1,35 +1,33 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// Copyright (c) 2019, paplhjak +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +/// #include diff --git a/src/subscriber.cpp b/src/subscriber.cpp index ed48b96..cecb649 100644 --- a/src/subscriber.cpp +++ b/src/subscriber.cpp @@ -1,35 +1,33 @@ -/* - * Copyright (c) 2023, Czech Technical University in Prague - * Copyright (c) 2019, paplhjak - * 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 copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// Copyright (c) 2023, Czech Technical University in Prague +// Copyright (c) 2019, paplhjak +// Copyright (c) 2009, Willow Garage, Inc. +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// #include #include From dc885294a0e8cd2a71ab2d61ead6f0c2b9a462c1 Mon Sep 17 00:00:00 2001 From: john-maidbot <78750993+john-maidbot@users.noreply.github.com> Date: Thu, 17 Aug 2023 09:18:33 -0500 Subject: [PATCH 28/28] One more cleanup pass (#19) * Add gitignore * Test coverage for message filter subscriber * Remove cras expected.hpp * Standardize docstrings * Fix typo --- .gitignore | 1 + .vscode/settings.json | 90 - CMakeLists.txt | 10 +- ThirdParty/expected/include/tl/expected.hpp | 2519 +++++++++-------- doc/.gitignore | 2 - doc/conf.py | 5 - doc/index.rst | 36 - doc/modules.rst | 10 - include/point_cloud_transport/expected.hpp | 76 - .../point_cloud_codec.hpp | 163 +- .../point_cloud_common.hpp | 28 +- .../point_cloud_transport.hpp | 94 +- .../publisher_plugin.hpp | 22 +- .../point_cloud_transport/raw_subscriber.hpp | 12 +- .../simple_publisher_plugin.hpp | 106 +- .../simple_subscriber_plugin.hpp | 62 +- include/point_cloud_transport/subscriber.hpp | 54 +- .../subscriber_filter.hpp | 89 +- .../subscriber_plugin.hpp | 95 +- .../point_cloud_transport/transport_hints.hpp | 24 +- rosdoc.yaml | 10 - setup.py | 12 - src/publisher.cpp | 37 - src/publisher_plugin.cpp | 4 +- src/utilities/utilities.hpp | 16 +- test/test_message_filter.cpp | 89 + test/test_message_passing.cpp | 4 +- test/test_publisher.cpp | 8 +- test/test_qos_override.cpp | 36 +- test/test_subscriber.cpp | 8 +- 30 files changed, 1929 insertions(+), 1793 deletions(-) create mode 100644 .gitignore delete mode 100644 .vscode/settings.json delete mode 100644 doc/.gitignore delete mode 100644 doc/conf.py delete mode 100644 doc/index.rst delete mode 100644 doc/modules.rst delete mode 100644 include/point_cloud_transport/expected.hpp delete mode 100644 rosdoc.yaml delete mode 100644 setup.py create mode 100644 test/test_message_filter.cpp diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..75ec3f0 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +.vscode/* \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index 4ad68de..0000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,90 +0,0 @@ -{ - "files.associations": { - "functional": "cpp", - "cctype": "cpp", - "clocale": "cpp", - "cmath": "cpp", - "csignal": "cpp", - "cstdarg": "cpp", - "cstddef": "cpp", - "cstdio": "cpp", - "cstdlib": "cpp", - "cstring": "cpp", - "ctime": "cpp", - "cwchar": "cpp", - "cwctype": "cpp", - "any": "cpp", - "array": "cpp", - "atomic": "cpp", - "hash_map": "cpp", - "hash_set": "cpp", - "strstream": "cpp", - "bit": "cpp", - "*.tcc": "cpp", - "bitset": "cpp", - "cfenv": "cpp", - "charconv": "cpp", - "chrono": "cpp", - "cinttypes": "cpp", - "codecvt": "cpp", - "compare": "cpp", - "complex": "cpp", - "concepts": "cpp", - "condition_variable": "cpp", - "coroutine": "cpp", - "cstdint": "cpp", - "deque": "cpp", - "forward_list": "cpp", - "list": "cpp", - "map": "cpp", - "set": "cpp", - "string": "cpp", - "unordered_map": "cpp", - "unordered_set": "cpp", - "vector": "cpp", - "exception": "cpp", - "algorithm": "cpp", - "iterator": "cpp", - "memory": "cpp", - "memory_resource": "cpp", - "numeric": "cpp", - "optional": "cpp", - "random": "cpp", - "ratio": "cpp", - "regex": "cpp", - "source_location": "cpp", - "string_view": "cpp", - "system_error": "cpp", - "tuple": "cpp", - "type_traits": "cpp", - "utility": "cpp", - "slist": "cpp", - "fstream": "cpp", - "future": "cpp", - "initializer_list": "cpp", - "iomanip": "cpp", - "iosfwd": "cpp", - "iostream": "cpp", - "istream": "cpp", - "limits": "cpp", - "mutex": "cpp", - "new": "cpp", - "numbers": "cpp", - "ostream": "cpp", - "ranges": "cpp", - "scoped_allocator": "cpp", - "semaphore": "cpp", - "shared_mutex": "cpp", - "span": "cpp", - "sstream": "cpp", - "stdexcept": "cpp", - "stop_token": "cpp", - "streambuf": "cpp", - "thread": "cpp", - "typeindex": "cpp", - "typeinfo": "cpp", - "valarray": "cpp", - "variant": "cpp", - "expected": "cpp" - } -} \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index f8a0f97..59d6f77 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -162,7 +162,6 @@ ament_export_dependencies(message_filters rclcpp sensor_msgs pluginlib) if(BUILD_TESTING) set( _linter_excludes - doc/conf.py ${expected_SOURCE_DIR}/include/tl/expected.hpp ) @@ -245,6 +244,15 @@ if(BUILD_TESTING) ) endif() + ament_add_gtest(${PROJECT_NAME}-message-filter test/test_message_filter.cpp) + if(TARGET ${PROJECT_NAME}-message-filter) + target_link_libraries(${PROJECT_NAME}-message-filter ${PROJECT_NAME}) + ament_target_dependencies(${PROJECT_NAME}-message-filter + message_filters + pluginlib + ) + endif() + ament_add_gtest(${PROJECT_NAME}-single_subscriber_publisher test/test_single_subscriber_publisher.cpp) if(TARGET ${PROJECT_NAME}-single_subscriber_publisher) target_link_libraries(${PROJECT_NAME}-single_subscriber_publisher ${PROJECT_NAME}) diff --git a/ThirdParty/expected/include/tl/expected.hpp b/ThirdParty/expected/include/tl/expected.hpp index afee404..c0def20 100644 --- a/ThirdParty/expected/include/tl/expected.hpp +++ b/ThirdParty/expected/include/tl/expected.hpp @@ -36,18 +36,18 @@ #define TL_EXPECTED_MSVC2015_CONSTEXPR constexpr #endif -#if (defined(__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ <= 9 && \ - !defined(__clang__)) +#if (defined(__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ <= 9 && \ + !defined(__clang__)) #define TL_EXPECTED_GCC49 #endif -#if (defined(__GNUC__) && __GNUC__ == 5 && __GNUC_MINOR__ <= 4 && \ - !defined(__clang__)) +#if (defined(__GNUC__) && __GNUC__ == 5 && __GNUC_MINOR__ <= 4 && \ + !defined(__clang__)) #define TL_EXPECTED_GCC54 #endif -#if (defined(__GNUC__) && __GNUC__ == 5 && __GNUC_MINOR__ <= 5 && \ - !defined(__clang__)) +#if (defined(__GNUC__) && __GNUC__ == 5 && __GNUC_MINOR__ <= 5 && \ + !defined(__clang__)) #define TL_EXPECTED_GCC55 #endif @@ -56,24 +56,24 @@ #if (__cplusplus > 201103L) && !defined(TL_EXPECTED_GCC49) #include #define TL_ASSERT(x) assert(x) -#else +#else #define TL_ASSERT(x) #endif #endif -#if (defined(__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ <= 9 && \ - !defined(__clang__)) +#if (defined(__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ <= 9 && \ + !defined(__clang__)) // GCC < 5 doesn't support overloading on const&& for member functions #define TL_EXPECTED_NO_CONSTRR // GCC < 5 doesn't support some standard C++11 type traits -#define TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(T) \ +#define TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(T) \ std::has_trivial_copy_constructor -#define TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(T) \ +#define TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(T) \ std::has_trivial_copy_assign // This one will be different for GCC 5.7 if it's ever supported -#define TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(T) \ +#define TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(T) \ std::is_trivially_destructible // GCC 5 < v < 8 has a bug in is_trivially_copy_constructible which breaks @@ -81,31 +81,33 @@ #elif (defined(__GNUC__) && __GNUC__ < 8 && !defined(__clang__)) #ifndef TL_GCC_LESS_8_TRIVIALLY_COPY_CONSTRUCTIBLE_MUTEX #define TL_GCC_LESS_8_TRIVIALLY_COPY_CONSTRUCTIBLE_MUTEX -namespace tl { -namespace detail { -template +namespace tl +{ +namespace detail +{ +template struct is_trivially_copy_constructible - : std::is_trivially_copy_constructible {}; + : std::is_trivially_copy_constructible {}; #ifdef _GLIBCXX_VECTOR -template -struct is_trivially_copy_constructible> : std::false_type {}; +template +struct is_trivially_copy_constructible>: std::false_type {}; #endif } // namespace detail } // namespace tl #endif -#define TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(T) \ +#define TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(T) \ tl::detail::is_trivially_copy_constructible -#define TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(T) \ +#define TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(T) \ std::is_trivially_copy_assignable -#define TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(T) \ +#define TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(T) \ std::is_trivially_destructible #else -#define TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(T) \ +#define TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(T) \ std::is_trivially_copy_constructible -#define TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(T) \ +#define TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(T) \ std::is_trivially_copy_assignable -#define TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(T) \ +#define TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(T) \ std::is_trivially_destructible #endif @@ -119,97 +121,115 @@ struct is_trivially_copy_constructible> : std::false_type {}; #define TL_EXPECTED_GCC49_CONSTEXPR constexpr #endif -#if (__cplusplus == 201103L || defined(TL_EXPECTED_MSVC2015) || \ - defined(TL_EXPECTED_GCC49)) +#if (__cplusplus == 201103L || defined(TL_EXPECTED_MSVC2015) || \ + defined(TL_EXPECTED_GCC49)) #define TL_EXPECTED_11_CONSTEXPR #else #define TL_EXPECTED_11_CONSTEXPR constexpr #endif -namespace tl { -template class expected; +namespace tl +{ +template +class expected; #ifndef TL_MONOSTATE_INPLACE_MUTEX #define TL_MONOSTATE_INPLACE_MUTEX class monostate {}; -struct in_place_t { +struct in_place_t +{ explicit in_place_t() = default; }; static constexpr in_place_t in_place{}; #endif -template class unexpected { +template +class unexpected +{ public: static_assert(!std::is_same::value, "E must not be void"); unexpected() = delete; - constexpr explicit unexpected(const E &e) : m_val(e) {} - - constexpr explicit unexpected(E &&e) : m_val(std::move(e)) {} - - template ::value>::type * = nullptr> - constexpr explicit unexpected(Args &&...args) - : m_val(std::forward(args)...) {} - template < - class U, class... Args, - typename std::enable_if &, Args &&...>::value>::type * = nullptr> - constexpr explicit unexpected(std::initializer_list l, Args &&...args) - : m_val(l, std::forward(args)...) {} - - constexpr const E &value() const & { return m_val; } - TL_EXPECTED_11_CONSTEXPR E &value() & { return m_val; } - TL_EXPECTED_11_CONSTEXPR E &&value() && { return std::move(m_val); } - constexpr const E &&value() const && { return std::move(m_val); } + constexpr explicit unexpected(const E & e) + : m_val(e) {} + + constexpr explicit unexpected(E && e) + : m_val(std::move(e)) {} + + template::value>::type * = nullptr> + constexpr explicit unexpected(Args &&... args) + : m_val(std::forward(args)...) {} + template< + class U, class ... Args, + typename std::enable_if &, Args &&...>::value>::type * = nullptr> + constexpr explicit unexpected(std::initializer_list l, Args &&... args) + : m_val(l, std::forward(args)...) {} + + constexpr const E & value() const & {return m_val;} + TL_EXPECTED_11_CONSTEXPR E & value() & {return m_val;} + TL_EXPECTED_11_CONSTEXPR E && value() && {return std::move(m_val);} + constexpr const E && value() const && {return std::move(m_val);} private: E m_val; }; #ifdef __cpp_deduction_guides -template unexpected(E) -> unexpected; +template +unexpected(E)->unexpected; #endif -template -constexpr bool operator==(const unexpected &lhs, const unexpected &rhs) { +template +constexpr bool operator==(const unexpected & lhs, const unexpected & rhs) +{ return lhs.value() == rhs.value(); } -template -constexpr bool operator!=(const unexpected &lhs, const unexpected &rhs) { +template +constexpr bool operator!=(const unexpected & lhs, const unexpected & rhs) +{ return lhs.value() != rhs.value(); } -template -constexpr bool operator<(const unexpected &lhs, const unexpected &rhs) { +template +constexpr bool operator<(const unexpected & lhs, const unexpected & rhs) +{ return lhs.value() < rhs.value(); } -template -constexpr bool operator<=(const unexpected &lhs, const unexpected &rhs) { +template +constexpr bool operator<=(const unexpected & lhs, const unexpected & rhs) +{ return lhs.value() <= rhs.value(); } -template -constexpr bool operator>(const unexpected &lhs, const unexpected &rhs) { +template +constexpr bool operator>(const unexpected & lhs, const unexpected & rhs) +{ return lhs.value() > rhs.value(); } -template -constexpr bool operator>=(const unexpected &lhs, const unexpected &rhs) { +template +constexpr bool operator>=(const unexpected & lhs, const unexpected & rhs) +{ return lhs.value() >= rhs.value(); } -template -unexpected::type> make_unexpected(E &&e) { +template +unexpected::type> make_unexpected(E && e) +{ return unexpected::type>(std::forward(e)); } -struct unexpect_t { +struct unexpect_t +{ unexpect_t() = default; }; static constexpr unexpect_t unexpect{}; -namespace detail { -template -[[noreturn]] TL_EXPECTED_11_CONSTEXPR void throw_exception(E &&e) { +namespace detail +{ +template +[[noreturn]] TL_EXPECTED_11_CONSTEXPR void throw_exception(E && e) +{ #ifdef TL_EXPECTED_EXCEPTIONS_ENABLED throw std::forward(e); #else @@ -225,21 +245,23 @@ template #ifndef TL_TRAITS_MUTEX #define TL_TRAITS_MUTEX // C++14-style aliases for brevity -template using remove_const_t = typename std::remove_const::type; -template +template using remove_const_t = typename std::remove_const::type; +template using remove_reference_t = typename std::remove_reference::type; -template using decay_t = typename std::decay::type; -template +template using decay_t = typename std::decay::type; +template using enable_if_t = typename std::enable_if::type; -template +template using conditional_t = typename std::conditional::type; // std::conjunction from C++17 -template struct conjunction : std::true_type {}; -template struct conjunction : B {}; -template +template +struct conjunction : std::true_type {}; +template +struct conjunction: B {}; +template struct conjunction - : std::conditional, B>::type {}; + : std::conditional, B>::type {}; #if defined(_LIBCPP_VERSION) && __cplusplus == 201103L #define TL_TRAITS_LIBCXX_MEM_FN_WORKAROUND @@ -249,191 +271,204 @@ struct conjunction // which results in a hard-error when using it in a noexcept expression // in some cases. This is a check to workaround the common failing case. #ifdef TL_TRAITS_LIBCXX_MEM_FN_WORKAROUND -template +template struct is_pointer_to_non_const_member_func : std::false_type {}; -template +template struct is_pointer_to_non_const_member_func - : std::true_type {}; -template + : std::true_type {}; +template struct is_pointer_to_non_const_member_func - : std::true_type {}; -template + : std::true_type {}; +template struct is_pointer_to_non_const_member_func - : std::true_type {}; -template + : std::true_type {}; +template struct is_pointer_to_non_const_member_func - : std::true_type {}; -template + : std::true_type {}; +template struct is_pointer_to_non_const_member_func - : std::true_type {}; -template + : std::true_type {}; +template struct is_pointer_to_non_const_member_func - : std::true_type {}; - -template struct is_const_or_const_ref : std::false_type {}; -template struct is_const_or_const_ref : std::true_type {}; -template struct is_const_or_const_ref : std::true_type {}; + : std::true_type {}; + +template +struct is_const_or_const_ref : std::false_type {}; +template +struct is_const_or_const_ref: std::true_type {}; +template +struct is_const_or_const_ref: std::true_type {}; #endif // std::invoke from C++17 // https://stackoverflow.com/questions/38288042/c11-14-invoke-workaround -template < - typename Fn, typename... Args, +template< + typename Fn, typename ... Args, #ifdef TL_TRAITS_LIBCXX_MEM_FN_WORKAROUND - typename = enable_if_t::value && - is_const_or_const_ref::value)>, + typename = enable_if_t::value && + is_const_or_const_ref::value)>, #endif - typename = enable_if_t>::value>, int = 0> -constexpr auto invoke(Fn &&f, Args &&...args) noexcept( - noexcept(std::mem_fn(f)(std::forward(args)...))) - -> decltype(std::mem_fn(f)(std::forward(args)...)) { + typename = enable_if_t>::value>, int = 0> +constexpr auto invoke(Fn && f, Args &&... args) noexcept( + noexcept(std::mem_fn(f)(std::forward(args)...))) +-> decltype(std::mem_fn(f)(std::forward(args)...)) +{ return std::mem_fn(f)(std::forward(args)...); } -template >::value>> -constexpr auto invoke(Fn &&f, Args &&...args) noexcept( - noexcept(std::forward(f)(std::forward(args)...))) - -> decltype(std::forward(f)(std::forward(args)...)) { +template>::value>> +constexpr auto invoke(Fn && f, Args &&... args) noexcept( + noexcept(std::forward(f)(std::forward(args)...))) +-> decltype(std::forward(f)(std::forward(args)...)) +{ return std::forward(f)(std::forward(args)...); } // std::invoke_result from C++17 -template struct invoke_result_impl; +template +struct invoke_result_impl; -template +template struct invoke_result_impl< - F, - decltype(detail::invoke(std::declval(), std::declval()...), void()), - Us...> { + F, + decltype(detail::invoke(std::declval(), std::declval()...), void()), + Us...> +{ using type = - decltype(detail::invoke(std::declval(), std::declval()...)); + decltype(detail::invoke(std::declval(), std::declval()...)); }; -template +template using invoke_result = invoke_result_impl; -template +template using invoke_result_t = typename invoke_result::type; #if defined(_MSC_VER) && _MSC_VER <= 1900 // TODO make a version which works with MSVC 2015 -template struct is_swappable : std::true_type {}; +template +struct is_swappable : std::true_type {}; -template struct is_nothrow_swappable : std::true_type {}; +template +struct is_nothrow_swappable : std::true_type {}; #else // https://stackoverflow.com/questions/26744589/what-is-a-proper-way-to-implement-is-swappable-to-test-for-the-swappable-concept -namespace swap_adl_tests { +namespace swap_adl_tests +{ // if swap ADL finds this then it would call std::swap otherwise (same // signature) struct tag {}; -template tag swap(T &, T &); -template tag swap(T (&a)[N], T (&b)[N]); +template tag swap(T &, T &); +template tag swap(T(&a)[N], T(&b)[N]); // helper functions to test if an unqualified swap is possible, and if it // becomes std::swap -template std::false_type can_swap(...) noexcept(false); -template (), std::declval()))> -std::true_type can_swap(int) noexcept(noexcept(swap(std::declval(), - std::declval()))); - -template std::false_type uses_std(...); -template +template std::false_type can_swap(...) noexcept(false); +template(), std::declval()))> +std::true_type can_swap(int) noexcept(noexcept(swap( + std::declval(), + std::declval()))); + +template std::false_type uses_std(...); +template std::is_same(), std::declval())), tag> uses_std(int); -template +template struct is_std_swap_noexcept - : std::integral_constant::value && - std::is_nothrow_move_assignable::value> {}; + : std::integral_constant::value && + std::is_nothrow_move_assignable::value> {}; -template -struct is_std_swap_noexcept : is_std_swap_noexcept {}; +template +struct is_std_swap_noexcept: is_std_swap_noexcept {}; -template +template struct is_adl_swap_noexcept - : std::integral_constant(0))> {}; + : std::integral_constant(0))> {}; } // namespace swap_adl_tests -template +template struct is_swappable - : std::integral_constant< - bool, - decltype(detail::swap_adl_tests::can_swap(0))::value && - (!decltype(detail::swap_adl_tests::uses_std(0))::value || - (std::is_move_assignable::value && - std::is_move_constructible::value))> {}; - -template + : std::integral_constant< + bool, + decltype(detail::swap_adl_tests::can_swap(0))::value && + (!decltype(detail::swap_adl_tests::uses_std(0))::value || + (std::is_move_assignable::value && + std::is_move_constructible::value))> {}; + +template struct is_swappable - : std::integral_constant< - bool, - decltype(detail::swap_adl_tests::can_swap(0))::value && - (!decltype(detail::swap_adl_tests::uses_std( - 0))::value || - is_swappable::value)> {}; - -template + : std::integral_constant< + bool, + decltype(detail::swap_adl_tests::can_swap(0))::value && + (!decltype(detail::swap_adl_tests::uses_std( + 0))::value || + is_swappable::value)> {}; + +template struct is_nothrow_swappable - : std::integral_constant< - bool, - is_swappable::value && - ((decltype(detail::swap_adl_tests::uses_std(0))::value && - detail::swap_adl_tests::is_std_swap_noexcept::value) || - (!decltype(detail::swap_adl_tests::uses_std(0))::value && - detail::swap_adl_tests::is_adl_swap_noexcept::value))> {}; + : std::integral_constant< + bool, + is_swappable::value && + ((decltype(detail::swap_adl_tests::uses_std(0))::value && + detail::swap_adl_tests::is_std_swap_noexcept::value) || + (!decltype(detail::swap_adl_tests::uses_std(0))::value && + detail::swap_adl_tests::is_adl_swap_noexcept::value))> {}; #endif #endif // Trait for checking if a type is a tl::expected -template struct is_expected_impl : std::false_type {}; -template -struct is_expected_impl> : std::true_type {}; -template using is_expected = is_expected_impl>; +template +struct is_expected_impl : std::false_type {}; +template +struct is_expected_impl>: std::true_type {}; +template using is_expected = is_expected_impl>; -template +template using expected_enable_forward_value = detail::enable_if_t< - std::is_constructible::value && - !std::is_same, in_place_t>::value && - !std::is_same, detail::decay_t>::value && - !std::is_same, detail::decay_t>::value>; + std::is_constructible::value && + !std::is_same, in_place_t>::value && + !std::is_same, detail::decay_t>::value && + !std::is_same, detail::decay_t>::value>; -template +template using expected_enable_from_other = detail::enable_if_t< - std::is_constructible::value && - std::is_constructible::value && - !std::is_constructible &>::value && - !std::is_constructible &&>::value && - !std::is_constructible &>::value && - !std::is_constructible &&>::value && - !std::is_convertible &, T>::value && - !std::is_convertible &&, T>::value && - !std::is_convertible &, T>::value && - !std::is_convertible &&, T>::value>; - -template + std::is_constructible::value && + std::is_constructible::value && + !std::is_constructible &>::value && + !std::is_constructible&&>::value && + !std::is_constructible &>::value && + !std::is_constructible&&>::value && + !std::is_convertible &, T>::value && + !std::is_convertible&&, T>::value && + !std::is_convertible &, T>::value && + !std::is_convertible&&, T>::value>; + +template using is_void_or = conditional_t::value, std::true_type, U>; -template +template using is_copy_constructible_or_void = - is_void_or>; + is_void_or>; -template +template using is_move_constructible_or_void = - is_void_or>; + is_void_or>; -template +template using is_copy_assignable_or_void = is_void_or>; -template +template using is_move_assignable_or_void = is_void_or>; } // namespace detail -namespace detail { +namespace detail +{ struct no_init_t {}; static constexpr no_init_t no_init{}; @@ -443,39 +478,45 @@ static constexpr no_init_t no_init{}; // This specialization is for where neither `T` or `E` is trivially // destructible, so the destructors must be called on destruction of the // `expected` -template ::value, - bool = std::is_trivially_destructible::value> -struct expected_storage_base { - constexpr expected_storage_base() : m_val(T{}), m_has_val(true) {} - constexpr expected_storage_base(no_init_t) : m_no_init(), m_has_val(false) {} - - template ::value> * = - nullptr> - constexpr expected_storage_base(in_place_t, Args &&...args) - : m_val(std::forward(args)...), m_has_val(true) {} - - template &, Args &&...>::value> * = nullptr> - constexpr expected_storage_base(in_place_t, std::initializer_list il, - Args &&...args) - : m_val(il, std::forward(args)...), m_has_val(true) {} - template ::value> * = - nullptr> - constexpr explicit expected_storage_base(unexpect_t, Args &&...args) - : m_unexpect(std::forward(args)...), m_has_val(false) {} - - template &, Args &&...>::value> * = nullptr> - constexpr explicit expected_storage_base(unexpect_t, - std::initializer_list il, - Args &&...args) - : m_unexpect(il, std::forward(args)...), m_has_val(false) {} - - ~expected_storage_base() { +template::value, + bool = std::is_trivially_destructible::value> +struct expected_storage_base +{ + constexpr expected_storage_base() + : m_val(T{}), m_has_val(true) {} + constexpr expected_storage_base(no_init_t) + : m_no_init(), m_has_val(false) {} + + template::value> * = + nullptr> + constexpr expected_storage_base(in_place_t, Args &&... args) + : m_val(std::forward(args)...), m_has_val(true) {} + + template &, Args &&...>::value> * = nullptr> + constexpr expected_storage_base( + in_place_t, std::initializer_list il, + Args &&... args) + : m_val(il, std::forward(args)...), m_has_val(true) {} + template::value> * = + nullptr> + constexpr explicit expected_storage_base(unexpect_t, Args &&... args) + : m_unexpect(std::forward(args)...), m_has_val(false) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected_storage_base( + unexpect_t, + std::initializer_list il, + Args &&... args) + : m_unexpect(il, std::forward(args)...), m_has_val(false) {} + + ~expected_storage_base() + { if (m_has_val) { m_val.~T(); } else { @@ -492,35 +533,41 @@ struct expected_storage_base { // This specialization is for when both `T` and `E` are trivially-destructible, // so the destructor of the `expected` can be trivial. -template struct expected_storage_base { - constexpr expected_storage_base() : m_val(T{}), m_has_val(true) {} - constexpr expected_storage_base(no_init_t) : m_no_init(), m_has_val(false) {} - - template ::value> * = - nullptr> - constexpr expected_storage_base(in_place_t, Args &&...args) - : m_val(std::forward(args)...), m_has_val(true) {} - - template &, Args &&...>::value> * = nullptr> - constexpr expected_storage_base(in_place_t, std::initializer_list il, - Args &&...args) - : m_val(il, std::forward(args)...), m_has_val(true) {} - template ::value> * = - nullptr> - constexpr explicit expected_storage_base(unexpect_t, Args &&...args) - : m_unexpect(std::forward(args)...), m_has_val(false) {} - - template &, Args &&...>::value> * = nullptr> - constexpr explicit expected_storage_base(unexpect_t, - std::initializer_list il, - Args &&...args) - : m_unexpect(il, std::forward(args)...), m_has_val(false) {} +template +struct expected_storage_base +{ + constexpr expected_storage_base() + : m_val(T{}), m_has_val(true) {} + constexpr expected_storage_base(no_init_t) + : m_no_init(), m_has_val(false) {} + + template::value> * = + nullptr> + constexpr expected_storage_base(in_place_t, Args &&... args) + : m_val(std::forward(args)...), m_has_val(true) {} + + template &, Args &&...>::value> * = nullptr> + constexpr expected_storage_base( + in_place_t, std::initializer_list il, + Args &&... args) + : m_val(il, std::forward(args)...), m_has_val(true) {} + template::value> * = + nullptr> + constexpr explicit expected_storage_base(unexpect_t, Args &&... args) + : m_unexpect(std::forward(args)...), m_has_val(false) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected_storage_base( + unexpect_t, + std::initializer_list il, + Args &&... args) + : m_unexpect(il, std::forward(args)...), m_has_val(false) {} ~expected_storage_base() = default; union { @@ -532,38 +579,44 @@ template struct expected_storage_base { }; // T is trivial, E is not. -template struct expected_storage_base { - constexpr expected_storage_base() : m_val(T{}), m_has_val(true) {} +template +struct expected_storage_base +{ + constexpr expected_storage_base() + : m_val(T{}), m_has_val(true) {} TL_EXPECTED_MSVC2015_CONSTEXPR expected_storage_base(no_init_t) - : m_no_init(), m_has_val(false) {} - - template ::value> * = - nullptr> - constexpr expected_storage_base(in_place_t, Args &&...args) - : m_val(std::forward(args)...), m_has_val(true) {} - - template &, Args &&...>::value> * = nullptr> - constexpr expected_storage_base(in_place_t, std::initializer_list il, - Args &&...args) - : m_val(il, std::forward(args)...), m_has_val(true) {} - template ::value> * = - nullptr> - constexpr explicit expected_storage_base(unexpect_t, Args &&...args) - : m_unexpect(std::forward(args)...), m_has_val(false) {} - - template &, Args &&...>::value> * = nullptr> - constexpr explicit expected_storage_base(unexpect_t, - std::initializer_list il, - Args &&...args) - : m_unexpect(il, std::forward(args)...), m_has_val(false) {} - - ~expected_storage_base() { + : m_no_init(), m_has_val(false) {} + + template::value> * = + nullptr> + constexpr expected_storage_base(in_place_t, Args &&... args) + : m_val(std::forward(args)...), m_has_val(true) {} + + template &, Args &&...>::value> * = nullptr> + constexpr expected_storage_base( + in_place_t, std::initializer_list il, + Args &&... args) + : m_val(il, std::forward(args)...), m_has_val(true) {} + template::value> * = + nullptr> + constexpr explicit expected_storage_base(unexpect_t, Args &&... args) + : m_unexpect(std::forward(args)...), m_has_val(false) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected_storage_base( + unexpect_t, + std::initializer_list il, + Args &&... args) + : m_unexpect(il, std::forward(args)...), m_has_val(false) {} + + ~expected_storage_base() + { if (!m_has_val) { m_unexpect.~unexpected(); } @@ -578,37 +631,44 @@ template struct expected_storage_base { }; // E is trivial, T is not. -template struct expected_storage_base { - constexpr expected_storage_base() : m_val(T{}), m_has_val(true) {} - constexpr expected_storage_base(no_init_t) : m_no_init(), m_has_val(false) {} - - template ::value> * = - nullptr> - constexpr expected_storage_base(in_place_t, Args &&...args) - : m_val(std::forward(args)...), m_has_val(true) {} - - template &, Args &&...>::value> * = nullptr> - constexpr expected_storage_base(in_place_t, std::initializer_list il, - Args &&...args) - : m_val(il, std::forward(args)...), m_has_val(true) {} - template ::value> * = - nullptr> - constexpr explicit expected_storage_base(unexpect_t, Args &&...args) - : m_unexpect(std::forward(args)...), m_has_val(false) {} - - template &, Args &&...>::value> * = nullptr> - constexpr explicit expected_storage_base(unexpect_t, - std::initializer_list il, - Args &&...args) - : m_unexpect(il, std::forward(args)...), m_has_val(false) {} - - ~expected_storage_base() { +template +struct expected_storage_base +{ + constexpr expected_storage_base() + : m_val(T{}), m_has_val(true) {} + constexpr expected_storage_base(no_init_t) + : m_no_init(), m_has_val(false) {} + + template::value> * = + nullptr> + constexpr expected_storage_base(in_place_t, Args &&... args) + : m_val(std::forward(args)...), m_has_val(true) {} + + template &, Args &&...>::value> * = nullptr> + constexpr expected_storage_base( + in_place_t, std::initializer_list il, + Args &&... args) + : m_val(il, std::forward(args)...), m_has_val(true) {} + template::value> * = + nullptr> + constexpr explicit expected_storage_base(unexpect_t, Args &&... args) + : m_unexpect(std::forward(args)...), m_has_val(false) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected_storage_base( + unexpect_t, + std::initializer_list il, + Args &&... args) + : m_unexpect(il, std::forward(args)...), m_has_val(false) {} + + ~expected_storage_base() + { if (m_has_val) { m_val.~T(); } @@ -622,31 +682,37 @@ template struct expected_storage_base { }; // `T` is `void`, `E` is trivially-destructible -template struct expected_storage_base { +template +struct expected_storage_base +{ #if __GNUC__ <= 5 //no constexpr for GCC 4/5 bug #else TL_EXPECTED_MSVC2015_CONSTEXPR - #endif - expected_storage_base() : m_has_val(true) {} - - constexpr expected_storage_base(no_init_t) : m_val(), m_has_val(false) {} - - constexpr expected_storage_base(in_place_t) : m_has_val(true) {} - - template ::value> * = - nullptr> - constexpr explicit expected_storage_base(unexpect_t, Args &&...args) - : m_unexpect(std::forward(args)...), m_has_val(false) {} - - template &, Args &&...>::value> * = nullptr> - constexpr explicit expected_storage_base(unexpect_t, - std::initializer_list il, - Args &&...args) - : m_unexpect(il, std::forward(args)...), m_has_val(false) {} + #endif + expected_storage_base() + : m_has_val(true) {} + + constexpr expected_storage_base(no_init_t) + : m_val(), m_has_val(false) {} + + constexpr expected_storage_base(in_place_t) + : m_has_val(true) {} + + template::value> * = + nullptr> + constexpr explicit expected_storage_base(unexpect_t, Args &&... args) + : m_unexpect(std::forward(args)...), m_has_val(false) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected_storage_base( + unexpect_t, + std::initializer_list il, + Args &&... args) + : m_unexpect(il, std::forward(args)...), m_has_val(false) {} ~expected_storage_base() = default; struct dummy {}; @@ -658,27 +724,34 @@ template struct expected_storage_base { }; // `T` is `void`, `E` is not trivially-destructible -template struct expected_storage_base { - constexpr expected_storage_base() : m_dummy(), m_has_val(true) {} - constexpr expected_storage_base(no_init_t) : m_dummy(), m_has_val(false) {} - - constexpr expected_storage_base(in_place_t) : m_dummy(), m_has_val(true) {} - - template ::value> * = - nullptr> - constexpr explicit expected_storage_base(unexpect_t, Args &&...args) - : m_unexpect(std::forward(args)...), m_has_val(false) {} - - template &, Args &&...>::value> * = nullptr> - constexpr explicit expected_storage_base(unexpect_t, - std::initializer_list il, - Args &&...args) - : m_unexpect(il, std::forward(args)...), m_has_val(false) {} - - ~expected_storage_base() { +template +struct expected_storage_base +{ + constexpr expected_storage_base() + : m_dummy(), m_has_val(true) {} + constexpr expected_storage_base(no_init_t) + : m_dummy(), m_has_val(false) {} + + constexpr expected_storage_base(in_place_t) + : m_dummy(), m_has_val(true) {} + + template::value> * = + nullptr> + constexpr explicit expected_storage_base(unexpect_t, Args &&... args) + : m_unexpect(std::forward(args)...), m_has_val(false) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected_storage_base( + unexpect_t, + std::initializer_list il, + Args &&... args) + : m_unexpect(il, std::forward(args)...), m_has_val(false) {} + + ~expected_storage_base() + { if (!m_has_val) { m_unexpect.~unexpected(); } @@ -693,23 +766,27 @@ template struct expected_storage_base { // This base class provides some handy member functions which can be used in // further derived classes -template -struct expected_operations_base : expected_storage_base { +template +struct expected_operations_base : expected_storage_base +{ using expected_storage_base::expected_storage_base; - template void construct(Args &&...args) noexcept { + template void construct(Args &&... args) noexcept + { new (std::addressof(this->m_val)) T(std::forward(args)...); this->m_has_val = true; } - template void construct_with(Rhs &&rhs) noexcept { + template void construct_with(Rhs && rhs) noexcept + { new (std::addressof(this->m_val)) T(std::forward(rhs).get()); this->m_has_val = true; } - template void construct_error(Args &&...args) noexcept { + template void construct_error(Args &&... args) noexcept + { new (std::addressof(this->m_unexpect)) - unexpected(std::forward(args)...); + unexpected(std::forward(args)...); this->m_has_val = false; } @@ -721,10 +798,11 @@ struct expected_operations_base : expected_storage_base { // // This overload handles the case where we can just copy-construct `T` // directly into place without throwing. - template ::value> - * = nullptr> - void assign(const expected_operations_base &rhs) noexcept { + template::value> + * = nullptr> + void assign(const expected_operations_base & rhs) noexcept + { if (!this->m_has_val && rhs.m_has_val) { geterr().~unexpected(); construct(rhs.get()); @@ -735,11 +813,12 @@ struct expected_operations_base : expected_storage_base { // This overload handles the case where we can attempt to create a copy of // `T`, then no-throw move it into place if the copy was successful. - template ::value && - std::is_nothrow_move_constructible::value> - * = nullptr> - void assign(const expected_operations_base &rhs) noexcept { + template::value && + std::is_nothrow_move_constructible::value> + * = nullptr> + void assign(const expected_operations_base & rhs) noexcept + { if (!this->m_has_val && rhs.m_has_val) { T tmp = rhs.get(); geterr().~unexpected(); @@ -754,11 +833,12 @@ struct expected_operations_base : expected_storage_base { // If the construction succeeds, then everything is fine, but if it throws, // then we move the old unexpected value back into place before rethrowing the // exception. - template ::value && - !std::is_nothrow_move_constructible::value> - * = nullptr> - void assign(const expected_operations_base &rhs) { + template::value && + !std::is_nothrow_move_constructible::value> + * = nullptr> + void assign(const expected_operations_base & rhs) + { if (!this->m_has_val && rhs.m_has_val) { auto tmp = std::move(geterr()); geterr().~unexpected(); @@ -779,10 +859,11 @@ struct expected_operations_base : expected_storage_base { } // These overloads do the same as above, but for rvalues - template ::value> - * = nullptr> - void assign(expected_operations_base &&rhs) noexcept { + template::value> + * = nullptr> + void assign(expected_operations_base && rhs) noexcept + { if (!this->m_has_val && rhs.m_has_val) { geterr().~unexpected(); construct(std::move(rhs).get()); @@ -791,10 +872,11 @@ struct expected_operations_base : expected_storage_base { } } - template ::value> - * = nullptr> - void assign(expected_operations_base &&rhs) { + template::value> + * = nullptr> + void assign(expected_operations_base && rhs) + { if (!this->m_has_val && rhs.m_has_val) { auto tmp = std::move(geterr()); geterr().~unexpected(); @@ -816,7 +898,8 @@ struct expected_operations_base : expected_storage_base { #else // If exceptions are disabled then we can just copy-construct - void assign(const expected_operations_base &rhs) noexcept { + void assign(const expected_operations_base & rhs) noexcept + { if (!this->m_has_val && rhs.m_has_val) { geterr().~unexpected(); construct(rhs.get()); @@ -825,7 +908,8 @@ struct expected_operations_base : expected_storage_base { } } - void assign(expected_operations_base &&rhs) noexcept { + void assign(expected_operations_base && rhs) noexcept + { if (!this->m_has_val && rhs.m_has_val) { geterr().~unexpected(); construct(std::move(rhs).get()); @@ -837,7 +921,8 @@ struct expected_operations_base : expected_storage_base { #endif // The common part of move/copy assigning - template void assign_common(Rhs &&rhs) { + template void assign_common(Rhs && rhs) + { if (this->m_has_val) { if (rhs.m_has_val) { get() = std::forward(rhs).get(); @@ -852,52 +937,59 @@ struct expected_operations_base : expected_storage_base { } } - bool has_value() const { return this->m_has_val; } + bool has_value() const {return this->m_has_val;} - TL_EXPECTED_11_CONSTEXPR T &get() & { return this->m_val; } - constexpr const T &get() const & { return this->m_val; } - TL_EXPECTED_11_CONSTEXPR T &&get() && { return std::move(this->m_val); } + TL_EXPECTED_11_CONSTEXPR T & get() & {return this->m_val;} + constexpr const T & get() const & {return this->m_val;} + TL_EXPECTED_11_CONSTEXPR T && get() && {return std::move(this->m_val);} #ifndef TL_EXPECTED_NO_CONSTRR - constexpr const T &&get() const && { return std::move(this->m_val); } + constexpr const T && get() const && {return std::move(this->m_val);} #endif - TL_EXPECTED_11_CONSTEXPR unexpected &geterr() & { + TL_EXPECTED_11_CONSTEXPR unexpected & geterr() & + { return this->m_unexpect; } - constexpr const unexpected &geterr() const & { return this->m_unexpect; } - TL_EXPECTED_11_CONSTEXPR unexpected &&geterr() && { + constexpr const unexpected & geterr() const & {return this->m_unexpect;} + TL_EXPECTED_11_CONSTEXPR unexpected && geterr() && + { return std::move(this->m_unexpect); } #ifndef TL_EXPECTED_NO_CONSTRR - constexpr const unexpected &&geterr() const && { + constexpr const unexpected && geterr() const && + { return std::move(this->m_unexpect); } #endif - TL_EXPECTED_11_CONSTEXPR void destroy_val() { get().~T(); } + TL_EXPECTED_11_CONSTEXPR void destroy_val() {get().~T();} }; // This base class provides some handy member functions which can be used in // further derived classes -template -struct expected_operations_base : expected_storage_base { +template +struct expected_operations_base: expected_storage_base +{ using expected_storage_base::expected_storage_base; - template void construct() noexcept { this->m_has_val = true; } + template void construct() noexcept {this->m_has_val = true;} // This function doesn't use its argument, but needs it so that code in // levels above this can work independently of whether T is void - template void construct_with(Rhs &&) noexcept { + template void construct_with(Rhs &&) noexcept + { this->m_has_val = true; } - template void construct_error(Args &&...args) noexcept { + template void construct_error(Args &&... args) noexcept + { new (std::addressof(this->m_unexpect)) - unexpected(std::forward(args)...); + unexpected(std::forward(args)...); this->m_has_val = false; } - template void assign(Rhs &&rhs) noexcept { + template void assign(Rhs && rhs) noexcept + { if (!this->m_has_val) { if (rhs.m_has_val) { geterr().~unexpected(); @@ -912,43 +1004,50 @@ struct expected_operations_base : expected_storage_base { } } - bool has_value() const { return this->m_has_val; } + bool has_value() const {return this->m_has_val;} - TL_EXPECTED_11_CONSTEXPR unexpected &geterr() & { + TL_EXPECTED_11_CONSTEXPR unexpected & geterr() & + { return this->m_unexpect; } - constexpr const unexpected &geterr() const & { return this->m_unexpect; } - TL_EXPECTED_11_CONSTEXPR unexpected &&geterr() && { + constexpr const unexpected & geterr() const & {return this->m_unexpect;} + TL_EXPECTED_11_CONSTEXPR unexpected && geterr() && + { return std::move(this->m_unexpect); } #ifndef TL_EXPECTED_NO_CONSTRR - constexpr const unexpected &&geterr() const && { + constexpr const unexpected && geterr() const && + { return std::move(this->m_unexpect); } #endif - TL_EXPECTED_11_CONSTEXPR void destroy_val() { + TL_EXPECTED_11_CONSTEXPR void destroy_val() + { // no-op } }; // This class manages conditionally having a trivial copy constructor // This specialization is for when T and E are trivially copy constructible -template :: - value &&TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(E)::value> -struct expected_copy_base : expected_operations_base { +template:: + value && TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(E) ::value> +struct expected_copy_base : expected_operations_base +{ using expected_operations_base::expected_operations_base; }; // This specialization is for when T or E are not trivially copy constructible -template -struct expected_copy_base : expected_operations_base { +template +struct expected_copy_base: expected_operations_base +{ using expected_operations_base::expected_operations_base; expected_copy_base() = default; - expected_copy_base(const expected_copy_base &rhs) - : expected_operations_base(no_init) { + expected_copy_base(const expected_copy_base & rhs) + : expected_operations_base(no_init) + { if (rhs.has_value()) { this->construct_with(rhs); } else { @@ -956,9 +1055,9 @@ struct expected_copy_base : expected_operations_base { } } - expected_copy_base(expected_copy_base &&rhs) = default; - expected_copy_base &operator=(const expected_copy_base &rhs) = default; - expected_copy_base &operator=(expected_copy_base &&rhs) = default; + expected_copy_base(expected_copy_base && rhs) = default; + expected_copy_base & operator=(const expected_copy_base & rhs) = default; + expected_copy_base & operator=(expected_copy_base && rhs) = default; }; // This class manages conditionally having a trivial move constructor @@ -967,62 +1066,69 @@ struct expected_copy_base : expected_operations_base { // have to make do with a non-trivial move constructor even if T is trivially // move constructible #ifndef TL_EXPECTED_GCC49 -template >::value - &&std::is_trivially_move_constructible::value> -struct expected_move_base : expected_copy_base { +template>::value && + std::is_trivially_move_constructible::value> +struct expected_move_base : expected_copy_base +{ using expected_copy_base::expected_copy_base; }; #else -template struct expected_move_base; +template +struct expected_move_base; #endif -template -struct expected_move_base : expected_copy_base { +template +struct expected_move_base: expected_copy_base +{ using expected_copy_base::expected_copy_base; expected_move_base() = default; - expected_move_base(const expected_move_base &rhs) = default; + expected_move_base(const expected_move_base & rhs) = default; - expected_move_base(expected_move_base &&rhs) noexcept( - std::is_nothrow_move_constructible::value) - : expected_copy_base(no_init) { + expected_move_base(expected_move_base && rhs) noexcept( + std::is_nothrow_move_constructible::value) + : expected_copy_base(no_init) + { if (rhs.has_value()) { this->construct_with(std::move(rhs)); } else { this->construct_error(std::move(rhs.geterr())); } } - expected_move_base &operator=(const expected_move_base &rhs) = default; - expected_move_base &operator=(expected_move_base &&rhs) = default; + expected_move_base & operator=(const expected_move_base & rhs) = default; + expected_move_base & operator=(expected_move_base && rhs) = default; }; // This class manages conditionally having a trivial copy assignment operator -template >::value - &&TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(E)::value - &&TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(E)::value - &&TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(E)::value> -struct expected_copy_assign_base : expected_move_base { +template>::value && + TL_EXPECTED_IS_TRIVIALLY_COPY_ASSIGNABLE(E) ::value && + TL_EXPECTED_IS_TRIVIALLY_COPY_CONSTRUCTIBLE(E) ::value && + TL_EXPECTED_IS_TRIVIALLY_DESTRUCTIBLE(E) ::value> +struct expected_copy_assign_base : expected_move_base +{ using expected_move_base::expected_move_base; }; -template -struct expected_copy_assign_base : expected_move_base { +template +struct expected_copy_assign_base: expected_move_base +{ using expected_move_base::expected_move_base; expected_copy_assign_base() = default; - expected_copy_assign_base(const expected_copy_assign_base &rhs) = default; + expected_copy_assign_base(const expected_copy_assign_base & rhs) = default; - expected_copy_assign_base(expected_copy_assign_base &&rhs) = default; - expected_copy_assign_base &operator=(const expected_copy_assign_base &rhs) { + expected_copy_assign_base(expected_copy_assign_base && rhs) = default; + expected_copy_assign_base & operator=(const expected_copy_assign_base & rhs) + { this->assign(rhs); return *this; } expected_copy_assign_base & - operator=(expected_copy_assign_base &&rhs) = default; + operator=(expected_copy_assign_base && rhs) = default; }; // This class manages conditionally having a trivial move assignment operator @@ -1031,38 +1137,42 @@ struct expected_copy_assign_base : expected_move_base { // to make do with a non-trivial move assignment operator even if T is trivially // move assignable #ifndef TL_EXPECTED_GCC49 -template , - std::is_trivially_move_constructible, - std::is_trivially_move_assignable>>:: - value &&std::is_trivially_destructible::value - &&std::is_trivially_move_constructible::value - &&std::is_trivially_move_assignable::value> -struct expected_move_assign_base : expected_copy_assign_base { +template, + std::is_trivially_move_constructible, + std::is_trivially_move_assignable>>:: + value && std::is_trivially_destructible::value && + std::is_trivially_move_constructible::value && + std::is_trivially_move_assignable::value> +struct expected_move_assign_base : expected_copy_assign_base +{ using expected_copy_assign_base::expected_copy_assign_base; }; #else -template struct expected_move_assign_base; +template +struct expected_move_assign_base; #endif -template +template struct expected_move_assign_base - : expected_copy_assign_base { + : expected_copy_assign_base +{ using expected_copy_assign_base::expected_copy_assign_base; expected_move_assign_base() = default; - expected_move_assign_base(const expected_move_assign_base &rhs) = default; + expected_move_assign_base(const expected_move_assign_base & rhs) = default; - expected_move_assign_base(expected_move_assign_base &&rhs) = default; + expected_move_assign_base(expected_move_assign_base && rhs) = default; expected_move_assign_base & - operator=(const expected_move_assign_base &rhs) = default; + operator=(const expected_move_assign_base & rhs) = default; expected_move_assign_base & - operator=(expected_move_assign_base &&rhs) noexcept( - std::is_nothrow_move_constructible::value - &&std::is_nothrow_move_assignable::value) { + operator=(expected_move_assign_base && rhs) noexcept( + std::is_nothrow_move_constructible::value && + std::is_nothrow_move_assignable::value) + { this->assign(std::move(rhs)); return *this; } @@ -1070,12 +1180,13 @@ struct expected_move_assign_base // expected_delete_ctor_base will conditionally delete copy and move // constructors depending on whether T is copy/move constructible -template ::value && - std::is_copy_constructible::value), - bool EnableMove = (is_move_constructible_or_void::value && - std::is_move_constructible::value)> -struct expected_delete_ctor_base { +template::value && + std::is_copy_constructible::value), + bool EnableMove = (is_move_constructible_or_void::value && + std::is_move_constructible::value)> +struct expected_delete_ctor_base +{ expected_delete_ctor_base() = default; expected_delete_ctor_base(const expected_delete_ctor_base &) = default; expected_delete_ctor_base(expected_delete_ctor_base &&) noexcept = default; @@ -1085,8 +1196,9 @@ struct expected_delete_ctor_base { operator=(expected_delete_ctor_base &&) noexcept = default; }; -template -struct expected_delete_ctor_base { +template +struct expected_delete_ctor_base +{ expected_delete_ctor_base() = default; expected_delete_ctor_base(const expected_delete_ctor_base &) = default; expected_delete_ctor_base(expected_delete_ctor_base &&) noexcept = delete; @@ -1096,8 +1208,9 @@ struct expected_delete_ctor_base { operator=(expected_delete_ctor_base &&) noexcept = default; }; -template -struct expected_delete_ctor_base { +template +struct expected_delete_ctor_base +{ expected_delete_ctor_base() = default; expected_delete_ctor_base(const expected_delete_ctor_base &) = delete; expected_delete_ctor_base(expected_delete_ctor_base &&) noexcept = default; @@ -1107,8 +1220,9 @@ struct expected_delete_ctor_base { operator=(expected_delete_ctor_base &&) noexcept = default; }; -template -struct expected_delete_ctor_base { +template +struct expected_delete_ctor_base +{ expected_delete_ctor_base() = default; expected_delete_ctor_base(const expected_delete_ctor_base &) = delete; expected_delete_ctor_base(expected_delete_ctor_base &&) noexcept = delete; @@ -1121,56 +1235,60 @@ struct expected_delete_ctor_base { // expected_delete_assign_base will conditionally delete copy and move // constructors depending on whether T and E are copy/move constructible + // assignable -template ::value && - std::is_copy_constructible::value && - is_copy_assignable_or_void::value && - std::is_copy_assignable::value), - bool EnableMove = (is_move_constructible_or_void::value && - std::is_move_constructible::value && - is_move_assignable_or_void::value && - std::is_move_assignable::value)> -struct expected_delete_assign_base { +template::value && + std::is_copy_constructible::value && + is_copy_assignable_or_void::value && + std::is_copy_assignable::value), + bool EnableMove = (is_move_constructible_or_void::value && + std::is_move_constructible::value && + is_move_assignable_or_void::value && + std::is_move_assignable::value)> +struct expected_delete_assign_base +{ expected_delete_assign_base() = default; expected_delete_assign_base(const expected_delete_assign_base &) = default; expected_delete_assign_base(expected_delete_assign_base &&) noexcept = - default; + default; expected_delete_assign_base & operator=(const expected_delete_assign_base &) = default; expected_delete_assign_base & operator=(expected_delete_assign_base &&) noexcept = default; }; -template -struct expected_delete_assign_base { +template +struct expected_delete_assign_base +{ expected_delete_assign_base() = default; expected_delete_assign_base(const expected_delete_assign_base &) = default; expected_delete_assign_base(expected_delete_assign_base &&) noexcept = - default; + default; expected_delete_assign_base & operator=(const expected_delete_assign_base &) = default; expected_delete_assign_base & operator=(expected_delete_assign_base &&) noexcept = delete; }; -template -struct expected_delete_assign_base { +template +struct expected_delete_assign_base +{ expected_delete_assign_base() = default; expected_delete_assign_base(const expected_delete_assign_base &) = default; expected_delete_assign_base(expected_delete_assign_base &&) noexcept = - default; + default; expected_delete_assign_base & operator=(const expected_delete_assign_base &) = delete; expected_delete_assign_base & operator=(expected_delete_assign_base &&) noexcept = default; }; -template -struct expected_delete_assign_base { +template +struct expected_delete_assign_base +{ expected_delete_assign_base() = default; expected_delete_assign_base(const expected_delete_assign_base &) = default; expected_delete_assign_base(expected_delete_assign_base &&) noexcept = - default; + default; expected_delete_assign_base & operator=(const expected_delete_assign_base &) = delete; expected_delete_assign_base & @@ -1179,22 +1297,24 @@ struct expected_delete_assign_base { // This is needed to be able to construct the expected_default_ctor_base which // follows, while still conditionally deleting the default constructor. -struct default_constructor_tag { +struct default_constructor_tag +{ explicit constexpr default_constructor_tag() = default; }; // expected_default_ctor_base will ensure that expected has a deleted default // consturctor if T is not default constructible. // This specialization is for when T is default constructible -template ::value || std::is_void::value> -struct expected_default_ctor_base { +template::value || std::is_void::value> +struct expected_default_ctor_base +{ constexpr expected_default_ctor_base() noexcept = default; constexpr expected_default_ctor_base( - expected_default_ctor_base const &) noexcept = default; + expected_default_ctor_base const &) noexcept = default; constexpr expected_default_ctor_base(expected_default_ctor_base &&) noexcept = - default; + default; expected_default_ctor_base & operator=(expected_default_ctor_base const &) noexcept = default; expected_default_ctor_base & @@ -1204,12 +1324,14 @@ struct expected_default_ctor_base { }; // This specialization is for when T is not default constructible -template struct expected_default_ctor_base { +template +struct expected_default_ctor_base +{ constexpr expected_default_ctor_base() noexcept = delete; constexpr expected_default_ctor_base( - expected_default_ctor_base const &) noexcept = default; + expected_default_ctor_base const &) noexcept = default; constexpr expected_default_ctor_base(expected_default_ctor_base &&) noexcept = - default; + default; expected_default_ctor_base & operator=(expected_default_ctor_base const &) noexcept = default; expected_default_ctor_base & @@ -1219,18 +1341,22 @@ template struct expected_default_ctor_base { }; } // namespace detail -template class bad_expected_access : public std::exception { +template +class bad_expected_access : public std::exception +{ public: - explicit bad_expected_access(E e) : m_val(std::move(e)) {} + explicit bad_expected_access(E e) + : m_val(std::move(e)) {} - virtual const char *what() const noexcept override { + virtual const char * what() const noexcept override + { return "Bad expected access"; } - const E &error() const & { return m_val; } - E &error() & { return m_val; } - const E &&error() const && { return std::move(m_val); } - E &&error() && { return std::move(m_val); } + const E & error() const & {return m_val;} + E & error() & {return m_val;} + const E && error() const && {return std::move(m_val);} + E && error() && {return std::move(m_val);} private: E m_val; @@ -1243,41 +1369,47 @@ template class bad_expected_access : public std::exception { /// has been initialized, and may not be destroyed before the expected object /// has been destroyed. The initialization state of the contained object is /// tracked by the expected object. -template +template class expected : private detail::expected_move_assign_base, - private detail::expected_delete_ctor_base, - private detail::expected_delete_assign_base, - private detail::expected_default_ctor_base { + private detail::expected_delete_ctor_base, + private detail::expected_delete_assign_base, + private detail::expected_default_ctor_base +{ static_assert(!std::is_reference::value, "T must not be a reference"); - static_assert(!std::is_same::type>::value, - "T must not be in_place_t"); - static_assert(!std::is_same::type>::value, - "T must not be unexpect_t"); static_assert( - !std::is_same>::type>::value, - "T must not be unexpected"); + !std::is_same::type>::value, + "T must not be in_place_t"); + static_assert( + !std::is_same::type>::value, + "T must not be unexpect_t"); + static_assert( + !std::is_same>::type>::value, + "T must not be unexpected"); static_assert(!std::is_reference::value, "E must not be a reference"); - T *valptr() { return std::addressof(this->m_val); } - const T *valptr() const { return std::addressof(this->m_val); } - unexpected *errptr() { return std::addressof(this->m_unexpect); } - const unexpected *errptr() const { + T * valptr() {return std::addressof(this->m_val);} + const T * valptr() const {return std::addressof(this->m_val);} + unexpected * errptr() {return std::addressof(this->m_unexpect);} + const unexpected * errptr() const + { return std::addressof(this->m_unexpect); } - template ::value> * = nullptr> - TL_EXPECTED_11_CONSTEXPR U &val() { + template::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR U & val() + { return this->m_val; } - TL_EXPECTED_11_CONSTEXPR unexpected &err() { return this->m_unexpect; } + TL_EXPECTED_11_CONSTEXPR unexpected & err() {return this->m_unexpect;} - template ::value> * = nullptr> - constexpr const U &val() const { + template::value> * = nullptr> + constexpr const U & val() const + { return this->m_val; } - constexpr const unexpected &err() const { return this->m_unexpect; } + constexpr const unexpected & err() const {return this->m_unexpect;} using impl_base = detail::expected_move_assign_base; using ctor_base = detail::expected_default_ctor_base; @@ -1287,322 +1419,368 @@ class expected : private detail::expected_move_assign_base, typedef E error_type; typedef unexpected unexpected_type; -#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ - !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) - template TL_EXPECTED_11_CONSTEXPR auto and_then(F &&f) & { +#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ + !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) + template TL_EXPECTED_11_CONSTEXPR auto and_then(F && f) & + { return and_then_impl(*this, std::forward(f)); } - template TL_EXPECTED_11_CONSTEXPR auto and_then(F &&f) && { + template TL_EXPECTED_11_CONSTEXPR auto and_then(F && f) && + { return and_then_impl(std::move(*this), std::forward(f)); } - template constexpr auto and_then(F &&f) const & { + template constexpr auto and_then(F && f) const & + { return and_then_impl(*this, std::forward(f)); } #ifndef TL_EXPECTED_NO_CONSTRR - template constexpr auto and_then(F &&f) const && { + template constexpr auto and_then(F && f) const && + { return and_then_impl(std::move(*this), std::forward(f)); } #endif #else - template + template TL_EXPECTED_11_CONSTEXPR auto - and_then(F &&f) & -> decltype(and_then_impl(std::declval(), - std::forward(f))) { + and_then(F && f) &->decltype(and_then_impl( + std::declval(), + std::forward(f))) + { return and_then_impl(*this, std::forward(f)); } - template + template TL_EXPECTED_11_CONSTEXPR auto - and_then(F &&f) && -> decltype(and_then_impl(std::declval(), - std::forward(f))) { + and_then(F && f) &&->decltype(and_then_impl( + std::declval(), + std::forward(f))) + { return and_then_impl(std::move(*this), std::forward(f)); } - template - constexpr auto and_then(F &&f) const & -> decltype(and_then_impl( - std::declval(), std::forward(f))) { + template + constexpr auto and_then(F && f) const & ->decltype(and_then_impl( + std::declval(), std::forward(f))) + { return and_then_impl(*this, std::forward(f)); } #ifndef TL_EXPECTED_NO_CONSTRR - template - constexpr auto and_then(F &&f) const && -> decltype(and_then_impl( - std::declval(), std::forward(f))) { + template + constexpr auto and_then(F && f) const && ->decltype(and_then_impl( + std::declval(), std::forward(f))) + { return and_then_impl(std::move(*this), std::forward(f)); } #endif #endif -#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ - !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) - template TL_EXPECTED_11_CONSTEXPR auto map(F &&f) & { +#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ + !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) + template TL_EXPECTED_11_CONSTEXPR auto map(F && f) & + { return expected_map_impl(*this, std::forward(f)); } - template TL_EXPECTED_11_CONSTEXPR auto map(F &&f) && { + template TL_EXPECTED_11_CONSTEXPR auto map(F && f) && + { return expected_map_impl(std::move(*this), std::forward(f)); } - template constexpr auto map(F &&f) const & { + template constexpr auto map(F && f) const & + { return expected_map_impl(*this, std::forward(f)); } - template constexpr auto map(F &&f) const && { + template constexpr auto map(F && f) const && + { return expected_map_impl(std::move(*this), std::forward(f)); } #else - template + template TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl( - std::declval(), std::declval())) - map(F &&f) & { + std::declval(), std::declval())) + map(F && f) & { return expected_map_impl(*this, std::forward(f)); } - template - TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl(std::declval(), - std::declval())) - map(F &&f) && { + template + TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl( + std::declval(), + std::declval())) + map(F && f) && { return expected_map_impl(std::move(*this), std::forward(f)); } - template - constexpr decltype(expected_map_impl(std::declval(), - std::declval())) - map(F &&f) const & { + template + constexpr decltype(expected_map_impl( + std::declval(), + std::declval())) + map(F && f) const & { return expected_map_impl(*this, std::forward(f)); } #ifndef TL_EXPECTED_NO_CONSTRR - template - constexpr decltype(expected_map_impl(std::declval(), - std::declval())) - map(F &&f) const && { + template + constexpr decltype(expected_map_impl( + std::declval(), + std::declval())) + map(F && f) const && { return expected_map_impl(std::move(*this), std::forward(f)); } #endif #endif -#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ - !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) - template TL_EXPECTED_11_CONSTEXPR auto transform(F &&f) & { +#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ + !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) + template TL_EXPECTED_11_CONSTEXPR auto transform(F && f) & + { return expected_map_impl(*this, std::forward(f)); } - template TL_EXPECTED_11_CONSTEXPR auto transform(F &&f) && { + template TL_EXPECTED_11_CONSTEXPR auto transform(F && f) && + { return expected_map_impl(std::move(*this), std::forward(f)); } - template constexpr auto transform(F &&f) const & { + template constexpr auto transform(F && f) const & + { return expected_map_impl(*this, std::forward(f)); } - template constexpr auto transform(F &&f) const && { + template constexpr auto transform(F && f) const && + { return expected_map_impl(std::move(*this), std::forward(f)); } #else - template + template TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl( - std::declval(), std::declval())) - transform(F &&f) & { + std::declval(), std::declval())) + transform(F && f) & { return expected_map_impl(*this, std::forward(f)); } - template - TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl(std::declval(), - std::declval())) - transform(F &&f) && { + template + TL_EXPECTED_11_CONSTEXPR decltype(expected_map_impl( + std::declval(), + std::declval())) + transform(F && f) && { return expected_map_impl(std::move(*this), std::forward(f)); } - template - constexpr decltype(expected_map_impl(std::declval(), - std::declval())) - transform(F &&f) const & { + template + constexpr decltype(expected_map_impl( + std::declval(), + std::declval())) + transform(F && f) const & { return expected_map_impl(*this, std::forward(f)); } #ifndef TL_EXPECTED_NO_CONSTRR - template - constexpr decltype(expected_map_impl(std::declval(), - std::declval())) - transform(F &&f) const && { + template + constexpr decltype(expected_map_impl( + std::declval(), + std::declval())) + transform(F && f) const && { return expected_map_impl(std::move(*this), std::forward(f)); } #endif #endif -#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ - !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) - template TL_EXPECTED_11_CONSTEXPR auto map_error(F &&f) & { +#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ + !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) + template TL_EXPECTED_11_CONSTEXPR auto map_error(F && f) & + { return map_error_impl(*this, std::forward(f)); } - template TL_EXPECTED_11_CONSTEXPR auto map_error(F &&f) && { + template TL_EXPECTED_11_CONSTEXPR auto map_error(F && f) && + { return map_error_impl(std::move(*this), std::forward(f)); } - template constexpr auto map_error(F &&f) const & { + template constexpr auto map_error(F && f) const & + { return map_error_impl(*this, std::forward(f)); } - template constexpr auto map_error(F &&f) const && { + template constexpr auto map_error(F && f) const && + { return map_error_impl(std::move(*this), std::forward(f)); } #else - template - TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl(std::declval(), - std::declval())) - map_error(F &&f) & { + template + TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl( + std::declval(), + std::declval())) + map_error(F && f) & { return map_error_impl(*this, std::forward(f)); } - template - TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl(std::declval(), - std::declval())) - map_error(F &&f) && { + template + TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl( + std::declval(), + std::declval())) + map_error(F && f) && { return map_error_impl(std::move(*this), std::forward(f)); } - template - constexpr decltype(map_error_impl(std::declval(), - std::declval())) - map_error(F &&f) const & { + template + constexpr decltype(map_error_impl( + std::declval(), + std::declval())) + map_error(F && f) const & { return map_error_impl(*this, std::forward(f)); } #ifndef TL_EXPECTED_NO_CONSTRR - template - constexpr decltype(map_error_impl(std::declval(), - std::declval())) - map_error(F &&f) const && { + template + constexpr decltype(map_error_impl( + std::declval(), + std::declval())) + map_error(F && f) const && { return map_error_impl(std::move(*this), std::forward(f)); } #endif #endif -#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ - !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) - template TL_EXPECTED_11_CONSTEXPR auto transform_error(F &&f) & { +#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ + !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) + template TL_EXPECTED_11_CONSTEXPR auto transform_error(F && f) & + { return map_error_impl(*this, std::forward(f)); } - template TL_EXPECTED_11_CONSTEXPR auto transform_error(F &&f) && { + template TL_EXPECTED_11_CONSTEXPR auto transform_error(F && f) && + { return map_error_impl(std::move(*this), std::forward(f)); } - template constexpr auto transform_error(F &&f) const & { + template constexpr auto transform_error(F && f) const & + { return map_error_impl(*this, std::forward(f)); } - template constexpr auto transform_error(F &&f) const && { + template constexpr auto transform_error(F && f) const && + { return map_error_impl(std::move(*this), std::forward(f)); } #else - template - TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl(std::declval(), - std::declval())) - transform_error(F &&f) & { + template + TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl( + std::declval(), + std::declval())) + transform_error(F && f) & { return map_error_impl(*this, std::forward(f)); } - template - TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl(std::declval(), - std::declval())) - transform_error(F &&f) && { + template + TL_EXPECTED_11_CONSTEXPR decltype(map_error_impl( + std::declval(), + std::declval())) + transform_error(F && f) && { return map_error_impl(std::move(*this), std::forward(f)); } - template - constexpr decltype(map_error_impl(std::declval(), - std::declval())) - transform_error(F &&f) const & { + template + constexpr decltype(map_error_impl( + std::declval(), + std::declval())) + transform_error(F && f) const & { return map_error_impl(*this, std::forward(f)); } #ifndef TL_EXPECTED_NO_CONSTRR - template - constexpr decltype(map_error_impl(std::declval(), - std::declval())) - transform_error(F &&f) const && { + template + constexpr decltype(map_error_impl( + std::declval(), + std::declval())) + transform_error(F && f) const && { return map_error_impl(std::move(*this), std::forward(f)); } #endif #endif - template expected TL_EXPECTED_11_CONSTEXPR or_else(F &&f) & { + template expected TL_EXPECTED_11_CONSTEXPR or_else(F && f) & + { return or_else_impl(*this, std::forward(f)); } - template expected TL_EXPECTED_11_CONSTEXPR or_else(F &&f) && { + template expected TL_EXPECTED_11_CONSTEXPR or_else(F && f) && + { return or_else_impl(std::move(*this), std::forward(f)); } - template expected constexpr or_else(F &&f) const & { + template expected constexpr or_else(F && f) const & + { return or_else_impl(*this, std::forward(f)); } #ifndef TL_EXPECTED_NO_CONSTRR - template expected constexpr or_else(F &&f) const && { + template expected constexpr or_else(F && f) const && + { return or_else_impl(std::move(*this), std::forward(f)); } #endif constexpr expected() = default; - constexpr expected(const expected &rhs) = default; - constexpr expected(expected &&rhs) = default; - expected &operator=(const expected &rhs) = default; - expected &operator=(expected &&rhs) = default; - - template ::value> * = - nullptr> - constexpr expected(in_place_t, Args &&...args) - : impl_base(in_place, std::forward(args)...), - ctor_base(detail::default_constructor_tag{}) {} - - template &, Args &&...>::value> * = nullptr> - constexpr expected(in_place_t, std::initializer_list il, Args &&...args) - : impl_base(in_place, il, std::forward(args)...), - ctor_base(detail::default_constructor_tag{}) {} - - template ::value> * = - nullptr, - detail::enable_if_t::value> * = - nullptr> - explicit constexpr expected(const unexpected &e) - : impl_base(unexpect, e.value()), - ctor_base(detail::default_constructor_tag{}) {} - - template < - class G = E, - detail::enable_if_t::value> * = - nullptr, - detail::enable_if_t::value> * = nullptr> - constexpr expected(unexpected const &e) - : impl_base(unexpect, e.value()), - ctor_base(detail::default_constructor_tag{}) {} - - template < - class G = E, - detail::enable_if_t::value> * = nullptr, - detail::enable_if_t::value> * = nullptr> - explicit constexpr expected(unexpected &&e) noexcept( - std::is_nothrow_constructible::value) - : impl_base(unexpect, std::move(e.value())), - ctor_base(detail::default_constructor_tag{}) {} - - template < - class G = E, - detail::enable_if_t::value> * = nullptr, - detail::enable_if_t::value> * = nullptr> - constexpr expected(unexpected &&e) noexcept( - std::is_nothrow_constructible::value) - : impl_base(unexpect, std::move(e.value())), - ctor_base(detail::default_constructor_tag{}) {} - - template ::value> * = - nullptr> - constexpr explicit expected(unexpect_t, Args &&...args) - : impl_base(unexpect, std::forward(args)...), - ctor_base(detail::default_constructor_tag{}) {} - - template &, Args &&...>::value> * = nullptr> - constexpr explicit expected(unexpect_t, std::initializer_list il, - Args &&...args) - : impl_base(unexpect, il, std::forward(args)...), - ctor_base(detail::default_constructor_tag{}) {} - - template ::value && - std::is_convertible::value)> * = - nullptr, - detail::expected_enable_from_other - * = nullptr> - explicit TL_EXPECTED_11_CONSTEXPR expected(const expected &rhs) - : ctor_base(detail::default_constructor_tag{}) { + constexpr expected(const expected & rhs) = default; + constexpr expected(expected && rhs) = default; + expected & operator=(const expected & rhs) = default; + expected & operator=(expected && rhs) = default; + + template::value> * = + nullptr> + constexpr expected(in_place_t, Args &&... args) + : impl_base(in_place, std::forward(args)...), + ctor_base(detail::default_constructor_tag{}) {} + + template &, Args &&...>::value> * = nullptr> + constexpr expected(in_place_t, std::initializer_list il, Args &&... args) + : impl_base(in_place, il, std::forward(args)...), + ctor_base(detail::default_constructor_tag{}) {} + + template::value> * = + nullptr, + detail::enable_if_t::value> * = + nullptr> + explicit constexpr expected(const unexpected & e) + : impl_base(unexpect, e.value()), + ctor_base(detail::default_constructor_tag{}) {} + + template< + class G = E, + detail::enable_if_t::value> * = + nullptr, + detail::enable_if_t::value> * = nullptr> + constexpr expected(unexpected const & e) + : impl_base(unexpect, e.value()), + ctor_base(detail::default_constructor_tag{}) {} + + template< + class G = E, + detail::enable_if_t::value> * = nullptr, + detail::enable_if_t::value> * = nullptr> + explicit constexpr expected(unexpected && e) noexcept( + std::is_nothrow_constructible::value) + : impl_base(unexpect, std::move(e.value())), + ctor_base(detail::default_constructor_tag{}) {} + + template< + class G = E, + detail::enable_if_t::value> * = nullptr, + detail::enable_if_t::value> * = nullptr> + constexpr expected(unexpected && e) noexcept( + std::is_nothrow_constructible::value) + : impl_base(unexpect, std::move(e.value())), + ctor_base(detail::default_constructor_tag{}) {} + + template::value> * = + nullptr> + constexpr explicit expected(unexpect_t, Args &&... args) + : impl_base(unexpect, std::forward(args)...), + ctor_base(detail::default_constructor_tag{}) {} + + template &, Args &&...>::value> * = nullptr> + constexpr explicit expected( + unexpect_t, std::initializer_list il, + Args &&... args) + : impl_base(unexpect, il, std::forward(args)...), + ctor_base(detail::default_constructor_tag{}) {} + + template::value && + std::is_convertible::value)> * = + nullptr, + detail::expected_enable_from_other + * = nullptr> + explicit TL_EXPECTED_11_CONSTEXPR expected(const expected & rhs) + : ctor_base(detail::default_constructor_tag{}) + { if (rhs.has_value()) { this->construct(*rhs); } else { @@ -1610,14 +1788,15 @@ class expected : private detail::expected_move_assign_base, } } - template ::value && - std::is_convertible::value)> * = - nullptr, - detail::expected_enable_from_other - * = nullptr> - TL_EXPECTED_11_CONSTEXPR expected(const expected &rhs) - : ctor_base(detail::default_constructor_tag{}) { + template::value && + std::is_convertible::value)> * = + nullptr, + detail::expected_enable_from_other + * = nullptr> + TL_EXPECTED_11_CONSTEXPR expected(const expected & rhs) + : ctor_base(detail::default_constructor_tag{}) + { if (rhs.has_value()) { this->construct(*rhs); } else { @@ -1625,13 +1804,14 @@ class expected : private detail::expected_move_assign_base, } } - template < - class U, class G, - detail::enable_if_t::value && - std::is_convertible::value)> * = nullptr, - detail::expected_enable_from_other * = nullptr> - explicit TL_EXPECTED_11_CONSTEXPR expected(expected &&rhs) - : ctor_base(detail::default_constructor_tag{}) { + template< + class U, class G, + detail::enable_if_t::value && + std::is_convertible::value)> * = nullptr, + detail::expected_enable_from_other * = nullptr> + explicit TL_EXPECTED_11_CONSTEXPR expected(expected && rhs) + : ctor_base(detail::default_constructor_tag{}) + { if (rhs.has_value()) { this->construct(std::move(*rhs)); } else { @@ -1639,13 +1819,14 @@ class expected : private detail::expected_move_assign_base, } } - template < - class U, class G, - detail::enable_if_t<(std::is_convertible::value && - std::is_convertible::value)> * = nullptr, - detail::expected_enable_from_other * = nullptr> - TL_EXPECTED_11_CONSTEXPR expected(expected &&rhs) - : ctor_base(detail::default_constructor_tag{}) { + template< + class U, class G, + detail::enable_if_t<(std::is_convertible::value && + std::is_convertible::value)> * = nullptr, + detail::expected_enable_from_other * = nullptr> + TL_EXPECTED_11_CONSTEXPR expected(expected && rhs) + : ctor_base(detail::default_constructor_tag{}) + { if (rhs.has_value()) { this->construct(std::move(*rhs)); } else { @@ -1653,33 +1834,34 @@ class expected : private detail::expected_move_assign_base, } } - template < - class U = T, - detail::enable_if_t::value> * = nullptr, - detail::expected_enable_forward_value * = nullptr> - explicit TL_EXPECTED_MSVC2015_CONSTEXPR expected(U &&v) - : expected(in_place, std::forward(v)) {} - - template < - class U = T, - detail::enable_if_t::value> * = nullptr, - detail::expected_enable_forward_value * = nullptr> - TL_EXPECTED_MSVC2015_CONSTEXPR expected(U &&v) - : expected(in_place, std::forward(v)) {} - - template < - class U = T, class G = T, - detail::enable_if_t::value> * = - nullptr, - detail::enable_if_t::value> * = nullptr, - detail::enable_if_t< - (!std::is_same, detail::decay_t>::value && - !detail::conjunction, - std::is_same>>::value && - std::is_constructible::value && - std::is_assignable::value && - std::is_nothrow_move_constructible::value)> * = nullptr> - expected &operator=(U &&v) { + template< + class U = T, + detail::enable_if_t::value> * = nullptr, + detail::expected_enable_forward_value * = nullptr> + explicit TL_EXPECTED_MSVC2015_CONSTEXPR expected(U && v) + : expected(in_place, std::forward(v)) {} + + template< + class U = T, + detail::enable_if_t::value> * = nullptr, + detail::expected_enable_forward_value * = nullptr> + TL_EXPECTED_MSVC2015_CONSTEXPR expected(U && v) + : expected(in_place, std::forward(v)) {} + + template< + class U = T, class G = T, + detail::enable_if_t::value> * = + nullptr, + detail::enable_if_t::value> * = nullptr, + detail::enable_if_t< + (!std::is_same, detail::decay_t>::value && + !detail::conjunction, + std::is_same>>::value && + std::is_constructible::value && + std::is_assignable::value && + std::is_nothrow_move_constructible::value)> * = nullptr> + expected & operator=(U && v) + { if (has_value()) { val() = std::forward(v); } else { @@ -1691,19 +1873,20 @@ class expected : private detail::expected_move_assign_base, return *this; } - template < - class U = T, class G = T, - detail::enable_if_t::value> * = - nullptr, - detail::enable_if_t::value> * = nullptr, - detail::enable_if_t< - (!std::is_same, detail::decay_t>::value && - !detail::conjunction, - std::is_same>>::value && - std::is_constructible::value && - std::is_assignable::value && - std::is_nothrow_move_constructible::value)> * = nullptr> - expected &operator=(U &&v) { + template< + class U = T, class G = T, + detail::enable_if_t::value> * = + nullptr, + detail::enable_if_t::value> * = nullptr, + detail::enable_if_t< + (!std::is_same, detail::decay_t>::value && + !detail::conjunction, + std::is_same>>::value && + std::is_constructible::value && + std::is_assignable::value && + std::is_nothrow_move_constructible::value)> * = nullptr> + expected & operator=(U && v) + { if (has_value()) { val() = std::forward(v); } else { @@ -1727,10 +1910,11 @@ class expected : private detail::expected_move_assign_base, return *this; } - template ::value && - std::is_assignable::value> * = nullptr> - expected &operator=(const unexpected &rhs) { + template::value && + std::is_assignable::value> * = nullptr> + expected & operator=(const unexpected & rhs) + { if (!has_value()) { err() = rhs; } else { @@ -1742,10 +1926,11 @@ class expected : private detail::expected_move_assign_base, return *this; } - template ::value && - std::is_move_assignable::value> * = nullptr> - expected &operator=(unexpected &&rhs) noexcept { + template::value && + std::is_move_assignable::value> * = nullptr> + expected & operator=(unexpected && rhs) noexcept + { if (!has_value()) { err() = std::move(rhs); } else { @@ -1757,9 +1942,10 @@ class expected : private detail::expected_move_assign_base, return *this; } - template ::value> * = nullptr> - void emplace(Args &&...args) { + template::value> * = nullptr> + void emplace(Args &&... args) + { if (has_value()) { val().~T(); } else { @@ -1769,9 +1955,10 @@ class expected : private detail::expected_move_assign_base, ::new (valptr()) T(std::forward(args)...); } - template ::value> * = nullptr> - void emplace(Args &&...args) { + template::value> * = nullptr> + void emplace(Args &&... args) + { if (has_value()) { val().~T(); ::new (valptr()) T(std::forward(args)...); @@ -1794,10 +1981,11 @@ class expected : private detail::expected_move_assign_base, } } - template &, Args &&...>::value> * = nullptr> - void emplace(std::initializer_list il, Args &&...args) { + template &, Args &&...>::value> * = nullptr> + void emplace(std::initializer_list il, Args &&... args) + { if (has_value()) { T t(il, std::forward(args)...); val() = std::move(t); @@ -1808,10 +1996,11 @@ class expected : private detail::expected_move_assign_base, } } - template &, Args &&...>::value> * = nullptr> - void emplace(std::initializer_list il, Args &&...args) { + template &, Args &&...>::value> * = nullptr> + void emplace(std::initializer_list il, Args &&... args) + { if (has_value()) { T t(il, std::forward(args)...); val() = std::move(t); @@ -1842,31 +2031,36 @@ class expected : private detail::expected_move_assign_base, using e_is_nothrow_move_constructible = std::true_type; using move_constructing_e_can_throw = std::false_type; - void swap_where_both_have_value(expected & /*rhs*/, t_is_void) noexcept { + void swap_where_both_have_value(expected & /*rhs*/, t_is_void) noexcept + { // swapping void is a no-op } - void swap_where_both_have_value(expected &rhs, t_is_not_void) { + void swap_where_both_have_value(expected & rhs, t_is_not_void) + { using std::swap; swap(val(), rhs.val()); } - void swap_where_only_one_has_value(expected &rhs, t_is_void) noexcept( - std::is_nothrow_move_constructible::value) { + void swap_where_only_one_has_value(expected & rhs, t_is_void) noexcept( + std::is_nothrow_move_constructible::value) + { ::new (errptr()) unexpected_type(std::move(rhs.err())); rhs.err().~unexpected_type(); std::swap(this->m_has_val, rhs.m_has_val); } - void swap_where_only_one_has_value(expected &rhs, t_is_not_void) { + void swap_where_only_one_has_value(expected & rhs, t_is_not_void) + { swap_where_only_one_has_value_and_t_is_not_void( - rhs, typename std::is_nothrow_move_constructible::type{}, - typename std::is_nothrow_move_constructible::type{}); + rhs, typename std::is_nothrow_move_constructible::type{}, + typename std::is_nothrow_move_constructible::type{}); } void swap_where_only_one_has_value_and_t_is_not_void( - expected &rhs, t_is_nothrow_move_constructible, - e_is_nothrow_move_constructible) noexcept { + expected & rhs, t_is_nothrow_move_constructible, + e_is_nothrow_move_constructible) noexcept + { auto temp = std::move(val()); val().~T(); ::new (errptr()) unexpected_type(std::move(rhs.err())); @@ -1876,8 +2070,9 @@ class expected : private detail::expected_move_assign_base, } void swap_where_only_one_has_value_and_t_is_not_void( - expected &rhs, t_is_nothrow_move_constructible, - move_constructing_e_can_throw) { + expected & rhs, t_is_nothrow_move_constructible, + move_constructing_e_can_throw) + { auto temp = std::move(val()); val().~T(); #ifdef TL_EXPECTED_EXCEPTIONS_ENABLED @@ -1899,8 +2094,9 @@ class expected : private detail::expected_move_assign_base, } void swap_where_only_one_has_value_and_t_is_not_void( - expected &rhs, move_constructing_t_can_throw, - e_is_nothrow_move_constructible) { + expected & rhs, move_constructing_t_can_throw, + e_is_nothrow_move_constructible) + { auto temp = std::move(rhs.err()); rhs.err().~unexpected_type(); #ifdef TL_EXPECTED_EXCEPTIONS_ENABLED @@ -1922,16 +2118,17 @@ class expected : private detail::expected_move_assign_base, } public: - template + template detail::enable_if_t::value && - detail::is_swappable::value && - (std::is_nothrow_move_constructible::value || - std::is_nothrow_move_constructible::value)> - swap(expected &rhs) noexcept( - std::is_nothrow_move_constructible::value - &&detail::is_nothrow_swappable::value - &&std::is_nothrow_move_constructible::value - &&detail::is_nothrow_swappable::value) { + detail::is_swappable::value && + (std::is_nothrow_move_constructible::value || + std::is_nothrow_move_constructible::value)> + swap(expected & rhs) noexcept( + std::is_nothrow_move_constructible::value && + detail::is_nothrow_swappable::value && + std::is_nothrow_move_constructible::value && + detail::is_nothrow_swappable::value) + { if (has_value() && rhs.has_value()) { swap_where_both_have_value(rhs, typename std::is_void::type{}); } else if (!has_value() && rhs.has_value()) { @@ -1944,174 +2141,206 @@ class expected : private detail::expected_move_assign_base, } } - constexpr const T *operator->() const { + constexpr const T * operator->() const + { TL_ASSERT(has_value()); return valptr(); } - TL_EXPECTED_11_CONSTEXPR T *operator->() { + TL_EXPECTED_11_CONSTEXPR T * operator->() + { TL_ASSERT(has_value()); return valptr(); } - template ::value> * = nullptr> - constexpr const U &operator*() const & { + template::value> * = nullptr> + constexpr const U & operator*() const & + { TL_ASSERT(has_value()); return val(); } - template ::value> * = nullptr> - TL_EXPECTED_11_CONSTEXPR U &operator*() & { + template::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR U & operator*() & + { TL_ASSERT(has_value()); return val(); } - template ::value> * = nullptr> - constexpr const U &&operator*() const && { + template::value> * = nullptr> + constexpr const U && operator*() const && { TL_ASSERT(has_value()); return std::move(val()); } - template ::value> * = nullptr> - TL_EXPECTED_11_CONSTEXPR U &&operator*() && { + template::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR U && operator*() && { TL_ASSERT(has_value()); return std::move(val()); } - constexpr bool has_value() const noexcept { return this->m_has_val; } - constexpr explicit operator bool() const noexcept { return this->m_has_val; } + constexpr bool has_value() const noexcept {return this->m_has_val;} + constexpr explicit operator bool() const noexcept {return this->m_has_val;} - template ::value> * = nullptr> - TL_EXPECTED_11_CONSTEXPR const U &value() const & { - if (!has_value()) + template::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR const U & value() const & + { + if (!has_value()) { detail::throw_exception(bad_expected_access(err().value())); + } return val(); } - template ::value> * = nullptr> - TL_EXPECTED_11_CONSTEXPR U &value() & { - if (!has_value()) + template::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR U & value() & + { + if (!has_value()) { detail::throw_exception(bad_expected_access(err().value())); + } return val(); } - template ::value> * = nullptr> - TL_EXPECTED_11_CONSTEXPR const U &&value() const && { - if (!has_value()) + template::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR const U && value() const && { + if (!has_value()) { detail::throw_exception(bad_expected_access(std::move(err()).value())); + } return std::move(val()); } - template ::value> * = nullptr> - TL_EXPECTED_11_CONSTEXPR U &&value() && { - if (!has_value()) + template::value> * = nullptr> + TL_EXPECTED_11_CONSTEXPR U && value() && { + if (!has_value()) { detail::throw_exception(bad_expected_access(std::move(err()).value())); + } return std::move(val()); } - constexpr const E &error() const & { + constexpr const E & error() const & + { TL_ASSERT(!has_value()); return err().value(); } - TL_EXPECTED_11_CONSTEXPR E &error() & { + TL_EXPECTED_11_CONSTEXPR E & error() & + { TL_ASSERT(!has_value()); return err().value(); } - constexpr const E &&error() const && { + constexpr const E && error() const && + { TL_ASSERT(!has_value()); return std::move(err().value()); } - TL_EXPECTED_11_CONSTEXPR E &&error() && { + TL_EXPECTED_11_CONSTEXPR E && error() && + { TL_ASSERT(!has_value()); return std::move(err().value()); } - template constexpr T value_or(U &&v) const & { - static_assert(std::is_copy_constructible::value && - std::is_convertible::value, - "T must be copy-constructible and convertible to from U&&"); + template constexpr T value_or(U && v) const & + { + static_assert( + std::is_copy_constructible::value && + std::is_convertible::value, + "T must be copy-constructible and convertible to from U&&"); return bool(*this) ? **this : static_cast(std::forward(v)); } - template TL_EXPECTED_11_CONSTEXPR T value_or(U &&v) && { - static_assert(std::is_move_constructible::value && - std::is_convertible::value, - "T must be move-constructible and convertible to from U&&"); + template TL_EXPECTED_11_CONSTEXPR T value_or(U && v) && + { + static_assert( + std::is_move_constructible::value && + std::is_convertible::value, + "T must be move-constructible and convertible to from U&&"); return bool(*this) ? std::move(**this) : static_cast(std::forward(v)); } }; -namespace detail { -template using exp_t = typename detail::decay_t::value_type; -template using err_t = typename detail::decay_t::error_type; -template using ret_t = expected>; +namespace detail +{ +template using exp_t = typename detail::decay_t::value_type; +template using err_t = typename detail::decay_t::error_type; +template using ret_t = expected>; #ifdef TL_EXPECTED_CXX14 -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval(), - *std::declval()))> -constexpr auto and_then_impl(Exp &&exp, F &&f) { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke( + std::declval(), + *std::declval()))> +constexpr auto and_then_impl(Exp && exp, F && f) +{ static_assert(detail::is_expected::value, "F must return an expected"); - return exp.has_value() - ? detail::invoke(std::forward(f), *std::forward(exp)) - : Ret(unexpect, std::forward(exp).error()); + return exp.has_value() ? + detail::invoke(std::forward(f), *std::forward(exp)) : + Ret(unexpect, std::forward(exp).error()); } -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval()))> -constexpr auto and_then_impl(Exp &&exp, F &&f) { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval()))> +constexpr auto and_then_impl(Exp && exp, F && f) +{ static_assert(detail::is_expected::value, "F must return an expected"); - return exp.has_value() ? detail::invoke(std::forward(f)) - : Ret(unexpect, std::forward(exp).error()); + return exp.has_value() ? detail::invoke(std::forward(f)) : + Ret(unexpect, std::forward(exp).error()); } #else -template struct TC; -template (), - *std::declval())), - detail::enable_if_t>::value> * = nullptr> -auto and_then_impl(Exp &&exp, F &&f) -> Ret { +template +struct TC; +template(), + *std::declval())), + detail::enable_if_t>::value> * = nullptr> +auto and_then_impl(Exp && exp, F && f) -> Ret +{ static_assert(detail::is_expected::value, "F must return an expected"); - return exp.has_value() - ? detail::invoke(std::forward(f), *std::forward(exp)) - : Ret(unexpect, std::forward(exp).error()); + return exp.has_value() ? + detail::invoke(std::forward(f), *std::forward(exp)) : + Ret(unexpect, std::forward(exp).error()); } -template ())), - detail::enable_if_t>::value> * = nullptr> -constexpr auto and_then_impl(Exp &&exp, F &&f) -> Ret { +template())), + detail::enable_if_t>::value> * = nullptr> +constexpr auto and_then_impl(Exp && exp, F && f) -> Ret +{ static_assert(detail::is_expected::value, "F must return an expected"); - return exp.has_value() ? detail::invoke(std::forward(f)) - : Ret(unexpect, std::forward(exp).error()); + return exp.has_value() ? detail::invoke(std::forward(f)) : + Ret(unexpect, std::forward(exp).error()); } #endif #ifdef TL_EXPECTED_CXX14 -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval(), - *std::declval())), - detail::enable_if_t::value> * = nullptr> -constexpr auto expected_map_impl(Exp &&exp, F &&f) { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke( + std::declval(), + *std::declval())), + detail::enable_if_t::value> * = nullptr> +constexpr auto expected_map_impl(Exp && exp, F && f) +{ using result = ret_t>; - return exp.has_value() ? result(detail::invoke(std::forward(f), - *std::forward(exp))) - : result(unexpect, std::forward(exp).error()); + return exp.has_value() ? result( + detail::invoke( + std::forward(f), + *std::forward(exp))) : + result(unexpect, std::forward(exp).error()); } -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval(), - *std::declval())), - detail::enable_if_t::value> * = nullptr> -auto expected_map_impl(Exp &&exp, F &&f) { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke( + std::declval(), + *std::declval())), + detail::enable_if_t::value> * = nullptr> +auto expected_map_impl(Exp && exp, F && f) +{ using result = expected>; if (exp.has_value()) { detail::invoke(std::forward(f), *std::forward(exp)); @@ -2121,21 +2350,23 @@ auto expected_map_impl(Exp &&exp, F &&f) { return result(unexpect, std::forward(exp).error()); } -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval())), - detail::enable_if_t::value> * = nullptr> -constexpr auto expected_map_impl(Exp &&exp, F &&f) { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval())), + detail::enable_if_t::value> * = nullptr> +constexpr auto expected_map_impl(Exp && exp, F && f) +{ using result = ret_t>; - return exp.has_value() ? result(detail::invoke(std::forward(f))) - : result(unexpect, std::forward(exp).error()); + return exp.has_value() ? result(detail::invoke(std::forward(f))) : + result(unexpect, std::forward(exp).error()); } -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval())), - detail::enable_if_t::value> * = nullptr> -auto expected_map_impl(Exp &&exp, F &&f) { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval())), + detail::enable_if_t::value> * = nullptr> +auto expected_map_impl(Exp && exp, F && f) +{ using result = expected>; if (exp.has_value()) { detail::invoke(std::forward(f)); @@ -2145,28 +2376,34 @@ auto expected_map_impl(Exp &&exp, F &&f) { return result(unexpect, std::forward(exp).error()); } #else -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval(), - *std::declval())), - detail::enable_if_t::value> * = nullptr> - -constexpr auto expected_map_impl(Exp &&exp, F &&f) - -> ret_t> { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke( + std::declval(), + *std::declval())), + detail::enable_if_t::value> * = nullptr> + +constexpr auto expected_map_impl(Exp && exp, F && f) +-> ret_t> +{ using result = ret_t>; - return exp.has_value() ? result(detail::invoke(std::forward(f), - *std::forward(exp))) - : result(unexpect, std::forward(exp).error()); + return exp.has_value() ? result( + detail::invoke( + std::forward(f), + *std::forward(exp))) : + result(unexpect, std::forward(exp).error()); } -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval(), - *std::declval())), - detail::enable_if_t::value> * = nullptr> +template>::value> * = nullptr, + class Ret = decltype(detail::invoke( + std::declval(), + *std::declval())), + detail::enable_if_t::value> * = nullptr> -auto expected_map_impl(Exp &&exp, F &&f) -> expected> { +auto expected_map_impl(Exp && exp, F && f) -> expected> +{ if (exp.has_value()) { detail::invoke(std::forward(f), *std::forward(exp)); return {}; @@ -2175,25 +2412,27 @@ auto expected_map_impl(Exp &&exp, F &&f) -> expected> { return unexpected>(std::forward(exp).error()); } -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval())), - detail::enable_if_t::value> * = nullptr> +template>::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval())), + detail::enable_if_t::value> * = nullptr> -constexpr auto expected_map_impl(Exp &&exp, F &&f) - -> ret_t> { +constexpr auto expected_map_impl(Exp && exp, F && f) +-> ret_t> +{ using result = ret_t>; - return exp.has_value() ? result(detail::invoke(std::forward(f))) - : result(unexpect, std::forward(exp).error()); + return exp.has_value() ? result(detail::invoke(std::forward(f))) : + result(unexpect, std::forward(exp).error()); } -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval())), - detail::enable_if_t::value> * = nullptr> +template>::value> * = nullptr, + class Ret = decltype(detail::invoke(std::declval())), + detail::enable_if_t::value> * = nullptr> -auto expected_map_impl(Exp &&exp, F &&f) -> expected> { +auto expected_map_impl(Exp && exp, F && f) -> expected> +{ if (exp.has_value()) { detail::invoke(std::forward(f)); return {}; @@ -2203,26 +2442,32 @@ auto expected_map_impl(Exp &&exp, F &&f) -> expected> { } #endif -#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ - !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -constexpr auto map_error_impl(Exp &&exp, F &&f) { +#if defined(TL_EXPECTED_CXX14) && !defined(TL_EXPECTED_GCC49) && \ + !defined(TL_EXPECTED_GCC54) && !defined(TL_EXPECTED_GCC55) +template>::value> * = nullptr, + class Ret = decltype(detail::invoke( + std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +constexpr auto map_error_impl(Exp && exp, F && f) +{ using result = expected, detail::decay_t>; - return exp.has_value() - ? result(*std::forward(exp)) - : result(unexpect, detail::invoke(std::forward(f), - std::forward(exp).error())); + return exp.has_value() ? + result(*std::forward(exp)) : + result( + unexpect, detail::invoke( + std::forward(f), + std::forward(exp).error())); } -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -auto map_error_impl(Exp &&exp, F &&f) { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke( + std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +auto map_error_impl(Exp && exp, F && f) +{ using result = expected, monostate>; if (exp.has_value()) { return result(*std::forward(exp)); @@ -2231,24 +2476,30 @@ auto map_error_impl(Exp &&exp, F &&f) { detail::invoke(std::forward(f), std::forward(exp).error()); return result(unexpect, monostate{}); } -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -constexpr auto map_error_impl(Exp &&exp, F &&f) { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke( + std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +constexpr auto map_error_impl(Exp && exp, F && f) +{ using result = expected, detail::decay_t>; - return exp.has_value() - ? result() - : result(unexpect, detail::invoke(std::forward(f), - std::forward(exp).error())); + return exp.has_value() ? + result() : + result( + unexpect, detail::invoke( + std::forward(f), + std::forward(exp).error())); } -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -auto map_error_impl(Exp &&exp, F &&f) { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke( + std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +auto map_error_impl(Exp && exp, F && f) +{ using result = expected, monostate>; if (exp.has_value()) { return result(); @@ -2258,27 +2509,33 @@ auto map_error_impl(Exp &&exp, F &&f) { return result(unexpect, monostate{}); } #else -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -constexpr auto map_error_impl(Exp &&exp, F &&f) - -> expected, detail::decay_t> { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke( + std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +constexpr auto map_error_impl(Exp && exp, F && f) +-> expected, detail::decay_t> +{ using result = expected, detail::decay_t>; - return exp.has_value() - ? result(*std::forward(exp)) - : result(unexpect, detail::invoke(std::forward(f), - std::forward(exp).error())); + return exp.has_value() ? + result(*std::forward(exp)) : + result( + unexpect, detail::invoke( + std::forward(f), + std::forward(exp).error())); } -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -auto map_error_impl(Exp &&exp, F &&f) -> expected, monostate> { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke( + std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +auto map_error_impl(Exp && exp, F && f) -> expected, monostate> +{ using result = expected, monostate>; if (exp.has_value()) { return result(*std::forward(exp)); @@ -2288,27 +2545,33 @@ auto map_error_impl(Exp &&exp, F &&f) -> expected, monostate> { return result(unexpect, monostate{}); } -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -constexpr auto map_error_impl(Exp &&exp, F &&f) - -> expected, detail::decay_t> { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke( + std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +constexpr auto map_error_impl(Exp && exp, F && f) +-> expected, detail::decay_t> +{ using result = expected, detail::decay_t>; - return exp.has_value() - ? result() - : result(unexpect, detail::invoke(std::forward(f), - std::forward(exp).error())); + return exp.has_value() ? + result() : + result( + unexpect, detail::invoke( + std::forward(f), + std::forward(exp).error())); } -template >::value> * = nullptr, - class Ret = decltype(detail::invoke(std::declval(), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -auto map_error_impl(Exp &&exp, F &&f) -> expected, monostate> { +template>::value> * = nullptr, + class Ret = decltype(detail::invoke( + std::declval(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +auto map_error_impl(Exp && exp, F && f) -> expected, monostate> +{ using result = expected, monostate>; if (exp.has_value()) { return result(); @@ -2320,123 +2583,153 @@ auto map_error_impl(Exp &&exp, F &&f) -> expected, monostate> { #endif #ifdef TL_EXPECTED_CXX14 -template (), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -constexpr auto or_else_impl(Exp &&exp, F &&f) { +template(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +constexpr auto or_else_impl(Exp && exp, F && f) +{ static_assert(detail::is_expected::value, "F must return an expected"); - return exp.has_value() ? std::forward(exp) - : detail::invoke(std::forward(f), - std::forward(exp).error()); + return exp.has_value() ? std::forward(exp) : + detail::invoke( + std::forward(f), + std::forward(exp).error()); } -template (), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -detail::decay_t or_else_impl(Exp &&exp, F &&f) { - return exp.has_value() ? std::forward(exp) - : (detail::invoke(std::forward(f), - std::forward(exp).error()), - std::forward(exp)); +template(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +detail::decay_t or_else_impl(Exp && exp, F && f) +{ + return exp.has_value() ? std::forward(exp) : + (detail::invoke( + std::forward(f), + std::forward(exp).error()), + std::forward(exp)); } #else -template (), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -auto or_else_impl(Exp &&exp, F &&f) -> Ret { +template(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +auto or_else_impl(Exp && exp, F && f) -> Ret +{ static_assert(detail::is_expected::value, "F must return an expected"); - return exp.has_value() ? std::forward(exp) - : detail::invoke(std::forward(f), - std::forward(exp).error()); + return exp.has_value() ? std::forward(exp) : + detail::invoke( + std::forward(f), + std::forward(exp).error()); } -template (), - std::declval().error())), - detail::enable_if_t::value> * = nullptr> -detail::decay_t or_else_impl(Exp &&exp, F &&f) { - return exp.has_value() ? std::forward(exp) - : (detail::invoke(std::forward(f), - std::forward(exp).error()), - std::forward(exp)); +template(), + std::declval().error())), + detail::enable_if_t::value> * = nullptr> +detail::decay_t or_else_impl(Exp && exp, F && f) +{ + return exp.has_value() ? std::forward(exp) : + (detail::invoke( + std::forward(f), + std::forward(exp).error()), + std::forward(exp)); } #endif } // namespace detail -template -constexpr bool operator==(const expected &lhs, - const expected &rhs) { - return (lhs.has_value() != rhs.has_value()) - ? false - : (!lhs.has_value() ? lhs.error() == rhs.error() : *lhs == *rhs); +template +constexpr bool operator==( + const expected & lhs, + const expected & rhs) +{ + return (lhs.has_value() != rhs.has_value()) ? + false : + (!lhs.has_value() ? lhs.error() == rhs.error() : *lhs == *rhs); } -template -constexpr bool operator!=(const expected &lhs, - const expected &rhs) { - return (lhs.has_value() != rhs.has_value()) - ? true - : (!lhs.has_value() ? lhs.error() != rhs.error() : *lhs != *rhs); +template +constexpr bool operator!=( + const expected & lhs, + const expected & rhs) +{ + return (lhs.has_value() != rhs.has_value()) ? + true : + (!lhs.has_value() ? lhs.error() != rhs.error() : *lhs != *rhs); } -template -constexpr bool operator==(const expected &lhs, - const expected &rhs) { - return (lhs.has_value() != rhs.has_value()) - ? false - : (!lhs.has_value() ? lhs.error() == rhs.error() : true); +template +constexpr bool operator==( + const expected & lhs, + const expected & rhs) +{ + return (lhs.has_value() != rhs.has_value()) ? + false : + (!lhs.has_value() ? lhs.error() == rhs.error() : true); } -template -constexpr bool operator!=(const expected &lhs, - const expected &rhs) { - return (lhs.has_value() != rhs.has_value()) - ? true - : (!lhs.has_value() ? lhs.error() == rhs.error() : false); +template +constexpr bool operator!=( + const expected & lhs, + const expected & rhs) +{ + return (lhs.has_value() != rhs.has_value()) ? + true : + (!lhs.has_value() ? lhs.error() == rhs.error() : false); } -template -constexpr bool operator==(const expected &x, const U &v) { +template +constexpr bool operator==(const expected & x, const U & v) +{ return x.has_value() ? *x == v : false; } -template -constexpr bool operator==(const U &v, const expected &x) { +template +constexpr bool operator==(const U & v, const expected & x) +{ return x.has_value() ? *x == v : false; } -template -constexpr bool operator!=(const expected &x, const U &v) { +template +constexpr bool operator!=(const expected & x, const U & v) +{ return x.has_value() ? *x != v : true; } -template -constexpr bool operator!=(const U &v, const expected &x) { +template +constexpr bool operator!=(const U & v, const expected & x) +{ return x.has_value() ? *x != v : true; } -template -constexpr bool operator==(const expected &x, const unexpected &e) { +template +constexpr bool operator==(const expected & x, const unexpected & e) +{ return x.has_value() ? false : x.error() == e.value(); } -template -constexpr bool operator==(const unexpected &e, const expected &x) { +template +constexpr bool operator==(const unexpected & e, const expected & x) +{ return x.has_value() ? false : x.error() == e.value(); } -template -constexpr bool operator!=(const expected &x, const unexpected &e) { +template +constexpr bool operator!=(const expected & x, const unexpected & e) +{ return x.has_value() ? true : x.error() != e.value(); } -template -constexpr bool operator!=(const unexpected &e, const expected &x) { +template +constexpr bool operator!=(const unexpected & e, const expected & x) +{ return x.has_value() ? true : x.error() != e.value(); } -template ::value || - std::is_move_constructible::value) && - detail::is_swappable::value && - std::is_move_constructible::value && - detail::is_swappable::value> * = nullptr> -void swap(expected &lhs, - expected &rhs) noexcept(noexcept(lhs.swap(rhs))) { +template::value || + std::is_move_constructible::value) && + detail::is_swappable::value && + std::is_move_constructible::value && + detail::is_swappable::value> * = nullptr> +void swap( + expected & lhs, + expected & rhs) noexcept(noexcept(lhs.swap(rhs))) +{ lhs.swap(rhs); } } // namespace tl diff --git a/doc/.gitignore b/doc/.gitignore deleted file mode 100644 index f80636a..0000000 --- a/doc/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -html -manifest.yaml \ No newline at end of file diff --git a/doc/conf.py b/doc/conf.py deleted file mode 100644 index ec66a0c..0000000 --- a/doc/conf.py +++ /dev/null @@ -1,5 +0,0 @@ -# Configuration file for the Sphinx documentation builder. -import os -os.environ['CRAS_DOCS_COMMON_SPHINX_PACKAGE_PATH'] = \ - os.path.dirname(os.path.dirname(os.path.abspath(__file__))) -from cras_docs_common.sphinx_docs_conf import * # noqa: E402, I201, F401, F403 diff --git a/doc/index.rst b/doc/index.rst deleted file mode 100644 index 00dcb9c..0000000 --- a/doc/index.rst +++ /dev/null @@ -1,36 +0,0 @@ -.. documentation master file - -|project| -=============================================================================== - -|description| - -Python API ----------- - -.. toctree:: - :maxdepth: 3 - - modules - -ROS API -------- - -See |ros2_documentation| for the ROS API description. - -C and C++ API -------------- - -The C and C++ API of this package is documented `here <../c++>`_. - -.. - .. mdinclude:: ../README.md - :start-line: 2 - :end-line: 6 - -Indices and tables -================== - -* :ref:`genindex` -* :ref:`modindex` -* :ref:`search` diff --git a/doc/modules.rst b/doc/modules.rst deleted file mode 100644 index 2a08efd..0000000 --- a/doc/modules.rst +++ /dev/null @@ -1,10 +0,0 @@ -point_cloud_transport package -============================= - -Generic API working with possibly compressed pointclouds --------------------------------------------------------- - -.. automodule:: point_cloud_transport - :members: - :undoc-members: - :show-inheritance: diff --git a/include/point_cloud_transport/expected.hpp b/include/point_cloud_transport/expected.hpp deleted file mode 100644 index 1dca329..0000000 --- a/include/point_cloud_transport/expected.hpp +++ /dev/null @@ -1,76 +0,0 @@ -// Copyright (c) 2023, Czech Technical University in Prague -// -// 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 HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. -// - -#ifndef POINT_CLOUD_TRANSPORT__EXPECTED_HPP_ -#define POINT_CLOUD_TRANSPORT__EXPECTED_HPP_ - - -// SPDX-License-Identifier: BSD-3-Clause -// SPDX-FileCopyrightText: Czech Technical University in Prague - -/** - * \file - * \brief An implementation of the `std::expected` proposal. - * - * `std::expected` should be used in functions that can either succeed and return a value, or fail and return an error. - * \author Martin Pecka - */ - -#include - -namespace cras -{ -using ::tl::bad_expected_access; -using ::tl::expected; -using ::tl::in_place; -using ::tl::make_unexpected; -using ::tl::unexpect; -using ::tl::unexpected; -} // namespace cras - -#include - -namespace cras -{ -/** - * \brief Type trait determining whether type T is cras::expected or not. - * \tparam T The type to test. - */ -template -struct is_cras_expected : public ::std::false_type {}; - -/** - * \brief Type trait determining whether type T is std::optional or not. - * \tparam T Type of the expected value. - * \tparam E Type of the expected error. - */ -template -struct is_cras_expected<::cras::expected>: public ::std::true_type {}; -} // namespace cras -#endif // POINT_CLOUD_TRANSPORT__EXPECTED_HPP_ diff --git a/include/point_cloud_transport/point_cloud_codec.hpp b/include/point_cloud_transport/point_cloud_codec.hpp index 7607eb1..ad2729c 100644 --- a/include/point_cloud_transport/point_cloud_codec.hpp +++ b/include/point_cloud_transport/point_cloud_codec.hpp @@ -48,9 +48,8 @@ namespace point_cloud_transport { -/** - * Class to expose all the functionality of pointcloud transport (encode/decode msgs) without needing to spin a node. - */ +//! Class to expose all the functionality of pointcloud transport (encode/decode msgs) +//! without needing to spin a node. class PointCloudCodec { @@ -61,45 +60,44 @@ class PointCloudCodec //! Destructor virtual ~PointCloudCodec(); - /** - * Get a shared pointer to an instance of a publisher plugin given its transport name (publishers encode messages). - * e.g. if you want the raw encoder, call getEncoderByName("raw"). - * - * \param name The name of the transport to load. - * \returns A shared pointer to the publisher plugin. - */ + /// \brief Get a shared pointer to an instance of a publisher plugin given its + /// transport name (publishers encode messages). + /// e.g. if you want the raw encoder, call getEncoderByName("raw"). + /// + /// \param name The name of the transport to load. + /// \returns A shared pointer to the publisher plugin. std::shared_ptr getEncoderByName( const std::string & name); - /** - * Get a shared pointer to an instance of a publisher plugin given its transport name (subscribers decode messages). - * e.g. if you want the raw decoder, call getDecoderByName("raw"). - * - * \param name The name of the transport to load. - * \returns A shared pointer to the subscriber plugin. - */ + /// \brief Get a shared pointer to an instance of a publisher plugin given its transport name + /// (subscribers decode messages). + /// e.g. if you want the raw decoder, call getDecoderByName("raw"). + /// + /// \param name The name of the transport to load. + /// \returns A shared pointer to the subscriber plugin. std::shared_ptr getDecoderByName( const std::string & name); - /** - * Get a list of all the transports that can be loaded. - * - * \param[out] transports Vector of the loadable transport plugins. - * \param[out] names Vector of string identifieries for the transport provided by each plugin - */ + /// + /// \brief Get a list of all the transports that can be loaded. + /// + /// \param[out] transports Vector of the loadable transport plugins. + /// \param[out] names Vector of string identifieries for the transport provided by each plugin + /// void getLoadableTransports( std::vector & transports, std::vector & names); - /** - * Get a list of all the transport plugins, topics, transport names, and their data types that can be loaded. - * - * \param[in] baseTopic The base topic to use for the transport. - * \param[out] transports Vector of the loadable transport plugins. - * \param[out] topics Vector of the topics that can be published. - * \param[out] names Vector of string identifieries for the transport provided by each plugin - * \param[out] dataTypes Vector of the data types the transports encode a PointCloud2 into - */ + /// + /// \brief Get a list of all the transport plugins, topics, transport names, and their data + /// types that can be loaded. + /// + /// \param[in] baseTopic The base topic to use for the transport. + /// \param[out] transports Vector of the loadable transport plugins. + /// \param[out] topics Vector of the topics that can be published. + /// \param[out] names Vector of string identifieries for the transport provided by each plugin + /// \param[out] dataTypes Vector of the data types the transports encode a PointCloud2 into + /// void getTopicsToPublish( const std::string & baseTopic, std::vector & transports, @@ -107,15 +105,16 @@ class PointCloudCodec std::vector & names, std::vector & dataTypes); - /** - * Get the topic, transport name, and data type that a given topic is published on for a particular transport plugin. - * - * \param[in] baseTopic The base topic to use for the transport. - * \param[in] transport The transport plugin to load. - * \param[out] topic The topic that should be subscribed to. - * \param[out] name String identifier for the transport provided by the plugin - * \param[out] dataType The data type the transport encodes a PointCloud2 into - */ + /// + /// \brief Get the topic, transport name, and data type that a given topic is published on for a + /// particular transport plugin. + /// + /// \param[in] baseTopic The base topic to use for the transport. + /// \param[in] transport The transport plugin to load. + /// \param[out] topic The topic that should be subscribed to. + /// \param[out] name String identifier for the transport provided by the plugin + /// \param[out] dataType The data type the transport encodes a PointCloud2 into + /// void getTopicToSubscribe( const std::string & baseTopic, const std::string & transport, @@ -123,64 +122,62 @@ class PointCloudCodec std::string & name, std::string & dataType); - /** - * Encode a PointCloud2 message into a serialized message - * using the specified transport plugin. The underlying type - * of the serialized message is determined by the transport plugin, - * but doesnt need to be known by this function. - * - * \param[in] transport_name The name of the transport plugin to use. - * \param[in] msg The message to encode. - * \param[out] serialized_msg The serialized message to store the encoded message in. - * \returns True if the message was successfully encoded, false otherwise. - */ + /// + /// \brief Encode a PointCloud2 message into a serialized message + /// using the specified transport plugin. The underlying type + /// of the serialized message is determined by the transport plugin, + /// but doesnt need to be known by this function. + /// + /// \param[in] transport_name The name of the transport plugin to use. + /// \param[in] msg The message to encode. + /// \param[out] serialized_msg The serialized message to store the encoded message in. + /// \returns True if the message was successfully encoded, false otherwise. + /// bool encode( const std::string & transport_name, const sensor_msgs::msg::PointCloud2 & msg, rclcpp::SerializedMessage & serialized_msg); - /** - * Encode a PointCloud2 message into some compressed message type - * using the specified transport plugin. The compressed message type - * is determined by the transport plugin. - * - * \param[in] transport_name The name of the transport plugin to use. - * \param[in] msg The message to encode. - * \param[out] compressed_msg The compressed message to store the encoded message in. - * \returns True if the message was successfully encoded, false otherwise. - */ + /// + /// \brief Encode a PointCloud2 message into some compressed message type + /// using the specified transport plugin. The compressed message type + /// is determined by the transport plugin. + /// + /// \param[in] transport_name The name of the transport plugin to use. + /// \param[in] msg The message to encode. + /// \param[out] compressed_msg The compressed message to store the encoded message in. + /// \returns True if the message was successfully encoded, false otherwise. + /// template bool encodeTyped( const std::string & transport_name, const sensor_msgs::msg::PointCloud2 & msg, M & compressed_msg); - /** - * Dencode a serialized message into a PointCloud2 - * using the specified transport plugin. The underlying type - * of the serialized message is determined by the transport plugin, - * but doesnt need to be known by this function. - * - * \param[in] transport_name The name of the transport plugin to use. - * \param[in] serialized_msg The serialized message to decode. - * \param[out] msg The message to store the decoded output in. - * \returns True if the message was successfully decoded, false otherwise. - */ + /// \brief Decode a serialized message into a PointCloud2 + /// using the specified transport plugin. The underlying type + /// of the serialized message is determined by the transport plugin, + /// but doesnt need to be known by this function. + /// + /// \param[in] transport_name The name of the transport plugin to use. + /// \param[in] serialized_msg The serialized message to decode. + /// \param[out] msg The message to store the decoded output in. + /// \returns True if the message was successfully decoded, false otherwise. + /// bool decode( const std::string & transport_name, const rclcpp::SerializedMessage & serialized_msg, sensor_msgs::msg::PointCloud2 & msg); - /** - * Decode some compressed message type - * into a PointCloud2 based on the specified transport plugin. The compressed message type - * is determined by the transport plugin. - * - * \param[in] transport_name The name of the transport plugin to use. - * \param[in] compressed_msg The compressed message to decode. - * \param[out] msg The message to store the decoded output in. - * \returns True if the message was successfully decoded, false otherwise. - */ + /// \brief Decode some compressed message type + /// into a PointCloud2 based on the specified transport plugin. The compressed message type + /// is determined by the transport plugin. + /// + /// \param[in] transport_name The name of the transport plugin to use. + /// \param[in] compressed_msg The compressed message to decode. + /// \param[out] msg The message to store the decoded output in. + /// \returns True if the message was successfully decoded, false otherwise. + /// template bool decodeTyped( const std::string & transport_name, diff --git a/include/point_cloud_transport/point_cloud_common.hpp b/include/point_cloud_transport/point_cloud_common.hpp index 24bd5ed..64d862d 100644 --- a/include/point_cloud_transport/point_cloud_common.hpp +++ b/include/point_cloud_transport/point_cloud_common.hpp @@ -39,27 +39,45 @@ namespace point_cloud_transport { -/** - * \brief Replacement for uses of boost::erase_last_copy - */ +/// \brief Remove the last copy of the given substring from the given string. +/// \param input original string +/// \param search substring to remove the last entry of +/// \return "input" with the last entry of "search" removed POINT_CLOUD_TRANSPORT_PUBLIC std::string erase_last_copy(const std::string & input, const std::string & search); +/// \brief Split a string into substrings using the given delimiter. +/// \param str string to split +/// \param delimiter delimiter to split on +/// \param maxSplits limit on the max number of splits to perform (-1 for unlimited splits) +/// \return vector of string tokens POINT_CLOUD_TRANSPORT_PUBLIC std::vector split( const std::string & str, const std::string & delimiter, int maxSplits = -1); -// from cras::string_utils +/// \brief Check if a string ends with a given suffix. (from cras::string_utils) +/// \param str string to check +/// \param suffix suffix to check for +/// \return true if "str" ends with "suffix", false otherwise POINT_CLOUD_TRANSPORT_PUBLIC bool endsWith(const std::string & str, const std::string & suffix); -// from cras::string_utils +/// \brief Remove a suffix from a string if it exists. +/// \param str string to remove suffix from +/// \param suffix suffix to remove +/// \param hadSuffix if non-null, set to true if "str" had "suffix" at the end +/// \return "str" with "suffix" removed POINT_CLOUD_TRANSPORT_PUBLIC std::string removeSuffix( const std::string & str, const std::string & suffix, bool * hadSuffix = nullptr); +/// \brief Check if the given transport matches the given name. +/// \param lookup_name name of the transport to check +/// \param name name to check against +/// \param suffix suffix to remove from name before checking for equality +/// \return true if "lookup_name" matches "name" with "suffix" removed, false otherwise POINT_CLOUD_TRANSPORT_PUBLIC bool transportNameMatches( const std::string & lookup_name, diff --git a/include/point_cloud_transport/point_cloud_transport.hpp b/include/point_cloud_transport/point_cloud_transport.hpp index 2abb706..26013cd 100644 --- a/include/point_cloud_transport/point_cloud_transport.hpp +++ b/include/point_cloud_transport/point_cloud_transport.hpp @@ -52,12 +52,9 @@ namespace point_cloud_transport { -/** -* Advertise and subscribe to PointCloud2 topics. -* -* PointCloudTransport is analogous to ros::NodeHandle in that it contains advertise() and -* subscribe() functions for creating advertisements and subscriptions of PointCloud2 topics. -*/ +//! Advertise and subscribe to PointCloud2 topics. +//! PointCloudTransport is analogous to rclcpp::Node in that it contains functions +//! to create publishers and subscriptions of PointCloud2 topics. class PointCloudTransportLoader { @@ -76,7 +73,7 @@ class PointCloudTransportLoader std::vector getDeclaredTransports() const; //! Returns the names of all transports that are loadable in the system - // (keys are lookup names, values are names). + //! (keys are lookup names, values are names). POINT_CLOUD_TRANSPORT_PUBLIC std::unordered_map getLoadableTransports() const; @@ -99,9 +96,12 @@ class PointCloudTransportLoader point_cloud_transport::SubLoaderPtr sub_loader_; }; -/*! - * \brief Advertise an image topic, free function version. - */ +/// \brief Advertise every available transport on pointcloud topics, free function version. +/// \param node The ROS node to use for any ROS operations +/// \param base_topic The base topic for the publisher +/// \param custom_qos The QoS profile to use for the underlying publisher(s) +/// \param options The publisher options to use for the underlying publisher(s) +/// \return The advertised publisher POINT_CLOUD_TRANSPORT_PUBLIC Publisher create_publisher( std::shared_ptr node, @@ -109,9 +109,14 @@ Publisher create_publisher( rmw_qos_profile_t custom_qos = rmw_qos_profile_default, const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions()); -/** - * \brief Subscribe to an image topic, free function version. - */ +/// \brief Subscribe to a pointcloud transport topic, free function version. +/// \param node The ROS node to use for any ROS operations +/// \param base_topic The base topic for the sbuscription +/// \param callback The callback to invoke on receipt of a message +/// \param transport The transport to use for the subscription +/// \param custom_qos The QoS profile to use for the underlying publisher +/// \param options The publisher options to use for the underlying publisher +/// \return The subscriber POINT_CLOUD_TRANSPORT_PUBLIC Subscriber create_subscription( std::shared_ptr node, @@ -121,13 +126,6 @@ Subscriber create_subscription( rmw_qos_profile_t custom_qos = rmw_qos_profile_default, rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()); -/** -* Advertise and subscribe to PointCloud2 topics. -* -* PointCloudTransport is analogous to rclcpp::Node in that it contains create_publisher() and -* create_subscription() functions for creating publishers and subscriptions of PointCloud2 topics. -*/ - class PointCloudTransport : public PointCloudTransportLoader { using VoidPtr = std::shared_ptr; @@ -184,21 +182,45 @@ class PointCloudTransport : public PointCloudTransportLoader //! Subscribe to a point cloud topic, version for arbitrary std::function object. POINT_CLOUD_TRANSPORT_PUBLIC point_cloud_transport::Subscriber subscribe( - const std::string & base_topic, uint32_t queue_size, + const std::string & base_topic, rmw_qos_profile_t custom_qos, const std::function & callback, const VoidPtr & tracked_object = {}, const point_cloud_transport::TransportHints * transport_hints = nullptr) { (void)tracked_object; - rmw_qos_profile_t custom_qos = rmw_qos_profile_sensor_data; - custom_qos.depth = queue_size; rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions(); return Subscriber( node_, base_topic, callback, sub_loader_, getTransportOrDefault(transport_hints), custom_qos, options); } + //! Subscribe to a point cloud topic, version for arbitrary std::function object. + POINT_CLOUD_TRANSPORT_PUBLIC + point_cloud_transport::Subscriber subscribe( + const std::string & base_topic, uint32_t queue_size, + const std::function & callback, + const VoidPtr & tracked_object = {}, + const point_cloud_transport::TransportHints * transport_hints = nullptr) + { + rmw_qos_profile_t custom_qos = rmw_qos_profile_sensor_data; + custom_qos.depth = queue_size; + return subscribe( + base_topic, custom_qos, callback, tracked_object, transport_hints); + } + //! Subscribe to a point cloud topic, version for bare function. + POINT_CLOUD_TRANSPORT_PUBLIC + point_cloud_transport::Subscriber subscribe( + const std::string & base_topic, rmw_qos_profile_t custom_qos, + void (* fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &), + const point_cloud_transport::TransportHints * transport_hints = nullptr) + { + return subscribe( + base_topic, custom_qos, + std::function(fp), + VoidPtr(), transport_hints); + } + POINT_CLOUD_TRANSPORT_PUBLIC point_cloud_transport::Subscriber subscribe( const std::string & base_topic, uint32_t queue_size, @@ -212,6 +234,19 @@ class PointCloudTransport : public PointCloudTransportLoader } //! Subscribe to a point cloud topic, version for class member function with bare pointer. + template + point_cloud_transport::Subscriber subscribe( + const std::string & base_topic, rmw_qos_profile_t custom_qos, + void (T::* fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &) const, T * obj, + const point_cloud_transport::TransportHints * transport_hints = nullptr, + bool allow_concurrent_callbacks = false) + { + return subscribe( + base_topic, custom_qos, std::bind( + fp, + obj.get(), std::placeholders::_1), VoidPtr(), transport_hints); + } + template point_cloud_transport::Subscriber subscribe( const std::string & base_topic, uint32_t queue_size, @@ -226,6 +261,19 @@ class PointCloudTransport : public PointCloudTransportLoader } //! Subscribe to a point cloud topic, version for class member function with shared_ptr. + template + point_cloud_transport::Subscriber subscribe( + const std::string & base_topic, rmw_qos_profile_t custom_qos, + void (T::* fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &) const, + const std::shared_ptr & obj, + const point_cloud_transport::TransportHints * transport_hints = nullptr) + { + return subscribe( + base_topic, custom_qos, std::bind( + fp, + obj.get(), std::placeholders::_1), obj, transport_hints); + } + template point_cloud_transport::Subscriber subscribe( const std::string & base_topic, uint32_t queue_size, diff --git a/include/point_cloud_transport/publisher_plugin.hpp b/include/point_cloud_transport/publisher_plugin.hpp index c98f5ea..ff4a25d 100644 --- a/include/point_cloud_transport/publisher_plugin.hpp +++ b/include/point_cloud_transport/publisher_plugin.hpp @@ -38,8 +38,8 @@ #include "rclcpp/node.hpp" #include +#include -#include #include #include "point_cloud_transport/visibility_control.hpp" @@ -50,9 +50,9 @@ namespace point_cloud_transport class POINT_CLOUD_TRANSPORT_PUBLIC PublisherPlugin { public: - //! \brief Result of cloud encoding. Either an holding the compressed cloud, - // empty value or error message. - typedef cras::expected>, + /// \brief Result of cloud encoding. Either an holding the compressed cloud, + /// empty value or error message. + typedef tl::expected>, std::string> EncodeResult; PublisherPlugin() = default; @@ -64,7 +64,7 @@ class POINT_CLOUD_TRANSPORT_PUBLIC PublisherPlugin //! \brief Advertise a topic, simple version. void advertise( - std::shared_ptr nh, + std::shared_ptr node, const std::string & base_topic, rmw_qos_profile_t custom_qos = rmw_qos_profile_default, const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions()); @@ -78,11 +78,11 @@ class POINT_CLOUD_TRANSPORT_PUBLIC PublisherPlugin //! Return the datatype of the transported messages (as text in the form `package/Message`). virtual std::string getDataType() const = 0; - /** - * \brief Encode the given raw pointcloud into EncodeResult - * \param[in] raw The input raw pointcloud. - * \return The output EncodeResult holding the compressed cloud message (if encoding succeeds), or an error message. - */ + /// \brief Encode the given raw pointcloud into EncodeResult + /// \param[in] raw The input raw pointcloud. + /// \return The output EncodeResult holding the compressed cloud message (if encoding succeeds), + /// or an error message. + /// virtual EncodeResult encode(const sensor_msgs::msg::PointCloud2 & raw) const; //! Publish a point cloud using the transport associated with this PublisherPlugin. @@ -105,7 +105,7 @@ class POINT_CLOUD_TRANSPORT_PUBLIC PublisherPlugin protected: //! Advertise a topic. Must be implemented by the subclass. virtual void advertiseImpl( - std::shared_ptr nh, const std::string & base_topic, + std::shared_ptr node, const std::string & base_topic, rmw_qos_profile_t custom_qos, const rclcpp::PublisherOptions & options = rclcpp::PublisherOptions()) = 0; }; diff --git a/include/point_cloud_transport/raw_subscriber.hpp b/include/point_cloud_transport/raw_subscriber.hpp index 5e53128..a7553fd 100644 --- a/include/point_cloud_transport/raw_subscriber.hpp +++ b/include/point_cloud_transport/raw_subscriber.hpp @@ -44,12 +44,12 @@ namespace point_cloud_transport { -/** - * The default SubscriberPlugin. - * - * RawSubscriber is a simple wrapper for ros::Subscriber which listens for PointCloud2 messages - * and passes them through to the callback. - */ +/// +/// \brief The default SubscriberPlugin. +/// +/// RawSubscriber is a simple wrapper for rclcpp::Subscription which listens for +/// PointCloud2 messages and passes them through to the callback. +/// class RawSubscriber : public point_cloud_transport::SimpleSubscriberPlugin { diff --git a/include/point_cloud_transport/simple_publisher_plugin.hpp b/include/point_cloud_transport/simple_publisher_plugin.hpp index da2daa1..ae9562d 100644 --- a/include/point_cloud_transport/simple_publisher_plugin.hpp +++ b/include/point_cloud_transport/simple_publisher_plugin.hpp @@ -42,8 +42,8 @@ #include #include "rclcpp/serialization.hpp" #include +#include -#include #include #include #include @@ -52,33 +52,30 @@ namespace point_cloud_transport { -/** - * \brief Base class to simplify implementing most plugins to Publisher. - * - * This base class vastly simplifies implementing a PublisherPlugin in the common - * case that all communication with the matching SubscriberPlugin happens over a - * single ROS topic using a transport-specific message type. SimplePublisherPlugin - * is templated on the transport-specific message type and publisher dynamic - * reconfigure type. - * - * A subclass need implement only two methods: - * - getTransportName() from PublisherPlugin - * - encodeTyped() - * - * For access to the parameter server and name remappings, use nh(). - * - * getTopicToAdvertise() controls the name of the internal communication topic. - * It defaults to \/\. - * - * \tparam M Type of the published messages. - */ +/// +/// \brief Base class to simplify implementing most plugins to Publisher. +/// +/// This base class vastly simplifies implementing a PublisherPlugin in the common +/// case that all communication with the matching SubscriberPlugin happens over a +/// single ROS topic using a transport-specific message type. SimplePublisherPlugin +/// is templated on the transport-specific message type and publisher dynamic +/// reconfigure type. +/// +/// A subclass needs to implement: +/// - getTransportName() from PublisherPlugin +/// - encodeTyped() +/// - getDataType() +/// - declareParameters() +/// +/// \tparam M Type of the published messages. +/// template class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin { public: - //! \brief Result of cloud encoding. Either the compressed cloud message, - // empty value, or error message. - typedef cras::expected, std::string> TypedEncodeResult; + /// \brief Result of cloud encoding. Either the compressed cloud message, + /// empty value, or error message. + typedef tl::expected, std::string> TypedEncodeResult; ~SimplePublisherPlugin() { @@ -92,7 +89,7 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin return rclcpp::get_logger("point_cloud_transport"); } - // template function for getting parameter of a given type + //! template function for getting parameter of a given type template bool getParam(const std::string & name, T & value) const { @@ -145,10 +142,8 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin void publish(const sensor_msgs::msg::PointCloud2 & message) const override { if (!simple_impl_ || !simple_impl_->pub_) { - auto logger = simple_impl_ ? simple_impl_->logger_ : rclcpp::get_logger( - "point_cloud_transport"); RCLCPP_ERROR( - logger, + this->getLogger(), "Call to publish() on an invalid point_cloud_transport::SimplePublisherPlugin"); return; } @@ -161,12 +156,12 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin simple_impl_.reset(); } - /** - * \brief Encode the given raw pointcloud into a compressed message. - * \param[in] raw The input raw pointcloud. - * \return The output rmw serialized msg holding the compressed cloud message - * (if encoding succeeds), or an error message. - */ + /// + /// \brief Encode the given raw pointcloud into a compressed message. + /// \param[in] raw The input raw pointcloud. + /// \return The output rmw serialized msg holding the compressed cloud message + /// (if encoding succeeds), or an error message. + /// POINT_CLOUD_TRANSPORT_PUBLIC virtual TypedEncodeResult encodeTyped( const sensor_msgs::msg::PointCloud2 & raw) const = 0; @@ -176,7 +171,7 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin // encode the message using the expected transport method auto res = this->encodeTyped(raw); if (!res) { - return cras::make_unexpected(res.error()); + return tl::make_unexpected(res.error()); } if (!res.value()) { return std::nullopt; @@ -200,7 +195,7 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin std::string transport_topic = getTopicToAdvertise(base_topic); simple_impl_ = std::make_unique(node); - RCLCPP_DEBUG(simple_impl_->logger_, "getTopicToAdvertise: %s", transport_topic.c_str()); + RCLCPP_DEBUG(node->get_logger(), "getTopicToAdvertise: %s", transport_topic.c_str()); auto qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(custom_qos), custom_qos); simple_impl_->pub_ = node->create_publisher(transport_topic, qos, options); @@ -212,14 +207,13 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin //! Generic function for publishing the internal message type. typedef std::function PublishFn; - /** - * Publish a point cloud using the specified publish function. Must be implemented by - * the subclass. - * - * The PublishFn publishes the transport-specific message type. This indirection allows - * SimplePublisherPlugin to use this function for both normal broadcast publishing and - * single subscriber publishing (in subscription callbacks). - */ + /// + /// \brief Publish a point cloud using the specified publish function. + /// + /// The PublishFn publishes the transport-specific message type. This indirection allows + /// SimplePublisherPlugin to use this function for both normal broadcast publishing and + /// single subscriber publishing (in subscription callbacks). + /// virtual void publish( const sensor_msgs::msg::PointCloud2 & message, const PublishFn & publish_fn) const @@ -227,19 +221,18 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin const auto res = this->encodeTyped(message); if (!res) { RCLCPP_ERROR( - rclcpp::get_logger( - "point_cloud_transport"), "Error encoding message by transport %s: %s.", + this->getLogger(), "Error encoding message by transport %s: %s.", this->getTransportName().c_str(), res.error().c_str()); } else if (res.value()) { publish_fn(res.value().value()); } } - /** - * \brief Return the communication topic name for a given base topic. - * - * Defaults to \/\. - */ + /// + /// \brief Return the communication topic name for a given base topic. + /// + /// Defaults to \/\. + /// std::string getTopicToAdvertise(const std::string & base_topic) const override { return base_topic + "/" + getTransportName(); @@ -264,12 +257,11 @@ class SimplePublisherPlugin : public point_cloud_transport::PublisherPlugin typedef std::function PointCloudPublishFn; - /** - * Returns a function object for publishing the transport-specific message type - * through some ROS publisher type. - * - * \param pub An object with method void publish(const M&) - */ + /// + /// \brief Returns a function object for publishing the transport-specific message type + /// through some ROS publisher type. + /// \param pub An object with method void publish(const M&) + /// template PublishFn bindInternalPublisher(PubT * pub) const { diff --git a/include/point_cloud_transport/simple_subscriber_plugin.hpp b/include/point_cloud_transport/simple_subscriber_plugin.hpp index 19f2a83..792565d 100644 --- a/include/point_cloud_transport/simple_subscriber_plugin.hpp +++ b/include/point_cloud_transport/simple_subscriber_plugin.hpp @@ -47,24 +47,22 @@ namespace point_cloud_transport { -/** - * Base class to simplify implementing most plugins to Subscriber. - * - * The base class simplifies implementing a SubscriberPlugin in the common case that - * all communication with the matching PublisherPlugin happens over a single ROS - * topic using a transport-specific message type. SimpleSubscriberPlugin is templated - * on the transport-specific message type. - * - * A subclass need implement only two methods: - * - getTransportName() from SubscriberPlugin - * - callback() - processes a message and invoked the user PointCloud2 callback if - * appropriate. - * - * For access to the parameter server and name remappings, use nh(). - * - * getTopicToSubscribe() controls the name of the internal communication topic. It - * defaults to \/\. - */ +/// +/// \brief Base class to simplify implementing most plugins to Subscriber. +/// +/// The base class simplifies implementing a SubscriberPlugin in the common case that +/// all communication with the matching PublisherPlugin happens over a single ROS +/// topic using a transport-specific message type. SimpleSubscriberPlugin is templated +/// on the transport-specific message type. +/// +/// A subclass needs to implement: +/// - getTransportName() from SubscriberPlugin +/// - decodeTyped() +/// - getDataType() +/// - declareParameters() +/// +/// \tparam M Type of the subscribed messages. +/// template class SimpleSubscriberPlugin : public SubscriberPlugin { @@ -122,11 +120,11 @@ class SimpleSubscriberPlugin : public SubscriberPlugin impl_.reset(); } - /** - * \brief Decode the given compressed pointcloud into a raw message. - * \param[in] compressed The input compressed pointcloud. - * \return The raw cloud message (if encoding succeeds), or an error message. - */ + /// + /// \brief Decode the given compressed pointcloud into a raw message. + /// \param[in] compressed The input compressed pointcloud. + /// \return The raw cloud message (if encoding succeeds), or an error message. + /// virtual DecodeResult decodeTyped(const M & compressed) const = 0; DecodeResult decode(const std::shared_ptr & compressed) const override @@ -136,7 +134,7 @@ class SimpleSubscriberPlugin : public SubscriberPlugin auto serializer = rclcpp::Serialization(); serializer.deserialize_message(compressed.get(), msg.get()); } catch (const std::exception & e) { - return cras::make_unexpected( + return tl::make_unexpected( "Error deserializing message for transport decoder: " + std::string( e.what()) + "."); } @@ -145,9 +143,9 @@ class SimpleSubscriberPlugin : public SubscriberPlugin } protected: - /** - * Process a message. Must be implemented by the subclass. - */ + /// + /// \brief Process a message. Must be implemented by the subclass. + /// virtual void callback(const typename std::shared_ptr & message, const Callback & user_cb) { DecodeResult res = this->decodeTyped(*message); @@ -161,11 +159,11 @@ class SimpleSubscriberPlugin : public SubscriberPlugin } } - /** - * \brief Return the communication topic name for a given base topic. - * - * Defaults to \/\. - */ + /// + /// \brief Return the communication topic name for a given base topic. + /// + /// Defaults to \/\. + /// std::string getTopicToSubscribe(const std::string & base_topic) const override { return base_topic + "/" + getTransportName(); diff --git a/include/point_cloud_transport/subscriber.hpp b/include/point_cloud_transport/subscriber.hpp index 0fe1fa8..91c88e7 100644 --- a/include/point_cloud_transport/subscriber.hpp +++ b/include/point_cloud_transport/subscriber.hpp @@ -48,21 +48,21 @@ namespace point_cloud_transport { -/** - * Manages a subscription callback on a specific topic that can be interpreted - * as a PointCloud2 topic. - * - * Subscriber is the client-side counterpart to Publisher. By loading the - * appropriate plugin, it can subscribe to a base point cloud topic using any available - * transport. The complexity of what transport is actually used is hidden from the user, - * who sees only a normal PointCloud2 callback. - * - * A Subscriber should always be created through a call to PointCloudTransport::subscribe(), - * or copied from one that was. - * Once all copies of a specific Subscriber go out of scope, the subscription callback - * associated with that handle will stop being called. Once all Subscriber for a given - * topic go out of scope the topic will be unsubscribed. - */ +/// +/// Manages a subscription callback on a specific topic that can be interpreted +/// as a PointCloud2 topic. +/// +/// Subscriber is the client-side counterpart to Publisher. By loading the +/// appropriate plugin, it can subscribe to a base point cloud topic using any available +/// transport. The complexity of what transport is actually used is hidden from the user, +/// who sees only a normal PointCloud2 callback. +/// +/// A Subscriber should always be created through a call to PointCloudTransport::subscribe(), +/// or copied from one that was. +/// Once all copies of a specific Subscriber go out of scope, the subscription callback +/// associated with that handle will stop being called. Once all Subscriber for a given +/// topic go out of scope the topic will be unsubscribed. +/// class Subscriber { public: @@ -81,30 +81,24 @@ class Subscriber rmw_qos_profile_t custom_qos = rmw_qos_profile_default, rclcpp::SubscriptionOptions options = rclcpp::SubscriptionOptions()); - /** - * Returns the base point cloud topic. - * - * The Subscriber may actually be subscribed to some transport-specific topic that - * differs from the base topic. - */ + /// + /// \brief Returns the base point cloud topic. + /// + /// The Subscriber may actually be subscribed to some transport-specific topic that + /// differs from the base topic. + /// POINT_CLOUD_TRANSPORT_PUBLIC std::string getTopic() const; - /** - * Returns the number of publishers this subscriber is connected to. - */ + //! Returns the number of publishers this subscriber is connected to. POINT_CLOUD_TRANSPORT_PUBLIC uint32_t getNumPublishers() const; - /** - * Returns the name of the transport being used. - */ + //! Returns the name of the transport being used. POINT_CLOUD_TRANSPORT_PUBLIC std::string getTransport() const; - /** - * Unsubscribe the callback associated with this Subscriber. - */ + //! Unsubscribe the callback associated with this Subscriber. POINT_CLOUD_TRANSPORT_PUBLIC void shutdown(); diff --git a/include/point_cloud_transport/subscriber_filter.hpp b/include/point_cloud_transport/subscriber_filter.hpp index 8b9f42a..0a34511 100644 --- a/include/point_cloud_transport/subscriber_filter.hpp +++ b/include/point_cloud_transport/subscriber_filter.hpp @@ -32,48 +32,43 @@ #ifndef POINT_CLOUD_TRANSPORT__SUBSCRIBER_FILTER_HPP_ #define POINT_CLOUD_TRANSPORT__SUBSCRIBER_FILTER_HPP_ -#include - #include #include +#include // NOLINT #include #include #include - #include "point_cloud_transport/visibility_control.hpp" namespace point_cloud_transport { -/** - * PointCloud2 subscription filter. - * - * This class wraps Subscriber as a "filter" compatible with the message_filters - * package. It acts as a highest-level filter, simply passing messages from a point cloud - * transport subscription through to the filters which have connected to it. - * - * When this object is destroyed it will unsubscribe from the ROS subscription. - * - * \section connections CONNECTIONS - * - * SubscriberFilter has no input connection. - * - * The output connection for the SubscriberFilter object is the same signature as for rclcpp - * subscription callbacks, ie. - */ +/// +/// PointCloud2 subscription filter. +/// +/// This class wraps Subscriber as a "filter" compatible with the message_filters +/// package. It acts as a highest-level filter, simply passing messages from a point cloud +/// transport subscription through to the filters which have connected to it. +/// +/// When this object is destroyed it will unsubscribe from the ROS subscription. +/// +/// SubscriberFilter has no input connection. +/// +/// The output connection for the SubscriberFilter object is the same signature as for rclcpp +/// subscription callbacks. +/// class SubscriberFilter : public message_filters::SimpleFilter { public: - /** - * Constructor - * - * \param node The rclcpp node to use to subscribe. - * \param base_topic The topic to subscribe to. - * \param queue_size The subscription queue size - * \param transport The transport hint to pass along - */ + /// + /// \brief Constructor + /// \param node The rclcpp node to use to subscribe. + /// \param base_topic The topic to subscribe to. + /// \param queue_size The subscription queue size + /// \param transport The transport hint to pass along + /// POINT_CLOUD_TRANSPORT_PUBLIC SubscriberFilter( std::shared_ptr node, const std::string & base_topic, @@ -82,9 +77,7 @@ class SubscriberFilter : public message_filters::SimpleFilter node, @@ -122,9 +113,7 @@ class SubscriberFilter : public message_filters::SimpleFilter +#include -#include #include #include "point_cloud_transport/visibility_control.hpp" @@ -49,15 +49,15 @@ namespace point_cloud_transport { -/** - * Base class for plugins to Subscriber. - */ +/// +/// Base class for plugins to Subscriber. +/// class POINT_CLOUD_TRANSPORT_PUBLIC SubscriberPlugin { public: - //! \brief Result of cloud decoding. Either a `sensor_msgs::msg::PointCloud2` - //! holding the raw message, empty value or error message. - typedef cras::expected, + /// \brief Result of cloud decoding. Either a `sensor_msgs::msg::PointCloud2` + /// holding the raw message, empty value or error message. + typedef tl::expected, std::string> DecodeResult; SubscriberPlugin() = default; @@ -68,23 +68,24 @@ class POINT_CLOUD_TRANSPORT_PUBLIC SubscriberPlugin typedef std::function Callback; - /** - * Get a string identifier for the transport provided by - * this plugin. - */ + /// + /// Get a string identifier for the transport provided by + /// this plugin. + /// virtual std::string getTransportName() const = 0; - /** - * \brief Decode the given compressed pointcloud into a raw cloud. - * \param[in] compressed The rclcpp::SerializedMessage of the compressed pointcloud to be decoded. - * \return The decoded raw pointcloud (if decoding succeeds), or an error message. - */ + /// + /// \brief Decode the given compressed pointcloud into a raw cloud. + /// \param[in] compressed The rclcpp::SerializedMessage of the compressed pointcloud + /// to be decoded. + /// \return The decoded raw pointcloud (if decoding succeeds), or an error message. + /// virtual DecodeResult decode(const std::shared_ptr & compressed) const = 0; - /** - * \brief Subscribe to an pointcloud topic, version for arbitrary std::function object. - */ + /// + /// \brief Subscribe to an pointcloud topic, version for arbitrary std::function object. + /// void subscribe( std::shared_ptr node, const std::string & base_topic, const Callback & callback, @@ -94,9 +95,9 @@ class POINT_CLOUD_TRANSPORT_PUBLIC SubscriberPlugin return subscribeImpl(node, base_topic, callback, custom_qos, options); } - /** - * \brief Subscribe to an pointcloud topic, version for bare function. - */ + /// + /// \brief Subscribe to an pointcloud topic, version for bare function. + /// void subscribe( std::shared_ptr node, const std::string & base_topic, void (* fp)(const sensor_msgs::msg::PointCloud2::ConstSharedPtr &), @@ -109,9 +110,9 @@ class POINT_CLOUD_TRANSPORT_PUBLIC SubscriberPlugin custom_qos, options); } - /** - * \brief Subscribe to an pointcloud topic, version for class member function with bare pointer. - */ + /// + /// \brief Subscribe to an pointcloud topic, version for class member function with bare pointer. + /// template void subscribe( std::shared_ptr node, const std::string & base_topic, @@ -124,49 +125,43 @@ class POINT_CLOUD_TRANSPORT_PUBLIC SubscriberPlugin std::bind(fp, obj, std::placeholders::_1), custom_qos, options); } - /** - * Get the transport-specific communication topic. - */ + //! Get the transport-specific communication topic. virtual std::string getTopic() const = 0; - /** - * Returns the number of publishers this subscriber is connected to. - */ + //! Returns the number of publishers this subscriber is connected to. virtual uint32_t getNumPublishers() const = 0; - /** - * Unsubscribe the callback associated with this SubscriberPlugin. - */ + //! Unsubscribe the callback associated with this SubscriberPlugin. virtual void shutdown() = 0; - /** - * Return the datatype of the transported messages (as text in the form `package/Message`). - */ + //! Return the datatype of the transported messages (as text in the form `package/Message`). virtual std::string getDataType() const = 0; - /** - * Declare parameter with this SubscriberPlugin. - */ + //! Declare parameter with this SubscriberPlugin. virtual void declareParameters() = 0; - /** - * Get the name of the topic that this SubscriberPlugin will subscribe to. - */ + /// + /// \brief Get the name of the topic that this SubscriberPlugin will subscribe to. + /// \param[in] base_topic The topic that the subscriber was constructed with. + /// \return The name of the topic that this SubscriberPlugin will subscribe to + /// (e.g. /). virtual std::string getTopicToSubscribe(const std::string & base_topic) const = 0; - /** - * Return the lookup name of the SubscriberPlugin associated with a specific - * transport identifier. - */ + /// + /// \brief Return the lookup name of the SubscriberPlugin associated with a specific + /// transport identifier. + /// \param transport_type The transport identifier. + /// \return The lookup name of the SubscriberPlugin associated with a specific + /// static std::string getLookupName(const std::string & transport_type) { return "point_cloud_transport/" + transport_type + "_sub"; } protected: - /** - * Subscribe to a point cloud transport topic. Must be implemented by the subclass. - */ + /// + /// Subscribe to a point cloud transport topic. Must be implemented by the subclass. + /// virtual void subscribeImpl( std::shared_ptr node, const std::string & base_topic, diff --git a/include/point_cloud_transport/transport_hints.hpp b/include/point_cloud_transport/transport_hints.hpp index be533f5..04c32f6 100644 --- a/include/point_cloud_transport/transport_hints.hpp +++ b/include/point_cloud_transport/transport_hints.hpp @@ -46,19 +46,17 @@ namespace point_cloud_transport class TransportHints { public: - /** - * Constructor. - * - * The default transport can be overridden by setting a certain parameter to the - * name of the desired transport. By default this parameter is named "point_cloud_transport" - * in the node's local namespace. For consistency across ROS applications, the - * name of this parameter should not be changed without good reason. - * - * \param node Node to use when looking up the transport parameter. - * \param default_transport Preferred transport to use - * \param parameter_name The name of the transport parameter - * - */ + /// + /// Constructor. + /// + /// The default transport can be overridden by setting a certain parameter to the + /// name of the desired transport. By default this parameter is named "point_cloud_transport" + /// in the node's local namespace. For consistency across ROS applications, the + /// name of this parameter should not be changed without good reason. + /// + /// \param node Node to use when looking up the transport parameter. + /// \param default_transport Preferred transport to use + /// \param parameter_name The name of the transport parameter POINT_CLOUD_TRANSPORT_PUBLIC TransportHints( const std::shared_ptr node, diff --git a/rosdoc.yaml b/rosdoc.yaml deleted file mode 100644 index fb83a32..0000000 --- a/rosdoc.yaml +++ /dev/null @@ -1,10 +0,0 @@ -- builder: doxygen - file_patterns: '*.h *.hpp *.hh *.dox' - exclude_patterns: '*/cmake-build-*/*' - output_dir: c++ - required: True -- builder: sphinx - name: Python API - output_dir: python - sphinx_root_dir: doc - required: True \ No newline at end of file diff --git a/setup.py b/setup.py deleted file mode 100644 index 5dd19f7..0000000 --- a/setup.py +++ /dev/null @@ -1,12 +0,0 @@ -# SPDX-License-Identifier: BSD-3-Clause -# SPDX-FileCopyrightText: Czech Technical University in Prague - -from catkin_pkg.python_setup import generate_distutils_setup -from setuptools import setup - -setup_args = generate_distutils_setup( - packages=['point_cloud_transport'], - package_dir={'': 'src'}, -) - -setup(**setup_args) diff --git a/src/publisher.cpp b/src/publisher.cpp index 2b3f554..c6ba16e 100644 --- a/src/publisher.cpp +++ b/src/publisher.cpp @@ -94,17 +94,6 @@ struct Publisher::Impl } } - void subscriberCB( - const point_cloud_transport::SingleSubscriberPublisher & plugin_pub, - const point_cloud_transport::SubscriberStatusCallback & user_cb) - { - point_cloud_transport::SingleSubscriberPublisher ssp( - plugin_pub.getSubscriberName(), getTopic(), - std::bind(&Publisher::Impl::getNumSubscribers, this), - plugin_pub.publish_fn_); - user_cb(ssp); - } - rclcpp::Logger logger_; std::string base_topic_; PubLoaderPtr loader_; @@ -238,30 +227,4 @@ Publisher::operator void *() const return (impl_ && impl_->isValid()) ? reinterpret_cast(1) : reinterpret_cast(0); } -// TODO(anyone): fix this -// void Publisher::weakSubscriberCb(const ImplWPtr& impl_wptr, -// const point_cloud_transport::SingleSubscriberPublisher& plugin_pub, -// const point_cloud_transport::SubscriberStatusCallback& user_cb) -// { -// if (ImplPtr impl = impl_wptr.lock()) -// { -// impl->subscriberCB(plugin_pub, user_cb); -// } -// } - -// SubscriberStatusCallback Publisher::rebindCB( -// const point_cloud_transport::SubscriberStatusCallback& user_cb) -// { -// // Note: the subscriber callback must be bound to the internal Impl object, not -// // 'this'. Due to copying behavior the Impl object may outlive the original Publisher -// // instance. But it should not outlive the last Publisher, so we use a weak_ptr. -// if (user_cb) -// { -// ImplWPtr impl_wptr(impl_); -// return std::bind(&Publisher::weakSubscriberCb, impl_wptr, _1, user_cb); -// } - -// return {}; -// } - } // namespace point_cloud_transport diff --git a/src/publisher_plugin.cpp b/src/publisher_plugin.cpp index adb580b..62d8514 100644 --- a/src/publisher_plugin.cpp +++ b/src/publisher_plugin.cpp @@ -45,12 +45,12 @@ const } void PublisherPlugin::advertise( - std::shared_ptr nh, + std::shared_ptr node, const std::string & base_topic, rmw_qos_profile_t custom_qos, const rclcpp::PublisherOptions & options) { - advertiseImpl(nh, base_topic, custom_qos, options); + advertiseImpl(node, base_topic, custom_qos, options); } void PublisherPlugin::publish(const sensor_msgs::msg::PointCloud2::ConstSharedPtr & message) const diff --git a/src/utilities/utilities.hpp b/src/utilities/utilities.hpp index b2a5318..fb818bc 100644 --- a/src/utilities/utilities.hpp +++ b/src/utilities/utilities.hpp @@ -20,16 +20,16 @@ namespace point_cloud_transport { -/// Get the option from a list of arguments -/// param[in] args List of arguments -/// param[in] option name to extract -/// return option value +/// \brief Get the option from a list of arguments +/// \param[in] args List of arguments +/// \param[in] option name to extract +/// \return option value std::string get_option(const std::vector & args, const std::string & option_name); -/// Is the option available in the list of arguments -/// param[in] args List of arguments -/// param[in] option name to extract -/// return true if the option exists or false otherwise +/// \brief Is the option available in the list of arguments +/// \param[in] args List of arguments +/// \param[in] option name to extract +/// \return true if the option exists or false otherwise bool has_option(const std::vector & args, const std::string & option_name); } // namespace point_cloud_transport #endif // UTILITIES__UTILITIES_HPP_ diff --git a/test/test_message_filter.cpp b/test/test_message_filter.cpp new file mode 100644 index 0000000..b169f41 --- /dev/null +++ b/test/test_message_filter.cpp @@ -0,0 +1,89 @@ +// Copyright (c) 2023 John D'Angelo +// +// 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 HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include +#include +#include + +#include +#include + +#include + +#include "point_cloud_transport/point_cloud_transport.hpp" +#include "point_cloud_transport/subscriber_filter.hpp" + +class TestSubscriber : public ::testing::Test +{ +protected: + void SetUp() + { + node_ = rclcpp::Node::make_shared("test_subscriber"); + } + + rclcpp::Node::SharedPtr node_; +}; + +void callback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & msg1, + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & msg2) +{ + (void) msg1; + (void) msg2; +} + +TEST_F(TestSubscriber, create_and_release_filter) +{ + typedef message_filters::sync_policies::ApproximateTime< + sensor_msgs::msg::PointCloud2, sensor_msgs::msg::PointCloud2> + ApproximateTimePolicy; + typedef std::shared_ptr> Sync; + + point_cloud_transport::SubscriberFilter pcl_sub1(node_, "pointcloud1", "raw"); + point_cloud_transport::SubscriberFilter pcl_sub2(node_, "pointcloud2", "raw"); + + auto sync = std::make_shared>( + ApproximateTimePolicy( + 10), pcl_sub1, pcl_sub2); + sync->registerCallback(std::bind(callback, std::placeholders::_1, std::placeholders::_2)); + + pcl_sub1.unsubscribe(); + pcl_sub2.unsubscribe(); + sync.reset(); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; +} diff --git a/test/test_message_passing.cpp b/test/test_message_passing.cpp index c2df7e0..6bc91b4 100644 --- a/test/test_message_passing.cpp +++ b/test/test_message_passing.cpp @@ -76,10 +76,10 @@ TEST_F(MessagePassingTesting, one_message_passing) rclcpp::executors::SingleThreadedExecutor executor; - auto pub = point_cloud_transport::create_publisher(node_, "camera/pointcloud"); + auto pub = point_cloud_transport::create_publisher(node_, "pointcloud"); auto sub = point_cloud_transport::create_subscription( - node_, "camera/pointcloud", pointcloudCallback, + node_, "pointcloud", pointcloudCallback, "raw"); test_rclcpp::wait_for_subscriber(node_, sub.getTopic()); diff --git a/test/test_publisher.cpp b/test/test_publisher.cpp index 0daff5e..edcb427 100644 --- a/test/test_publisher.cpp +++ b/test/test_publisher.cpp @@ -48,10 +48,10 @@ class TestPublisher : public ::testing::Test TEST_F(TestPublisher, publisher) { - auto pub = point_cloud_transport::create_publisher(node_, "camera/point_cloud"); - EXPECT_EQ(node_->get_node_graph_interface()->count_publishers("camera/point_cloud"), 1u); + auto pub = point_cloud_transport::create_publisher(node_, "point_cloud"); + EXPECT_EQ(node_->get_node_graph_interface()->count_publishers("point_cloud"), 1u); pub.shutdown(); - EXPECT_EQ(node_->get_node_graph_interface()->count_publishers("camera/point_cloud"), 0u); + EXPECT_EQ(node_->get_node_graph_interface()->count_publishers("point_cloud"), 0u); // coverage tests: invalid publisher should fail but not crash pub.publish(sensor_msgs::msg::PointCloud2()); pub.publish(sensor_msgs::msg::PointCloud2::ConstSharedPtr()); @@ -60,7 +60,7 @@ TEST_F(TestPublisher, publisher) TEST_F(TestPublisher, point_cloud_transport_publisher) { point_cloud_transport::PointCloudTransport it(node_); - auto pub = it.advertise("camera/point_cloud", rmw_qos_profile_sensor_data); + auto pub = it.advertise("point_cloud", rmw_qos_profile_sensor_data); } int main(int argc, char ** argv) diff --git a/test/test_qos_override.cpp b/test/test_qos_override.cpp index eebb653..ddb544e 100644 --- a/test/test_qos_override.cpp +++ b/test/test_qos_override.cpp @@ -45,14 +45,14 @@ class TestQosOverride : public ::testing::Test "test_qos_override_publisher", rclcpp::NodeOptions().parameter_overrides( { rclcpp::Parameter( - "qos_overrides./camera/pointcloud.publisher.reliability", "best_effort"), + "qos_overrides./pointcloud.publisher.reliability", "best_effort"), })); sub_node_ = rclcpp::Node::make_shared("test_subscriber"); qos_override_sub_node_ = rclcpp::Node::make_shared( "test_qos_override_subscriber", rclcpp::NodeOptions().parameter_overrides( { rclcpp::Parameter( - "qos_overrides./camera/pointcloud.subscription.reliability", "best_effort"), + "qos_overrides./pointcloud.subscription.reliability", "best_effort"), })); } @@ -64,16 +64,16 @@ class TestQosOverride : public ::testing::Test TEST_F(TestQosOverride, qos_override_publisher_without_options) { auto pub = point_cloud_transport::create_publisher( - pub_node_, "camera/pointcloud", + pub_node_, "pointcloud", rmw_qos_profile_default); - auto endpoint_info_vec = pub_node_->get_publishers_info_by_topic("camera/pointcloud"); + auto endpoint_info_vec = pub_node_->get_publishers_info_by_topic("pointcloud"); EXPECT_EQ(endpoint_info_vec[0].qos_profile().reliability(), rclcpp::ReliabilityPolicy::Reliable); pub.shutdown(); pub = point_cloud_transport::create_publisher( - qos_override_pub_node_, "camera/pointcloud", rmw_qos_profile_default); + qos_override_pub_node_, "pointcloud", rmw_qos_profile_default); - endpoint_info_vec = qos_override_pub_node_->get_publishers_info_by_topic("camera/pointcloud"); + endpoint_info_vec = qos_override_pub_node_->get_publishers_info_by_topic("pointcloud"); EXPECT_EQ( endpoint_info_vec[0].qos_profile().reliability(), rclcpp::ReliabilityPolicy::Reliable); @@ -91,15 +91,15 @@ TEST_F(TestQosOverride, qos_override_publisher_with_options) { }); auto pub = point_cloud_transport::create_publisher( - pub_node_, "camera/pointcloud", rmw_qos_profile_default, options); - auto endpoint_info_vec = pub_node_->get_publishers_info_by_topic("camera/pointcloud"); + pub_node_, "pointcloud", rmw_qos_profile_default, options); + auto endpoint_info_vec = pub_node_->get_publishers_info_by_topic("pointcloud"); EXPECT_EQ(endpoint_info_vec[0].qos_profile().reliability(), rclcpp::ReliabilityPolicy::Reliable); pub.shutdown(); pub = point_cloud_transport::create_publisher( - qos_override_pub_node_, "camera/pointcloud", rmw_qos_profile_default, options); + qos_override_pub_node_, "pointcloud", rmw_qos_profile_default, options); - endpoint_info_vec = qos_override_pub_node_->get_publishers_info_by_topic("camera/pointcloud"); + endpoint_info_vec = qos_override_pub_node_->get_publishers_info_by_topic("pointcloud"); EXPECT_EQ( endpoint_info_vec[0].qos_profile().reliability(), rclcpp::ReliabilityPolicy::BestEffort); @@ -111,15 +111,15 @@ TEST_F(TestQosOverride, qos_override_subscriber_without_options) { [](const auto & msg) {(void)msg;}; auto sub = point_cloud_transport::create_subscription( - sub_node_, "camera/pointcloud", fcn, "raw", rmw_qos_profile_default); - auto endpoint_info_vec = sub_node_->get_subscriptions_info_by_topic("camera/pointcloud"); + sub_node_, "pointcloud", fcn, "raw", rmw_qos_profile_default); + auto endpoint_info_vec = sub_node_->get_subscriptions_info_by_topic("pointcloud"); EXPECT_EQ(endpoint_info_vec[0].qos_profile().reliability(), rclcpp::ReliabilityPolicy::Reliable); sub.shutdown(); sub = point_cloud_transport::create_subscription( - qos_override_sub_node_, "camera/pointcloud", fcn, "raw", rmw_qos_profile_default); + qos_override_sub_node_, "pointcloud", fcn, "raw", rmw_qos_profile_default); - endpoint_info_vec = qos_override_sub_node_->get_subscriptions_info_by_topic("camera/pointcloud"); + endpoint_info_vec = qos_override_sub_node_->get_subscriptions_info_by_topic("pointcloud"); EXPECT_EQ( endpoint_info_vec[0].qos_profile().reliability(), rclcpp::ReliabilityPolicy::Reliable); @@ -139,15 +139,15 @@ TEST_F(TestQosOverride, qos_override_subscriber_with_options) { }); auto sub = point_cloud_transport::create_subscription( - sub_node_, "camera/pointcloud", fcn, "raw", rmw_qos_profile_default, options); - auto endpoint_info_vec = sub_node_->get_subscriptions_info_by_topic("camera/pointcloud"); + sub_node_, "pointcloud", fcn, "raw", rmw_qos_profile_default, options); + auto endpoint_info_vec = sub_node_->get_subscriptions_info_by_topic("pointcloud"); EXPECT_EQ(endpoint_info_vec[0].qos_profile().reliability(), rclcpp::ReliabilityPolicy::Reliable); sub.shutdown(); sub = point_cloud_transport::create_subscription( - qos_override_sub_node_, "camera/pointcloud", fcn, "raw", rmw_qos_profile_default, options); + qos_override_sub_node_, "pointcloud", fcn, "raw", rmw_qos_profile_default, options); - endpoint_info_vec = qos_override_sub_node_->get_subscriptions_info_by_topic("camera/pointcloud"); + endpoint_info_vec = qos_override_sub_node_->get_subscriptions_info_by_topic("pointcloud"); EXPECT_EQ( endpoint_info_vec[0].qos_profile().reliability(), rclcpp::ReliabilityPolicy::BestEffort); diff --git a/test/test_subscriber.cpp b/test/test_subscriber.cpp index 8494f1b..d1bcb1e 100644 --- a/test/test_subscriber.cpp +++ b/test/test_subscriber.cpp @@ -53,7 +53,7 @@ TEST_F(TestSubscriber, construction_and_destruction) (void)msg; }; - auto sub = point_cloud_transport::create_subscription(node_, "camera/pointcloud", fcn, "raw"); + auto sub = point_cloud_transport::create_subscription(node_, "pointcloud", fcn, "raw"); rclcpp::executors::SingleThreadedExecutor executor; executor.spin_node_some(node_); @@ -64,10 +64,10 @@ TEST_F(TestSubscriber, shutdown) std::function fcn = [](const auto & msg) {(void)msg;}; - auto sub = point_cloud_transport::create_subscription(node_, "camera/pointcloud", fcn, "raw"); - EXPECT_EQ(node_->get_node_graph_interface()->count_subscribers("camera/pointcloud"), 1u); + auto sub = point_cloud_transport::create_subscription(node_, "pointcloud", fcn, "raw"); + EXPECT_EQ(node_->get_node_graph_interface()->count_subscribers("pointcloud"), 1u); sub.shutdown(); - EXPECT_EQ(node_->get_node_graph_interface()->count_subscribers("camera/pointcloud"), 0u); + EXPECT_EQ(node_->get_node_graph_interface()->count_subscribers("pointcloud"), 0u); } int main(int argc, char ** argv)