fix tf callback contention and incorrect node parameter loading

Author: ajshank

state_manager/include/state_manager.h

@@ -9,6 +9,7 @@
 #ifndef __STATE_INFO_GENERATOR_H__
 #define __STATE_INFO_GENERATOR_H__
 
+#include <memory>
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/transform_stamped.hpp"
 #include "geometry_msgs/msg/twist_stamped.hpp"
@@ -22,6 +23,7 @@
 #include <tf2/impl/utils.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
 
 #include "freyja_msgs/msg/current_state.hpp"
 #include "freyja_msgs/msg/asctec_data.hpp"
@@ -83,7 +85,8 @@ class StateManager: public rclcpp::Node
   std::string tf_base_frame_;
   std::string tf_my_frame_;
   std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
-  
+  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
   /* containers for handling gps data */
   double home_lat_, home_lon_;
   bool have_location_fix_;

state_manager/src/callback_implementations.cpp

@@ -88,7 +88,11 @@ void StateManager::timerTfCallback()
   /* This is a timer callback */
   static TFStamped tform;
 
-  tform = tf_buffer_ -> lookupTransform( tf_my_frame_, tf_base_frame_, tf2::TimePointZero );
+  if( tf_buffer_ -> canTransform( tf_my_frame_, tf_base_frame_, tf2::TimePointZero ) )
+  {
+    tform = tf_buffer_ -> lookupTransform( tf_my_frame_, tf_base_frame_, tf2::TimePointZero );
+    mocapCallback( std::make_shared<const TFStamped>( tform ) );
+  }
 
 }
 

state_manager/src/state_manager.cpp

@@ -22,6 +22,8 @@ StateManager::StateManager() : Node( rclcpp_NODE_NAME )
 
 
   get_parameter( "state_source", state_source_ );
+  get_parameter( "filter_type", filter_type_ );
+  get_parameter( "filter_length", filter_len_ );
 
   if( state_source_ == "mocap" )
     initMocapManager();
@@ -68,6 +70,8 @@ StateManager::StateManager() : Node( rclcpp_NODE_NAME )
     rate_filter_ = FreyjaFilters( -1, "median", "~" );
     filter_len_ = pose_filter_.getCurrentFilterLen();
   }
+  else
+    RCLCPP_WARN( get_logger(), "No filter initialised by Freyja." );
 
   // init history containers
   prev_pn_.resize( filter_len_ );
@@ -105,9 +109,12 @@ void StateManager::initTfManager()
   get_parameter( "state_base_frame", tf_base_frame_ );
   get_parameter( "state_my_frame", tf_my_frame_ );
 
+  RCLCPP_INFO( get_logger(), "Registered tf frames: [%s] to [%s]", tf_base_frame_.c_str(), tf_my_frame_.c_str() );
+  tf_buffer_ = std::make_unique<tf2_ros::Buffer>( get_clock() );
+  tf_listener_ = std::make_shared<tf2_ros::TransformListener>( *tf_buffer_ );
   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()