[jog_arm] Changes before porting to ROS2

[AdamPettinger]

Description

These are changes I wanted to make (in addition to moveit#2103) before starting on porting the jogger to ROS2 because I want these changes there as well.

The changes consist of a lot of minor changes, see list below for details. @tylerjw

• General cleanup: defining const in functions where applicable, fixing comments, combining if statements and making them flow better
• I found a stale but nonzero command could make it through the timer callback function without updating the joint position filters, so fixed that and made sure the filters are updated every cycle
• Removed the suddenHalt(Eigen::ArrayXd& delta_theta) overload. It included comments saying position and velocity controlled robots were handled differently, but this function did not handle them differently (and only set delta_theta to all 0 which could be really bad for a position controlled robot). It was only called one place with the Eigen Array, and I replaced the call with a setZero() there and deleted the function so future users don't use it and accidentally send the robot to 0's.
• Small fixes in the drift dimensions use and in singularity scaling. Previously the singularity scaling wasn't doing what the algorithm in the comments said it was. Tested after fixing and it looks really good
• Small changes around the have_nonzero_twist_stamped_ and have_nonzero_joint_jog_ which were used intermittently in the run() timer callback but also set in the command subscriber callbacks. Moved them to mutex protected implementation similar to the rest of subscriber callback variables
• Avoid recalculating the planning -> command transform if the incoming command is empty or in the command frame
• Reworked the enforceSRDFAccelVelLimits() function which was clipping the change in joint angles joint by joint so they fit within the acceleration and velocity limits - but going joint by joint may result in a nonlinear change in the joint angles (inside the enforcing limits function). Instead I go joint by joint and track how much each joint needs to be scaled down to be within limits, then apply the most significant scaling to the entire array. This is a linear operation, and ensures the robot motion after enforcing the limits is a linearly-scaled version of the user request (also scaled by singularity and collision)

Checklist

• Required by CI: Code is auto formatted using clang-format
• Extend the tutorials / documentation reference
• Document API changes relevant to the user in the MIGRATION.md notes
• Create tests, which fail without this PR reference
• Include a screenshot if changing a GUI
• While waiting for someone to review your request, please help review another open pull request to support the maintainers

[welcome]

Thanks for helping in improving MoveIt and open source robotics!

[henningkayser]

Overall very reasonable changes

[codecov-commenter]

Codecov Report

Merging #8 into p/tylerjw/jog_arm_threading will increase coverage by 0.35%.
The diff coverage is 78.37%.

@@                       Coverage Diff                       @@
##           p/tylerjw/jog_arm_threading       #8      +/-   ##
===============================================================
+ Coverage                        57.56%   57.92%   +0.35%     
===============================================================
  Files                              327      327              
  Lines                            25641    25645       +4     
===============================================================
+ Hits                             14761    14854      +93     
+ Misses                           10880    10791      -89     

Impacted Files	Coverage Δ
...og_arm/include/moveit_jog_arm/jog_arm_parameters.h	100.00% <ø> (ø)
.../moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h	100.00% <ø> (ø)
...xperimental/moveit_jog_arm/src/collision_check.cpp	80.51% <ø> (ø)
...veit_experimental/moveit_jog_arm/src/jog_calcs.cpp	74.12% <78.37%> (+3.02%)	⬆️
.../ompl_interface/src/detail/constrained_sampler.cpp	46.15% <0.00%> (-17.95%)	⬇️
...anning_scene_monitor/src/current_state_monitor.cpp	51.28% <0.00%> (-1.54%)	⬇️
...nning_scene_monitor/src/planning_scene_monitor.cpp	70.10% <0.00%> (-0.60%)	⬇️
moveit_core/robot_state/src/robot_state.cpp	47.30% <0.00%> (-0.59%)	⬇️
...raint_samplers/src/default_constraint_samplers.cpp	82.54% <0.00%> (-0.37%)	⬇️
...meterization/work_space/pose_model_state_space.cpp	82.99% <0.00%> (+0.68%)	⬆️
... and 7 more

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 26cd273...b220d69. Read the comment docs.

[AndyZe]

Looks good! I'm going to approve it, assuming you'll make the one change about ROS_LOG_THROTTLE_PERIOD

[AdamPettinger]

Re-targeted this to the main moveit repo, see moveit#2151