From 668a3b7f65a673ef1b692ce66f6607cf17c0e361 Mon Sep 17 00:00:00 2001
From: Katie Hughes <157421702+khughes-bdai@users.noreply.github.com>
Date: Thu, 19 Sep 2024 17:06:43 -0400
Subject: [PATCH] [SW-1392] move all xacros to spot_description (#484)
## Change Overview
It was confusing to have two different xacros for spot (one in spot description and one in spot ros2 control). This PR moves everything into spot description. The default URDF that gets generated by the spot driver is still the same, but now if you add the `add_ros2_control_tags:=True` when generating `spot.xacro.urdf` we get the ROS 2 control tags added.
Additionally fixes a one line issue with taking the lease in the hardware interface -- now even if the tablet has the lease, the hardware interface can take it.
## Testing Done
- [x] successfully ran ros2 control stack on robot
- [x] successfully ran driver on robot
---
.../urdf/spot.ros2_control.xacro | 29 ++++++++++++++++
spot_description/urdf/spot.urdf.xacro | 23 ++++++++++++-
spot_ros2_control/CMakeLists.txt | 2 +-
.../hardware/src/spot_hardware_interface.cpp | 2 +-
.../launch/spot_ros2_control.launch.py | 20 +++++------
.../xacro/spot.ros2_control.xacro | 33 -------------------
spot_ros2_control/xacro/spot.urdf.xacro | 31 -----------------
7 files changed, 63 insertions(+), 77 deletions(-)
rename spot_ros2_control/xacro/spot_interfaces.ros2_control.xacro => spot_description/urdf/spot.ros2_control.xacro (66%)
delete mode 100644 spot_ros2_control/xacro/spot.ros2_control.xacro
delete mode 100644 spot_ros2_control/xacro/spot.urdf.xacro
diff --git a/spot_ros2_control/xacro/spot_interfaces.ros2_control.xacro b/spot_description/urdf/spot.ros2_control.xacro
similarity index 66%
rename from spot_ros2_control/xacro/spot_interfaces.ros2_control.xacro
rename to spot_description/urdf/spot.ros2_control.xacro
index 1978c31c3..d8b5de3fa 100644
--- a/spot_ros2_control/xacro/spot_interfaces.ros2_control.xacro
+++ b/spot_description/urdf/spot.ros2_control.xacro
@@ -65,4 +65,33 @@
+
+
+
+
+
+ mock_components/GenericSystem
+ false
+ 0.0
+
+
+ spot_ros2_control/SpotHardware
+ $(optenv SPOT_IP ${hostname})
+ $(optenv BOSDYN_CLIENT_USERNAME ${username})
+ $(optenv BOSDYN_CLIENT_PASSWORD ${password})
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/spot_description/urdf/spot.urdf.xacro b/spot_description/urdf/spot.urdf.xacro
index 5d58172e8..ed0ad8faa 100644
--- a/spot_description/urdf/spot.urdf.xacro
+++ b/spot_description/urdf/spot.urdf.xacro
@@ -1,17 +1,38 @@
+
+
+
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/spot_ros2_control/CMakeLists.txt b/spot_ros2_control/CMakeLists.txt
index 39a5f79e7..eb51cfd4e 100644
--- a/spot_ros2_control/CMakeLists.txt
+++ b/spot_ros2_control/CMakeLists.txt
@@ -90,7 +90,7 @@ install(
)
install(
- DIRECTORY xacro launch config rviz
+ DIRECTORY launch config rviz
DESTINATION share/${PROJECT_NAME}
)
diff --git a/spot_ros2_control/hardware/src/spot_hardware_interface.cpp b/spot_ros2_control/hardware/src/spot_hardware_interface.cpp
index b11149c1b..e5106dcd1 100644
--- a/spot_ros2_control/hardware/src/spot_hardware_interface.cpp
+++ b/spot_ros2_control/hardware/src/spot_hardware_interface.cpp
@@ -339,7 +339,7 @@ bool SpotHardware::get_lease() {
}
lease_client_ = lease_client_resp.response;
// Then acquire the lease for the body.
- const auto lease_res = lease_client_->AcquireLease("body");
+ const auto lease_res = lease_client_->TakeLease("body");
if (!lease_res) {
RCLCPP_ERROR(rclcpp::get_logger("SpotHardware"), "Could not acquire body lease");
return false;
diff --git a/spot_ros2_control/launch/spot_ros2_control.launch.py b/spot_ros2_control/launch/spot_ros2_control.launch.py
index 5ed0c9e71..abecbd79d 100644
--- a/spot_ros2_control/launch/spot_ros2_control.launch.py
+++ b/spot_ros2_control/launch/spot_ros2_control.launch.py
@@ -93,17 +93,17 @@ def create_rviz_config(spot_name: str) -> str:
def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
hardware_interface: str = LaunchConfiguration("hardware_interface").perform(context)
controllers_config: str = LaunchConfiguration("controllers_config").perform(context)
- mock_has_arm: bool = IfCondition(LaunchConfiguration("mock_has_arm")).evaluate(context)
+ mock_arm: bool = IfCondition(LaunchConfiguration("mock_arm")).evaluate(context)
spot_name: str = LaunchConfiguration("spot_name").perform(context)
- # If connected to a physical robot, query if it has an arm. Otherwise, use the value in mock_has_arm.
+ # If connected to a physical robot, query if it has an arm. Otherwise, use the value in mock_arm.
if hardware_interface == "robot":
config_file = LaunchConfiguration("config_file").perform(context)
- has_arm = spot_has_arm(config_file_path=config_file, spot_name="")
+ 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:
- has_arm = mock_has_arm
+ arm = mock_arm
login_params = ""
tf_prefix = f"{spot_name}/" if spot_name else ""
@@ -113,9 +113,9 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
- PathJoinSubstitution([FindPackageShare(THIS_PACKAGE), "xacro", "spot.urdf.xacro"]),
- " has_arm:=",
- str(has_arm),
+ PathJoinSubstitution([FindPackageShare("spot_description"), "urdf", "spot.urdf.xacro"]),
+ " add_ros2_control_tag:=True arm:=",
+ str(arm),
" tf_prefix:=",
tf_prefix,
" hardware_interface_type:=",
@@ -128,8 +128,8 @@ def launch_setup(context: LaunchContext, ld: LaunchDescription) -> None:
# If no controller config file is selected, use the appropriate default. Else, just use the yaml that is passed in.
if controllers_config == "":
# Generate spot_default_controllers.yaml depending on namespace and whether the robot has an arm.
- create_controllers_config(spot_name, has_arm)
- controllers_config = create_controllers_config(spot_name, has_arm)
+ create_controllers_config(spot_name, arm)
+ controllers_config = create_controllers_config(spot_name, arm)
# Add nodes
ld.add_action(
@@ -217,7 +217,7 @@ def generate_launch_description():
description="Robot controller to start. Must match an entry in controller_config.",
),
DeclareLaunchArgument(
- "mock_has_arm",
+ "mock_arm",
default_value="false",
choices=["True", "true", "False", "false"],
description="If in hardware_interface:=mock mode, whether or not the mocked robot has an arm.",
diff --git a/spot_ros2_control/xacro/spot.ros2_control.xacro b/spot_ros2_control/xacro/spot.ros2_control.xacro
deleted file mode 100644
index 3bca56910..000000000
--- a/spot_ros2_control/xacro/spot.ros2_control.xacro
+++ /dev/null
@@ -1,33 +0,0 @@
-
-
-
-
-
-
-
-
- mock_components/GenericSystem
- false
- 0.0
-
-
- spot_ros2_control/SpotHardware
- $(optenv SPOT_IP ${hostname})
- $(optenv BOSDYN_CLIENT_USERNAME ${username})
- $(optenv BOSDYN_CLIENT_PASSWORD ${password})
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/spot_ros2_control/xacro/spot.urdf.xacro b/spot_ros2_control/xacro/spot.urdf.xacro
deleted file mode 100644
index 3f36be7e2..000000000
--- a/spot_ros2_control/xacro/spot.urdf.xacro
+++ /dev/null
@@ -1,31 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-