diff --git a/technologies/BehaviorTree.CPP/README.md b/technologies/BehaviorTree.CPP/README.md index b98bea1..a50b6b5 100644 --- a/technologies/BehaviorTree.CPP/README.md +++ b/technologies/BehaviorTree.CPP/README.md @@ -9,7 +9,7 @@ We suggest creating your XML trees in the folder `technologies/BehaviorTree.CPP/ **Careful**: make sure to run this command in `/delib_ws` folder. ```bash -colcon build --symlink-install --packages-select pyrobosim_btcpp +colcon build --symlink-install --packages-up-to pyrobosim_btcpp ``` ## How to run @@ -34,7 +34,7 @@ For instance, consider this sample XML in [trees/navigation_demo.xml](pyrobosim_ NOTE: remember that the `name` attribute in the XML is optional and used for debugging only. -You can run the BeahviorTree with the command: +You can run the BehaviorTree with the command: ```bash ros2 run pyrobosim_btcpp btcpp_executor --ros-args -p tree:=trees/navigation_demo.xml diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/get_current_location_node.hpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/get_current_location_node.hpp index e69de29..712680d 100644 --- a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/get_current_location_node.hpp +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/get_current_location_node.hpp @@ -0,0 +1,41 @@ + +#pragma once + +#include +#include + +namespace BT +{ + +class GetCurrentLocation : public SyncActionNode +{ +public: + GetCurrentLocation(const std::string& name, const NodeConfig& config, rclcpp::Logger logger) + : SyncActionNode(name, config), logger_(logger) + {} + + static PortsList providedPorts() + { + return { OutputPort("location", "The robot's current location.") }; + } + + // You must override the virtual function tick() + NodeStatus tick() override + { + // Read a value from the blackboard + std::string current_location = ""; + if(!config().blackboard->get("@last_visited_location", current_location)) + { + RCLCPP_ERROR(logger_, "Robot location not available yet"); + return NodeStatus::FAILURE; + } + RCLCPP_INFO(logger_, "Robot location level: %s", current_location.c_str()); + setOutput("location", current_location); + return NodeStatus::SUCCESS; + } + +private: + rclcpp::Logger logger_; +}; + +} // namespace BT diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/src/btcpp_executor.cpp b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/src/btcpp_executor.cpp index 4bdeab0..30be81d 100644 --- a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/src/btcpp_executor.cpp +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/src/btcpp_executor.cpp @@ -13,6 +13,7 @@ // BTCPP nodes in this package #include "pyrobosim_btcpp/nodes/battery_nodes.hpp" +#include "pyrobosim_btcpp/nodes/get_current_location_node.hpp" #include "pyrobosim_btcpp/nodes/open_node.hpp" #include "pyrobosim_btcpp/nodes/close_node.hpp" #include "pyrobosim_btcpp/nodes/detect_object_node.hpp" @@ -72,6 +73,7 @@ int main(int argc, char** argv) factory.registerNodeType("IsBatteryLow", nh->get_logger()); factory.registerNodeType("IsBatteryFull", nh->get_logger()); + factory.registerNodeType("GetCurrentLocation", nh->get_logger()); factory.registerNodeType("Close", params); factory.registerNodeType("DetectObject", params); factory.registerNodeType("Navigate", params); diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem2.xml b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem2.xml deleted file mode 100644 index f12f1a9..0000000 --- a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem2.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/pyrobosim_btcpp_model.xml b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/pyrobosim_btcpp_model.xml index a33a602..ffd45ab 100644 --- a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/pyrobosim_btcpp_model.xml +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/pyrobosim_btcpp_model.xml @@ -16,6 +16,9 @@ Return SUCCESS if {@battery_level} below this value + + + Robot name diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem1.xml b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem1.xml similarity index 100% rename from technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem1.xml rename to technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem1.xml diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem2.xml b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem2.xml new file mode 100644 index 0000000..704c057 --- /dev/null +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem2.xml @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem3.xml b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem3.xml new file mode 100644 index 0000000..453282a --- /dev/null +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem3.xml @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem4.xml b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem4.xml new file mode 100644 index 0000000..a19d553 --- /dev/null +++ b/technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem4.xml @@ -0,0 +1,175 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +