From e8b4eb7f1a11ad490d0785de4b8eab01af2d258c Mon Sep 17 00:00:00 2001 From: Andy Zelenak Date: Wed, 27 Jul 2022 08:46:23 -0500 Subject: [PATCH] Small improvement in remapping --- .../src/joint_trajectory_controller.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 235681b069..130cd2cf48 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1136,8 +1136,12 @@ void JointTrajectoryController::sort_to_local_joint_order( get_node()->get_logger(), "Invalid input size (%zu) for sorting", to_remap.size()); return to_remap; } - std::vector output; - output.resize(mapping.size(), 0.0); + static std::vector output(dof_, 0.0); + // Only resize if necessary since it's an expensive operation + if (output.size() != mapping.size()) + { + output.resize(mapping.size(), 0.0); + } for (size_t index = 0; index < mapping.size(); ++index) { auto map_index = mapping[index];