From 6410c75e43a439cd22e3f51b60f7710c8338dfc2 Mon Sep 17 00:00:00 2001 From: Katie Hughes <157421702+khughes-bdai@users.noreply.github.com> Date: Wed, 18 Dec 2024 09:56:04 -0500 Subject: [PATCH] [SW-1658] Allow body frame to be root of TF tree (#536) ## Change Overview The goal of this PR is to allow the body frame to be the root of the tf tree, in case you want to broadcast your own custom odometry transform to "body". Currently that's not possible as body always has either odom or vision as a parent, and a tf frame cannot have more than two parents. * adds new parameter `tf_root` to allow you to specify what frame will be the root of the TF tree. * If unset it will default to "odom", which is the current behavior on main * you can set `tf_root` to "vision" or "body" and pass this in via config file, and those frames will become the TF root as expected. (All the other frames and transforms still exist, it just changes the layout of the tree). * Nothing about the odometry topic parent frame changes as the expectation is this should be handled by the `preferred_odom_frame` parameter. (athough it is worth noting that this can't actually be changed currently, as it is hardcoded to be "odom" in the launchfile... https://github.com/bdaiinstitute/spot_ros2/blob/941ab5ec2366a2c703e751c1719914cc5e41d694/spot_driver/launch/spot_driver.launch.py#L109) * deleted some dead python code dealing with `preferred_odom_frame` parameter, as this was adjacent to the parameter I added. (all of this is in C++ now). ## Testing Done - [x] default behavior on robot unchanged - [x] tested new parameter set to "odom", "vision", "body" on robot and made sure TF tree matched expectations - [x] ensure object sync/ fiducials are still read as expected as some calls there changed - [x] updated unit tests pass --- spot_driver/config/spot_ros_example.yaml | 3 ++- .../interfaces/parameter_interface_base.hpp | 2 ++ .../interfaces/rclcpp_parameter_interface.hpp | 1 + .../robot_state/state_publisher.hpp | 2 +- spot_driver/spot_driver/spot_ros2.py | 24 ------------------- spot_driver/src/conversions/robot_state.cpp | 2 +- .../interfaces/rclcpp_parameter_interface.cpp | 5 ++++ .../src/object_sync/object_synchronizer.cpp | 4 ++-- .../src/robot_state/state_publisher.cpp | 7 +++--- .../fake/fake_parameter_interface.hpp | 2 ++ .../test/src/test_parameter_interface.cpp | 4 ++++ 11 files changed, 24 insertions(+), 32 deletions(-) diff --git a/spot_driver/config/spot_ros_example.yaml b/spot_driver/config/spot_ros_example.yaml index a19a1158a..00d850036 100644 --- a/spot_driver/config/spot_ros_example.yaml +++ b/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. diff --git a/spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp b/spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp index 4a837bb2e..aa1751c6b 100644 --- a/spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp +++ b/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 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"}; diff --git a/spot_driver/include/spot_driver/interfaces/rclcpp_parameter_interface.hpp b/spot_driver/include/spot_driver/interfaces/rclcpp_parameter_interface.hpp index 0a205ac49..0a98a5905 100644 --- a/spot_driver/include/spot_driver/interfaces/rclcpp_parameter_interface.hpp +++ b/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 getDefaultCamerasUsed(const bool has_arm, diff --git a/spot_driver/include/spot_driver/robot_state/state_publisher.hpp b/spot_driver/include/spot_driver/robot_state/state_publisher.hpp index d6a054092..6fe72f7d3 100644 --- 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/spot_driver/spot_ros2.py b/spot_driver/spot_driver/spot_ros2.py index 3d486f8b4..2c95747bc 100644 --- a/spot_driver/spot_driver/spot_ros2.py +++ b/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" diff --git a/spot_driver/src/conversions/robot_state.cpp b/spot_driver/src/conversions/robot_state.cpp index 33da7f1d8..5b8658cb5 100644 --- a/spot_driver/src/conversions/robot_state.cpp +++ b/spot_driver/src/conversions/robot_state.cpp @@ -151,7 +151,7 @@ std::optional 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/interfaces/rclcpp_parameter_interface.cpp b/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp index 1eaa024ba..a2d6b0c2e 100644 --- a/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp +++ b/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(node_, kParameterPreferredOdomFrame, kDefaultPreferredOdomFrame); } +std::string RclcppParameterInterface::getTFRoot() const { + return declareAndGetParameter(node_, kParameterTFRoot, kDefaultTFRoot); +} + bool RclcppParameterInterface::getGripperless() const { return declareAndGetParameter(node_, kParameterNameGripperless, kDefaultGripperless); } diff --git a/spot_driver/src/object_sync/object_synchronizer.cpp b/spot_driver/src/object_sync/object_synchronizer.cpp index e4d4eb2da..2c688e69f 100644 --- 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_ptrgetSpotName(); 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 index 176b764ca..7e4d702f8 100644 --- 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& 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), diff --git a/spot_driver/test/include/spot_driver/fake/fake_parameter_interface.hpp b/spot_driver/test/include/spot_driver/fake/fake_parameter_interface.hpp index 433db5c0a..122ad8cb1 100644 --- a/spot_driver/test/include/spot_driver/fake/fake_parameter_interface.hpp +++ b/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; } diff --git a/spot_driver/test/src/test_parameter_interface.cpp b/spot_driver/test/src/test_parameter_interface.cpp index 74546d22a..4ad4be3a2 100644 --- a/spot_driver/test/src/test_parameter_interface.cpp +++ b/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) {