[Servo] Refactoring servo

[ibrahiminfinite]

Description

This is an effort supported by GSoC 2023 to refactor the MoveIt Servo package to improve its usability and performance.
The primary focus is on implementing clearly seperated and performant C++ and ROS APIs to interact with Servo.
An effort has also been made to make the logic more readable.

The moveit_msgs PR ros-planning/moveit_msgs#161 must be merged before this can go in.

Please feel free to test it out and provide your feedback.

JontJog: Video, Code
Twist: Video, Code
Pose: Video, Code

---

Using the ROS APIs

To run the demo:

NOTE : The moveit_msgs package should be updated to https://github.com/ibrahiminfinite/moveit_msgs/tree/new_servo

ros2 launch moveit_servo demo_ros_api.launch.py

Once the demo is running, the jointjog and twist inputs can be tried out using demos/servo_keyboard_input.cpp by running.

ros2 run moveit_servo servo_keyboard_input

The servo loop runs automatically on Servo instance creation.
Servo can be paused/ unpaused using the bool type service /servo_node/pause_servo.
There is no notion of start/stop in the current implementation.

Before sending any commands, you should set the command type for servo using the /servo_node/switch_command_type service.

0 - JointJog
1 - Twist
2 - Pose

So if you want to send pose command

ros2 service call /servo_node/switch_command_type moveit_msgs/srv/ServoCommandType "{command_type: 2}"

And then you can send the specific type of commands to their respective topics:

/servo_node/delta_joint_cmds
/servo_node/delta_twist_cmds
/servo_node/pose_target_cmds

---

Timings for C++ API calls:

JointJog ~ 100 us
Twist ~ 350 us
Pose ~ 350 us

---

TODO :

• C++ API demos
• Tests for C++ APIs
• ROS input type switching service
• ROS API pause service
• ROS API demo
• Tests for ROS APIs

---

• Keyboard teleop demo

---

[codecov]

Codecov Report

Patch coverage: 84.47% and project coverage change: +0.18% 🎉

Comparison is base (548f5fe) 50.50% compared to head (b40a11d) 50.68%.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2224      +/-   ##
==========================================
+ Coverage   50.50%   50.68%   +0.18%     
==========================================
  Files         386      386              
  Lines       31736    31912     +176     
==========================================
+ Hits        16026    16172     +146     
- Misses      15710    15740      +30     

Files Changed	Coverage Δ
moveit_ros/moveit_servo/src/servo.cpp	66.26% <66.25%> (+9.12%)	⬆️
moveit_ros/moveit_servo/src/utils/command.cpp	76.00% <76.00%> (ø)
moveit_ros/moveit_servo/src/servo_node.cpp	77.39% <77.71%> (+2.39%)	⬆️
...oveit_ros/moveit_servo/tests/servo_ros_fixture.hpp	89.84% <89.84%> (ø)
moveit_ros/moveit_servo/src/collision_monitor.cpp	90.75% <90.75%> (ø)
moveit_ros/moveit_servo/src/utils/common.cpp	91.73% <91.73%> (ø)
...eit_servo/include/moveit_servo/utils/datatypes.hpp	100.00% <100.00%> (ø)
...oveit_ros/moveit_servo/tests/servo_cpp_fixture.hpp	100.00% <100.00%> (ø)
moveit_ros/moveit_servo/tests/test_integration.cpp	100.00% <100.00%> (ø)
...it_ros/moveit_servo/tests/test_ros_integration.cpp	100.00% <100.00%> (ø)
... and 1 more

... and 12 files with indirect coverage changes

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

[sea-bass]

Took a first pass through!

[sea-bass]

Tests looking nice! Took another look and have a few suggestions.

I guess one of my biggest questions on the tests is... given that we took care to make a standalone C++ API, is there no way to write unit tests that do not require standing up a ROS node? Like, see here for example for a way to get a robot model: https://github.com/PickNikRobotics/pick_ik/blob/main/tests/robot_tests.cpp#L97C1-L100

Not sure how one would load an IK plugin in the similar way. Maybe something like this? https://github.com/ros-planning/moveit2/blob/main/moveit_kinematics/test/test_kinematics_plugin.cpp#LL187C53-L187C53

[ibrahiminfinite]

is there no way to write unit tests that do not require standing up a ROS node?

The tests for the utils can be done without having to setup a ROS node.
The integration tests for jointjog, twist and pose however still need a node instance. (used for transform_buffer and smoother by Servo), and also for the PlanningSceneMonitor and ParamListener.
Since a node is required anyway, I was planning to use the same fixture for the integration tests as well, less stuff to manage.

[AndyZe]

Looking at the most recent commit (24f907 Fix twist frame conversion), I have some questions.

// We only convert the linear velocities to planning frame, the angular velocities should be applied in the command frame itself.
...
// Transform translation to end effector frame and apply.
...
ee_pose.rotate(target_rotation);

^ I believe this only works if EE frame == command frame == ik_solver_->getTipFrame().

Are we OK with that? Can you think of any unusual use cases where that wouldn't work?

I can think of one. Before, we talked about the case where somebody is using a VR hand controller to move the robot, like this video. If we go with this approach, the user would be necessary for transforming the twist before they publish it to Servo. Otherwise, it won't be correct. I guess that's OK, it just requires extra work from the user.

I'd suggest searching the Servo codebase for "command frame" and "ee frame" --> replace with "ik tip frame"

It will be confusing to have 3 different names for the same frame.

[AndyZe]

Can you add a test where a twist command has the wrong frame and it gets rejected by Servo?

[ibrahiminfinite]

Can you add a test where a twist command has the wrong frame and it gets rejected by Servo?

Have added the rejection test for both twist and pose.

[sea-bass]

Tested again with MoveIt Studio.

Things worked in mock hardware, except that after the stale command timeout occurs, the controller never stops publishing. It now sustains the rate even after you stop commanding the robot for a long time. The idea is the controller should sustain its rate until the incoming_command_timeout is reached, and then stop.

For a robot with perfect sensors, this is fine, but eventually the controller must stop publishing-- take, for example, Gazebo:

2023-08-03.17-25-16.mp4

---

Also, seems one of the unit tests for invalid vector is actually segfaulting?

[sea-bass]

This should really be a parameter (we found issues with that on Kinova robots), but definitely not for this PR!

[ibrahiminfinite]

Also, seems one of the unit tests for invalid vector is actually segfaulting?

Have to check what is happening in the CI, only the "leaving singularity" test fails for me locally, no segfaults.

[sea-bass]

This looks good to me! My last tests with MoveIt Studio before the test/clang updates confirmed that all the functionality we expected to be in this refactor is all set.

I'll approve the moveit_msgs PR and left some very small things here.

[sea-bass]

I am still not sure why we had to change from panda_hand to panda_link8 -- what was this again?

[sea-bass]

Weird that just adding a comment broke clang-tidy again... maybe move the comment above the NOLINTNEXTLINE line?

EDIT: Oh I see because the inline comment made it wrap into 2 lines. Went ahead and fixed it.

[ibrahiminfinite]

Not sure if we need these https://github.com/ibrahiminfinite/moveit2/blob/eda31330572e27faf1e1cb4366cda3e6de843f7c/moveit_ros/moveit_servo/src/servo_node.cpp#L226
It floods the logs if the incoming command rate is low.

[sea-bass]

Not sure if we need these https://github.com/ibrahiminfinite/moveit2/blob/eda31330572e27faf1e1cb4366cda3e6de843f7c/moveit_ros/moveit_servo/src/servo_node.cpp#L226 It floods the logs if the incoming command rate is low.

Sure, feel free to get rid of it -- or put it as a DEBUG?

How about just publishing it while halting, but not after it's stopped?