Add option for tf root parameter

Signed-off-by: Katie Hughes <[email protected]>
bdaiinstitute · Dec 16, 2024 · c8f0410 · c8f0410
1 parent 941ab5e
commit c8f0410
Show file tree

Hide file tree

Showing 5 changed files with 10 additions and 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
@@ -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"};

diff --git 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<spot_ros2::SpotCamera> getDefaultCamerasUsed(const bool has_arm,

diff --git 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
@@ -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);
 }

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
@@ -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; }