diff --git a/ros_utilities b/ros_utilities index 23a79c95b..539c962f4 160000 --- a/ros_utilities +++ b/ros_utilities @@ -1 +1 @@ -Subproject commit 23a79c95b163b4e791810309df5d54bb6341a58e +Subproject commit 539c962f42b51cd4206be3f0348b5984384f0cd2 diff --git a/spot_description/launch/description.launch.py b/spot_description/launch/description.launch.py index bd1ea47b5..0398d0a75 100644 --- a/spot_description/launch/description.launch.py +++ b/spot_description/launch/description.launch.py @@ -39,6 +39,12 @@ def generate_launch_description() -> launch.LaunchDescription: choices=["True", "true", "False", "false"], description="Flag to enable putting frames at the feet", ), + DeclareLaunchArgument( + name="gripperless", + default_value="False", + choices=["True", "true", "False", "false"], + description="Flag to remove the gripper from the arm model", + ), DeclareLaunchArgument( "tf_prefix", default_value='""', description="Apply namespace prefix to robot links and joints" ), @@ -56,6 +62,8 @@ def generate_launch_description() -> launch.LaunchDescription: LaunchConfiguration("arm"), " feet:=", LaunchConfiguration("feet"), + " gripperless:=", + LaunchConfiguration("gripperless"), " tf_prefix:=", LaunchConfiguration("tf_prefix"), ] diff --git a/spot_description/urdf/spot.ros2_control.xacro b/spot_description/urdf/spot.ros2_control.xacro index 216219c26..2e8d6af70 100644 --- a/spot_description/urdf/spot.ros2_control.xacro +++ b/spot_description/urdf/spot.ros2_control.xacro @@ -20,10 +20,6 @@ ${-effort_max} ${effort_max} - - @@ -65,7 +61,7 @@ - + @@ -80,6 +76,8 @@ $(optenv SPOT_IP ${hostname}) $(optenv BOSDYN_CLIENT_USERNAME ${username}) $(optenv BOSDYN_CLIENT_PASSWORD ${password}) + ${k_q_p} + ${k_qd_p} diff --git a/spot_description/urdf/spot.urdf.xacro b/spot_description/urdf/spot.urdf.xacro index ed0ad8faa..51774251e 100644 --- a/spot_description/urdf/spot.urdf.xacro +++ b/spot_description/urdf/spot.urdf.xacro @@ -7,23 +7,34 @@ + + + + + + + + + + - - + + + tf_prefix="$(arg tf_prefix)" + gripperless="$(arg gripperless)"/> @@ -32,7 +43,9 @@ hostname="$(arg hostname)" username="$(arg username)" password="$(arg password)" - tf_prefix="$(arg tf_prefix)" /> + tf_prefix="$(arg tf_prefix)" + k_q_p="$(arg k_q_p)" + k_qd_p="$(arg k_qd_p)" /> diff --git a/spot_description/urdf/spot_arm_macro.urdf b/spot_description/urdf/spot_arm_macro.urdf index 146c9bca3..d8a848d7f 100644 --- a/spot_description/urdf/spot_arm_macro.urdf +++ b/spot_description/urdf/spot_arm_macro.urdf @@ -1,7 +1,12 @@ + tf_prefix + gripperless:=false + custom_gripper_base_link:=''"> + + + @@ -201,107 +206,127 @@ upper="1.83259571459404613236" /> - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/spot_description/urdf/spot_macro.xacro b/spot_description/urdf/spot_macro.xacro index 5f947457c..f145f1b96 100644 --- a/spot_description/urdf/spot_macro.xacro +++ b/spot_description/urdf/spot_macro.xacro @@ -4,6 +4,8 @@ @@ -417,7 +419,9 @@ - + diff --git a/spot_driver/config/spot_ros_example.yaml b/spot_driver/config/spot_ros_example.yaml index a19a1158a..00d850036 100644 --- a/spot_driver/config/spot_ros_example.yaml +++ b/spot_driver/config/spot_ros_example.yaml @@ -21,7 +21,8 @@ estop_timeout: 9.0 start_estop: False - preferred_odom_frame: "odom" # pass either odom/vision. This frame will become the parent of body in tf2 tree and will be used in odometry topic. https://dev.bostondynamics.com/docs/concepts/geometry_and_frames.html?highlight=frame#frames-in-the-spot-robot-world for more info. + preferred_odom_frame: "odom" # pass either odom/vision. This will be used in the odometry topic. https://dev.bostondynamics.com/docs/concepts/geometry_and_frames.html?highlight=frame#frames-in-the-spot-robot-world for more info. + tf_root: "odom" # Use "odom" (default), "vision", or "body" for the root frame in your TF tree. cmd_duration: 0.25 # The duration of cmd_vel commands. Increase this if spot stutters when publishing cmd_vel. rgb_cameras: True # Set to False if your robot has greyscale cameras -- otherwise you won't receive data. diff --git a/spot_driver/include/spot_driver/conversions/robot_state.hpp b/spot_driver/include/spot_driver/conversions/robot_state.hpp index 3997403cf..7aed87e2a 100644 --- a/spot_driver/include/spot_driver/conversions/robot_state.hpp +++ b/spot_driver/include/spot_driver/conversions/robot_state.hpp @@ -134,8 +134,9 @@ std::optional getTf(const ::bosdyn::api::FrameTreeSnap * @return If the robot state message contains the velocity of the Spot's body relative to the odometry frame in its * kinematic state, return a TwistWithCovarianceStamped containing this data. Otherwise, return nullopt. */ -std::optional getOdomTwist( - const ::bosdyn::api::RobotState& robot_state, const google::protobuf::Duration& clock_skew); +std::optional getOdomTwist(const ::bosdyn::api::RobotState& robot_state, + const google::protobuf::Duration& clock_skew, + const bool is_using_vision); /** * @brief Create an Odometry ROS message representing Spot's pose and velocity relative to a fixed world frame by @@ -152,7 +153,7 @@ std::optional getOdomTwist( */ std::optional getOdom(const ::bosdyn::api::RobotState& robot_state, const google::protobuf::Duration& clock_skew, const std::string& prefix, - bool is_using_vision); + const bool is_using_vision); /** * @brief Create a PowerState ROS message by parsing a RobotState message. 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 e11fdd360..aa1751c6b 100644 --- a/spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp +++ b/spot_driver/include/spot_driver/interfaces/parameter_interface_base.hpp @@ -40,9 +40,12 @@ class ParameterInterfaceBase { virtual bool getPublishDepthImages() const = 0; virtual bool getPublishDepthRegisteredImages() const = 0; virtual std::string getPreferredOdomFrame() const = 0; + virtual std::string getTFRoot() const = 0; virtual std::string getSpotName() const = 0; - virtual std::set getDefaultCamerasUsed(bool has_arm) const = 0; - virtual tl::expected, std::string> getCamerasUsed(bool has_arm) 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; protected: // These are the definitions of the default values for optional parameters. @@ -57,7 +60,9 @@ class ParameterInterfaceBase { static constexpr bool kDefaultPublishDepthImages{true}; static constexpr bool kDefaultPublishDepthRegisteredImages{true}; static constexpr auto kDefaultPreferredOdomFrame = "odom"; - static constexpr auto kDefaultCamerasUsedWithoutArm = {"frontleft", "frontright", "left", "right", "back"}; - static constexpr auto kDefaultCamerasUsedWithArm = {"frontleft", "frontright", "left", "right", "back", "hand"}; + static constexpr auto kDefaultTFRoot = "odom"; + static constexpr bool kDefaultGripperless{false}; + static constexpr auto kCamerasWithoutHand = {"frontleft", "frontright", "left", "right", "back"}; + static constexpr auto kCamerasWithHand = {"frontleft", "frontright", "left", "right", "back", "hand"}; }; } // namespace spot_ros2 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 1d86f8f7f..0a98a5905 100644 --- a/spot_driver/include/spot_driver/interfaces/rclcpp_parameter_interface.hpp +++ b/spot_driver/include/spot_driver/interfaces/rclcpp_parameter_interface.hpp @@ -35,10 +35,13 @@ class RclcppParameterInterface : public ParameterInterfaceBase { [[nodiscard]] bool getPublishDepthImages() const override; [[nodiscard]] bool getPublishDepthRegisteredImages() const override; [[nodiscard]] std::string getPreferredOdomFrame() const override; + [[nodiscard]] std::string getTFRoot() const override; [[nodiscard]] std::string getSpotName() const override; - [[nodiscard]] std::set getDefaultCamerasUsed(const bool has_arm) 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 override; + const bool has_arm, const bool gripperless) const override; private: std::shared_ptr node_; diff --git a/spot_driver/include/spot_driver/robot_state/state_publisher.hpp b/spot_driver/include/spot_driver/robot_state/state_publisher.hpp index d6a054092..6fe72f7d3 100644 --- a/spot_driver/include/spot_driver/robot_state/state_publisher.hpp +++ b/spot_driver/include/spot_driver/robot_state/state_publisher.hpp @@ -60,7 +60,7 @@ class StatePublisher { */ void timerCallback(); - std::string full_odom_frame_id_; + std::string full_tf_root_id_; std::string frame_prefix_; diff --git a/spot_driver/launch/point_cloud_xyz.launch.py b/spot_driver/launch/point_cloud_xyz.launch.py index 57f2173e9..813ac687b 100644 --- a/spot_driver/launch/point_cloud_xyz.launch.py +++ b/spot_driver/launch/point_cloud_xyz.launch.py @@ -36,6 +36,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from synchros2.launch.actions import update_sigterm_sigkill_timeout def generate_launch_description() -> LaunchDescription: @@ -73,4 +74,5 @@ def generate_launch_description() -> LaunchDescription: ), ] ) + update_sigterm_sigkill_timeout(ld) return ld diff --git a/spot_driver/launch/point_cloud_xyzrgb.launch.py b/spot_driver/launch/point_cloud_xyzrgb.launch.py index 8fbf2f725..32794ecae 100644 --- a/spot_driver/launch/point_cloud_xyzrgb.launch.py +++ b/spot_driver/launch/point_cloud_xyzrgb.launch.py @@ -36,6 +36,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from synchros2.launch.actions import update_sigterm_sigkill_timeout def generate_launch_description() -> LaunchDescription: @@ -76,4 +77,5 @@ def generate_launch_description() -> LaunchDescription: ), ] ) + update_sigterm_sigkill_timeout(ld) return ld diff --git a/spot_driver/launch/rviz.launch.py b/spot_driver/launch/rviz.launch.py index 8d2b92a94..51dda9849 100644 --- a/spot_driver/launch/rviz.launch.py +++ b/spot_driver/launch/rviz.launch.py @@ -13,6 +13,7 @@ PathJoinSubstitution, ) from launch_ros.substitutions import FindPackageShare +from synchros2.launch.actions import update_sigterm_sigkill_timeout THIS_PACKAGE = "spot_driver" @@ -43,6 +44,7 @@ def create_rviz_config(robot_name: str) -> None: def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: + update_sigterm_sigkill_timeout(ld) rviz_config_file = LaunchConfiguration("rviz_config_file").perform(context) spot_name = LaunchConfiguration("spot_name").perform(context) diff --git a/spot_driver/launch/spot_driver.launch.py b/spot_driver/launch/spot_driver.launch.py index 8b07fd7fd..42250bc3f 100644 --- a/spot_driver/launch/spot_driver.launch.py +++ b/spot_driver/launch/spot_driver.launch.py @@ -2,14 +2,14 @@ import os -import launch -import launch_ros from launch import LaunchContext, LaunchDescription from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution, TextSubstitution +from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare +from synchros2.launch.actions import DeclareBooleanLaunchArgument, update_sigterm_sigkill_timeout from spot_driver.launch.spot_launch_helpers import IMAGE_PUBLISHER_ARGS, declare_image_publisher_args, spot_has_arm @@ -17,12 +17,14 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: + update_sigterm_sigkill_timeout(ld) 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) + robot_description_package = LaunchConfiguration("robot_description_package").perform(context) # if config_file has been set (and is not the default empty string) and is also not a file, do not launch anything. config_file_path = config_file.perform(context) @@ -35,10 +37,8 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: else: has_arm = spot_has_arm(config_file_path=config_file.perform(context), spot_name=spot_name) - pkg_share = FindPackageShare("spot_description").find("spot_description") + robot_description_pkg_share = FindPackageShare(robot_description_package).find(robot_description_package) - # 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, @@ -49,7 +49,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: # Merge the two dicts spot_driver_params = {**spot_driver_params, **mock_spot_driver_params} - spot_driver_node = launch_ros.actions.Node( + spot_driver_node = Node( package="spot_driver", executable="spot_ros2", name="spot_ros2", @@ -62,21 +62,22 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: 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( + spot_name_param = {"spot_name": spot_name} + + kinematic_node = Node( package="spot_driver", executable="spot_inverse_kinematics_node", output="screen", - parameters=[config_file, kinematc_node_params], + parameters=[config_file, spot_name_param], namespace=spot_name, ) ld.add_action(kinematic_node) - object_sync_node = launch_ros.actions.Node( + object_sync_node = Node( package="spot_driver", executable="object_synchronizer_node", output="screen", - parameters=[config_file, {"spot_name": spot_name}], + parameters=[config_file, spot_name_param], namespace=spot_name, ) ld.add_action(object_sync_node) @@ -85,7 +86,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: [ PathJoinSubstitution([FindExecutable(name="xacro")]), " ", - PathJoinSubstitution([pkg_share, "urdf", "spot.urdf.xacro"]), + PathJoinSubstitution([robot_description_pkg_share, "urdf", "spot.urdf.xacro"]), " ", "arm:=", TextSubstitution(text=str(has_arm).lower()), @@ -96,7 +97,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: ] ) robot_description_params = {"robot_description": robot_description} - robot_state_publisher = launch_ros.actions.Node( + robot_state_publisher = Node( package="robot_state_publisher", executable="robot_state_publisher", output="screen", @@ -105,17 +106,16 @@ 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( + spot_robot_state_publisher = Node( package="spot_driver", executable="state_publisher_node", output="screen", - parameters=[config_file, spot_robot_state_publisher_params], + parameters=[config_file, spot_name_param], namespace=spot_name, ) ld.add_action(spot_robot_state_publisher) - spot_alert_node = launch_ros.actions.Node( + spot_alert_node = Node( package="spot_driver", executable="spot_alerts", name="spot_alerts", @@ -125,7 +125,9 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: ld.add_action(spot_alert_node) rviz = IncludeLaunchDescription( - PythonLaunchDescriptionSource([FindPackageShare(THIS_PACKAGE), "/launch", "/rviz.launch.py"]), + PythonLaunchDescriptionSource( + PathJoinSubstitution([FindPackageShare(THIS_PACKAGE), "launch", "rviz.launch.py"]) + ), launch_arguments={ "spot_name": spot_name, "rviz_config_file": rviz_config_file, @@ -135,7 +137,9 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: ld.add_action(rviz) spot_image_publishers = IncludeLaunchDescription( - PythonLaunchDescriptionSource([FindPackageShare(THIS_PACKAGE), "/launch", "/spot_image_publishers.launch.py"]), + PythonLaunchDescriptionSource( + PathJoinSubstitution([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(), @@ -144,7 +148,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: ld.add_action(spot_image_publishers) -def generate_launch_description() -> launch.LaunchDescription: +def generate_launch_description() -> LaunchDescription: launch_args = [] launch_args.append( @@ -162,10 +166,9 @@ def generate_launch_description() -> launch.LaunchDescription: ) ) launch_args.append( - DeclareLaunchArgument( + DeclareBooleanLaunchArgument( "launch_rviz", - default_value="False", - choices=["True", "true", "False", "false"], + default_value=False, description="Choose whether to launch RViz", ) ) @@ -177,17 +180,23 @@ def generate_launch_description() -> launch.LaunchDescription: ) ) launch_args.append( - DeclareLaunchArgument( + DeclareBooleanLaunchArgument( "launch_image_publishers", - default_value="True", - choices=["True", "true", "False", "false"], + default_value=True, description="Choose whether to launch the image publishing nodes from Spot.", ) ) + launch_args.append( + DeclareLaunchArgument( + "robot_description_package", + default_value="spot_description", + description="Package from where the robot model description is. Must have path /urdf/spot.urdf.xacro", + ) + ) launch_args += declare_image_publisher_args() launch_args.append(DeclareLaunchArgument("spot_name", default_value="", description="Name of Spot")) - ld = launch.LaunchDescription(launch_args) + ld = LaunchDescription(launch_args) ld.add_action(OpaqueFunction(function=launch_setup, args=[ld])) diff --git a/spot_driver/launch/spot_image_publishers.launch.py b/spot_driver/launch/spot_image_publishers.launch.py index 46b822d7f..6c81eb203 100644 --- a/spot_driver/launch/spot_image_publishers.launch.py +++ b/spot_driver/launch/spot_image_publishers.launch.py @@ -9,6 +9,7 @@ from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from synchros2.launch.actions import update_sigterm_sigkill_timeout from spot_driver.launch.spot_launch_helpers import ( DepthRegisteredMode, @@ -91,6 +92,7 @@ def create_point_cloud_nodelets( def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: + update_sigterm_sigkill_timeout(ld) config_file = LaunchConfiguration("config_file") spot_name = LaunchConfiguration("spot_name").perform(context) depth_registered_mode_config = LaunchConfiguration("depth_registered_mode") diff --git a/spot_driver/spot_driver/launch/spot_launch_helpers.py b/spot_driver/spot_driver/launch/spot_launch_helpers.py index 50914467b..a94a4ee05 100644 --- a/spot_driver/spot_driver/launch/spot_launch_helpers.py +++ b/spot_driver/spot_driver/launch/spot_launch_helpers.py @@ -3,16 +3,27 @@ import logging import os from enum import Enum -from typing import Any, Dict, List, Optional, Tuple +from typing import Any, Dict, List, Literal, Optional, Tuple import yaml from launch.actions import DeclareLaunchArgument +from synchros2.launch.actions import DeclareBooleanLaunchArgument from spot_wrapper.wrapper import SpotWrapper COLOR_END = "\33[0m" COLOR_YELLOW = "\33[33m" +# The following constants are parameters we are interested in pulling from our config yaml file +_USERNAME: Literal["username"] = "username" +_PASSWORD: Literal["password"] = "password" +_HOSTNAME: Literal["hostname"] = "hostname" +_CERTIFICATE: Literal["certificate"] = "certificate" +_PORT: Literal["port"] = "port" +_CAMERAS_USED: Literal["cameras_used"] = "cameras_used" +_GRIPPERLESS: Literal["gripperless"] = "gripperless" + + IMAGE_PUBLISHER_ARGS = [ "depth_registered_mode", "publish_point_clouds", @@ -57,10 +68,9 @@ def declare_image_publisher_args() -> List[DeclareLaunchArgument]: ) ) launch_args.append( - DeclareLaunchArgument( + DeclareBooleanLaunchArgument( "publish_point_clouds", - default_value="False", - choices=["True", "true", "False", "false"], + default_value=False, description=( "If true, create and publish point clouds for each depth registered and RGB camera pair. Requires that" " the depth_register_mode launch argument is set to a value that is not `disable`." @@ -68,26 +78,23 @@ def declare_image_publisher_args() -> List[DeclareLaunchArgument]: ) ) launch_args.append( - DeclareLaunchArgument( + DeclareBooleanLaunchArgument( "uncompress_images", - default_value="True", - choices=["True", "true", "False", "false"], + default_value=True, description="Choose whether to publish uncompressed images from Spot.", ) ) launch_args.append( - DeclareLaunchArgument( + DeclareBooleanLaunchArgument( "publish_compressed_images", - default_value="False", - choices=["True", "true", "False", "false"], + default_value=False, description="Choose whether to publish compressed images from Spot.", ) ) launch_args.append( - DeclareLaunchArgument( + DeclareBooleanLaunchArgument( "stitch_front_images", - default_value="False", - choices=["True", "true", "False", "false"], + default_value=False, description=( "Choose whether to publish a stitched image constructed from Spot's front left and right cameras." ), @@ -151,16 +158,16 @@ def get_login_parameters(config_file_path: str) -> Tuple[str, str, str, Optional ros_params = get_ros_param_dict(config_file_path) # only set username/password/hostname if they were not already set as environment variables. - if not username and "username" in ros_params: - username = ros_params["username"] - if not password and "password" in ros_params: - password = ros_params["password"] - if not hostname and "hostname" in ros_params: - hostname = ros_params["hostname"] - if not port and "port" in ros_params: - port = ros_params["port"] - if not certificate and "certificate" in ros_params: - certificate = ros_params["certificate"] + if not username and _USERNAME in ros_params: + username = ros_params[_USERNAME] + if not password and _PASSWORD in ros_params: + password = ros_params[_PASSWORD] + if not hostname and _HOSTNAME in ros_params: + hostname = ros_params[_HOSTNAME] + if not port and _PORT in ros_params: + port = ros_params[_PORT] + if not certificate and _CERTIFICATE in ros_params: + certificate = ros_params[_CERTIFICATE] if (not username) or (not password) or (not hostname): raise ValueError( "One or more of your login credentials has not been specified! Got invalid values of " @@ -170,13 +177,22 @@ def get_login_parameters(config_file_path: str) -> Tuple[str, str, str, Optional return username, password, hostname, port, certificate -def default_camera_sources(has_arm: bool) -> List[str]: +def default_camera_sources(has_arm: bool, gripperless: bool) -> List[str]: camera_sources = ["frontleft", "frontright", "left", "right", "back"] - if has_arm: + if has_arm and not gripperless: camera_sources.append("hand") return camera_sources +def get_gripperless(ros_params: Dict[str, Any]) -> bool: + """Read the ros parameters to get the value of the gripperless parameter. Defaults to False if not set.""" + gripperless = False + if _GRIPPERLESS in ros_params: + if isinstance(ros_params[_GRIPPERLESS], bool): + gripperless = ros_params[_GRIPPERLESS] + return gripperless + + def get_camera_sources_from_ros_params(ros_params: Dict[str, Any], has_arm: bool) -> List[str]: """Get the list of cameras to stream from. This will be taken from the parameters in the config yaml if it exists and contains valid cameras. If this list contains invalid cameras, fall back to the default of all cameras. @@ -191,9 +207,10 @@ def get_camera_sources_from_ros_params(ros_params: Dict[str, Any], has_arm: bool Returns: List[str]: List of cameras the driver will stream from. """ - default_sources = default_camera_sources(has_arm) - if "cameras_used" in ros_params: - camera_sources = ros_params["cameras_used"] + gripperless = get_gripperless(ros_params) + default_sources = default_camera_sources(has_arm, gripperless) + if _CAMERAS_USED in ros_params: + camera_sources = ros_params[_CAMERAS_USED] if isinstance(camera_sources, List): # check if the user inputted any camera that's not in the default sources. invalid_cameras = [cam for cam in camera_sources if cam not in default_sources] @@ -231,6 +248,7 @@ def spot_has_arm(config_file_path: str, spot_name: str) -> bool: """ logger = logging.getLogger("spot_driver_launch") 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, password=password, @@ -239,5 +257,6 @@ def spot_has_arm(config_file_path: str, spot_name: str) -> bool: cert_resource_glob=certificate, robot_name=spot_name, logger=logger, + gripperless=gripperless, ) return spot_wrapper.has_arm() diff --git a/spot_driver/spot_driver/spot_ros2.py b/spot_driver/spot_driver/spot_ros2.py index 080a42177..6b8d335ad 100644 --- a/spot_driver/spot_driver/spot_ros2.py +++ b/spot_driver/spot_driver/spot_ros2.py @@ -338,34 +338,10 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw get_from_env_and_fall_back_to_param("SPOT_CERTIFICATE", self, "certificate", "") or None ) - # Spot has 2 types of odometries: 'odom' and 'vision' - # The former one is kinematic odometry and the second one is a combined odometry of vision and kinematics - # These params enables to change which odometry frame is a parent of body frame and to change tf names of each - # odometry frames. frame_prefix = "" if self.name is not None: frame_prefix = self.name + "/" self.frame_prefix: str = frame_prefix - self.preferred_odom_frame: Parameter = self.declare_parameter( - "preferred_odom_frame", self.frame_prefix + "odom" - ) # 'vision' or 'odom' - self.tf_name_kinematic_odom: Parameter = self.declare_parameter( - "tf_name_kinematic_odom", self.frame_prefix + "odom" - ) - self.tf_name_raw_kinematic: str = frame_prefix + "odom" - self.tf_name_vision_odom: Parameter = self.declare_parameter( - "tf_name_vision_odom", self.frame_prefix + "vision" - ) - self.tf_name_raw_vision: str = self.frame_prefix + "vision" - - preferred_odom_frame_references = [self.tf_name_raw_kinematic, self.tf_name_raw_vision] - if self.preferred_odom_frame.value not in preferred_odom_frame_references: - error_msg = ( - f'rosparam "preferred_odom_frame" should be one of {preferred_odom_frame_references}, got' - f' "{self.preferred_odom_frame.value}"' - ) - self.get_logger().error(error_msg) - raise ValueError(error_msg) self.tf_name_graph_nav_body: str = self.frame_prefix + "body" @@ -1018,6 +994,7 @@ def init_spot_wrapper(self): continually_try_stand=self.continually_try_stand.value, rgb_cameras=self.rgb_cameras.value, cert_resource_glob=self.certificate, + gripperless=self.gripperless ) def take_lease_callback(self, request: Trigger.Request, response: Trigger.Response) -> Trigger.Response: diff --git a/spot_driver/src/conversions/robot_state.cpp b/spot_driver/src/conversions/robot_state.cpp index 33da7f1d8..dfafc343b 100644 --- a/spot_driver/src/conversions/robot_state.cpp +++ b/spot_driver/src/conversions/robot_state.cpp @@ -151,7 +151,7 @@ std::optional getTf(const ::bosdyn::api::FrameTreeSnap : transform.parent_frame_name(); const auto frame_name = frame_id.find('/') == std::string::npos ? prefix + frame_id : frame_id; - // set target frame(preferred odom frame) as the root node in tf tree + // set preferred base frame as the root node in tf tree if (preferred_base_frame_id == frame_name) { tf_msg.transforms.push_back( toTransformStamped(~(transform.parent_tform_child()), frame_name, parent_frame_name, timestamp_local)); @@ -163,30 +163,41 @@ std::optional getTf(const ::bosdyn::api::FrameTreeSnap return tf_msg; } -std::optional getOdomTwist( - const ::bosdyn::api::RobotState& robot_state, const google::protobuf::Duration& clock_skew) { - if (!robot_state.has_kinematic_state() || !robot_state.kinematic_state().has_velocity_of_body_in_odom()) { +std::optional getOdomTwist(const ::bosdyn::api::RobotState& robot_state, + const google::protobuf::Duration& clock_skew, + const bool is_using_vision) { + if (!robot_state.has_kinematic_state()) { + return std::nullopt; + } + + const auto& kinematic_state = robot_state.kinematic_state(); + if (is_using_vision && !kinematic_state.has_velocity_of_body_in_vision()) { + return std::nullopt; + } else if (!is_using_vision && !kinematic_state.has_velocity_of_body_in_odom()) { return std::nullopt; } geometry_msgs::msg::TwistWithCovarianceStamped odom_twist_msg; // TODO(schornakj): need to add the frame ID here? + + const bosdyn::api::SE3Velocity& velocity_of_body_in_world = + is_using_vision ? kinematic_state.velocity_of_body_in_vision() : kinematic_state.velocity_of_body_in_odom(); + odom_twist_msg.header.stamp = spot_ros2::robotTimeToLocalTime(robot_state.kinematic_state().acquisition_timestamp(), clock_skew); - convertToRos(robot_state.kinematic_state().velocity_of_body_in_odom(), odom_twist_msg.twist.twist); + convertToRos(velocity_of_body_in_world, odom_twist_msg.twist.twist); return odom_twist_msg; } std::optional getOdom(const ::bosdyn::api::RobotState& robot_state, const google::protobuf::Duration& clock_skew, const std::string& prefix, - bool is_using_vision) { + const bool is_using_vision) { if (!robot_state.has_kinematic_state() || !robot_state.kinematic_state().has_acquisition_timestamp() || - !robot_state.kinematic_state().has_transforms_snapshot() || - !robot_state.kinematic_state().has_velocity_of_body_in_odom()) { + !robot_state.kinematic_state().has_transforms_snapshot()) { return std::nullopt; } - const auto odom_twist = getOdomTwist(robot_state, clock_skew); + const auto odom_twist = getOdomTwist(robot_state, clock_skew, is_using_vision); if (!odom_twist) { return std::nullopt; } diff --git a/spot_driver/src/images/spot_image_publisher.cpp b/spot_driver/src/images/spot_image_publisher.cpp index 2f885e2dc..9bc7675f6 100644 --- a/spot_driver/src/images/spot_image_publisher.cpp +++ b/spot_driver/src/images/spot_image_publisher.cpp @@ -89,15 +89,16 @@ bool SpotImagePublisher::initialize() { const auto publish_raw_rgb_cameras = false; const auto uncompress_images = parameters_->getUncompressImages(); const auto publish_compressed_images = parameters_->getPublishCompressedImages(); + const auto gripperless = parameters_->getGripperless(); std::set cameras_used; - const auto cameras_used_parameter = parameters_->getCamerasUsed(has_arm_); + const auto cameras_used_parameter = parameters_->getCamerasUsed(has_arm_, gripperless); if (cameras_used_parameter.has_value()) { cameras_used = cameras_used_parameter.value(); } else { logger_->logWarn("Invalid cameras_used parameter! Got error: " + cameras_used_parameter.error() + " Defaulting to publishing from all cameras."); - cameras_used = parameters_->getDefaultCamerasUsed(has_arm_); + cameras_used = parameters_->getDefaultCamerasUsed(has_arm_, gripperless); } // Generate the set of image sources based on which cameras the user has requested that we publish diff --git a/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp b/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp index 117d41d98..a2d6b0c2e 100644 --- a/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp +++ b/spot_driver/src/interfaces/rclcpp_parameter_interface.cpp @@ -26,6 +26,8 @@ constexpr auto kParameterNamePublishCompressedImages = "publish_compressed_image constexpr auto kParameterNamePublishDepthImages = "publish_depth"; constexpr auto kParameterNamePublishDepthRegisteredImages = "publish_depth_registered"; constexpr auto kParameterPreferredOdomFrame = "preferred_odom_frame"; +constexpr auto kParameterTFRoot = "tf_root"; +constexpr auto kParameterNameGripperless = "gripperless"; /** * @brief Get a rclcpp parameter. If the parameter has not been declared, declare it with the provided default value and @@ -186,8 +188,18 @@ std::string RclcppParameterInterface::getPreferredOdomFrame() const { return declareAndGetParameter(node_, kParameterPreferredOdomFrame, kDefaultPreferredOdomFrame); } -std::set RclcppParameterInterface::getDefaultCamerasUsed(const bool has_arm) const { - const auto kDefaultCamerasUsed = has_arm ? kDefaultCamerasUsedWithArm : kDefaultCamerasUsedWithoutArm; +std::string RclcppParameterInterface::getTFRoot() const { + return declareAndGetParameter(node_, kParameterTFRoot, kDefaultTFRoot); +} + +bool RclcppParameterInterface::getGripperless() const { + return declareAndGetParameter(node_, kParameterNameGripperless, kDefaultGripperless); +} + +std::set RclcppParameterInterface::getDefaultCamerasUsed(const bool has_arm, + const bool gripperless) const { + const bool has_hand_camera = has_arm && (!gripperless); + const auto kDefaultCamerasUsed = (has_hand_camera) ? kCamerasWithHand : kCamerasWithoutHand; std::set spot_cameras_used; for (const auto& camera : kDefaultCamerasUsed) { spot_cameras_used.insert(kRosStringToSpotCamera.at(std::string(camera))); @@ -196,8 +208,9 @@ std::set RclcppParameterInterface::getDefaultCamerasUsed( } tl::expected, std::string> RclcppParameterInterface::getCamerasUsed( - const bool has_arm) const { - const auto kDefaultCamerasUsed = has_arm ? kDefaultCamerasUsedWithArm : kDefaultCamerasUsedWithoutArm; + const bool has_arm, const bool gripperless) const { + const bool has_hand_camera = has_arm && (!gripperless); + const auto kDefaultCamerasUsed = (has_hand_camera) ? kCamerasWithHand : kCamerasWithoutHand; const std::vector kDefaultCamerasUsedVector(std::begin(kDefaultCamerasUsed), std::end(kDefaultCamerasUsed)); const auto cameras_used_param = @@ -208,6 +221,8 @@ tl::expected, std::string> RclcppParameterInterf const auto spot_camera = kRosStringToSpotCamera.at(camera); if ((spot_camera == SpotCamera::HAND) && (!has_arm)) { return tl::make_unexpected("Cannot add SpotCamera 'hand', the robot does not have an arm!"); + } else if ((spot_camera == SpotCamera::HAND) && gripperless) { + return tl::make_unexpected("Cannot add SpotCamera 'hand', the robot is gripperless!"); } spot_cameras_used.insert(spot_camera); } catch (const std::out_of_range& e) { diff --git a/spot_driver/src/object_sync/object_synchronizer.cpp b/spot_driver/src/object_sync/object_synchronizer.cpp index e4d4eb2da..2c688e69f 100644 --- a/spot_driver/src/object_sync/object_synchronizer.cpp +++ b/spot_driver/src/object_sync/object_synchronizer.cpp @@ -332,9 +332,9 @@ ObjectSynchronizer::ObjectSynchronizer(const std::shared_ptrgetSpotName(); frame_prefix_ = spot_name.empty() ? "" : spot_name + "/"; - preferred_base_frame_ = stripPrefix(parameter_interface_->getPreferredOdomFrame(), frame_prefix_); + preferred_base_frame_ = stripPrefix(parameter_interface_->getTFRoot(), frame_prefix_); preferred_base_frame_with_prefix_ = preferred_base_frame_.find('/') == std::string::npos - ? spot_name + "/" + preferred_base_frame_ + ? frame_prefix_ + preferred_base_frame_ : preferred_base_frame_; // TODO(khughes): This is temporarily disabled to reduce driver's spew about TF extrapolation. diff --git a/spot_driver/src/robot_state/state_publisher.cpp b/spot_driver/src/robot_state/state_publisher.cpp index 176b764ca..66d6bc57d 100644 --- a/spot_driver/src/robot_state/state_publisher.cpp +++ b/spot_driver/src/robot_state/state_publisher.cpp @@ -35,8 +35,9 @@ StatePublisher::StatePublisher(const std::shared_ptr& stat const auto preferred_odom_frame = parameter_interface_->getPreferredOdomFrame(); is_using_vision_ = preferred_odom_frame == "vision"; - full_odom_frame_id_ = - preferred_odom_frame.find('/') == std::string::npos ? frame_prefix_ + preferred_odom_frame : preferred_odom_frame; + + const auto tf_root = parameter_interface_->getTFRoot(); + full_tf_root_id_ = tf_root.find('/') == std::string::npos ? frame_prefix_ + tf_root : tf_root; // Create a timer to request and publish robot state at a fixed rate timer_interface_->setTimer(kRobotStateCallbackPeriod, [this] { @@ -67,8 +68,8 @@ void StatePublisher::timerCallback() { getFootState(robot_state), getEstopStates(robot_state, clock_skew), getJointStates(robot_state, clock_skew, frame_prefix_), - getTf(robot_state, clock_skew, frame_prefix_, full_odom_frame_id_), - getOdomTwist(robot_state, clock_skew), + getTf(robot_state, clock_skew, frame_prefix_, full_tf_root_id_), + getOdomTwist(robot_state, clock_skew, is_using_vision_), getOdom(robot_state, clock_skew, frame_prefix_, is_using_vision_), getPowerState(robot_state, clock_skew), getSystemFaultState(robot_state, clock_skew), diff --git a/spot_driver/test/include/spot_driver/fake/fake_parameter_interface.hpp b/spot_driver/test/include/spot_driver/fake/fake_parameter_interface.hpp index 32af0efee..122ad8cb1 100644 --- a/spot_driver/test/include/spot_driver/fake/fake_parameter_interface.hpp +++ b/spot_driver/test/include/spot_driver/fake/fake_parameter_interface.hpp @@ -38,10 +38,14 @@ class FakeParameterInterface : public ParameterInterfaceBase { std::string getPreferredOdomFrame() const override { return "odom"; } + std::string getTFRoot() const override { return "odom"; } + std::string getSpotName() const override { return spot_name; } - std::set getDefaultCamerasUsed(const bool has_arm) const override { - const auto kDefaultCamerasUsed = has_arm ? kDefaultCamerasUsedWithArm : kDefaultCamerasUsedWithoutArm; + bool getGripperless() const override { return gripperless; } + + std::set getDefaultCamerasUsed(const bool has_arm, const bool gripperless) const override { + const auto kDefaultCamerasUsed = (has_arm && !gripperless) ? kCamerasWithHand : kCamerasWithoutHand; std::set spot_cameras_used; for (const auto& camera : kDefaultCamerasUsed) { spot_cameras_used.insert(kRosStringToSpotCamera.at(std::string(camera))); @@ -49,8 +53,9 @@ class FakeParameterInterface : public ParameterInterfaceBase { return spot_cameras_used; } - tl::expected, std::string> getCamerasUsed(const bool has_arm) const override { - return getDefaultCamerasUsed(has_arm); + tl::expected, std::string> getCamerasUsed(const bool has_arm, + const bool gripperless) const override { + return getDefaultCamerasUsed(has_arm, gripperless); } static constexpr auto kExampleHostname{"192.168.0.10"}; @@ -64,6 +69,7 @@ class FakeParameterInterface : public ParameterInterfaceBase { bool publish_rgb_images = ParameterInterfaceBase::kDefaultPublishRGBImages; bool publish_depth_images = ParameterInterfaceBase::kDefaultPublishDepthImages; bool publish_depth_registered_images = ParameterInterfaceBase::kDefaultPublishDepthRegisteredImages; + bool gripperless = ParameterInterfaceBase::kDefaultGripperless; std::string spot_name; }; } // namespace spot_ros2::test diff --git a/spot_driver/test/include/spot_driver/robot_state_test_tools.hpp b/spot_driver/test/include/spot_driver/robot_state_test_tools.hpp index 9ae03edb7..20006a48c 100644 --- a/spot_driver/test/include/spot_driver/robot_state_test_tools.hpp +++ b/spot_driver/test/include/spot_driver/robot_state_test_tools.hpp @@ -116,6 +116,18 @@ inline void addBodyVelocityOdom(::bosdyn::api::KinematicState* mutable_kinematic velocity_angular->set_z(rz); } +inline void addBodyVelocityVision(::bosdyn::api::KinematicState* mutable_kinematic_state, double x, double y, double z, + double rx, double ry, double rz) { + auto* velocity_linear = mutable_kinematic_state->mutable_velocity_of_body_in_vision()->mutable_linear(); + velocity_linear->set_x(x); + velocity_linear->set_y(y); + velocity_linear->set_z(z); + auto* velocity_angular = mutable_kinematic_state->mutable_velocity_of_body_in_vision()->mutable_angular(); + velocity_angular->set_x(rx); + velocity_angular->set_y(ry); + velocity_angular->set_z(rz); +} + inline void addAcquisitionTimestamp(::bosdyn::api::KinematicState* mutable_kinematic_state, const google::protobuf::Timestamp& timestamp) { mutable_kinematic_state->mutable_acquisition_timestamp()->CopyFrom(timestamp); diff --git a/spot_driver/test/src/conversions/test_robot_state.cpp b/spot_driver/test/src/conversions/test_robot_state.cpp index 9dc4bf657..0712a166b 100644 --- a/spot_driver/test/src/conversions/test_robot_state.cpp +++ b/spot_driver/test/src/conversions/test_robot_state.cpp @@ -343,21 +343,15 @@ TEST(RobotStateConversions, TestGetOdomTwist) { timestamp.set_nanos(0); addAcquisitionTimestamp(robot_state.mutable_kinematic_state(), timestamp); - auto* velocity_linear = robot_state.mutable_kinematic_state()->mutable_velocity_of_body_in_odom()->mutable_linear(); - velocity_linear->set_x(1.0); - velocity_linear->set_y(2.0); - velocity_linear->set_z(3.0); - auto* velocity_angular = robot_state.mutable_kinematic_state()->mutable_velocity_of_body_in_odom()->mutable_angular(); - velocity_angular->set_x(1.0); - velocity_angular->set_y(2.0); - velocity_angular->set_z(3.0); + addBodyVelocityOdom(robot_state.mutable_kinematic_state(), 1.0, 2.0, 3.0, 1.0, 2.0, 3.0); // GIVEN some nominal clock skew google::protobuf::Duration clock_skew; clock_skew.set_seconds(1); // WHEN we create a TwistWithCovarianceStamped ROS message - const auto out = getOdomTwist(robot_state, clock_skew); + const auto is_using_vision = false; + const auto out = getOdomTwist(robot_state, clock_skew, is_using_vision); // THEN this succeeds ASSERT_THAT(out.has_value(), IsTrue()); @@ -377,7 +371,8 @@ TEST(RobotStateConversions, TestGetOdomTwistNoBodyVelocityInRobotState) { clock_skew.set_seconds(1); // WHEN we attempt to create a TwistWithCovarianceStamped ROS message - const auto out = getOdomTwist(robot_state, clock_skew); + const auto is_using_vision = false; + const auto out = getOdomTwist(robot_state, clock_skew, is_using_vision); // THEN no message is output ASSERT_THAT(out.has_value(), IsFalse()); @@ -435,7 +430,7 @@ TEST(RobotStateConversions, TestGetOdomInVisionFrame) { timestamp.set_seconds(99); timestamp.set_nanos(0); addAcquisitionTimestamp(robot_state.mutable_kinematic_state(), timestamp); - addBodyVelocityOdom(robot_state.mutable_kinematic_state(), 1.0, 2.0, 3.0, 4.0, 5.0, 6.0); + addBodyVelocityVision(robot_state.mutable_kinematic_state(), 1.0, 2.0, 3.0, 4.0, 5.0, 6.0); addRootFrame(robot_state.mutable_kinematic_state()->mutable_transforms_snapshot(), "vision"); addTransform(robot_state.mutable_kinematic_state()->mutable_transforms_snapshot(), "body", "vision", 1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.0); diff --git a/spot_driver/test/src/test_parameter_interface.cpp b/spot_driver/test/src/test_parameter_interface.cpp index b615f8926..4ad4be3a2 100644 --- a/spot_driver/test/src/test_parameter_interface.cpp +++ b/spot_driver/test/src/test_parameter_interface.cpp @@ -195,6 +195,8 @@ TEST_F(RclcppParameterInterfaceEnvVarTest, GetSpotConfigFromParameters) { node_->declare_parameter("publish_depth", publish_depth_images_parameter); constexpr auto publish_depth_registered_images_parameter = false; node_->declare_parameter("publish_depth_registered", publish_depth_registered_images_parameter); + constexpr auto tf_root_parameter = "body"; + node_->declare_parameter("tf_root", tf_root_parameter); // GIVEN we create a RclcppParameterInterface using the node RclcppParameterInterface parameter_interface{node_}; @@ -213,6 +215,7 @@ TEST_F(RclcppParameterInterfaceEnvVarTest, GetSpotConfigFromParameters) { EXPECT_THAT(parameter_interface.getPublishRGBImages(), Eq(publish_rgb_images_parameter)); EXPECT_THAT(parameter_interface.getPublishDepthImages(), Eq(publish_depth_images_parameter)); EXPECT_THAT(parameter_interface.getPublishDepthRegisteredImages(), Eq(publish_depth_registered_images_parameter)); + EXPECT_THAT(parameter_interface.getTFRoot(), Eq(tf_root_parameter)); } TEST_F(RclcppParameterInterfaceEnvVarTest, GetSpotConfigEnvVarsOverruleParameters) { @@ -272,6 +275,7 @@ TEST_F(RclcppParameterInterfaceEnvVarTest, GetConfigDefaults) { EXPECT_THAT(parameter_interface.getPublishRGBImages(), IsTrue()); EXPECT_THAT(parameter_interface.getPublishDepthImages(), IsTrue()); EXPECT_THAT(parameter_interface.getPublishDepthRegisteredImages(), IsTrue()); + EXPECT_THAT(parameter_interface.getTFRoot(), StrEq("odom")); } TEST_F(RclcppParameterInterfaceEnvVarTest, GetCamerasUsedDefaultWithArm) { @@ -279,9 +283,13 @@ TEST_F(RclcppParameterInterfaceEnvVarTest, GetCamerasUsedDefaultWithArm) { // GIVEN we create a RclcppParameterInterface using the node RclcppParameterInterface parameter_interface{node_}; + // GIVEN we are operating on a robot with an arm, and without custom gripperless firmware + bool arm = true; + bool gripperless = false; + // WHEN we call the functions to get the config values from the parameter interface // THEN we get the default of all available cameras. - const auto cameras_used_arm = parameter_interface.getCamerasUsed(true); + const auto cameras_used_arm = parameter_interface.getCamerasUsed(arm, gripperless); EXPECT_THAT(cameras_used_arm.has_value(), IsTrue()); EXPECT_THAT(cameras_used_arm.value(), UnorderedElementsAre(SpotCamera::FRONTLEFT, SpotCamera::FRONTRIGHT, SpotCamera::LEFT, SpotCamera::RIGHT, @@ -296,9 +304,13 @@ TEST_F(RclcppParameterInterfaceEnvVarTest, GetCamerasUsedDefaultWithoutArm) { // GIVEN we create a RclcppParameterInterface using the node RclcppParameterInterface parameter_interface{node_}; + // GIVEN we are operating on a robot without an arm, and without custom gripperless firmware + bool arm = false; + bool gripperless = false; + // WHEN we call the functions to get the config values from the parameter interface // THEN we get the default of all available cameras. - const auto cameras_used_no_arm = parameter_interface.getCamerasUsed(false); + const auto cameras_used_no_arm = parameter_interface.getCamerasUsed(arm, gripperless); EXPECT_THAT(cameras_used_no_arm.has_value(), IsTrue()); EXPECT_THAT(cameras_used_no_arm.value(), UnorderedElementsAre(SpotCamera::FRONTLEFT, SpotCamera::FRONTRIGHT, SpotCamera::LEFT, SpotCamera::RIGHT, SpotCamera::BACK)); @@ -312,13 +324,22 @@ TEST_F(RclcppParameterInterfaceEnvVarTest, GetCamerasUsedSubset) { // GIVEN we create a RclcppParameterInterface using the node RclcppParameterInterface parameter_interface{node_}; + // GIVEN we are operating on a robot with an arm, and without custom gripperless firmware + bool arm = true; + bool gripperless = false; + // WHEN we call the functions to get the config values from the parameter interface - // THEN the returned values match the values we used when declaring the parameters, regardless of if there is an arm - const auto cameras_used_arm = parameter_interface.getCamerasUsed(true); + // THEN the returned values match the values we used when declaring the parameters + const auto cameras_used_arm = parameter_interface.getCamerasUsed(arm, gripperless); EXPECT_THAT(cameras_used_arm.has_value(), IsTrue()); EXPECT_THAT(cameras_used_arm.value(), UnorderedElementsAre(SpotCamera::FRONTLEFT, SpotCamera::FRONTRIGHT)); - const auto cameras_used_no_arm = parameter_interface.getCamerasUsed(false); + // GIVEN we are operating on a robot without an arm + arm = false; + + // WHEN we call the functions to get the config values from the parameter interface + // THEN the returned values match the values we used when declaring the parameters + const auto cameras_used_no_arm = parameter_interface.getCamerasUsed(arm, gripperless); EXPECT_THAT(cameras_used_no_arm.has_value(), IsTrue()); EXPECT_THAT(cameras_used_no_arm.value(), UnorderedElementsAre(SpotCamera::FRONTLEFT, SpotCamera::FRONTRIGHT)); } @@ -331,16 +352,21 @@ TEST_F(RclcppParameterInterfaceEnvVarTest, GetCamerasUsedSubsetWithHand) { // GIVEN we create a RclcppParameterInterface using the node RclcppParameterInterface parameter_interface{node_}; + // GIVEN we are operating on a robot with an arm, and without custom gripperless firmware + bool arm = true; + bool gripperless = false; + // WHEN we call the functions to get the config values from the parameter interface if the robot has an arm // THEN the returned values match the values we used when declaring the parameters - const auto cameras_used_arm = parameter_interface.getCamerasUsed(true); + const auto cameras_used_arm = parameter_interface.getCamerasUsed(arm, gripperless); EXPECT_THAT(cameras_used_arm.has_value(), IsTrue()); EXPECT_THAT(cameras_used_arm.value(), UnorderedElementsAre(SpotCamera::FRONTLEFT, SpotCamera::FRONTRIGHT, SpotCamera::HAND)); // WHEN we call the functions to get the config values from the parameter interface if the robot does not have an arm // THEN this is an invalid choice of parameters. - const auto cameras_used_no_arm = parameter_interface.getCamerasUsed(false); + arm = false; + const auto cameras_used_no_arm = parameter_interface.getCamerasUsed(arm, gripperless); EXPECT_THAT(cameras_used_no_arm.has_value(), IsFalse()); EXPECT_THAT(cameras_used_no_arm.error(), StrEq("Cannot add SpotCamera 'hand', the robot does not have an arm!")); } @@ -353,13 +379,62 @@ TEST_F(RclcppParameterInterfaceEnvVarTest, GetCamerasUsedWithInvalidCamera) { // GIVEN we create a RclcppParameterInterface using the node RclcppParameterInterface parameter_interface{node_}; + // GIVEN we are operating on a robot with an arm, and without custom gripperless firmware + bool arm = true; + bool gripperless = false; + // WHEN we call the functions to get the config values from the parameter interface // THEN the result is invalid for robots with and without arms, as the camera "not_a_camera" does not exist on Spot. - const auto cameras_used_arm = parameter_interface.getCamerasUsed(true); + const auto cameras_used_arm = parameter_interface.getCamerasUsed(arm, gripperless); EXPECT_THAT(cameras_used_arm.has_value(), IsFalse()); EXPECT_THAT(cameras_used_arm.error(), StrEq("Cannot convert camera 'not_a_camera' to a SpotCamera.")); - const auto cameras_used_no_arm = parameter_interface.getCamerasUsed(false); + arm = false; + const auto cameras_used_no_arm = parameter_interface.getCamerasUsed(arm, gripperless); EXPECT_THAT(cameras_used_no_arm.has_value(), IsFalse()); EXPECT_THAT(cameras_used_no_arm.error(), StrEq("Cannot convert camera 'not_a_camera' to a SpotCamera.")); } + +TEST_F(RclcppParameterInterfaceEnvVarTest, GetDefaultCamerasUsedGripperless) { + // GIVEN we create a RclcppParameterInterface using the node + RclcppParameterInterface parameter_interface{node_}; + + // GIVEN we are operating on a robot with an arm, and WITH custom gripperless firmware + bool arm = true; + bool gripperless = true; + + // WHEN we call the functions to get the config values from the parameter interface + // THEN we get the default of all available cameras, excluding the hand! + const auto cameras_used_arm = parameter_interface.getCamerasUsed(arm, gripperless); + EXPECT_THAT(cameras_used_arm.has_value(), IsTrue()); + EXPECT_THAT(cameras_used_arm.value(), UnorderedElementsAre(SpotCamera::FRONTLEFT, SpotCamera::FRONTRIGHT, + SpotCamera::LEFT, SpotCamera::RIGHT, SpotCamera::BACK)); + + // WHEN gripperless is set to true on a robot without an arm + // THEN we still get the default of all available cameras, excluding the hand + arm = false; + const auto cameras_used_no_arm = parameter_interface.getCamerasUsed(arm, gripperless); + EXPECT_THAT(cameras_used_no_arm.has_value(), IsTrue()); + EXPECT_THAT(cameras_used_no_arm.value(), UnorderedElementsAre(SpotCamera::FRONTLEFT, SpotCamera::FRONTRIGHT, + SpotCamera::LEFT, SpotCamera::RIGHT, SpotCamera::BACK)); +} + +TEST_F(RclcppParameterInterfaceEnvVarTest, GetSelectedCamerasUsedGripperless) { + // GIVEN we set cameras used to a subset of the available cameras including the hand camera + const std::vector cameras_used_parameter = {"frontleft", "frontright", "hand"}; + node_->declare_parameter("cameras_used", cameras_used_parameter); + + // GIVEN we create a RclcppParameterInterface using the node + RclcppParameterInterface parameter_interface{node_}; + + // GIVEN we are operating on a robot with an arm, and WITH custom gripperless firmware + bool arm = true; + bool gripperless = true; + + // WHEN we call the functions to get the config values from the parameter interface + // THEN this is an invalid choice of parameters, as the hand camera is not available on gripperless robots. + const auto cameras_used_arm = parameter_interface.getCamerasUsed(arm, gripperless); + EXPECT_THAT(cameras_used_arm.has_value(), IsFalse()); + EXPECT_THAT(cameras_used_arm.error(), StrEq("Cannot add SpotCamera 'hand', the robot is gripperless!")); +} + } // namespace spot_ros2::test diff --git a/spot_hardware_interface/include/spot_hardware_interface/spot_constants.hpp b/spot_hardware_interface/include/spot_hardware_interface/spot_constants.hpp index 5e1b90f53..92169fe99 100644 --- a/spot_hardware_interface/include/spot_hardware_interface/spot_constants.hpp +++ b/spot_hardware_interface/include/spot_hardware_interface/spot_constants.hpp @@ -26,22 +26,24 @@ inline constexpr int kNjointsNoArm = 12; /// @brief Number of joints we expect if the robot has an arm inline constexpr int kNjointsArm = 19; -// Gain values https://github.com/boston-dynamics/spot-cpp-sdk/blob/master/cpp/examples/joint_control/constants.hpp -// This will be handled via a parameter in the future so there is the option to change them, for now they are hardcoded +// Default gain values obtained from +// https://github.com/boston-dynamics/spot-cpp-sdk/blob/master/cpp/examples/joint_control/constants.hpp -/// @brief Default Kp gains for robot without an arm -inline constexpr float kDefaultKpNoArm[] = {624.0, 936.0, 286.0, 624.0, 936.0, 286.0, - 624.0, 936.0, 286.0, 624.0, 936.0, 286.0}; +/// @brief Default k_q_p gains for robot without an arm +inline constexpr float kDefaultKqpNoArm[] = {624.0, 936.0, 286.0, 624.0, 936.0, 286.0, + 624.0, 936.0, 286.0, 624.0, 936.0, 286.0}; -/// @brief Default Kd gains for robot without an arm -inline constexpr float kDefaultKdNoArm[] = {5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04}; +/// @brief Default k_qd_p gains for robot without an arm +inline constexpr float kDefaultKqdpNoArm[] = {5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04}; -/// @brief Default Kp gains for robot with an arm -inline constexpr float kDefaultKpArm[] = {624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 624.0, - 936.0, 286.0, 1020.0, 255.0, 204.0, 102.0, 102.0, 102.0, 16.0}; +/// @brief Default k_q_p gains for robot with an arm (note that the first 12 elements that correspond to the leg joints +/// are the same as `kDefaultKqpNoArm`) +inline constexpr float kDefaultKqpArm[] = {624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 624.0, + 936.0, 286.0, 1020.0, 255.0, 204.0, 102.0, 102.0, 102.0, 16.0}; -/// @brief Default Kd gains for robot with an arm -inline constexpr float kDefaultKdArm[] = {5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, - 5.20, 2.04, 10.2, 15.3, 10.2, 2.04, 2.04, 2.04, 0.32}; +/// @brief Default k_qd_p gains for robot with an arm (note that the first 12 elements that correspond to the leg joints +/// are the same as `kDefaultKqdpNoArm`) +inline constexpr float kDefaultKqdpArm[] = {5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, + 5.20, 2.04, 10.2, 15.3, 10.2, 2.04, 2.04, 2.04, 0.32}; } // namespace spot_hardware_interface diff --git a/spot_hardware_interface/include/spot_hardware_interface/spot_hardware_interface.hpp b/spot_hardware_interface/include/spot_hardware_interface/spot_hardware_interface.hpp index 6731ffcb3..360eeece5 100644 --- a/spot_hardware_interface/include/spot_hardware_interface/spot_hardware_interface.hpp +++ b/spot_hardware_interface/include/spot_hardware_interface/spot_hardware_interface.hpp @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -128,6 +129,10 @@ class SpotHardware : public hardware_interface::SystemInterface { std::string username_; std::string password_; + // Stores gains to be used in the joint level command + std::vector k_q_p_; + std::vector k_qd_p_; + // Power status bool powered_on_ = false; @@ -157,6 +162,17 @@ class SpotHardware : public hardware_interface::SystemInterface { ::bosdyn::api::JointControlStreamRequest joint_request_; + /// @brief Gains are passed in as hardware parameters as a space separated string. This function translates this + /// information into the corresponding std::vector to be used in constructing the robot streaming command. + /// @param gain_string Space separated string of k_q_p or k_qd_p values -- e.g., "1.0 2.0 3.0" + /// @param default_gains Vector of default gains to fall back to if the input is not formatted correctly. + /// @param gain_name Human readable name of the parameter parsed -- e.g., "k_q_p". Used in logging a warning if the + /// input is malformed. + /// @return The input gain_string formatted as an std::vector if it is the appropriate number of elements, and + /// default_gains if not. + std::vector parse_gains_parameter(const std::string gains_string, const std::vector& default_gains, + const std::string gain_name); + // The following are functions that interact with the BD SDK to set up the robot and get the robot states. /** diff --git a/spot_hardware_interface/src/spot_hardware_interface.cpp b/spot_hardware_interface/src/spot_hardware_interface.cpp index e9d7fee96..99bc950ac 100644 --- a/spot_hardware_interface/src/spot_hardware_interface.cpp +++ b/spot_hardware_interface/src/spot_hardware_interface.cpp @@ -51,6 +51,23 @@ void StateStreamingHandler::get_joint_states(JointStates& joint_states) { joint_states.load.assign(current_load_.begin(), current_load_.end()); } +std::vector SpotHardware::parse_gains_parameter(const std::string gains_string, + const std::vector& default_gains, + const std::string gain_name) { + std::istringstream gains_stream(gains_string); + std::vector gains((std::istream_iterator(gains_stream)), (std::istream_iterator())); + const auto expected_size = default_gains.size(); + if (gains.size() != expected_size) { + if (!gains.empty()) { + RCLCPP_WARN(rclcpp::get_logger("SpotHardware"), + "%s has %ld entries, expected %ld. Check your config file! Falling back to default gains.", + gain_name.c_str(), gains.size(), expected_size); + } + return default_gains; + } + return gains; +} + hardware_interface::CallbackReturn SpotHardware::on_init(const hardware_interface::HardwareInfo& info) { if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) { return hardware_interface::CallbackReturn::ERROR; @@ -65,6 +82,29 @@ hardware_interface::CallbackReturn SpotHardware::on_init(const hardware_interfac njoints_ = hw_states_.size() / interfaces_per_joint_; + // check that the number of joints matches what we expect, and determine default k_q_p/k_qd_p from this + std::vector default_k_q_p, default_k_qd_p; + if (njoints_ == kNjointsArm) { + default_k_q_p.assign(std::begin(kDefaultKqpArm), std::end(kDefaultKqpArm)); + default_k_qd_p.assign(std::begin(kDefaultKqdpArm), std::end(kDefaultKqdpArm)); + } else if (njoints_ == kNjointsNoArm) { + default_k_q_p.assign(std::begin(kDefaultKqpNoArm), std::end(kDefaultKqpNoArm)); + default_k_qd_p.assign(std::begin(kDefaultKqdpNoArm), std::end(kDefaultKqdpNoArm)); + } else { + RCLCPP_ERROR(rclcpp::get_logger("SpotHardware"), + "Got %ld joints, expected either %d (Spot with arm) or %d (Spot without arm)!!", njoints_, kNjointsArm, + kNjointsNoArm); + return hardware_interface::CallbackReturn::ERROR; + } + + // Get the user-passed in k_q_p and k_q_d values. + const std::string k_q_p_string = info_.hardware_parameters["k_q_p"]; + const std::string k_qd_p_string = info_.hardware_parameters["k_qd_p"]; + + // Determine the gains that should be used on robot. + k_q_p_ = parse_gains_parameter(k_q_p_string, default_k_q_p, "k_q_p"); + k_qd_p_ = parse_gains_parameter(k_qd_p_string, default_k_qd_p, "k_qd_p"); + for (const hardware_interface::ComponentInfo& joint : info_.joints) { // Assumes three state and three command interfaces for each joint (position, velocity, and effort). if (joint.command_interfaces.size() != interfaces_per_joint_) { @@ -469,30 +509,10 @@ bool SpotHardware::start_command_stream() { // Fill in the parts of the joint streaming command request that are constant. auto* joint_cmd = joint_request_.mutable_joint_command(); - std::vector kp; - std::vector kd; - - // Assign k values depending on if the robot has an arm or not - switch (njoints_) { - case spot_hardware_interface::kNjointsArm: - kp.assign(std::begin(spot_hardware_interface::kDefaultKpArm), std::end(spot_hardware_interface::kDefaultKpArm)); - kd.assign(std::begin(spot_hardware_interface::kDefaultKdArm), std::end(spot_hardware_interface::kDefaultKdArm)); - break; - case spot_hardware_interface::kNjointsNoArm: - kp.assign(std::begin(spot_hardware_interface::kDefaultKpNoArm), - std::end(spot_hardware_interface::kDefaultKpNoArm)); - kd.assign(std::begin(spot_hardware_interface::kDefaultKdNoArm), - std::end(spot_hardware_interface::kDefaultKdNoArm)); - break; - default: - RCLCPP_ERROR(rclcpp::get_logger("SpotHardware"), "WRONG # OF JOINTS"); - return false; - } - joint_cmd->mutable_gains()->mutable_k_q_p()->Clear(); - joint_cmd->mutable_gains()->mutable_k_q_p()->Add(kp.begin(), kp.end()); + joint_cmd->mutable_gains()->mutable_k_q_p()->Add(k_q_p_.begin(), k_q_p_.end()); joint_cmd->mutable_gains()->mutable_k_qd_p()->Clear(); - joint_cmd->mutable_gains()->mutable_k_qd_p()->Add(kd.begin(), kd.end()); + joint_cmd->mutable_gains()->mutable_k_qd_p()->Add(k_qd_p_.begin(), k_qd_p_.end()); // Let it extrapolate the command a little joint_cmd->mutable_extrapolation_duration()->CopyFrom( diff --git a/spot_ros2_control/README.md b/spot_ros2_control/README.md index 99f39f0a6..70604be68 100644 --- a/spot_ros2_control/README.md +++ b/spot_ros2_control/README.md @@ -24,6 +24,14 @@ You can then run the launchfile with the following command: ros2 launch spot_ros2_control spot_ros2_control.launch.py hardware_interface:=robot config_file:=path/to/spot_ros.yaml ``` +Joint level gains can also be specified in this same config file under the parameter names `k_q_p` and `k_qd_p`. If you do not specify these parameters, the default gains from the `spot-sdk` joint control examples are used during command streaming. The example below shows a valid configuration for a robot with an arm as each list contains 19 elements. More information on how these gains are used by Spot can be found [here](https://dev.bostondynamics.com/docs/concepts/joint_control/readme). +``` +/**: + ros__parameters: + k_q_p: [624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 624.0, 936.0, 286.0, 1020.0, 255.0, 204.0, 102.0, 102.0, 102.0, 16.0] + k_qd_p: [5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 5.20, 5.20, 2.04, 10.2, 15.3, 10.2, 2.04, 2.04, 2.04, 0.32] +``` + If you wish to launch these nodes in a namespace, add the argument `spot_name:=`. This hardware interface will stream the joint angles of the robot at 333 Hz onto the topic `//low_level/joint_states`. diff --git a/spot_ros2_control/launch/spot_ros2_control.launch.py b/spot_ros2_control/launch/spot_ros2_control.launch.py index 0d68251c0..923ab2767 100644 --- a/spot_ros2_control/launch/spot_ros2_control.launch.py +++ b/spot_ros2_control/launch/spot_ros2_control.launch.py @@ -16,11 +16,13 @@ ) from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare +from synchros2.launch.actions import DeclareBooleanLaunchArgument, update_sigterm_sigkill_timeout from spot_driver.launch.spot_launch_helpers import ( IMAGE_PUBLISHER_ARGS, declare_image_publisher_args, get_login_parameters, + get_ros_param_dict, spot_has_arm, ) @@ -104,18 +106,30 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: spot_name: str = LaunchConfiguration("spot_name").perform(context) config_file: str = LaunchConfiguration("config_file").perform(context) - # If connected to a physical robot, query if it has an arm. Otherwise, use the value in mock_arm. + # Default parameters used in the URDF if not connected to a robot + arm = mock_arm + login_params = "" + gain_params = "" + + # If running on robot, query if it has an arm, and parse config for login parameters and gains if hardware_interface == "robot": arm = spot_has_arm(config_file_path=config_file, spot_name="") username, password, hostname = get_login_parameters(config_file)[:3] - login_params = f" hostname:={hostname} username:={username} password:={password}" - else: - arm = mock_arm - login_params = "" + login_params = f" hostname:={hostname} username:={username} password:={password} " + param_dict = get_ros_param_dict(config_file) + if "k_q_p" in param_dict: + # we pass the gains to the xacro as space-separated strings as the hardware interface needs to read in all + # of its hardware parameters as strings, and it is easier to parse them out from the config file here. + # eg: k_q_p: [1, 2, 3] in the config file will get translated to the string "1 2 3" here + k_q_p = " ".join(map(str, param_dict["k_q_p"])) + gain_params += f' k_q_p:="{k_q_p}" ' + if "k_qd_p" in param_dict: + k_qd_p = " ".join(map(str, param_dict["k_qd_p"])) + gain_params += f' k_qd_p:="{k_qd_p}" ' tf_prefix = f"{spot_name}/" if spot_name else "" - # Generate the robot description based off if the robot has an arm. + # Generate the robot description containing the ros2 control tags and hardware interface parameters. robot_urdf = Command( [ PathJoinSubstitution([FindExecutable(name="xacro")]), @@ -128,6 +142,7 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None: " hardware_interface_type:=", LaunchConfiguration("hardware_interface"), login_params, + gain_params, ] ) robot_description = {"robot_description": robot_urdf} @@ -262,16 +277,14 @@ def generate_launch_description(): default_value="forward_position_controller", description="Robot controller to start. Must match an entry in controllers_config.", ), - DeclareLaunchArgument( + DeclareBooleanLaunchArgument( "mock_arm", - default_value="false", - choices=["True", "true", "False", "false"], + default_value=False, description="If in hardware_interface:=mock mode, whether or not the mocked robot has an arm.", ), - DeclareLaunchArgument( + DeclareBooleanLaunchArgument( "launch_rviz", - default_value="true", - choices=["True", "true", "False", "false"], + default_value=True, description="Flag to enable rviz.", ), DeclareLaunchArgument( @@ -279,15 +292,15 @@ def generate_launch_description(): default_value="", description="Name of the Spot that will be used as a namespace.", ), - DeclareLaunchArgument( + DeclareBooleanLaunchArgument( "launch_image_publishers", - default_value="true", - choices=["True", "true", "False", "false"], + default_value=True, description="Choose whether to launch the image publishers.", ), ] + declare_image_publisher_args() ) # Add nodes to launch description + update_sigterm_sigkill_timeout(ld) ld.add_action(OpaqueFunction(function=launch_setup, args=[ld])) return ld diff --git a/spot_wrapper b/spot_wrapper index 76e522bbb..c78ebfb12 160000 --- a/spot_wrapper +++ b/spot_wrapper @@ -1 +1 @@ -Subproject commit 76e522bbb7c6351290e71b8d2acfea3aae0adb49 +Subproject commit c78ebfb1236921c820fe27ae70fd58d4f2257a16