update state manager for general tf support

Author: ajshank

state_manager/CMakeLists.txt

@@ -37,6 +37,8 @@ ament_target_dependencies( state_manager_node
   std_srvs
   nav_msgs
   tf2
+  tf2_ros
+  tf2_geometry_msgs
 )
 
 install(TARGETS

state_manager/include/state_manager.h

@@ -21,6 +21,7 @@
 #include <tf2/LinearMath/Quaternion.h>
 #include <tf2/impl/utils.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/buffer.h>
 
 #include "freyja_msgs/msg/current_state.hpp"
 #include "freyja_msgs/msg/asctec_data.hpp"
@@ -77,6 +78,11 @@ class StateManager: public rclcpp::Node
   std::string filter_type_;
   FreyjaFilters pose_filter_;
   FreyjaFilters rate_filter_;
+
+  /* tf related */
+  std::string tf_base_frame_;
+  std::string tf_my_frame_;
+  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
 
   /* containers for handling gps data */
   double home_lat_, home_lon_;
@@ -93,6 +99,7 @@ class StateManager: public rclcpp::Node
     StateManager();
     /* launch-time parameter specifies which one to pick */
     void initPixhawkManager();
+    void initTfManager();
     void initAsctecManager();
     void initMocapManager();
     void initCameraManager();
@@ -101,6 +108,9 @@ class StateManager: public rclcpp::Node
     rclcpp::Subscription<TFStamped>::SharedPtr mocap_data_sub_;
     void mocapCallback( const TFStamped::ConstSharedPtr ) __attribute__((hot));
 
+    rclcpp::TimerBase::SharedPtr tf_timer_;
+    void timerTfCallback();
+
     // /* Callback handler for asctec_onboard_data */
     // rclcpp::Subscription asctec_data_sub_;
     // void asctecDataCallback( const freyja_msgs::msg::AsctecData::ConstSharedPtr );

state_manager/src/callback_implementations.cpp

@@ -83,6 +83,15 @@ void StateManager::mocapCallback( const TFStamped::ConstSharedPtr msg )
   state_pub_ -> publish( state_msg );
 }
 
+void StateManager::timerTfCallback()
+{
+  /* This is a timer callback */
+  static TFStamped tform;
+  
+  tform = tf_buffer_ -> lookupTransform( tf_my_frame_, tf_base_frame_, tf2::TimePointZero );
+
+}
+
 // void StateManager::asctecDataCallback( const freyja_msgs::AsctecData::ConstPtr &msg )
 // {
 //   double time_since = (ros::Time::now() - lastUpdateTime_).toSec();

state_manager/src/state_manager.cpp

@@ -25,6 +25,8 @@ StateManager::StateManager() : Node( rclcpp_NODE_NAME )
 
   if( state_source_ == "mocap" )
     initMocapManager();
+  else if( state_source_ == "tf_mocap" )
+    initTfManager();
   // else if( state_source_ == "asctec" )
   //   initAsctecManager();
   // else if (state_source_ == "apm" )
@@ -90,6 +92,24 @@ void StateManager::initMocapManager()
                                     std::bind( &StateManager::mocapCallback, this, _1 ) );
 }
 
+void StateManager::initTfManager()
+{
+  declare_parameter<int>( "tf_rate", 200 );
+  declare_parameter<std::string>( "state_base_frame", "map" );
+  declare_parameter<std::string>( "state_my_frame", "agent_1" );
+  
+  int tf_lookup_rate;
+  float tf_timer_freq;
+  
+  get_parameter( "tf_rate", tf_lookup_rate );
+  get_parameter( "state_base_frame", tf_base_frame_ );
+  get_parameter( "state_my_frame", tf_my_frame_ );
+
+  tf_timer_ = rclcpp::create_timer( this, get_clock(), std::chrono::duration<float>(1.0/tf_lookup_rate),
+                          std::bind(&StateManager::timerTfCallback, this) );
+  tf_buffer_ = std::make_unique<tf2_ros::Buffer>( get_clock() );
+}
+
 // void StateManager::initAsctecManager()
 // {
 //   declare_parameter<std::string>( "vehicle_topic", "/asctec_onboard_data" );