diff --git a/nav2_util/include/nav2_util/base_footprint_publisher.hpp b/nav2_util/include/nav2_util/base_footprint_publisher.hpp new file mode 100644 index 0000000000..52bcdb53eb --- /dev/null +++ b/nav2_util/include/nav2_util/base_footprint_publisher.hpp @@ -0,0 +1,129 @@ +// Copyright (c) 2023 Open Navigation LLC +// +// 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 NAV2_UTIL__BASE_FOOTPRINT_PUBLISHER_HPP_ +#define NAV2_UTIL__BASE_FOOTPRINT_PUBLISHER_HPP_ + +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "tf2_msgs/msg/tf_message.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "tf2_ros/create_timer_ros.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/buffer.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/utils.h" + +namespace nav2_util +{ + +/** + * @brief A TF2 listener that overrides the subscription callback + * to inject base footprint publisher removing Z, Pitch, and Roll for + * 3D state estimation but desiring a 2D frame for navigation, visualization, or other reasons + */ +class BaseFootprintPublisherListener : public tf2_ros::TransformListener +{ +public: + BaseFootprintPublisherListener(tf2::BufferCore & buffer, bool spin_thread, rclcpp::Node & node) + : tf2_ros::TransformListener(buffer, spin_thread) + { + node.declare_parameter( + "base_link_frame", rclcpp::ParameterValue(std::string("base_link"))); + node.declare_parameter( + "base_footprint_frame", rclcpp::ParameterValue(std::string("base_footprint"))); + base_link_frame_ = node.get_parameter("base_link_frame").as_string(); + base_footprint_frame_ = node.get_parameter("base_footprint_frame").as_string(); + tf_broadcaster_ = std::make_shared(node); + } + + /** + * @brief Overrides TF2 subscription callback to inject base footprint publisher + */ + void subscription_callback(tf2_msgs::msg::TFMessage::ConstSharedPtr msg, bool is_static) override + { + TransformListener::subscription_callback(msg, is_static); + + if (is_static) { + return; + } + + for (unsigned int i = 0; i != msg->transforms.size(); i++) { + auto & t = msg->transforms[i]; + if (t.child_frame_id == base_link_frame_) { + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = t.header.stamp; + transform.header.frame_id = base_link_frame_; + transform.child_frame_id = base_footprint_frame_; + + // Project to Z-zero + transform.transform.translation = t.transform.translation; + transform.transform.translation.z = 0.0; + + // Remove Roll and Pitch + tf2::Quaternion q; + q.setRPY(0, 0, tf2::getYaw(t.transform.rotation)); + q.normalize(); + transform.transform.rotation.x = q.x(); + transform.transform.rotation.y = q.y(); + transform.transform.rotation.z = q.z(); + transform.transform.rotation.w = q.w(); + + tf_broadcaster_->sendTransform(transform); + return; + } + } + } + +protected: + std::shared_ptr tf_broadcaster_; + std::string base_link_frame_, base_footprint_frame_; +}; + +/** + * @class nav2_util::BaseFootprintPublisher + * @brief Republishes the ``base_link`` frame as ``base_footprint`` + * stripping away the Z, Roll, and Pitch of the full 3D state to provide + * a 2D projection for navigation when state estimation is full 3D + */ +class BaseFootprintPublisher : public rclcpp::Node +{ +public: + /** + * @brief A constructor + */ + explicit BaseFootprintPublisher(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : Node("base_footprint_publisher", options) + { + RCLCPP_INFO(get_logger(), "Creating base footprint publisher"); + tf_buffer_ = std::make_shared(get_clock()); + auto timer_interface = std::make_shared( + get_node_base_interface(), + get_node_timers_interface()); + tf_buffer_->setCreateTimerInterface(timer_interface); + listener_publisher_ = std::make_shared( + *tf_buffer_, true, *this); + } + +protected: + std::shared_ptr tf_buffer_; + std::shared_ptr listener_publisher_; +}; + +} // end namespace nav2_util + +#endif // NAV2_UTIL__BASE_FOOTPRINT_PUBLISHER_HPP_ diff --git a/nav2_util/src/CMakeLists.txt b/nav2_util/src/CMakeLists.txt index 104966e219..6ddcdc6d32 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/nav2_util/src/CMakeLists.txt @@ -29,6 +29,11 @@ add_executable(lifecycle_bringup ) target_link_libraries(lifecycle_bringup ${library_name}) +add_executable(base_footprint_publisher + base_footprint_publisher.cpp +) +target_link_libraries(base_footprint_publisher ${library_name}) + find_package(Boost REQUIRED COMPONENTS program_options) install(TARGETS diff --git a/nav2_util/src/base_footprint_publisher.cpp b/nav2_util/src/base_footprint_publisher.cpp new file mode 100644 index 0000000000..f3b6791db4 --- /dev/null +++ b/nav2_util/src/base_footprint_publisher.cpp @@ -0,0 +1,27 @@ +// Copyright (c) 2023 Open Navigation LLC +// +// 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 + +#include "nav2_util/base_footprint_publisher.hpp" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); + + return 0; +} diff --git a/nav2_util/test/CMakeLists.txt b/nav2_util/test/CMakeLists.txt index fb4b6c107f..14e1a361d3 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -42,6 +42,10 @@ ament_add_gtest(test_robot_utils test_robot_utils.cpp) ament_target_dependencies(test_robot_utils geometry_msgs) target_link_libraries(test_robot_utils ${library_name}) +ament_add_gtest(test_base_footprint_publisher test_base_footprint_publisher.cpp) +ament_target_dependencies(test_base_footprint_publisher geometry_msgs) +target_link_libraries(test_base_footprint_publisher ${library_name}) + ament_add_gtest(test_array_parser test_array_parser.cpp) target_link_libraries(test_array_parser ${library_name}) diff --git a/nav2_util/test/test_base_footprint_publisher.cpp b/nav2_util/test/test_base_footprint_publisher.cpp new file mode 100644 index 0000000000..47dc83c7f3 --- /dev/null +++ b/nav2_util/test/test_base_footprint_publisher.cpp @@ -0,0 +1,72 @@ +// Copyright (c) 2023 Open Navigation LLC +// +// 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 +#include + +#include "nav2_util/base_footprint_publisher.hpp" +#include "gtest/gtest.h" +#include "tf2/exceptions.h" + +class RclCppFixture +{ +public: + RclCppFixture() {rclcpp::init(0, nullptr);} + ~RclCppFixture() {rclcpp::shutdown();} +}; +RclCppFixture g_rclcppfixture; + +TEST(TestBaseFootprintPublisher, TestBaseFootprintPublisher) +{ + auto node = std::make_shared(); + rclcpp::spin_some(node->get_node_base_interface()); + + auto tf_broadcaster = std::make_shared(node); + auto buffer = std::make_shared(node->get_clock()); + auto timer_interface = std::make_shared( + node->get_node_base_interface(), + node->get_node_timers_interface()); + buffer->setCreateTimerInterface(timer_interface); + auto listener = std::make_shared(*buffer, true); + + std::string base_link = "base_link"; + std::string base_footprint = "base_footprint"; + + // Publish something to TF, should fail, doesn't exist + geometry_msgs::msg::TransformStamped transform; + transform.header.stamp = node->now(); + transform.header.frame_id = "test1_1"; + transform.child_frame_id = "test1"; + tf_broadcaster->sendTransform(transform); + rclcpp::spin_some(node->get_node_base_interface()); + EXPECT_THROW( + buffer->lookupTransform(base_link, base_footprint, tf2::TimePointZero), + tf2::TransformException); + + // This is valid, should work now and communicate the Z-removed info + transform.header.stamp = node->now(); + transform.header.frame_id = "odom"; + transform.child_frame_id = "base_link"; + transform.transform.translation.x = 1.0; + transform.transform.translation.y = 1.0; + transform.transform.translation.z = 1.0; + tf_broadcaster->sendTransform(transform); + rclcpp::Rate r(1.0); + r.sleep(); + rclcpp::spin_some(node->get_node_base_interface()); + auto t = buffer->lookupTransform(base_link, base_footprint, tf2::TimePointZero); + EXPECT_EQ(t.transform.translation.x, 1.0); + EXPECT_EQ(t.transform.translation.y, 1.0); + EXPECT_EQ(t.transform.translation.z, 0.0); +}