Fix JTC segfault

[mechwiz]

@swiz23 noticed a segfault in the JTC specifically when aborting a motion, followed by deactivating, followed by activating where the segfault would occur. After looking at this PR, I think it very much has to do with the line
output_state = trajectory_msgs::msg::JointTrajectoryPoint().
I think the flow of events occur as follows:

When aborting, set_hold_position() is called. That essentially adds and writes an empty trajectory message in order to disregard the current one.
When sample() is called in the update() loop, state_desired_ gets overwritten with that empty trajectory point meaning it has an empty position, velocity, and acceleration array.
The controllers get deactivated
The controllers get reactivated. When this happens, read_state_frome_hardware is called on state_desired_. Within that function, we try to index the position array within state_deisred_ as if it has the same size as dof_ when it in fact no longer does. This causes the segault.
The solution is to simply move the line defining the output_state within the sample() function in trajectory.cpp to after the last possible place that the function can return false -> here. I tested this out at least for the case that @swiz23 had issues with and verified that there was no longer a segfault.

This probably fixes #423
@JafarAbdi

[swiz23]

Just double checked this and also verified that it fixes my issue.

[JafarAbdi]

lgtm

[bmagyar]

@Mergifyio backport to humble

[mergify]

backport to humble

✅ Backports have been created

• Backport to branch to failed: Branch not found
• #524 Fix JTC segfault (backport #518) has been created for branch humble