├── .gitignore ├── dependencies.repos ├── test ├── CMakeLists.txt └── test_base_footprint.cpp ├── .github ├── dependabot.yml ├── ISSUE_TEMPLATE │ ├── 2_feature_request.md │ ├── 3_need_help.md │ └── 1_bug_report.md └── workflows │ ├── build_and_test_jazzy.yaml │ ├── build_and_test_humble.yaml │ └── build_and_test_rolling.yaml ├── CONTRIBUTING.md ├── launch └── base_footprint.launch ├── package.xml ├── CMakeLists.txt ├── README.md ├── include └── humanoid_base_footprint │ └── base_footprint.hpp ├── src └── base_footprint.cpp └── LICENSE /.gitignore: -------------------------------------------------------------------------------- 1 | # auto-generated documentation 2 | **/docs/_build 3 | **/docs/_out 4 | **/docs/cppapi 5 | **/docs/pyapi 6 | -------------------------------------------------------------------------------- /dependencies.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | biped_interfaces: 3 | type: git 4 | url: https://github.com/ros-sports/biped_interfaces.git 5 | version: rolling 6 | -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Build test_base_footprint 2 | ament_add_gtest(test_base_footprint 3 | test_base_footprint.cpp) 4 | 5 | find_package(sensor_msgs REQUIRED) 6 | 7 | ament_target_dependencies(test_base_footprint 8 | sensor_msgs 9 | ) 10 | 11 | target_link_libraries(test_base_footprint 12 | base_footprint_node 13 | ) 14 | -------------------------------------------------------------------------------- /.github/dependabot.yml: -------------------------------------------------------------------------------- 1 | # Set update schedule for GitHub Actions 2 | # (https://docs.github.com/en/code-security/dependabot/working-with-dependabot/keeping-your-actions-up-to-date-with-dependabot) 3 | 4 | version: 2 5 | updates: 6 | 7 | - package-ecosystem: "github-actions" 8 | directory: "/" 9 | schedule: 10 | # Check for updates to GitHub Actions every week 11 | interval: "weekly" 12 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | Any contribution that you make to this repository will 2 | be under the Apache 2 License, as dictated by that 3 | [license](http://www.apache.org/licenses/LICENSE-2.0.html): 4 | 5 | ~~~ 6 | 5. Submission of Contributions. Unless You explicitly state otherwise, 7 | any Contribution intentionally submitted for inclusion in the Work 8 | by You to the Licensor shall be under the terms and conditions of 9 | this License, without any additional terms or conditions. 10 | Notwithstanding the above, nothing herein shall supersede or modify 11 | the terms of any separate license agreement you may have executed 12 | with Licensor regarding such Contributions. 13 | ~~~ 14 | -------------------------------------------------------------------------------- /launch/base_footprint.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/2_feature_request.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: "\U0001F680 Feature request" 3 | about: Suggest an idea for this project 4 | labels: 'enhancement' 5 | --- 6 | 7 | 12 | 13 | ## Is your feature request related to a problem? Please describe. 14 | 15 | 16 | ## Describe the solution you'd like 17 | 18 | 19 | ## Describe alternatives you've considered 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/3_need_help.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: "⁉️ Need help?" 3 | about: "Get help with using or improving our software" 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | --- 8 | 9 | 10 | 11 | ## What I'm trying to do 12 | 13 | 14 | ## What I've tried 15 | 16 | 17 | ## Additional context 18 | 19 | 20 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/1_bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: "\U0001F41B Bug report" 3 | about: Report a misbehavior or other bug 4 | title: '' 5 | labels: 'bug' 6 | assignees: '' 7 | --- 8 | 9 | 10 | ## Expected behavior 11 | 12 | 13 | ## Current behavior 14 | 15 | 16 | ## Steps to Reproduce 17 | 18 | 19 | 1. 20 | 2. 21 | 3. 22 | ... 23 | 24 | ## Context (Environment) 25 | 26 | 27 | - [ ] RViz 28 | - [ ] Simulator 29 | - [ ] Robot 30 | - [ ] Local 31 | 32 | ## Possible Solution 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | humanoid_base_footprint 4 | 1.2.0 5 | 6 | Publishes a transform to the base footprint of the robot, based on the orientation and position of the feet. 7 | 8 | Jasper Güldenstein 9 | Hamburg Bit-Bots 10 | 11 | Jasper Güldenstein 12 | 13 | Apache License 2.0 14 | 15 | ament_cmake 16 | 17 | tf2_eigen 18 | tf2_geometry_msgs 19 | tf2_ros 20 | rclcpp_components 21 | eigen 22 | rot_conv 23 | biped_interfaces 24 | 25 | ament_lint_auto 26 | ament_lint_common 27 | sensor_msgs 28 | 29 | 30 | ament_cmake 31 | 32 | 33 | -------------------------------------------------------------------------------- /.github/workflows/build_and_test_jazzy.yaml: -------------------------------------------------------------------------------- 1 | # This is a basic workflow to help you get started with Actions 2 | 3 | name: Build and Test (jazzy) 4 | 5 | # Controls when the action will run. 6 | on: 7 | # Triggers the workflow on push 8 | push: 9 | branches: [ master ] 10 | 11 | # Triggers the workflow on pull requests 12 | pull_request: 13 | branches: [ master ] 14 | 15 | # Allows you to run this workflow manually from the Actions tab 16 | workflow_dispatch: 17 | 18 | # A workflow run is made up of one or more jobs that can run sequentially or in parallel 19 | jobs: 20 | # This workflow contains a single job called "build" 21 | build: 22 | # The type of runner that the job will run on 23 | runs-on: ubuntu-latest 24 | container: 25 | image: ubuntu:noble 26 | 27 | # Steps represent a sequence of tasks that will be executed as part of the job 28 | steps: 29 | - uses: actions/checkout@v4 30 | - uses: ros-tooling/setup-ros@v0.7 31 | with: 32 | use-ros2-testing: true 33 | - uses: ros-tooling/action-ros-ci@v0.4 34 | with: 35 | target-ros2-distro: jazzy 36 | vcs-repo-file-url: dependencies.repos 37 | -------------------------------------------------------------------------------- /.github/workflows/build_and_test_humble.yaml: -------------------------------------------------------------------------------- 1 | # This is a basic workflow to help you get started with Actions 2 | 3 | name: Build and Test (humble) 4 | 5 | # Controls when the action will run. 6 | on: 7 | # Triggers the workflow on push 8 | push: 9 | branches: [ master ] 10 | 11 | # Triggers the workflow on pull requests 12 | pull_request: 13 | branches: [ master ] 14 | 15 | # Allows you to run this workflow manually from the Actions tab 16 | workflow_dispatch: 17 | 18 | # A workflow run is made up of one or more jobs that can run sequentially or in parallel 19 | jobs: 20 | # This workflow contains a single job called "build" 21 | build: 22 | # The type of runner that the job will run on 23 | runs-on: ubuntu-latest 24 | container: 25 | image: ubuntu:jammy 26 | 27 | # Steps represent a sequence of tasks that will be executed as part of the job 28 | steps: 29 | - uses: actions/checkout@v4 30 | - uses: ros-tooling/setup-ros@v0.7 31 | with: 32 | use-ros2-testing: true 33 | - uses: ros-tooling/action-ros-ci@v0.4 34 | with: 35 | target-ros2-distro: humble 36 | vcs-repo-file-url: dependencies.repos 37 | -------------------------------------------------------------------------------- /.github/workflows/build_and_test_rolling.yaml: -------------------------------------------------------------------------------- 1 | # This is a basic workflow to help you get started with Actions 2 | 3 | name: Build and Test (rolling) 4 | 5 | # Controls when the action will run. 6 | on: 7 | # Triggers the workflow on push 8 | push: 9 | branches: [ master ] 10 | 11 | # Triggers the workflow on pull requests 12 | pull_request: 13 | branches: [ master ] 14 | 15 | # Allows you to run this workflow manually from the Actions tab 16 | workflow_dispatch: 17 | 18 | # A workflow run is made up of one or more jobs that can run sequentially or in parallel 19 | jobs: 20 | # This workflow contains a single job called "build" 21 | build: 22 | # The type of runner that the job will run on 23 | runs-on: ubuntu-latest 24 | container: 25 | image: ubuntu:noble 26 | 27 | # Steps represent a sequence of tasks that will be executed as part of the job 28 | steps: 29 | - uses: actions/checkout@v4 30 | - uses: ros-tooling/setup-ros@v0.7 31 | with: 32 | use-ros2-testing: true 33 | - uses: ros-tooling/action-ros-ci@v0.4 34 | with: 35 | target-ros2-distro: rolling 36 | vcs-repo-file-url: dependencies.repos 37 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(humanoid_base_footprint) 3 | 4 | # Add support for C++17 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 17) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | # Find dependencies 14 | find_package(biped_interfaces REQUIRED) 15 | find_package(Eigen3 REQUIRED) 16 | find_package(rclcpp REQUIRED) 17 | find_package(rclcpp_components REQUIRED) 18 | find_package(rot_conv REQUIRED) 19 | find_package(tf2_eigen REQUIRED) 20 | find_package(tf2_geometry_msgs REQUIRED) 21 | find_package(tf2_ros REQUIRED) 22 | 23 | set(THIS_PACKAGE_INCLUDE_DEPENDS 24 | biped_interfaces 25 | Eigen3 26 | rclcpp 27 | rclcpp_components 28 | rot_conv 29 | tf2_eigen 30 | tf2_geometry_msgs 31 | tf2_ros) 32 | 33 | # Build 34 | add_library(base_footprint_node SHARED 35 | src/base_footprint.cpp) 36 | 37 | target_include_directories(base_footprint_node PUBLIC 38 | $ 39 | $) 40 | 41 | rclcpp_components_register_node(base_footprint_node 42 | PLUGIN "humanoid_base_footprint::BaseFootprintBroadcaster" 43 | EXECUTABLE base_footprint) 44 | 45 | ament_target_dependencies(base_footprint_node ${THIS_PACKAGE_INCLUDE_DEPENDS}) 46 | 47 | ament_export_targets(export_base_footprint HAS_LIBRARY_TARGET) 48 | ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) 49 | 50 | # Install 51 | install( 52 | DIRECTORY include/ 53 | DESTINATION include) 54 | install( 55 | TARGETS base_footprint_node 56 | EXPORT export_base_footprint 57 | ARCHIVE DESTINATION lib 58 | LIBRARY DESTINATION lib 59 | RUNTIME DESTINATION bin 60 | INCLUDES DESTINATION include) 61 | install(DIRECTORY launch 62 | DESTINATION share/${PROJECT_NAME}) 63 | 64 | ## Run tests 65 | if(BUILD_TESTING) 66 | find_package(ament_cmake_gtest REQUIRED) 67 | add_subdirectory(test) 68 | 69 | find_package(ament_lint_auto REQUIRED) 70 | ament_lint_auto_find_test_dependencies() 71 | endif() 72 | 73 | # Package 74 | ament_package() 75 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # humanoid_base_footprint 2 | 3 | [![Build and Test (humble)](../../actions/workflows/build_and_test_humble.yaml/badge.svg?branch=master)](../../actions/workflows/build_and_test_humble.yaml?query=branch:master) 4 | [![Build and Test (jazzy)](../../actions/workflows/build_and_test_jazzy.yaml/badge.svg?branch=master)](../../actions/workflows/build_and_test_jazzy.yaml?query=branch:master) 5 | [![Build and Test (rolling)](../../actions/workflows/build_and_test_rolling.yaml/badge.svg?branch=master)](../../actions/workflows/build_and_test_rolling.yaml?query=branch:master) 6 | 7 | This ROS2 package includes a node which provides the base footprint frame for any humanoid robot following [REP - 120](https://www.ros.org/reps/rep-0120.html). 8 | 9 | Only **ROS2 Rolling** is supported currently. 10 | 11 | ## Installation 12 | 13 | Only source installation is available currently. Run the following in your ROS workspace: 14 | 15 | ``` 16 | git clone https://github.com/ros-sports/humanoid_base_footprint.git src/humanoid_base_footprint 17 | vcs import src < src/humanoid_base_footprint/dependencies.repos --recursive 18 | colcon build 19 | ``` 20 | 21 | ## Usage 22 | 23 | To run the node: 24 | 25 | ``` 26 | ros2 run humanoid_base_footprint base_footprint 27 | ``` 28 | 29 | ## Description 30 | 31 | Definition of the base footprint frame (from [here](https://www.ros.org/reps/rep-0120.html)): 32 | 33 | >The base_footprint is the representation of the robot position on the floor. The floor is usually the level where the supporting leg rests, i.e. z = min(l_sole_z, r_sole_z) where l_sole_z and r_sole_z are the left and right sole height respecitvely. The translation component of the frame should be the barycenter of the feet projections on the floor. With respect to the odom frame, the roll and pitch angles should be zero and the yaw angle should correspond to the base_link yaw angle. 34 | > 35 | >Rationale: base_footprint provides a fairly stable 2D planar representation of the humanoid even while walking and swaying with the base_link. 36 | 37 | The node listens to [Phase](https://github.com/ros-sports/biped_interfaces/blob/rolling/msg/Phase.msg) msgs that define the current stance of the robot. By default, it will listen on the `walk_support_state` topic, but you can define a list of topics on the `support_state_topics` parameter. 38 | 39 | If the links of your robot have non-standard names you can set the following parameters: 40 | `base_link_frame`, `r_sole_frame`, `l_sole_frame`, `l_sole_frame`, `odom_frame` 41 | 42 | 43 | The resulting frame will be published as `base_footprint` but the name can be changed by setting the `base_footprint_frame` parameter. 44 | -------------------------------------------------------------------------------- /include/humanoid_base_footprint/base_footprint.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Hamburg Bit-Bots 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef HUMANOID_BASE_FOOTPRINT__BASE_FOOTPRINT_HPP_ 16 | #define HUMANOID_BASE_FOOTPRINT__BASE_FOOTPRINT_HPP_ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | // taken from REP120 28 | // 29 | // base_footprint 30 | // 31 | // The base_footprint is the representation of the robot position on the floor. 32 | // The floor is usually the level where the supporting leg rests, 33 | // i.e. z = min(l_sole_z, r_sole_z) where l_sole_z and r_sole_z are the left and right sole height 34 | // respecitvely. 35 | // The translation component of the frame should be the barycenter of the feet projections on the 36 | // floor. 37 | // With respect to the odom frame, the roll and pitch angles should be zero and the yaw angle should 38 | // correspond to the base_link yaw angle. 39 | // 40 | // Rationale: base_footprint provides a fairly stable 2D planar representation of the humanoid even 41 | // while walking and swaying with the base_link. 42 | 43 | namespace humanoid_base_footprint 44 | { 45 | 46 | class BaseFootprintBroadcaster : public rclcpp::Node 47 | { 48 | public: 49 | explicit BaseFootprintBroadcaster(const rclcpp::NodeOptions & options = rclcpp::NodeOptions{}); 50 | 51 | void loop(); 52 | 53 | private: 54 | geometry_msgs::msg::TransformStamped tf_; 55 | std::unique_ptr tfBuffer_; 56 | std::shared_ptr tfListener_; 57 | bool is_left_support_, got_support_foot_; 58 | std::string base_link_frame_, base_footprint_frame_, r_sole_frame_, l_sole_frame_, odom_frame_; 59 | void supportFootCallback(const biped_interfaces::msg::Phase msg); 60 | rclcpp::Subscription::SharedPtr walking_support_foot_subscriber_; 61 | rclcpp::Subscription::SharedPtr 62 | dynamic_kick_support_foot_subscriber_; 63 | 64 | std::unique_ptr tfBroadcaster_; 65 | rclcpp::TimerBase::SharedPtr timer_; 66 | void timerCallback(); 67 | }; 68 | 69 | } // namespace humanoid_base_footprint 70 | 71 | #endif // HUMANOID_BASE_FOOTPRINT__BASE_FOOTPRINT_HPP_ 72 | -------------------------------------------------------------------------------- /test/test_base_footprint.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2021 Kenji Brameld 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include "humanoid_base_footprint/base_footprint.hpp" 17 | #include "sensor_msgs/msg/joint_state.hpp" 18 | #include "tf2_ros/transform_listener.h" 19 | #include "tf2_ros/buffer.h" 20 | 21 | class TestBaseFootprint : public ::testing::Test 22 | { 23 | protected: 24 | static void SetUpTestCase() 25 | { 26 | rclcpp::init(0, nullptr); 27 | } 28 | 29 | static void TearDownTestCase() 30 | { 31 | rclcpp::shutdown(); 32 | } 33 | }; 34 | 35 | TEST_F(TestBaseFootprint, timestamp_matching) 36 | { 37 | auto footprint_node = std::make_shared(); 38 | auto node = std::make_shared("test_node"); 39 | 40 | tf2_ros::TransformBroadcaster br(node); 41 | 42 | builtin_interfaces::msg::Time stamp(rclcpp::Time(1000000000, 000000001)); 43 | 44 | geometry_msgs::msg::TransformStamped tf; 45 | 46 | tf.header.stamp = stamp; 47 | tf.header.frame_id = "odom"; 48 | tf.child_frame_id = "base_link"; 49 | br.sendTransform(tf); 50 | 51 | tf.header.stamp = stamp; 52 | tf.header.frame_id = "base_link"; 53 | tf.child_frame_id = "l_sole"; 54 | br.sendTransform(tf); 55 | 56 | tf.header.stamp = stamp; 57 | tf.header.frame_id = "base_link"; 58 | tf.child_frame_id = "r_sole"; 59 | br.sendTransform(tf); 60 | 61 | tf2_ros::Buffer tfBuffer{node->get_clock()}; 62 | tf2_ros::TransformListener tfListener{tfBuffer, node}; 63 | 64 | rclcpp::sleep_for(std::chrono::milliseconds(50)); // Wait for timer callback to be called 65 | rclcpp::spin_some(footprint_node); 66 | rclcpp::sleep_for(std::chrono::milliseconds(5)); // Wait for transforms to arrive into buffer 67 | 68 | ASSERT_NO_THROW( 69 | { 70 | auto tf = tfBuffer.lookupTransform("base_link", "base_footprint", stamp); 71 | EXPECT_EQ(tf.header.stamp, stamp); 72 | }); 73 | } 74 | 75 | TEST_F(TestBaseFootprint, timestamp_non_matching) 76 | { 77 | auto footprint_node = std::make_shared(); 78 | auto node = std::make_shared("test_node"); 79 | 80 | tf2_ros::TransformBroadcaster br(node); 81 | 82 | builtin_interfaces::msg::Time stamp(rclcpp::Time(1000000000, 000000001)); 83 | 84 | geometry_msgs::msg::TransformStamped tf; 85 | 86 | tf.header.frame_id = "odom"; 87 | tf.child_frame_id = "base_link"; 88 | tf.header.stamp = rclcpp::Time(1000000000, 000000001); 89 | br.sendTransform(tf); 90 | tf.header.stamp = rclcpp::Time(1000000001, 000000001); 91 | br.sendTransform(tf); 92 | 93 | tf.header.frame_id = "base_link"; 94 | tf.child_frame_id = "l_sole"; 95 | tf.header.stamp = rclcpp::Time(1000000000, 000000001); 96 | br.sendTransform(tf); 97 | tf.header.stamp = rclcpp::Time(1000000002, 000000001); 98 | br.sendTransform(tf); 99 | 100 | tf.header.frame_id = "base_link"; 101 | tf.child_frame_id = "r_sole"; 102 | tf.header.stamp = rclcpp::Time(1000000000, 000000001); 103 | br.sendTransform(tf); 104 | tf.header.stamp = rclcpp::Time(1000000003, 000000001); 105 | br.sendTransform(tf); 106 | 107 | tf2_ros::Buffer tfBuffer{node->get_clock()}; 108 | tf2_ros::TransformListener tfListener{tfBuffer, node}; 109 | 110 | rclcpp::sleep_for(std::chrono::milliseconds(50)); // Wait for timer callback to be called 111 | rclcpp::spin_some(footprint_node); 112 | rclcpp::sleep_for(std::chrono::milliseconds(5)); // Wait for transforms to arrive into buffer 113 | 114 | ASSERT_NO_THROW( 115 | { 116 | auto tf = tfBuffer.lookupTransform("base_link", "base_footprint", tf2::TimePointZero); 117 | EXPECT_EQ(tf.header.stamp, rclcpp::Time(1000000001, 000000001)); 118 | }); 119 | } 120 | 121 | TEST_F(TestBaseFootprint, no_overlapping_time) 122 | { 123 | auto footprint_node = std::make_shared(); 124 | auto node = std::make_shared("test_node"); 125 | 126 | tf2_ros::TransformBroadcaster br(node); 127 | 128 | builtin_interfaces::msg::Time stamp(rclcpp::Time(1000000000, 000000001)); 129 | 130 | geometry_msgs::msg::TransformStamped tf; 131 | 132 | tf.header.frame_id = "odom"; 133 | tf.child_frame_id = "base_link"; 134 | tf.header.stamp = rclcpp::Time(1000000000, 000000001); 135 | br.sendTransform(tf); 136 | tf.header.stamp = rclcpp::Time(1000000001, 000000001); 137 | br.sendTransform(tf); 138 | 139 | tf.header.frame_id = "base_link"; 140 | tf.child_frame_id = "l_sole"; 141 | tf.header.stamp = rclcpp::Time(1000000002, 000000001); 142 | br.sendTransform(tf); 143 | tf.header.stamp = rclcpp::Time(1000000003, 000000001); 144 | br.sendTransform(tf); 145 | 146 | tf.header.frame_id = "base_link"; 147 | tf.child_frame_id = "r_sole"; 148 | tf.header.stamp = rclcpp::Time(1000000004, 000000001); 149 | br.sendTransform(tf); 150 | tf.header.stamp = rclcpp::Time(1000000005, 000000001); 151 | br.sendTransform(tf); 152 | 153 | tf2_ros::Buffer tfBuffer{node->get_clock()}; 154 | tf2_ros::TransformListener tfListener{tfBuffer, node}; 155 | 156 | rclcpp::sleep_for(std::chrono::milliseconds(50)); // Wait for timer callback to be called 157 | rclcpp::spin_some(footprint_node); 158 | rclcpp::sleep_for(std::chrono::milliseconds(5)); // Wait for transforms to arrive into buffer 159 | 160 | ASSERT_THROW( 161 | { 162 | tfBuffer.lookupTransform("base_link", "base_footprint", tf2::TimePointZero); 163 | }, tf2::TransformException); 164 | } 165 | 166 | TEST_F(TestBaseFootprint, test_tf_listener_node_isnt_created) 167 | { 168 | // Ensure the tf listener does not create its own tf listener node. 169 | // See https://github.com/ros-sports/humanoid_base_footprint/pull/41 for more details. 170 | auto footprint_node = std::make_shared(); 171 | auto nodes = footprint_node->get_node_names(); 172 | ASSERT_EQ(nodes.size(), 1u); 173 | } 174 | -------------------------------------------------------------------------------- /src/base_footprint.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Hamburg Bit-Bots 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | // This has to be included after #include , due to reasons 23 | // explained in https://github.com/ros2/geometry2/pull/485 24 | // "tf2/utils.h" is used instead of to prevent ament_cpplint from complaining about 25 | // including a C system header after C++ system headers. 26 | #include "tf2/utils.h" 27 | 28 | using std::placeholders::_1; 29 | using namespace std::chrono_literals; 30 | 31 | namespace humanoid_base_footprint 32 | { 33 | 34 | BaseFootprintBroadcaster::BaseFootprintBroadcaster(const rclcpp::NodeOptions &) 35 | : Node("base_footprint"), 36 | tfBuffer_(std::make_unique(this->get_clock())), 37 | tfListener_(std::make_shared(*tfBuffer_, this)) 38 | { 39 | // setup tf listener and broadcaster as class members 40 | base_link_frame_ = this->declare_parameter("base_link_frame", "base_link"); 41 | base_footprint_frame_ = this->declare_parameter( 42 | "base_footprint_frame", "base_footprint"); 43 | r_sole_frame_ = this->declare_parameter("r_sole_frame", "r_sole"); 44 | l_sole_frame_ = this->declare_parameter("l_sole_frame", "l_sole"); 45 | odom_frame_ = this->declare_parameter("odom_frame", "odom"); 46 | 47 | got_support_foot_ = false; 48 | 49 | this->declare_parameter>("support_state_topics", {"walk_support_state"}); 50 | std::vector support_state_topics; 51 | this->get_parameter("support_state_topics", support_state_topics); 52 | for (auto topic : support_state_topics) { 53 | this->create_subscription( 54 | topic, 55 | 1, 56 | std::bind( 57 | &BaseFootprintBroadcaster::supportFootCallback, 58 | this, 59 | _1)); 60 | } 61 | 62 | tfBroadcaster_ = std::make_unique(this); 63 | 64 | double publish_frequency = this->declare_parameter("publish_frequency", 30.0); 65 | std::chrono::milliseconds publish_interval_ms = 66 | std::chrono::milliseconds(static_cast(1000.0 / publish_frequency)); 67 | timer_ = rclcpp::create_timer( 68 | this, get_clock(), rclcpp::Duration(publish_interval_ms), 69 | std::bind(&BaseFootprintBroadcaster::timerCallback, this)); 70 | } 71 | 72 | void BaseFootprintBroadcaster::timerCallback() 73 | { 74 | try { 75 | geometry_msgs::msg::TransformStamped tf_right = 76 | tfBuffer_->lookupTransform(base_link_frame_, r_sole_frame_, tf2::TimePointZero); 77 | geometry_msgs::msg::TransformStamped tf_left = 78 | tfBuffer_->lookupTransform(base_link_frame_, l_sole_frame_, tf2::TimePointZero); 79 | geometry_msgs::msg::TransformStamped odom = 80 | tfBuffer_->lookupTransform(base_link_frame_, odom_frame_, tf2::TimePointZero); 81 | 82 | auto stamp = std::min( 83 | { 84 | rclcpp::Time{tf_right.header.stamp}, 85 | rclcpp::Time{tf_left.header.stamp}, 86 | rclcpp::Time{odom.header.stamp} 87 | }); 88 | tf_right = tfBuffer_->lookupTransform(base_link_frame_, r_sole_frame_, stamp); 89 | tf_left = tfBuffer_->lookupTransform(base_link_frame_, l_sole_frame_, stamp); 90 | odom = tfBuffer_->lookupTransform(base_link_frame_, odom_frame_, stamp); 91 | 92 | geometry_msgs::msg::TransformStamped support_foot, non_support_foot; 93 | 94 | if (got_support_foot_) { 95 | if (is_left_support_) { 96 | support_foot = tf_left; 97 | non_support_foot = tf_right; 98 | } else { 99 | support_foot = tf_right; 100 | non_support_foot = tf_left; 101 | } 102 | } else { 103 | // check which foot is support foot (which foot is on the ground) 104 | if (tf_right.transform.translation.z < tf_left.transform.translation.z) { 105 | support_foot = tf_right; 106 | non_support_foot = tf_left; 107 | } else { 108 | support_foot = tf_left; 109 | non_support_foot = tf_right; 110 | } 111 | } 112 | // get the position of the non-support foot in the support frame, used for computing the 113 | // barycenter 114 | geometry_msgs::msg::TransformStamped non_support_foot_in_support_foot_frame = 115 | tfBuffer_->lookupTransform( 116 | support_foot.child_frame_id, non_support_foot.child_frame_id, 117 | stamp); 118 | 119 | geometry_msgs::msg::TransformStamped 120 | support_to_base_link = tfBuffer_->lookupTransform( 121 | support_foot.header.frame_id, 122 | support_foot.child_frame_id, 123 | stamp); 124 | 125 | geometry_msgs::msg::PoseStamped base_footprint; 126 | // z at ground leven (support foot height) 127 | base_footprint.pose.position.z = 0; 128 | // x and y at barycenter of feet projections on the ground 129 | base_footprint.pose.position.x = 130 | non_support_foot_in_support_foot_frame.transform.translation.x / 2; 131 | base_footprint.pose.position.y = 132 | non_support_foot_in_support_foot_frame.transform.translation.y / 2; 133 | 134 | double yaw = rot_conv::FYawOfQuat( 135 | Eigen::Quaterniond( 136 | odom.transform.rotation.w, 137 | odom.transform.rotation.x, 138 | odom.transform.rotation.y, 139 | odom.transform.rotation.z)); 140 | 141 | // pitch and roll from support foot, yaw from base link 142 | tf2::Quaternion rotation; 143 | rotation.setRPY(0.0, 0.0, yaw); 144 | 145 | tf2::Quaternion odom_rot; 146 | tf2::fromMsg(odom.transform.rotation, odom_rot); 147 | 148 | base_footprint.pose.orientation = tf2::toMsg(odom_rot * rotation.inverse()); 149 | 150 | // transform the position and orientation of the base footprint into the base_link frame 151 | geometry_msgs::msg::PoseStamped base_footprint_in_base_link; 152 | tf2::doTransform(base_footprint, base_footprint_in_base_link, support_to_base_link); 153 | 154 | // set the broadcasted transform to the position and orientation of the base footprint 155 | geometry_msgs::msg::TransformStamped tf; 156 | tf.header.stamp = stamp; 157 | tf.header.frame_id = base_link_frame_; 158 | tf.child_frame_id = base_footprint_frame_; 159 | tf.transform.translation.x = base_footprint_in_base_link.pose.position.x; 160 | tf.transform.translation.y = base_footprint_in_base_link.pose.position.y; 161 | tf.transform.translation.z = base_footprint_in_base_link.pose.position.z; 162 | tf.transform.rotation = base_footprint.pose.orientation; 163 | tfBroadcaster_->sendTransform(tf); 164 | } catch (const tf2::TransformException & ex) { 165 | // RCLCPP_WARN(this->get_logger(), ex.what()); 166 | } 167 | } 168 | 169 | void BaseFootprintBroadcaster::supportFootCallback(const biped_interfaces::msg::Phase msg) 170 | { 171 | got_support_foot_ = true; 172 | is_left_support_ = (msg.phase == biped_interfaces::msg::Phase::LEFT_STANCE); 173 | } 174 | 175 | } // namespace humanoid_base_footprint 176 | 177 | #include "rclcpp_components/register_node_macro.hpp" 178 | RCLCPP_COMPONENTS_REGISTER_NODE(humanoid_base_footprint::BaseFootprintBroadcaster) 179 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. --------------------------------------------------------------------------------