From c8f041038fb00fd2921af337b9c7d6eb1f852d4e Mon Sep 17 00:00:00 2001 From: Katie Hughes Date: Mon, 16 Dec 2024 14:32:13 -0500 Subject: [PATCH 1/4] Add option for tf root parameter Signed-off-by: Katie Hughes --- .../interfaces/parameter_interface_base.hpp | 2 ++ .../interfaces/rclcpp_parameter_interface.hpp | 1 + spot_driver/spot_driver/spot_ros2.py | 24 ------------------- .../interfaces/rclcpp_parameter_interface.cpp | 5 ++++ .../fake/fake_parameter_interface.hpp | 2 ++ 5 files changed, 10 insertions(+), 24 deletions(-) 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 4a837bb2..aa1751c6 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 0a205ac4..0a98a590 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/spot_driver/spot_ros2.py b/spot_driver/spot_driver/spot_ros2.py index 3d486f8b..2c95747b 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/interfaces/rclcpp_parameter_interface.cpp b/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp index 1eaa024b..a2d6b0c2 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/test/include/spot_driver/fake/fake_parameter_interface.hpp b/spot_driver/test/include/spot_driver/fake/fake_parameter_interface.hpp index 433db5c0..122ad8cb 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; } From 1be88b3bfccf6aca10514c402ae8eb4ed7b0a352 Mon Sep 17 00:00:00 2001 From: Katie Hughes Date: Mon, 16 Dec 2024 17:48:21 -0500 Subject: [PATCH 2/4] Untested additions to state publisher and object synchronizer Signed-off-by: Katie Hughes --- .../include/spot_driver/robot_state/state_publisher.hpp | 2 +- spot_driver/src/conversions/robot_state.cpp | 2 +- spot_driver/src/object_sync/object_synchronizer.cpp | 4 ++-- spot_driver/src/robot_state/state_publisher.cpp | 7 ++++--- 4 files changed, 8 insertions(+), 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 index d6a05409..6fe72f7d 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/src/conversions/robot_state.cpp b/spot_driver/src/conversions/robot_state.cpp index 33da7f1d..5b8658cb 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/object_sync/object_synchronizer.cpp b/spot_driver/src/object_sync/object_synchronizer.cpp index e4d4eb2d..2c688e69 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 176b764c..7e4d702f 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), From fb22a20fe7a112482cd88599924ff7bf9d7181a3 Mon Sep 17 00:00:00 2001 From: Katie Hughes Date: Tue, 17 Dec 2024 10:31:27 -0500 Subject: [PATCH 3/4] Modify example config Signed-off-by: Katie Hughes --- spot_driver/config/spot_ros_example.yaml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/spot_driver/config/spot_ros_example.yaml b/spot_driver/config/spot_ros_example.yaml index a19a1158..00d85003 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. From d2f7cec0c6ed614a23ba83909bc595801c7e315f Mon Sep 17 00:00:00 2001 From: Katie Hughes Date: Tue, 17 Dec 2024 11:38:46 -0500 Subject: [PATCH 4/4] Update tests for new parameter Signed-off-by: Katie Hughes --- spot_driver/test/src/test_parameter_interface.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/spot_driver/test/src/test_parameter_interface.cpp b/spot_driver/test/src/test_parameter_interface.cpp index 74546d22..4ad4be3a 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) {