diff --git a/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/Move.hpp b/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/Move.hpp index ce5c810..3b8e083 100644 --- a/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/Move.hpp +++ b/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/Move.hpp @@ -17,6 +17,7 @@ #include #include +#include #include "geometry_msgs/msg/pose.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" @@ -39,6 +40,9 @@ class Move : public plansys2::BtActionNode< BT::NodeStatus on_tick() override; BT::NodeStatus on_success() override; + void on_feedback( + const std::shared_ptr feedback) + override; static BT::PortsList providedPorts() { diff --git a/plansys2_bt_example/src/behavior_tree_nodes/ApproachObject.cpp b/plansys2_bt_example/src/behavior_tree_nodes/ApproachObject.cpp index fd5649b..5cadfb1 100644 --- a/plansys2_bt_example/src/behavior_tree_nodes/ApproachObject.cpp +++ b/plansys2_bt_example/src/behavior_tree_nodes/ApproachObject.cpp @@ -14,6 +14,7 @@ #include #include +#include #include "plansys2_bt_example/behavior_tree_nodes/ApproachObject.hpp" @@ -40,6 +41,9 @@ ApproachObject::tick() { std::cout << "ApproachObject tick " << counter_ << std::endl; + float completion = std::min(1.0f, counter_ / 5.0f); + config().blackboard->set("completion", completion); + if (counter_++ < 5) { return BT::NodeStatus::RUNNING; } else { diff --git a/plansys2_bt_example/src/behavior_tree_nodes/CloseGripper.cpp b/plansys2_bt_example/src/behavior_tree_nodes/CloseGripper.cpp index 4fa8948..1792b55 100644 --- a/plansys2_bt_example/src/behavior_tree_nodes/CloseGripper.cpp +++ b/plansys2_bt_example/src/behavior_tree_nodes/CloseGripper.cpp @@ -14,6 +14,7 @@ #include #include +#include #include "plansys2_bt_example/behavior_tree_nodes/CloseGripper.hpp" @@ -40,6 +41,9 @@ CloseGripper::tick() { std::cout << "CloseGripper tick " << counter_ << std::endl; + float completion = std::min(1.0f, counter_ / 5.0f); + config().blackboard->set("completion", completion); + if (counter_++ < 5) { return BT::NodeStatus::RUNNING; } else { diff --git a/plansys2_bt_example/src/behavior_tree_nodes/Move.cpp b/plansys2_bt_example/src/behavior_tree_nodes/Move.cpp index 61e87e5..dea7103 100644 --- a/plansys2_bt_example/src/behavior_tree_nodes/Move.cpp +++ b/plansys2_bt_example/src/behavior_tree_nodes/Move.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -107,9 +108,21 @@ Move::on_tick() BT::NodeStatus Move::on_success() { + config().blackboard->set("completion", 1.0f); return BT::NodeStatus::SUCCESS; } +void +Move::on_feedback( + const std::shared_ptr feedback) +{ + // nav2_sim sends distance_remaining counting down from ~10 to ~0 + constexpr float kInitialDistance = 10.0f; + float completion = 1.0f - std::min( + 1.0f, std::max(0.0f, feedback->distance_remaining / kInitialDistance)); + config().blackboard->set("completion", completion); +} + } // namespace plansys2_bt_tests diff --git a/plansys2_bt_example/src/behavior_tree_nodes/OpenGripper.cpp b/plansys2_bt_example/src/behavior_tree_nodes/OpenGripper.cpp index 1c21991..f1a1dc5 100644 --- a/plansys2_bt_example/src/behavior_tree_nodes/OpenGripper.cpp +++ b/plansys2_bt_example/src/behavior_tree_nodes/OpenGripper.cpp @@ -14,6 +14,7 @@ #include #include +#include #include "plansys2_bt_example/behavior_tree_nodes/OpenGripper.hpp" @@ -40,6 +41,9 @@ OpenGripper::tick() { std::cout << "OpenGripper tick " << counter_ << std::endl; + float completion = std::min(1.0f, counter_ / 5.0f); + config().blackboard->set("completion", completion); + if (counter_++ < 5) { return BT::NodeStatus::RUNNING; } else { diff --git a/plansys2_bt_example/src/behavior_tree_nodes/Recharge.cpp b/plansys2_bt_example/src/behavior_tree_nodes/Recharge.cpp index 0502114..a46ba32 100644 --- a/plansys2_bt_example/src/behavior_tree_nodes/Recharge.cpp +++ b/plansys2_bt_example/src/behavior_tree_nodes/Recharge.cpp @@ -14,6 +14,7 @@ #include #include +#include #include "plansys2_bt_example/behavior_tree_nodes/Recharge.hpp" @@ -40,6 +41,9 @@ Recharge::tick() { std::cout << "Recharge tick " << counter_ << std::endl; + float completion = std::min(1.0f, counter_ / 10.0f); + config().blackboard->set("completion", completion); + if (counter_++ < 10) { return BT::NodeStatus::RUNNING; } else {