Simplify code and remove debug output

Author: christophfroehlich

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -1835,7 +1835,6 @@ void JointTrajectoryController::add_new_trajectory_msg_nonRT(
       RCLCPP_ERROR(get_node()->get_logger(), "Failed to compute gains for trajectory.");
     }
   }
-  RCLCPP_INFO(get_node()->get_logger(), "Called add_new_trajectory_msg_nonRT.");
 }
 
 void JointTrajectoryController::add_new_trajectory_msg_RT(
@@ -1855,7 +1854,6 @@ void JointTrajectoryController::add_new_trajectory_msg_RT(
       RCLCPP_ERROR(get_node()->get_logger(), "Failed to compute gains for trajectory.");
     }
   }
-  RCLCPP_INFO(get_node()->get_logger(), "Called add_new_trajectory_msg_RT.");
 }
 
 void JointTrajectoryController::preempt_active_goal()

joint_trajectory_controller_plugins/src/pid_trajectory_plugin.cpp

@@ -133,11 +133,12 @@ void PidTrajectoryPlugin::compute_commands(
   // Update PIDs
   for (auto i = 0ul; i < num_cmd_joints_; ++i)
   {
-    tmp_command[map_cmd_to_joints_[i]] =
-      (desired.velocities[map_cmd_to_joints_[i]] * ff_velocity_scale_[i]) +
+    const auto map_cmd_to_joint = map_cmd_to_joints_[i];
+    tmp_command[map_cmd_to_joint] =
+      (desired.velocities[map_cmd_to_joint] * ff_velocity_scale_[i]) +
       (has_effort_command_interface ? desired.effort[i] : 0.0) +
       pids_[i]->compute_command(
-        error.positions[map_cmd_to_joints_[i]], error.velocities[map_cmd_to_joints_[i]], period);
+        error.positions[map_cmd_to_joint], error.velocities[map_cmd_to_joint], period);
   }
 }