Return time which was taken to compute the motion plan via ComputePat…

[abrzozowski]

Basic Info

Info
Ticket(s) this addresses	#200
Primary OS tested on	Ubuntu 20.04

Next step to solving the upper mentioned issue (previous step #1801)

---

Added new field to the result of the action message

---

I'm not sure, how can I properly test this feature? Does ComputePathToPoseAction class needed some changes?

[SteveMacenski]

Love it, can we add these for the recoveries too?

[SteveMacenski]

You can test by running the navigation2 stack and in a terminal, call the compute path to pose action it a position and check the result once its done that its there and filled in

[abrzozowski]

Love it, can we add these for the recoveries too?

:) Ok, I've just added support for the recoveries.

You can test by running the navigation2 stack and in a terminal, call the compute path to pose action it a position and check the result once its done that its there and filled in

I've done this by call ros2 action send_goal /compute_path_to_pose nav2_msgs/action/ComputePathToPose "pose: {header: {frame_id: map}, pose: {position: {x: 2.0, y: 0.0, z: 0.0}, orientation:{x: 0.0, y: 0.0, z: 0, w: 1.0000000}}}", and it's working as it should. But we need to keep in mind that it's not real time taken on path planning, its depends on relative flow time in a simulation, so very often it returns 0.0 on my simulation.

[SteveMacenski]

It really shouldn't return 0.0 because it must at least take 1 millisecond to compute, maybe you have a significant figure issue?

[SteveMacenski]

I'd like this in before Foxy, so lets try to get this over the line ASAP since we're totally unblocked to release otherwise

I think we just need verification that it works and whatever Circle is complaining about. Maybe a unit test would be good here; call planner to plan as far as possible, get result, check result is > 0.0 & path is non-empty (or add to planner tests). The time duration could be added to the recoveries tests that already exist, just need to add a > 0.0 check.

[abrzozowski]

It really shouldn't return 0.0 because it must at least take 1 millisecond to compute, maybe you have a significant figure issue?

I've made some tests. I inserted below snippet at the top of the PlannerServer::computePlan()

  {
    std::cout << "node clock" << std::endl;
    auto a = now();
    std::cout << "A s: " << a.seconds() << " ns: " << a.nanoseconds() << std::endl;
    auto b = now();
    std::cout << "B s: " << b.seconds() << " ns: " << b.nanoseconds() << std::endl;
    std::this_thread::sleep_for(std::chrono::milliseconds (1));
    std::cout << "sleep on 1 ms" << std::endl;
    auto c = now();
    std::cout << "C s: " << c.seconds() << " ns: " << c.nanoseconds() << std::endl;
    std::this_thread::sleep_for(std::chrono::milliseconds (10));
    std::cout << "sleep on 10 ms" << std::endl;
    auto d = now();
    std::cout << "D s: " << d.seconds() << " ns: " << d.nanoseconds() << std::endl;
    std::this_thread::sleep_for(std::chrono::milliseconds (100));
    std::cout << "sleep on 100 ms" << std::endl;
    auto e = now();
    std::cout << "E s: " << e.seconds() << " ns: " << e.nanoseconds() << std::endl;
    std::this_thread::sleep_for(std::chrono::milliseconds (1000));
    std::cout << "sleep on 1000 ms" << std::endl;
    auto f = now();
    std::cout << "F s: " << f.seconds() << " ns: " << f.nanoseconds() << std::endl;
  }

  {
    std::cout << "rclcpp::Clock" << std::endl;
    auto a = rclcpp::Clock().now();
    std::cout << "A s: " << a.seconds() << " ns: " << a.nanoseconds() << std::endl;
    auto b = rclcpp::Clock().now();
    std::cout << "B s: " << b.seconds() << " ns: " << b.nanoseconds() << std::endl;
    std::this_thread::sleep_for(std::chrono::milliseconds (1));
    std::cout << "sleep on 1 ms" << std::endl;
    auto c = rclcpp::Clock().now();
    std::cout << "C s: " << c.seconds() << " ns: " << c.nanoseconds() << std::endl;
    std::this_thread::sleep_for(std::chrono::milliseconds (10));
    std::cout << "sleep on 10 ms" << std::endl;
    auto d = rclcpp::Clock().now();
    std::cout << "D s: " << d.seconds() << " ns: " << d.nanoseconds() << std::endl;
    std::this_thread::sleep_for(std::chrono::milliseconds (100));
    std::cout << "sleep on 100 ms" << std::endl;
    auto e = rclcpp::Clock().now();
    std::cout << "E s: " << e.seconds() << " ns: " << e.nanoseconds() << std::endl;
    std::this_thread::sleep_for(std::chrono::milliseconds (1000));
    std::cout << "sleep on 1000 ms" << std::endl;
    auto f = rclcpp::Clock().now();
    std::cout << "F s: " << f.seconds() << " ns: " << f.nanoseconds() << std::endl;
  }

and I got results as

node clock
A s: 14.211 ns: 14211000000
B s: 14.211 ns: 14211000000
sleep on 1 ms
C s: 14.211 ns: 14211000000
sleep on 10 ms
D s: 14.211 ns: 14211000000
sleep on 100 ms
E s: 14.276 ns: 14276000000
sleep on 1000 ms
F s: 15.083 ns: 15083000000

rclcpp::Clock
A s: 1.59325e+09 ns: 1593252170601471068
B s: 1.59325e+09 ns: 1593252170601488220
sleep on 1 ms
C s: 1.59325e+09 ns: 1593252170602888410
sleep on 10 ms
D s: 1.59325e+09 ns: 1593252170613020106
sleep on 100 ms
E s: 1.59325e+09 ns: 1593252170713407435
sleep on 1000 ms
F s: 1.59325e+09 ns: 1593252171713550024

and another

node clock
A s: 18.67 ns: 18670000000
B s: 18.738 ns: 18738000000
sleep on 1 ms
C s: 18.738 ns: 18738000000
sleep on 10 ms
D s: 18.738 ns: 18738000000
sleep on 100 ms
E s: 18.801 ns: 18801000000
sleep on 1000 ms
F s: 19.376 ns: 19376000000

rclcpp::Clock
A s: 1.59325e+09 ns: 1593252177162594823
B s: 1.59325e+09 ns: 1593252177162603651
sleep on 1 ms
C s: 1.59325e+09 ns: 1593252177163688471
sleep on 10 ms
D s: 1.59325e+09 ns: 1593252177173827970
sleep on 100 ms
E s: 1.59325e+09 ns: 1593252177275033009

Maybe, we should use rclcpp::Clock, which defaults to RCL_SYSTEM_TIME ("System time"). RCL_SYSTEM_TIME represents the true rate of progression of time, this is not connected to simulation factor (does't listen /clock for knowledge of time), which I suppose that causes this issue. What I'm wondering is number of zeros on the end of nanoseconds value obtained from node clock.

[naiveHobo]

The time duration could be added to the recoveries tests that already exist, just need to add a > 0.0 check.

@abrzozowski In case you're wondering where to add the tests (wait and backup tests are present in the analog directories):

https://github.com/ros-planning/navigation2/blob/c6fccc054d2f8082273e7ea1d6987449a5e36749/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp#L105

[SteveMacenski]

I could see that, but I'm surprised that durations have that effect. It seems like rclcpp::Clock is the right way to go.

[abrzozowski]

I could see that, but I'm surprised that durations have that effect. It seems like rclcpp::Clock is the right way to go.

Ok, so I'm going to use RCL_STEADY_TIME, exactly a rclcpp::Clock(RCL_STEADY_TIME)

[abrzozowski]

@abrzozowski In case you're wondering where to add the tests (wait and backup tests are present in the analog directories):

@naiveHobo How to run these tests? I'm trying call ./test_spin_recovery_launch.py (from nav2_system_tests/src/recoveries/spin directory), but after that i get some python errors, like ...TypeError: join() argument must be str, bytes, or os.PathLike object, not 'NoneType'

[SteveMacenski]

try colcon test (or using ros2 launch with a launch file to setup all the paths)

[abrzozowski]

try colcon test (or using ros2 launch with a launch file to setup all the paths)

Call a launch file with relative or absolute path like

$ ros2 launch nav2_system_tests/src/recoveries/spin/test_spin_recovery_launch.py

causes

[INFO] [launch]: All log files can be found below (...)
[INFO] [launch]: Default logging verbosity is set to INFO
[ERROR] [launch]: Caught exception in launch (see debug for traceback): Caught exception when trying to load file of format [py]: join() argument must be str, bytes, or os.PathLike object, not 'NoneType'

When I type $ colcon test --packages-select nav2_system_tests i don't see any running tests, I think.

Starting >>> nav2_system_tests
[0.766s] WARNING:colcon.colcon_core.shell:The following packages are in the workspace but haven't been built:
- nav2_common
(...)
- nav2_system_tests
They are being used from the following locations instead:
- (...)/install/nav2_common
(...)
- (...)/install/nav2_system_tests
To suppress this warning ignore these packages in the workspace:
--packages-ignore nav2_common nav_2d_msgs dwb_msgs nav2_msgs nav2_voxel_grid nav2_util nav2_amcl nav2_behavior_tree nav2_lifecycle_manager nav2_map_server nav2_waypoint_follower nav_2d_utils nav2_bt_navigator nav2_costmap_2d nav2_rviz_plugins costmap_queue nav2_core dwb_core nav2_controller nav2_navfn_planner nav2_planner nav2_recoveries dwb_critics dwb_plugins nav2_dwb_controller navigation2 nav2_bringup nav2_system_tests
Finished <<< nav2_system_tests [0.09s]

Summary: 1 package finished [0.61s]

[SteveMacenski]

Are you using 20.04 with correct python3 versions? I haven't run into that problem.

[abrzozowski]

Are you using 20.04 with correct python3 versions? I haven't run into that problem.

Yes, I'm on Ubuntu 20.04 LTS (5.4.0-39-generic) with default python executable pointing on: /usr/bin/python -> python2 and

$ python3 --version
Python 3.8.2

When I call python2

$ python2 test_spin_recovery_launch.py 
Traceback (most recent call last):
  File "test_spin_recovery_launch.py", line 21, in <module>
    from launch import LaunchDescription
  File "/home/abrzozowski/dev/ros2_masters/ros2/build/launch/launch/__init__.py", line 17, in <module>
    from . import actions
  File "/home/abrzozowski/dev/ros2_masters/ros2/build/launch/launch/actions/__init__.py", line 17, in <module>
    from .declare_launch_argument import DeclareLaunchArgument
  File "/home/abrzozowski/dev/ros2_masters/ros2/build/launch/launch/actions/declare_launch_argument.py", line 79
    name: Text,
        ^
SyntaxError: invalid syntax

and when I call python3

$ python3 test_spin_recovery_launch.py 
Traceback (most recent call last):
  File "test_spin_recovery_launch.py", line 102, in <module>
    sys.exit(main())
  File "test_spin_recovery_launch.py", line 85, in main
    ld = generate_launch_description()
  File "test_spin_recovery_launch.py", line 35, in generate_launch_description
    bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'),
  File "/usr/lib/python3.8/posixpath.py", line 90, in join
    genericpath._check_arg_types('join', a, *p)
  File "/usr/lib/python3.8/genericpath.py", line 152, in _check_arg_types
    raise TypeError(f'{funcname}() argument must be str, bytes, or '
TypeError: join() argument must be str, bytes, or os.PathLike object, not 'NoneType'

[abrzozowski]

Are you using 20.04 with correct python3 versions? I haven't run into that problem.
TypeError: join() argument must be str, bytes, or os.PathLike object, not 'NoneType'

After that i see i haven't got an env variable BT_NAVIGATOR_XML. I realized I had to invoke, in some way test_spin_recovery from CMakeLists.txt

ament_add_test(test_spin_recovery
  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_spin_recovery_launch.py"
  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
  TIMEOUT 180
  ENV
    TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml
    TEST_EXECUTABLE=$<TARGET_FILE:${test_spin_recovery_exec}>
    TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world
    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
    BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml
)

[SteveMacenski]

keep in mind there is no python2 anymore, its all python3 now

[SteveMacenski]

Rebase / pull in master to fix the mixin CI problem. Fixed in #1835