Untested additions to state publisher and object synchronizer

Signed-off-by: Katie Hughes <khughes-bdai@theaiinstitute.com>
bdaiinstitute · Dec 16, 2024 · 1be88b3 · 1be88b3
1 parent c8f0410
commit 1be88b3
Show file tree

Hide file tree

Showing 4 changed files with 8 additions and 7 deletions.
diff --git a/spot_driver/include/spot_driver/robot_state/state_publisher.hpp b/spot_driver/include/spot_driver/robot_state/state_publisher.hpp
@@ -60,7 +60,7 @@ class StatePublisher {
    */
   void timerCallback();
 
-  std::string full_odom_frame_id_;
+  std::string full_tf_root_id_;
 
   std::string frame_prefix_;
 

diff --git a/spot_driver/src/conversions/robot_state.cpp b/spot_driver/src/conversions/robot_state.cpp
@@ -151,7 +151,7 @@ std::optional<tf2_msgs::msg::TFMessage> getTf(const ::bosdyn::api::FrameTreeSnap
                                        : transform.parent_frame_name();
     const auto frame_name = frame_id.find('/') == std::string::npos ? prefix + frame_id : frame_id;
 
-    // set target frame(preferred odom frame) as the root node in tf tree
+    // set preferred base frame as the root node in tf tree
     if (preferred_base_frame_id == frame_name) {
       tf_msg.transforms.push_back(
           toTransformStamped(~(transform.parent_tform_child()), frame_name, parent_frame_name, timestamp_local));

diff --git a/spot_driver/src/object_sync/object_synchronizer.cpp b/spot_driver/src/object_sync/object_synchronizer.cpp
@@ -332,9 +332,9 @@ ObjectSynchronizer::ObjectSynchronizer(const std::shared_ptr<WorldObjectClientIn
   const auto spot_name = parameter_interface_->getSpotName();
   frame_prefix_ = spot_name.empty() ? "" : spot_name + "/";
 
-  preferred_base_frame_ = stripPrefix(parameter_interface_->getPreferredOdomFrame(), frame_prefix_);
+  preferred_base_frame_ = stripPrefix(parameter_interface_->getTFRoot(), frame_prefix_);
   preferred_base_frame_with_prefix_ = preferred_base_frame_.find('/') == std::string::npos
-                                          ? spot_name + "/" + preferred_base_frame_
+                                          ? frame_prefix_ + preferred_base_frame_
                                           : preferred_base_frame_;
 
   // TODO(khughes): This is temporarily disabled to reduce driver's spew about TF extrapolation.

diff --git a/spot_driver/src/robot_state/state_publisher.cpp b/spot_driver/src/robot_state/state_publisher.cpp
@@ -35,8 +35,9 @@ StatePublisher::StatePublisher(const std::shared_ptr<StateClientInterface>& stat
 
   const auto preferred_odom_frame = parameter_interface_->getPreferredOdomFrame();
   is_using_vision_ = preferred_odom_frame == "vision";
-  full_odom_frame_id_ =
-      preferred_odom_frame.find('/') == std::string::npos ? frame_prefix_ + preferred_odom_frame : preferred_odom_frame;
+
+  const auto tf_root = parameter_interface_->getTFRoot();
+  full_tf_root_id_ = tf_root.find('/') == std::string::npos ? frame_prefix_ + tf_root : tf_root;
 
   // Create a timer to request and publish robot state at a fixed rate
   timer_interface_->setTimer(kRobotStateCallbackPeriod, [this] {
@@ -67,7 +68,7 @@ void StatePublisher::timerCallback() {
                          getFootState(robot_state),
                          getEstopStates(robot_state, clock_skew),
                          getJointStates(robot_state, clock_skew, frame_prefix_),
-                         getTf(robot_state, clock_skew, frame_prefix_, full_odom_frame_id_),
+                         getTf(robot_state, clock_skew, frame_prefix_, full_tf_root_id_),
                          getOdomTwist(robot_state, clock_skew),
                          getOdom(robot_state, clock_skew, frame_prefix_, is_using_vision_),
                          getPowerState(robot_state, clock_skew),