From 7efe1221810e32991421abdcfe626816cb5a66c6 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Wed, 2 Oct 2024 10:17:26 -0400 Subject: [PATCH 1/6] [dependabot] Bump ros_utilities from `2dc3c6b` to `e4f6138` (#496) Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- ros_utilities | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros_utilities b/ros_utilities index 2dc3c6b0..e4f61380 160000 --- a/ros_utilities +++ b/ros_utilities @@ -1 +1 @@ -Subproject commit 2dc3c6b04e1ca5b901328a3af705f9baf5dcf82a +Subproject commit e4f61380dac5acfc03cc22439b0eaca6d9e37238 From f5ee65365f16354000db869176062af6d58b6e26 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Thu, 3 Oct 2024 09:47:17 -0400 Subject: [PATCH 2/6] [dependabot] Bump ros_utilities from `e4f6138` to `6ed50d7` (#498) Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- ros_utilities | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros_utilities b/ros_utilities index e4f61380..6ed50d76 160000 --- a/ros_utilities +++ b/ros_utilities @@ -1 +1 @@ -Subproject commit e4f61380dac5acfc03cc22439b0eaca6d9e37238 +Subproject commit 6ed50d7686c238c9a721fed3c7d0a6cdca6c0f4e From e0d249c3bfdced8c70c38943e137ed2d2a2c2702 Mon Sep 17 00:00:00 2001 From: Katie Hughes <157421702+khughes-bdai@users.noreply.github.com> Date: Thu, 3 Oct 2024 10:38:44 -0400 Subject: [PATCH 3/6] [SW-1391] controller that lets you specify position, velocity, and load at the same time (#493) ## Change Overview Adds a new package `spot_controllers` that can contain custom ros2 controllers for spot. Adds a custom controller `forward_state_controller`. This uses the same base class that the built-in `forward_command_controller` and `multi_interface_forward_command_controller` use. The main difference is that this controller accepts a list of joints and a list of interfaces to forward commands over. This controller is then set up with `spot_ros2_control` so that you can forward a list of position, velocity, and load commands to the low level API at the same time. To launch with this controller add the launch argument `robot_controller:=forward_state_controller` To send commands, publish to the `//forward_state_controller/commands` topic, eg `ros2 topic pub /forward_state_controller/commands std_msgs/msg/Float64MultiArray "{data: [ ]}" ` Additionally, if joint gains are ever added to the command interface, they could be forwarded via this same type of controller by just inputting a different set of parameters ## Testing Done - mock mode / forward position controller - mock mode / forward state controller - robot mode / forward position controller - robot mode / forward state controller all are loaded successfully! - [x] mock mode correctly forwards a dummy list of position/velocity/effort into the joint states - [x] examples running regular forward position commands still work --- spot_controllers/CMakeLists.txt | 82 +++++++++++++++++++ spot_controllers/README.md | 7 ++ .../forward_state_controller.hpp | 57 +++++++++++++ .../forward_state_controller_parameters.yaml | 11 +++ .../spot_controllers/visibility_control.h | 60 ++++++++++++++ spot_controllers/package.xml | 24 ++++++ spot_controllers/spot_controllers.xml | 7 ++ .../src/forward_state_controller.cpp | 69 ++++++++++++++++ spot_ros2_control/CMakeLists.txt | 19 ++--- spot_ros2_control/README.md | 6 +- .../spot_default_controllers_with_arm.yaml | 30 +++++++ .../spot_default_controllers_without_arm.yaml | 23 ++++++ .../launch/spot_ros2_control.launch.py | 17 ++-- spot_ros2_control/package.xml | 14 +++- 14 files changed, 400 insertions(+), 26 deletions(-) create mode 100644 spot_controllers/CMakeLists.txt create mode 100644 spot_controllers/README.md create mode 100644 spot_controllers/include/spot_controllers/forward_state_controller.hpp create mode 100644 spot_controllers/include/spot_controllers/forward_state_controller_parameters.yaml create mode 100644 spot_controllers/include/spot_controllers/visibility_control.h create mode 100644 spot_controllers/package.xml create mode 100644 spot_controllers/spot_controllers.xml create mode 100644 spot_controllers/src/forward_state_controller.cpp diff --git a/spot_controllers/CMakeLists.txt b/spot_controllers/CMakeLists.txt new file mode 100644 index 00000000..46948938 --- /dev/null +++ b/spot_controllers/CMakeLists.txt @@ -0,0 +1,82 @@ +# Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. + +cmake_minimum_required(VERSION 3.22) + +# This is here so we can use jthread from C++ 20 +set(CMAKE_CXX_STANDARD 20) + +project(spot_controllers) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Dependencies +find_package(ament_cmake REQUIRED) +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + forward_command_controller + pluginlib + rclcpp + rclcpp_lifecycle +) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +generate_parameter_library( + forward_state_controller_parameters + include/spot_controllers/forward_state_controller_parameters.yaml +) + +# Add the hardware interface +add_library( + spot_controllers + SHARED + src/forward_state_controller.cpp +) +target_compile_features(spot_controllers PUBLIC cxx_std_20) +target_include_directories(spot_controllers PUBLIC + $ + $ +) +target_link_libraries( + spot_controllers PUBLIC + forward_state_controller_parameters forward_command_controller::forward_command_controller +) +ament_target_dependencies( + spot_controllers PUBLIC + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} PRIVATE "SPOT_CONTROLLERS_BUILDING_DLL") + +# Export controller plugin +pluginlib_export_plugin_description_file(controller_interface spot_controllers.xml) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install(TARGETS spot_controllers forward_state_controller_parameters + EXPORT export_spot_controllers + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +ament_export_targets(export_spot_controllers HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) + +ament_package() diff --git a/spot_controllers/README.md b/spot_controllers/README.md new file mode 100644 index 00000000..deeb63c4 --- /dev/null +++ b/spot_controllers/README.md @@ -0,0 +1,7 @@ +# spot_controllers + +This is a ROS 2 package that provides custom ROS 2 controllers that can be used with [spot_ros2_control](../spot_ros2_control/). + +Currently, this package consists of a single generic controller: `spot_controllers/ForwardStateController`. This controller allows you to forward a set of commands over a set of interfaces. It is used with `spot_ros2_control` to forwad commands for position, velocity, and effort for all joints at the same time. + +Example configurations for setting up this controller can be found in [`spot_ros2_control/config`](../spot_ros2_control/config/). diff --git a/spot_controllers/include/spot_controllers/forward_state_controller.hpp b/spot_controllers/include/spot_controllers/forward_state_controller.hpp new file mode 100644 index 00000000..01526d2d --- /dev/null +++ b/spot_controllers/include/spot_controllers/forward_state_controller.hpp @@ -0,0 +1,57 @@ +// File modified. Modifications Copyright (c) 2024 Boston Dynamics AI Institute LLC. +// All rights reserved. + +// -------------------------------------------------------------- +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include + +#include "forward_command_controller/forward_command_controller/forward_controllers_base.hpp" +#include "forward_state_controller_parameters.hpp" // NOLINT(build/include_subdir) +#include "spot_controllers/visibility_control.h" + +namespace spot_controllers { +/** + * \brief Forward command controller for a set of interfaces. + * + * This class forwards the command signal for a set of interfaces over a set of joints. + * + * \param joints Names of the joint to control. + * \param interface_names Names of the interfaces to command. + * + * Subscribes to: + * - \b commands (std_msgs::msg::Float64MultiArray) : The commands to apply. + */ +class ForwardStateController : public forward_command_controller::ForwardControllersBase { + public: + SPOT_CONTROLLERS_PUBLIC + ForwardStateController(); + + protected: + void declare_parameters() override; + controller_interface::CallbackReturn read_parameters() override; + + using Params = forward_state_controller::Params; + using ParamListener = forward_state_controller::ParamListener; + + std::shared_ptr param_listener_; + Params params_; +}; + +} // namespace spot_controllers diff --git a/spot_controllers/include/spot_controllers/forward_state_controller_parameters.yaml b/spot_controllers/include/spot_controllers/forward_state_controller_parameters.yaml new file mode 100644 index 00000000..8ae61753 --- /dev/null +++ b/spot_controllers/include/spot_controllers/forward_state_controller_parameters.yaml @@ -0,0 +1,11 @@ +forward_state_controller: + joints: { + type: string_array, + default_value: [], + description: "Names of the joint to control", + } + interface_names: { + type: string_array, + default_value: [], + description: "Names of the interfaces to command", + } diff --git a/spot_controllers/include/spot_controllers/visibility_control.h b/spot_controllers/include/spot_controllers/visibility_control.h new file mode 100644 index 00000000..ecc3c30d --- /dev/null +++ b/spot_controllers/include/spot_controllers/visibility_control.h @@ -0,0 +1,60 @@ +// File modified. Modifications Copyright (c) 2024 Boston Dynamics AI Institute LLC. +// All rights reserved. + +// -------------------------------------------------------------- +// Copyright 2021 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +// Source: +// https://github.com/ros-controls/ros2_control_demos/blob/master/example_1/hardware/include/ros2_control_demo_example_1/visibility_control.h + +#pragma once + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define SPOT_CONTROLLERS_EXPORT __attribute__((dllexport)) +#define SPOT_CONTROLLERS_IMPORT __attribute__((dllimport)) +#else +#define SPOT_CONTROLLERS_EXPORT __declspec(dllexport) +#define SPOT_CONTROLLERS_IMPORT __declspec(dllimport) +#endif +#ifdef SPOT_CONTROLLERS_BUILDING_DLL +#define SPOT_CONTROLLERS_PUBLIC SPOT_CONTROLLERS_EXPORT +#else +#define SPOT_CONTROLLERS_PUBLIC SPOT_CONTROLLERS_IMPORT +#endif +#define SPOT_CONTROLLERS_PUBLIC_TYPE SPOT_CONTROLLERS_PUBLIC +#define SPOT_CONTROLLERS_LOCAL +#else +#define SPOT_CONTROLLERS_EXPORT __attribute__((visibility("default"))) +#define SPOT_CONTROLLERS_IMPORT +#if __GNUC__ >= 4 +#define SPOT_CONTROLLERS_PUBLIC __attribute__((visibility("default"))) +#define SPOT_CONTROLLERS_LOCAL __attribute__((visibility("hidden"))) +#else +#define SPOT_CONTROLLERS_PUBLIC +#define SPOT_CONTROLLERS_LOCAL +#endif +#define SPOT_CONTROLLERS_PUBLIC_TYPE +#endif diff --git a/spot_controllers/package.xml b/spot_controllers/package.xml new file mode 100644 index 00000000..0b441fbb --- /dev/null +++ b/spot_controllers/package.xml @@ -0,0 +1,24 @@ + + + + spot_controllers + 0.0.0 + ROS 2 controllers that can be used with Spot + khughes + MIT + + ament_cmake + + ament_lint_auto + ament_lint_common + + controller_interface + forward_command_controller + pluginlib + rclcpp + rclcpp_lifecycle + + + ament_cmake + + diff --git a/spot_controllers/spot_controllers.xml b/spot_controllers/spot_controllers.xml new file mode 100644 index 00000000..e967a8da --- /dev/null +++ b/spot_controllers/spot_controllers.xml @@ -0,0 +1,7 @@ + + + + General passthrough controller that can forward commands for a set of joints over a set of interfaces. + + + diff --git a/spot_controllers/src/forward_state_controller.cpp b/spot_controllers/src/forward_state_controller.cpp new file mode 100644 index 00000000..34635c40 --- /dev/null +++ b/spot_controllers/src/forward_state_controller.cpp @@ -0,0 +1,69 @@ +// File modified. Modifications Copyright (c) 2024 Boston Dynamics AI Institute LLC. +// All rights reserved. + +// -------------------------------------------------------------- +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "spot_controllers/forward_state_controller.hpp" + +#include +#include +#include +#include +#include + +#include "rclcpp/logging.hpp" +#include "rclcpp/qos.hpp" + +namespace spot_controllers { +ForwardStateController::ForwardStateController() : forward_command_controller::ForwardControllersBase() {} + +void ForwardStateController::declare_parameters() { + param_listener_ = std::make_shared(get_node()); +} + +controller_interface::CallbackReturn ForwardStateController::read_parameters() { + if (!param_listener_) { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + return controller_interface::CallbackReturn::ERROR; + } + params_ = param_listener_->get_params(); + + if (params_.joints.empty()) { + RCLCPP_ERROR(get_node()->get_logger(), "'joints' parameter was empty"); + return controller_interface::CallbackReturn::ERROR; + } + + if (params_.interface_names.empty()) { + RCLCPP_ERROR(get_node()->get_logger(), "'interface_name' parameter was empty"); + return controller_interface::CallbackReturn::ERROR; + } + + // Example: if you input joints [1,2,3] and interfaces [A,B,C] as parameters, the order of the command will be + // [1/A, 1/B, 1/C, 2/A, 2/B, 2/C, 3/A, 3/B, 3/C] + for (const auto& interface_name : params_.interface_names) { + for (const auto& joint : params_.joints) { + command_interface_types_.push_back(joint + "/" + interface_name); + } + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +} // namespace spot_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(spot_controllers::ForwardStateController, controller_interface::ControllerInterface) diff --git a/spot_ros2_control/CMakeLists.txt b/spot_ros2_control/CMakeLists.txt index eb51cfd4..a6c878f7 100644 --- a/spot_ros2_control/CMakeLists.txt +++ b/spot_ros2_control/CMakeLists.txt @@ -19,7 +19,6 @@ find_package(ament_cmake REQUIRED) find_package(bosdyn REQUIRED) set(THIS_PACKAGE_INCLUDE_DEPENDS hardware_interface - controller_interface controller_manager pluginlib rclcpp @@ -27,6 +26,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS sensor_msgs std_msgs spot_description + spot_controllers ) foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) @@ -48,8 +48,8 @@ add_library( ) target_compile_features(spot_ros2_control PUBLIC cxx_std_20) target_include_directories(spot_ros2_control PUBLIC -$ -$ + $ + $ ) target_link_libraries(spot_ros2_control PUBLIC bosdyn::bosdyn_client_static) ament_target_dependencies( @@ -61,24 +61,17 @@ ament_target_dependencies( # which is appropriate when building the dll but not consuming it. target_compile_definitions(${PROJECT_NAME} PRIVATE "SPOT_ROS2_CONTROL_BUILDING_DLL") -# Export hardware plugins +# Export hardware plugin pluginlib_export_plugin_description_file(hardware_interface spot_ros2_control.xml) # Add example nodes and install them add_executable(noarm_squat examples/src/noarm_squat.cpp) -# ament_target_dependencies(noarm_squat -# ${THIS_PACKAGE_INCLUDE_DEPENDS} -# ) target_link_libraries(noarm_squat spot_ros2_control) + add_executable(wiggle_arm examples/src/wiggle_arm.cpp) -# ament_target_dependencies(wiggle_arm -# ${THIS_PACKAGE_INCLUDE_DEPENDS} -# ) target_link_libraries(wiggle_arm spot_ros2_control) + add_executable(joint_command_passthrough examples/src/joint_command_passthrough.cpp) -# ament_target_dependencies(joint_command_passthrough -# ${THIS_PACKAGE_INCLUDE_DEPENDS} -# ) target_link_libraries(joint_command_passthrough spot_ros2_control) install(TARGETS noarm_squat wiggle_arm joint_command_passthrough diff --git a/spot_ros2_control/README.md b/spot_ros2_control/README.md index 438287db..b1f74dda 100644 --- a/spot_ros2_control/README.md +++ b/spot_ros2_control/README.md @@ -28,10 +28,12 @@ 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 using the low level API at 333 Hz onto the topic `//low_level/joint_states`. -Commands can be sent on the topic `//forward_position_controller/commands`. This will forward position commands directly to the spot sdk through the hardware interface. +Commands can be sent on the topic `//forward_position_controller/commands`. This will forward position commands directly to the spot sdk through the hardware interface. The controller expects the command array to contain the list of positions to forward for each joint on the robot. + +An alternative feed-forward controller provided by the `spot_controllers` package can be used to specify the position, velocity, and effort of all joints at the same time. To bring up this controller, add the launch argument `robot_controller:=forward_state_controller`. Commands can then be sent on the topic `//forward_state_controller/commands`. This controller expects the ordering of the command array to be `[, , ]`. > [!CAUTION] -> When using the forward position controller, there is no safety mechanism in place to ensure smooth motion. The ordering of the command must match the ordering of the joints specified in the controller configuration file ([here for robots with an arm](config/spot_default_controllers_with_arm.yaml) or [here for robots without an arm](config/spot_default_controllers_without_arm.yaml)), and the robot can move in unpredictable and dangerous ways if this is not set correctly. Make sure to keep a safe distance from the robot when working with this controller and ensure the e-stop can easily be pressed if needed. +> When using the forward position and state controllers, there is no safety mechanism in place to ensure smooth motion. The ordering of the command must match the ordering of the joints specified in the controller configuration file ([here for robots with an arm](config/spot_default_controllers_with_arm.yaml) or [here for robots without an arm](config/spot_default_controllers_without_arm.yaml)), and the robot can move in unpredictable and dangerous ways if this is not set correctly. Make sure to keep a safe distance from the robot when working with these controllers and ensure the e-stop can easily be pressed if needed. Additionally, the state publisher node, object synchronization node, and image publisher nodes from [`spot_driver`](../spot_driver/) will get launched by default when running on the robot to provide extra information such as odometry topics and camera feeds. To turn off the image publishers (which can cause problems with bandwidth), add the launch argument `launch_image_publishers:=false`. diff --git a/spot_ros2_control/config/spot_default_controllers_with_arm.yaml b/spot_ros2_control/config/spot_default_controllers_with_arm.yaml index dd9a5014..557322fe 100644 --- a/spot_ros2_control/config/spot_default_controllers_with_arm.yaml +++ b/spot_ros2_control/config/spot_default_controllers_with_arm.yaml @@ -11,6 +11,9 @@ controller_manager: forward_position_controller: type: forward_command_controller/ForwardCommandController + forward_state_controller: + type: spot_controllers/ForwardStateController + forward_position_controller: ros__parameters: @@ -35,3 +38,30 @@ forward_position_controller: - arm_wr1 - arm_f1x interface_name: position + +forward_state_controller: + ros__parameters: + joints: + - front_left_hip_x + - front_left_hip_y + - front_left_knee + - front_right_hip_x + - front_right_hip_y + - front_right_knee + - rear_left_hip_x + - rear_left_hip_y + - rear_left_knee + - rear_right_hip_x + - rear_right_hip_y + - rear_right_knee + - arm_sh0 + - arm_sh1 + - arm_el0 + - arm_el1 + - arm_wr0 + - arm_wr1 + - arm_f1x + interface_names: + - position + - velocity + - effort diff --git a/spot_ros2_control/config/spot_default_controllers_without_arm.yaml b/spot_ros2_control/config/spot_default_controllers_without_arm.yaml index 33e72c16..a462a4bb 100644 --- a/spot_ros2_control/config/spot_default_controllers_without_arm.yaml +++ b/spot_ros2_control/config/spot_default_controllers_without_arm.yaml @@ -11,6 +11,9 @@ controller_manager: forward_position_controller: type: forward_command_controller/ForwardCommandController + forward_state_controller: + type: spot_controllers/ForwardStateController + forward_position_controller: ros__parameters: @@ -28,3 +31,23 @@ forward_position_controller: - rear_right_hip_y - rear_right_knee interface_name: position + +forward_state_controller: + ros__parameters: + joints: + - front_left_hip_x + - front_left_hip_y + - front_left_knee + - front_right_hip_x + - front_right_hip_y + - front_right_knee + - rear_left_hip_x + - rear_left_hip_y + - rear_left_knee + - rear_right_hip_x + - rear_right_hip_y + - rear_right_knee + interface_names: + - position + - velocity + - effort diff --git a/spot_ros2_control/launch/spot_ros2_control.launch.py b/spot_ros2_control/launch/spot_ros2_control.launch.py index 82aa9eb1..0d68251c 100644 --- a/spot_ros2_control/launch/spot_ros2_control.launch.py +++ b/spot_ros2_control/launch/spot_ros2_control.launch.py @@ -34,8 +34,8 @@ def create_controllers_config(spot_name: str, has_arm: bool) -> str: Args: spot_name (str): Name of spot. If it's the empty string, the default controller file with no namespace is used. - has_arm (bool): Whether or not your robot has an arm. Necessary for defining the joints that the forward - position controller should use. + has_arm (bool): Whether or not your robot has an arm. Necessary for defining the joints that the controllers + should use. Returns: str: Path to controllers config file to use @@ -49,14 +49,15 @@ def create_controllers_config(spot_name: str, has_arm: bool) -> str: if spot_name: with open(template_filename, "r") as template_file: config = yaml.safe_load(template_file) - forward_position_controller_joints = config["forward_position_controller"]["ros__parameters"]["joints"] - config["forward_position_controller"]["ros__parameters"]["joints"] = [ - f"{spot_name}/{joint}" for joint in forward_position_controller_joints - ] config[f"{spot_name}/controller_manager"] = config["controller_manager"] del config["controller_manager"] - config[f"{spot_name}/forward_position_controller"] = config["forward_position_controller"] - del config["forward_position_controller"] + keys_to_namespace = ["forward_position_controller", "forward_state_controller"] + + for key in keys_to_namespace: + key_joints = config[key]["ros__parameters"]["joints"] + config[key]["ros__parameters"]["joints"] = [f"{spot_name}/{joint}" for joint in key_joints] + config[f"{spot_name}/{key}"] = config[key] + del config[key] with NamedTemporaryFile(suffix=".yaml", mode="w", delete=False) as out_file: yaml.dump(config, out_file) diff --git a/spot_ros2_control/package.xml b/spot_ros2_control/package.xml index c29cd633..1f1e8a67 100644 --- a/spot_ros2_control/package.xml +++ b/spot_ros2_control/package.xml @@ -12,11 +12,19 @@ Copyright (c) 2024 Boston Dynamics AI Institute LLC. All rights reserved. ament_cmake - controller_manager + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + bosdyn std_msgs sensor_msgs - spot_description - spot_driver + + controller_manager + spot_description + spot_driver + spot_controllers + xacro ament_lint_auto ament_lint_common From 188c86e764f09aa3e1a1fdd3d6253c9e182b0add Mon Sep 17 00:00:00 2001 From: Katie Hughes <157421702+khughes-bdai@users.noreply.github.com> Date: Mon, 7 Oct 2024 15:29:17 -0400 Subject: [PATCH 4/6] [SW-1465] Fix image stitching for greyscale images (#499) ## Change Overview Image stitcher currently crashes if run on a spot with greyscale cameras, this should resolve that error. See: https://github.com/bdaiinstitute/spot_ros2/issues/448 ## Testing Done - [x] image stitcher on a spot with RGB cameras still works as expected - [x] image stitcher on a spot with greyscale cameras doesn't crash and produces meaningful stitched images. --- spot_driver/src/image_stitcher/image_stitcher.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/spot_driver/src/image_stitcher/image_stitcher.cpp b/spot_driver/src/image_stitcher/image_stitcher.cpp index 2b155ccc..75c7771f 100644 --- a/spot_driver/src/image_stitcher/image_stitcher.cpp +++ b/spot_driver/src/image_stitcher/image_stitcher.cpp @@ -327,8 +327,8 @@ Image::SharedPtr MiddleCamera::stitch(const std::shared_ptr& left, // While the image is coming from the camera on the left of the robot, it sees the right side // of the scene and vice versa. This may need to be extracted if this code is to be generalized // for something other than the Boston Dynamics Spot Robot, as well as checking the homographies. - const auto scene_right = cv_bridge::toCvShare(left); - const auto scene_left = cv_bridge::toCvShare(right); + const auto scene_right = cv_bridge::toCvShare(left, sensor_msgs::image_encodings::BGR8); + const auto scene_left = cv_bridge::toCvShare(right, sensor_msgs::image_encodings::BGR8); // Transform the images into the virtual center camera space cv::warpPerspective(scene_left->image, warped_images_[0], homography_[0], result_size_); From fd07abc4083a0a8ffa8c2cb8f3f91fe2b519f7fb Mon Sep 17 00:00:00 2001 From: Laura Lee <129791566+llee-bdai@users.noreply.github.com> Date: Tue, 8 Oct 2024 15:57:38 -0400 Subject: [PATCH 5/6] [MAPLE-743] Add Cartesian arm pose commands (#501) --- spot_driver/spot_driver/spot_ros2.py | 26 +++++++++++++++++++++++++- 1 file changed, 25 insertions(+), 1 deletion(-) diff --git a/spot_driver/spot_driver/spot_ros2.py b/spot_driver/spot_driver/spot_ros2.py index 3818145a..bddec9d5 100644 --- a/spot_driver/spot_driver/spot_ros2.py +++ b/spot_driver/spot_driver/spot_ros2.py @@ -569,6 +569,9 @@ def __init__(self, parameter_list: Optional[typing.List[Parameter]] = None, **kw self.create_subscription( JointState, "arm_joint_commands", self.arm_joint_cmd_callback, 100, callback_group=self.group ) + self.create_subscription( + PoseStamped, "arm_pose_commands", self.arm_pose_cmd_callback, 100, callback_group=self.group + ) if not self.gripperless: self.create_service( @@ -2530,7 +2533,7 @@ def cmd_velocity_callback(self, data: Twist) -> None: def body_pose_callback(self, data: Pose) -> None: """Callback for cmd_vel command""" if self.spot_wrapper is None: - self.get_logger().info("Mock mode, received command vel " + str(data)) + self.get_logger().info("Mock mode, received command pose " + str(data)) return q = Quaternion() q.x = data.orientation.x @@ -2574,6 +2577,27 @@ def arm_joint_cmd_callback(self, data: JointState) -> None: self.spot_wrapper.arm_joint_cmd(**arm_joint_map) + def arm_pose_cmd_callback(self, data: PoseStamped) -> None: + if not self.spot_wrapper: + self.get_logger().info(f"Mock mode, received arm pose command {data}") + return + result = self.spot_wrapper.spot_arm.hand_pose( + x=data.pose.position.x, + y=data.pose.position.y, + z=data.pose.position.z, + qx=data.pose.orientation.x, + qy=data.pose.orientation.y, + qz=data.pose.orientation.z, + qw=data.pose.orientation.w, + ref_frame=data.header.frame_id, + duration=self.cmd_duration, + ensure_power_on_and_stand=False, + ) + if not result[0]: + self.get_logger().warning(f"Failed to go to arm pose: {result[1]}") + else: + self.get_logger().info("Successfully went to arm pose") + def handle_graph_nav_get_localization_pose( self, request: GraphNavGetLocalizationPose.Response, From 3187a453d7974adf0be952bae558df2c3225c441 Mon Sep 17 00:00:00 2001 From: "dependabot[bot]" <49699333+dependabot[bot]@users.noreply.github.com> Date: Wed, 9 Oct 2024 12:42:22 -0400 Subject: [PATCH 6/6] [dependabot] Bump ros_utilities from `6ed50d7` to `06077a2` (#503) Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> --- ros_utilities | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ros_utilities b/ros_utilities index 6ed50d76..06077a2f 160000 --- a/ros_utilities +++ b/ros_utilities @@ -1 +1 @@ -Subproject commit 6ed50d7686c238c9a721fed3c7d0a6cdca6c0f4e +Subproject commit 06077a2fdcecc444e0127fe421cca3fbded5b72d