fixed increment

lbr-stack · Nov 20, 2024 · 8639214 · 8639214
1 parent 7afb59f
commit 8639214
Showing 1 changed file with 7 additions and 4 deletions.
diff --git a/lbr_ros2_control/src/controllers/lbr_joint_position_command_controller.cpp b/lbr_ros2_control/src/controllers/lbr_joint_position_command_controller.cpp
@@ -49,10 +49,13 @@ LBRJointPositionCommandController::update(const rclcpp::Time & /*time*/,
   if (!lbr_joint_position_command || !(*lbr_joint_position_command)) {
     return controller_interface::return_type::OK;
   }
-  std::for_each(command_interfaces_.begin(), command_interfaces_.end(),
-                [lbr_joint_position_command, idx = 0](auto &command_interface) mutable {
-                  command_interface.set_value((*lbr_joint_position_command)->joint_position[++idx]);
-                });
+  std::for_each(
+      command_interfaces_.begin(), command_interfaces_.end(),
+      [lbr_joint_position_command, idx = 0](auto &command_interface) mutable {
+        command_interface.set_value(
+            (*lbr_joint_position_command)
+                ->joint_position[idx++]); // important to post-increment idx, don't use ++idx
+      });
   return controller_interface::return_type::OK;
 }