Use wait for tf instead of waiting for joint states

bitbots_odometry/CMakeLists.txt

@@ -6,10 +6,11 @@ if (NOT CMAKE_CXX_STANDARD)
     set(CMAKE_CXX_STANDARD 17)
 endif ()
 
-find_package(Eigen3 REQUIRED)
 find_package(ament_cmake REQUIRED)
 find_package(biped_interfaces REQUIRED)
 find_package(bitbots_docs REQUIRED)
+find_package(bitbots_utils REQUIRED)
+find_package(Eigen3 REQUIRED)
 find_package(generate_parameter_library REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(message_filters REQUIRED)
@@ -43,38 +44,40 @@ target_link_libraries(
 ## Specify libraries to link a library or executable target against
 ament_target_dependencies(
   motion_odometry
-  tf2_eigen
-  tf2
-  nav_msgs
-  message_filters
-  tf2_geometry_msgs
+  ament_cmake
   biped_interfaces
-  geometry_msgs
   bitbots_docs
+  bitbots_utils
+  generate_parameter_library
+  geometry_msgs
+  message_filters
+  nav_msgs
+  rclcpp
   rot_conv
   sensor_msgs
+  tf2
+  tf2_eigen
+  tf2_geometry_msgs
   tf2_ros
-  ament_cmake
-  generate_parameter_library
-  rclcpp
 )
 
 ament_target_dependencies(
   odometry_fuser
-  tf2_eigen
-  tf2
-  nav_msgs
-  message_filters
-  tf2_geometry_msgs
+  ament_cmake
   biped_interfaces
-  geometry_msgs
   bitbots_docs
+  bitbots_utils
+  Eigen3
+  geometry_msgs
+  message_filters
+  nav_msgs
+  rclcpp
   rot_conv
   sensor_msgs
+  tf2
+  tf2_eigen
+  tf2_geometry_msgs
   tf2_ros
-  ament_cmake
-  rclcpp
-  Eigen3
 )
 
 enable_bitbots_docs()

bitbots_odometry/include/bitbots_odometry/motion_odometry.h

@@ -1,16 +1,18 @@
 #include <rclcpp/rclcpp.hpp>
+#include "odometry_parameters.hpp"
+
+#include <biped_interfaces/msg/phase.hpp>
+#include <bitbots_utils/utils.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <rclcpp/experimental/executors/events_executor/events_executor.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>
 #include <std_msgs/msg/char.hpp>
-#include <tf2_ros/transform_broadcaster.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros/transform_listener.h>
 #include <tf2/utils.h>
-#include <nav_msgs/msg/odometry.hpp>
-#include <biped_interfaces/msg/phase.hpp>
 #include <unistd.h>
-#include <tf2_ros/buffer.h>
-#include <rclcpp/experimental/executors/events_executor/events_executor.hpp>
-#include "odometry_parameters.hpp"
 
 using std::placeholders::_1;
 
@@ -25,15 +27,12 @@ class MotionOdometry : public rclcpp::Node {
   char current_support_state_;
   char previous_support_state_;
   rclcpp::Time current_support_state_time_{rclcpp::Time(0, 0, RCL_ROS_TIME)};
-  sensor_msgs::msg::JointState current_joint_states_;
   nav_msgs::msg::Odometry current_odom_msg_;
   tf2::Transform odometry_to_support_foot_;
-  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
   std::string base_link_frame_, r_sole_frame_, l_sole_frame_, odom_frame_;
   rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pub_odometry_;
   rclcpp::Subscription<biped_interfaces::msg::Phase>::SharedPtr walk_support_state_sub_;
   rclcpp::Subscription<biped_interfaces::msg::Phase>::SharedPtr kick_support_state_sub_;
-  rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
   rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_subscriber_;
 
   // Declare parameter listener and struct from the generate_parameter_library
@@ -42,9 +41,9 @@ class MotionOdometry : public rclcpp::Node {
   motion_odometry::Params config_;
 
   void supportCallback(const biped_interfaces::msg::Phase::SharedPtr msg);
-  void jointStateCb(const sensor_msgs::msg::JointState::SharedPtr msg);
   void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg);
 
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
   std::unique_ptr<tf2_ros::TransformBroadcaster> br_;
   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
   rclcpp::Time foot_change_time_;

bitbots_odometry/include/bitbots_odometry/odometry_fuser.h

@@ -1,42 +1,44 @@
 #include <rclcpp/rclcpp.hpp>
 
-#include <sensor_msgs/msg/imu.hpp>
-#include <tf2_ros/transform_broadcaster.h>
-#include <tf2_eigen/tf2_eigen.hpp>
+#include <biped_interfaces/msg/phase.hpp>
+#include <bitbots_utils/utils.hpp>
+#include <builtin_interfaces/msg/time.hpp>
+#include <Eigen/Geometry>
 #include <geometry_msgs/msg/transform_stamped.hpp>
+#include <message_filters/cache.h>
+#include <message_filters/subscriber.h>
+#include <message_filters/sync_policies/approximate_time.h>
+#include <message_filters/synchronizer.h>
 #include <nav_msgs/msg/odometry.hpp>
+#include <rclcpp/experimental/executors/events_executor/events_executor.hpp>
+#include <rot_conv/rot_conv.h>
+#include <sensor_msgs/msg/imu.hpp>
 #include <std_msgs/msg/char.hpp>
-#include <builtin_interfaces/msg/time.hpp>
-#include <tf2/utils.h>
+#include <tf2_eigen/tf2_eigen.hpp>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_broadcaster.h>
 #include <tf2_ros/transform_listener.h>
-#include <tf2/LinearMath/Vector3.h>
-#include <tf2/LinearMath/Scalar.h>
 #include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Scalar.h>
 #include <tf2/LinearMath/Transform.h>
-#include <Eigen/Geometry>
-#include <rot_conv/rot_conv.h>
-#include <message_filters/cache.h>
-#include <message_filters/subscriber.h>
-#include <message_filters/synchronizer.h>
-#include <message_filters/sync_policies/approximate_time.h>
-#include <biped_interfaces/msg/phase.hpp>
+#include <tf2/LinearMath/Vector3.h>
+#include <tf2/utils.h>
 #include <unistd.h>
-#include <rclcpp/experimental/executors/events_executor/events_executor.hpp>
 
 using std::placeholders::_1;
+using bitbots_utils::wait_for_tf;
 
 typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Imu, nav_msgs::msg::Odometry> SyncPolicy;
 
 class OdometryFuser : public rclcpp::Node {
  public:
   OdometryFuser();
   void loop();
-  bool wait_for_tf();
  private:
   sensor_msgs::msg::Imu imu_data_;
   nav_msgs::msg::Odometry odom_data_;
-  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
   rclcpp::Time fused_time_;
   std::string base_link_frame_, r_sole_frame_, l_sole_frame_, odom_frame_, rotation_frame_, imu_frame_;

bitbots_odometry/package.xml

@@ -14,16 +14,17 @@
   <author email="[email protected]">Jasper Güldenstein</author>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
-  <depend>nav_msgs</depend>
+  <depend>biped_interfaces</depend>
+  <depend>bitbots_docs</depend>
+  <depend>bitbots_utils</depend>
+  <depend>generate_parameter_library</depend>
   <depend>message_filters</depend>
+  <depend>nav_msgs</depend>
+  <depend>rot_conv</depend>
   <depend>sensor_msgs</depend>
+  <depend>tf2_eigen</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2</depend>
-  <depend>tf2_eigen</depend>
-  <depend>rot_conv</depend>
-  <depend>bitbots_docs</depend>
-  <depend>biped_interfaces</depend>
-  <depend>generate_parameter_library</depend>
 
   <export>
     <bitbots_documentation>