Can't run two goals in a row

[Marc-Morcos]

If I run the demo,
replacing the main function of demo_coverage.py with

def main():
    rclpy.init()

    navigator = CoverageNavigatorTester()
    navigator.startup()

    # Some example field
    field = [[5.0, 5.0], [5.0, 8.0], [8.0, 8.0], [5.0, 5.0]]
    navigator.navigateCoverage(field)

    i = 0
    while not navigator.isTaskComplete():
        # Do something with the feedback
        i = i + 1
        feedback = navigator.getFeedback()
        if feedback and i % 5 == 0:
            print('Estimated time of arrival: ' + '{0:.0f}'.format(
                  Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9)
                  + ' seconds.')
        time.sleep(1)

    # Do something depending on the return code
    result = navigator.getResult()
    if result == TaskResult.SUCCEEDED:
        print('Goal succeeded!')
    elif result == TaskResult.CANCELED:
        print('Goal was canceled!')
    elif result == TaskResult.FAILED:
        print('Goal failed!')
    else:
        print('Goal has an invalid return status!')

    field = [[6.0, 5.0], [5.0, 8.0], [8.0, 8.0], [6.0, 5.0]]

    navigator.navigateCoverage(field)

    i = 0
    while not navigator.isTaskComplete():
        # Do something with the feedback
        i = i + 1
        feedback = navigator.getFeedback()
        if feedback and i % 5 == 0:
            print('Estimated time of arrival: ' + '{0:.0f}'.format(
                  Duration.from_msg(feedback.estimated_time_remaining).nanoseconds / 1e9)
                  + ' seconds.')
        time.sleep(1)

    # Do something depending on the return code
    result = navigator.getResult()
    if result == TaskResult.SUCCEEDED:
        print('Goal succeeded!')
    elif result == TaskResult.CANCELED:
        print('Goal was canceled!')
    elif result == TaskResult.FAILED:
        print('Goal failed!')
    else:
        print('Goal has an invalid return status!')

I get the following error
[coverage_server]: Invalid GML File or Coordinates: Outer polygon malformed, first element must equal last!

[SteveMacenski]

I don't see any reason for that to be happening. Adding some prints in to see what's being sent might be helpful. I believe I see that the goal_msg and field is reallocated each call, so it should work fine from the client perspective - nothing should be stored between calls to navigateCoverage. This all pretty much follows Nav2's simple commander API boilerplate that we have tests for multiple requests with, but perhaps there's a subtle mistake here that I'm not seeing.

[Marc-Morcos]

Just to add to this. It works fine if I send the exact same goal twice. But if I change the first and last point, it breaks.

[SteveMacenski]

Print out the goal_msg and see what it says. If its right, then the issue is on the server side. If its wrong, then its something in the python.

[Marc-Morcos]

First goal: Polygon points: [geometry_msgs.msg.Point32(x=5.0, y=5.0, z=0.0), geometry_msgs.msg.Point32(x=5.0, y=8.0, z=0.0), geometry_msgs.msg.Point32(x=8.0, y=8.0, z=0.0), geometry_msgs.msg.Point32(x=5.0, y=5.0, z=0.0)]

Second goal: Polygon points: [geometry_msgs.msg.Point32(x=6.0, y=5.0, z=0.0), geometry_msgs.msg.Point32(x=5.0, y=8.0, z=0.0), geometry_msgs.msg.Point32(x=8.0, y=8.0, z=0.0), geometry_msgs.msg.Point32(x=6.0, y=5.0, z=0.0)]

[Marc-Morcos]

I also tried setting RateController hz="1" instead of RateController hz="0.0000001" in the behavior tree but that didnt fix it

[Marc-Morcos]

Actually, this pull request seems to reference this issue #43 .

fix polygons not being cleared in ComputeCoveragePathAction

[SteveMacenski]

What does your logs say on https://github.com/open-navigation/opennav_coverage/blob/main/opennav_coverage_navigator/src/coverage_navigator.cpp#L210-L218?

Edit: Ah OK - want to submit that as a PR for just those changes needed?

[Marc-Morcos]

Alright, I''ve made the pull request

[Marc-Morcos]

I would also like to backport to iron, how would I go about that?

[SteveMacenski]

Cherry pick and open a PR against iron/humble :-)

[Marc-Morcos]

Alright, I made a PR to backport a few of my other PR's

[SteveMacenski]

@Marc-Morcos can you do humble too while you're at it?

[Marc-Morcos]

I could, but I wont have a valid environment to test it, so I probably shouldn't right?

[SteveMacenski]

No problem - I'll put it on my queue

[shawnhanna]

@SteveMacenski - Is there any way we can get this backported to the humble branch? I am seeing issues when I run multiple coverages in a row and this fixed the issue. Confirmed on my robot testing IRL

Happy to help with testing or doing the PR if you'd prefer

[SteveMacenski]

@shawnhanna feel free to open the PR, I should be able to merge it in < 1 hr

[shawnhanna]

Added. Thanks @SteveMacenski

#84