Wait for complete state duration fix #590

tylerjw · 2021-08-03T21:19:56Z

Description

This fixes a bug I found while trying to use the servo demo on rolling.

codecov · 2021-08-03T22:22:47Z

Codecov Report

Merging #590 (8e36d6c) into main (7064da6) will increase coverage by 0.09%.
The diff coverage is 46.16%.

❗ Current head 8e36d6c differs from pull request most recent head ceb2a99. Consider uploading reports for the commit ceb2a99 to get more accurate results

@@            Coverage Diff             @@
##             main     #590      +/-   ##
==========================================
+ Coverage   46.89%   46.98%   +0.09%     
==========================================
  Files         185      185              
  Lines       19656    19657       +1     
==========================================
+ Hits         9216     9234      +18     
+ Misses      10440    10423      -17

Impacted Files	Coverage Δ
moveit_ros/moveit_servo/src/servo.cpp	`0.00% <0.00%> (ø)`
...eit/planning_scene_monitor/current_state_monitor.h	`22.23% <ø> (+22.23%)`	⬆️
...or/src/current_state_monitor_middleware_handle.cpp	`0.00% <0.00%> (ø)`
...anning_scene_monitor/src/current_state_monitor.cpp	`21.14% <75.00%> (+8.32%)`	⬆️

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 7064da6...ceb2a99. Read the comment docs.

tylerjw · 2021-08-04T14:44:50Z

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp

-  double slept_time = 0.0;
-  double sleep_step_s = std::min(0.05, wait_time / 10.0);
-  rclcpp::Duration sleep_step(sleep_step_s);
-  while (!haveCompleteState() && slept_time < wait_time)
+  const double sleep_step_s = std::min(0.05, wait_time / 10.0);
+  const auto sleep_step_duration = rclcpp::Duration::from_seconds(sleep_step_s * 1e9);
+  double slept_time_s = 0.0;
+  while (!haveCompleteState() && slept_time_s < wait_time)
  {
-    rclcpp::sleep_for(sleep_step.to_chrono<std::chrono::nanoseconds>());
-    slept_time += sleep_step_s;
+    rclcpp::sleep_for(sleep_step_duration.to_chrono<std::chrono::nanoseconds>());
+    slept_time_s += sleep_step_s;
  }
  return haveCompleteState();
 }


What happens with the old logic (on rolling):

wait_time = 10 (seconds)
slept_time = 0.0
sleep_time_s = 1
sleep_step = 1ns (constructor for double seconds is gone and the single argument constructor for Duration is now nanoseconds, there is a compiler warning that prints saying this is deprecated)

sleeps for 10 1ns amounts and returns...

As a result servo never hasCompleteState and calls exit(EXIT_FAILURE); in the constructor of Servo. I moved the wait for complete state into the start function.

As an aside I really wish I had colorized ros2 output. Has anyone figured out how to get that? It would have been much quicker to debug if the error it printed was red.

As an aside I really wish I had colorized ros2 output. Has anyone figured out how to get that? It would have been much quicker to debug if the error it printed was red.

You could do it with export RCUTILS_COLORIZED_OUTPUT=1

^Thanks @JafarAbdi! Adding to my bashrc

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp

tylerjw · 2021-08-04T15:18:34Z

This now comes with a test, restoring the old code in waitForCompleteState fails like I'd expect:

[ RUN      ] CurrentStateMonitorTests.WaitForCompleteStateWaits
[INFO] [1628090240.874086555] [moveit_robot_model.robot_model]: Loading robot model 'panda'...
/home/tyler/code/moveit2/ws_moveit2/src/moveit2/moveit_ros/planning/planning_scene_monitor/test/current_state_monitor_tests.cpp:187: Failure
Expected: (std::chrono::duration_cast<std::chrono::nanoseconds>(end - start).count()) > (5e+7), actual: 1946203 vs 5e+07
[  FAILED  ] CurrentStateMonitorTests.WaitForCompleteStateWaits (12 ms)

jliukkonen · 2021-08-04T15:34:23Z

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp

-    rclcpp::sleep_for(sleep_step.to_chrono<std::chrono::nanoseconds>());
-    slept_time += sleep_step_s;
+    rclcpp::sleep_for(sleep_step_duration.to_chrono<std::chrono::nanoseconds>());
+    slept_time_s += sleep_step_s;


Nit about variable naming. I know ROS is "by default" SI units and the base unit for time is second and that should always be the implicit default. Yet, I find it much easier to read code and API if the units are documented as part of the variable names. In this case, it would mean that wait_time would be wait_time_s and sleep_step_duration would be sleep_step_duration_s. Thoughts?

I didn't thought this before but really liked it 10/10 will use next time I write something with durations

sleep_step_duration is a Duration object and is not specifically in the unit seconds, it is in whatever unit you want. I will change wait_time though.

+1, Google style is to include units

vatanaksoytezer

Should this (and all this DI stuff) be backported to foxy, or we don't care about that?

AndyZe · 2021-08-04T15:56:00Z

moveit_ros/planning/planning_scene_monitor/test/current_state_monitor_tests.cpp

+  };
+
+  // WHEN it is constructed
+  // THEN we expect haveCompleteState to be false


nit: this capitalization is weird/hard to read. Not sure it adds anything. WHEN, THEN, etc.

Does Doxygen support pulling out GWT conditions in some format? That would be a nice motivation for this explicit formatting.

The given-when-then comes from BDD (https://en.wikipedia.org/wiki/Behavior-driven_development) and is a practice of testing. I'll be the first to admit I don't fully understand BDD but that it does seem to provide helpful patterns for writing nice tests.

AndyZe

Approving assuming you update variable names like Jere suggested

tylerjw · 2021-08-04T16:45:56Z

Should this (and all this DI stuff) be backported to foxy, or we don't care about that?

I don't really care to backport it to foxy, but we can if we are using foxy for some project that uses servo. I think the issue with the constructor for Duration no longer taking double seconds is a rolling/galactic problem. In foxy I think this code still works the way we think it does (because you can construct a duration with double seconds).

Co-authored-by: Jafar Abdi <cafer.abdi@gmail.com>

davetcoleman · 2021-08-05T16:51:10Z

So exciting to see test coverage growing!

Co-authored-by: Jafar Abdi <cafer.abdi@gmail.com>

tylerjw requested a review from henningkayser August 3, 2021 21:19

tylerjw commented Aug 4, 2021

View reviewed changes

tylerjw requested a review from JafarAbdi August 4, 2021 14:45

JafarAbdi reviewed Aug 4, 2021

View reviewed changes

moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp Outdated Show resolved Hide resolved

tylerjw changed the title ~~Wait for current state duration fix~~ Wait for complete state duration fix Aug 4, 2021

JafarAbdi approved these changes Aug 4, 2021

View reviewed changes

tylerjw force-pushed the wait_for_current_state branch from efa77f1 to e7af397 Compare August 4, 2021 15:20

jliukkonen reviewed Aug 4, 2021

View reviewed changes

vatanaksoytezer reviewed Aug 4, 2021

View reviewed changes

AndyZe reviewed Aug 4, 2021

View reviewed changes

AndyZe approved these changes Aug 4, 2021

View reviewed changes

tylerjw force-pushed the wait_for_current_state branch from 8e36d6c to ec8e470 Compare August 4, 2021 16:43

tylerjw and others added 2 commits August 4, 2021 10:51

Wait for complete state duration fix

db87cca

Co-authored-by: Jafar Abdi <cafer.abdi@gmail.com>

Add unit to wait_time and remove use of time in test

ceb2a99

tylerjw force-pushed the wait_for_current_state branch from ec8e470 to ceb2a99 Compare August 4, 2021 16:51

tylerjw merged commit 1dceb42 into moveit:main Aug 4, 2021

tylerjw deleted the wait_for_current_state branch August 4, 2021 17:53

christianlandgraf pushed a commit to christianlandgraf/moveit2 that referenced this pull request Aug 12, 2021

Wait for complete state duration fix (moveit#590)

f7171c6

Co-authored-by: Jafar Abdi <cafer.abdi@gmail.com>

v4hn mentioned this pull request Aug 17, 2021

CI: Enable -Werror only for TARGET_WORKSPACE moveit/moveit#2821

Merged

tylerjw added a commit to tylerjw/moveit2 that referenced this pull request Aug 27, 2021

Wait for complete state duration fix (moveit#590)

d269e3b

Co-authored-by: Jafar Abdi <cafer.abdi@gmail.com>

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Wait for complete state duration fix #590

Wait for complete state duration fix #590

tylerjw commented Aug 3, 2021

codecov bot commented Aug 3, 2021 •

edited

Loading

tylerjw Aug 4, 2021

JafarAbdi Aug 4, 2021

AndyZe Aug 4, 2021

tylerjw commented Aug 4, 2021

jliukkonen Aug 4, 2021

vatanaksoytezer Aug 4, 2021 •

edited

Loading

tylerjw Aug 4, 2021

davetcoleman Aug 5, 2021

vatanaksoytezer left a comment

AndyZe Aug 4, 2021 •

edited

Loading

nbbrooks Aug 4, 2021

tylerjw Aug 4, 2021

AndyZe left a comment

tylerjw commented Aug 4, 2021 •

edited

Loading

davetcoleman commented Aug 5, 2021

Wait for complete state duration fix #590

Wait for complete state duration fix #590

Conversation

tylerjw commented Aug 3, 2021

Description

codecov bot commented Aug 3, 2021 • edited Loading

Codecov Report

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

tylerjw commented Aug 4, 2021

Choose a reason for hiding this comment

vatanaksoytezer Aug 4, 2021 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

vatanaksoytezer left a comment

Choose a reason for hiding this comment

AndyZe Aug 4, 2021 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

AndyZe left a comment

Choose a reason for hiding this comment

tylerjw commented Aug 4, 2021 • edited Loading

davetcoleman commented Aug 5, 2021

codecov bot commented Aug 3, 2021 •

edited

Loading

vatanaksoytezer Aug 4, 2021 •

edited

Loading

AndyZe Aug 4, 2021 •

edited

Loading

tylerjw commented Aug 4, 2021 •

edited

Loading