[SW-1658] Allow body frame to be root of TF tree

spot_driver/config/spot_ros_example.yaml

@@ -21,7 +21,8 @@
     estop_timeout: 9.0
     start_estop: False
 
-    preferred_odom_frame: "odom" # pass either odom/vision. This frame will become the parent of body in tf2 tree and will be used in odometry topic. https://dev.bostondynamics.com/docs/concepts/geometry_and_frames.html?highlight=frame#frames-in-the-spot-robot-world for more info.
+    preferred_odom_frame: "odom" # pass either odom/vision. This will be used in the odometry topic. https://dev.bostondynamics.com/docs/concepts/geometry_and_frames.html?highlight=frame#frames-in-the-spot-robot-world for more info.
+    tf_root: "odom" # Use "odom" (default), "vision", or "body" for the root frame in your TF tree.
 
     cmd_duration: 0.25 # The duration of cmd_vel commands. Increase this if spot stutters when publishing cmd_vel.
     rgb_cameras: True  # Set to False if your robot has greyscale cameras -- otherwise you won't receive data.

spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp

@@ -40,6 +40,7 @@ class ParameterInterfaceBase {
   virtual bool getPublishDepthImages() const = 0;
   virtual bool getPublishDepthRegisteredImages() const = 0;
   virtual std::string getPreferredOdomFrame() const = 0;
+  virtual std::string getTFRoot() const = 0;
   virtual std::string getSpotName() const = 0;
   virtual bool getGripperless() const = 0;
   virtual std::set<spot_ros2::SpotCamera> getDefaultCamerasUsed(bool has_arm, bool gripperless) const = 0;
@@ -59,6 +60,7 @@ class ParameterInterfaceBase {
   static constexpr bool kDefaultPublishDepthImages{true};
   static constexpr bool kDefaultPublishDepthRegisteredImages{true};
   static constexpr auto kDefaultPreferredOdomFrame = "odom";
+  static constexpr auto kDefaultTFRoot = "odom";
   static constexpr bool kDefaultGripperless{false};
   static constexpr auto kCamerasWithoutHand = {"frontleft", "frontright", "left", "right", "back"};
   static constexpr auto kCamerasWithHand = {"frontleft", "frontright", "left", "right", "back", "hand"};

spot_driver/include/spot_driver/interfaces/rclcpp_parameter_interface.hpp

@@ -35,6 +35,7 @@ class RclcppParameterInterface : public ParameterInterfaceBase {
   [[nodiscard]] bool getPublishDepthImages() const override;
   [[nodiscard]] bool getPublishDepthRegisteredImages() const override;
   [[nodiscard]] std::string getPreferredOdomFrame() const override;
+  [[nodiscard]] std::string getTFRoot() const override;
   [[nodiscard]] std::string getSpotName() const override;
   [[nodiscard]] bool getGripperless() const override;
   [[nodiscard]] std::set<spot_ros2::SpotCamera> getDefaultCamerasUsed(const bool has_arm,

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_;
 

spot_driver/spot_driver/spot_ros2.py

@@ -338,34 +338,10 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw
             get_from_env_and_fall_back_to_param("SPOT_CERTIFICATE", self, "certificate", "") or None
         )
 
-        # Spot has 2 types of odometries: 'odom' and 'vision'
-        # The former one is kinematic odometry and the second one is a combined odometry of vision and kinematics
-        # These params enables to change which odometry frame is a parent of body frame and to change tf names of each
-        # odometry frames.
         frame_prefix = ""
         if self.name is not None:
             frame_prefix = self.name + "/"
         self.frame_prefix: str = frame_prefix
-        self.preferred_odom_frame: Parameter = self.declare_parameter(
-            "preferred_odom_frame", self.frame_prefix + "odom"
-        )  # 'vision' or 'odom'
-        self.tf_name_kinematic_odom: Parameter = self.declare_parameter(
-            "tf_name_kinematic_odom", self.frame_prefix + "odom"
-        )
-        self.tf_name_raw_kinematic: str = frame_prefix + "odom"
-        self.tf_name_vision_odom: Parameter = self.declare_parameter(
-            "tf_name_vision_odom", self.frame_prefix + "vision"
-        )
-        self.tf_name_raw_vision: str = self.frame_prefix + "vision"
-
-        preferred_odom_frame_references = [self.tf_name_raw_kinematic, self.tf_name_raw_vision]
-        if self.preferred_odom_frame.value not in preferred_odom_frame_references:
-            error_msg = (
-                f'rosparam "preferred_odom_frame" should be one of {preferred_odom_frame_references}, got'
-                f' "{self.preferred_odom_frame.value}"'
-            )
-            self.get_logger().error(error_msg)
-            raise ValueError(error_msg)
 
         self.tf_name_graph_nav_body: str = self.frame_prefix + "body"
 

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));

spot_driver/src/interfaces/rclcpp_parameter_interface.cpp

@@ -26,6 +26,7 @@ constexpr auto kParameterNamePublishCompressedImages = "publish_compressed_image
 constexpr auto kParameterNamePublishDepthImages = "publish_depth";
 constexpr auto kParameterNamePublishDepthRegisteredImages = "publish_depth_registered";
 constexpr auto kParameterPreferredOdomFrame = "preferred_odom_frame";
+constexpr auto kParameterTFRoot = "tf_root";
 constexpr auto kParameterNameGripperless = "gripperless";
 
 /**
@@ -187,6 +188,10 @@ std::string RclcppParameterInterface::getPreferredOdomFrame() const {
   return declareAndGetParameter<std::string>(node_, kParameterPreferredOdomFrame, kDefaultPreferredOdomFrame);
 }
 
+std::string RclcppParameterInterface::getTFRoot() const {
+  return declareAndGetParameter<std::string>(node_, kParameterTFRoot, kDefaultTFRoot);
+}
+
 bool RclcppParameterInterface::getGripperless() const {
   return declareAndGetParameter<bool>(node_, kParameterNameGripperless, kDefaultGripperless);
 }

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.

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),

spot_driver/test/include/spot_driver/fake/fake_parameter_interface.hpp

@@ -38,6 +38,8 @@ class FakeParameterInterface : public ParameterInterfaceBase {
 
   std::string getPreferredOdomFrame() const override { return "odom"; }
 
+  std::string getTFRoot() const override { return "odom"; }
+
   std::string getSpotName() const override { return spot_name; }
 
   bool getGripperless() const override { return gripperless; }

spot_driver/test/src/test_parameter_interface.cpp

@@ -195,6 +195,8 @@ TEST_F(RclcppParameterInterfaceEnvVarTest, GetSpotConfigFromParameters) {
   node_->declare_parameter("publish_depth", publish_depth_images_parameter);
   constexpr auto publish_depth_registered_images_parameter = false;
   node_->declare_parameter("publish_depth_registered", publish_depth_registered_images_parameter);
+  constexpr auto tf_root_parameter = "body";
+  node_->declare_parameter("tf_root", tf_root_parameter);
 
   // GIVEN we create a RclcppParameterInterface using the node
   RclcppParameterInterface parameter_interface{node_};
@@ -213,6 +215,7 @@ TEST_F(RclcppParameterInterfaceEnvVarTest, GetSpotConfigFromParameters) {
   EXPECT_THAT(parameter_interface.getPublishRGBImages(), Eq(publish_rgb_images_parameter));
   EXPECT_THAT(parameter_interface.getPublishDepthImages(), Eq(publish_depth_images_parameter));
   EXPECT_THAT(parameter_interface.getPublishDepthRegisteredImages(), Eq(publish_depth_registered_images_parameter));
+  EXPECT_THAT(parameter_interface.getTFRoot(), Eq(tf_root_parameter));
 }
 
 TEST_F(RclcppParameterInterfaceEnvVarTest, GetSpotConfigEnvVarsOverruleParameters) {
@@ -272,6 +275,7 @@ TEST_F(RclcppParameterInterfaceEnvVarTest, GetConfigDefaults) {
   EXPECT_THAT(parameter_interface.getPublishRGBImages(), IsTrue());
   EXPECT_THAT(parameter_interface.getPublishDepthImages(), IsTrue());
   EXPECT_THAT(parameter_interface.getPublishDepthRegisteredImages(), IsTrue());
+  EXPECT_THAT(parameter_interface.getTFRoot(), StrEq("odom"));
 }
 
 TEST_F(RclcppParameterInterfaceEnvVarTest, GetCamerasUsedDefaultWithArm) {