From c8f041038fb00fd2921af337b9c7d6eb1f852d4e Mon Sep 17 00:00:00 2001 From: Katie Hughes Date: Mon, 16 Dec 2024 14:32:13 -0500 Subject: [PATCH] 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; }