diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index ccb82121e5..3046bfa910 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -189,6 +189,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa rclcpp::TimerBase::SharedPtr goal_handle_timer_; rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); + // Pre-allocated action feedback message to avoid heap allocation in RT loop + std::shared_ptr preallocated_feedback_; + // callback for topic interface void topic_callback(const std::shared_ptr msg); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 5bc8da69c9..57c1ccbb02 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -367,16 +367,13 @@ controller_interface::return_type JointTrajectoryController::update( if (active_goal) { - // send feedback - auto feedback = std::make_shared(); - feedback->header.stamp = time; - feedback->joint_names = params_.joints; - - feedback->actual = state_current_; - feedback->desired = state_desired_; - feedback->error = state_error_; - feedback->index = static_cast(next_point_index); - active_goal->setFeedback(feedback); + // send feedback using preallocated message to avoid heap allocation in RT loop + preallocated_feedback_->header.stamp = time; + preallocated_feedback_->actual = state_current_; + preallocated_feedback_->desired = state_desired_; + preallocated_feedback_->error = state_error_; + preallocated_feedback_->index = static_cast(next_point_index); + active_goal->setFeedback(preallocated_feedback_); // check abort if (tolerance_violated_while_moving) @@ -1938,6 +1935,10 @@ void JointTrajectoryController::init_hold_position_msg() { hold_position_msg_ptr_->points[0].effort.resize(dof_, 0.0); } + + // Pre-allocate feedback message to avoid heap allocation in RT loop + preallocated_feedback_ = std::make_shared(); + preallocated_feedback_->joint_names = params_.joints; } } // namespace joint_trajectory_controller