Added Timeout to searchPositionIK(...)

[Jmeyer1292]

The interface for searchPositionIK includes a timeout argument. A timeout variable is even created inside the function (maxTime) but is never used. I added a check to see if the time was exceeded after finishing a round of discretizations.

This both satisfies an interface requirement that is not addressed at the moment (time out) and will ensure that the plugin doesn't freeze even if it has to do a bunch of collision checking. As mentioned in #43, the cheapest solution is usually found upfront anyway and this PR shouldn't affect that.

Argh:

I meant to make this against indigo-devel, but I believe this still applies.

[Jmeyer1292]

ping

[jrgnicho]

+1 for merge from me

[Jmeyer1292]

ping

[gavanderhoorn]

Change makes sense, I'm just wondering if MoveItErrorCodes::NO_IK_SOLUTION is the right code to return.

MoveItErrorCodes::TIMED_OUT would seem like a good candidate, but I'm not sure it is in the set of 'expected' error codes for moveit_ikfast plugins.

[Jmeyer1292]

Well I adjusted it, but I realize now that with the way the function is written, the error_code flag is unconditionally overwritten to be either SUCCESS or NO_IK_SOLUTION so maybe just getting rid of the flag write altogether inside the main loop is the right answer.

[jrgnicho]

It's probably best to get rid of the flag write since it gets overwritten anyway

[gavanderhoorn]

I'd suggest to leave it in (to record the intention of the check) but to document the fact that due to the current setup of the code, it'll overwrite the return value. Ideally we'd then also create an issue to keep track of the fact that it is something to correct in the future.

[gavanderhoorn]

I'm a bit confused: after the break statement, the infinite while will be exited, and if ((search_mode & OPTIMIZE_MAX_JOINT) && best_costs != -1.0) the plugin will actually return a solution. In the case of a timeout, I'd say that is unwanted behaviour (unless of course the plugin should optimize until a certain amount of time has passed).

[jrgnicho]

Based on the lines preceding the the time threshold check starting at 842

 if (search_mode & OPTIMIZE_MAX_JOINT)
{
              // Costs for solution: Largest joint motion
              double costs = 0.0;

It looks to me that the intent is to continue searching/optimizing until time runs out since there doesn't seem to be a way of exiting out of search mode from within the scope of the while loop other than the time and getCount checks.

[jrgnicho]

@gavanderhoorn I'm not entirely sure about what was in dispute here but it appears that the the condition you mentioned if ((search_mode & OPTIMIZE_MAX_JOINT) && best_costs != -1.0) could get triggered when it shouldn't. One way of fixing this would be to return the corresponding error code inside the if statement that is entered as a result of a timeout.

[jrgnicho]

In order to avoid overwriting the error flag could we just have a return statement here ?

[davetcoleman]

I do not believe a return statement is a good idea because currently this while loop has two modes - find the first solution or find the best solution, see OPTIMIZE_MAX_JOINT

[jrgnicho]

I see, then perhaps we should just move the last error assignment line near the end of the method to a location right before entering the big while loop. This would prevent overriding the error flag.

[jrgnicho]

@davetcoleman ping

[davetcoleman]

yes, I think we should have a bool flag indicating that it timed out, and if no solution was found at the end of the function, return either NO_SOLUTION or, if a timeout occured, return TIMED_OUT

[jrgnicho]

@Jmeyer1292 could you make this change?

[Jmeyer1292]

So I removed the final over-write all together and I think that covers our bases. Check my logic:

The search loop can exit three ways:

1. A valid solution was found AND search_mode == OPTIMIZE_MAX_JOINT
2. getCount fails indicating we've exhausted our search
3. A timeout is exceeded

If search_mode != OPTIMIZE_MAX_JOINT

1. solution is set, error code == SUCCESS, return true
2. error code is set to NO_IK, return false, solution undefined
3. error code is set to TIMED_OUT, return false, solution undefined

If search mode == OPTIMIZE_MAX_JOINT

1. N/A (best solution is set, cost != -1.0)
2. search exhausted;
   • if a solution was found, error_code set to SUCCESS. return true
   • if not, error_code remains NO_IK, return false
3. timeout:
   • if solution was found, error_code set to SUCCESS, return true
   • if solution was not found, error_code remains TIMED_OUT, return false

[davetcoleman]

I'm not sure I follow your above comment logic. I've made modifications/clarifications in bold:

The search loop can exit three ways:

1. A valid solution was found AND search_mode != (NOT EQUAL) OPTIMIZE_MAX_JOINT
2. getCount fails indicating we've exhausted our search
3. A timeout is exceeded

These three ways of exiting behave differently depending on search_mode:

If search_mode != OPTIMIZE_MAX_JOINT

1. when first solution is found, error code == SUCCESS, return true
2. error code is set to NO_IK, return false, solution undefined
3. error code is set to TIMED_OUT, return false, solution undefined

If search mode == OPTIMIZE_MAX_JOINT

1. N/A (best solution is set, cost != -1.0) I don't understand your parenthesis comment here, should just be N/A
2. search exhausted;
   • if a solution was found, error_code set to SUCCESS. return true
   • if not, error_code remains NO_IK, return false
3. timeout:
   • if solution was found, error_code set to SUCCESS, return true
   • if solution was not found, error_code remains TIMED_OUT, return false

I think we should add this logic explanation into the code comments

[davetcoleman]

Are these issues being moved to the merged moveit repo?

[Jmeyer1292]

I'm trying to pair down on the number of open issues and pull requests that I have. I do not have plans to open this again against the main moveit repo.