Make spot_name first argument

bdaiinstitute · Oct 16, 2024 · f01da52 · f01da52
1 parent 3e899f8
commit f01da52
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 9 deletions.
diff --git a/spot_ros2_control/examples/src/noarm_squat.cpp b/spot_ros2_control/examples/src/noarm_squat.cpp
@@ -74,10 +74,10 @@ class NoarmSquat : public rclcpp::Node {
   void joint_states_callback(const sensor_msgs::msg::JointState& msg) {
     if (!initialized_) {
       RCLCPP_INFO_STREAM(get_logger(), "Received starting joint states");
-      sensor_msgs::msg::JointState ordered_joint_angles;
-      bool successful = spot_ros2_control::order_joint_states(msg, ordered_joint_angles, spot_name);
+      sensor_msgs::msg::JointState ordered_joint_states;
+      bool successful = spot_ros2_control::order_joint_states(spot_name, msg, ordered_joint_states);
       if (successful) {
-        init_joint_angles_ = ordered_joint_angles.position;
+        init_joint_angles_ = ordered_joint_states.position;
         RCLCPP_INFO_STREAM(get_logger(), "Initialized! Robot will begin to move.");
         initialized_ = true;
       }

diff --git a/spot_ros2_control/examples/src/wiggle_arm.cpp b/spot_ros2_control/examples/src/wiggle_arm.cpp
@@ -70,10 +70,10 @@ class WiggleArm : public rclcpp::Node {
   void joint_states_callback(const sensor_msgs::msg::JointState& msg) {
     if (!initialized_) {
       RCLCPP_INFO_STREAM(get_logger(), "Received starting joint states");
-      sensor_msgs::msg::JointState ordered_joint_angles;
-      bool successful = spot_ros2_control::order_joint_states(msg, ordered_joint_angles, spot_name);
+      sensor_msgs::msg::JointState ordered_joint_states;
+      bool successful = spot_ros2_control::order_joint_states(spot_name, msg, ordered_joint_states);
       if (successful) {
-        nominal_joint_angles_ = ordered_joint_angles.position;
+        nominal_joint_angles_ = ordered_joint_states.position;
         command_.data = nominal_joint_angles_;
         RCLCPP_INFO_STREAM(get_logger(), "Initialized! Robot will begin to move.");
         initialized_ = true;

diff --git a/spot_ros2_control/hardware/include/spot_ros2_control/spot_joint_map.hpp b/spot_ros2_control/hardware/include/spot_ros2_control/spot_joint_map.hpp
@@ -80,12 +80,12 @@ std::unordered_map<std::string, size_t> get_namespaced_joint_map(const std::stri
 
 /// @brief Given a list of joints from a JointStates message, put them in the correct order that the Spot Hardware
 /// interface expects.
+/// @param spot_name Namespace that the ros2 control stack was launched in that prefixes the joint names
 /// @param input_joint_states The JointStates message received from the robot
 /// @param output_joint_states A JointStates message that will be ordered properly
-/// @param spot_name Namespace that the ros2 control stack was launched in that prefixes the joint names
 /// @return boolean indicating if the joint angles got ordered successfully.
-bool order_joint_states(const sensor_msgs::msg::JointState& input_joint_states,
-                        sensor_msgs::msg::JointState& output_joint_states, const std::string& spot_name) {
+bool order_joint_states(const std::string& spot_name, const sensor_msgs::msg::JointState& input_joint_states,
+                        sensor_msgs::msg::JointState& output_joint_states) {
   const auto njoints = input_joint_states.position.size();
   bool has_arm;
   if (njoints == kNjointsArm) {