From 8639214e19dddf90d7934bf580c56becd416c52a Mon Sep 17 00:00:00 2001 From: mhubii Date: Wed, 20 Nov 2024 19:49:09 +0000 Subject: [PATCH] fixed increment --- .../lbr_joint_position_command_controller.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 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 index 5556159f..d78d825b 100644 --- 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; }