Tick frequency not respected (tree.sleep(ms) issue)

[mtlazaro]

Hi,
I'm using a tree_execution_server as provided in the example, with one simple BT test:

<root BTCPP_format="4" main_tree_to_execute="MainTree">
	<BehaviorTree ID="TestBtRos">
		<ReactiveSequence name="MainReactiveSequence">
			<Sequence name="CheckRobotSequence">
				<Fallback>
					<CheckAutoSubscriber name="CheckAuto" topic_name="/check_auto" />
					<SetAutoService name="SetAuto" service_name="/set_auto" value="true" />
				</Fallback>
				<CheckSafetySubscriber name="CheckSafety" topic_name="/check_safety" />
			</Sequence>
			<Sequence name="MainOperationSequence">
				<Script code=" navigate_goal:='PointA' " />
				<SleepAction name="MoveTo" action_name="sleep_service" msec="5000" />
			</Sequence>
		</ReactiveSequence>
	</BehaviorTree>
</root>

Mainly, I have two topic subscribers (Bool msg) to check securities in the robot, and a move action (for now it is just an sleep action), so if any check fails, the action will be interrupted. For this dummy test I'm publishing 'True' through the console into the /check_x topics.

I realized that even if I publish those topics at a very high rate, eventually the RosTopicSubNode returns no new message has been received since the last tick, even if I reduce the tick frequency.

I've been debugging and realized that after few ticks, the tree.sleep() function in tree_execution_server.cpp starts to fail, and doesn't sleep the required time. Here, I show some console prints where you can see that we have two ticks at the same timestamp, and that tree wakes up inmediately. This causes two consecutive ticks to the check_auto subscriber without having enough time to receive a new message.

If I replace the line:
p_->tree.sleep(std::chrono::duration_cast<std::chrono::system_clock::duration>(loop_deadline - now));
by a simple
std::this_thread::sleep_for(loop_deadline - now);
everything works fine and synchronized.

What could be producing a wake_up event in the tree.sleep() function so it is not executed?

[mtlazaro]

Just for more info, this would be the correct behavior I observe if I use the std::this_thread::sleep_for(loop_deadline - now);
with the thread waking up at the programmed loop deadline timestamp. Then the Tick ticks the two checkers only once.