From 28dda8b22a8c4db86fc192220021a4220d46b033 Mon Sep 17 00:00:00 2001 From: Imaniac230 <44968160+Imaniac230@users.noreply.github.com> Date: Tue, 1 Oct 2024 13:19:06 +0200 Subject: [PATCH 01/13] feat: Integrate robot name and frame prefix into the centralized ros parameters API for easier customizability. * reduced ambiguity when handling the robot name and frame prefix (exposed as explicit ros params) - maintaining the original namespace semantics if frame prefix is not explicitly specified * added a getFramePrefix() method to parameter interface - returns an optional value from the frame_prefix argument * added a getFramePrefixWithDefaultFallback() to the parameter interface - returns a valid prefix based on the frame_prefix and/or robot_name arguments or an empty string * refactored DefaultImageClient, DefaultSpotApi, and the decompress image utils to use the more explicit frame_prefix argument instead of robot_name * exported the local stripPrefix function from object synchronizer to robot state conversions for reusability * SpotImagePublisher, Kinematic, ObjectSynchronizer, and StatePublisher nodes now all use the single getFramePrefixWithDefaultFallback method to get the prefix instead of each creating it independently --- spot_driver/config/spot_ros_example.yaml | 2 + .../spot_driver/api/default_image_client.hpp | 4 +- .../spot_driver/api/default_spot_api.hpp | 8 ++-- .../include/spot_driver/api/spot_api.hpp | 5 ++- .../conversions/decompress_images.hpp | 4 +- .../spot_driver/conversions/robot_state.hpp | 9 ++++ .../interfaces/parameter_interface_base.hpp | 4 +- .../interfaces/rclcpp_parameter_interface.hpp | 4 +- spot_driver/launch/spot_driver.launch.py | 42 ++++++++----------- .../launch/spot_image_publishers.launch.py | 14 ++++--- .../spot_driver/launch/spot_launch_helpers.py | 14 ++++--- spot_driver/spot_driver/spot_ros2.py | 25 +++++++---- spot_driver/src/api/default_image_client.cpp | 32 +++++++------- spot_driver/src/api/default_spot_api.cpp | 10 ++--- .../src/conversions/decompress_images.cpp | 9 ++-- spot_driver/src/conversions/robot_state.cpp | 15 +++++++ .../src/image_stitcher/image_stitcher.cpp | 6 ++- .../src/images/spot_image_publisher_node.cpp | 4 +- .../interfaces/rclcpp_parameter_interface.cpp | 25 +++++++++-- spot_driver/src/kinematic/kinematic_node.cpp | 4 +- .../src/object_sync/object_synchronizer.cpp | 37 ++++------------ .../object_sync/object_synchronizer_node.cpp | 4 +- .../src/robot_state/state_publisher.cpp | 5 +-- .../src/robot_state/state_publisher_node.cpp | 4 +- .../fake/fake_parameter_interface.hpp | 6 ++- .../spot_driver/mock/mock_spot_api.hpp | 2 +- .../test/src/test_parameter_interface.cpp | 6 +-- 27 files changed, 170 insertions(+), 134 deletions(-) diff --git a/spot_driver/config/spot_ros_example.yaml b/spot_driver/config/spot_ros_example.yaml index a19a1158a..2a633cf49 100644 --- a/spot_driver/config/spot_ros_example.yaml +++ b/spot_driver/config/spot_ros_example.yaml @@ -22,6 +22,8 @@ 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. + spot_name: "" + frame_prefix: "" 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/api/default_image_client.hpp b/spot_driver/include/spot_driver/api/default_image_client.hpp index 875942a61..7077bc422 100644 --- a/spot_driver/include/spot_driver/api/default_image_client.hpp +++ b/spot_driver/include/spot_driver/api/default_image_client.hpp @@ -18,7 +18,7 @@ namespace spot_ros2 { class DefaultImageClient : public ImageClientInterface { public: DefaultImageClient(::bosdyn::client::ImageClient* image_client, std::shared_ptr time_sync_api, - const std::string& robot_name); + const std::string& frame_prefix); [[nodiscard]] tl::expected getImages(::bosdyn::api::GetImageRequest request, bool uncompress_images, @@ -27,6 +27,6 @@ class DefaultImageClient : public ImageClientInterface { private: ::bosdyn::client::ImageClient* image_client_; std::shared_ptr time_sync_api_; - std::string robot_name_; + std::string frame_prefix_; }; } // namespace spot_ros2 diff --git a/spot_driver/include/spot_driver/api/default_spot_api.hpp b/spot_driver/include/spot_driver/api/default_spot_api.hpp index bd60b0a86..99f881ae6 100644 --- a/spot_driver/include/spot_driver/api/default_spot_api.hpp +++ b/spot_driver/include/spot_driver/api/default_spot_api.hpp @@ -22,9 +22,9 @@ class DefaultSpotApi : public SpotApi { explicit DefaultSpotApi(const std::string& sdk_client_name, const std::optional& certificate = std::nullopt); - [[nodiscard]] tl::expected createRobot(const std::string& robot_name, - const std::string& ip_address, - const std::optional& port = std::nullopt) override; + [[nodiscard]] tl::expected createRobot(const std::string& ip_address, + const std::optional& port = std::nullopt, + const std::string& frame_prefix = "") override; [[nodiscard]] tl::expected authenticate(const std::string& username, const std::string& password) override; [[nodiscard]] tl::expected hasArm() const override; @@ -42,6 +42,6 @@ class DefaultSpotApi : public SpotApi { std::shared_ptr state_client_interface_; std::shared_ptr time_sync_api_; std::shared_ptr world_object_client_interface_; - std::string robot_name_; + std::string frame_prefix_; }; } // namespace spot_ros2 diff --git a/spot_driver/include/spot_driver/api/spot_api.hpp b/spot_driver/include/spot_driver/api/spot_api.hpp index e0d558e80..739920c2a 100644 --- a/spot_driver/include/spot_driver/api/spot_api.hpp +++ b/spot_driver/include/spot_driver/api/spot_api.hpp @@ -26,8 +26,9 @@ class SpotApi { virtual ~SpotApi() = default; - virtual tl::expected createRobot(const std::string& robot_name, const std::string& ip_address, - const std::optional& port = std::nullopt) = 0; + virtual tl::expected createRobot(const std::string& ip_address, + const std::optional& port = std::nullopt, + const std::string& frame_prefix = "") = 0; virtual tl::expected authenticate(const std::string& username, const std::string& password) = 0; virtual tl::expected hasArm() const = 0; /** diff --git a/spot_driver/include/spot_driver/conversions/decompress_images.hpp b/spot_driver/include/spot_driver/conversions/decompress_images.hpp index 7ce4724ed..8bb4eb52a 100644 --- a/spot_driver/include/spot_driver/conversions/decompress_images.hpp +++ b/spot_driver/include/spot_driver/conversions/decompress_images.hpp @@ -14,10 +14,10 @@ namespace spot_ros2 { tl::expected getCvPixelFormat(const bosdyn::api::Image_PixelFormat& format); -std_msgs::msg::Header createImageHeader(const bosdyn::api::ImageCapture& image_capture, const std::string& robot_name, +std_msgs::msg::Header createImageHeader(const bosdyn::api::ImageCapture& image_capture, const std::string& frame_prefix, const google::protobuf::Duration& clock_skew); tl::expected getDecompressImageMsg(const bosdyn::api::ImageCapture& image_capture, - const std::string& robot_name, + const std::string& frame_prefix, const google::protobuf::Duration& clock_skew); } // namespace spot_ros2 diff --git a/spot_driver/include/spot_driver/conversions/robot_state.hpp b/spot_driver/include/spot_driver/conversions/robot_state.hpp index 3997403cf..3e5ae7bd7 100644 --- a/spot_driver/include/spot_driver/conversions/robot_state.hpp +++ b/spot_driver/include/spot_driver/conversions/robot_state.hpp @@ -41,6 +41,15 @@ inline const std::map kFriendlyJointNames {"arm0.wr1", "arm_wr1"}, {"arm0.f1x", "arm_f1x"}, }; +/** + * @brief Given an input string and a prefix string which is a substring starting at the beginning of the input string, + * return a new string which is the difference between the input string and the prefix string. + * @param input + * @param prefix + * @return A new string which is the difference between the input string and the prefix string. + */ +std::string stripPrefix(const std::string& input, const std::string& prefix); + /** * @brief Create a BatteryStateArray ROS message by parsing a RobotState message and applying a clock skew to it. * 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..2ca26a05e 100644 --- a/spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp +++ b/spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp @@ -40,11 +40,13 @@ class ParameterInterfaceBase { virtual bool getPublishDepthImages() const = 0; virtual bool getPublishDepthRegisteredImages() const = 0; virtual std::string getPreferredOdomFrame() const = 0; - virtual std::string getSpotName() const = 0; + virtual std::optional getFramePrefix() const = 0; + virtual std::string getSpotNameWithFallbackToNamespace() const = 0; virtual bool getGripperless() const = 0; virtual std::set getDefaultCamerasUsed(bool has_arm, bool gripperless) const = 0; virtual tl::expected, std::string> getCamerasUsed(bool has_arm, bool gripperless) const = 0; + virtual std::string getFramePrefixWithDefaultFallback() const = 0; protected: // These are the definitions of the default values for optional parameters. 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..4ef2ee2c7 100644 --- a/spot_driver/include/spot_driver/interfaces/rclcpp_parameter_interface.hpp +++ b/spot_driver/include/spot_driver/interfaces/rclcpp_parameter_interface.hpp @@ -35,12 +35,14 @@ class RclcppParameterInterface : public ParameterInterfaceBase { [[nodiscard]] bool getPublishDepthImages() const override; [[nodiscard]] bool getPublishDepthRegisteredImages() const override; [[nodiscard]] std::string getPreferredOdomFrame() const override; - [[nodiscard]] std::string getSpotName() const override; + [[nodiscard]] std::optional getFramePrefix() const override; + [[nodiscard]] std::string getSpotNameWithFallbackToNamespace() const override; [[nodiscard]] bool getGripperless() const override; [[nodiscard]] std::set getDefaultCamerasUsed(const bool has_arm, const bool gripperless) const override; [[nodiscard]] tl::expected, std::string> getCamerasUsed( const bool has_arm, const bool gripperless) const override; + [[nodiscard]] std::string getFramePrefixWithDefaultFallback() const override; private: std::shared_ptr node_; diff --git a/spot_driver/launch/spot_driver.launch.py b/spot_driver/launch/spot_driver.launch.py index 8b07fd7fd..9badab344 100644 --- a/spot_driver/launch/spot_driver.launch.py +++ b/spot_driver/launch/spot_driver.launch.py @@ -11,7 +11,12 @@ from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution, TextSubstitution from launch_ros.substitutions import FindPackageShare -from spot_driver.launch.spot_launch_helpers import IMAGE_PUBLISHER_ARGS, declare_image_publisher_args, spot_has_arm +from spot_driver.launch.spot_launch_helpers import ( + IMAGE_PUBLISHER_ARGS, + declare_image_publisher_args, + get_ros_param_dict, + spot_has_arm, +) THIS_PACKAGE = "spot_driver" @@ -20,8 +25,6 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: config_file = LaunchConfiguration("config_file") launch_rviz = LaunchConfiguration("launch_rviz") rviz_config_file = LaunchConfiguration("rviz_config_file").perform(context) - spot_name = LaunchConfiguration("spot_name").perform(context) - tf_prefix = LaunchConfiguration("tf_prefix").perform(context) mock_enable = IfCondition(LaunchConfiguration("mock_enable", default="False")).evaluate(context) # if config_file has been set (and is not the default empty string) and is also not a file, do not launch anything. @@ -29,18 +32,21 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: if (config_file_path != "") and (not os.path.isfile(config_file_path)): raise FileNotFoundError("Configuration file '{}' does not exist!".format(config_file_path)) + ros_params = get_ros_param_dict(config_file_path) + spot_name: str = ros_params["spot_name"] if "spot_name" in ros_params else "" + tf_prefix: str = ros_params["frame_prefix"] if "frame_prefix" in ros_params else "" + if mock_enable: mock_has_arm = IfCondition(LaunchConfiguration("mock_has_arm")).evaluate(context) has_arm = mock_has_arm else: - has_arm = spot_has_arm(config_file_path=config_file.perform(context), spot_name=spot_name) + has_arm = spot_has_arm(config_file_path=config_file.perform(context)) pkg_share = FindPackageShare("spot_description").find("spot_description") # Since spot_image_publisher_node is responsible for retrieving and publishing images, disable all image publishing # in spot_driver. spot_driver_params = { - "spot_name": spot_name, "mock_enable": mock_enable, } @@ -59,15 +65,11 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: ) ld.add_action(spot_driver_node) - if not tf_prefix and spot_name: - tf_prefix = PathJoinSubstitution([spot_name, ""]) - - kinematc_node_params = {"spot_name": spot_name} kinematic_node = launch_ros.actions.Node( package="spot_driver", executable="spot_inverse_kinematics_node", output="screen", - parameters=[config_file, kinematc_node_params], + parameters=[config_file], namespace=spot_name, ) ld.add_action(kinematic_node) @@ -76,11 +78,14 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: package="spot_driver", executable="object_synchronizer_node", output="screen", - parameters=[config_file, {"spot_name": spot_name}], + parameters=[config_file], namespace=spot_name, ) ld.add_action(object_sync_node) + if not tf_prefix and spot_name: + tf_prefix = spot_name + "/" + robot_description = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), @@ -105,12 +110,11 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: ) ld.add_action(robot_state_publisher) - spot_robot_state_publisher_params = {"spot_name": spot_name, "preferred_odom_frame": "odom"} spot_robot_state_publisher = launch_ros.actions.Node( package="spot_driver", executable="state_publisher_node", output="screen", - parameters=[config_file, spot_robot_state_publisher_params], + parameters=[config_file], namespace=spot_name, ) ld.add_action(spot_robot_state_publisher) @@ -136,9 +140,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: spot_image_publishers = IncludeLaunchDescription( PythonLaunchDescriptionSource([FindPackageShare(THIS_PACKAGE), "/launch", "/spot_image_publishers.launch.py"]), - launch_arguments={ - key: LaunchConfiguration(key) for key in ["config_file", "spot_name"] + IMAGE_PUBLISHER_ARGS - }.items(), + launch_arguments={key: LaunchConfiguration(key) for key in ["config_file"] + IMAGE_PUBLISHER_ARGS}.items(), condition=IfCondition(LaunchConfiguration("launch_image_publishers")), ) ld.add_action(spot_image_publishers) @@ -154,13 +156,6 @@ def generate_launch_description() -> launch.LaunchDescription: description="Path to configuration file for the driver.", ) ) - launch_args.append( - DeclareLaunchArgument( - "tf_prefix", - default_value="", - description="apply namespace prefix to robot links and joints", - ) - ) launch_args.append( DeclareLaunchArgument( "launch_rviz", @@ -185,7 +180,6 @@ def generate_launch_description() -> launch.LaunchDescription: ) ) launch_args += declare_image_publisher_args() - launch_args.append(DeclareLaunchArgument("spot_name", default_value="", description="Name of Spot")) ld = launch.LaunchDescription(launch_args) diff --git a/spot_driver/launch/spot_image_publishers.launch.py b/spot_driver/launch/spot_image_publishers.launch.py index 46b822d7f..0bb2febfa 100644 --- a/spot_driver/launch/spot_image_publishers.launch.py +++ b/spot_driver/launch/spot_image_publishers.launch.py @@ -15,12 +15,13 @@ declare_image_publisher_args, get_camera_sources, spot_has_arm, + get_ros_param_dict, ) def create_depth_registration_nodelets( context: launch.LaunchContext, - spot_name: LaunchConfiguration, + spot_name: str, camera_sources: List[str], ) -> List[launch_ros.descriptions.ComposableNode]: """Create the list of depth_image_proc::RegisterNode composable nodes required to generate registered depth images @@ -58,7 +59,7 @@ def create_depth_registration_nodelets( def create_point_cloud_nodelets( context: launch.LaunchContext, - spot_name: LaunchConfiguration, + spot_name: str, camera_sources: List[str], ) -> List[launch_ros.descriptions.ComposableNode]: """Create the list of depth_image_proc::PointCloudXyzrgbNode composable nodes required to generate point clouds for @@ -92,7 +93,6 @@ def create_point_cloud_nodelets( def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: config_file = LaunchConfiguration("config_file") - spot_name = LaunchConfiguration("spot_name").perform(context) depth_registered_mode_config = LaunchConfiguration("depth_registered_mode") publish_point_clouds_config = LaunchConfiguration("publish_point_clouds") mock_enable = IfCondition(LaunchConfiguration("mock_enable", default="False")).evaluate(context) @@ -102,11 +102,14 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: if (config_file_path != "") and (not os.path.isfile(config_file_path)): raise FileNotFoundError("Configuration file '{}' does not exist!".format(config_file_path)) + ros_params = get_ros_param_dict(config_file_path) + spot_name: str = ros_params["spot_name"] if "spot_name" in ros_params else "" + if mock_enable: mock_has_arm = IfCondition(LaunchConfiguration("mock_has_arm")).evaluate(context) has_arm = mock_has_arm else: - has_arm = spot_has_arm(config_file_path=config_file.perform(context), spot_name=spot_name) + has_arm = spot_has_arm(config_file_path=config_file.perform(context)) camera_sources = get_camera_sources(config_file_path, has_arm) @@ -122,7 +125,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: publish_point_clouds = False spot_image_publisher_params = { - key: LaunchConfiguration(key) for key in ["spot_name", "uncompress_images", "publish_compressed_images"] + key: LaunchConfiguration(key) for key in ["uncompress_images", "publish_compressed_images"] } # If using nodelets to generate registered depth images, do not retrieve and publish registered depth images using @@ -194,7 +197,6 @@ def generate_launch_description() -> launch.LaunchDescription: description="Path to configuration file for the driver.", ) ) - launch_args.append(DeclareLaunchArgument("spot_name", default_value="", description="Name of Spot")) launch_args += declare_image_publisher_args() ld = launch.LaunchDescription(launch_args) diff --git a/spot_driver/spot_driver/launch/spot_launch_helpers.py b/spot_driver/spot_driver/launch/spot_launch_helpers.py index 124702eb6..2018c2245 100644 --- a/spot_driver/spot_driver/launch/spot_launch_helpers.py +++ b/spot_driver/spot_driver/launch/spot_launch_helpers.py @@ -116,6 +116,7 @@ def get_ros_param_dict(config_file_path: str) -> Dict[str, Any]: with open(config_file_path, "r") as config_yaml: try: config_dict = yaml.safe_load(config_yaml) + # FIXME: this will not generalize if explicit node names are used in the yaml config if ("/**" in config_dict) and ("ros__parameters" in config_dict["/**"]): ros_params = config_dict["/**"]["ros__parameters"] return ros_params @@ -128,7 +129,7 @@ def get_ros_param_dict(config_file_path: str) -> Dict[str, Any]: raise yaml.YAMLError(f"Config file {config_file_path} couldn't be parsed: failed with '{exc}'") -def get_login_parameters(config_file_path: str) -> Tuple[str, str, str, Optional[int], Optional[str]]: +def get_login_parameters(config_file_path: str) -> Tuple[str, str, str, Optional[int], Optional[str], Optional[str]]: """Obtain the username, password, hostname, port, and certificate of Spot from the environment variables or, if they are not set, the configuration file yaml. @@ -148,6 +149,7 @@ def get_login_parameters(config_file_path: str) -> Tuple[str, str, str, Optional portnum = os.getenv("SPOT_PORT") port = int(portnum) if portnum else None certificate = os.getenv("SPOT_CERTIFICATE") + spot_name: Optional[str] = None ros_params = get_ros_param_dict(config_file_path) # only set username/password/hostname if they were not already set as environment variables. @@ -167,7 +169,9 @@ def get_login_parameters(config_file_path: str) -> Tuple[str, str, str, Optional f"[Username: '{username}' Password: '{password}' Hostname: '{hostname}']. Ensure that your environment " "variables are set or update your config_file yaml." ) - return username, password, hostname, port, certificate + if "spot_name" in ros_params and ros_params["spot_name"]: + spot_name = ros_params["spot_name"] + return username, password, hostname, port, certificate, spot_name def default_camera_sources(has_arm: bool, gripperless: bool) -> List[str]: @@ -223,25 +227,23 @@ def get_camera_sources(config_file_path: str, has_arm: bool) -> List[str]: return camera_sources -def spot_has_arm(config_file_path: str, spot_name: str) -> bool: +def spot_has_arm(config_file_path: str) -> bool: """Check if Spot has an arm querying the robot through SpotWrapper Args: config_file_path (str): Path to configuration yaml - spot_name (str): Name of spot Returns: bool: True if spot has an arm, False otherwise """ logger = logging.getLogger("spot_driver_launch") - username, password, hostname, port, certificate = get_login_parameters(config_file_path) + username, password, hostname, port, certificate, spot_name = get_login_parameters(config_file_path) spot_wrapper = SpotWrapper( username=username, password=password, hostname=hostname, port=port, cert_resource_glob=certificate, - robot_name=spot_name, logger=logger, ) return spot_wrapper.has_arm() diff --git a/spot_driver/spot_driver/spot_ros2.py b/spot_driver/spot_driver/spot_ros2.py index 09b2d9cd5..5856d711a 100644 --- a/spot_driver/spot_driver/spot_ros2.py +++ b/spot_driver/spot_driver/spot_ros2.py @@ -250,6 +250,7 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw self.declare_parameter("initialize_spot_cam", False) self.declare_parameter("spot_name", "") + self.declare_parameter("frame_prefix", "") self.declare_parameter("mock_enable", False) self.declare_parameter("gripperless", False) @@ -342,8 +343,9 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw # 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_param: Optional[str] = self.get_parameter("frame_prefix").value + frame_prefix = frame_prefix_param if frame_prefix_param is not None else "" + if frame_prefix_param is None and self.name is not None: frame_prefix = self.name + "/" self.frame_prefix: str = frame_prefix self.preferred_odom_frame: Parameter = self.declare_parameter( @@ -359,13 +361,17 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw 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) + preferred_odom_frame_param: str = self.preferred_odom_frame.value + if preferred_odom_frame_param not in preferred_odom_frame_references: + if self.frame_prefix + preferred_odom_frame_param in preferred_odom_frame_references: + preferred_odom_frame_param = self.frame_prefix + preferred_odom_frame_param + else: + error_msg = ( + f'The rosparam "preferred_odom_frame" should be one of {preferred_odom_frame_references}, got' + f' "{preferred_odom_frame_param}", which could not be composed into any valid option.' + ) + self.get_logger().error(error_msg) + raise ValueError(error_msg) self.tf_name_graph_nav_body: str = self.frame_prefix + "body" @@ -395,6 +401,7 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw hostname=self.ip, port=self.port, robot_name=self.name, + frame_prefix=self.frame_prefix, logger=self.wrapper_logger, start_estop=self.start_estop.value, estop_timeout=self.estop_timeout.value, diff --git a/spot_driver/src/api/default_image_client.cpp b/spot_driver/src/api/default_image_client.cpp index 8c32bc222..000896108 100644 --- a/spot_driver/src/api/default_image_client.cpp +++ b/spot_driver/src/api/default_image_client.cpp @@ -47,15 +47,13 @@ static const std::set kExcludedStaticTfFrames{ }; tl::expected toCameraInfoMsg( - const bosdyn::api::ImageResponse& image_response, const std::string& robot_name, + const bosdyn::api::ImageResponse& image_response, const std::string& frame_prefix, const google::protobuf::Duration& clock_skew) { sensor_msgs::msg::CameraInfo info_msg; info_msg.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB; info_msg.height = image_response.shot().image().rows(); info_msg.width = image_response.shot().image().cols(); - // Omit leading `/` from frame ID if robot_name is empty - info_msg.header.frame_id = - (robot_name.empty() ? "" : robot_name + "/") + image_response.shot().frame_name_image_sensor(); + info_msg.header.frame_id = frame_prefix + image_response.shot().frame_name_image_sensor(); info_msg.header.stamp = spot_ros2::robotTimeToLocalTime(image_response.shot().acquisition_time(), clock_skew); // We assume that the camera images have already been corrected for distortion, so the 5 distortion parameters are all @@ -92,17 +90,16 @@ tl::expected toCameraInfoMsg( return info_msg; } -std_msgs::msg::Header createImageHeader(const bosdyn::api::ImageCapture& image_capture, const std::string& robot_name, +std_msgs::msg::Header createImageHeader(const bosdyn::api::ImageCapture& image_capture, const std::string& frame_prefix, const google::protobuf::Duration& clock_skew) { std_msgs::msg::Header header; - // Omit leading `/` from frame ID if robot_name is empty - header.frame_id = (robot_name.empty() ? "" : robot_name + "/") + image_capture.frame_name_image_sensor(); + header.frame_id = frame_prefix + image_capture.frame_name_image_sensor(); header.stamp = spot_ros2::robotTimeToLocalTime(image_capture.acquisition_time(), clock_skew); return header; } tl::expected, std::string> getImageTransforms( - const bosdyn::api::ImageResponse& image_response, const std::string& robot_name, + const bosdyn::api::ImageResponse& image_response, const std::string& frame_prefix, const google::protobuf::Duration& clock_skew) { std::vector out; for (const auto& [child_frame_id, transform] : @@ -119,8 +116,7 @@ tl::expected, std::string> get (transform.parent_frame_name() == "arm0.link_wr1") ? "arm_link_wr1" : transform.parent_frame_name(); const auto tform_msg = spot_ros2::toTransformStamped( - transform.parent_tform_child(), robot_name.empty() ? parent_frame_id : (robot_name + "/" + parent_frame_id), - robot_name.empty() ? child_frame_id : (robot_name + "/" + child_frame_id), + transform.parent_tform_child(), frame_prefix + parent_frame_id, frame_prefix + child_frame_id, spot_ros2::robotTimeToLocalTime(image_response.shot().acquisition_time(), clock_skew)); out.push_back(tform_msg); @@ -129,7 +125,7 @@ tl::expected, std::string> get } tl::expected toCompressedImageMsg( - const bosdyn::api::ImageCapture& image_capture, const std::string& robot_name, + const bosdyn::api::ImageCapture& image_capture, const std::string& frame_prefix, const google::protobuf::Duration& clock_skew) { const auto& image = image_capture.image(); if (image.format() != bosdyn::api::Image_Format_FORMAT_JPEG) { @@ -139,7 +135,7 @@ tl::expected toCompressedImageMs auto data = image.data(); sensor_msgs::msg::CompressedImage compressed_image; - compressed_image.header = createImageHeader(image_capture, robot_name, clock_skew); + compressed_image.header = createImageHeader(image_capture, frame_prefix, clock_skew); compressed_image.format = "jpeg"; compressed_image.data.insert(compressed_image.data.begin(), data.begin(), data.end()); return compressed_image; @@ -149,8 +145,8 @@ tl::expected toCompressedImageMs namespace spot_ros2 { DefaultImageClient::DefaultImageClient(::bosdyn::client::ImageClient* image_client, - std::shared_ptr time_sync_api, const std::string& robot_name) - : image_client_{image_client}, time_sync_api_{time_sync_api}, robot_name_{robot_name} {} + std::shared_ptr time_sync_api, const std::string& frame_prefix) + : image_client_{image_client}, time_sync_api_{time_sync_api}, frame_prefix_{frame_prefix} {} tl::expected DefaultImageClient::getImages(::bosdyn::api::GetImageRequest request, bool uncompress_images, @@ -173,7 +169,7 @@ tl::expected DefaultImageClient::getImages(::bosdy const auto& image = image_response.shot().image(); auto data = image.data(); - const auto info_msg = toCameraInfoMsg(image_response, robot_name_, clock_skew_result.value()); + const auto info_msg = toCameraInfoMsg(image_response, frame_prefix_, clock_skew_result.value()); if (!info_msg) { return tl::make_unexpected("Failed to convert SDK image response to ROS CameraInfo message: " + info_msg.error()); } @@ -187,7 +183,7 @@ tl::expected DefaultImageClient::getImages(::bosdy if (image.format() == bosdyn::api::Image_Format_FORMAT_JPEG && publish_compressed_images) { const auto compressed_image_msg = - toCompressedImageMsg(image_response.shot(), robot_name_, clock_skew_result.value()); + toCompressedImageMsg(image_response.shot(), frame_prefix_, clock_skew_result.value()); if (!compressed_image_msg) { return tl::make_unexpected("Failed to convert SDK image response to ROS Image message: " + compressed_image_msg.error()); @@ -197,14 +193,14 @@ tl::expected DefaultImageClient::getImages(::bosdy } if (image.format() != bosdyn::api::Image_Format_FORMAT_JPEG || uncompress_images) { - const auto image_msg = getDecompressImageMsg(image_response.shot(), robot_name_, clock_skew_result.value()); + const auto image_msg = getDecompressImageMsg(image_response.shot(), frame_prefix_, clock_skew_result.value()); if (!image_msg) { return tl::make_unexpected("Failed to convert SDK image response to ROS Image message: " + image_msg.error()); } out.images_.try_emplace(get_source_name_result.value(), ImageWithCameraInfo{image_msg.value(), info_msg.value()}); } - const auto transforms_result = getImageTransforms(image_response, robot_name_, clock_skew_result.value()); + const auto transforms_result = getImageTransforms(image_response, frame_prefix_, clock_skew_result.value()); if (transforms_result.has_value()) { out.transforms_.insert(out.transforms_.end(), transforms_result.value().begin(), transforms_result.value().end()); } else { diff --git a/spot_driver/src/api/default_spot_api.cpp b/spot_driver/src/api/default_spot_api.cpp index 173fc222f..783a859a6 100644 --- a/spot_driver/src/api/default_spot_api.cpp +++ b/spot_driver/src/api/default_spot_api.cpp @@ -27,10 +27,10 @@ DefaultSpotApi::DefaultSpotApi(const std::string& sdk_client_name, const std::op } } -tl::expected DefaultSpotApi::createRobot(const std::string& robot_name, - const std::string& ip_address, - const std::optional& port) { - robot_name_ = robot_name; +tl::expected DefaultSpotApi::createRobot(const std::string& ip_address, + const std::optional& port, + const std::string& frame_prefix) { + frame_prefix_ = frame_prefix; auto create_robot_result = client_sdk_->CreateRobot(ip_address, ::bosdyn::client::USE_PROXY); if (!create_robot_result.status) { @@ -74,7 +74,7 @@ tl::expected DefaultSpotApi::authenticate(const std::string& } // TODO(jschornak-bdai): apply clock skew in the image publisher instead of in DefaultImageClient image_client_interface_ = - std::make_shared(image_client_result.response, time_sync_api_, robot_name_); + std::make_shared(image_client_result.response, time_sync_api_, frame_prefix_); const auto robot_state_result = robot_->EnsureServiceClient<::bosdyn::client::RobotStateClient>( ::bosdyn::client::RobotStateClient::GetDefaultServiceName()); diff --git a/spot_driver/src/conversions/decompress_images.cpp b/spot_driver/src/conversions/decompress_images.cpp index aa96feae7..79e2e111f 100644 --- a/spot_driver/src/conversions/decompress_images.cpp +++ b/spot_driver/src/conversions/decompress_images.cpp @@ -40,22 +40,21 @@ tl::expected getCvPixelFormat(const bosdyn::api::Image_PixelFo } } -std_msgs::msg::Header createImageHeader(const bosdyn::api::ImageCapture& image_capture, const std::string& robot_name, +std_msgs::msg::Header createImageHeader(const bosdyn::api::ImageCapture& image_capture, const std::string& frame_prefix, const google::protobuf::Duration& clock_skew) { std_msgs::msg::Header header; - // Omit leading `/` from frame ID if robot_name is empty - header.frame_id = (robot_name.empty() ? "" : robot_name + "/") + image_capture.frame_name_image_sensor(); + header.frame_id = frame_prefix + image_capture.frame_name_image_sensor(); header.stamp = spot_ros2::robotTimeToLocalTime(image_capture.acquisition_time(), clock_skew); return header; } tl::expected getDecompressImageMsg(const bosdyn::api::ImageCapture& image_capture, - const std::string& robot_name, + const std::string& frame_prefix, const google::protobuf::Duration& clock_skew) { const auto& image = image_capture.image(); auto data = image.data(); - const auto header = createImageHeader(image_capture, robot_name, clock_skew); + const auto header = createImageHeader(image_capture, frame_prefix, clock_skew); const auto pixel_format_cv = getCvPixelFormat(image.pixel_format()); if (!pixel_format_cv) { return tl::make_unexpected("Failed to determine pixel format: " + pixel_format_cv.error()); diff --git a/spot_driver/src/conversions/robot_state.cpp b/spot_driver/src/conversions/robot_state.cpp index 33da7f1d8..99054ae3c 100644 --- a/spot_driver/src/conversions/robot_state.cpp +++ b/spot_driver/src/conversions/robot_state.cpp @@ -15,6 +15,21 @@ namespace spot_ros2 { +std::string stripPrefix(const std::string& input, const std::string& prefix) { + const std::size_t prefix_index = input.find(prefix); + if (prefix_index == std::string::npos) { + // The input does not contain the prefix + return input; + } + if (prefix_index > 0) { + // The input does contain the prefix substring, but it does not begin at the start of the input. + // Return the unmodified input. + return input; + } + + return input.substr(prefix.size()); +} + spot_msgs::msg::BatteryStateArray getBatteryStates(const ::bosdyn::api::RobotState& robot_state, const google::protobuf::Duration& clock_skew) { spot_msgs::msg::BatteryStateArray battery_states; diff --git a/spot_driver/src/image_stitcher/image_stitcher.cpp b/spot_driver/src/image_stitcher/image_stitcher.cpp index 75c7771fc..d66c8d878 100644 --- a/spot_driver/src/image_stitcher/image_stitcher.cpp +++ b/spot_driver/src/image_stitcher/image_stitcher.cpp @@ -211,7 +211,11 @@ RclcppCameraHandle::RclcppCameraHandle(const std::shared_ptr& node image_transport_.advertiseCamera("virtual_camera/image", 1)}, // Remap to actual topic in launch file tf_broadcaster_{node} { const auto spot_name = node->declare_parameter("spot_name", ""); - const auto frame_prefix = spot_name.empty() ? "" : spot_name + "/"; + std::cout << "image_stitcher -> got spot_name: " << spot_name << std::endl; //FIXME(debug): this is only for debug + //FIXME(params-refactor): this should be refactored using the general parameter interface, we should NOT be modifying + // the string value explicitly like this ! + const auto frame_prefix = spot_name.empty() ? "" : spot_name + "_"; + std::cout << "image_stitcher -> using frame_prefix: " << frame_prefix << std::endl; //FIXME(debug): this is only for debug // Name of the frame to relate the virtual camera with respect to const auto body_frame_param = node->declare_parameter("body_frame", "body"); body_frame_ = frame_prefix + body_frame_param; diff --git a/spot_driver/src/images/spot_image_publisher_node.cpp b/spot_driver/src/images/spot_image_publisher_node.cpp index 3d6fbbef2..782f8cffc 100644 --- a/spot_driver/src/images/spot_image_publisher_node.cpp +++ b/spot_driver/src/images/spot_image_publisher_node.cpp @@ -55,12 +55,12 @@ void SpotImagePublisherNode::initialize(std::unique_ptr spot_api, const auto hostname = parameters->getHostname(); const auto port = parameters->getPort(); - const auto robot_name = parameters->getSpotName(); const auto username = parameters->getUsername(); const auto password = parameters->getPassword(); + const std::string frame_prefix = parameters->getFramePrefixWithDefaultFallback(); // create and authenticate robot - if (const auto create_robot_result = spot_api_->createRobot(robot_name, hostname, port); !create_robot_result) { + if (const auto create_robot_result = spot_api_->createRobot(hostname, port, frame_prefix); !create_robot_result) { const auto error_msg{std::string{"Failed to create interface to robot: "}.append(create_robot_result.error())}; logger->logError(error_msg); throw std::runtime_error(error_msg); diff --git a/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp b/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp index 1eaa024ba..9a62c03a9 100644 --- a/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp +++ b/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp @@ -27,6 +27,8 @@ constexpr auto kParameterNamePublishDepthImages = "publish_depth"; constexpr auto kParameterNamePublishDepthRegisteredImages = "publish_depth_registered"; constexpr auto kParameterPreferredOdomFrame = "preferred_odom_frame"; constexpr auto kParameterNameGripperless = "gripperless"; +constexpr auto kParameterSpotName = "spot_name"; +constexpr auto kParameterFramePrefix = "frame_prefix"; /** * @brief Get a rclcpp parameter. If the parameter has not been declared, declare it with the provided default value and @@ -187,6 +189,14 @@ std::string RclcppParameterInterface::getPreferredOdomFrame() const { return declareAndGetParameter(node_, kParameterPreferredOdomFrame, kDefaultPreferredOdomFrame); } +std::optional RclcppParameterInterface::getFramePrefix() const { + return declareAndGetParameter(node_, kParameterFramePrefix); +} + +std::optional RclcppParameterInterface::getFramePrefix() const { + return declareAndGetParameter(node_, kParameterFramePrefix); +} + bool RclcppParameterInterface::getGripperless() const { return declareAndGetParameter(node_, kParameterNameGripperless, kDefaultGripperless); } @@ -227,14 +237,23 @@ tl::expected, std::string> RclcppParameterInterf return spot_cameras_used; } -std::string RclcppParameterInterface::getSpotName() const { - // The spot_name parameter always matches the namespace of this node, minus the leading `/` character. +std::string RclcppParameterInterface::getSpotNameWithFallbackToNamespace() const { + // We use the explicit robot_name parameter value if provided. + // If the spot_name parameter is not found explicitly, then we expect it to always match the namespace of this node, + // minus the leading `/` character. try { - return std::string{node_->get_namespace()}.substr(1); + const std::optional spot_name = declareAndGetParameter(node_, kParameterSpotName); + return spot_name.value_or(std::string{node_->get_namespace()}.substr(1)); } catch (const std::out_of_range& e) { // get_namespace() should not return an empty string, but we handle this situation just in case. // Note that if no namespace was set when creating the node, get_namespace() will return `/`. return ""; } } + +std::string RclcppParameterInterface::getFramePrefixWithDefaultFallback() const { + const std::string robot_name = getSpotNameWithFallbackToNamespace(); + const std::optional frame_prefix = getFramePrefix(); + return frame_prefix.value_or(!robot_name.empty() ? robot_name + "/" : ""); +} } // namespace spot_ros2 diff --git a/spot_driver/src/kinematic/kinematic_node.cpp b/spot_driver/src/kinematic/kinematic_node.cpp index 56f852881..6a8432705 100644 --- a/spot_driver/src/kinematic/kinematic_node.cpp +++ b/spot_driver/src/kinematic/kinematic_node.cpp @@ -38,12 +38,12 @@ void KinematicNode::initialize(std::shared_ptr node, std::unique_p const auto hostname = parameter_interface->getHostname(); const auto port = parameter_interface->getPort(); - const auto robot_name = parameter_interface->getSpotName(); const auto username = parameter_interface->getUsername(); const auto password = parameter_interface->getPassword(); + const std::string frame_prefix = parameter_interface->getFramePrefixWithDefaultFallback(); // create and authenticate robot - if (const auto create_robot_result = spot_api_->createRobot(robot_name, hostname, port); !create_robot_result) { + if (const auto create_robot_result = spot_api_->createRobot(hostname, port, frame_prefix); !create_robot_result) { const auto error_msg{std::string{"Failed to create interface to robot: "}.append(create_robot_result.error())}; logger_interface->logError(error_msg); throw std::runtime_error(error_msg); diff --git a/spot_driver/src/object_sync/object_synchronizer.cpp b/spot_driver/src/object_sync/object_synchronizer.cpp index e4d4eb2da..fa26e3edb 100644 --- a/spot_driver/src/object_sync/object_synchronizer.cpp +++ b/spot_driver/src/object_sync/object_synchronizer.cpp @@ -96,28 +96,6 @@ inline const std::set> kSpotInternalFrames{ "arm0.link_wr1", }; -/** - * @brief Given an input string and a prefix string which is a substring starting at the beginning of the input string, - * return a new string which is the difference between the input string and the prefix string. - * @param input - * @param prefix - * @return A new string which is the difference between the input string and the prefix string. - */ -std::string stripPrefix(const std::string& input, const std::string& prefix) { - const std::size_t prefix_index = input.find(prefix); - if (prefix_index == std::string::npos) { - // The input does not contain the prefix - return input; - } - if (prefix_index > 0) { - // The input does contain the prefix substring, but it does not begin at the start of the input. - // Return the unmodified input. - return input; - } - - return input.substr(prefix.size()); -} - /** * @brief Create string messages corresponding to the values of the MutateWorldObjectResponse_Status enum. * @param status Enum to convert. @@ -329,13 +307,14 @@ 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_with_prefix_ = preferred_base_frame_.find('/') == std::string::npos - ? spot_name + "/" + preferred_base_frame_ - : preferred_base_frame_; + frame_prefix_ = parameter_interface_->getFramePrefixWithDefaultFallback(); + + // TODO(param-refactor): this was refactored to match the logic from state_publisher, + // find out if the stripping and re-adding wasn't done in the particular way intentionally for something + const std::string preferred_odom_frame = parameter_interface_->getPreferredOdomFrame(); + preferred_base_frame_ = stripPrefix(preferred_odom_frame, frame_prefix_); + preferred_base_frame_with_prefix_ = + preferred_odom_frame.find('/') == std::string::npos ? frame_prefix_ + preferred_odom_frame : preferred_odom_frame; // TODO(khughes): This is temporarily disabled to reduce driver's spew about TF extrapolation. // world_object_update_timer_->setTimer(kWorldObjectSyncPeriod, [this]() { diff --git a/spot_driver/src/object_sync/object_synchronizer_node.cpp b/spot_driver/src/object_sync/object_synchronizer_node.cpp index 3c9a67be1..baaf6b35e 100644 --- a/spot_driver/src/object_sync/object_synchronizer_node.cpp +++ b/spot_driver/src/object_sync/object_synchronizer_node.cpp @@ -67,12 +67,12 @@ void ObjectSynchronizerNode::initialize(std::unique_ptr spot_api, const auto address = parameter_interface->getHostname(); const auto port = parameter_interface->getPort(); - const auto robot_name = parameter_interface->getSpotName(); const auto username = parameter_interface->getUsername(); const auto password = parameter_interface->getPassword(); + const std::string frame_prefix = parameter_interface->getFramePrefixWithDefaultFallback(); // create and authenticate robot - if (const auto create_robot_result = spot_api_->createRobot(robot_name, address, port); !create_robot_result) { + if (const auto create_robot_result = spot_api_->createRobot(address, port, frame_prefix); !create_robot_result) { const auto error_msg{std::string{"Failed to create interface to robot: "}.append(create_robot_result.error())}; logger_interface->logError(error_msg); throw std::runtime_error(error_msg); diff --git a/spot_driver/src/robot_state/state_publisher.cpp b/spot_driver/src/robot_state/state_publisher.cpp index 176b764ca..801e47b63 100644 --- a/spot_driver/src/robot_state/state_publisher.cpp +++ b/spot_driver/src/robot_state/state_publisher.cpp @@ -30,11 +30,10 @@ StatePublisher::StatePublisher(const std::shared_ptr& stat logger_interface_{std::move(logger_interface)}, tf_broadcaster_interface_{std::move(tf_broadcaster_interface)}, timer_interface_{std::move(timer_interface)} { - const auto spot_name = parameter_interface_->getSpotName(); - frame_prefix_ = spot_name.empty() ? "" : spot_name + "/"; + frame_prefix_ = parameter_interface_->getFramePrefixWithDefaultFallback(); const auto preferred_odom_frame = parameter_interface_->getPreferredOdomFrame(); - is_using_vision_ = preferred_odom_frame == "vision"; + is_using_vision_ = stripPrefix(preferred_odom_frame, frame_prefix_) == "vision"; full_odom_frame_id_ = preferred_odom_frame.find('/') == std::string::npos ? frame_prefix_ + preferred_odom_frame : preferred_odom_frame; diff --git a/spot_driver/src/robot_state/state_publisher_node.cpp b/spot_driver/src/robot_state/state_publisher_node.cpp index 6ecce6b70..637b79ac4 100644 --- a/spot_driver/src/robot_state/state_publisher_node.cpp +++ b/spot_driver/src/robot_state/state_publisher_node.cpp @@ -58,12 +58,12 @@ void StatePublisherNode::initialize(std::unique_ptr spot_api, const auto hostname = parameter_interface->getHostname(); const auto port = parameter_interface->getPort(); - const auto robot_name = parameter_interface->getSpotName(); const auto username = parameter_interface->getUsername(); const auto password = parameter_interface->getPassword(); + const std::string frame_prefix = parameter_interface->getFramePrefixWithDefaultFallback(); // create and authenticate robot - if (const auto create_robot_result = spot_api_->createRobot(robot_name, hostname, port); !create_robot_result) { + if (const auto create_robot_result = spot_api_->createRobot(hostname, port, frame_prefix); !create_robot_result) { const auto error_msg{std::string{"Failed to create interface to robot: "}.append(create_robot_result.error())}; logger_interface->logError(error_msg); throw std::runtime_error(error_msg); 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..0760c82a2 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,7 +38,9 @@ class FakeParameterInterface : public ParameterInterfaceBase { std::string getPreferredOdomFrame() const override { return "odom"; } - std::string getSpotName() const override { return spot_name; } + std::optional getFramePrefix() const override { return std::nullopt; } + + std::string getSpotNameWithFallbackToNamespace() const override { return spot_name; } bool getGripperless() const override { return gripperless; } @@ -56,6 +58,8 @@ class FakeParameterInterface : public ParameterInterfaceBase { return getDefaultCamerasUsed(has_arm, gripperless); } + std::string getFramePrefixWithDefaultFallback() const override { return spot_name + "/"; } + static constexpr auto kExampleHostname{"192.168.0.10"}; static constexpr auto kExampleUsername{"spot_user"}; static constexpr auto kExamplePassword{"hunter2"}; diff --git a/spot_driver/test/include/spot_driver/mock/mock_spot_api.hpp b/spot_driver/test/include/spot_driver/mock/mock_spot_api.hpp index 74c8c2e5c..810330157 100644 --- a/spot_driver/test/include/spot_driver/mock/mock_spot_api.hpp +++ b/spot_driver/test/include/spot_driver/mock/mock_spot_api.hpp @@ -19,7 +19,7 @@ namespace spot_ros2::test { class MockSpotApi : public SpotApi { public: MOCK_METHOD((tl::expected), createRobot, - (const std::string&, const std::string&, const std::optional&), (override)); + (const std::string&, const std::optional&, const std::string&), (override)); MOCK_METHOD((tl::expected), authenticate, (const std::string&, const std::string&), (override)); MOCK_METHOD((tl::expected), hasArm, (), (const, override)); MOCK_METHOD(std::shared_ptr, kinematicInterface, (), (const, override)); diff --git a/spot_driver/test/src/test_parameter_interface.cpp b/spot_driver/test/src/test_parameter_interface.cpp index 74546d22a..59e5a80a0 100644 --- a/spot_driver/test/src/test_parameter_interface.cpp +++ b/spot_driver/test/src/test_parameter_interface.cpp @@ -119,7 +119,7 @@ TEST_F(RclcppParameterInterfaceTest, GetSpotNameWithNamespace) { // WHEN we call getSpotName // THEN the parameter interface returns the namespace of the node - EXPECT_THAT(parameter_interface.getSpotName(), StrEq(kNamespace)); + EXPECT_THAT(parameter_interface.getSpotNameWithFallbackToNamespace(), StrEq(kNamespace)); } TEST_F(RclcppParameterInterfaceTest, GetSpotNameWithEmptyNamespace) { @@ -130,7 +130,7 @@ TEST_F(RclcppParameterInterfaceTest, GetSpotNameWithEmptyNamespace) { // WHEN we call getSpotName // THEN the parameter interface returns an empty string - EXPECT_THAT(parameter_interface.getSpotName(), IsEmpty()); + EXPECT_THAT(parameter_interface.getSpotNameWithFallbackToNamespace(), IsEmpty()); } TEST_F(RclcppParameterInterfaceTest, GetSpotNameWithDefaultNamespace) { @@ -141,7 +141,7 @@ TEST_F(RclcppParameterInterfaceTest, GetSpotNameWithDefaultNamespace) { // WHEN we call getSpotName // THEN the parameter interface returns an empty string - EXPECT_THAT(parameter_interface.getSpotName(), IsEmpty()); + EXPECT_THAT(parameter_interface.getSpotNameWithFallbackToNamespace(), IsEmpty()); } TEST_F(RclcppParameterInterfaceEnvVarTest, GetSpotConfigFromEnvVars) { From 9e07521e4af058850ca748e8947ee888b59550e1 Mon Sep 17 00:00:00 2001 From: Imaniac230 <44968160+Imaniac230@users.noreply.github.com> Date: Tue, 15 Oct 2024 20:12:35 +0200 Subject: [PATCH 02/13] feat: Refactor image stitcher to utilize the general parameter interface with robot name and frame prefix. * created the initialize() method to handle all interface initializations * added a frame_prefix argument to the RclcppCameraHandle class constructor - should be acquired from the parameter interface * wrapped the internal stitcher class into a unique pointer --- .../image_stitcher/image_stitcher.hpp | 7 ++++-- .../image_stitcher/image_stitcher_node.hpp | 6 ++++- .../launch/spot_image_publishers.launch.py | 3 +-- .../src/image_stitcher/image_stitcher.cpp | 14 ++++------- .../image_stitcher/image_stitcher_node.cpp | 24 +++++++++++++++---- 5 files changed, 35 insertions(+), 19 deletions(-) diff --git a/spot_driver/include/spot_driver/image_stitcher/image_stitcher.hpp b/spot_driver/include/spot_driver/image_stitcher/image_stitcher.hpp index fdcf0a415..dcae49641 100644 --- a/spot_driver/include/spot_driver/image_stitcher/image_stitcher.hpp +++ b/spot_driver/include/spot_driver/image_stitcher/image_stitcher.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -101,7 +102,7 @@ class CameraHandleBase { class RclcppCameraHandle : public CameraHandleBase { public: - explicit RclcppCameraHandle(const std::shared_ptr& node); + explicit RclcppCameraHandle(const std::shared_ptr& node, const std::string& frame_prefix); void publish(const Image& image, const CameraInfo& info) const override; void broadcast(const Transform& tf, const Time& stamp) override; @@ -165,7 +166,8 @@ class ImageStitcher { public: ImageStitcher(std::unique_ptr synchronizer, std::unique_ptr tf_listener, std::unique_ptr camera_handle, - std::unique_ptr logger); + std::unique_ptr logger, + std::unique_ptr parameter_interface); private: void callback(const std::shared_ptr&, const std::shared_ptr&, @@ -175,6 +177,7 @@ class ImageStitcher { std::unique_ptr tf_listener_; std::unique_ptr camera_handle_; std::unique_ptr logger_; + std::unique_ptr parameter_interface_; std::optional camera_; }; diff --git a/spot_driver/include/spot_driver/image_stitcher/image_stitcher_node.hpp b/spot_driver/include/spot_driver/image_stitcher/image_stitcher_node.hpp index e0ca010ae..9608de1c5 100644 --- a/spot_driver/include/spot_driver/image_stitcher/image_stitcher_node.hpp +++ b/spot_driver/include/spot_driver/image_stitcher/image_stitcher_node.hpp @@ -15,7 +15,11 @@ class ImageStitcherNode { std::shared_ptr get_node_base_interface(); private: + void initialize(std::unique_ptr synchronizer, + std::unique_ptr tf_listener, std::unique_ptr logger, + std::unique_ptr parameter_interface); + std::shared_ptr node_; - ImageStitcher stitcher_; + std::unique_ptr stitcher_; }; } // namespace spot_ros2 diff --git a/spot_driver/launch/spot_image_publishers.launch.py b/spot_driver/launch/spot_image_publishers.launch.py index 0bb2febfa..1631bdec8 100644 --- a/spot_driver/launch/spot_image_publishers.launch.py +++ b/spot_driver/launch/spot_image_publishers.launch.py @@ -14,8 +14,8 @@ DepthRegisteredMode, declare_image_publisher_args, get_camera_sources, - spot_has_arm, get_ros_param_dict, + spot_has_arm, ) @@ -163,7 +163,6 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: if "frontleft" in camera_sources and "frontright" in camera_sources: virtual_camera_frame = "frontmiddle_virtual" stitcher_params = { - "spot_name": spot_name, "body_frame": "body", "virtual_camera_frame": virtual_camera_frame, } diff --git a/spot_driver/src/image_stitcher/image_stitcher.cpp b/spot_driver/src/image_stitcher/image_stitcher.cpp index d66c8d878..1bff56c1b 100644 --- a/spot_driver/src/image_stitcher/image_stitcher.cpp +++ b/spot_driver/src/image_stitcher/image_stitcher.cpp @@ -205,17 +205,11 @@ void RclcppCameraSynchronizer::registerCallback(const DualImageCallbackFn& fn) { std::bind(fn, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4)); } -RclcppCameraHandle::RclcppCameraHandle(const std::shared_ptr& node) +RclcppCameraHandle::RclcppCameraHandle(const std::shared_ptr& node, const std::string& frame_prefix) : image_transport_{node}, camera_publisher_{ image_transport_.advertiseCamera("virtual_camera/image", 1)}, // Remap to actual topic in launch file tf_broadcaster_{node} { - const auto spot_name = node->declare_parameter("spot_name", ""); - std::cout << "image_stitcher -> got spot_name: " << spot_name << std::endl; //FIXME(debug): this is only for debug - //FIXME(params-refactor): this should be refactored using the general parameter interface, we should NOT be modifying - // the string value explicitly like this ! - const auto frame_prefix = spot_name.empty() ? "" : spot_name + "_"; - std::cout << "image_stitcher -> using frame_prefix: " << frame_prefix << std::endl; //FIXME(debug): this is only for debug // Name of the frame to relate the virtual camera with respect to const auto body_frame_param = node->declare_parameter("body_frame", "body"); body_frame_ = frame_prefix + body_frame_param; @@ -379,11 +373,13 @@ Transform MiddleCamera::getTransform() { ImageStitcher::ImageStitcher(std::unique_ptr synchronizer, std::unique_ptr tf_listener, std::unique_ptr camera_handle, - std::unique_ptr logger) + std::unique_ptr logger, + std::unique_ptr parameter_interface) : synchronizer_{std::move(synchronizer)}, tf_listener_{std::move(tf_listener)}, camera_handle_{std::move(camera_handle)}, - logger_{std::move(logger)} { + logger_{std::move(logger)}, + parameter_interface_{std::move(parameter_interface)} { synchronizer_->registerCallback( [this](const std::shared_ptr& image_left, const std::shared_ptr& info_left, const std::shared_ptr& image_right, const std::shared_ptr& info_right) { diff --git a/spot_driver/src/image_stitcher/image_stitcher_node.cpp b/spot_driver/src/image_stitcher/image_stitcher_node.cpp index 0eacba4c5..3cb106db3 100644 --- a/spot_driver/src/image_stitcher/image_stitcher_node.cpp +++ b/spot_driver/src/image_stitcher/image_stitcher_node.cpp @@ -4,15 +4,29 @@ #include #include +#include #include #include namespace spot_ros2 { -ImageStitcherNode::ImageStitcherNode(const rclcpp::NodeOptions& options) - : node_{std::make_shared("image_stitcher", options)}, - stitcher_{std::make_unique(node_), std::make_unique(node_), - std::make_unique(node_), - std::make_unique(node_->get_logger())} {} +ImageStitcherNode::ImageStitcherNode(const rclcpp::NodeOptions& options) { + node_ = std::make_shared("image_stitcher", options); + + initialize(std::make_unique(node_), std::make_unique(node_), + std::make_unique(node_->get_logger()), + std::make_unique(node_)); +} + +void ImageStitcherNode::initialize(std::unique_ptr synchronizer, + std::unique_ptr tf_listener, + std::unique_ptr logger, + std::unique_ptr parameter_interface) { + const std::string frame_prefix = parameter_interface->getFramePrefixWithDefaultFallback(); + + stitcher_ = std::make_unique(std::move(synchronizer), std::move(tf_listener), + std::make_unique(node_, frame_prefix), + std::move(logger), std::move(parameter_interface)); +} std::shared_ptr ImageStitcherNode::get_node_base_interface() { return node_->get_node_base_interface(); From 14a7b9d1d3b72cba02b958bf7e804e2e489578e5 Mon Sep 17 00:00:00 2001 From: Imaniac230 <44968160+Imaniac230@users.noreply.github.com> Date: Sun, 27 Oct 2024 22:44:12 +0100 Subject: [PATCH 03/13] fix: Implement tests for the added parameter interface methods, add missing preferred_odom_frame to general spot config tests. * default behavior without spot_name and frame_prefix parameters should still be preserved * verifying that the spot_name parameter is correctly used when specified * verifying that the frame_prefix parameter is correctly used when specified * verifying that all fallback behaviors work as expected --- .../test/src/test_parameter_interface.cpp | 107 +++++++++++++++++- 1 file changed, 104 insertions(+), 3 deletions(-) diff --git a/spot_driver/test/src/test_parameter_interface.cpp b/spot_driver/test/src/test_parameter_interface.cpp index 59e5a80a0..ab6bc8710 100644 --- a/spot_driver/test/src/test_parameter_interface.cpp +++ b/spot_driver/test/src/test_parameter_interface.cpp @@ -22,6 +22,10 @@ using ::testing::UnorderedElementsAre; constexpr auto kNodeName = "my_node_name"; constexpr auto kNamespace = "my_namespace"; +static constexpr auto kSpotName = "my_spot_name"; +static constexpr auto kFramePrefix = "some_random_prefix_/_"; +static constexpr auto kFramePrefixSeparator = "/"; + constexpr auto kEnvVarNameHostname = "SPOT_IP"; constexpr auto kEnvVarNamePort = "SPOT_PORT"; constexpr auto kEnvVarNameCertificate = "SPOT_CERTIFICATE"; @@ -117,7 +121,7 @@ TEST_F(RclcppParameterInterfaceTest, GetSpotNameWithNamespace) { // GIVEN we create a RclcppParameterInterface using this node RclcppParameterInterface parameter_interface{node}; - // WHEN we call getSpotName + // WHEN we call getSpotNameWithFallbackToNamespace // THEN the parameter interface returns the namespace of the node EXPECT_THAT(parameter_interface.getSpotNameWithFallbackToNamespace(), StrEq(kNamespace)); } @@ -128,7 +132,7 @@ TEST_F(RclcppParameterInterfaceTest, GetSpotNameWithEmptyNamespace) { // GIVEN we create a RclcppParameterInterface using this node RclcppParameterInterface parameter_interface{node}; - // WHEN we call getSpotName + // WHEN we call getSpotNameWithFallbackToNamespace // THEN the parameter interface returns an empty string EXPECT_THAT(parameter_interface.getSpotNameWithFallbackToNamespace(), IsEmpty()); } @@ -139,11 +143,29 @@ TEST_F(RclcppParameterInterfaceTest, GetSpotNameWithDefaultNamespace) { // GIVEN we create a RclcppParameterInterface using this node RclcppParameterInterface parameter_interface{node}; - // WHEN we call getSpotName + // WHEN we call getSpotNameWithFallbackToNamespace // THEN the parameter interface returns an empty string EXPECT_THAT(parameter_interface.getSpotNameWithFallbackToNamespace(), IsEmpty()); } +TEST_F(RclcppParameterInterfaceTest, GetSpotNameFromExplicitParameter) { + // GIVEN we create rclcpp nodes with and without a specific namespace + const auto node = std::make_shared(kNodeName); + const auto namespaced_node = std::make_shared(kNodeName, kNamespace); + // GIVEN we set the spot name config parameter to an explicit value + node->declare_parameter("spot_name", kSpotName); + namespaced_node->declare_parameter("spot_name", kSpotName); + + // GIVEN we create a RclcppParameterInterface using these nodes + RclcppParameterInterface parameter_interface_a{node}; + RclcppParameterInterface parameter_interface_b{namespaced_node}; + + // WHEN we call getSpotNameWithFallbackToNamespace + // THEN the parameter interface returns the explicit spot name in both cases + EXPECT_THAT(parameter_interface_a.getSpotNameWithFallbackToNamespace(), StrEq(kSpotName)); + EXPECT_THAT(parameter_interface_b.getSpotNameWithFallbackToNamespace(), StrEq(kSpotName)); +} + TEST_F(RclcppParameterInterfaceEnvVarTest, GetSpotConfigFromEnvVars) { // GIVEN we declare environment variables for Spot's hostname, username, and password constexpr auto hostname_env_var = "10.0.20.5"; @@ -195,6 +217,9 @@ 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 preferred_odom_frame_parameter = "vision"; + node_->declare_parameter("preferred_odom_frame", preferred_odom_frame_parameter); + node_->declare_parameter("frame_prefix", kFramePrefix); // GIVEN we create a RclcppParameterInterface using the node RclcppParameterInterface parameter_interface{node_}; @@ -213,6 +238,8 @@ 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.getPreferredOdomFrame(), StrEq(preferred_odom_frame_parameter)); + EXPECT_THAT(parameter_interface.getFramePrefix(), Optional(kFramePrefix)); } TEST_F(RclcppParameterInterfaceEnvVarTest, GetSpotConfigEnvVarsOverruleParameters) { @@ -272,6 +299,8 @@ 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.getPreferredOdomFrame(), StrEq("odom")); + EXPECT_THAT(parameter_interface.getFramePrefix(), Eq(std::nullopt)); } TEST_F(RclcppParameterInterfaceEnvVarTest, GetCamerasUsedDefaultWithArm) { @@ -433,4 +462,76 @@ TEST_F(RclcppParameterInterfaceEnvVarTest, GetSelectedCamerasUsedGripperless) { EXPECT_THAT(cameras_used_arm.error(), StrEq("Cannot add SpotCamera 'hand', the robot is gripperless!")); } + +TEST_F(RclcppParameterInterfaceTest, GetFramePrefixFromNamespaceFallback) { + // GIVEN we create rclcpp nodes with and without a specific namespace + const auto node = std::make_shared(kNodeName); + const auto namespaced_node = std::make_shared(kNodeName, kNamespace); + // GIVEN we create a RclcppParameterInterface using these nodes + RclcppParameterInterface parameter_interface_a{node}; + RclcppParameterInterface parameter_interface_b{namespaced_node}; + + // WHEN we call getFramePrefixWithDefaultFallback + // THEN the parameter interface returns a frame prefix based on the nodes' namespaces + const std::string expected_prefix = std::string(kNamespace) + kFramePrefixSeparator; + EXPECT_THAT(parameter_interface_a.getFramePrefixWithDefaultFallback(), StrEq("")); + EXPECT_THAT(parameter_interface_b.getFramePrefixWithDefaultFallback(), StrEq(expected_prefix)); +} + +TEST_F(RclcppParameterInterfaceTest, GetFramePrefixFromSpotNameFallback) { + // GIVEN we create rclcpp nodes with and without a specific namespace + const auto node = std::make_shared(kNodeName); + const auto namespaced_node = std::make_shared(kNodeName, kNamespace); + // GIVEN we set the spot name config parameter to an explicit value + node->declare_parameter("spot_name", kSpotName); + namespaced_node->declare_parameter("spot_name", kSpotName); + + // GIVEN we create a RclcppParameterInterface using these nodes + RclcppParameterInterface parameter_interface_a{node}; + RclcppParameterInterface parameter_interface_b{namespaced_node}; + + // WHEN we call getFramePrefixWithDefaultFallback + // THEN the parameter interface returns a frame prefix based on the spot name in both cases + const std::string expected_prefix = std::string(kSpotName) + kFramePrefixSeparator; + EXPECT_THAT(parameter_interface_a.getFramePrefixWithDefaultFallback(), StrEq(expected_prefix)); + EXPECT_THAT(parameter_interface_b.getFramePrefixWithDefaultFallback(), StrEq(expected_prefix)); +} + +TEST_F(RclcppParameterInterfaceTest, GetFramePrefixFromExplicitParameter) { + const auto verifyExpectedFramePrefix = [](std::shared_ptr node, + std::shared_ptr namespaced_node) -> void { + // GIVEN we create a RclcppParameterInterface using these nodes + RclcppParameterInterface parameter_interface_a{node}; + RclcppParameterInterface parameter_interface_b{namespaced_node}; + + // WHEN we call getFramePrefixWithDefaultFallback + // THEN the parameter interface returns the explicit frame prefix in both cases + EXPECT_THAT(parameter_interface_a.getFramePrefixWithDefaultFallback(), StrEq(kFramePrefix)); + EXPECT_THAT(parameter_interface_b.getFramePrefixWithDefaultFallback(), StrEq(kFramePrefix)); + }; + + // Set up first test scenario. + // GIVEN we create rclcpp nodes with and without a specific namespace + auto node = std::make_shared(kNodeName); + auto namespaced_node = std::make_shared(kNodeName, kNamespace); + // GIVEN we only set the frame prefix config parameter to an explicit value, without any spot name + node->declare_parameter("frame_prefix", kFramePrefix); + namespaced_node->declare_parameter("frame_prefix", kFramePrefix); + // Finish first test scenario. + verifyExpectedFramePrefix(node, namespaced_node); + + // Set up second test scenario. + // NOTE: We're creating new nodes for the second test scenario, since we can't undeclare statically typed parameters. + node.reset(); + namespaced_node.reset(); + node = std::make_shared(kNodeName); + namespaced_node = std::make_shared(kNodeName, kNamespace); + // GIVEN we set both, the spot name and frame prefix, config parameters to explicit values + node->declare_parameter("spot_name", kSpotName); + namespaced_node->declare_parameter("spot_name", kSpotName); + node->declare_parameter("frame_prefix", kFramePrefix); + namespaced_node->declare_parameter("frame_prefix", kFramePrefix); + // Finish second test scenario. + verifyExpectedFramePrefix(node, namespaced_node); +} } // namespace spot_ros2::test From c53e5b11d35c5097798aacb9f8f6b5f8ac66a36a Mon Sep 17 00:00:00 2001 From: Imaniac230 <44968160+Imaniac230@users.noreply.github.com> Date: Tue, 29 Oct 2024 10:39:13 +0100 Subject: [PATCH 04/13] feat: Maintain backwards compatibility for spot_name and tf_prefix launch file arguments. * implemented a parameter substitution launch helper util, which passes the given launch arguments into the used parameter configuration * substitution values are type-constrained explicitly to LaunchSubstitution * all type handling is offloaded to the internal ros substitution mechanisms - is usable for any ros parameter type * launch parameters will always take precedence over config file parameters * updated rviz launch to use the prefix argument correctly * updated relevant README files * minor fix - redefined lambda in parameter interface test as a static expression --- README.md | 2 +- spot_driver/README.md | 1 + spot_driver/launch/rviz.launch.py | 14 ++++-- spot_driver/launch/spot_driver.launch.py | 49 ++++++++++++++----- .../launch/spot_image_publishers.launch.py | 32 ++++++++++-- .../spot_driver/launch/spot_launch_helpers.py | 32 +++++++++++- .../test/src/test_parameter_interface.cpp | 4 +- 7 files changed, 110 insertions(+), 24 deletions(-) diff --git a/README.md b/README.md index a16733703..d314f27c1 100644 --- a/README.md +++ b/README.md @@ -77,7 +77,7 @@ The image does not have the `proto2ros_tests` package built. You'll need to buil The Spot driver contains all of the necessary topics, services, and actions for controlling Spot over ROS 2. To launch the driver, run: ``` -ros2 launch spot_driver spot_driver.launch.py [config_file:=] [spot_name:=] [publish_point_clouds:=] [launch_rviz:=] [uncompress_images:=] [publish_compressed_images:=] +ros2 launch spot_driver spot_driver.launch.py [config_file:=] [spot_name:=] [publish_point_clouds:=] [launch_rviz:=] [uncompress_images:=] [publish_compressed_images:=] [tf_prefix:=] ``` ## Configuration diff --git a/spot_driver/README.md b/spot_driver/README.md index 8d0cb5690..bd5d261f9 100644 --- a/spot_driver/README.md +++ b/spot_driver/README.md @@ -10,6 +10,7 @@ The primary launchfile in this package is `spot_driver.launch.py`. Once you've b * To launch the process within a namespace, add the launch argument `spot_name:={name}` * To visualize Spot in RViz, add the launch argument `launch_rviz:=True`. This will automatically generate the appropriate RViz config file for your robot's name using `rviz.launch.py`. * To publish point clouds, add the launch argument `publish_point_clouds:=True`. This is disabled by default. +* To use a custom prefix for all frames in the TF tree, add the launch argument `tf_prefix:={prefix}`. Otherwise, the spot_name/namespace will be used to construct the prefix automatically. The Spot driver contains both Python and C++ nodes. Spot's Python SDK is used for many operations. For example, `spot_ros2` is the primary node that connects with Spot and creates the ROS 2 action servers and services. Spot's C++ SDK is used in nodes like `spot_image_publisher_node` to retrieve images from Spot's RGB and depth cameras at close to their native refresh rate of 15 Hz -- something that is not possible using the Python SDK. diff --git a/spot_driver/launch/rviz.launch.py b/spot_driver/launch/rviz.launch.py index 8d2b92a94..983b9a1dc 100644 --- a/spot_driver/launch/rviz.launch.py +++ b/spot_driver/launch/rviz.launch.py @@ -17,7 +17,7 @@ THIS_PACKAGE = "spot_driver" -def create_rviz_config(robot_name: str) -> None: +def create_rviz_config(robot_name: str, tf_prefix: str) -> None: """Writes a configuration file for rviz to visualize a single spot robot""" RVIZ_TEMPLATE_FILENAME = os.path.join(get_package_share_directory(THIS_PACKAGE), "rviz", "spot_template.yaml") @@ -28,7 +28,7 @@ def create_rviz_config(robot_name: str) -> None: if robot_name: # replace fixed frame with robot body frame - config["Visualization Manager"]["Global Options"]["Fixed Frame"] = f"{robot_name}/vision" + config["Visualization Manager"]["Global Options"]["Fixed Frame"] = f"{tf_prefix}vision" # Add robot models for each robot for display in config["Visualization Manager"]["Displays"]: if "RobotModel" in display["Class"]: @@ -45,10 +45,11 @@ def create_rviz_config(robot_name: str) -> None: def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: rviz_config_file = LaunchConfiguration("rviz_config_file").perform(context) spot_name = LaunchConfiguration("spot_name").perform(context) + tf_prefix = LaunchConfiguration("tf_prefix").perform(context) # It looks like passing an optional of value "None" gets converted to a string of value "None" if not rviz_config_file or rviz_config_file == "None": - create_rviz_config(spot_name) + create_rviz_config(spot_name, tf_prefix) rviz_config_file = PathJoinSubstitution([FindPackageShare(THIS_PACKAGE), "rviz", "spot.rviz"]).perform(context) rviz = launch_ros.actions.Node( @@ -72,6 +73,13 @@ def generate_launch_description() -> launch.LaunchDescription: description="RViz config file", ) ) + launch_args.append( + DeclareLaunchArgument( + "tf_prefix", + default_value="", + description="apply namespace prefix to robot links and joints", + ) + ) launch_args.append(DeclareLaunchArgument("spot_name", default_value="", description="Name of Spot")) ld = launch.LaunchDescription(launch_args) diff --git a/spot_driver/launch/spot_driver.launch.py b/spot_driver/launch/spot_driver.launch.py index 9badab344..f025e6f79 100644 --- a/spot_driver/launch/spot_driver.launch.py +++ b/spot_driver/launch/spot_driver.launch.py @@ -1,6 +1,7 @@ # Copyright (c) 2023-2024 Boston Dynamics AI Institute LLC. All rights reserved. import os +from typing import Optional, Union import launch import launch_ros @@ -14,8 +15,8 @@ from spot_driver.launch.spot_launch_helpers import ( IMAGE_PUBLISHER_ARGS, declare_image_publisher_args, - get_ros_param_dict, spot_has_arm, + substitute_launch_parameters, ) THIS_PACKAGE = "spot_driver" @@ -24,6 +25,8 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: config_file = LaunchConfiguration("config_file") launch_rviz = LaunchConfiguration("launch_rviz") + spot_name_arg = LaunchConfiguration("spot_name") + tf_prefix_arg = LaunchConfiguration("tf_prefix") rviz_config_file = LaunchConfiguration("rviz_config_file").perform(context) mock_enable = IfCondition(LaunchConfiguration("mock_enable", default="False")).evaluate(context) @@ -32,9 +35,23 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: if (config_file_path != "") and (not os.path.isfile(config_file_path)): raise FileNotFoundError("Configuration file '{}' does not exist!".format(config_file_path)) - ros_params = get_ros_param_dict(config_file_path) - spot_name: str = ros_params["spot_name"] if "spot_name" in ros_params else "" - tf_prefix: str = ros_params["frame_prefix"] if "frame_prefix" in ros_params else "" + substitutions = { + "spot_name": spot_name_arg, + "frame_prefix": tf_prefix_arg, + } + configured_params = substitute_launch_parameters(config_file_path, substitutions, context) + spot_name: Optional[Union[str, LaunchConfiguration]] = ( + configured_params["spot_name"] if "spot_name" in configured_params else None + ) + tf_prefix: Optional[Union[str, LaunchConfiguration]] = ( + configured_params["frame_prefix"] if "frame_prefix" in configured_params else None + ) + if tf_prefix is None and spot_name is not None: + tf_prefix = (spot_name if isinstance(spot_name, str) else spot_name.perform(context)) + "/" + if tf_prefix is None: + tf_prefix = "" + if spot_name is None: + spot_name = "" if mock_enable: mock_has_arm = IfCondition(LaunchConfiguration("mock_has_arm")).evaluate(context) @@ -60,7 +77,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: executable="spot_ros2", name="spot_ros2", output="screen", - parameters=[config_file, spot_driver_params], + parameters=[configured_params, spot_driver_params], namespace=spot_name, ) ld.add_action(spot_driver_node) @@ -69,7 +86,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: package="spot_driver", executable="spot_inverse_kinematics_node", output="screen", - parameters=[config_file], + parameters=[configured_params], namespace=spot_name, ) ld.add_action(kinematic_node) @@ -78,14 +95,11 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: package="spot_driver", executable="object_synchronizer_node", output="screen", - parameters=[config_file], + parameters=[configured_params], namespace=spot_name, ) ld.add_action(object_sync_node) - if not tf_prefix and spot_name: - tf_prefix = spot_name + "/" - robot_description = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), @@ -114,7 +128,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: package="spot_driver", executable="state_publisher_node", output="screen", - parameters=[config_file], + parameters=[configured_params], namespace=spot_name, ) ld.add_action(spot_robot_state_publisher) @@ -133,6 +147,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: launch_arguments={ "spot_name": spot_name, "rviz_config_file": rviz_config_file, + "tf_prefix": tf_prefix, }.items(), condition=IfCondition(launch_rviz), ) @@ -140,7 +155,9 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: spot_image_publishers = IncludeLaunchDescription( PythonLaunchDescriptionSource([FindPackageShare(THIS_PACKAGE), "/launch", "/spot_image_publishers.launch.py"]), - launch_arguments={key: LaunchConfiguration(key) for key in ["config_file"] + IMAGE_PUBLISHER_ARGS}.items(), + launch_arguments={ + key: LaunchConfiguration(key) for key in ["config_file", "tf_prefix", "spot_name"] + IMAGE_PUBLISHER_ARGS + }.items(), condition=IfCondition(LaunchConfiguration("launch_image_publishers")), ) ld.add_action(spot_image_publishers) @@ -156,6 +173,13 @@ def generate_launch_description() -> launch.LaunchDescription: description="Path to configuration file for the driver.", ) ) + launch_args.append( + DeclareLaunchArgument( + "tf_prefix", + default_value="", + description="apply namespace prefix to robot links and joints", + ) + ) launch_args.append( DeclareLaunchArgument( "launch_rviz", @@ -180,6 +204,7 @@ def generate_launch_description() -> launch.LaunchDescription: ) ) launch_args += declare_image_publisher_args() + launch_args.append(DeclareLaunchArgument("spot_name", default_value="", description="Name of Spot")) ld = launch.LaunchDescription(launch_args) diff --git a/spot_driver/launch/spot_image_publishers.launch.py b/spot_driver/launch/spot_image_publishers.launch.py index 1631bdec8..01417c95c 100644 --- a/spot_driver/launch/spot_image_publishers.launch.py +++ b/spot_driver/launch/spot_image_publishers.launch.py @@ -14,8 +14,8 @@ DepthRegisteredMode, declare_image_publisher_args, get_camera_sources, - get_ros_param_dict, spot_has_arm, + substitute_launch_parameters, ) @@ -93,6 +93,8 @@ def create_point_cloud_nodelets( def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: config_file = LaunchConfiguration("config_file") + spot_name_arg = LaunchConfiguration("spot_name") + tf_prefix_arg = LaunchConfiguration("tf_prefix") depth_registered_mode_config = LaunchConfiguration("depth_registered_mode") publish_point_clouds_config = LaunchConfiguration("publish_point_clouds") mock_enable = IfCondition(LaunchConfiguration("mock_enable", default="False")).evaluate(context) @@ -102,8 +104,20 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: if (config_file_path != "") and (not os.path.isfile(config_file_path)): raise FileNotFoundError("Configuration file '{}' does not exist!".format(config_file_path)) - ros_params = get_ros_param_dict(config_file_path) - spot_name: str = ros_params["spot_name"] if "spot_name" in ros_params else "" + substitutions = { + "spot_name": spot_name_arg, + "frame_prefix": tf_prefix_arg, + } + configured_params = substitute_launch_parameters(config_file, substitutions, context) + spot_name: str = ( + ( + configured_params["spot_name"] + if isinstance(configured_params["spot_name"], str) + else configured_params["spot_name"].perform(context) + ) + if "spot_name" in configured_params + else "" + ) if mock_enable: mock_has_arm = IfCondition(LaunchConfiguration("mock_has_arm")).evaluate(context) @@ -137,7 +151,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: package="spot_driver", executable="spot_image_publisher_node", output="screen", - parameters=[config_file, spot_image_publisher_params], + parameters=[configured_params, spot_image_publisher_params], namespace=spot_name, ) ld.add_action(spot_image_publisher_node) @@ -180,7 +194,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: (f"{cam_prefix}/virtual_camera/image", f"{cam_prefix}/camera/{virtual_camera_frame}/image"), (f"{cam_prefix}/virtual_camera/camera_info", f"{cam_prefix}/camera/{virtual_camera_frame}/camera_info"), ], - parameters=[config_file, stitcher_params], + parameters=[configured_params, stitcher_params], condition=IfCondition(LaunchConfiguration("stitch_front_images")), ) ld.add_action(image_stitcher_node) @@ -196,6 +210,14 @@ def generate_launch_description() -> launch.LaunchDescription: description="Path to configuration file for the driver.", ) ) + launch_args.append( + DeclareLaunchArgument( + "tf_prefix", + default_value="", + description="apply namespace prefix to robot links and joints", + ) + ) + launch_args.append(DeclareLaunchArgument("spot_name", default_value="", description="Name of Spot")) launch_args += declare_image_publisher_args() ld = launch.LaunchDescription(launch_args) diff --git a/spot_driver/spot_driver/launch/spot_launch_helpers.py b/spot_driver/spot_driver/launch/spot_launch_helpers.py index 2018c2245..66d51f81a 100644 --- a/spot_driver/spot_driver/launch/spot_launch_helpers.py +++ b/spot_driver/spot_driver/launch/spot_launch_helpers.py @@ -3,10 +3,12 @@ import logging import os from enum import Enum -from typing import Any, Dict, List, Optional, Tuple +from typing import Any, Dict, List, Optional, Tuple, Union import yaml +from launch import LaunchContext from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration from spot_wrapper.wrapper import SpotWrapper @@ -247,3 +249,31 @@ def spot_has_arm(config_file_path: str) -> bool: logger=logger, ) return spot_wrapper.has_arm() + + +def substitute_launch_parameters( + config_file_path: Union[str, LaunchConfiguration], + substitutions: Dict[str, LaunchConfiguration], + context: LaunchContext, +) -> Dict[str, Any]: + """Pass the given ROS launch parameter substitutions into parameters from the ROS config yaml file. + + Args: + config_file_path (str | LaunchConfiguration): Path to the config yaml. + substitutions (Dict[str, LaunchConfiguration]): Dictionary of parameter_name: parameter_value containing the + desired launch parameter substitutions. + context (LaunchContext): Context for acquiring the launch configuration inner values. + + Returns: + dict[str, Any]: dictionary of the substituted parameter_name: parameter_value. + If there is no config file, returns a dictionary containing only the given substitutions. + If there is no config file and the substitutions don't have any values, returns an empty dictionary. + """ + config_params: Dict[str, Any] = get_ros_param_dict( + config_file_path if isinstance(config_file_path, str) else config_file_path.perform(context) + ) + for key, value in substitutions.items(): + if value.perform(context): + config_params[key] = value + + return config_params diff --git a/spot_driver/test/src/test_parameter_interface.cpp b/spot_driver/test/src/test_parameter_interface.cpp index ab6bc8710..7beb3000d 100644 --- a/spot_driver/test/src/test_parameter_interface.cpp +++ b/spot_driver/test/src/test_parameter_interface.cpp @@ -498,8 +498,8 @@ TEST_F(RclcppParameterInterfaceTest, GetFramePrefixFromSpotNameFallback) { } TEST_F(RclcppParameterInterfaceTest, GetFramePrefixFromExplicitParameter) { - const auto verifyExpectedFramePrefix = [](std::shared_ptr node, - std::shared_ptr namespaced_node) -> void { + static constexpr auto verifyExpectedFramePrefix = [](std::shared_ptr node, + std::shared_ptr namespaced_node) -> void { // GIVEN we create a RclcppParameterInterface using these nodes RclcppParameterInterface parameter_interface_a{node}; RclcppParameterInterface parameter_interface_b{namespaced_node}; From 403e75699a858905a490333cda561ee10d99e86f Mon Sep 17 00:00:00 2001 From: Imaniac230 <44968160+Imaniac230@users.noreply.github.com> Date: Tue, 29 Oct 2024 21:14:38 +0100 Subject: [PATCH 05/13] fix: Update rviz template fixed frame with prefix independently of the robot name. --- spot_driver/launch/rviz.launch.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/spot_driver/launch/rviz.launch.py b/spot_driver/launch/rviz.launch.py index 983b9a1dc..cb89a6339 100644 --- a/spot_driver/launch/rviz.launch.py +++ b/spot_driver/launch/rviz.launch.py @@ -26,9 +26,10 @@ def create_rviz_config(robot_name: str, tf_prefix: str) -> None: with open(RVIZ_TEMPLATE_FILENAME, "r") as template_file: config = yaml.safe_load(template_file) - if robot_name: + if tf_prefix: # replace fixed frame with robot body frame config["Visualization Manager"]["Global Options"]["Fixed Frame"] = f"{tf_prefix}vision" + if robot_name: # Add robot models for each robot for display in config["Visualization Manager"]["Displays"]: if "RobotModel" in display["Class"]: From 8f8e599ac295047aaa555a9af1d8f2721bad60df Mon Sep 17 00:00:00 2001 From: Tiffany Cappellari Date: Wed, 20 Nov 2024 20:12:12 +0000 Subject: [PATCH 06/13] fixed merge conflicts and rebased; testing in progress --- spot_driver/src/interfaces/rclcpp_parameter_interface.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp b/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp index 9a62c03a9..6866ce157 100644 --- a/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp +++ b/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp @@ -193,10 +193,6 @@ std::optional RclcppParameterInterface::getFramePrefix() const { return declareAndGetParameter(node_, kParameterFramePrefix); } -std::optional RclcppParameterInterface::getFramePrefix() const { - return declareAndGetParameter(node_, kParameterFramePrefix); -} - bool RclcppParameterInterface::getGripperless() const { return declareAndGetParameter(node_, kParameterNameGripperless, kDefaultGripperless); } From 3f2d35a6c6bf0d8bf9b51451903da5dba66ac76a Mon Sep 17 00:00:00 2001 From: Imaniac230 <44968160+Imaniac230@users.noreply.github.com> Date: Thu, 21 Nov 2024 10:50:33 +0100 Subject: [PATCH 07/13] fix: Fix spot_name and frame_prefix empty string handling, implement validation checks for preferred_odom_frame in cpp nodes. * Fixed incorrect usage of the spot_name parameter in spot_driver.launch if given as an empty string - do not try to create a prefix from the empty name. * Fixed incorrect usage of the frame_prefix parameter in spot_ros2 if not set - empty string cannot be used as a default value, since it is also used as a valid prefix option. * Implemented a validatePreferredOdomFrame() function in cpp conversion utils for checking if the given preferred_odom_frame can be used as a valid option in state_publisher and object_synchronizer - if it is not valid, the nodes will use a default option and output a warning message. * Implemented a prependPrefix() function in conversion utils. * Extended the spot_ros2 error message to print out the full list of all valid preferred_odom_frame options. * Added some explanations to the parameters in config and commented out frame_prefix by default. * Implemented tests for all prefix handling utils - stripPrefix(), prependPrefix(), validatePreferredOdomFrame(). Signed-off-by: Imaniac230 <44968160+Imaniac230@users.noreply.github.com> --- spot_driver/config/spot_ros_example.yaml | 4 +- .../spot_driver/conversions/robot_state.hpp | 23 ++++++++ spot_driver/launch/spot_driver.launch.py | 15 ++--- spot_driver/spot_driver/spot_ros2.py | 25 ++++----- spot_driver/src/conversions/robot_state.cpp | 17 ++++++ .../src/object_sync/object_synchronizer.cpp | 13 +++-- .../src/robot_state/state_publisher.cpp | 13 +++-- .../fake/fake_parameter_interface.hpp | 2 +- .../test/src/conversions/test_robot_state.cpp | 55 +++++++++++++++++++ .../test/src/test_parameter_interface.cpp | 1 - 10 files changed, 135 insertions(+), 33 deletions(-) diff --git a/spot_driver/config/spot_ros_example.yaml b/spot_driver/config/spot_ros_example.yaml index 2a633cf49..fbed654fb 100644 --- a/spot_driver/config/spot_ros_example.yaml +++ b/spot_driver/config/spot_ros_example.yaml @@ -22,8 +22,10 @@ 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. + + # These parameters may be overridden by the 'spot_name' and 'tf_prefix' launch arguments. spot_name: "" - frame_prefix: "" + # frame_prefix: "" # Set an explicit prefix for all frames in the tf2 tree (an empty string is a valid prefix option). 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/conversions/robot_state.hpp b/spot_driver/include/spot_driver/conversions/robot_state.hpp index 3e5ae7bd7..d24d29bcf 100644 --- a/spot_driver/include/spot_driver/conversions/robot_state.hpp +++ b/spot_driver/include/spot_driver/conversions/robot_state.hpp @@ -41,6 +41,8 @@ inline const std::map kFriendlyJointNames {"arm0.wr1", "arm_wr1"}, {"arm0.f1x", "arm_f1x"}, }; +static constexpr std::array kValidOdomFrameOptions{"odom", "vision"}; + /** * @brief Given an input string and a prefix string which is a substring starting at the beginning of the input string, * return a new string which is the difference between the input string and the prefix string. @@ -50,6 +52,27 @@ inline const std::map kFriendlyJointNames */ std::string stripPrefix(const std::string& input, const std::string& prefix); +/** + * @brief Given an input string and a prefix string, return a new string which is the addition of the prefix string and + * the input string. If the input string already contains the prefix substring at the beginning, the output will be the + * same as input. + * @param input + * @param prefix + * @return A new string which is the addition of the prefix string and the input string. If the input string already + * contains the prefix substring at the beginning, the output will be the same as input. + */ +std::string prependPrefix(const std::string& input, const std::string& prefix); + +/** + * @brief Given an input frame string and a prefix string, return a new optional string which is guaranteed to be a + * valid odom frame option. If a valid option cannot be created, std::nullopt is returned instead. + * @param frame + * @param prefix + * @return A new optional string which is guaranteed to be a valid odom frame option. If a valid option cannot be + * created, std::nullopt is returned instead. + */ +std::optional validatePreferredOdomFrame(const std::string& frame, const std::string& prefix); + /** * @brief Create a BatteryStateArray ROS message by parsing a RobotState message and applying a clock skew to it. * diff --git a/spot_driver/launch/spot_driver.launch.py b/spot_driver/launch/spot_driver.launch.py index f025e6f79..c4dc3a648 100644 --- a/spot_driver/launch/spot_driver.launch.py +++ b/spot_driver/launch/spot_driver.launch.py @@ -40,18 +40,19 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: "frame_prefix": tf_prefix_arg, } configured_params = substitute_launch_parameters(config_file_path, substitutions, context) - spot_name: Optional[Union[str, LaunchConfiguration]] = ( - configured_params["spot_name"] if "spot_name" in configured_params else None + spot_name: Union[str, LaunchConfiguration] = ( + configured_params["spot_name"] if "spot_name" in configured_params else "" ) tf_prefix: Optional[Union[str, LaunchConfiguration]] = ( configured_params["frame_prefix"] if "frame_prefix" in configured_params else None ) - if tf_prefix is None and spot_name is not None: - tf_prefix = (spot_name if isinstance(spot_name, str) else spot_name.perform(context)) + "/" if tf_prefix is None: - tf_prefix = "" - if spot_name is None: - spot_name = "" + if isinstance(spot_name, LaunchConfiguration): + tf_prefix = spot_name.perform(context) + "/" + elif isinstance(spot_name, str) and spot_name: + tf_prefix = spot_name + "/" + else: + tf_prefix = "" if mock_enable: mock_has_arm = IfCondition(LaunchConfiguration("mock_has_arm")).evaluate(context) diff --git a/spot_driver/spot_driver/spot_ros2.py b/spot_driver/spot_driver/spot_ros2.py index 5856d711a..7bce33ddd 100644 --- a/spot_driver/spot_driver/spot_ros2.py +++ b/spot_driver/spot_driver/spot_ros2.py @@ -250,7 +250,7 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw self.declare_parameter("initialize_spot_cam", False) self.declare_parameter("spot_name", "") - self.declare_parameter("frame_prefix", "") + self.declare_parameter("frame_prefix", Parameter.Type.STRING) self.declare_parameter("mock_enable", False) self.declare_parameter("gripperless", False) @@ -343,31 +343,28 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw # 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_param: Optional[str] = self.get_parameter("frame_prefix").value - frame_prefix = frame_prefix_param if frame_prefix_param is not None else "" - if frame_prefix_param is None and self.name is not None: - frame_prefix = self.name + "/" - self.frame_prefix: str = frame_prefix + self.frame_prefix: Optional[str] = self.get_parameter_or("frame_prefix", None).value + if self.frame_prefix is None: + self.frame_prefix = self.name + "/" if self.name is not None else "" 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_kinematic: str = self.frame_prefix + "odom" self.tf_name_raw_vision: str = self.frame_prefix + "vision" preferred_odom_frame_references = [self.tf_name_raw_kinematic, self.tf_name_raw_vision] + preferred_odom_frame_all_options = ( + preferred_odom_frame_references + ["odom", "vision"] + if self.frame_prefix + else preferred_odom_frame_references + ) preferred_odom_frame_param: str = self.preferred_odom_frame.value if preferred_odom_frame_param not in preferred_odom_frame_references: if self.frame_prefix + preferred_odom_frame_param in preferred_odom_frame_references: preferred_odom_frame_param = self.frame_prefix + preferred_odom_frame_param else: error_msg = ( - f'The rosparam "preferred_odom_frame" should be one of {preferred_odom_frame_references}, got' + f'The rosparam "preferred_odom_frame" should be one of {preferred_odom_frame_all_options}, got' f' "{preferred_odom_frame_param}", which could not be composed into any valid option.' ) self.get_logger().error(error_msg) diff --git a/spot_driver/src/conversions/robot_state.cpp b/spot_driver/src/conversions/robot_state.cpp index 99054ae3c..0236d3e81 100644 --- a/spot_driver/src/conversions/robot_state.cpp +++ b/spot_driver/src/conversions/robot_state.cpp @@ -30,6 +30,23 @@ std::string stripPrefix(const std::string& input, const std::string& prefix) { return input.substr(prefix.size()); } +std::string prependPrefix(const std::string& input, const std::string& prefix) { + return input.find(prefix) == 0 ? input : prefix + input; +} + +std::optional validatePreferredOdomFrame(const std::string& frame, const std::string& prefix) { + // Make sure we already have the frame and prefix combined. + const std::string frame_with_prefix = prependPrefix(frame, prefix); + + // Compare the given prefixed frame with all valid prefixed options and set false if no match is found. + const bool is_valid = std::find_if(kValidOdomFrameOptions.begin(), kValidOdomFrameOptions.end(), + [&prefix, &frame_with_prefix](const auto& option) -> bool { + return frame_with_prefix == (prefix + option); + }) != kValidOdomFrameOptions.end(); + + return is_valid ? std::make_optional(frame_with_prefix) : std::nullopt; +} + spot_msgs::msg::BatteryStateArray getBatteryStates(const ::bosdyn::api::RobotState& robot_state, const google::protobuf::Duration& clock_skew) { spot_msgs::msg::BatteryStateArray battery_states; diff --git a/spot_driver/src/object_sync/object_synchronizer.cpp b/spot_driver/src/object_sync/object_synchronizer.cpp index fa26e3edb..5759def2c 100644 --- a/spot_driver/src/object_sync/object_synchronizer.cpp +++ b/spot_driver/src/object_sync/object_synchronizer.cpp @@ -309,12 +309,15 @@ ObjectSynchronizer::ObjectSynchronizer(const std::shared_ptrgetFramePrefixWithDefaultFallback(); - // TODO(param-refactor): this was refactored to match the logic from state_publisher, - // find out if the stripping and re-adding wasn't done in the particular way intentionally for something const std::string preferred_odom_frame = parameter_interface_->getPreferredOdomFrame(); - preferred_base_frame_ = stripPrefix(preferred_odom_frame, frame_prefix_); - preferred_base_frame_with_prefix_ = - preferred_odom_frame.find('/') == std::string::npos ? frame_prefix_ + preferred_odom_frame : preferred_odom_frame; + const std::optional valid_odom_frame = validatePreferredOdomFrame(preferred_odom_frame, frame_prefix_); + preferred_base_frame_ = stripPrefix(valid_odom_frame.value_or(kValidOdomFrameOptions[0]), frame_prefix_); + preferred_base_frame_with_prefix_ = valid_odom_frame.value_or(frame_prefix_ + kValidOdomFrameOptions[0]); + if (!valid_odom_frame.has_value()) { + logger_interface_->logWarn(std::string{"Given preferred odom frame '"}.append( + preferred_odom_frame + "' could not be composed into any valid option, defaulting to: '" + + preferred_base_frame_with_prefix_ + "'.")); + } // TODO(khughes): This is temporarily disabled to reduce driver's spew about TF extrapolation. // world_object_update_timer_->setTimer(kWorldObjectSyncPeriod, [this]() { diff --git a/spot_driver/src/robot_state/state_publisher.cpp b/spot_driver/src/robot_state/state_publisher.cpp index 801e47b63..fd22748a0 100644 --- a/spot_driver/src/robot_state/state_publisher.cpp +++ b/spot_driver/src/robot_state/state_publisher.cpp @@ -32,10 +32,15 @@ StatePublisher::StatePublisher(const std::shared_ptr& stat timer_interface_{std::move(timer_interface)} { frame_prefix_ = parameter_interface_->getFramePrefixWithDefaultFallback(); - const auto preferred_odom_frame = parameter_interface_->getPreferredOdomFrame(); - is_using_vision_ = stripPrefix(preferred_odom_frame, frame_prefix_) == "vision"; - full_odom_frame_id_ = - preferred_odom_frame.find('/') == std::string::npos ? frame_prefix_ + preferred_odom_frame : preferred_odom_frame; + const std::string preferred_odom_frame = parameter_interface_->getPreferredOdomFrame(); + const std::optional valid_odom_frame = validatePreferredOdomFrame(preferred_odom_frame, frame_prefix_); + is_using_vision_ = stripPrefix(valid_odom_frame.value_or(kValidOdomFrameOptions[0]), frame_prefix_) == "vision"; + full_odom_frame_id_ = valid_odom_frame.value_or(frame_prefix_ + kValidOdomFrameOptions[0]); + if (!valid_odom_frame.has_value()) { + logger_interface_->logWarn(std::string{"Given preferred odom frame '"}.append( + preferred_odom_frame + "' could not be composed into any valid option, defaulting to: '" + full_odom_frame_id_ + + "'.")); + } // Create a timer to request and publish robot state at a fixed rate timer_interface_->setTimer(kRobotStateCallbackPeriod, [this] { 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 0760c82a2..44639ad4d 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 @@ -36,7 +36,7 @@ class FakeParameterInterface : public ParameterInterfaceBase { bool getPublishDepthRegisteredImages() const override { return publish_depth_registered_images; } - std::string getPreferredOdomFrame() const override { return "odom"; } + std::string getPreferredOdomFrame() const override { return kDefaultPreferredOdomFrame; } std::optional getFramePrefix() const override { return std::nullopt; } diff --git a/spot_driver/test/src/conversions/test_robot_state.cpp b/spot_driver/test/src/conversions/test_robot_state.cpp index 9dc4bf657..3f6fb9303 100644 --- a/spot_driver/test/src/conversions/test_robot_state.cpp +++ b/spot_driver/test/src/conversions/test_robot_state.cpp @@ -846,4 +846,59 @@ TEST(RobotStateConversions, TestGetBehaviorFaultStateNoFaults) { // THEN the optional which wraps the output ROS message is set to nullopt EXPECT_THAT(out.has_value(), IsFalse()); } + +TEST(RobotStateConversions, TestFramePrefixParsing) { + // GIVEN we have some frame name and prefix + const std::string frame = "some_frame"; + const std::string prefix = "some_frame_prefix/"; + + // WHEN we try to modify an input frame, which does not contain the prefix at the beginning + // THEN we don't strip anything and the output is the same as input + ASSERT_THAT(stripPrefix(frame, prefix), StrEq(frame)); + ASSERT_THAT(stripPrefix(frame + prefix, prefix), StrEq(frame + prefix)); + // THEN we prepend the prefix to the input + ASSERT_THAT(prependPrefix(frame, prefix), StrEq(prefix + frame)); + ASSERT_THAT(prependPrefix(frame + prefix, prefix), StrEq(prefix + frame + prefix)); + + // WHEN we try to modify an input frame, which does contain the prefix at the beginning + // THEN we strip the prefix from the beginning + ASSERT_THAT(stripPrefix(prefix + frame, prefix), StrEq(frame)); + ASSERT_THAT(stripPrefix(prefix + frame + prefix, prefix), StrEq(frame + prefix)); + // THEN we don't prepend anything and the output is the same as input + ASSERT_THAT(prependPrefix(prefix + frame, prefix), StrEq(prefix + frame)); + ASSERT_THAT(prependPrefix(prefix + frame + prefix, prefix), StrEq(prefix + frame + prefix)); + + // WHEN we try to modify an input frame with an empty prefix + // THEN the output is always the same as input + ASSERT_THAT(stripPrefix(frame, ""), StrEq(frame)); + ASSERT_THAT(prependPrefix(frame, ""), StrEq(frame)); +} + +TEST(RobotStateConversions, TestPreferredOdomFrameValidation) { + // GIVEN we have some frame prefix + const std::string prefix = "some_frame_prefix/"; + + for (const auto& odom_frame : kValidOdomFrameOptions) { + // WHEN we provide valid preferred odom frame options + // THEN the optional contains the valid prefixed frame + const auto raw_option = validatePreferredOdomFrame(odom_frame, prefix); + ASSERT_THAT(raw_option.has_value(), IsTrue()); + ASSERT_THAT(raw_option.value(), StrEq(prefix + odom_frame)); + const auto prefixed_option = validatePreferredOdomFrame(prefix + odom_frame, prefix); + ASSERT_THAT(prefixed_option.has_value(), IsTrue()); + ASSERT_THAT(prefixed_option.value(), StrEq(prefix + odom_frame)); + + // WHEN we provide an invalid preferred odom frame + // THEN the optional is set to nullopt + ASSERT_THAT(validatePreferredOdomFrame(odom_frame + prefix, prefix).has_value(), IsFalse()); + ASSERT_THAT(validatePreferredOdomFrame(prefix + odom_frame + prefix, prefix).has_value(), IsFalse()); + + // GIVEN the frame prefix is empty + // THEN we only have one valid option + const auto option_without_prefix = validatePreferredOdomFrame(odom_frame, ""); + ASSERT_THAT(option_without_prefix.has_value(), IsTrue()); + ASSERT_THAT(option_without_prefix.value(), StrEq(odom_frame)); + ASSERT_THAT(validatePreferredOdomFrame(prefix + odom_frame, "").has_value(), IsFalse()); + } +} } // namespace spot_ros2::test diff --git a/spot_driver/test/src/test_parameter_interface.cpp b/spot_driver/test/src/test_parameter_interface.cpp index 7beb3000d..716a2ced5 100644 --- a/spot_driver/test/src/test_parameter_interface.cpp +++ b/spot_driver/test/src/test_parameter_interface.cpp @@ -462,7 +462,6 @@ TEST_F(RclcppParameterInterfaceEnvVarTest, GetSelectedCamerasUsedGripperless) { EXPECT_THAT(cameras_used_arm.error(), StrEq("Cannot add SpotCamera 'hand', the robot is gripperless!")); } - TEST_F(RclcppParameterInterfaceTest, GetFramePrefixFromNamespaceFallback) { // GIVEN we create rclcpp nodes with and without a specific namespace const auto node = std::make_shared(kNodeName); From 10ca6ae31ea5d8914a1d511ddf765a809842994b Mon Sep 17 00:00:00 2001 From: Imaniac230 <44968160+Imaniac230@users.noreply.github.com> Date: Sat, 23 Nov 2024 22:26:00 +0100 Subject: [PATCH 08/13] fix: Generalize launch helper types from LaunchConfiguration to Substitution, implement tests for prefix and name argument handling in launch files and the spot_ros2 python node. * The spot name and frame prefix handling was ported from the launchfiles into a separate get_name_and_prefix launch helper util. * Refactored argument and return types of substitute_launch_parameters to the base Substitution type instead of LaunchConfiguration. * Implemented unit tests for the added substitute_launch_parameters and get_name_and_prefix utils. * Added a new test in test_ros_interfaces to check that the frame_prefix and preferred_odom_frame in spot_ros2 are correctly set using the spot_name. Signed-off-by: Imaniac230 <44968160+Imaniac230@users.noreply.github.com> --- spot_driver/launch/spot_driver.launch.py | 16 +- .../launch/spot_image_publishers.launch.py | 15 +- .../spot_driver/launch/spot_launch_helpers.py | 43 ++++- spot_driver/spot_driver/spot_ros2.py | 16 +- .../test/pytests/test_launch_helpers.py | 173 ++++++++++++++++++ .../test/pytests/test_ros_interfaces.py | 5 + 6 files changed, 227 insertions(+), 41 deletions(-) mode change 100644 => 100755 spot_driver/spot_driver/launch/spot_launch_helpers.py create mode 100644 spot_driver/test/pytests/test_launch_helpers.py diff --git a/spot_driver/launch/spot_driver.launch.py b/spot_driver/launch/spot_driver.launch.py index c4dc3a648..6889dd08a 100644 --- a/spot_driver/launch/spot_driver.launch.py +++ b/spot_driver/launch/spot_driver.launch.py @@ -1,7 +1,6 @@ # Copyright (c) 2023-2024 Boston Dynamics AI Institute LLC. All rights reserved. import os -from typing import Optional, Union import launch import launch_ros @@ -15,6 +14,7 @@ from spot_driver.launch.spot_launch_helpers import ( IMAGE_PUBLISHER_ARGS, declare_image_publisher_args, + get_name_and_prefix, spot_has_arm, substitute_launch_parameters, ) @@ -40,19 +40,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: "frame_prefix": tf_prefix_arg, } configured_params = substitute_launch_parameters(config_file_path, substitutions, context) - spot_name: Union[str, LaunchConfiguration] = ( - configured_params["spot_name"] if "spot_name" in configured_params else "" - ) - tf_prefix: Optional[Union[str, LaunchConfiguration]] = ( - configured_params["frame_prefix"] if "frame_prefix" in configured_params else None - ) - if tf_prefix is None: - if isinstance(spot_name, LaunchConfiguration): - tf_prefix = spot_name.perform(context) + "/" - elif isinstance(spot_name, str) and spot_name: - tf_prefix = spot_name + "/" - else: - tf_prefix = "" + spot_name, tf_prefix = get_name_and_prefix(configured_params) if mock_enable: mock_has_arm = IfCondition(LaunchConfiguration("mock_has_arm")).evaluate(context) diff --git a/spot_driver/launch/spot_image_publishers.launch.py b/spot_driver/launch/spot_image_publishers.launch.py index 01417c95c..de10babe6 100644 --- a/spot_driver/launch/spot_image_publishers.launch.py +++ b/spot_driver/launch/spot_image_publishers.launch.py @@ -5,7 +5,7 @@ import launch import launch_ros -from launch import LaunchContext, LaunchDescription +from launch import LaunchContext, LaunchDescription, Substitution from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PathJoinSubstitution @@ -14,6 +14,7 @@ DepthRegisteredMode, declare_image_publisher_args, get_camera_sources, + get_name_and_prefix, spot_has_arm, substitute_launch_parameters, ) @@ -109,15 +110,9 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: "frame_prefix": tf_prefix_arg, } configured_params = substitute_launch_parameters(config_file, substitutions, context) - spot_name: str = ( - ( - configured_params["spot_name"] - if isinstance(configured_params["spot_name"], str) - else configured_params["spot_name"].perform(context) - ) - if "spot_name" in configured_params - else "" - ) + spot_name = get_name_and_prefix(configured_params) + if isinstance(spot_name, Substitution): + spot_name = spot_name.perform(context) if mock_enable: mock_has_arm = IfCondition(LaunchConfiguration("mock_has_arm")).evaluate(context) diff --git a/spot_driver/spot_driver/launch/spot_launch_helpers.py b/spot_driver/spot_driver/launch/spot_launch_helpers.py old mode 100644 new mode 100755 index 66d51f81a..283154b42 --- a/spot_driver/spot_driver/launch/spot_launch_helpers.py +++ b/spot_driver/spot_driver/launch/spot_launch_helpers.py @@ -6,9 +6,9 @@ from typing import Any, Dict, List, Optional, Tuple, Union import yaml -from launch import LaunchContext +from launch import LaunchContext, Substitution from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration +from launch.substitutions import PathJoinSubstitution from spot_wrapper.wrapper import SpotWrapper @@ -132,7 +132,7 @@ def get_ros_param_dict(config_file_path: str) -> Dict[str, Any]: def get_login_parameters(config_file_path: str) -> Tuple[str, str, str, Optional[int], Optional[str], Optional[str]]: - """Obtain the username, password, hostname, port, and certificate of Spot from the environment variables or, + """Obtain the username, password, hostname, port, certificate, and name of Spot from the environment variables or, if they are not set, the configuration file yaml. Args: @@ -142,7 +142,8 @@ def get_login_parameters(config_file_path: str) -> Tuple[str, str, str, Optional ValueError: If any of username, password, hostname is not set. Returns: - Tuple[str, str, str, Optional[int], Optional[str]]: username, password, hostname, port, certificate + Tuple[str, str, str, Optional[int], Optional[str], Optional[str]]: username, password, hostname, port, + certificate, spot_name """ # Get value from environment variables username = os.getenv("BOSDYN_CLIENT_USERNAME") @@ -252,16 +253,16 @@ def spot_has_arm(config_file_path: str) -> bool: def substitute_launch_parameters( - config_file_path: Union[str, LaunchConfiguration], - substitutions: Dict[str, LaunchConfiguration], + config_file_path: Union[str, Substitution], + substitutions: Dict[str, Substitution], context: LaunchContext, ) -> Dict[str, Any]: """Pass the given ROS launch parameter substitutions into parameters from the ROS config yaml file. Args: - config_file_path (str | LaunchConfiguration): Path to the config yaml. - substitutions (Dict[str, LaunchConfiguration]): Dictionary of parameter_name: parameter_value containing the - desired launch parameter substitutions. + config_file_path (str | Substitution): Path to the config yaml. + substitutions (Dict[str, Substitution]): Dictionary of parameter_name: parameter_value containing the desired + launch parameter substitutions. context (LaunchContext): Context for acquiring the launch configuration inner values. Returns: @@ -277,3 +278,27 @@ def substitute_launch_parameters( config_params[key] = value return config_params + + +def get_name_and_prefix(ros_params: Dict[str, Any]) -> Tuple[Union[str, Substitution], Union[str, Substitution]]: + """Get the Spot robot name and ROS TF frame prefix from the provided ROS parameters, which may be taken directly + from the yaml config or passed through from launch arguments. This will compose the frame prefix from the Spot name + if not given explicitly. + + Args: + ros_params (dict[str, Any]): A dictionary of parameter_name: parameter_value. + + Returns: + Tuple[str | Substitution, str | Substitution]: spot_name, tf_prefix. + """ + spot_name: Union[str, Substitution] = ros_params["spot_name"] if "spot_name" in ros_params else "" + tf_prefix: Optional[Union[str, Substitution]] = ros_params["frame_prefix"] if "frame_prefix" in ros_params else None + if tf_prefix is None: + if isinstance(spot_name, Substitution): + tf_prefix = PathJoinSubstitution([spot_name, ""]) + elif isinstance(spot_name, str) and spot_name: + tf_prefix = spot_name + "/" + else: + tf_prefix = "" + + return spot_name, tf_prefix diff --git a/spot_driver/spot_driver/spot_ros2.py b/spot_driver/spot_driver/spot_ros2.py index 7bce33ddd..0f4a16203 100644 --- a/spot_driver/spot_driver/spot_ros2.py +++ b/spot_driver/spot_driver/spot_ros2.py @@ -252,6 +252,7 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw self.declare_parameter("spot_name", "") self.declare_parameter("frame_prefix", Parameter.Type.STRING) self.declare_parameter("mock_enable", False) + self.declare_parameter("preferred_odom_frame", "") # 'vision' or 'odom' self.declare_parameter("gripperless", False) @@ -346,11 +347,11 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw self.frame_prefix: Optional[str] = self.get_parameter_or("frame_prefix", None).value if self.frame_prefix is None: self.frame_prefix = self.name + "/" if self.name is not None else "" - self.preferred_odom_frame: Parameter = self.declare_parameter( - "preferred_odom_frame", self.frame_prefix + "odom" - ) # 'vision' or 'odom' + self.preferred_odom_frame: str = self.get_parameter("preferred_odom_frame").value self.tf_name_raw_kinematic: str = self.frame_prefix + "odom" self.tf_name_raw_vision: str = self.frame_prefix + "vision" + if not self.preferred_odom_frame: + self.preferred_odom_frame = self.tf_name_raw_kinematic preferred_odom_frame_references = [self.tf_name_raw_kinematic, self.tf_name_raw_vision] preferred_odom_frame_all_options = ( @@ -358,14 +359,13 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw if self.frame_prefix else preferred_odom_frame_references ) - preferred_odom_frame_param: str = self.preferred_odom_frame.value - if preferred_odom_frame_param not in preferred_odom_frame_references: - if self.frame_prefix + preferred_odom_frame_param in preferred_odom_frame_references: - preferred_odom_frame_param = self.frame_prefix + preferred_odom_frame_param + if self.preferred_odom_frame not in preferred_odom_frame_references: + if self.frame_prefix + self.preferred_odom_frame in preferred_odom_frame_references: + self.preferred_odom_frame = self.frame_prefix + self.preferred_odom_frame else: error_msg = ( f'The rosparam "preferred_odom_frame" should be one of {preferred_odom_frame_all_options}, got' - f' "{preferred_odom_frame_param}", which could not be composed into any valid option.' + f' "{self.preferred_odom_frame}", which could not be composed into any valid option.' ) self.get_logger().error(error_msg) raise ValueError(error_msg) diff --git a/spot_driver/test/pytests/test_launch_helpers.py b/spot_driver/test/pytests/test_launch_helpers.py new file mode 100644 index 000000000..ff245a75e --- /dev/null +++ b/spot_driver/test/pytests/test_launch_helpers.py @@ -0,0 +1,173 @@ +# Copyright (c) 2023-2024 Boston Dynamics AI Institute LLC. See LICENSE file for more info. + +""" +Tests to check the ROS launch helper utilities. +""" + +import tempfile +import unittest + +import yaml +from launch import LaunchContext, Substitution +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + +from spot_driver.launch.spot_launch_helpers import get_name_and_prefix, substitute_launch_parameters + + +class LaunchHelpersTest(unittest.TestCase): + def setUp(self) -> None: + self.name_key: str = "spot_name" + self.prefix_key: str = "frame_prefix" + self.user_key: str = "username" + + self.name_value: str = "test_spot" + self.prefix_value: str = "test_prefix_" + self.user_value: str = "test_username" + + self.context = LaunchContext() + + def test_substitute_launch_parameters(self) -> None: + """ + Test the util substitute_launch_parameters. + """ + with tempfile.NamedTemporaryFile(mode="w", suffix="config.yaml") as temp: + data = { + self.name_key: self.name_value, + self.prefix_key: self.prefix_value, + self.user_key: self.user_value, + } + yaml.dump({"/**": {"ros__parameters": data}}, temp.file) + temp.file.close() + + # Empty substitutions should not modify the yaml file params. + params = substitute_launch_parameters(temp.file.name, {}, self.context) + self.assertEqual(params[self.name_key], self.name_value, "Substitution empty, should not change.") + self.assertEqual(params[self.prefix_key], self.prefix_value, "Substitution empty, should not change.") + self.assertEqual(params[self.user_key], self.user_value, "Substitution empty, should not change.") + + # Empty launch arguments should not modify the yaml file params. + substitutions = { + self.name_key: LaunchConfiguration(self.name_key, default=""), + self.prefix_key: LaunchConfiguration(self.prefix_key, default=""), + self.user_key: LaunchConfiguration(self.user_key, default=""), + } + params = substitute_launch_parameters(temp.file.name, substitutions, self.context) + self.assertEqual(params[self.name_key], self.name_value, "Launch argument empty, should not change.") + self.assertEqual(params[self.prefix_key], self.prefix_value, "Launch argument empty, should not change.") + self.assertEqual(params[self.user_key], self.user_value, "Launch argument empty, should not change.") + + # Substitutions should modify only their corresponding keys. + substitutions = { + self.name_key: LaunchConfiguration(self.name_key, default="spot_name_overridden"), + self.user_key: LaunchConfiguration(self.user_key, default="username_overridden"), + } + params = substitute_launch_parameters(temp.file.name, substitutions, self.context) + self.assertIsInstance(params[self.name_key], Substitution, "Launch argument set, should override.") + self.assertEqual( + params[self.name_key].perform(self.context), + "spot_name_overridden", + "Launch argument set, should override.", + ) + self.assertEqual(params[self.prefix_key], self.prefix_value, "Substitution empty, should not change.") + self.assertIsInstance(params[self.user_key], Substitution, "Launch argument set, should override.") + self.assertEqual( + params[self.user_key].perform(self.context), + "username_overridden", + "Launch argument set, should override.", + ) + substitutions = { + self.prefix_key: PathJoinSubstitution(["prefix_overridden", ""]), + } + params = substitute_launch_parameters(temp.file.name, substitutions, self.context) + self.assertEqual(params[self.name_key], self.name_value, "Substitution empty, should not change.") + self.assertIsInstance(params[self.prefix_key], Substitution, "Substitution set, should override.") + self.assertEqual( + params[self.prefix_key].perform(self.context), + "prefix_overridden/", + "Substitution set, should override.", + ) + self.assertEqual(params[self.user_key], self.user_value, "Substitution empty, should not change.") + + # Giving non-substitution types as parameter substitutions should fail. + substitutions = {self.name_key: "overridden"} + self.assertRaises( + AttributeError, + substitute_launch_parameters, + temp.file.name, + substitutions, + self.context, + ) + + def test_get_name_and_prefix(self) -> None: + """ + Test the util get_name_and_prefix. + """ + name, prefix = get_name_and_prefix({}) + self.assertTrue(name == "" and prefix == "", "Empty parameters.") + + name, prefix = get_name_and_prefix({self.name_key: ""}) + self.assertTrue(name == "" and prefix == "", "Empty parameters.") + + name, prefix = get_name_and_prefix({self.name_key: "", self.prefix_key: ""}) + self.assertTrue(name == "" and prefix == "", "Empty parameters.") + + name, prefix = get_name_and_prefix({self.name_key: self.name_value}) + self.assertTrue(name == self.name_value and prefix == self.name_value + "/", "Prefix from name.") + + name, prefix = get_name_and_prefix({self.name_key: self.name_value, self.prefix_key: ""}) + self.assertTrue(name == self.name_value and prefix == "", "Explicit prefix.") + + name, prefix = get_name_and_prefix({self.name_key: self.name_value, self.prefix_key: self.prefix_value}) + self.assertTrue(name == self.name_value and prefix == self.prefix_value, "Explicit prefix.") + + name, prefix = get_name_and_prefix({self.name_key: "", self.prefix_key: self.prefix_value}) + self.assertTrue(name == "" and prefix == self.prefix_value, "Explicit prefix.") + + # Should also work when values are Substitution types. + name_launch_param = LaunchConfiguration(self.name_key, default=self.name_value) + prefix_launch_param = LaunchConfiguration(self.prefix_key, default=self.prefix_value) + + name, prefix = get_name_and_prefix({self.name_key: name_launch_param}) + self.assertTrue( + isinstance(name, Substitution) and isinstance(prefix, Substitution), "Launch argument: prefix from name." + ) + self.assertTrue( + name.perform(self.context) == self.name_value and prefix.perform(self.context) == self.name_value + "/", + "Launch argument: prefix from name.", + ) + + name, prefix = get_name_and_prefix({self.name_key: name_launch_param, self.prefix_key: prefix_launch_param}) + self.assertTrue( + isinstance(name, Substitution) and isinstance(prefix, Substitution), "Launch argument: explicit prefix." + ) + self.assertTrue( + name.perform(self.context) == self.name_value and prefix.perform(self.context) == self.prefix_value, + "Launch argument: explicit prefix.", + ) + + name_path_join_substitution = PathJoinSubstitution(self.name_value) + prefix_path_join_substitution = PathJoinSubstitution([self.prefix_value, ""]) + + name, prefix = get_name_and_prefix({self.name_key: name_path_join_substitution}) + self.assertTrue( + isinstance(name, Substitution) and isinstance(prefix, Substitution), "Substitution: prefix from name." + ) + self.assertTrue( + name.perform(self.context) == self.name_value and prefix.perform(self.context) == self.name_value + "/", + "Substitution: prefix from name.", + ) + + name, prefix = get_name_and_prefix( + {self.name_key: name_path_join_substitution, self.prefix_key: prefix_path_join_substitution} + ) + self.assertTrue( + isinstance(name, Substitution) and isinstance(prefix, Substitution), "Substitution: explicit prefix." + ) + self.assertTrue( + name.perform(self.context) == self.name_value and prefix.perform(self.context) == self.prefix_value + "/", + "Substitution: explicit prefix.", + ) + + +if __name__ == "__main__": + unittest.main() diff --git a/spot_driver/test/pytests/test_ros_interfaces.py b/spot_driver/test/pytests/test_ros_interfaces.py index 3414dbaa2..e071c12b7 100644 --- a/spot_driver/test/pytests/test_ros_interfaces.py +++ b/spot_driver/test/pytests/test_ros_interfaces.py @@ -26,6 +26,7 @@ def setUp(self) -> None: rclpy.parameter.Parameter("spot_name", value="Mock_spot"), rclpy.parameter.Parameter("mock_enable", value=True), rclpy.parameter.Parameter("mock_has_arm", value=False), + rclpy.parameter.Parameter("preferred_odom_frame", value="vision"), ], ) @@ -1019,6 +1020,10 @@ def test_robot_command_goal_complete(self) -> None: "NO FEEDBACK_BATTERY_CHANGE_POSE_FEEDBACK_SET | STATUS_COMMAND_OVERRIDDEN", ) + def test_frame_prefix_from_spot_name(self) -> None: + self.assertEqual(self.spot_ros2.frame_prefix, "Mock_spot/", "spot_name not used in frame_prefix") + self.assertEqual(self.spot_ros2.preferred_odom_frame, "Mock_spot/vision", "spot_name not used in frame_prefix") + if __name__ == "__main__": unittest.main() From 86661897c8d9c68c2aaf6b7b6520a3b3676820ec Mon Sep 17 00:00:00 2001 From: Imaniac230 <44968160+Imaniac230@users.noreply.github.com> Date: Mon, 25 Nov 2024 20:57:33 +0100 Subject: [PATCH 09/13] fix: Use spot_name from tuple output in spot_image_publishers.launch correctly, remove forgotten local fixme comment. Signed-off-by: Imaniac230 <44968160+Imaniac230@users.noreply.github.com> --- spot_driver/launch/spot_image_publishers.launch.py | 2 +- spot_driver/spot_driver/launch/spot_launch_helpers.py | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/spot_driver/launch/spot_image_publishers.launch.py b/spot_driver/launch/spot_image_publishers.launch.py index de10babe6..6af97f131 100644 --- a/spot_driver/launch/spot_image_publishers.launch.py +++ b/spot_driver/launch/spot_image_publishers.launch.py @@ -110,7 +110,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: "frame_prefix": tf_prefix_arg, } configured_params = substitute_launch_parameters(config_file, substitutions, context) - spot_name = get_name_and_prefix(configured_params) + spot_name, _ = get_name_and_prefix(configured_params) if isinstance(spot_name, Substitution): spot_name = spot_name.perform(context) diff --git a/spot_driver/spot_driver/launch/spot_launch_helpers.py b/spot_driver/spot_driver/launch/spot_launch_helpers.py index 283154b42..cf5dd9fca 100755 --- a/spot_driver/spot_driver/launch/spot_launch_helpers.py +++ b/spot_driver/spot_driver/launch/spot_launch_helpers.py @@ -118,7 +118,6 @@ def get_ros_param_dict(config_file_path: str) -> Dict[str, Any]: with open(config_file_path, "r") as config_yaml: try: config_dict = yaml.safe_load(config_yaml) - # FIXME: this will not generalize if explicit node names are used in the yaml config if ("/**" in config_dict) and ("ros__parameters" in config_dict["/**"]): ros_params = config_dict["/**"]["ros__parameters"] return ros_params From 917d52388d6314d496fa70b99eeb824bb1c5427c Mon Sep 17 00:00:00 2001 From: Imaniac230 <44968160+Imaniac230@users.noreply.github.com> Date: Fri, 14 Feb 2025 23:33:19 +0100 Subject: [PATCH 10/13] fix: Post-merge updates and fixes. * Refactored the frame parameter validation function in `robot_state` to be more universal and reusable for the new `tf_root` parameter. * Updated the corresponding `robot_state` test. * Refactored parameter validation with frame prefix in `object_synchronizer` and `state_publisher` to reflect the new `tf_root` parameter. * Updated the spot name parameter extraction in `get_login_parameters` to use the new literal definitions, replaced unused spot name in `spot_has_arm` with placeholder. * Removed the deprecated `preferred_odom_frame` member from `spot_ros2` tests. * Unified some README and config file wording. Signed-off-by: Imaniac230 <44968160+Imaniac230@users.noreply.github.com> --- README.md | 2 +- spot_driver/README.md | 4 +- spot_driver/config/spot_ros_example.yaml | 2 +- .../spot_driver/conversions/robot_state.hpp | 44 ++++++++++++++++--- .../spot_driver/launch/spot_launch_helpers.py | 8 ++-- spot_driver/src/conversions/robot_state.cpp | 13 ------ .../src/object_sync/object_synchronizer.cpp | 19 ++++---- .../src/robot_state/state_publisher.cpp | 21 +++++---- .../test/pytests/test_ros_interfaces.py | 2 - .../test/src/conversions/test_robot_state.cpp | 33 +++++++------- 10 files changed, 82 insertions(+), 66 deletions(-) diff --git a/README.md b/README.md index b50384e49..4490124c3 100644 --- a/README.md +++ b/README.md @@ -82,7 +82,7 @@ Further documentation on how each of these packages can be used can be found in * [`spot_driver`](spot_driver): Core driver for operating Spot. This contains all of the necessary topics, services, and actions for controlling Spot and receiving state information over ROS 2. * The driver can be launched via the following command after building and sourcing your workspace. More details can be found on the [`spot_driver` README](spot_driver/README.md). ``` - ros2 launch spot_driver spot_driver.launch.py [config_file:=] [spot_name:=] [launch_rviz:=] [launch_image_publishers:=] [publish_point_clouds:=] [uncompress_images:=] [publish_compressed_images:=] [stitch_front_images:=] + ros2 launch spot_driver spot_driver.launch.py [config_file:=] [spot_name:=] [tf_prefix:=] [launch_rviz:=] [launch_image_publishers:=] [publish_point_clouds:=] [uncompress_images:=] [publish_compressed_images:=] [stitch_front_images:=] ``` * [`spot_examples`](spot_examples): Examples of how to control Spot via the Spot driver. * [`spot_msgs`](spot_msgs): Custom messages, services, and interfaces relevant for operating Spot. diff --git a/spot_driver/README.md b/spot_driver/README.md index 8a3ea9a6b..3a00d856c 100644 --- a/spot_driver/README.md +++ b/spot_driver/README.md @@ -3,7 +3,7 @@ The Spot driver contains all of the necessary topics, services, and actions for controlling Spot over ROS 2. To launch the driver, run the following command, with the appropriate launch arguments and/or config file that are discussed below. ``` -ros2 launch spot_driver spot_driver.launch.py [config_file:=] [spot_name:=] [tf_prefix:=] [launch_rviz:=] [launch_image_publishers:=] [publish_point_clouds:=] [uncompress_images:=] [publish_compressed_images:=] [stitch_front_images:=] +ros2 launch spot_driver spot_driver.launch.py [config_file:=] [spot_name:=] [tf_prefix:=] [launch_rviz:=] [launch_image_publishers:=] [publish_point_clouds:=] [uncompress_images:=] [publish_compressed_images:=] [stitch_front_images:=] ``` ## Configuration @@ -17,7 +17,7 @@ To avoid this, it is recommended to either launch the driver with the launch arg This will place all of the nodes, topics, services, and actions provided by the driver in the `` namespace. By default, it will also prefix all of the TF frames and joints of the robot with ``. -If you want to change this behavior and instead use a custom prefix for all frames in the TF tree, either launch the driver with the launch argument `tf_prefix:=` or update the `frame_prefix` parameter in your config file (a specified launch argument will override the config file parameter). +If you want to change this behavior and instead use a custom prefix `` for all frames in the TF tree, either launch the driver with the launch argument `tf_prefix:=` or update the `frame_prefix` parameter in your config file (a specified launch argument will override the config file parameter). If you use the config file parameter `frame_prefix`, you can disable prefixing altogether by setting it to an empty string. ## Frames diff --git a/spot_driver/config/spot_ros_example.yaml b/spot_driver/config/spot_ros_example.yaml index 6de767602..c4968f9d6 100644 --- a/spot_driver/config/spot_ros_example.yaml +++ b/spot_driver/config/spot_ros_example.yaml @@ -26,7 +26,7 @@ # These parameters may be overridden by the 'spot_name' and 'tf_prefix' launch arguments. spot_name: "" - # frame_prefix: "" # Set an explicit prefix for all frames in the tf2 tree (an empty string is a valid prefix option). + # frame_prefix: "" # Set an explicit prefix for all frames in your TF tree (an empty string is a valid prefix option). 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/conversions/robot_state.hpp b/spot_driver/include/spot_driver/conversions/robot_state.hpp index 3acda4393..09f586cf5 100644 --- a/spot_driver/include/spot_driver/conversions/robot_state.hpp +++ b/spot_driver/include/spot_driver/conversions/robot_state.hpp @@ -23,9 +23,20 @@ #include #include #include +#include namespace spot_ros2 { +namespace type_traits { +template +static constexpr bool is_iterable{}; +template +inline static constexpr bool + is_iterable().begin()), decltype(std::declval().end())>> = + std::is_same_v().begin()), typename T::iterator>&& + std::is_same_v().end()), typename T::iterator>; +} // namespace type_traits + /** * @brief Mapping from the joint names used within the Spot API to the joint names used in the Spot driver and URDF. * @details As an example, "fl.hx" is the name of the joint in the Spot API, and "front_left_hip_x" is the name used for @@ -41,7 +52,9 @@ inline const std::map kFriendlyJointNames {"arm0.wr1", "arm_wr1"}, {"arm0.f1x", "arm_f1x"}, }; -static constexpr std::array kValidOdomFrameOptions{"odom", "vision"}; +static constexpr std::array kValidOdomFrameNames{"odom", "vision"}; + +static constexpr std::array kValidTFRootFrameNames{"odom", "vision", "body"}; /** * @brief Given an input string and a prefix string which is a substring starting at the beginning of the input string, @@ -64,14 +77,33 @@ std::string stripPrefix(const std::string& input, const std::string& prefix); std::string prependPrefix(const std::string& input, const std::string& prefix); /** - * @brief Given an input frame string and a prefix string, return a new optional string which is guaranteed to be a - * valid odom frame option. If a valid option cannot be created, std::nullopt is returned instead. + * @brief Given an input frame string, a prefix string, and a set of valid base names, return a new optional string + * which is guaranteed to be a valid frame option. If a valid option cannot be created, std::nullopt is returned + * instead. * @param frame * @param prefix - * @return A new optional string which is guaranteed to be a valid odom frame option. If a valid option cannot be - * created, std::nullopt is returned instead. + * @param base_names + * @return A new optional string which is guaranteed to be a valid frame option w.r.t. base_names. If a valid option + * cannot be created, std::nullopt is returned instead. */ -std::optional validatePreferredOdomFrame(const std::string& frame, const std::string& prefix); +template +static constexpr std::optional validateFrameWithPrefix(const std::string& frame, const std::string& prefix, + const T& base_names) { + static_assert(type_traits::is_iterable, "Trait bound not satisfied for argument 'base_names', type not iterable."); + static_assert(std::is_convertible_v, + "Trait bound not satisfied for argument 'base_names', iterator values not convertible to string."); + + // Make sure we already have the frame and prefix combined. + const std::string frame_with_prefix = prependPrefix(frame, prefix); + + // Compare the given prefixed frame with all valid prefixed options and set false if no match is found. + const bool is_valid = + std::find_if(base_names.begin(), base_names.end(), [&prefix, &frame_with_prefix](const auto& option) -> bool { + return frame_with_prefix == (prefix + option); + }) != base_names.end(); + + return is_valid ? std::make_optional(frame_with_prefix) : std::nullopt; +} /** * @brief Create a BatteryStateArray ROS message by parsing a RobotState message and applying a clock skew to it. diff --git a/spot_driver/spot_driver/launch/spot_launch_helpers.py b/spot_driver/spot_driver/launch/spot_launch_helpers.py index c5f19a10b..ddb1e0f5d 100755 --- a/spot_driver/spot_driver/launch/spot_launch_helpers.py +++ b/spot_driver/spot_driver/launch/spot_launch_helpers.py @@ -24,6 +24,7 @@ _PORT: Literal["port"] = "port" _CAMERAS_USED: Literal["cameras_used"] = "cameras_used" _GRIPPERLESS: Literal["gripperless"] = "gripperless" +_SPOT_NAME: Literal["spot_name"] = "spot_name" IMAGE_PUBLISHER_ARGS = [ @@ -178,9 +179,8 @@ def get_login_parameters(config_file_path: str) -> Tuple[str, str, str, Optional f"[Username: '{username}' Password: '{password}' Hostname: '{hostname}']. Ensure that your environment " "variables are set or update your config_file yaml." ) - #FIXME(frame-prefix): Export the `spot_name` param into the new literal constants. - if "spot_name" in ros_params and ros_params["spot_name"]: - spot_name = ros_params["spot_name"] + if _SPOT_NAME in ros_params and ros_params[_SPOT_NAME]: + spot_name = ros_params[_SPOT_NAME] return username, password, hostname, port, certificate, spot_name @@ -253,7 +253,7 @@ def spot_has_arm(config_file_path: str) -> bool: bool: True if spot has an arm, False otherwise """ logger = logging.getLogger("spot_driver_launch") - username, password, hostname, port, certificate, spot_name = get_login_parameters(config_file_path) + username, password, hostname, port, certificate, _ = get_login_parameters(config_file_path) gripperless = get_gripperless(get_ros_param_dict(config_file_path)) spot_wrapper = SpotWrapper( username=username, diff --git a/spot_driver/src/conversions/robot_state.cpp b/spot_driver/src/conversions/robot_state.cpp index 4761f319a..252db60db 100644 --- a/spot_driver/src/conversions/robot_state.cpp +++ b/spot_driver/src/conversions/robot_state.cpp @@ -34,19 +34,6 @@ std::string prependPrefix(const std::string& input, const std::string& prefix) { return input.find(prefix) == 0 ? input : prefix + input; } -std::optional validatePreferredOdomFrame(const std::string& frame, const std::string& prefix) { - // Make sure we already have the frame and prefix combined. - const std::string frame_with_prefix = prependPrefix(frame, prefix); - - // Compare the given prefixed frame with all valid prefixed options and set false if no match is found. - const bool is_valid = std::find_if(kValidOdomFrameOptions.begin(), kValidOdomFrameOptions.end(), - [&prefix, &frame_with_prefix](const auto& option) -> bool { - return frame_with_prefix == (prefix + option); - }) != kValidOdomFrameOptions.end(); - - return is_valid ? std::make_optional(frame_with_prefix) : std::nullopt; -} - spot_msgs::msg::BatteryStateArray getBatteryStates(const ::bosdyn::api::RobotState& robot_state, const google::protobuf::Duration& clock_skew) { spot_msgs::msg::BatteryStateArray battery_states; diff --git a/spot_driver/src/object_sync/object_synchronizer.cpp b/spot_driver/src/object_sync/object_synchronizer.cpp index 56763064a..9a04a1c9c 100644 --- a/spot_driver/src/object_sync/object_synchronizer.cpp +++ b/spot_driver/src/object_sync/object_synchronizer.cpp @@ -309,16 +309,15 @@ ObjectSynchronizer::ObjectSynchronizer(const std::shared_ptrgetFramePrefixWithDefaultFallback(); - //FIXME(frame-prefix): This is now using a different parameter, refactor this together with the associated - // `kValidOdomFrameOptions` and `validatePreferredOdomFrame` to reflect the change. - const std::string preferred_odom_frame = parameter_interface_->getTFRoot(); - const std::optional valid_odom_frame = validatePreferredOdomFrame(preferred_odom_frame, frame_prefix_); - preferred_base_frame_ = stripPrefix(valid_odom_frame.value_or(kValidOdomFrameOptions[0]), frame_prefix_); - preferred_base_frame_with_prefix_ = valid_odom_frame.value_or(frame_prefix_ + kValidOdomFrameOptions[0]); - if (!valid_odom_frame.has_value()) { - logger_interface_->logWarn(std::string{"Given preferred odom frame '"}.append( - preferred_odom_frame + "' could not be composed into any valid option, defaulting to: '" + - preferred_base_frame_with_prefix_ + "'.")); + const std::string tf_root = parameter_interface_->getTFRoot(); + const std::optional valid_tf_root = + validateFrameWithPrefix(tf_root, frame_prefix_, kValidTFRootFrameNames); + preferred_base_frame_ = stripPrefix(valid_tf_root.value_or(kValidTFRootFrameNames[0]), frame_prefix_); + preferred_base_frame_with_prefix_ = valid_tf_root.value_or(frame_prefix_ + kValidTFRootFrameNames[0]); + if (!valid_tf_root.has_value()) { + logger_interface_->logWarn(std::string{"Given TF root frame '"}.append( + tf_root + "' could not be composed into any valid option with prefix '" + frame_prefix_ + + "', defaulting to: '" + preferred_base_frame_with_prefix_ + "'.")); } // 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 3cc48001d..2996d5c03 100644 --- a/spot_driver/src/robot_state/state_publisher.cpp +++ b/spot_driver/src/robot_state/state_publisher.cpp @@ -32,25 +32,24 @@ StatePublisher::StatePublisher(const std::shared_ptr& stat timer_interface_{std::move(timer_interface)} { frame_prefix_ = parameter_interface_->getFramePrefixWithDefaultFallback(); - //FIXME(frame-prefix): This no longer being used for the full frame ID so the validation checks with prefix don't have to be performed here. const std::string preferred_odom_frame = parameter_interface_->getPreferredOdomFrame(); - const std::optional valid_odom_frame = validatePreferredOdomFrame(preferred_odom_frame, frame_prefix_); - is_using_vision_ = stripPrefix(valid_odom_frame.value_or(kValidOdomFrameOptions[0]), frame_prefix_) == "vision"; + const std::optional valid_odom_frame = + validateFrameWithPrefix(preferred_odom_frame, frame_prefix_, kValidOdomFrameNames); + is_using_vision_ = stripPrefix(valid_odom_frame.value_or(kValidOdomFrameNames[0]), frame_prefix_) == "vision"; if (!valid_odom_frame.has_value()) { logger_interface_->logWarn(std::string{"Given preferred odom frame '"}.append( - preferred_odom_frame + "' could not be composed into any valid option, defaulting to: '" + full_odom_frame_id_ + - "'.")); + preferred_odom_frame + "' could not be composed into any valid option with prefix '" + frame_prefix_ + + "', defaulting to: '" + kValidOdomFrameNames[0] + "'.")); } - //FIXME(frame-prefix): This is now using a different parameter, refactor this with the associated - // `validatePreferredOdomFrame` and `kValidOdomFrameOptions` to reflect that. const std::string tf_root = parameter_interface_->getTFRoot(); - const std::optional valid_tf_root = validatePreferredOdomFrame(tf_root, frame_prefix_); - full_tf_root_id_ = valid_tf_root.value_or(frame_prefix_ + kValidOdomFrameOptions[0]); + const std::optional valid_tf_root = + validateFrameWithPrefix(tf_root, frame_prefix_, kValidTFRootFrameNames); + full_tf_root_id_ = valid_tf_root.value_or(frame_prefix_ + kValidTFRootFrameNames[0]); if (!valid_tf_root.has_value()) { logger_interface_->logWarn(std::string{"Given TF root frame '"}.append( - tf_root + "' could not be composed into any valid option, defaulting to: '" + full_tf_root_id_ + - "'.")); + tf_root + "' could not be composed into any valid option with prefix '" + frame_prefix_ + + "', defaulting to: '" + full_tf_root_id_ + "'.")); } // Create a timer to request and publish robot state at a fixed rate diff --git a/spot_driver/test/pytests/test_ros_interfaces.py b/spot_driver/test/pytests/test_ros_interfaces.py index c166fba4d..6c5c31eef 100644 --- a/spot_driver/test/pytests/test_ros_interfaces.py +++ b/spot_driver/test/pytests/test_ros_interfaces.py @@ -26,7 +26,6 @@ def setUp(self) -> None: rclpy.parameter.Parameter("spot_name", value="Mock_spot"), rclpy.parameter.Parameter("mock_enable", value=True), rclpy.parameter.Parameter("mock_has_arm", value=False), - rclpy.parameter.Parameter("preferred_odom_frame", value="vision"), ], ) @@ -1022,7 +1021,6 @@ def test_robot_command_goal_complete(self) -> None: def test_frame_prefix_from_spot_name(self) -> None: self.assertEqual(self.spot_ros2.frame_prefix, "Mock_spot/", "spot_name not used in frame_prefix") - self.assertEqual(self.spot_ros2.preferred_odom_frame, "Mock_spot/vision", "spot_name not used in frame_prefix") if __name__ == "__main__": diff --git a/spot_driver/test/src/conversions/test_robot_state.cpp b/spot_driver/test/src/conversions/test_robot_state.cpp index 793529bf3..53c3b39cb 100644 --- a/spot_driver/test/src/conversions/test_robot_state.cpp +++ b/spot_driver/test/src/conversions/test_robot_state.cpp @@ -845,7 +845,7 @@ TEST(RobotStateConversions, TestGetBehaviorFaultStateNoFaults) { TEST(RobotStateConversions, TestFramePrefixParsing) { // GIVEN we have some frame name and prefix const std::string frame = "some_frame"; - const std::string prefix = "some_frame_prefix/"; + static constexpr auto prefix = "some_frame_prefix/"; // WHEN we try to modify an input frame, which does not contain the prefix at the beginning // THEN we don't strip anything and the output is the same as input @@ -869,31 +869,32 @@ TEST(RobotStateConversions, TestFramePrefixParsing) { ASSERT_THAT(prependPrefix(frame, ""), StrEq(frame)); } -TEST(RobotStateConversions, TestPreferredOdomFrameValidation) { - // GIVEN we have some frame prefix +TEST(RobotStateConversions, TestFrameWithPrefixValidation) { + // GIVEN we have some frame prefix and some valid frame base names const std::string prefix = "some_frame_prefix/"; + static constexpr auto base_names = {"first", "second", "third", "fourth", "fifth"}; - for (const auto& odom_frame : kValidOdomFrameOptions) { - // WHEN we provide valid preferred odom frame options + for (const auto& frame : base_names) { + // WHEN we provide valid frame options // THEN the optional contains the valid prefixed frame - const auto raw_option = validatePreferredOdomFrame(odom_frame, prefix); + const auto raw_option = validateFrameWithPrefix(frame, prefix, base_names); ASSERT_THAT(raw_option.has_value(), IsTrue()); - ASSERT_THAT(raw_option.value(), StrEq(prefix + odom_frame)); - const auto prefixed_option = validatePreferredOdomFrame(prefix + odom_frame, prefix); + ASSERT_THAT(raw_option.value(), StrEq(prefix + frame)); + const auto prefixed_option = validateFrameWithPrefix(prefix + frame, prefix, base_names); ASSERT_THAT(prefixed_option.has_value(), IsTrue()); - ASSERT_THAT(prefixed_option.value(), StrEq(prefix + odom_frame)); + ASSERT_THAT(prefixed_option.value(), StrEq(prefix + frame)); - // WHEN we provide an invalid preferred odom frame + // WHEN we provide an invalid frame option // THEN the optional is set to nullopt - ASSERT_THAT(validatePreferredOdomFrame(odom_frame + prefix, prefix).has_value(), IsFalse()); - ASSERT_THAT(validatePreferredOdomFrame(prefix + odom_frame + prefix, prefix).has_value(), IsFalse()); + ASSERT_THAT(validateFrameWithPrefix(frame + prefix, prefix, base_names).has_value(), IsFalse()); + ASSERT_THAT(validateFrameWithPrefix(prefix + frame + prefix, prefix, base_names).has_value(), IsFalse()); // GIVEN the frame prefix is empty - // THEN we only have one valid option - const auto option_without_prefix = validatePreferredOdomFrame(odom_frame, ""); + // THEN we only have one possible valid option + const auto option_without_prefix = validateFrameWithPrefix(frame, "", base_names); ASSERT_THAT(option_without_prefix.has_value(), IsTrue()); - ASSERT_THAT(option_without_prefix.value(), StrEq(odom_frame)); - ASSERT_THAT(validatePreferredOdomFrame(prefix + odom_frame, "").has_value(), IsFalse()); + ASSERT_THAT(option_without_prefix.value(), StrEq(frame)); + ASSERT_THAT(validateFrameWithPrefix(prefix + frame, "", base_names).has_value(), IsFalse()); } } } // namespace spot_ros2::test From 23ce6fd72c1f57f17cfe797e5add4a89897109ba Mon Sep 17 00:00:00 2001 From: Imaniac230 <44968160+Imaniac230@users.noreply.github.com> Date: Fri, 21 Feb 2025 18:29:29 +0100 Subject: [PATCH 11/13] fix: Reverted optional robot_name from SpotWrapper. Signed-off-by: Imaniac230 <44968160+Imaniac230@users.noreply.github.com> --- spot_driver/spot_driver/launch/spot_launch_helpers.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/spot_driver/spot_driver/launch/spot_launch_helpers.py b/spot_driver/spot_driver/launch/spot_launch_helpers.py index ddb1e0f5d..99990f60a 100755 --- a/spot_driver/spot_driver/launch/spot_launch_helpers.py +++ b/spot_driver/spot_driver/launch/spot_launch_helpers.py @@ -253,7 +253,7 @@ def spot_has_arm(config_file_path: str) -> bool: bool: True if spot has an arm, False otherwise """ logger = logging.getLogger("spot_driver_launch") - username, password, hostname, port, certificate, _ = get_login_parameters(config_file_path) + username, password, hostname, port, certificate, spot_name = get_login_parameters(config_file_path) gripperless = get_gripperless(get_ros_param_dict(config_file_path)) spot_wrapper = SpotWrapper( username=username, @@ -261,6 +261,7 @@ def spot_has_arm(config_file_path: str) -> bool: hostname=hostname, port=port, cert_resource_glob=certificate, + robot_name=spot_name, logger=logger, gripperless=gripperless, ) From 6247cd96f346a93ecc114ad747121e9d149c2e3e Mon Sep 17 00:00:00 2001 From: Imaniac230 <44968160+Imaniac230@users.noreply.github.com> Date: Fri, 21 Feb 2025 22:15:37 +0100 Subject: [PATCH 12/13] feat: Frame parameter validation logic exported to the parameter interface and simplified - restricting values only to base names, updated tests and README. Signed-off-by: Imaniac230 <44968160+Imaniac230@users.noreply.github.com> --- spot_driver/README.md | 4 +- spot_driver/config/spot_ros_example.yaml | 4 +- .../spot_driver/conversions/robot_state.hpp | 44 ---------------- .../interfaces/parameter_interface_base.hpp | 6 ++- .../interfaces/rclcpp_parameter_interface.cpp | 50 ++++++++++++++++++- .../src/object_sync/object_synchronizer.cpp | 13 ++--- .../src/robot_state/state_publisher.cpp | 22 ++------ .../test/src/conversions/test_robot_state.cpp | 29 ----------- .../test/src/test_parameter_interface.cpp | 26 ++++++++++ 9 files changed, 89 insertions(+), 109 deletions(-) diff --git a/spot_driver/README.md b/spot_driver/README.md index 3a00d856c..a6930647a 100644 --- a/spot_driver/README.md +++ b/spot_driver/README.md @@ -23,9 +23,9 @@ If you use the config file parameter `frame_prefix`, you can disable prefixing a ## Frames Background information about Spot's frames from Boston Dynamics can be found [here](https://dev.bostondynamics.com/docs/concepts/geometry_and_frames). By default, the Spot driver will place the "odom" frame as the root of the TF tree. -This can be changed by setting the `tf_root` parameter in your config file to either "vision" or "body". +This can be changed by setting the `tf_root` parameter in your config file to either "vision" or "body" (value must be given without a prefix). The Spot driver will also publish odometry topics with respect to the "odom" frame by default. -If you wish to change this to "vision", update the `preferred_odom_frame` parameter in your config file. +If you wish to change this to "vision", update the `preferred_odom_frame` parameter in your config file (value must be given without a prefix). ## Simple Robot Commands Many simple robot commands can be called as services from the command line once the driver is running. For example: diff --git a/spot_driver/config/spot_ros_example.yaml b/spot_driver/config/spot_ros_example.yaml index c4968f9d6..45c04d3da 100644 --- a/spot_driver/config/spot_ros_example.yaml +++ b/spot_driver/config/spot_ros_example.yaml @@ -21,8 +21,8 @@ estop_timeout: 9.0 start_estop: False - 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. + preferred_odom_frame: "odom" # Allowed values are: [odom, vision], without a prefix. 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" # Allowed values are: [odom, vision, body], without a prefix. This will set the root frame in your TF tree. # These parameters may be overridden by the 'spot_name' and 'tf_prefix' launch arguments. spot_name: "" diff --git a/spot_driver/include/spot_driver/conversions/robot_state.hpp b/spot_driver/include/spot_driver/conversions/robot_state.hpp index 09f586cf5..b1882c488 100644 --- a/spot_driver/include/spot_driver/conversions/robot_state.hpp +++ b/spot_driver/include/spot_driver/conversions/robot_state.hpp @@ -23,20 +23,9 @@ #include #include #include -#include namespace spot_ros2 { -namespace type_traits { -template -static constexpr bool is_iterable{}; -template -inline static constexpr bool - is_iterable().begin()), decltype(std::declval().end())>> = - std::is_same_v().begin()), typename T::iterator>&& - std::is_same_v().end()), typename T::iterator>; -} // namespace type_traits - /** * @brief Mapping from the joint names used within the Spot API to the joint names used in the Spot driver and URDF. * @details As an example, "fl.hx" is the name of the joint in the Spot API, and "front_left_hip_x" is the name used for @@ -52,10 +41,6 @@ inline const std::map kFriendlyJointNames {"arm0.wr1", "arm_wr1"}, {"arm0.f1x", "arm_f1x"}, }; -static constexpr std::array kValidOdomFrameNames{"odom", "vision"}; - -static constexpr std::array kValidTFRootFrameNames{"odom", "vision", "body"}; - /** * @brief Given an input string and a prefix string which is a substring starting at the beginning of the input string, * return a new string which is the difference between the input string and the prefix string. @@ -76,35 +61,6 @@ std::string stripPrefix(const std::string& input, const std::string& prefix); */ std::string prependPrefix(const std::string& input, const std::string& prefix); -/** - * @brief Given an input frame string, a prefix string, and a set of valid base names, return a new optional string - * which is guaranteed to be a valid frame option. If a valid option cannot be created, std::nullopt is returned - * instead. - * @param frame - * @param prefix - * @param base_names - * @return A new optional string which is guaranteed to be a valid frame option w.r.t. base_names. If a valid option - * cannot be created, std::nullopt is returned instead. - */ -template -static constexpr std::optional validateFrameWithPrefix(const std::string& frame, const std::string& prefix, - const T& base_names) { - static_assert(type_traits::is_iterable, "Trait bound not satisfied for argument 'base_names', type not iterable."); - static_assert(std::is_convertible_v, - "Trait bound not satisfied for argument 'base_names', iterator values not convertible to string."); - - // Make sure we already have the frame and prefix combined. - const std::string frame_with_prefix = prependPrefix(frame, prefix); - - // Compare the given prefixed frame with all valid prefixed options and set false if no match is found. - const bool is_valid = - std::find_if(base_names.begin(), base_names.end(), [&prefix, &frame_with_prefix](const auto& option) -> bool { - return frame_with_prefix == (prefix + option); - }) != base_names.end(); - - return is_valid ? std::make_optional(frame_with_prefix) : std::nullopt; -} - /** * @brief Create a BatteryStateArray ROS message by parsing a RobotState message and applying a clock skew to it. * 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 67ef4ba7a..1c7c737d5 100644 --- a/spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp +++ b/spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp @@ -63,8 +63,10 @@ class ParameterInterfaceBase { static constexpr bool kDefaultPublishCompressedImages{false}; static constexpr bool kDefaultPublishDepthImages{true}; static constexpr bool kDefaultPublishDepthRegisteredImages{true}; - static constexpr auto kDefaultPreferredOdomFrame = "odom"; - static constexpr auto kDefaultTFRoot = "odom"; + static constexpr std::array kValidOdomFrameNames{"odom", "vision"}; + static constexpr std::array kValidTFRootFrameNames{"odom", "vision", "body"}; + static constexpr auto kDefaultPreferredOdomFrame = kValidOdomFrameNames[0]; + static constexpr auto kDefaultTFRoot = kValidTFRootFrameNames[0]; 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/src/interfaces/rclcpp_parameter_interface.cpp b/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp index be5f4cd80..2c4c625b2 100644 --- a/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp +++ b/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp @@ -4,6 +4,7 @@ #include #include +#include #include namespace { @@ -33,6 +34,16 @@ constexpr auto kParameterFramePrefix = "frame_prefix"; constexpr auto kParameterNameGripperless = "gripperless"; constexpr auto kParameterTimeSyncTimeout = "timesync_timeout"; +namespace type_traits { +template +static constexpr bool is_iterable{}; +template +inline static constexpr bool + is_iterable().begin()), decltype(std::declval().end())>> = + std::is_same_v().begin()), typename T::iterator>&& + std::is_same_v().end()), typename T::iterator>; +} // namespace type_traits + /** * @brief Get a rclcpp parameter. If the parameter has not been declared, declare it with the provided default value and * then return the value. @@ -133,6 +144,29 @@ std::string getEnvironmentVariableParameterFallback(const std::shared_ptr(node, parameter_name, default_value); } +/** + * @brief Given an input frame string parameter and a set of valid base names, return a new optional string + * which is guaranteed to be a valid frame option. If the input parameter is not a valid option, std::nullopt is + * returned instead. + * @param frame + * @param base_names + * @return A new optional string which is guaranteed to be a valid frame option w.r.t. base_names. If the input + * parameter is not a valid option, std::nullopt is returned instead. + */ +template +static constexpr std::optional validateFrameParameter(const std::string& frame, + const OptionsT& base_names) { + static_assert(type_traits::is_iterable, + "Trait bound not satisfied for argument 'base_names', type not iterable."); + static_assert(std::is_convertible_v, + "Trait bound not satisfied for argument 'base_names', iterator values not convertible to string."); + + // Compare the given frame with all valid options and set false if no match is found. + const bool is_valid = std::find(base_names.begin(), base_names.end(), frame) != base_names.end(); + + return is_valid ? std::make_optional(frame) : std::nullopt; +} + } // namespace namespace spot_ros2 { @@ -189,11 +223,23 @@ bool RclcppParameterInterface::getPublishDepthRegisteredImages() const { } std::string RclcppParameterInterface::getPreferredOdomFrame() const { - return declareAndGetParameter(node_, kParameterPreferredOdomFrame, kDefaultPreferredOdomFrame); + const std::string preferred_odom_frame = + declareAndGetParameter(node_, kParameterPreferredOdomFrame, kDefaultPreferredOdomFrame); + const std::optional valid_preferred_odom_frame = + validateFrameParameter(preferred_odom_frame, kValidOdomFrameNames); + if (!valid_preferred_odom_frame.has_value()) + RCLCPP_WARN(node_->get_logger(), "Given preferred odom frame '%s' is not a valid option, defaulting to '%s'.", + preferred_odom_frame.c_str(), kDefaultPreferredOdomFrame); + return valid_preferred_odom_frame.value_or(kDefaultPreferredOdomFrame); } std::string RclcppParameterInterface::getTFRoot() const { - return declareAndGetParameter(node_, kParameterTFRoot, kDefaultTFRoot); + const std::string tf_root = declareAndGetParameter(node_, kParameterTFRoot, kDefaultTFRoot); + const std::optional valid_tf_root = validateFrameParameter(tf_root, kValidTFRootFrameNames); + if (!valid_tf_root.has_value()) + RCLCPP_WARN(node_->get_logger(), "Given TF root frame '%s' is not a valid option, defaulting to '%s'.", + tf_root.c_str(), kDefaultTFRoot); + return valid_tf_root.value_or(kDefaultTFRoot); } std::optional RclcppParameterInterface::getFramePrefix() const { diff --git a/spot_driver/src/object_sync/object_synchronizer.cpp b/spot_driver/src/object_sync/object_synchronizer.cpp index 9a04a1c9c..d8f6fadbf 100644 --- a/spot_driver/src/object_sync/object_synchronizer.cpp +++ b/spot_driver/src/object_sync/object_synchronizer.cpp @@ -309,16 +309,9 @@ ObjectSynchronizer::ObjectSynchronizer(const std::shared_ptrgetFramePrefixWithDefaultFallback(); - const std::string tf_root = parameter_interface_->getTFRoot(); - const std::optional valid_tf_root = - validateFrameWithPrefix(tf_root, frame_prefix_, kValidTFRootFrameNames); - preferred_base_frame_ = stripPrefix(valid_tf_root.value_or(kValidTFRootFrameNames[0]), frame_prefix_); - preferred_base_frame_with_prefix_ = valid_tf_root.value_or(frame_prefix_ + kValidTFRootFrameNames[0]); - if (!valid_tf_root.has_value()) { - logger_interface_->logWarn(std::string{"Given TF root frame '"}.append( - tf_root + "' could not be composed into any valid option with prefix '" + frame_prefix_ + - "', defaulting to: '" + preferred_base_frame_with_prefix_ + "'.")); - } + const auto tf_root = parameter_interface_->getTFRoot(); + preferred_base_frame_ = tf_root; + preferred_base_frame_with_prefix_ = frame_prefix_ + tf_root; // TODO(khughes): This is temporarily disabled to reduce driver's spew about TF extrapolation. // world_object_update_timer_->setTimer(kWorldObjectSyncPeriod, [this]() { diff --git a/spot_driver/src/robot_state/state_publisher.cpp b/spot_driver/src/robot_state/state_publisher.cpp index 2996d5c03..bdbbce11b 100644 --- a/spot_driver/src/robot_state/state_publisher.cpp +++ b/spot_driver/src/robot_state/state_publisher.cpp @@ -32,25 +32,11 @@ StatePublisher::StatePublisher(const std::shared_ptr& stat timer_interface_{std::move(timer_interface)} { frame_prefix_ = parameter_interface_->getFramePrefixWithDefaultFallback(); - const std::string preferred_odom_frame = parameter_interface_->getPreferredOdomFrame(); - const std::optional valid_odom_frame = - validateFrameWithPrefix(preferred_odom_frame, frame_prefix_, kValidOdomFrameNames); - is_using_vision_ = stripPrefix(valid_odom_frame.value_or(kValidOdomFrameNames[0]), frame_prefix_) == "vision"; - if (!valid_odom_frame.has_value()) { - logger_interface_->logWarn(std::string{"Given preferred odom frame '"}.append( - preferred_odom_frame + "' could not be composed into any valid option with prefix '" + frame_prefix_ + - "', defaulting to: '" + kValidOdomFrameNames[0] + "'.")); - } + const auto preferred_odom_frame = parameter_interface_->getPreferredOdomFrame(); + is_using_vision_ = preferred_odom_frame == "vision"; - const std::string tf_root = parameter_interface_->getTFRoot(); - const std::optional valid_tf_root = - validateFrameWithPrefix(tf_root, frame_prefix_, kValidTFRootFrameNames); - full_tf_root_id_ = valid_tf_root.value_or(frame_prefix_ + kValidTFRootFrameNames[0]); - if (!valid_tf_root.has_value()) { - logger_interface_->logWarn(std::string{"Given TF root frame '"}.append( - tf_root + "' could not be composed into any valid option with prefix '" + frame_prefix_ + - "', defaulting to: '" + full_tf_root_id_ + "'.")); - } + const auto tf_root = parameter_interface_->getTFRoot(); + full_tf_root_id_ = frame_prefix_ + tf_root; // Create a timer to request and publish robot state at a fixed rate timer_interface_->setTimer(kRobotStateCallbackPeriod, [this] { diff --git a/spot_driver/test/src/conversions/test_robot_state.cpp b/spot_driver/test/src/conversions/test_robot_state.cpp index 53c3b39cb..df2f39b9f 100644 --- a/spot_driver/test/src/conversions/test_robot_state.cpp +++ b/spot_driver/test/src/conversions/test_robot_state.cpp @@ -868,33 +868,4 @@ TEST(RobotStateConversions, TestFramePrefixParsing) { ASSERT_THAT(stripPrefix(frame, ""), StrEq(frame)); ASSERT_THAT(prependPrefix(frame, ""), StrEq(frame)); } - -TEST(RobotStateConversions, TestFrameWithPrefixValidation) { - // GIVEN we have some frame prefix and some valid frame base names - const std::string prefix = "some_frame_prefix/"; - static constexpr auto base_names = {"first", "second", "third", "fourth", "fifth"}; - - for (const auto& frame : base_names) { - // WHEN we provide valid frame options - // THEN the optional contains the valid prefixed frame - const auto raw_option = validateFrameWithPrefix(frame, prefix, base_names); - ASSERT_THAT(raw_option.has_value(), IsTrue()); - ASSERT_THAT(raw_option.value(), StrEq(prefix + frame)); - const auto prefixed_option = validateFrameWithPrefix(prefix + frame, prefix, base_names); - ASSERT_THAT(prefixed_option.has_value(), IsTrue()); - ASSERT_THAT(prefixed_option.value(), StrEq(prefix + frame)); - - // WHEN we provide an invalid frame option - // THEN the optional is set to nullopt - ASSERT_THAT(validateFrameWithPrefix(frame + prefix, prefix, base_names).has_value(), IsFalse()); - ASSERT_THAT(validateFrameWithPrefix(prefix + frame + prefix, prefix, base_names).has_value(), IsFalse()); - - // GIVEN the frame prefix is empty - // THEN we only have one possible valid option - const auto option_without_prefix = validateFrameWithPrefix(frame, "", base_names); - ASSERT_THAT(option_without_prefix.has_value(), IsTrue()); - ASSERT_THAT(option_without_prefix.value(), StrEq(frame)); - ASSERT_THAT(validateFrameWithPrefix(prefix + frame, "", base_names).has_value(), IsFalse()); - } -} } // namespace spot_ros2::test diff --git a/spot_driver/test/src/test_parameter_interface.cpp b/spot_driver/test/src/test_parameter_interface.cpp index 9ff4a7fb6..1efebdb46 100644 --- a/spot_driver/test/src/test_parameter_interface.cpp +++ b/spot_driver/test/src/test_parameter_interface.cpp @@ -542,4 +542,30 @@ TEST_F(RclcppParameterInterfaceTest, GetFramePrefixFromExplicitParameter) { // Finish second test scenario. verifyExpectedFramePrefix(node, namespaced_node); } + +TEST_F(RclcppParameterInterfaceTest, GetPreferredOdomFrameWithInvalidOption) { + // GIVEN we create an rclcpp node and set the preferred odom frame config parameter to an invalid value + const auto node = std::make_shared(kNodeName); + node->declare_parameter("preferred_odom_frame", "visionvision"); + + // GIVEN we create a RclcppParameterInterface using this node + RclcppParameterInterface parameter_interface{node}; + + // WHEN we call getPreferredOdomFrame + // THEN the parameter interface returns the default option + EXPECT_THAT(parameter_interface.getPreferredOdomFrame(), StrEq("odom")); +} + +TEST_F(RclcppParameterInterfaceTest, GetTFRootWithInvalidOption) { + // GIVEN we create an rclcpp node and set the TF root frame config parameter to an invalid value + const auto node = std::make_shared(kNodeName); + node->declare_parameter("tf_root", "visionvision"); + + // GIVEN we create a RclcppParameterInterface using this node + RclcppParameterInterface parameter_interface{node}; + + // WHEN we call getTFRoot + // THEN the parameter interface returns the default option + EXPECT_THAT(parameter_interface.getTFRoot(), StrEq("odom")); +} } // namespace spot_ros2::test From e9e0decc9ba6c0116e9f6726c283761ff1124680 Mon Sep 17 00:00:00 2001 From: Imaniac230 <44968160+Imaniac230@users.noreply.github.com> Date: Fri, 21 Feb 2025 22:55:25 +0100 Subject: [PATCH 13/13] fix: Streamline the initialization in`StatePublisher` and `ObjectSynchronizer` - does not have to create additional variables. Signed-off-by: Imaniac230 <44968160+Imaniac230@users.noreply.github.com> --- spot_driver/src/object_sync/object_synchronizer.cpp | 6 ++---- spot_driver/src/robot_state/state_publisher.cpp | 8 ++------ 2 files changed, 4 insertions(+), 10 deletions(-) diff --git a/spot_driver/src/object_sync/object_synchronizer.cpp b/spot_driver/src/object_sync/object_synchronizer.cpp index d8f6fadbf..287fcec1c 100644 --- a/spot_driver/src/object_sync/object_synchronizer.cpp +++ b/spot_driver/src/object_sync/object_synchronizer.cpp @@ -308,10 +308,8 @@ ObjectSynchronizer::ObjectSynchronizer(const std::shared_ptrgetFramePrefixWithDefaultFallback(); - - const auto tf_root = parameter_interface_->getTFRoot(); - preferred_base_frame_ = tf_root; - preferred_base_frame_with_prefix_ = frame_prefix_ + tf_root; + preferred_base_frame_ = parameter_interface_->getTFRoot(); + preferred_base_frame_with_prefix_ = frame_prefix_ + preferred_base_frame_; // TODO(khughes): This is temporarily disabled to reduce driver's spew about TF extrapolation. // world_object_update_timer_->setTimer(kWorldObjectSyncPeriod, [this]() { diff --git a/spot_driver/src/robot_state/state_publisher.cpp b/spot_driver/src/robot_state/state_publisher.cpp index bdbbce11b..d78be357a 100644 --- a/spot_driver/src/robot_state/state_publisher.cpp +++ b/spot_driver/src/robot_state/state_publisher.cpp @@ -31,12 +31,8 @@ StatePublisher::StatePublisher(const std::shared_ptr& stat tf_broadcaster_interface_{std::move(tf_broadcaster_interface)}, timer_interface_{std::move(timer_interface)} { frame_prefix_ = parameter_interface_->getFramePrefixWithDefaultFallback(); - - const auto preferred_odom_frame = parameter_interface_->getPreferredOdomFrame(); - is_using_vision_ = preferred_odom_frame == "vision"; - - const auto tf_root = parameter_interface_->getTFRoot(); - full_tf_root_id_ = frame_prefix_ + tf_root; + is_using_vision_ = parameter_interface_->getPreferredOdomFrame() == "vision"; + full_tf_root_id_ = frame_prefix_ + parameter_interface_->getTFRoot(); // Create a timer to request and publish robot state at a fixed rate timer_interface_->setTimer(kRobotStateCallbackPeriod, [this] {