-
Notifications
You must be signed in to change notification settings - Fork 1.8k
Description
Bug report
Required Info:
- Operating System:
- Ubuntu 24.04
- ROS2 Version:
- Jazzy / rolling
- Version or commit hash:
- Source
- DDS implementation:
- Cyclone
I have an problem since we moved to Jazzy, but its also happening in Rolling. Both installed from source.
I have a behavior tree where the battery level is checked before trying to undock the robot, similar to this one:
But I have to call the NavigateToPose action twice before the robot starts to move.
- On the first call, IsBatteryCharging returns FALSE, so the other branch of the BT is executed. As the robot is docked, the global result is a failure.
- On the second call, IsBatteryCharging returns SUCCESS, performing the undocking action and moving as usual.
After checking the code here, I noticed that the batteryCallback is never executed in the first call, but it is in the second one. I think it's because this spin_some doesn't spin the first time.
navigation2/nav2_behavior_tree/plugins/condition/is_battery_charging_condition.cpp
Lines 66 to 72 in 08268f9
| callback_group_executor_.spin_some(); | |
| if (is_battery_charging_) { | |
| return BT::NodeStatus::SUCCESS; | |
| } | |
| return BT::NodeStatus::FAILURE; | |
| } |
After reading other code, I found a similar problem fixed with a double call:
| // Spin multiple times due to rclcpp regression in Jazzy requiring a 'warm up' spin | |
| callback_group_executor_.spin_all(std::chrono::milliseconds(1)); | |
| callback_group_executor_.spin_all(std::chrono::milliseconds(49)); |
I'm not sure if this is fixed or not because putting another spin_some in the IsBatteryCharging BT fixed the problem.