Convert OMPL status to MoveItErrorCode in the OMPL interface

[AndyZe]

Description

Get better detail from planning errors by converting OMPL codes to MoveItErrorCode and returning them.

OMPL code documentation: https://github.com/ompl/ompl/blob/main/src/ompl/base/PlannerStatus.h
MoveItErrorCode documentation: https://github.com/ros-planning/moveit_msgs/blob/master/msg/MoveItErrorCodes.msg

[codecov]

Codecov Report

Base: 51.13% // Head: 51.09% // Decreases project coverage by -0.04% ⚠️

Coverage data is based on head (f3e66d7) compared to base (9c6aa94).
Patch coverage: 35.90% of modified lines in pull request are covered.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #1606      +/-   ##
==========================================
- Coverage   51.13%   51.09%   -0.03%     
==========================================
  Files         381      381              
  Lines       31711    31738      +27     
==========================================
+ Hits        16211    16213       +2     
- Misses      15500    15525      +25     

Impacted Files	Coverage Δ
...veit/ompl_interface/model_based_planning_context.h	84.45% <ø> (ø)
...mpl_interface/src/model_based_planning_context.cpp	50.57% <35.90%> (-0.92%)	⬇️
moveit_ros/moveit_servo/src/servo_calcs.cpp	66.45% <0.00%> (-1.49%)	⬇️
moveit_ros/moveit_servo/src/pose_tracking.cpp	76.78% <0.00%> (-0.47%)	⬇️
...e/collision_detection_fcl/src/collision_common.cpp	73.76% <0.00%> (-0.22%)	⬇️
...nning_scene_monitor/src/planning_scene_monitor.cpp	45.73% <0.00%> (+0.44%)	⬆️

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

☔ View full report at Codecov.
📢 Do you have feedback about the report comment? Let us know in this issue.

[sjahr]

This is great, thanks for adding it! After reviewing it, I felt like we should maybe extend the existing MoveItError codes, so we loose even fewer information during the translation OMPLStatus -> MoveItError code. But this can be done in a follow up PR

[AndyZe]

Deleted this bit of code because the "approximate" warning will be printed later

[tylerjw]

Why does this use an output parameter and why does it log?

[AndyZe]

I'll get rid of the output parameter.

As far as logging goes, this seems like a pretty good place to do it. We are already handling all possible cases with switch, so... that code would have to be duplicated if the logging moved elsewhere. Maybe the function name just needs to be changed to indicate it logs? Like, logPlannerStatus()

[tylerjw]

Maybe do something like this and then the call site can decide what they want:

// convert ompl status into moveit error code 
moveit_msgs::msg::MoveItErrorCodes toMoveItError(ompl::base::PlannerStatus status);

// test if ompl status or moveit error code is success 
bool isSuccess(ompl::base::PlannerStatus status);
bool isSuccess(moveit_msgs::msg::MoveItErrorCodes error_code);

// convert ompl status or moveit code into string, used in logging
std::string toString(ompl::base::PlannerStatus status);
std::string toString(moveit_msgs::msg::MoveItErrorCodes error_code);

// log warning if ompl status or moveit code is error
void logWarningIfError(ompl::base::PlannerStatus status);
void logWarningIfError(moveit_msgs::msg::MoveItErrorCodes error_code);

At the call site where you need to create a moveit error and log the ompl status you can do this:

auto ompl_status = ompl_simple_setup_->getLastPlannerStatus();
logWarningIfError(ompl_status);
result.val = toMoveItError(ompl_status);

In model_based_planning_context.cpp it can make the code simpler if you use a using statement for success and failure. This is fine to do because it is not in a header and won't propagate into other files.

using SUCCESS = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
using FAILURE = moveit_msgs::msg::MoveItErrorCodes::FAILURE;

Then, combined with the isSuccess functions you can make those lines where you need to test if both the moveit error code and the ompl status are good results and return a success/failure moveit code can become a bit less verbose:

result.val = (isSuccess(result.val) && isSuccess(ompl_status)) ? SUCCESS : FAILURE;

These are just ideas for how you can make some smaller functions you can combine to make the error types easier to use in various places. The important idea here is just separating the function of converting the types from the function that logs.

Another small detail of the above code is you can make the function names simpler because the type of the parameter you pass in determines which function is called.

[tylerjw]

Thank you for getting rid of the output parameter and using a return type. Above are just some suggestions on how we could maybe make this code a bit more flexible and easy to understand at the call site of the function.

[tylerjw]

@AndyZe I'm also happy to accept this change as is and try to make the change I described above myself. Would you rather that?

[AndyZe]

Give me a couple days. I want to try what you've suggested but prob won't get to it today