Propagate errors from planners.

[Abishalini]

Sometimes, it is hard to follow why a planner failed for stages Connect, MoveTo, and MoveRelative.

Here is an effort to propagate the error messages from the planners. I have incorporated the use of tl::expected so that the plan function can now return a bool success or an error string. The stages which use planners have been modified to add the error string to the solutions.

Example -

Before this PR, the comment would be empty and I couldn't see any errors on console too which would help debug.

[codecov]

Codecov Report

Attention: 31 lines in your changes are missing coverage. Please review.

Comparison is base (57e8490) 42.40% compared to head (5a2607e) 42.47%.

Files	Patch %	Lines
core/src/solvers/multi_planner.cpp	0.00%	8 Missing ⚠️
core/src/stages/move_relative.cpp	46.16%	7 Missing ⚠️
core/src/solvers/joint_interpolation.cpp	44.45%	5 Missing ⚠️
core/src/solvers/cartesian_path.cpp	50.00%	3 Missing ⚠️
core/src/solvers/pipeline_planner.cpp	57.15%	3 Missing ⚠️
core/src/stages/move_to.cpp	75.00%	3 Missing ⚠️
core/src/stages/connect.cpp	71.43%	2 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##             ros2     #525      +/-   ##
==========================================
+ Coverage   42.40%   42.47%   +0.08%     
==========================================
  Files          83       83              
  Lines        8049     8080      +31     
==========================================
+ Hits         3412     3431      +19     
- Misses       4637     4649      +12     

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

[sjahr]

Thank you! This change will help a lot with debug-ability!

While reading the code I was wondering if it make sense to keep the boolean return type if we always return true or unexpected? Maybe I am missing something but what about changing this to either:
return tl::expected<void, std::string> or return MoveItErrorCode?

[sjahr]

You'll need to apply some small cleanups but otherwise this is good to go in my opinion 👍

[DaniGarciaLopez]

Thanks a lot @Abishalini for this feature! Is there any chance it can be backported to humble?

[Abishalini]

@DaniGarciaLopez If you are building from source, the main branch of MoveIt2 and ros2 branch of MTC are compatible with humble. If not, feel free to cherry-pick these changes and make a PR to the humble branch.

[DaniGarciaLopez]

@DaniGarciaLopez If you are building from source, the main branch of MoveIt2 and ros2 branch of MTC are compatible with humble. If not, feel free to cherry-pick these changes and make a PR to the humble branch.

I actually compiled MoveIt and MTC from source, but encountered numerous errors related to MoveItErrorCodes because I didn't realize I was using moveit_msgs from humble binaries. After compiling moveit_msgs from the ros2 branch, MoveIt from the main, and MTC from ros2, it is now working fine. Thanks!