[Servo] Fix bugs when halting for collision + transforming commands to planning frame

[sea-bass]

This PR fixes a few bugs:

1. Robot was being commanded to zero state when halting for collision, now we simply don't publish outputs in this case.
2. When applying twists about the EE frame, the translation component of the transform was also being used, plus the TF lookupTransform() call had arguments reversed, causing drift. Fixes the issue described in [Servo] Using TF lookup instead of robot state for transforms causing drift #2352, and applies the same fixes to pose tracking conversions as well.
3. Changed the stopped velocity threshold for the smoothHalt() function to use a nonzero threshold instead of exact floating-point comparison to zero.
4. Make some functions accept input arguments as const to speed things up
5. Removed the redundant tf2::TransformBuffer and tf2::TransformListener that were already implicit in the Planning Scene Monitor.

Additionally, it simplifies some of the new twist conversion code from #2311 so there is no need to reorder a vector.

Closes #2352

[sea-bass]

Moved some of the unrelated fixes from #2349 (which I'm not sure will actually help) to here. These changes should definitely go in.

cc @ibrahiminfinite

[codecov]

Codecov Report

Patch coverage: 84.00% and project coverage change: -0.03% ⚠️

Comparison is base (16ac53c) 50.76% compared to head (931f54b) 50.72%.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2350      +/-   ##
==========================================
- Coverage   50.76%   50.72%   -0.03%     
==========================================
  Files         386      386              
  Lines       31963    31970       +7     
==========================================
- Hits        16223    16214       -9     
- Misses      15740    15756      +16     

Files Changed	Coverage Δ
moveit_ros/moveit_servo/src/servo.cpp	63.57% <82.23%> (-0.30%)	⬇️
moveit_ros/moveit_servo/src/servo_node.cpp	77.52% <100.00%> (-3.43%)	⬇️
moveit_ros/moveit_servo/src/utils/command.cpp	76.00% <100.00%> (ø)

... and 3 files with indirect coverage changes

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[ibrahiminfinite]

Great catch!
I had set it up so that getNextJointState() would return the current state in case of halting for collision, but it seems I forgot to copy the current positions to target state.
Since this should work for both C++ and ROS APIs, I think it might be better to add

robot_state->copyJointGroupPositions(joint_model_group, target_state.positions);

here, no need to check for the collision flag in servoLoop()

[sea-bass]

@ibrahiminfinite Implemented your comments, and also found a follow-on bug with converting twists to planning frame, so I changed this PR name/description.

[ibrahiminfinite]

Tested this, all the demos worked fine and applying twist commands to panda also worked as expected.