[SW-1391] controller that lets you specify position, velocity, and lo…

…ad 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 `/<robot name>/forward_state_controller/commands` topic, eg
 
`ros2 topic pub /forward_state_controller/commands std_msgs/msg/Float64MultiArray "{data: [<list of desired position> <list of desired velocity> <list of desired load>]}"
`

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

@@ -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
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/spot_controllers>
+)
+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()

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/).

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 <memory>
+#include <string>
+#include <vector>
+
+#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<ParamListener> param_listener_;
+  Params params_;
+};
+
+}  // namespace spot_controllers

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",
+  }

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

spot_controllers/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>spot_controllers</name>
+  <version>0.0.0</version>
+  <description>ROS 2 controllers that can be used with Spot</description>
+  <maintainer email="[email protected]">khughes</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <depend>controller_interface</depend>
+  <depend>forward_command_controller</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

spot_controllers/spot_controllers.xml

@@ -0,0 +1,7 @@
+<library path="spot_controllers">
+  <class name="spot_controllers/ForwardStateController" type="spot_controllers::ForwardStateController" base_class_type="controller_interface::ControllerInterface">
+  <description>
+    General passthrough controller that can forward commands for a set of joints over a set of interfaces.
+  </description>
+  </class>
+</library>

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 <algorithm>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#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<ParamListener>(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)

spot_ros2_control/CMakeLists.txt

@@ -19,14 +19,14 @@ find_package(ament_cmake REQUIRED)
 find_package(bosdyn REQUIRED)
 set(THIS_PACKAGE_INCLUDE_DEPENDS
   hardware_interface
-  controller_interface
   controller_manager
   pluginlib
   rclcpp
   rclcpp_lifecycle
   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
-$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/hardware/include>
-$<INSTALL_INTERFACE:include/spot_ros2_control>
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/hardware/include>
+  $<INSTALL_INTERFACE:include/spot_ros2_control>
 )
 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

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 `/<Robot Name>/low_level/joint_states`.
 
-Commands can be sent on the topic `/<Robot Name>/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 `/<Robot Name>/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 `/<Robot Name>/forward_state_controller/commands`. This controller expects the ordering of the command array to be `[<positions for each joint>, <velocities for each joint>, <efforts for each joint>]`.
 
 > [!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`.