diff --git a/nav2_util/include/nav2_util/base_footprint_publisher.hpp b/nav2_util/include/nav2_util/base_footprint_publisher.hpp deleted file mode 100644 index 52bcdb53eb..0000000000 --- a/nav2_util/include/nav2_util/base_footprint_publisher.hpp +++ /dev/null @@ -1,129 +0,0 @@ -// 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 e378eac514..a639a0e59e 100644 --- a/nav2_util/src/CMakeLists.txt +++ b/nav2_util/src/CMakeLists.txt @@ -28,11 +28,6 @@ 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 deleted file mode 100644 index f3b6791db4..0000000000 --- a/nav2_util/src/base_footprint_publisher.cpp +++ /dev/null @@ -1,27 +0,0 @@ -// 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 d68291f619..9f1ae99bbc 100644 --- a/nav2_util/test/CMakeLists.txt +++ b/nav2_util/test/CMakeLists.txt @@ -41,7 +41,3 @@ target_link_libraries(test_odometry_utils ${library_name}) 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}) diff --git a/nav2_util/test/test_base_footprint_publisher.cpp b/nav2_util/test/test_base_footprint_publisher.cpp deleted file mode 100644 index 47dc83c7f3..0000000000 --- a/nav2_util/test/test_base_footprint_publisher.cpp +++ /dev/null @@ -1,72 +0,0 @@ -// 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); -}