From cb6b42e37ad50a65e5c08045534490def0e54249 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Sun, 11 Dec 2022 03:33:12 -0800 Subject: [PATCH] Port fuse_publishers and some upstream tests Signed-off-by: methylDragon --- fuse_constraints/CMakeLists.txt | 5 + fuse_constraints/suitesparse-extras.cmake | 1 + .../include/fuse_core/async_publisher.hpp | 17 +- fuse_core/include/fuse_core/publisher.hpp | 3 +- fuse_core/src/async_publisher.cpp | 56 +++-- .../test/launch_tests/test_parameters.cpp | 2 +- fuse_core/test/test_async_publisher.cpp | 13 +- .../fuse_models/odometry_2d_publisher.h | 2 +- fuse_publishers/CMakeLists.txt | 200 ++++++---------- fuse_publishers/COLCON_IGNORE | 0 .../fuse_publishers/path_2d_publisher.h | 31 ++- .../fuse_publishers/pose_2d_publisher.h | 52 ++++- .../fuse_publishers/serialized_publisher.h | 34 ++- .../stamped_variable_synchronizer.h | 2 +- fuse_publishers/package.xml | 45 ++-- fuse_publishers/src/path_2d_publisher.cpp | 65 ++++-- fuse_publishers/src/pose_2d_publisher.cpp | 166 +++++++++----- fuse_publishers/src/serialized_publisher.cpp | 61 +++-- fuse_publishers/test/CMakeLists.txt | 25 ++ fuse_publishers/test/path_2d_publisher.test | 4 - fuse_publishers/test/pose_2d_publisher.test | 4 - .../test/test_path_2d_publisher.cpp | 137 +++++++---- .../test/test_pose_2d_publisher.cpp | 217 ++++++++++-------- .../test_stamped_variable_synchronizer.cpp | 66 +++--- .../include/fuse_tutorials/beacon_publisher.h | 2 +- fuse_tutorials/src/beacon_publisher.cpp | 2 +- fuse_variables/package.xml | 1 - fuse_variables/test/CMakeLists.txt | 18 +- .../test/launch_tests/test_load_device_id.py | 48 ---- .../launch_tests/test_load_device_id.yaml | 35 --- .../test_load_device_id.cpp | 9 + 31 files changed, 747 insertions(+), 576 deletions(-) delete mode 100644 fuse_publishers/COLCON_IGNORE create mode 100644 fuse_publishers/test/CMakeLists.txt delete mode 100644 fuse_publishers/test/path_2d_publisher.test delete mode 100644 fuse_publishers/test/pose_2d_publisher.test delete mode 100755 fuse_variables/test/launch_tests/test_load_device_id.py delete mode 100644 fuse_variables/test/launch_tests/test_load_device_id.yaml rename fuse_variables/test/{launch_tests => }/test_load_device_id.cpp (87%) diff --git a/fuse_constraints/CMakeLists.txt b/fuse_constraints/CMakeLists.txt index e71cc8ec0..660a7ee2e 100644 --- a/fuse_constraints/CMakeLists.txt +++ b/fuse_constraints/CMakeLists.txt @@ -100,6 +100,11 @@ install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME} ) +install( + DIRECTORY cmake + DESTINATION share/${PROJECT_NAME} +) + pluginlib_export_plugin_description_file(fuse_core fuse_plugins.xml) ament_export_targets(${PROJECT_NAME}-export HAS_LIBRARY_TARGET) diff --git a/fuse_constraints/suitesparse-extras.cmake b/fuse_constraints/suitesparse-extras.cmake index 8a38ab7ee..a30d648d0 100644 --- a/fuse_constraints/suitesparse-extras.cmake +++ b/fuse_constraints/suitesparse-extras.cmake @@ -26,4 +26,5 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}) find_package(SUITESPARSE REQUIRED COMPONENTS CCOLAMD) diff --git a/fuse_core/include/fuse_core/async_publisher.hpp b/fuse_core/include/fuse_core/async_publisher.hpp index 9b4ab3cc5..a830303bb 100644 --- a/fuse_core/include/fuse_core/async_publisher.hpp +++ b/fuse_core/include/fuse_core/async_publisher.hpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -84,7 +85,13 @@ class AsyncPublisher : public Publisher * @param[in] name A unique name to give this plugin instance * @throws runtime_error if already initialized */ - void initialize(const std::string & name) override; + void initialize( + node_interfaces::NodeInterfaces< + node_interfaces::Base, + node_interfaces::Waitables + > interfaces, + const std::string & name, + size_t thread_count); /** * @brief Get the unique name of this publisher @@ -151,11 +158,13 @@ class AsyncPublisher : public Publisher std::shared_ptr callback_queue_; std::string name_; //!< The unique name for this publisher instance - rclcpp::Node::SharedPtr node_; //!< The node for this publisher rclcpp::CallbackGroup::SharedPtr cb_group_; //!< Internal re-entrant callback group + //! The node interfaces + node_interfaces::NodeInterfaces interfaces_; + //! A single/multi-threaded executor assigned to the local callback queue - rclcpp::executors::MultiThreadedExecutor::SharedPtr executor_; + rclcpp::Executor::SharedPtr executor_; size_t executor_thread_count_; std::thread spinner_; //!< Internal thread for spinning the executor std::atomic initialized_ = false; //!< True if instance has been fully initialized @@ -167,7 +176,7 @@ class AsyncPublisher : public Publisher * * @param[in] thread_count The number of threads used to service the local callback queue */ - explicit AsyncPublisher(size_t thread_count = 1); + AsyncPublisher(); /** * @brief Perform any required initialization for the publisher diff --git a/fuse_core/include/fuse_core/publisher.hpp b/fuse_core/include/fuse_core/publisher.hpp index 88271a2b4..c5cc40cc5 100644 --- a/fuse_core/include/fuse_core/publisher.hpp +++ b/fuse_core/include/fuse_core/publisher.hpp @@ -82,7 +82,8 @@ class Publisher * * @param[in] name A unique name to give this plugin instance */ - virtual void initialize(const std::string & name) = 0; + template + void initialize(NodeT interfaces, const std::string & name, size_t thread_count = 1); /** * @brief Get the unique name of this publisher diff --git a/fuse_core/src/async_publisher.cpp b/fuse_core/src/async_publisher.cpp index 61798cda3..bb3b8ceab 100644 --- a/fuse_core/src/async_publisher.cpp +++ b/fuse_core/src/async_publisher.cpp @@ -37,9 +37,8 @@ namespace fuse_core { -AsyncPublisher::AsyncPublisher(size_t thread_count) -: name_("uninitialized"), - executor_thread_count_(thread_count) +AsyncPublisher::AsyncPublisher() +: name_("uninitialized") { } @@ -48,43 +47,50 @@ AsyncPublisher::~AsyncPublisher() internal_stop(); } -void AsyncPublisher::initialize(const std::string & name) +void AsyncPublisher::initialize( + node_interfaces::NodeInterfaces< + node_interfaces::Base, + node_interfaces::Waitables + > interfaces, + const std::string & name, + size_t thread_count) { + executor_thread_count_ = thread_count; + interfaces_ = interfaces; + if (initialized_) { throw std::runtime_error("Calling initialize on an already initialized AsyncPublisher!"); } // Initialize internal state - name_ = name; - std::string node_namespace = ""; - - // TODO(CH3): Pass in the context or a node to get the context from - rclcpp::Context::SharedPtr ros_context = rclcpp::contexts::get_global_default_context(); - auto node_options = rclcpp::NodeOptions(); - node_options.context(ros_context); // set a context to generate the node in - - // TODO(CH3): Potentially pass in the optimizer node instead of spinning a new one - node_ = rclcpp::Node::make_shared(name_, node_namespace, node_options); + name_ = name; // NOTE(methylDragon): Used in derived classes + auto context = interfaces_.get_node_base_interface()->get_context(); auto executor_options = rclcpp::ExecutorOptions(); - executor_options.context = ros_context; - executor_ = rclcpp::executors::MultiThreadedExecutor::make_shared( - executor_options, - executor_thread_count_); + executor_options.context = context; + + if (executor_thread_count_ == 1) { + executor_ = rclcpp::executors::SingleThreadedExecutor::make_shared(executor_options); + } else { + executor_ = rclcpp::executors::MultiThreadedExecutor::make_shared( + executor_options, executor_thread_count_); + } - callback_queue_ = std::make_shared(ros_context); + callback_queue_ = + std::make_shared(context); // This callback group MUST be re-entrant in order to support parallelization - cb_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::Reentrant); - node_->get_node_waitables_interface()->add_waitable( - callback_queue_, cb_group_); + cb_group_ = interfaces_.get_node_base_interface()->create_callback_group( + rclcpp::CallbackGroupType::Reentrant, false); + interfaces_.get_node_waitables_interface()->add_waitable(callback_queue_, cb_group_); // Call the derived onInit() function to perform implementation-specific initialization onInit(); // Make sure the executor will service the given node - // TODO(sloretz) add just the callback group here when using Optimizer's Node - executor_->add_node(node_); + // We can add this without any guards because the callback group was set to not get automatically + // added to executors + executor_->add_callback_group(cb_group_, interfaces_.get_node_base_interface()); // Start the executor spinner_ = std::thread( @@ -127,7 +133,7 @@ void AsyncPublisher::stop() { // Prefer to call onStop in executor's thread so downstream users don't have // to worry about threads in ROS callbacks when there's only 1 thread. - if (node_->get_node_base_interface()->get_context()->is_valid()) { + if (interfaces_.get_node_base_interface()->get_context()->is_valid()) { auto callback = std::make_shared>( std::bind(&AsyncPublisher::onStop, this) ); diff --git a/fuse_core/test/launch_tests/test_parameters.cpp b/fuse_core/test/launch_tests/test_parameters.cpp index b2738f046..bdc6133a5 100644 --- a/fuse_core/test/launch_tests/test_parameters.cpp +++ b/fuse_core/test/launch_tests/test_parameters.cpp @@ -114,9 +114,9 @@ TEST_F(TestParameters, GetCovarianceDiagonalParam) fuse_core::Matrix3d default_covariance = fuse_core::Matrix3d::Identity(); default_covariance *= default_variance; - // Load covariance matrix diagonal from the parameter server: auto node = rclcpp::Node::make_shared("test_parameters_node"); + // Load covariance matrix diagonal from the parameter server: // A covariance diagonal with the expected size and valid should be the same as the expected one: { const std::string parameter_name{"covariance_diagonal"}; diff --git a/fuse_core/test/test_async_publisher.cpp b/fuse_core/test/test_async_publisher.cpp index 2e22437d5..7ad0fb085 100644 --- a/fuse_core/test/test_async_publisher.cpp +++ b/fuse_core/test/test_async_publisher.cpp @@ -45,7 +45,7 @@ class MyPublisher : public fuse_core::AsyncPublisher { public: MyPublisher() - : fuse_core::AsyncPublisher(0), + : fuse_core::AsyncPublisher(), callback_processed(false), initialized(false) { @@ -87,24 +87,27 @@ class TestAsyncPublisher : public ::testing::Test TEST_F(TestAsyncPublisher, OnInit) { for (int i = 0; i < 250; i++) { + auto node = rclcpp::Node::make_shared("test_async_pub_node"); MyPublisher publisher; - publisher.initialize("my_publisher_" + std::to_string(i)); + publisher.initialize(node, "my_publisher_" + std::to_string(i), 0); EXPECT_TRUE(publisher.initialized); } } TEST_F(TestAsyncPublisher, DoubleInit) { + auto node = rclcpp::Node::make_shared("test_async_pub_node"); MyPublisher publisher; - publisher.initialize("my_publisher"); + publisher.initialize(node, "my_publisher", 0); EXPECT_TRUE(publisher.initialized); - EXPECT_THROW(publisher.initialize("test"), std::runtime_error); + EXPECT_THROW(publisher.initialize(node, "test", 0), std::runtime_error); } TEST_F(TestAsyncPublisher, notifyCallback) { + auto node = rclcpp::Node::make_shared("test_async_pub_node"); MyPublisher publisher; - publisher.initialize("my_publisher"); + publisher.initialize(node, "my_publisher", 0); // Execute the notify() method in this thread. This should push a call to // MyPublisher::notifyCallback() into MyPublisher's callback queue, which will get executed by diff --git a/fuse_models/include/fuse_models/odometry_2d_publisher.h b/fuse_models/include/fuse_models/odometry_2d_publisher.h index 13e418372..32bd6d0b5 100644 --- a/fuse_models/include/fuse_models/odometry_2d_publisher.h +++ b/fuse_models/include/fuse_models/odometry_2d_publisher.h @@ -93,7 +93,7 @@ namespace fuse_models * - tf, tf_static (tf2_msgs::TFMessage) Subscribes to tf data to obtain the requisite odom->base_link transform, * but only if the world_frame_id is set to the value of the map_frame_id. */ -class Odometry2DPublisher : public fuse_core::AsyncPublisher +class Odometry2DPublisher : public fuse_core::AsyncPublisher // TODO(methylDragon): Refactor this in the same way it was done in fuse_publishers { public: FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(Odometry2DPublisher) diff --git a/fuse_publishers/CMakeLists.txt b/fuse_publishers/CMakeLists.txt index 68885d310..c8473b506 100644 --- a/fuse_publishers/CMakeLists.txt +++ b/fuse_publishers/CMakeLists.txt @@ -1,35 +1,31 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.16) project(fuse_publishers) -set(build_depends - fuse_core - fuse_variables - geometry_msgs - nav_msgs - pluginlib - roscpp - tf2 - tf2_geometry_msgs - tf2_ros -) +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) + set(CXX_STANDARD_REQUIRED YES) +endif() -find_package(catkin REQUIRED COMPONENTS - ${build_depends} -) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -catkin_package( - INCLUDE_DIRS - include - LIBRARIES - ${PROJECT_NAME} - CATKIN_DEPENDS - ${build_depends} -) +find_package(ament_cmake_ros REQUIRED) +find_package(fuse_core REQUIRED) +find_package(fuse_msgs REQUIRED) +find_package(fuse_variables REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_ros REQUIRED) ########### ## Build ## ########### -add_compile_options(-Wall -Werror) # fuse_publishers library add_library(${PROJECT_NAME} @@ -37,128 +33,62 @@ add_library(${PROJECT_NAME} src/pose_2d_publisher.cpp src/serialized_publisher.cpp ) -add_dependencies(${PROJECT_NAME} - ${catkin_EXPORTED_TARGETS} -) -target_include_directories(${PROJECT_NAME} - PUBLIC - include - ${catkin_INCLUDE_DIRS} -) -target_link_libraries(${PROJECT_NAME} - ${catkin_LIBRARIES} +target_include_directories(${PROJECT_NAME} PUBLIC + "$" + "$" ) -set_target_properties(${PROJECT_NAME} - PROPERTIES - CXX_STANDARD 14 - CXX_STANDARD_REQUIRED YES +target_link_libraries(${PROJECT_NAME} PUBLIC + fuse_core::fuse_core + ${fuse_msgs_TARGETS} + fuse_variables::fuse_variables + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + pluginlib::pluginlib + rclcpp::rclcpp + tf2::tf2 + ${tf2_geometry_msgs_TARGETS} + tf2_ros::tf2_ros ) ############# -## Install ## +## Testing ## ############# -install( - TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -install(FILES fuse_plugins.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + add_subdirectory(test) +endif() ############# -## Testing ## +## Install ## ############# -if(CATKIN_ENABLE_TESTING) - set(test_depends - fuse_constraints - fuse_graphs - ) - - find_package(catkin REQUIRED COMPONENTS - ${build_depends} - ${test_depends} - ) - find_package(roslint REQUIRED) - find_package(rostest REQUIRED) +install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}-export + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) - # Lint tests - set(ROSLINT_CPP_OPTS "--filter=-build/c++11,-runtime/references") - roslint_cpp() - roslint_add_test() +install(DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) - # Path2DPublisher Tests - add_rostest_gtest(test_path_2d_publisher - test/path_2d_publisher.test - test/test_path_2d_publisher.cpp - ) - add_dependencies(test_path_2d_publisher - ${catkin_EXPORTED_TARGETS} - ) - target_include_directories(test_path_2d_publisher - PRIVATE - include - ${catkin_INCLUDE_DIRS} - ) - target_link_libraries(test_path_2d_publisher - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ) - set_target_properties(test_path_2d_publisher - PROPERTIES - CXX_STANDARD 14 - CXX_STANDARD_REQUIRED YES - ) +pluginlib_export_plugin_description_file(fuse_core fuse_plugins.xml) - # Pose2DPublisher Tests - add_rostest_gtest(test_pose_2d_publisher - test/pose_2d_publisher.test - test/test_pose_2d_publisher.cpp - ) - add_dependencies(test_pose_2d_publisher - ${catkin_EXPORTED_TARGETS} - ) - target_include_directories(test_pose_2d_publisher - PRIVATE - include - ${catkin_INCLUDE_DIRS} - ) - target_link_libraries(test_pose_2d_publisher - ${PROJECT_NAME} - ${catkin_LIBRARIES} - ) - set_target_properties(test_pose_2d_publisher - PROPERTIES - CXX_STANDARD 14 - CXX_STANDARD_REQUIRED YES - ) +ament_export_targets(${PROJECT_NAME}-export HAS_LIBRARY_TARGET) +ament_export_dependencies( + ament_cmake_ros + fuse_core + fuse_msgs + fuse_variables + geometry_msgs + nav_msgs + pluginlib + rclcpp + tf2 + tf2_geometry_msgs + tf2_ros +) - # StampedVariableSynchronizer Tests - catkin_add_gtest(test_stamped_variable_synchronizer - test/test_stamped_variable_synchronizer.cpp - ) - add_dependencies(test_stamped_variable_synchronizer - ${catkin_EXPORTED_TARGETS} - ) - target_include_directories(test_stamped_variable_synchronizer - PRIVATE - include - ${catkin_INCLUDE_DIRS} - ) - target_link_libraries(test_stamped_variable_synchronizer - ${catkin_LIBRARIES} - ) - set_target_properties(test_stamped_variable_synchronizer - PROPERTIES - CXX_STANDARD 14 - CXX_STANDARD_REQUIRED YES - ) -endif() +ament_package() diff --git a/fuse_publishers/COLCON_IGNORE b/fuse_publishers/COLCON_IGNORE deleted file mode 100644 index e69de29bb..000000000 diff --git a/fuse_publishers/include/fuse_publishers/path_2d_publisher.h b/fuse_publishers/include/fuse_publishers/path_2d_publisher.h index 11cd9f192..813fea629 100644 --- a/fuse_publishers/include/fuse_publishers/path_2d_publisher.h +++ b/fuse_publishers/include/fuse_publishers/path_2d_publisher.h @@ -38,7 +38,12 @@ #include #include #include -#include +#include +#include + +#include +#include +#include #include @@ -69,6 +74,19 @@ class Path2DPublisher : public fuse_core::AsyncPublisher */ virtual ~Path2DPublisher() = default; + /** + * @brief Shadowing extension to the AsyncPublisher::initialize call + */ + void initialize( + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Parameters, + fuse_core::node_interfaces::Topics, + fuse_core::node_interfaces::Waitables + > interfaces, + const std::string & name, + size_t thread_count); + /** * @brief Perform any required post-construction initialization, such as advertising publishers or reading from the * parameter server. @@ -86,10 +104,17 @@ class Path2DPublisher : public fuse_core::AsyncPublisher fuse_core::Graph::ConstSharedPtr graph) override; protected: + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Parameters, + fuse_core::node_interfaces::Topics, + fuse_core::node_interfaces::Waitables + > interfaces_; //!< Shadows AsyncPublisher interfaces_ + fuse_core::UUID device_id_; //!< The UUID of the device to be published std::string frame_id_; //!< The name of the frame for this path - ros::Publisher path_publisher_; //!< The publisher that sends the entire robot trajectory as a path - ros::Publisher pose_array_publisher_; //!< The publisher that sends the entire robot trajectory as a pose array + rclcpp::Publisher::SharedPtr path_publisher_; //!< The publisher that sends the entire robot trajectory as a path + rclcpp::Publisher::SharedPtr pose_array_publisher_; //!< The publisher that sends the entire robot trajectory as a pose array }; } // namespace fuse_publishers diff --git a/fuse_publishers/include/fuse_publishers/pose_2d_publisher.h b/fuse_publishers/include/fuse_publishers/pose_2d_publisher.h index ce5f9bf1f..c53742de2 100644 --- a/fuse_publishers/include/fuse_publishers/pose_2d_publisher.h +++ b/fuse_publishers/include/fuse_publishers/pose_2d_publisher.h @@ -44,10 +44,22 @@ #include #include -#include +#include +#include +#include + +#include +#include +#include + + +#include +#include #include #include + + #include #include @@ -109,6 +121,23 @@ class Pose2DPublisher : public fuse_core::AsyncPublisher */ virtual ~Pose2DPublisher() = default; + /** + * @brief Shadowing extension to the AsyncPublisher::initialize call + */ + void initialize( + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Clock, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters, + fuse_core::node_interfaces::Services, + fuse_core::node_interfaces::Timers, + fuse_core::node_interfaces::Topics, + fuse_core::node_interfaces::Waitables + > interfaces, + const std::string & name, + size_t thread_count); + /** * @brief Perform any required post-construction initialization, such as advertising publishers or reading from the * parameter server. @@ -149,21 +178,34 @@ class Pose2DPublisher : public fuse_core::AsyncPublisher void tfPublishTimerCallback(); protected: + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Clock, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters, + fuse_core::node_interfaces::Services, + fuse_core::node_interfaces::Timers, + fuse_core::node_interfaces::Topics, + fuse_core::node_interfaces::Waitables + > interfaces_; //!< Shadows AsyncPublisher interfaces_ + using Synchronizer = StampedVariableSynchronizer; - std::string base_frame_; //!< The name of the robot's base_link frame fuse_core::UUID device_id_; //!< The UUID of the device to be published + rclcpp::Clock::SharedPtr clock_; //!< The publisher's clock, for timestamping and logging + std::shared_ptr logger_; //!< The publisher's logger, shared_ptr for deferred init + std::string map_frame_; //!< The name of the robot's map frame std::string odom_frame_; //!< The name of the odom frame for this pose (or empty if the odom is not used) - ros::Publisher pose_publisher_; //!< Publish the pose as a geometry_msgs::msg::PoseStamped - ros::Publisher pose_with_covariance_publisher_; //!< Publish the pose as a geometry_msgs::msg::PoseWithCovarianceStamped + rclcpp::Publisher::SharedPtr pose_publisher_; //!< Publish the pose as a geometry_msgs::PoseStamped + rclcpp::Publisher::SharedPtr pose_with_covariance_publisher_; //!< Publish the pose as a geometry_msgs::PoseWithCovarianceStamped bool publish_to_tf_; //!< Flag indicating the pose should be sent to the tf system as well as the pose topics Synchronizer::UniquePtr synchronizer_; //!< Object that tracks the latest common timestamp of multiple variables std::unique_ptr tf_buffer_; //!< TF2 object that supports querying transforms by time and frame id std::unique_ptr tf_listener_; //!< TF2 object that subscribes to the tf topics and //!< inserts the received transforms into the tf buffer - tf2_ros::TransformBroadcaster tf_publisher_; //!< Publish the map->odom or map->base transform to the tf system + std::shared_ptr tf_publisher_ = nullptr; //!< Publish the map->odom or map->base transform to the tf system rclcpp::TimerBase::SharedPtr tf_publish_timer_; //!< Timer that publishes tf messages to ensure the tf transform doesn't get stale rclcpp::Duration tf_timeout_; //!< The max time to wait for a tf transform to become available geometry_msgs::msg::TransformStamped tf_transform_; //!< The transform to be published to tf diff --git a/fuse_publishers/include/fuse_publishers/serialized_publisher.h b/fuse_publishers/include/fuse_publishers/serialized_publisher.h index 7eabc5f3c..d8956b117 100644 --- a/fuse_publishers/include/fuse_publishers/serialized_publisher.h +++ b/fuse_publishers/include/fuse_publishers/serialized_publisher.h @@ -39,7 +39,11 @@ #include #include #include -#include +#include + +#include +#include + #include @@ -65,6 +69,21 @@ class SerializedPublisher : public fuse_core::AsyncPublisher */ virtual ~SerializedPublisher() = default; + /** + * @brief Shadowing extension to the AsyncPublisher::initialize call + */ + void initialize( + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Clock, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters, + fuse_core::node_interfaces::Topics, + fuse_core::node_interfaces::Waitables + > interfaces, + const std::string & name, + size_t thread_count); + /** * @brief Perform any required post-construction initialization, such as advertising publishers or reading from the * parameter server. @@ -82,6 +101,15 @@ class SerializedPublisher : public fuse_core::AsyncPublisher fuse_core::Graph::ConstSharedPtr graph) override; protected: + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Clock, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters, + fuse_core::node_interfaces::Topics, + fuse_core::node_interfaces::Waitables + > interfaces_; //!< Shadows AsyncPublisher interfaces_ + /** * @brief Publish the serialized graph * @@ -91,8 +119,8 @@ class SerializedPublisher : public fuse_core::AsyncPublisher void graphPublisherCallback(fuse_core::Graph::ConstSharedPtr graph, const rclcpp::Time& stamp) const; std::string frame_id_; //!< The name of the frame for the serialized graph and transaction messages published - ros::Publisher graph_publisher_; - ros::Publisher transaction_publisher_; + rclcpp::Publisher::SharedPtr graph_publisher_; + rclcpp::Publisher::SharedPtr transaction_publisher_; using GraphPublisherCallback = std::function; using GraphPublisherThrottledCallback = fuse_core::ThrottledCallback; diff --git a/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.h b/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.h index 6495cffbe..2b66a4ccf 100644 --- a/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.h +++ b/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.h @@ -68,7 +68,7 @@ template class StampedVariableSynchronizer { public: - FUSE_SMART_PTR_DEFINITIONS(StampedVariableSynchronizer); + FUSE_SMART_PTR_DEFINITIONS(StampedVariableSynchronizer) /** * @brief Construct a synchronizer object diff --git a/fuse_publishers/package.xml b/fuse_publishers/package.xml index ed9d998d2..9d5578a4d 100644 --- a/fuse_publishers/package.xml +++ b/fuse_publishers/package.xml @@ -1,5 +1,8 @@ - + + fuse_publishers 0.4.2 @@ -7,28 +10,42 @@ Stephen Williams + BSD Stephen Williams - BSD + ament_cmake_ros - catkin + fuse_core + fuse_msgs + fuse_variables + geometry_msgs + nav_msgs + pluginlib + rclcpp + tf2 + tf2_geometry_msgs + tf2_ros - fuse_core - fuse_variables - geometry_msgs - nav_msgs - pluginlib - roscpp - tf2 - tf2_geometry_msgs - tf2_ros + fuse_core + fuse_msgs + fuse_variables + geometry_msgs + nav_msgs + pluginlib + rclcpp + tf2 + tf2_geometry_msgs + tf2_ros + ament_cmake_gtest + ament_lint_auto + ament_lint_common fuse_constraints fuse_graphs - roslint - rostest + rclcpp + ament_cmake diff --git a/fuse_publishers/src/path_2d_publisher.cpp b/fuse_publishers/src/path_2d_publisher.cpp index 481097a2b..7b857909f 100644 --- a/fuse_publishers/src/path_2d_publisher.cpp +++ b/fuse_publishers/src/path_2d_publisher.cpp @@ -41,13 +41,15 @@ #include #include #include -#include -#include +#include +#include #include #include + +#include + #include -#include #include #include @@ -59,29 +61,50 @@ namespace fuse_publishers { Path2DPublisher::Path2DPublisher() : - fuse_core::AsyncPublisher(1), + fuse_core::AsyncPublisher(), device_id_(fuse_core::uuid::NIL), frame_id_("map") { } +void Path2DPublisher::initialize( + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Parameters, + fuse_core::node_interfaces::Topics, + fuse_core::node_interfaces::Waitables + > interfaces, + const std::string & name, + size_t thread_count) +{ + interfaces_ = interfaces; + fuse_core::AsyncPublisher::initialize(interfaces_, name, thread_count); +} + void Path2DPublisher::onInit() { // Configure the publisher std::string device_str; - if (private_node_handle_.getParam("device_id", device_str)) + device_str = fuse_core::getParam(interfaces_, "device_id", device_str); + if (device_str != "") { device_id_ = fuse_core::uuid::from_string(device_str); } - else if (private_node_handle_.getParam("device_name", device_str)) - { - device_id_ = fuse_core::uuid::generate(device_str); + else{ + device_str = fuse_core::getParam(interfaces_, "device_name", device_str); + if (device_str != "") + { + device_id_ = fuse_core::uuid::generate(device_str); + } } - private_node_handle_.getParam("frame_id", frame_id_); + frame_id_ = fuse_core::getParam(interfaces_, "frame_id", frame_id_); // Advertise the topic - path_publisher_ = private_node_handle_.advertise("path", 1); - pose_array_publisher_ = private_node_handle_.advertise("pose_array", 1); + rclcpp::PublisherOptions pub_options; + pub_options.callback_group = cb_group_; + + path_publisher_ = rclcpp::create_publisher(interfaces_, name_ + "/path", 1, pub_options); + pose_array_publisher_ = rclcpp::create_publisher(interfaces_, name_ + "/pose_array", 1, pub_options); } void Path2DPublisher::notifyCallback( @@ -89,7 +112,7 @@ void Path2DPublisher::notifyCallback( fuse_core::Graph::ConstSharedPtr graph) { // Exit early if no one is listening - if ((path_publisher_.getNumSubscribers() == 0) && (pose_array_publisher_.getNumSubscribers() == 0)) + if ((path_publisher_->get_subscription_count() == 0) && (pose_array_publisher_->get_subscription_count() == 0)) { return; } @@ -126,7 +149,15 @@ void Path2DPublisher::notifyCallback( // Sort the poses by timestamp auto compare_stamps = [](const geometry_msgs::msg::PoseStamped& pose1, const geometry_msgs::msg::PoseStamped& pose2) { - return pose1.header.stamp < pose2.header.stamp; + if(pose1.header.stamp.sec == pose2.header.stamp.sec){ + return pose1.header.stamp.nanosec < pose2.header.stamp.nanosec; + } + else + { + return pose1.header.stamp.sec < pose2.header.stamp.sec; + } + + }; std::sort(poses.begin(), poses.end(), compare_stamps); // Define the header for the aggregate message @@ -134,15 +165,15 @@ void Path2DPublisher::notifyCallback( header.stamp = poses.back().header.stamp; header.frame_id = frame_id_; // Convert the sorted poses into a Path msg - if (path_publisher_.getNumSubscribers() > 0) + if (path_publisher_->get_subscription_count() > 0) { nav_msgs::msg::Path path_msg; path_msg.header = header; path_msg.poses = poses; - path_publisher_.publish(path_msg); + path_publisher_->publish(path_msg); } // Convert the sorted poses into a PoseArray msg - if (pose_array_publisher_.getNumSubscribers() > 0) + if (pose_array_publisher_->get_subscription_count() > 0) { geometry_msgs::msg::PoseArray pose_array_msg; pose_array_msg.header = header; @@ -153,7 +184,7 @@ void Path2DPublisher::notifyCallback( { return pose.pose; }); // NOLINT(whitespace/braces) - pose_array_publisher_.publish(pose_array_msg); + pose_array_publisher_->publish(pose_array_msg); } } diff --git a/fuse_publishers/src/pose_2d_publisher.cpp b/fuse_publishers/src/pose_2d_publisher.cpp index 110ce3f50..7ac6b1345 100644 --- a/fuse_publishers/src/pose_2d_publisher.cpp +++ b/fuse_publishers/src/pose_2d_publisher.cpp @@ -35,6 +35,7 @@ #include #include +#include #include #include #include @@ -43,15 +44,15 @@ #include #include #include -#include -#include -#include +#include +#include #include #include +#include + +#include #include -#include -#include #include #include @@ -64,12 +65,15 @@ namespace { bool findPose( + rclcpp::Logger logger, + rclcpp::Clock clock, const fuse_core::Graph& graph, const rclcpp::Time& stamp, const fuse_core::UUID& device_id, fuse_core::UUID& orientation_uuid, fuse_core::UUID& position_uuid, - geometry_msgs::msg::Pose& pose) + geometry_msgs::msg::Pose& pose +) { try { @@ -86,14 +90,14 @@ bool findPose( } catch (const std::exception& e) { - RCLCPP_WARN_STREAM_THROTTLE(rclcpp::get_logger("fuse"), rclcpp::Clock(), 10.0 * 1000, - "Failed to find a pose at time " << stamp << ". Error" << e.what()); + RCLCPP_WARN_STREAM_THROTTLE(logger, clock, 10.0 * 1000, + "Failed to find a pose at time " << stamp.nanoseconds() << ". Error" << e.what()); return false; } catch (...) { - RCLCPP_WARN_STREAM_THROTTLE(rclcpp::get_logger("fuse"), rclcpp::Clock(), 10.0 * 1000, - "Failed to find a pose at time " << stamp << ". Error: unknown"); + RCLCPP_WARN_STREAM_THROTTLE(logger, clock, 10.0 * 1000, + "Failed to find a pose at time " << stamp.nanoseconds() << ". Error: unknown"); return false; } return true; @@ -105,29 +109,58 @@ namespace fuse_publishers { Pose2DPublisher::Pose2DPublisher() : - fuse_core::AsyncPublisher(1), + fuse_core::AsyncPublisher(), device_id_(fuse_core::uuid::NIL), publish_to_tf_(false), + tf_timeout_(rclcpp::Duration(0, 0)), use_tf_lookup_(false) { } +void Pose2DPublisher::initialize( + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Clock, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters, + fuse_core::node_interfaces::Services, + fuse_core::node_interfaces::Timers, + fuse_core::node_interfaces::Topics, + fuse_core::node_interfaces::Waitables + > interfaces, + const std::string & name, + size_t thread_count) +{ + interfaces_ = interfaces; + fuse_core::AsyncPublisher::initialize(interfaces_, name, thread_count); +} + void Pose2DPublisher::onInit() { + logger_ = std::make_shared(interfaces_.get_node_logging_interface()->get_logger()); + clock_ = interfaces_.get_node_clock_interface()->get_clock(); + + tf_publisher_ = std::make_shared(interfaces_); + // Read configuration from the parameter server - private_node_handle_.param("base_frame", base_frame_, std::string("base_link")); - private_node_handle_.param("map_frame", map_frame_, std::string("map")); - private_node_handle_.param("odom_frame", odom_frame_, std::string("odom")); + base_frame_ = fuse_core::getParam(interfaces_, "base_frame", std::string("base_link")); + map_frame_ = fuse_core::getParam(interfaces_, "map_frame", std::string("map")); + odom_frame_ = fuse_core::getParam(interfaces_, "odom_frame", std::string("odom")); + std::string device_str; - if (private_node_handle_.getParam("device_id", device_str)) + device_str = fuse_core::getParam(interfaces_, "device_id", device_str); + if (device_str != "") { device_id_ = fuse_core::uuid::from_string(device_str); } - else if (private_node_handle_.getParam("device_name", device_str)) - { - device_id_ = fuse_core::uuid::generate(device_str); + else{ + device_str = fuse_core::getParam(interfaces_, "device_name", device_str); + if (device_str != "") + { + device_id_ = fuse_core::uuid::generate(device_str); + } } - private_node_handle_.param("publish_to_tf", publish_to_tf_, false); + publish_to_tf_ = fuse_core::getParam(interfaces_, "publish_to_tf", false); // Configure tf, if requested if (publish_to_tf_) @@ -137,47 +170,52 @@ void Pose2DPublisher::onInit() { double tf_cache_time; double default_tf_cache_time = 10.0; - private_node_handle_.param("tf_cache_time", tf_cache_time, default_tf_cache_time); + tf_cache_time = fuse_core::getParam(interfaces_, "tf_cache_time", default_tf_cache_time); if (tf_cache_time <= 0) { - RCLCPP_WARN_STREAM(node_->get_logger(), - "The requested tf_cache_time is <= 0. Using the default value (" << - default_tf_cache_time << "s) instead."); + RCLCPP_WARN_STREAM( + *logger_, + "The requested tf_cache_time is <= 0. Using the default value (" + << default_tf_cache_time << "s) instead."); tf_cache_time = default_tf_cache_time; } double tf_timeout; double default_tf_timeout = 0.1; - private_node_handle_.param("tf_timeout", tf_timeout, default_tf_timeout); + tf_timeout = fuse_core::getParam(interfaces_, "tf_timeout", default_tf_timeout); if (tf_timeout <= 0) { - RCLCPP_WARN_STREAM(node_->get_logger(), - "The requested tf_timeout is <= 0. Using the default value (" << - default_tf_timeout << "s) instead."); + RCLCPP_WARN_STREAM( + *logger_, + "The requested tf_timeout is <= 0. Using the default value (" + << default_tf_timeout << "s) instead."); tf_timeout = default_tf_timeout; } tf_timeout_ = rclcpp::Duration::from_seconds(tf_timeout); - tf_buffer_ = std::make_unique(rclcpp::Duration::from_seconds(tf_cache_time)); - tf_listener_ = std::make_unique(*tf_buffer_, node_handle_); - } - - double tf_publish_frequency; - double default_tf_publish_frequency = 10.0; - private_node_handle_.param("tf_publish_frequency", tf_publish_frequency, default_tf_publish_frequency); - if (tf_publish_frequency <= 0) - { - RCLCPP_WARN_STREAM(node_->get_logger(), - "The requested tf_publish_frequency is <= 0. Using the default value (" << - default_tf_publish_frequency << "hz) instead."); - tf_publish_frequency = default_tf_publish_frequency; + tf_buffer_ = std::make_unique( + clock_, + rclcpp::Duration::from_seconds(tf_cache_time) + .to_chrono() + // , interfaces_ // NOTE(methylDragon): This one is pending a change on tf2_ros/buffer.h + // TODO(methylDragon): See above ^ + ); + tf_listener_ = std::make_unique( + *tf_buffer_, + interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface() + ); } } // Advertise the topics - pose_publisher_ = private_node_handle_.advertise("pose", 1); - pose_with_covariance_publisher_ = private_node_handle_.advertise( - "pose_with_covariance", 1); + rclcpp::PublisherOptions pub_options; + pub_options.callback_group = cb_group_; + + pose_publisher_ = rclcpp::create_publisher(interfaces_, name_ + "/pose", 1, pub_options); + pose_with_covariance_publisher_ = rclcpp::create_publisher(interfaces_, name_ + "/pose_with_covariance", 1, pub_options); } void Pose2DPublisher::onStart() @@ -189,9 +227,25 @@ void Pose2DPublisher::onStart() // Start the tf timer if (publish_to_tf_) { - tf_publish_timer_ = node_.create_timer( - rclcpp::Duration::from_seconds(1.0 / tf_publish_frequency), - std::bind(&Pose2DPublisher::tfPublishTimerCallback, this) + double tf_publish_frequency; + double default_tf_publish_frequency = 10.0; + + // We pull the param again on each start so the publisher can technically get set to a different + // publish frequency + tf_publish_frequency = fuse_core::getParam(interfaces_, "tf_publish_frequency", default_tf_publish_frequency); + if (tf_publish_frequency <= 0) + { + RCLCPP_WARN_STREAM(*logger_, + "The requested tf_publish_frequency is <= 0. Using the default value (" << + default_tf_publish_frequency << "hz) instead."); + tf_publish_frequency = default_tf_publish_frequency; + } + tf_publish_timer_ = rclcpp::create_timer( + interfaces_, + clock_, + std::chrono::duration(1.0 / tf_publish_frequency), + std::move(std::bind(&Pose2DPublisher::tfPublishTimerCallback, this)), + cb_group_ ); } } @@ -201,7 +255,7 @@ void Pose2DPublisher::onStop() // Stop the tf timer if (publish_to_tf_) { - tf_publish_timer_.cancel(); + tf_publish_timer_->cancel(); } } @@ -213,7 +267,7 @@ void Pose2DPublisher::notifyCallback( if (!fuse_core::is_valid(latest_stamp)) // If uninitialized { RCLCPP_WARN_STREAM_THROTTLE( - node_->get_logger(), *node_->get_clock(), 10.0 * 1000, + *logger_, *clock_, 10.0 * 1000, "Failed to find a matching set of stamped pose variables with device id '" << device_id_ << "'."); return; } @@ -221,7 +275,7 @@ void Pose2DPublisher::notifyCallback( fuse_core::UUID orientation_uuid; fuse_core::UUID position_uuid; geometry_msgs::msg::Pose pose; - if (!findPose(*graph, latest_stamp, device_id_, orientation_uuid, position_uuid, pose)) + if (!findPose(*logger_, *clock_, *graph, latest_stamp, device_id_, orientation_uuid, position_uuid, pose)) { return; } @@ -253,7 +307,7 @@ void Pose2DPublisher::notifyCallback( catch (const std::exception& e) { RCLCPP_WARN_STREAM_THROTTLE( - node_->get_logger(), *node_->get_clock(), 2.0 * 1000, + *logger_, *clock_, 2.0 * 1000, "Could not lookup the transform " << base_frame_ << "->" << odom_frame_ << ". Error: " << e.what()); } } @@ -263,15 +317,15 @@ void Pose2DPublisher::notifyCallback( tf_transform_ = map_to_base; } } - if (pose_publisher_.getNumSubscribers() > 0) + if (pose_publisher_->get_subscription_count() > 0) { geometry_msgs::msg::PoseStamped msg; msg.header.stamp = latest_stamp; msg.header.frame_id = map_frame_; msg.pose = pose; - pose_publisher_.publish(msg); + pose_publisher_->publish(msg); } - if (pose_with_covariance_publisher_.getNumSubscribers() > 0) + if (pose_with_covariance_publisher_->get_subscription_count() > 0) { // Get the covariance from the graph std::vector> requests; @@ -293,7 +347,7 @@ void Pose2DPublisher::notifyCallback( msg.pose.covariance[30] = covariance_blocks[1][0]; msg.pose.covariance[31] = covariance_blocks[1][1]; msg.pose.covariance[35] = covariance_blocks[2][0]; - pose_with_covariance_publisher_.publish(msg); + pose_with_covariance_publisher_->publish(msg); } } @@ -304,8 +358,8 @@ void Pose2DPublisher::tfPublishTimerCallback() if (fuse_core::is_valid(tf_transform_.header.stamp)) { // Update the timestamp of the transform so the tf tree will continue to be valid - tf_transform_.header.stamp = event.current_real; - tf_publisher_.sendTransform(tf_transform_); + tf_transform_.header.stamp = clock_->now(); + tf_publisher_->sendTransform(tf_transform_); } } diff --git a/fuse_publishers/src/serialized_publisher.cpp b/fuse_publishers/src/serialized_publisher.cpp index a8c87105c..641e5296c 100644 --- a/fuse_publishers/src/serialized_publisher.cpp +++ b/fuse_publishers/src/serialized_publisher.cpp @@ -41,8 +41,8 @@ #include #include #include -#include -#include +#include +#include // Register this publisher with ROS as a plugin. @@ -52,36 +52,64 @@ namespace fuse_publishers { SerializedPublisher::SerializedPublisher() : - fuse_core::AsyncPublisher(1), + fuse_core::AsyncPublisher(), frame_id_("map"), graph_publisher_throttled_callback_( std::bind(&SerializedPublisher::graphPublisherCallback, this, std::placeholders::_1, std::placeholders::_2)) { } +void SerializedPublisher::initialize( + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Clock, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters, + fuse_core::node_interfaces::Topics, + fuse_core::node_interfaces::Waitables + > interfaces, + const std::string & name, + size_t thread_count) +{ + interfaces_ = interfaces; + fuse_core::AsyncPublisher::initialize(interfaces_, name, thread_count); +} + void SerializedPublisher::onInit() { // Configure the publisher - private_node_handle_.getParam("frame_id", frame_id_); + frame_id_ = fuse_core::getParam(interfaces_, "frame_id", frame_id_); bool latch = false; - private_node_handle_.getParam("latch", latch); + latch = fuse_core::getParam(interfaces_, "latch", latch); - rclcpp::Duration graph_throttle_period{ 0 }; - fuse_core::getPositiveParam(private_node_handle_, "graph_throttle_period", graph_throttle_period, false); + rclcpp::Duration graph_throttle_period{ 0, 0 }; + fuse_core::getPositiveParam(interfaces_, "graph_throttle_period", graph_throttle_period, false); bool graph_throttle_use_wall_time{ false }; - private_node_handle_.getParam("graph_throttle_use_wall_time", graph_throttle_use_wall_time); + graph_throttle_use_wall_time = + fuse_core::getParam( + interfaces_, "graph_throttle_use_wall_time", graph_throttle_use_wall_time); graph_publisher_throttled_callback_.setThrottlePeriod(graph_throttle_period); if (!graph_throttle_use_wall_time) { - graph_publisher_throttled_callback_.setClock(node_->get_clock()); + graph_publisher_throttled_callback_.setClock(interfaces_.get_node_clock_interface()->get_clock()); } // Advertise the topics - graph_publisher_ = private_node_handle_.advertise("graph", 1, latch); - transaction_publisher_ = private_node_handle_.advertise("transaction", 1, latch); + rclcpp::QoS qos(1); // Queue size of 1 + if (latch) { + qos.transient_local(); + } + + rclcpp::PublisherOptions pub_options; + pub_options.callback_group = cb_group_; + + graph_publisher_ = + rclcpp::create_publisher(interfaces_, "graph", qos, pub_options); + transaction_publisher_ = + rclcpp::create_publisher(interfaces_, "transaction", qos, pub_options); } void SerializedPublisher::notifyCallback( @@ -89,28 +117,29 @@ void SerializedPublisher::notifyCallback( fuse_core::Graph::ConstSharedPtr graph) { const auto& stamp = transaction->stamp(); - if (graph_publisher_.getNumSubscribers() > 0) + if (graph_publisher_->get_subscription_count() > 0) { graph_publisher_throttled_callback_(graph, stamp); } - if (transaction_publisher_.getNumSubscribers() > 0) + if (transaction_publisher_->get_subscription_count() > 0) { fuse_msgs::msg::SerializedTransaction msg; msg.header.stamp = stamp; msg.header.frame_id = frame_id_; fuse_core::serializeTransaction(*transaction, msg); - transaction_publisher_.publish(msg); + transaction_publisher_->publish(msg); } } -void SerializedPublisher::graphPublisherCallback(fuse_core::Graph::ConstSharedPtr graph, const rclcpp::Time& stamp) const +void SerializedPublisher::graphPublisherCallback( + fuse_core::Graph::ConstSharedPtr graph, const rclcpp::Time& stamp) const { fuse_msgs::msg::SerializedGraph msg; msg.header.stamp = stamp; msg.header.frame_id = frame_id_; fuse_core::serializeGraph(*graph, msg); - graph_publisher_.publish(msg); + graph_publisher_->publish(msg); } } // namespace fuse_publishers diff --git a/fuse_publishers/test/CMakeLists.txt b/fuse_publishers/test/CMakeLists.txt new file mode 100644 index 000000000..374182980 --- /dev/null +++ b/fuse_publishers/test/CMakeLists.txt @@ -0,0 +1,25 @@ +find_package(fuse_constraints REQUIRED) +find_package(fuse_graphs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) + +# CORE GTESTS ====================================================================================== +ament_add_gtest("test_stamped_variable_synchronizer" "test_stamped_variable_synchronizer.cpp") +target_link_libraries("test_stamped_variable_synchronizer" + ${PROJECT_NAME} + fuse_constraints::fuse_constraints + fuse_graphs::fuse_graphs +) + +ament_add_gtest("test_path_2d_publisher" "test_path_2d_publisher.cpp") +target_link_libraries("test_path_2d_publisher" + ${PROJECT_NAME} + fuse_constraints::fuse_constraints + fuse_graphs::fuse_graphs +) + +ament_add_gtest("test_pose_2d_publisher" "test_pose_2d_publisher.cpp") +target_link_libraries("test_pose_2d_publisher" + ${PROJECT_NAME} + fuse_constraints::fuse_constraints + fuse_graphs::fuse_graphs +) diff --git a/fuse_publishers/test/path_2d_publisher.test b/fuse_publishers/test/path_2d_publisher.test deleted file mode 100644 index 31eed260c..000000000 --- a/fuse_publishers/test/path_2d_publisher.test +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/fuse_publishers/test/pose_2d_publisher.test b/fuse_publishers/test/pose_2d_publisher.test deleted file mode 100644 index 657d3a027..000000000 --- a/fuse_publishers/test/pose_2d_publisher.test +++ /dev/null @@ -1,4 +0,0 @@ - - - - diff --git a/fuse_publishers/test/test_path_2d_publisher.cpp b/fuse_publishers/test/test_path_2d_publisher.cpp index 4b6a66371..0539f9a6f 100644 --- a/fuse_publishers/test/test_path_2d_publisher.cpp +++ b/fuse_publishers/test/test_path_2d_publisher.cpp @@ -42,7 +42,9 @@ #include #include #include -#include +#include +// Workaround ros2/geometry2#242 +#include // NOLINT(build/include_order) #include #include @@ -60,34 +62,39 @@ class Path2DPublisherTestFixture : public ::testing::Test { public: Path2DPublisherTestFixture() : - private_node_handle_("~"), graph_(fuse_graphs::HashGraph::make_shared()), transaction_(fuse_core::Transaction::make_shared()), received_path_msg_(false), received_pose_array_msg_(false) { // Add a few pose variables - auto position1 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1234, 10)); + auto position1 = + fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1234, 10, RCL_ROS_TIME)); position1->x() = 1.01; position1->y() = 2.01; - auto orientation1 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1234, 10)); + auto orientation1 = + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1234, 10, RCL_ROS_TIME)); orientation1->yaw() = 3.01; - auto position2 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 10)); + auto position2 = + fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 10, RCL_ROS_TIME)); position2->x() = 1.02; position2->y() = 2.02; - auto orientation2 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 10)); + auto orientation2 = + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 10, RCL_ROS_TIME)); orientation2->yaw() = 3.02; - auto position3 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 9)); + auto position3 = + fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 9, RCL_ROS_TIME)); position3->x() = 1.03; position3->y() = 2.03; - auto orientation3 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 9)); + auto orientation3 = + fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 9, RCL_ROS_TIME)); orientation3->yaw() = 3.03; - auto position4 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 11), - fuse_core::uuid::generate("kitt")); + auto position4 = fuse_variables::Position2DStamped::make_shared( + rclcpp::Time(1235, 11, RCL_ROS_TIME), fuse_core::uuid::generate("kitt")); position4->x() = 1.04; position4->y() = 2.04; - auto orientation4 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 11), - fuse_core::uuid::generate("kitt")); + auto orientation4 = fuse_variables::Orientation2DStamped::make_shared( + rclcpp::Time(1235, 11, RCL_ROS_TIME), fuse_core::uuid::generate("kitt")); orientation4->yaw() = 3.04; transaction_->addInvolvedStamp(position1->stamp()); @@ -110,27 +117,39 @@ class Path2DPublisherTestFixture : public ::testing::Test fuse_core::Vector3d mean1; mean1 << 1.01, 2.01, 3.01; fuse_core::Matrix3d cov1; + /* *INDENT-OFF* */ cov1 << 1.01, 0.0, 0.0, 0.0, 2.01, 0.0, 0.0, 0.0, 3.01; - auto constraint1 = fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( - "test", *position1, *orientation1, mean1, cov1); + /* *INDENT-ON* */ + auto constraint1 = + fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( + "test", *position1, *orientation1, mean1, cov1); fuse_core::Vector3d mean2; mean2 << 1.02, 2.02, 3.02; fuse_core::Matrix3d cov2; + /* *INDENT-OFF* */ cov2 << 1.02, 0.0, 0.0, 0.0, 2.02, 0.0, 0.0, 0.0, 3.02; - auto constraint2 = fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( - "test", *position2, *orientation2, mean2, cov2); + /* *INDENT-ON* */ + auto constraint2 = + fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( + "test", *position2, *orientation2, mean2, cov2); fuse_core::Vector3d mean3; mean3 << 1.03, 2.03, 3.03; fuse_core::Matrix3d cov3; + /* *INDENT-OFF* */ cov3 << 1.03, 0.0, 0.0, 0.0, 2.03, 0.0, 0.0, 0.0, 3.03; - auto constraint3 = fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( - "test", *position3, *orientation3, mean3, cov3); + /* *INDENT-ON* */ + auto constraint3 = + fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( + "test", *position3, *orientation3, mean3, cov3); fuse_core::Vector3d mean4; mean4 << 1.04, 2.04, 3.04; fuse_core::Matrix3d cov4; + /* *INDENT-OFF* */ cov4 << 1.04, 0.0, 0.0, 0.0, 2.04, 0.0, 0.0, 0.0, 3.04; - auto constraint4 = fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( - "test", *position4, *orientation4, mean4, cov4); + /* *INDENT-ON* */ + auto constraint4 = + fuse_constraints::AbsolutePose2DStampedConstraint::make_shared( + "test", *position4, *orientation4, mean4, cov4); transaction_->addConstraint(constraint1); transaction_->addConstraint(constraint2); transaction_->addConstraint(constraint3); @@ -141,6 +160,24 @@ class Path2DPublisherTestFixture : public ::testing::Test graph_->optimize(); } + void SetUp() override + { + executor_ = std::make_shared(); + spinner_ = std::thread( + [&]() { + executor_->spin(); + }); + } + + void TearDown() override + { + executor_->cancel(); + if (spinner_.joinable()) { + spinner_.join(); + } + executor_.reset(); + } + void pathCallback(const nav_msgs::msg::Path& msg) { path_msg_ = msg; @@ -153,10 +190,10 @@ class Path2DPublisherTestFixture : public ::testing::Test received_pose_array_msg_ = true; } + std::thread spinner_; //!< Internal thread for spinning the executor + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + protected: - // TODO(CH3): Replace with node_ (also, we don't need to support node interfaces here since it's a test...) - ros::NodeHandle node_handle_; - ros::NodeHandle private_node_handle_; fuse_graphs::HashGraph::SharedPtr graph_; fuse_core::Transaction::SharedPtr transaction_; bool received_path_msg_; @@ -168,66 +205,66 @@ class Path2DPublisherTestFixture : public ::testing::Test TEST_F(Path2DPublisherTestFixture, PublishPath) { // Test that the expected PoseStamped message is published + auto node = rclcpp::Node::make_shared("test_path_2d_publisher_node"); + executor_->add_node(node); + + fuse_core::getParam(node, "frame_id", std::string("test_map")); // Create a publisher and send it the graph - private_node_handle_.setParam("test_publisher/frame_id", "test_map"); fuse_publishers::Path2DPublisher publisher; - publisher.initialize("test_publisher"); + + // This is the name of the pub's inner node (to set params against) + publisher.initialize(node, "test_publisher", 0); publisher.start(); // Subscribe to the "path" topic - ros::Subscriber subscriber1 = private_node_handle_.subscribe( - "test_publisher/path", - 1, - &Path2DPublisherTestFixture::pathCallback, - reinterpret_cast(this)); + auto subscriber1 = node->create_subscription( + "/test_publisher/path", 1, + std::bind(&Path2DPublisherTestFixture::pathCallback, this, std::placeholders::_1)); // Subscribe to the "pose_array" topic - ros::Subscriber subscriber2 = private_node_handle_.subscribe( - "test_publisher/pose_array", - 1, - &Path2DPublisherTestFixture::poseArrayCallback, - reinterpret_cast(this)); + auto subscriber2 = node->create_subscription( + "/test_publisher/pose_array", 1, + std::bind(&Path2DPublisherTestFixture::poseArrayCallback, this, std::placeholders::_1)); // Send the graph to the Publisher to trigger message publishing publisher.notify(transaction_, graph_); // Verify the subscriber received the expected pose - rclcpp::Time timeout = node_->now() + rclcpp::Duration::from_seconds(10.0); - while ((!received_path_msg_) && (node_->now() < timeout)) + rclcpp::Time timeout = node->now() + rclcpp::Duration::from_seconds(10.0); + while ((!received_path_msg_) && (node->now() < timeout)) { - rclcpp::sleep_for(rclcpp::Duration::from_seconds(0.10)); + rclcpp::sleep_for(rclcpp::Duration::from_seconds(0.10).to_chrono()); } ASSERT_TRUE(received_path_msg_); - EXPECT_EQ(rclcpp::Time(1235, 10), path_msg_.header.stamp); + EXPECT_EQ(rclcpp::Time(1235, 10, RCL_ROS_TIME), path_msg_.header.stamp); EXPECT_EQ("test_map", path_msg_.header.frame_id); ASSERT_EQ(3ul, path_msg_.poses.size()); - EXPECT_EQ(rclcpp::Time(1234, 10), path_msg_.poses[0].header.stamp); + EXPECT_EQ(rclcpp::Time(1234, 10, RCL_ROS_TIME), path_msg_.poses[0].header.stamp); EXPECT_EQ("test_map", path_msg_.poses[0].header.frame_id); EXPECT_NEAR(1.01, path_msg_.poses[0].pose.position.x, 1.0e-9); EXPECT_NEAR(2.01, path_msg_.poses[0].pose.position.y, 1.0e-9); EXPECT_NEAR(0.00, path_msg_.poses[0].pose.position.z, 1.0e-9); EXPECT_NEAR(3.01, tf2::getYaw(path_msg_.poses[0].pose.orientation), 1.0e-9); - EXPECT_EQ(rclcpp::Time(1235, 9), path_msg_.poses[1].header.stamp); + EXPECT_EQ(rclcpp::Time(1235, 9, RCL_ROS_TIME), path_msg_.poses[1].header.stamp); EXPECT_EQ("test_map", path_msg_.poses[1].header.frame_id); EXPECT_NEAR(1.03, path_msg_.poses[1].pose.position.x, 1.0e-9); EXPECT_NEAR(2.03, path_msg_.poses[1].pose.position.y, 1.0e-9); EXPECT_NEAR(0.00, path_msg_.poses[1].pose.position.z, 1.0e-9); EXPECT_NEAR(3.03, tf2::getYaw(path_msg_.poses[1].pose.orientation), 1.0e-9); - EXPECT_EQ(rclcpp::Time(1235, 10), path_msg_.poses[2].header.stamp); + EXPECT_EQ(rclcpp::Time(1235, 10, RCL_ROS_TIME), path_msg_.poses[2].header.stamp); EXPECT_EQ("test_map", path_msg_.poses[2].header.frame_id); EXPECT_NEAR(1.02, path_msg_.poses[2].pose.position.x, 1.0e-9); EXPECT_NEAR(2.02, path_msg_.poses[2].pose.position.y, 1.0e-9); EXPECT_NEAR(0.00, path_msg_.poses[2].pose.position.z, 1.0e-9); EXPECT_NEAR(3.02, tf2::getYaw(path_msg_.poses[2].pose.orientation), 1.0e-9); - ASSERT_TRUE(received_pose_array_msg_); - EXPECT_EQ(rclcpp::Time(1235, 10), pose_array_msg_.header.stamp); + EXPECT_EQ(rclcpp::Time(1235, 10, RCL_ROS_TIME), pose_array_msg_.header.stamp); EXPECT_EQ("test_map", pose_array_msg_.header.frame_id); ASSERT_EQ(3ul, pose_array_msg_.poses.size()); @@ -247,15 +284,13 @@ TEST_F(Path2DPublisherTestFixture, PublishPath) EXPECT_NEAR(3.02, tf2::getYaw(pose_array_msg_.poses[2].orientation), 1.0e-9); } -int main(int argc, char** argv) + +// NOTE(CH3): This main is required because the test is manually run by a launch test +int main(int argc, char ** argv) { + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "test_path_2d_publisher"); - - ros::AsyncSpinner spinner(1); - spinner.start(); int ret = RUN_ALL_TESTS(); - spinner.stop(); - ros::shutdown(); + rclcpp::shutdown(); return ret; } diff --git a/fuse_publishers/test/test_pose_2d_publisher.cpp b/fuse_publishers/test/test_pose_2d_publisher.cpp index c7ccf9f60..bc85e5357 100644 --- a/fuse_publishers/test/test_pose_2d_publisher.cpp +++ b/fuse_publishers/test/test_pose_2d_publisher.cpp @@ -41,7 +41,9 @@ #include #include #include -#include +#include +// Workaround ros2/geometry2#242 +#include // NOLINT(build/include_order) #include #include #include @@ -61,7 +63,6 @@ class Pose2DPublisherTestFixture : public ::testing::Test { public: Pose2DPublisherTestFixture() : - private_node_handle_("~"), graph_(fuse_graphs::HashGraph::make_shared()), transaction_(fuse_core::Transaction::make_shared()), received_pose_msg_(false), @@ -69,26 +70,26 @@ class Pose2DPublisherTestFixture : public ::testing::Test received_tf_msg_(false) { // Add a few pose variables - auto position1 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1234, 10)); + auto position1 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1234, 10, RCL_ROS_TIME)); position1->x() = 1.01; position1->y() = 2.01; - auto orientation1 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1234, 10)); + auto orientation1 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1234, 10, RCL_ROS_TIME)); orientation1->yaw() = 3.01; - auto position2 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 10)); + auto position2 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 10, RCL_ROS_TIME)); position2->x() = 1.02; position2->y() = 2.02; - auto orientation2 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 10)); + auto orientation2 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 10, RCL_ROS_TIME)); orientation2->yaw() = 3.02; - auto position3 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 9)); + auto position3 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 9, RCL_ROS_TIME)); position3->x() = 1.03; position3->y() = 2.03; - auto orientation3 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 9)); + auto orientation3 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 9, RCL_ROS_TIME)); orientation3->yaw() = 3.03; - auto position4 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 11), + auto position4 = fuse_variables::Position2DStamped::make_shared(rclcpp::Time(1235, 11, RCL_ROS_TIME), fuse_core::uuid::generate("kitt")); position4->x() = 1.04; position4->y() = 2.04; - auto orientation4 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 11), + auto orientation4 = fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(1235, 11, RCL_ROS_TIME), fuse_core::uuid::generate("kitt")); orientation4->yaw() = 3.04; @@ -143,18 +144,35 @@ class Pose2DPublisherTestFixture : public ::testing::Test graph_->optimize(); // Publish a odom->base transform so tf lookups will succeed - geometry_msgs::msg::TransformStamped odom_to_base; - odom_to_base.header.stamp = rclcpp::Time(0, 0); - odom_to_base.header.frame_id = "test_odom"; - odom_to_base.child_frame_id = "test_base"; - odom_to_base.transform.translation.x = -0.10; - odom_to_base.transform.translation.y = -0.20; - odom_to_base.transform.translation.z = -0.30; - odom_to_base.transform.rotation.x = 0.0; - odom_to_base.transform.rotation.y = 0.0; - odom_to_base.transform.rotation.z = -0.1986693307950612164; - odom_to_base.transform.rotation.w = 0.98006657784124162625; // -0.4rad in yaw - static_broadcaster_.sendTransform(odom_to_base); + odom_to_base_.header.stamp = rclcpp::Time(0, 0, RCL_ROS_TIME); + odom_to_base_.header.frame_id = "test_odom"; + odom_to_base_.child_frame_id = "test_base"; + odom_to_base_.transform.translation.x = -0.10; + odom_to_base_.transform.translation.y = -0.20; + odom_to_base_.transform.translation.z = -0.30; + odom_to_base_.transform.rotation.x = 0.0; + odom_to_base_.transform.rotation.y = 0.0; + odom_to_base_.transform.rotation.z = -0.1986693307950612164; + odom_to_base_.transform.rotation.w = 0.98006657784124162625; // -0.4rad in yaw + } + + void SetUp() override + { + executor_ = std::make_shared(); + + spinner_ = std::thread( + [&]() { + executor_->spin(); + }); + } + + void TearDown() override + { + executor_->cancel(); + if (spinner_.joinable()) { + spinner_.join(); + } + executor_.reset(); } void poseCallback(const geometry_msgs::msg::PoseStamped& msg) @@ -175,10 +193,11 @@ class Pose2DPublisherTestFixture : public ::testing::Test tf_msg_ = msg; } + std::thread spinner_; //!< Internal thread for spinning the executor + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + protected: - // TODO(CH3): Replace with node_ (also, we don't need to support node interfaces here since it's a test...) - ros::NodeHandle node_handle_; - ros::NodeHandle private_node_handle_; + geometry_msgs::msg::TransformStamped odom_to_base_; fuse_graphs::HashGraph::SharedPtr graph_; fuse_core::Transaction::SharedPtr transaction_; bool received_pose_msg_; @@ -187,41 +206,41 @@ class Pose2DPublisherTestFixture : public ::testing::Test geometry_msgs::msg::PoseWithCovarianceStamped pose_with_covariance_msg_; bool received_tf_msg_; tf2_msgs::msg::TFMessage tf_msg_; - tf2_ros::StaticTransformBroadcaster static_broadcaster_; }; TEST_F(Pose2DPublisherTestFixture, PublishPose) { // Test that the expected PoseStamped message is published + auto node = rclcpp::Node::make_shared("test_pose_2d_publisher_node"); + executor_->add_node(node); + + fuse_core::getParam(node, "map_frame", std::string("test_map")); + fuse_core::getParam(node, "odom_frame", std::string("test_odom")); + fuse_core::getParam(node, "base_frame", std::string("test_base")); + fuse_core::getParam(node, "publish_to_tf", false); // Create a publisher and send it the graph - private_node_handle_.setParam("test_publisher/map_frame", "test_map"); - private_node_handle_.setParam("test_publisher/odom_frame", "test_odom"); - private_node_handle_.setParam("test_publisher/base_frame", "test_base"); - private_node_handle_.setParam("test_publisher/publish_to_tf", false); fuse_publishers::Pose2DPublisher publisher; - publisher.initialize("test_publisher"); + publisher.initialize(node, "test_publisher", 0); publisher.start(); // Subscribe to the "pose" topic - ros::Subscriber subscriber = private_node_handle_.subscribe( - "test_publisher/pose", - 1, - &Pose2DPublisherTestFixture::poseCallback, - reinterpret_cast(this)); + auto subscriber1 = node->create_subscription( + "/test_publisher/pose", 1, + std::bind(&Pose2DPublisherTestFixture::poseCallback, this, std::placeholders::_1)); // Send the graph to the Publisher to trigger message publishing publisher.notify(transaction_, graph_); // Verify the subscriber received the expected pose - rclcpp::Time timeout = node_->now() + rclcpp::Duration::from_seconds(10.0); - while ((!received_pose_msg_) && (node_->now() < timeout)) + rclcpp::Time timeout = node->now() + rclcpp::Duration::from_seconds(10.0); + while ((!received_pose_msg_) && (node->now() < timeout)) { - rclcpp::sleep_for(rclcpp::Duration::from_seconds(0.10)); + rclcpp::sleep_for(rclcpp::Duration::from_seconds(0.10).to_chrono()); } ASSERT_TRUE(received_pose_msg_); - EXPECT_EQ(rclcpp::Time(1235, 10), pose_msg_.header.stamp); + EXPECT_EQ(rclcpp::Time(1235, 10, RCL_ROS_TIME), pose_msg_.header.stamp); EXPECT_EQ("test_map", pose_msg_.header.frame_id); EXPECT_NEAR(1.02, pose_msg_.pose.position.x, 1.0e-9); EXPECT_NEAR(2.02, pose_msg_.pose.position.y, 1.0e-9); @@ -232,35 +251,38 @@ TEST_F(Pose2DPublisherTestFixture, PublishPose) TEST_F(Pose2DPublisherTestFixture, PublishPoseWithCovariance) { // Test that the expected PoseWithCovarianceStamped message is published + auto node = rclcpp::Node::make_shared("test_pose_2d_publisher_node"); + executor_->add_node(node); + + fuse_core::getParam(node, "map_frame", std::string("test_map")); + fuse_core::getParam(node, "odom_frame", std::string("test_odom")); + fuse_core::getParam(node, "base_frame", std::string("test_base")); + fuse_core::getParam(node, "publish_to_tf", false); // Create a publisher and send it the graph - private_node_handle_.setParam("test_publisher/map_frame", "test_map"); - private_node_handle_.setParam("test_publisher/odom_frame", "test_odom"); - private_node_handle_.setParam("test_publisher/base_frame", "test_base"); - private_node_handle_.setParam("test_publisher/publish_to_tf", false); fuse_publishers::Pose2DPublisher publisher; - publisher.initialize("test_publisher"); + publisher.initialize(node, "test_publisher", 0); publisher.start(); // Subscribe to the "pose_with_covariance" topic - ros::Subscriber subscriber = private_node_handle_.subscribe( - "test_publisher/pose_with_covariance", - 1, - &Pose2DPublisherTestFixture::poseWithCovarianceCallback, - reinterpret_cast(this)); + auto subscriber1 = node->create_subscription( + "/test_publisher/pose_with_covariance", 1, + std::bind(&Pose2DPublisherTestFixture::poseWithCovarianceCallback, this, std::placeholders::_1)); // Send the graph to the Publisher to trigger message publishing publisher.notify(transaction_, graph_); // Verify the subscriber received the expected pose - rclcpp::Time timeout = node_->now() + rclcpp::Duration::from_seconds(10.0); - while ((!received_pose_with_covariance_msg_) && (node_->now() < timeout)) + rclcpp::Time timeout = node->now() + rclcpp::Duration::from_seconds(10.0); + while ((!received_pose_with_covariance_msg_) && (node->now() < timeout)) { - rclcpp::sleep_for(rclcpp::Duration::from_seconds(0.10)); + rclcpp::sleep_for(rclcpp::Duration::from_seconds(0.10).to_chrono()); } + rclcpp::sleep_for(rclcpp::Duration::from_seconds(5).to_chrono()); + ASSERT_TRUE(received_pose_with_covariance_msg_); - EXPECT_EQ(rclcpp::Time(1235, 10), pose_with_covariance_msg_.header.stamp); + EXPECT_EQ(rclcpp::Time(1235, 10, RCL_ROS_TIME), pose_with_covariance_msg_.header.stamp); EXPECT_EQ("test_map", pose_with_covariance_msg_.header.frame_id); EXPECT_NEAR(1.02, pose_with_covariance_msg_.pose.pose.position.x, 1.0e-9); EXPECT_NEAR(2.02, pose_with_covariance_msg_.pose.pose.position.y, 1.0e-9); @@ -268,12 +290,14 @@ TEST_F(Pose2DPublisherTestFixture, PublishPoseWithCovariance) EXPECT_NEAR(3.02, tf2::getYaw(pose_with_covariance_msg_.pose.pose.orientation), 1.0e-9); std::vector expected_covariance = { + /* *INDENT-OFF* */ 1.02, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.02, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 3.02 + /* *INDENT-ON* */ }; for (size_t i = 0; i < 36; ++i) { @@ -284,33 +308,41 @@ TEST_F(Pose2DPublisherTestFixture, PublishPoseWithCovariance) TEST_F(Pose2DPublisherTestFixture, PublishTfWithoutOdom) { // Test that the expected TFMessage is published + auto node = rclcpp::Node::make_shared("test_pose_2d_publisher_node"); + executor_->add_node(node); + + fuse_core::getParam(node, "map_frame", std::string("test_map")); + fuse_core::getParam(node, "odom_frame", std::string("test_base")); + fuse_core::getParam(node, "base_frame", std::string("test_base")); + fuse_core::getParam(node, "publish_to_tf", true); + + auto tf_node_ = rclcpp::Node::make_shared("tf_pub_node"); + executor_->add_node(tf_node_); + auto static_broadcaster_ = tf2_ros::StaticTransformBroadcaster(tf_node_); + static_broadcaster_.sendTransform(odom_to_base_); + + // Subscribe to the "pose" topic + auto subscriber1 = node->create_subscription( + "/tf", 1, + std::bind(&Pose2DPublisherTestFixture::tfCallback, this, std::placeholders::_1)); // Create a publisher and send it the graph - private_node_handle_.setParam("test_publisher/map_frame", "test_map"); - private_node_handle_.setParam("test_publisher/odom_frame", "test_base"); - private_node_handle_.setParam("test_publisher/base_frame", "test_base"); - private_node_handle_.setParam("test_publisher/publish_to_tf", true); fuse_publishers::Pose2DPublisher publisher; - publisher.initialize("test_publisher"); + publisher.initialize(node, "test_publisher", 0); publisher.start(); - // Subscribe to the "pose" topic - ros::Subscriber subscriber = private_node_handle_.subscribe( - "/tf", - 1, - &Pose2DPublisherTestFixture::tfCallback, - reinterpret_cast(this)); - // Send the graph to the Publisher to trigger message publishing publisher.notify(transaction_, graph_); // Verify the subscriber received the expected pose - rclcpp::Time timeout = node_->now() + rclcpp::Duration::from_seconds(10.0); - while ((!received_tf_msg_) && (node_->now() < timeout)) + rclcpp::Time timeout = node->now() + rclcpp::Duration::from_seconds(10.0); + while ((!received_tf_msg_) && (node->now() < timeout)) { - rclcpp::sleep_for(rclcpp::Duration::from_seconds(0.10)); + rclcpp::sleep_for(rclcpp::Duration::from_seconds(0.10).to_chrono()); } + rclcpp::sleep_for(rclcpp::Duration::from_seconds(5).to_chrono()); + ASSERT_TRUE(received_tf_msg_); ASSERT_EQ(1ul, tf_msg_.transforms.size()); EXPECT_EQ("test_map", tf_msg_.transforms[0].header.frame_id); @@ -324,31 +356,37 @@ TEST_F(Pose2DPublisherTestFixture, PublishTfWithoutOdom) TEST_F(Pose2DPublisherTestFixture, PublishTfWithOdom) { // Test that the expected TFMessage is published + auto node = rclcpp::Node::make_shared("test_pose_2d_publisher_node"); + executor_->add_node(node); + + fuse_core::getParam(node, "map_frame", std::string("test_map")); + fuse_core::getParam(node, "odom_frame", std::string("test_odom")); + fuse_core::getParam(node, "base_frame", std::string("test_base")); + fuse_core::getParam(node, "publish_to_tf", true); + + // Subscribe to the "pose" topic + auto subscriber1 = node->create_subscription( + "/tf", 1, + std::bind(&Pose2DPublisherTestFixture::tfCallback, this, std::placeholders::_1)); + + auto tf_node_ = rclcpp::Node::make_shared("tf_pub_node"); + executor_->add_node(tf_node_); + auto static_broadcaster_ = tf2_ros::StaticTransformBroadcaster(tf_node_); + static_broadcaster_.sendTransform(odom_to_base_); // Create a publisher and send it the graph - private_node_handle_.setParam("test_publisher/map_frame", "test_map"); - private_node_handle_.setParam("test_publisher/odom_frame", "test_odom"); - private_node_handle_.setParam("test_publisher/base_frame", "test_base"); - private_node_handle_.setParam("test_publisher/publish_to_tf", true); fuse_publishers::Pose2DPublisher publisher; - publisher.initialize("test_publisher"); + publisher.initialize(node, "test_publisher", 0); publisher.start(); - // Subscribe to the "pose" topic - ros::Subscriber subscriber = private_node_handle_.subscribe( - "/tf", - 1, - &Pose2DPublisherTestFixture::tfCallback, - reinterpret_cast(this)); - // Send the graph to the Publisher to trigger message publishing publisher.notify(transaction_, graph_); // Verify the subscriber received the expected pose - rclcpp::Time timeout = node_->now() + rclcpp::Duration::from_seconds(10.0); - while ((!received_tf_msg_) && (node_->now() < timeout)) + rclcpp::Time timeout = node->now() + rclcpp::Duration::from_seconds(10.0); + while ((!received_tf_msg_) && (node->now() < timeout)) { - rclcpp::sleep_for(rclcpp::Duration::from_seconds(0.10)); + rclcpp::sleep_for(rclcpp::Duration::from_seconds(0.10).to_chrono()); } ASSERT_TRUE(received_tf_msg_); @@ -361,15 +399,12 @@ TEST_F(Pose2DPublisherTestFixture, PublishTfWithOdom) EXPECT_NEAR(-2.8631853072, tf2::getYaw(tf_msg_.transforms[0].transform.rotation), 1.0e-9); } -int main(int argc, char** argv) +// NOTE(CH3): This main is required because the test is manually run by a launch test +int main(int argc, char ** argv) { + rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "test_pose_2d_publisher"); - - ros::AsyncSpinner spinner(1); - spinner.start(); int ret = RUN_ALL_TESTS(); - spinner.stop(); - ros::shutdown(); + rclcpp::shutdown(); return ret; } diff --git a/fuse_publishers/test/test_stamped_variable_synchronizer.cpp b/fuse_publishers/test/test_stamped_variable_synchronizer.cpp index bdcc75603..bd2d75a53 100644 --- a/fuse_publishers/test/test_stamped_variable_synchronizer.cpp +++ b/fuse_publishers/test/test_stamped_variable_synchronizer.cpp @@ -75,13 +75,13 @@ TEST(StampedVariableSynchronizer, FullSearch) // Define the transaction and graph auto transaction = fuse_core::Transaction(); auto graph = fuse_graphs::HashGraph(); - graph.addVariable(fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(10, 0), generate("blank"))); - graph.addVariable(fuse_variables::Position2DStamped::make_shared(rclcpp::Time(10, 0), generate("blank"))); - graph.addVariable(fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(20, 0), generate("blank"))); - graph.addVariable(fuse_variables::Position2DStamped::make_shared(rclcpp::Time(20, 0), generate("blank"))); - graph.addVariable(fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(30, 0), generate("Dadblank"))); - graph.addVariable(fuse_variables::Position2DStamped::make_shared(rclcpp::Time(30, 0), generate("Dadblank"))); - graph.addVariable(fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(40, 0), generate("blank"))); + graph.addVariable(fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(10, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable(fuse_variables::Position2DStamped::make_shared(rclcpp::Time(10, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable(fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(20, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable(fuse_variables::Position2DStamped::make_shared(rclcpp::Time(20, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable(fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("Dadblank"))); + graph.addVariable(fuse_variables::Position2DStamped::make_shared(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("Dadblank"))); + graph.addVariable(fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(40, 0, RCL_ROS_TIME), generate("blank"))); // Use the synchronizer auto actual = sync.findLatestCommonStamp(transaction, graph); @@ -89,7 +89,7 @@ TEST(StampedVariableSynchronizer, FullSearch) // Expect Time(20, 0). // Time(30, 0) entries have a different device_id // Time(40, 0) entries don't have a whole set - EXPECT_EQ(rclcpp::Time(20, 0), actual); + EXPECT_EQ(rclcpp::Time(20, 0, RCL_ROS_TIME), actual); } TEST(StampedVariableSynchronizer, Update) @@ -102,27 +102,27 @@ TEST(StampedVariableSynchronizer, Update) // Define the first transaction and graph auto transaction1 = fuse_core::Transaction(); auto graph = fuse_graphs::HashGraph(); - graph.addVariable(fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(10, 0), generate("blank"))); - graph.addVariable(fuse_variables::Position2DStamped::make_shared(rclcpp::Time(10, 0), generate("blank"))); - graph.addVariable(fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(20, 0), generate("blank"))); - graph.addVariable(fuse_variables::Position2DStamped::make_shared(rclcpp::Time(20, 0), generate("blank"))); + graph.addVariable(fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(10, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable(fuse_variables::Position2DStamped::make_shared(rclcpp::Time(10, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable(fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(20, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable(fuse_variables::Position2DStamped::make_shared(rclcpp::Time(20, 0, RCL_ROS_TIME), generate("blank"))); // Use the synchronizer auto actual1 = sync.findLatestCommonStamp(transaction1, graph); - EXPECT_EQ(rclcpp::Time(20, 0), actual1); + EXPECT_EQ(rclcpp::Time(20, 0, RCL_ROS_TIME), actual1); // Create an incremental transaction update auto transaction2 = fuse_core::Transaction(); - transaction2.addVariable(fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(30, 0), generate("blank"))); - transaction2.addVariable(fuse_variables::Position2DStamped::make_shared(rclcpp::Time(30, 0), generate("blank"))); - transaction2.addVariable(fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(40, 0), generate("blank"))); - graph.addVariable(fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(30, 0), generate("blank"))); - graph.addVariable(fuse_variables::Position2DStamped::make_shared(rclcpp::Time(30, 0), generate("blank"))); - graph.addVariable(fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(40, 0), generate("blank"))); + transaction2.addVariable(fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("blank"))); + transaction2.addVariable(fuse_variables::Position2DStamped::make_shared(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("blank"))); + transaction2.addVariable(fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(40, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable(fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable(fuse_variables::Position2DStamped::make_shared(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable(fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(40, 0, RCL_ROS_TIME), generate("blank"))); // Use the synchronizer auto actual2 = sync.findLatestCommonStamp(transaction2, graph); - EXPECT_EQ(rclcpp::Time(30, 0), actual2); + EXPECT_EQ(rclcpp::Time(30, 0, RCL_ROS_TIME), actual2); } TEST(StampedVariableSynchronizer, Remove) @@ -134,29 +134,23 @@ TEST(StampedVariableSynchronizer, Remove) // Define the first transaction and graph auto transaction1 = fuse_core::Transaction(); auto graph = fuse_graphs::HashGraph(); - graph.addVariable(fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(10, 0), generate("blank"))); - graph.addVariable(fuse_variables::Position2DStamped::make_shared(rclcpp::Time(10, 0), generate("blank"))); - graph.addVariable(fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(20, 0), generate("blank"))); - graph.addVariable(fuse_variables::Position2DStamped::make_shared(rclcpp::Time(20, 0), generate("blank"))); - graph.addVariable(fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(30, 0), generate("blank"))); - graph.addVariable(fuse_variables::Position2DStamped::make_shared(rclcpp::Time(30, 0), generate("blank"))); + graph.addVariable(fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(10, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable(fuse_variables::Position2DStamped::make_shared(rclcpp::Time(10, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable(fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(20, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable(fuse_variables::Position2DStamped::make_shared(rclcpp::Time(20, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable(fuse_variables::Orientation2DStamped::make_shared(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("blank"))); + graph.addVariable(fuse_variables::Position2DStamped::make_shared(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("blank"))); // Use the synchronizer auto actual1 = sync.findLatestCommonStamp(transaction1, graph); - EXPECT_EQ(rclcpp::Time(30, 0), actual1); + EXPECT_EQ(rclcpp::Time(30, 0, RCL_ROS_TIME), actual1); // Create an incremental transaction that removes one of the latest variables auto transaction2 = fuse_core::Transaction(); - transaction2.removeVariable(fuse_variables::Position2DStamped(rclcpp::Time(30, 0), generate("blank")).uuid()); - graph.removeVariable(fuse_variables::Position2DStamped(rclcpp::Time(30, 0), generate("blank")).uuid()); + transaction2.removeVariable(fuse_variables::Position2DStamped(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("blank")).uuid()); + graph.removeVariable(fuse_variables::Position2DStamped(rclcpp::Time(30, 0, RCL_ROS_TIME), generate("blank")).uuid()); // Use the synchronizer auto actual2 = sync.findLatestCommonStamp(transaction2, graph); - EXPECT_EQ(rclcpp::Time(20, 0), actual2); -} - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + EXPECT_EQ(rclcpp::Time(20, 0, RCL_ROS_TIME), actual2); } diff --git a/fuse_tutorials/include/fuse_tutorials/beacon_publisher.h b/fuse_tutorials/include/fuse_tutorials/beacon_publisher.h index fa4d751c9..1493e8e50 100644 --- a/fuse_tutorials/include/fuse_tutorials/beacon_publisher.h +++ b/fuse_tutorials/include/fuse_tutorials/beacon_publisher.h @@ -111,7 +111,7 @@ class BeaconPublisher : public fuse_core::AsyncPublisher * the number of threads to use to spin the callback queue. Generally this will be 1, unless you have a good reason * to use a multi-threaded spinner. */ - BeaconPublisher() : fuse_core::AsyncPublisher(1) {} + BeaconPublisher() : fuse_core::AsyncPublisher(1) {} // TODO(methylDragon): Refactor this in the same way it was done in fuse_publishers /** * @brief Perform any required initialization for the publisher diff --git a/fuse_tutorials/src/beacon_publisher.cpp b/fuse_tutorials/src/beacon_publisher.cpp index f1099c53f..998642e8d 100644 --- a/fuse_tutorials/src/beacon_publisher.cpp +++ b/fuse_tutorials/src/beacon_publisher.cpp @@ -56,7 +56,7 @@ void BeaconPublisher::onInit() private_node_handle_.param("map_frame_id", map_frame_id_, std::string("map")); // Advertise the output topics - beacon_publisher_ = private_node_handle_.advertise("beacons", 1); + beacon_publisher_ = node_->create_publisher("beacons", 1); } void BeaconPublisher::notifyCallback( diff --git a/fuse_variables/package.xml b/fuse_variables/package.xml index 42c98fc5b..47e0fa40f 100644 --- a/fuse_variables/package.xml +++ b/fuse_variables/package.xml @@ -29,7 +29,6 @@ ament_cmake_pytest ament_lint_auto ament_lint_common - launch_pytest rclcpp diff --git a/fuse_variables/test/CMakeLists.txt b/fuse_variables/test/CMakeLists.txt index 57b9df7b7..3b425e309 100644 --- a/fuse_variables/test/CMakeLists.txt +++ b/fuse_variables/test/CMakeLists.txt @@ -13,6 +13,7 @@ set(TEST_TARGETS test_point_3d_landmark test_position_2d_stamped test_position_3d_stamped + test_load_device_id test_velocity_angular_2d_stamped test_velocity_angular_3d_stamped test_velocity_linear_2d_stamped @@ -23,20 +24,3 @@ foreach(test_name ${TEST_TARGETS}) ament_add_gtest("${test_name}" "${test_name}.cpp") target_link_libraries("${test_name}" ${PROJECT_NAME}) endforeach() - - -# ROS TESTS (WITH LAUNCH) ========================================================================== -find_package(ament_cmake_pytest REQUIRED) - -ament_add_gtest_executable(test_load_device_id launch_tests/test_load_device_id.cpp) -target_link_libraries(test_load_device_id ${PROJECT_NAME}) - -ament_add_pytest_test(test_load_device_id_py "launch_tests/test_load_device_id.py" - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" -) - -configure_file( - "launch_tests/test_load_device_id.yaml" - "launch_tests/test_load_device_id.yaml" - COPYONLY -) diff --git a/fuse_variables/test/launch_tests/test_load_device_id.py b/fuse_variables/test/launch_tests/test_load_device_id.py deleted file mode 100755 index 3d5498c69..000000000 --- a/fuse_variables/test/launch_tests/test_load_device_id.py +++ /dev/null @@ -1,48 +0,0 @@ -#! /usr/bin/env python3 - -# Copyright 2022 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. - -import os - -from launch import LaunchDescription -from launch.actions import ExecuteProcess - -import launch_pytest -from launch_pytest.actions import ReadyToTest -from launch_pytest.tools import process as process_tools - -import pytest - - -@pytest.fixture -def test_proc(): - test_root = '.' - - param_path = os.path.join(test_root, 'launch_tests', 'test_load_device_id.yaml') - test_path = os.path.join(test_root, 'test_load_device_id') - - cmd = [test_path, '--ros-args', '--params-file', param_path] - return ExecuteProcess(cmd=cmd, shell=True, output='screen', cached_output=True) - - -@launch_pytest.fixture -def generate_test_description(test_proc): - return LaunchDescription([test_proc, ReadyToTest()]) - - -@pytest.mark.launch(fixture=generate_test_description) -async def test_no_failed_gtests(test_proc, launch_context): - await process_tools.wait_for_exit(launch_context, test_proc, timeout=10) - assert test_proc.return_code == 0, "GTests failed" diff --git a/fuse_variables/test/launch_tests/test_load_device_id.yaml b/fuse_variables/test/launch_tests/test_load_device_id.yaml deleted file mode 100644 index f1b683114..000000000 --- a/fuse_variables/test/launch_tests/test_load_device_id.yaml +++ /dev/null @@ -1,35 +0,0 @@ -id1_node: - ros__parameters: - device_id: "01234567-89AB-CDEF-0123-456789ABCDEF" - -id2_node: - ros__parameters: - device_id: "01234567-89ab-cdef-0123-456789abcdef" - -id3_node: - ros__parameters: - device_id: "0123456789ABCDEF0123456789ABCDEF" - -id4_node: - ros__parameters: - device_id: "0123456789abcdef0123456789abcdef" - -id5_node: - ros__parameters: - device_id: "{01234567-89ab-cdef-0123-456789abcdef}" - -id6_node: - ros__parameters: - device_id: "{01234567-89AB-CDEF-0123-456789ABCDEF}" - -id7_node: - ros__parameters: - device_id: "{THIS IS NOT VALID!!! 123456789ABCDEF}" - -name_node: - ros__parameters: - device_name: "Test" - -none_node: - ros__parameters: - some_other_parameter: 1 diff --git a/fuse_variables/test/launch_tests/test_load_device_id.cpp b/fuse_variables/test/test_load_device_id.cpp similarity index 87% rename from fuse_variables/test/launch_tests/test_load_device_id.cpp rename to fuse_variables/test/test_load_device_id.cpp index a895c4b37..41d715a8c 100644 --- a/fuse_variables/test/launch_tests/test_load_device_id.cpp +++ b/fuse_variables/test/test_load_device_id.cpp @@ -71,6 +71,7 @@ TEST_F(TestLoadDeviceId, LoadDeviceId) // Test loading a device id from each test namespace { auto node = rclcpp::Node::make_shared("id1_node"); + node->declare_parameter("device_id", std::string("01234567-89AB-CDEF-0123-456789ABCDEF")); fuse_core::UUID actual = fuse_variables::loadDeviceId(node); fuse_core::UUID expected = { @@ -84,6 +85,7 @@ TEST_F(TestLoadDeviceId, LoadDeviceId) } { auto node = rclcpp::Node::make_shared("id2_node"); + node->declare_parameter("device_id", std::string("01234567-89ab-cdef-0123-456789abcdef")); fuse_core::UUID actual = fuse_variables::loadDeviceId(node); fuse_core::UUID expected = { @@ -97,6 +99,7 @@ TEST_F(TestLoadDeviceId, LoadDeviceId) } { auto node = rclcpp::Node::make_shared("id3_node"); + node->declare_parameter("device_id", std::string("0123456789ABCDEF0123456789ABCDEF")); fuse_core::UUID actual = fuse_variables::loadDeviceId(node); fuse_core::UUID expected = { @@ -110,6 +113,7 @@ TEST_F(TestLoadDeviceId, LoadDeviceId) } { auto node = rclcpp::Node::make_shared("id4_node"); + node->declare_parameter("device_id", std::string("0123456789abcdef0123456789abcdef")); fuse_core::UUID actual = fuse_variables::loadDeviceId(node); fuse_core::UUID expected = { @@ -123,6 +127,7 @@ TEST_F(TestLoadDeviceId, LoadDeviceId) } { auto node = rclcpp::Node::make_shared("id5_node"); + node->declare_parameter("device_id", std::string("{01234567-89ab-cdef-0123-456789abcdef}")); fuse_core::UUID actual = fuse_variables::loadDeviceId(node); fuse_core::UUID expected = { @@ -136,6 +141,7 @@ TEST_F(TestLoadDeviceId, LoadDeviceId) } { auto node = rclcpp::Node::make_shared("id6_node"); + node->declare_parameter("device_id", std::string("{01234567-89AB-CDEF-0123-456789ABCDEF}")); fuse_core::UUID actual = fuse_variables::loadDeviceId(node); fuse_core::UUID expected = { @@ -149,10 +155,12 @@ TEST_F(TestLoadDeviceId, LoadDeviceId) } { auto node = rclcpp::Node::make_shared("id7_node"); + node->declare_parameter("device_id", std::string("{THIS IS NOT VALID!!! 123456789ABCDEF}")); EXPECT_THROW(fuse_variables::loadDeviceId(node), std::runtime_error); } { auto node = rclcpp::Node::make_shared("name_node"); + node->declare_parameter("device_name", std::string("Test")); fuse_core::UUID actual = fuse_variables::loadDeviceId(node); fuse_core::UUID expected = { @@ -166,6 +174,7 @@ TEST_F(TestLoadDeviceId, LoadDeviceId) } { auto node = rclcpp::Node::make_shared("none_node"); + node->declare_parameter("some_other_parameter", 1); fuse_core::UUID actual = fuse_variables::loadDeviceId(node); fuse_core::UUID expected = fuse_core::uuid::NIL; EXPECT_EQ(expected, actual);