Skip to content

Commit

Permalink
nav2_system_tests: ensure error_msg not empty (ros-navigation#4341)
Browse files Browse the repository at this point in the history
Signed-off-by: Mike Wake <michael.wake@aosgrp.com.au>
  • Loading branch information
aosmw committed Oct 4, 2024
1 parent cb1b3a2 commit 1c5f38d
Show file tree
Hide file tree
Showing 3 changed files with 14 additions and 0 deletions.
10 changes: 10 additions & 0 deletions nav2_system_tests/src/error_codes/test_error_codes.py
Original file line number Diff line number Diff line change
Expand Up @@ -79,6 +79,9 @@ def main(argv=sys.argv[1:]):
assert (
navigator.result_future.result().result.error_code == error_code
), 'Follow path error code does not match'
assert (
navigator.result_future.result().result.error_msg != ""
), 'Follow path error_msg is empty'

else:
assert False, 'Follow path was rejected'
Expand Down Expand Up @@ -111,6 +114,9 @@ def main(argv=sys.argv[1:]):
assert (
result.error_code == error_code
), 'Compute path to pose error does not match'
assert (
result.error_msg != ""
), 'Compute path to pose error_msg empty'

def cancel_task():
time.sleep(1)
Expand Down Expand Up @@ -145,6 +151,9 @@ def cancel_task():
assert (
result.error_code == error_code
), 'Compute path through pose error does not match'
assert (
result.error_msg != ""
), 'Compute path through pose error_msg is empty'
# Check compute path to pose cancel
threading.Thread(target=cancel_task).start()
result = navigator._getPathThroughPosesImpl(initial_pose, goal_poses, 'cancelled')
Expand Down Expand Up @@ -182,6 +191,7 @@ def cancel_task():
for smoother, error_code in smoother.items():
result = navigator._smoothPathImpl(a_path, smoother)
assert result.error_code == error_code, 'Smoother error does not match'
assert result.error_msg != "", 'Smoother error_msg is empty'

navigator.lifecycleShutdown()
rclpy.shutdown()
Expand Down
3 changes: 3 additions & 0 deletions nav2_system_tests/src/gps_navigation/tester.py
Original file line number Diff line number Diff line change
Expand Up @@ -190,6 +190,9 @@ def main(argv=sys.argv[1:]):
test.action_result.missed_waypoints[0].error_code
== ComputePathToPose.Result().GOAL_OUTSIDE_MAP
)
assert (
test.action_result.missed_waypoints[0].error_msg != ""
)

# stop on failure test with bogous waypoint
test.setStopFailureParam(True)
Expand Down
1 change: 1 addition & 0 deletions nav2_system_tests/src/waypoint_follower/tester.py
Original file line number Diff line number Diff line change
Expand Up @@ -241,6 +241,7 @@ def main(argv=sys.argv[1:]):
test.action_result.missed_waypoints[0].error_code
== ComputePathToPose.Result().GOAL_OUTSIDE_MAP
)
assert (test.action_result.missed_waypoints[0].error_msg != "")

# stop on failure test with bogous waypoint
test.setStopFailureParam(True)
Expand Down

0 comments on commit 1c5f38d

Please sign in to comment.