Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix invalid tick frequency in core library. #130

Draft
wants to merge 2 commits into
base: main
Choose a base branch
from
Draft

Fix invalid tick frequency in core library. #130

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
5c55f29
Update tick time reporting.
Oberacda Oct 22, 2024
dc22460
Fix wrong timestamp calculation.
Oberacda Oct 23, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
42 changes: 18 additions & 24 deletions ros_bt_py/ros_bt_py/tree_manager.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -371,8 +371,6 @@ def __init__(
if tick_frequency_hz == 0.0:
tick_frequency_hz = 10.0

self.tick_sliding_window = [tick_frequency_hz] * 10
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I like removing the sliding window stuff, that is something for the monitoring GUI to do if we even want it


self.nodes = {}

self._state_lock = Lock()
Expand Down Expand Up @@ -576,48 +574,44 @@ def tick(self, once=None):
)
return

tick_start_timestamp = self.ros_node.get_clock().now()
while True:
tick_start_timestamp = self.ros_node.get_clock().now()
Comment on lines +577 to -580
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I am not sure if I understand this change? Why not get the new timestamp at the beginning of each iteration instead of before the loop and at the end of each iteration?

if self.get_state() == Tree.STOP_REQUESTED:
break
root.tick()
tick_end_timestamp = self.ros_node.get_clock().now()

self.publish_info(
subtree_info_msg=self.subtree_manager.get_subtree_info_msg()
if self.subtree_manager is not None
else None,
ticked=True,
)

if self._stop_after_result:
if root.state in [NodeMsg.FAILED, NodeMsg.SUCCEEDED]:
break

if self._once:
# Return immediately, not unticking anything
self._once = False
self.set_state(Tree.WAITING_FOR_TICK)
return
tick_end_timestamp = self.ros_node.get_clock().now()

duration: Duration = tick_end_timestamp - tick_start_timestamp
tick_rate = self.tree_msg.tick_frequency_hz
measured_tick_period_s: float = (tick_end_timestamp - tick_start_timestamp).nanoseconds / 1e9
expected_tick_period_s: float = 1 / self.tree_msg.tick_frequency_hz

if (1 / tick_rate) > (duration.nanoseconds * 1e9):
if expected_tick_period_s < measured_tick_period_s:
get_logger("tree_manager").get_child(self.name).warn(
"Tick took longer than set period, cannot tick at "
f"{self.tree_msg.tick_frequency_hz:.2f} Hz"
)

self.tick_sliding_window.pop(0)
self.tick_sliding_window.append(duration.nanoseconds * 1e9)
tick_frequency_avg = sum(self.tick_sliding_window) / len(
self.tick_sliding_window
)

if self.publish_tick_frequency is not None:
tick_frequency_msg = Float64()
tick_frequency_msg.data = tick_frequency_avg
tick_frequency_msg.data = measured_tick_period_s
self.publish_tick_frequency(tick_frequency_msg)

if self._stop_after_result:
if root.state in [NodeMsg.FAILED, NodeMsg.SUCCEEDED]:
break

if self._once:
# Return immediately, not unticking anything
self._once = False
self.set_state(Tree.WAITING_FOR_TICK)
return
tick_start_timestamp = self.ros_node.get_clock().now()
self.rate.sleep()
Comment on lines +614 to 615
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Does this make sense? We first start our new timer and then sleep the rest of the rate of the old one?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think we can also switch this around, but we need to include the rate time in the tick calculation. Indeed it might be more correct to the reporting after wakeing up from the rate.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would switch it around and start at the beginning of the loop and end after the rate

self.set_state(Tree.IDLE)
self.publish_info(
Expand Down
Loading