Saving my progress, still non functional

spot_controllers/CMakeLists.txt

@@ -38,6 +38,11 @@ generate_parameter_library(
   include/spot_controllers/forward_state_controller_parameters.yaml
 )
 
+generate_parameter_library(
+  spot_forward_controller_parameters
+  include/spot_controllers/spot_forward_controller_parameters.yaml
+)
+
 # Add the hardware interface
 add_library(
   spot_controllers
@@ -53,6 +58,7 @@ target_include_directories(spot_controllers PUBLIC
 target_link_libraries(
   spot_controllers PUBLIC 
   forward_state_controller_parameters
+  spot_forward_controller_parameters
   forward_command_controller::forward_command_controller
 )
 ament_target_dependencies(
@@ -72,7 +78,7 @@ install(
   DESTINATION include/${PROJECT_NAME}
 )
 
-install(TARGETS spot_controllers forward_state_controller_parameters
+install(TARGETS spot_controllers forward_state_controller_parameters spot_forward_controller_parameters
   EXPORT export_spot_controllers
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib

spot_controllers/include/spot_controllers/spot_forward_controller.hpp

@@ -18,24 +18,27 @@
 #include <string>
 #include <vector>
 
+#include <spot_controllers/spot_forward_controller_parameters.hpp>
 #include "controller_interface/controller_interface.hpp"
 #include "rclcpp/subscription.hpp"
 #include "rclcpp_lifecycle/state.hpp"
 #include "realtime_tools/realtime_buffer.hpp"
+#include "sensor_msgs/msg/joint_state.hpp"
 #include "spot_controllers/visibility_control.h"
 #include "spot_msgs/msg/joint_command.hpp"
 #include "std_msgs/msg/float64_multi_array.hpp"
 
 namespace spot_controllers {
-using CmdType = spot_msgs::msg::JointCommand;
+// using CmdType = spot_msgs::msg::JointCommand;
+using CmdType = sensor_msgs::msg::JointState;
 
 /**
  * \brief Forward command controller for a set of joints and interfaces.
  *
  * This class forwards the command signal down to a set of joints or interfaces.
  *
  * Subscribes to:
- * - \b commands (spot_msgs::msg::JointCommand) : The commands to apply.
+ * - \b joint_commands (spot_msgs::msg::JointCommand) : The commands to apply.
  */
 class SpotForwardController : public controller_interface::ControllerInterface {
  public:
@@ -58,6 +61,12 @@ class SpotForwardController : public controller_interface::ControllerInterface {
   controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
 
  protected:
+  using Params = spot_forward_controller::Params;
+  using ParamListener = spot_forward_controller::ParamListener;
+
+  std::shared_ptr<ParamListener> param_listener_;
+  Params params_;
+
   std::vector<std::string> joint_names_;
   std::string interface_name_;
 

spot_controllers/include/spot_controllers/spot_forward_controller_parameters.yaml

@@ -0,0 +1,16 @@
+spot_forward_controller:
+  joints: {
+    type: string_array,
+    default_value: [],
+    description: "Names of the joint to control",
+  }
+  joint_prefix: {
+    type: string,
+    default_value: "",
+    description: "Optional prefix to apply to joint names"
+  }
+  interface_names: {
+    type: string_array,
+    default_value: [],
+    description: "Names of the interfaces to command",
+  }

spot_controllers/package.xml

@@ -17,6 +17,7 @@
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_lifecycle</depend>
+  <depend>spot_msgs</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

spot_controllers/src/spot_forward_controller.cpp

@@ -42,18 +42,15 @@ controller_interface::CallbackReturn SpotForwardController::on_init() {
   //   return controller_interface::CallbackReturn::ERROR;
   // }
 
+  auto param_listener = std::make_shared<ParamListener>(get_node());
+  params_ = param_listener->get_params();
+
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
 controller_interface::CallbackReturn SpotForwardController::on_configure(
     const rclcpp_lifecycle::State& /*previous_state*/) {
-  // auto ret = this->read_parameters();
-  // if (ret != controller_interface::CallbackReturn::SUCCESS)
-  // {
-  //   return ret;
-  // }
-
-  joints_command_subscriber_ = get_node()->create_subscription<CmdType>("~/commands", rclcpp::SystemDefaultsQoS(),
+  joints_command_subscriber_ = get_node()->create_subscription<CmdType>("~/joint_commands", rclcpp::SystemDefaultsQoS(),
                                                                         [this](const CmdType::SharedPtr msg) {
                                                                           rt_command_ptr_.writeFromNonRT(msg);
                                                                         });
@@ -65,12 +62,16 @@ controller_interface::CallbackReturn SpotForwardController::on_configure(
 controller_interface::InterfaceConfiguration SpotForwardController::command_interface_configuration() const {
   controller_interface::InterfaceConfiguration command_interfaces_config;
   command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
-  command_interfaces_config.names = command_interface_types_;
+  for (const auto& joint : params_.joints) {
+    const auto interface_name = joint + "/" + "position";
+    command_interfaces_config.names.push_back(interface_name);
+  }
 
   return command_interfaces_config;
 }
 
 controller_interface::InterfaceConfiguration SpotForwardController::state_interface_configuration() const {
+  // this controller has no knowledge of robot state... but it could (i.e. basic command validation?)
   return controller_interface::InterfaceConfiguration{controller_interface::interface_configuration_type::NONE};
 }
 

spot_ros2_control/config/spot_default_controllers_with_arm.yaml

@@ -14,6 +14,9 @@ controller_manager:
     forward_state_controller:
       type: spot_controllers/ForwardStateController
 
+    spot_forward_controller:
+      type: spot_controllers/SpotForwardController
+
 
 forward_position_controller:
   ros__parameters:
@@ -65,3 +68,27 @@ forward_state_controller:
       - position
       - velocity
       - effort
+
+
+spot_forward_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