Maybe working

spot_controllers/src/spot_forward_controller.cpp

@@ -63,8 +63,9 @@ controller_interface::InterfaceConfiguration SpotForwardController::command_inte
   controller_interface::InterfaceConfiguration command_interfaces_config;
   command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
   for (const auto& joint : params_.joints) {
-    const auto interface_name = joint + "/" + "position";
-    command_interfaces_config.names.push_back(interface_name);
+    // these are the interfaces that get claimed
+    command_interfaces_config.names.push_back(joint + "/" + "position");
+    command_interfaces_config.names.push_back(joint + "/" + "velocity");
   }
 
   return command_interfaces_config;
@@ -111,21 +112,42 @@ controller_interface::return_type SpotForwardController::update(const rclcpp::Ti
   if (!joint_commands || !(*joint_commands)) {
     return controller_interface::return_type::OK;
   }
-
-  // fix this for new command type
-  // if ((*joint_commands)->data.size() != command_interfaces_.size())
-  // {
-  //   RCLCPP_ERROR_THROTTLE(
-  //     get_node()->get_logger(), *(get_node()->get_clock()), 1000,
-  //     "command size (%zu) does not match number of interfaces (%zu)",
-  //     (*joint_commands)->data.size(), command_interfaces_.size());
-  //   return controller_interface::return_type::ERROR;
-  // }
-
-  // for (auto index = 0ul; index < command_interfaces_.size(); ++index)
-  // {
-  //   command_interfaces_[index].set_value((*joint_commands)->data[index]);
-  // }
+  // command_interfaces_.at(i).get_name() is full name i.e. arm_sh0/position
+  // .get_interface_name() is just interface i.e. position
+  // .get_prefix_name() is the joint name i.e. arm_sh0
+
+  const auto& joint_names = (*joint_commands)->name;
+  const auto& njoints_to_command = joint_names.size();
+
+  const bool using_position = (*joint_commands)->position.size() == njoints_to_command;
+  const bool using_velocity = (*joint_commands)->velocity.size() == njoints_to_command;
+  const bool using_effort = (*joint_commands)->effort.size() == njoints_to_command;
+
+  for (size_t i = 0; i < command_interfaces_.size(); i++) {
+    // idea: for every element in the command interface:
+    // first tell if the name is in the name field of the message
+    // if it is, set the interfaces accordingly
+    const auto& joint_name = command_interfaces_.at(i).get_prefix_name();
+    const auto& interface_name = command_interfaces_.at(i).get_interface_name();
+    // check if joint_name in name field of message
+    const auto& it = std::find(joint_names.begin(), joint_names.end(), joint_name);
+    if (it == joint_names.end()) {
+      // it wasn't here, keep looping
+      continue;
+    }
+    // get the index that the name is at in the command
+    const auto command_index = std::distance(joint_names.begin(), it);
+    // check if interface name is there and update accordingly
+    if (interface_name == "position" && using_position) {
+      command_interfaces_.at(i).set_value((*joint_commands)->position.at(command_index));
+    }
+    if (interface_name == "velocity" && using_velocity) {
+      command_interfaces_.at(i).set_value((*joint_commands)->position.at(command_index));
+    }
+    if (interface_name == "effort" && using_effort) {
+      command_interfaces_.at(i).set_value((*joint_commands)->effort.at(command_index));
+    }
+  }
 
   return controller_interface::return_type::OK;
 }