Pimpest/toid
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Use header time instead of actual time for determining time passed

Browse Source
This commit is contained in:
Pimpest
2026-04-11 13:38:13 +02:00
parent 94e7626fda
commit c48e483d60
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
toid_bt/include/toid_bt/plugins/in_position_decorator.hpp
View File

@@ -51,13 +51,13 @@ public:
if(checkIfInPose(pose)) { if(checkIfInPose(pose)) {
if(checkIfPosesAreClose(last_pos, pose)) { if(checkIfPosesAreClose(last_pos, pose)) {
if(clock.now() - last_pos_change > rclcpp::Duration::from_seconds(timeout)) { if(rclcpp::Time(pose.header.stamp) - last_pos_change > rclcpp::Duration::from_seconds(timeout)) {
haltChild(); haltChild();
return BT::NodeStatus::SUCCESS; return BT::NodeStatus::SUCCESS;
} }
} else { } else {
last_pos = pose; last_pos = pose;
last_pos_change = clock.now(); last_pos_change = pose.header.stamp;
} }
} }