[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

spot_ros2_control/xacro/spot_interfaces.ros2_control.xacro → spot_description/urdf/spot.ros2_control.xacro

@@ -65,4 +65,33 @@
 
     </xacro:macro>
 
+    <xacro:macro name="spot_ros2_control" params="interface_type has_arm hostname username password tf_prefix">
+        <!-- Currently implements a simple system interface that covers all joints of the robot.
+        In the future, we could make different hardware interfaces for the body, arm, etc. -->
+        <ros2_control name="SpotSystem" type="system">
+            <hardware>
+            <xacro:if value="${interface_type == 'mock'}">
+                <plugin>mock_components/GenericSystem</plugin>
+                <param name="mock_sensor_commands">false</param>
+                <param name="state_following_offset">0.0</param>
+            </xacro:if>
+            <xacro:if value="${interface_type == 'robot'}">
+                <plugin>spot_ros2_control/SpotHardware</plugin>
+                <param name="hostname">$(optenv SPOT_IP ${hostname})</param>
+                <param name="username">$(optenv BOSDYN_CLIENT_USERNAME ${username})</param>
+                <param name="password">$(optenv BOSDYN_CLIENT_PASSWORD ${password})</param>
+            </xacro:if>
+            </hardware>
+            <!-- Add the legs -->
+            <xacro:leg front_or_rear="front" left_or_right="left" tf_prefix="${tf_prefix}" />
+            <xacro:leg front_or_rear="front" left_or_right="right" tf_prefix="${tf_prefix}" />
+            <xacro:leg front_or_rear="rear" left_or_right="left" tf_prefix="${tf_prefix}" />
+            <xacro:leg front_or_rear="rear" left_or_right="right" tf_prefix="${tf_prefix}" />
+            <!-- Add the arm + the gripper if the robot has an arm -->
+            <xacro:if value="${has_arm}">
+                <xacro:arm tf_prefix="${tf_prefix}" />
+            </xacro:if>
+        </ros2_control>
+    </xacro:macro>
+
 </robot>

spot_description/urdf/spot.urdf.xacro

@@ -1,17 +1,38 @@
 <?xml version="1.0" ?>
 <robot name="spot" xmlns:xacro="http://www.ros.org/wiki/xacro">
 
+  <!-- Macro for loading Spot -->
   <xacro:include filename="$(find spot_description)/urdf/spot_macro.xacro" />
+  <!-- Macros for loading the ROS 2 control tags -->
+  <xacro:include filename="$(find spot_description)/urdf/spot.ros2_control.xacro" />
 
-  <!-- Parameters -->
+  <!-- General parameters -->
   <xacro:arg name="arm" default="false" />
   <xacro:arg name="feet" default="false" />
   <xacro:arg name="tf_prefix" default="" />
 
+  <!-- Parameters for ROS 2 control -->
+  <xacro:arg name="add_ros2_control_tag" default="false" />
+  <xacro:arg name="hardware_interface_type" default="mock" />
+  <xacro:arg name="tf_prefix" default="" />
+  <xacro:arg name="hostname" default="10.0.0.3" />
+  <xacro:arg name="username" default="username" />
+  <xacro:arg name="password" default="password" />
+
   <!-- Load Spot -->
   <xacro:load_spot
     arm="$(arg arm)"
     feet="$(arg feet)"
     tf_prefix="$(arg tf_prefix)" />
 
+  <!-- Adding the ROS 2 control tags -->
+  <xacro:if value="$(arg add_ros2_control_tag)">
+      <xacro:spot_ros2_control interface_type="$(arg hardware_interface_type)" 
+                               has_arm="$(arg arm)" 
+                               hostname="$(arg hostname)" 
+                               username="$(arg username)" 
+                               password="$(arg password)" 
+                               tf_prefix="$(arg tf_prefix)" />
+  </xacro:if>
+
 </robot>

spot_ros2_control/CMakeLists.txt

@@ -90,7 +90,7 @@ install(
 )
 
 install(
-  DIRECTORY xacro launch config rviz
+  DIRECTORY launch config rviz
   DESTINATION share/${PROJECT_NAME}
 )
 

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;

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.",