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