-
Notifications
You must be signed in to change notification settings - Fork 6
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Add BT.CPP solutions to Problems 2, 3, and 4 (#72)
* Add simple BT.CPP solutions to Problems 2 and 3 * WIP problem 4 * Add Problem 4 solution
- Loading branch information
Showing
9 changed files
with
307 additions
and
20 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,41 @@ | ||
|
|
||
| #pragma once | ||
|
|
||
| #include <behaviortree_cpp/action_node.h> | ||
| #include <rclcpp/rclcpp.hpp> | ||
|
|
||
| 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<std::string>("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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
18 changes: 0 additions & 18 deletions
18
technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem2.xml
This file was deleted.
Oops, something went wrong.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
File renamed without changes.
36 changes: 36 additions & 0 deletions
36
technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem2.xml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,36 @@ | ||
| <root BTCPP_format="4"> | ||
|
|
||
| <BehaviorTree ID="MainTree"> | ||
| <Sequence> | ||
| <Sequence> | ||
| <Fallback> | ||
| <Navigate name="ToTrash" target="dumpster"/> | ||
| <Sequence> | ||
| <Navigate name="ToTrashRoomDoor" target="hall_dining_trash"/> | ||
| <Open target="hall_dining_trash"/> | ||
| <Navigate name="ToTrash" target="dumpster"/> | ||
| </Sequence> | ||
| </Fallback> | ||
| <Open target="dumpster"/> | ||
| </Sequence> | ||
| <Sequence> | ||
| <Navigate name="ToDesk" target="desk"/> | ||
| <PickObject name="PickDeskWaste" object="waste0"/> | ||
| <Navigate name="ToTrash" target="dumpster"/> | ||
| <PlaceObject name="PlaceDeskWaste" object="waste0"/> | ||
| </Sequence> | ||
| <Sequence> | ||
| <Navigate name="ToBin" target="bin"/> | ||
| <PickObject name="PickBinWaste" object="waste1"/> | ||
| <Navigate name="ToTrash" target="dumpster"/> | ||
| <PlaceObject name="PlaceBinWaste" object="waste1"/> | ||
| </Sequence> | ||
| <Sequence> | ||
| <Navigate name="ToDining" target="dining"/> | ||
| <Navigate name="ToTrashRoomDoor" target="hall_dining_trash"/> | ||
| <Close target="hall_dining_trash"/> | ||
| </Sequence> | ||
| </Sequence> | ||
| </BehaviorTree> | ||
|
|
||
| </root> |
48 changes: 48 additions & 0 deletions
48
technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem3.xml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,48 @@ | ||
| <root BTCPP_format="4"> | ||
|
|
||
| <BehaviorTree ID="MainTree"> | ||
| <Sequence> | ||
| <RetryUntilSuccessful num_attempts="10"> | ||
| <Sequence> | ||
| <Navigate name="ToPantry" target="pantry"/> | ||
| <Open name="OpenPantry" target="pantry"/> | ||
| <PickObject name="PickBread" object="bread0"/> | ||
| </Sequence> | ||
| </RetryUntilSuccessful> | ||
| <RetryUntilSuccessful num_attempts="10"> | ||
| <Sequence> | ||
| <Navigate name="ToTable" target="table"/> | ||
| <PlaceObject name="PlaceBread" object="bread0"/> | ||
| </Sequence> | ||
| </RetryUntilSuccessful> | ||
| <RetryUntilSuccessful num_attempts="10"> | ||
| <Sequence> | ||
| <Navigate name="ToPantry" target="pantry"/> | ||
| <Close name="ClosePantry" target="pantry"/> | ||
| </Sequence> | ||
| </RetryUntilSuccessful> | ||
|
|
||
| <RetryUntilSuccessful num_attempts="10"> | ||
| <Sequence> | ||
| <Navigate name="ToFridge" target="fridge"/> | ||
| <Open name="OpenFridge" target="fridge"/> | ||
| <PickObject name="PickButter" object="butter0"/> | ||
| </Sequence> | ||
| </RetryUntilSuccessful> | ||
| <RetryUntilSuccessful num_attempts="10"> | ||
| <Sequence> | ||
| <Navigate name="ToTable" target="table"/> | ||
| <PlaceObject name="PlaceButter" object="butter0"/> | ||
| </Sequence> | ||
| </RetryUntilSuccessful> | ||
| <RetryUntilSuccessful num_attempts="10"> | ||
| <Sequence> | ||
| <Navigate name="ToFridge" target="fridge"/> | ||
| <Close name="CloseFridge" target="fridge"/> | ||
| </Sequence> | ||
| </RetryUntilSuccessful> | ||
|
|
||
| </Sequence> | ||
| </BehaviorTree> | ||
|
|
||
| </root> |
175 changes: 175 additions & 0 deletions
175
technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem4.xml
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,175 @@ | ||
| <root BTCPP_format="4"> | ||
|
|
||
| <!-- The main tree, which uses the other subtrees. --> | ||
| <BehaviorTree ID="MainTree"> | ||
| <Sequence> | ||
| <SubTree ID="OpenDoorToCharger"/> | ||
| <SubTree ID="PrepareMeal"/> | ||
| <SubTree ID="TakeOutTrash"/> | ||
| </Sequence> | ||
| </BehaviorTree> | ||
|
|
||
| <!-- Open the door to the closet, which contains the charger. --> | ||
| <BehaviorTree ID="OpenDoorToCharger"> | ||
| <Sequence> | ||
| <RetryUntilSuccessful num_attempts="10"> | ||
| <Navigate name="ToClosetDoor" target="hall_dining_closet"/> | ||
| </RetryUntilSuccessful> | ||
| <RetryUntilSuccessful num_attempts="10"> | ||
| <Open name="OpenClosetDoor" target="hall_dining_closet"/> | ||
| </RetryUntilSuccessful> | ||
| </Sequence> | ||
| </BehaviorTree> | ||
|
|
||
| <!-- Bring the bread and butter to the table, paying attention to battery. --> | ||
| <BehaviorTree ID="PrepareMeal"> | ||
| <Sequence> | ||
| <SubTree ID="NavigateRobustly" target="pantry"/> | ||
| <SubTree ID="OpenRobustly" target="pantry"/> | ||
| <SubTree ID="PickRobustly" object="bread0"/> | ||
| <SubTree ID="NavigateRobustly" target="table"/> | ||
| <SubTree ID="PlaceRobustly" object="bread0"/> | ||
| <SubTree ID="NavigateRobustly" target="pantry"/> | ||
| <SubTree ID="CloseRobustly" target="pantry"/> | ||
| <SubTree ID="NavigateRobustly" target="fridge"/> | ||
| <SubTree ID="OpenRobustly" target="fridge"/> | ||
| <SubTree ID="PickRobustly" object="butter0"/> | ||
| <SubTree ID="NavigateRobustly" target="table"/> | ||
| <SubTree ID="PlaceRobustly" object="butter0"/> | ||
| <SubTree ID="NavigateRobustly" target="fridge"/> | ||
| <SubTree ID="CloseRobustly" target="fridge"/> | ||
| </Sequence> | ||
| </BehaviorTree> | ||
|
|
||
|
|
||
| <!-- Take the trash to the dumpster, paying attention to battery. --> | ||
| <BehaviorTree ID="TakeOutTrash"> | ||
| <Sequence> | ||
| <SubTree ID="NavigateRobustly" target="hall_dining_trash"/> | ||
| <SubTree ID="OpenRobustly" target="hall_dining_trash"/> | ||
| <SubTree ID="NavigateRobustly" target="dumpster"/> | ||
| <SubTree ID="OpenRobustly" target="dumpster"/> | ||
| <SubTree ID="NavigateRobustly" target="desk"/> | ||
| <SubTree ID="PickRobustly" object="waste0"/> | ||
| <SubTree ID="NavigateRobustly" target="dumpster"/> | ||
| <SubTree ID="PlaceRobustly" object="waste0"/> | ||
| <SubTree ID="NavigateRobustly" target="bin"/> | ||
| <SubTree ID="PickRobustly" object="waste1"/> | ||
| <SubTree ID="NavigateRobustly" target="dumpster"/> | ||
| <SubTree ID="PlaceRobustly" object="waste1"/> | ||
| <SubTree ID="CloseRobustly" target="dumpster"/> | ||
| <SubTree ID="NavigateRobustly" target="dining"/> | ||
| <SubTree ID="NavigateRobustly" target="hall_dining_trash"/> | ||
| <SubTree ID="CloseRobustly" target="hall_dining_trash"/> | ||
| </Sequence> | ||
| </BehaviorTree> | ||
|
|
||
| <!-- Helper subtrees that perform basic actions, | ||
| paying attention to both failures and battery usage. --> | ||
| <BehaviorTree ID="NavigateRobustly"> | ||
| <RetryUntilSuccessful num_attempts="5"> | ||
| <Fallback> | ||
| <ReactiveSequence> | ||
| <IsBatteryFull full_threshold="30"/> | ||
| <RetryUntilSuccessful num_attempts="10"> | ||
| <Navigate name="GoToTarget" target="{target}"/> | ||
| </RetryUntilSuccessful> | ||
| </ReactiveSequence> | ||
| <AlwaysFailure> | ||
| <RetryUntilSuccessful num_attempts="10"> | ||
| <Navigate name="GoToCharger" target="charger"/> | ||
| </RetryUntilSuccessful> | ||
| </AlwaysFailure> | ||
| </Fallback> | ||
| </RetryUntilSuccessful> | ||
| </BehaviorTree> | ||
|
|
||
| <BehaviorTree ID="PickRobustly"> | ||
| <Sequence> | ||
| <GetCurrentLocation location="{start_location}"/> | ||
| <RetryUntilSuccessful num_attempts="10"> | ||
| <Sequence> | ||
| <PickObject name="PickObject" object="{object}"/> | ||
| <Fallback> | ||
| <IsBatteryFull full_threshold="30"/> | ||
| <Sequence> | ||
| <RetryUntilSuccessful num_attempts="10"> | ||
| <Navigate name="GoToCharger" target="charger"/> | ||
| </RetryUntilSuccessful> | ||
| <RetryUntilSuccessful num_attempts="10"> | ||
| <Navigate name="GoBackToStart" target="{start_location}"/> | ||
| </RetryUntilSuccessful> | ||
| </Sequence> | ||
| </Fallback> | ||
| </Sequence> | ||
| </RetryUntilSuccessful> | ||
| </Sequence> | ||
| </BehaviorTree> | ||
|
|
||
| <BehaviorTree ID="PlaceRobustly"> | ||
| <Sequence> | ||
| <GetCurrentLocation location="{start_location}"/> | ||
| <RetryUntilSuccessful num_attempts="10"> | ||
| <Sequence> | ||
| <PlaceObject name="PlaceObject" object="{object}"/> | ||
| <Fallback> | ||
| <IsBatteryFull full_threshold="30"/> | ||
| <Sequence> | ||
| <RetryUntilSuccessful num_attempts="10"> | ||
| <Navigate name="GoToCharger" target="charger"/> | ||
| </RetryUntilSuccessful> | ||
| <RetryUntilSuccessful num_attempts="10"> | ||
| <Navigate name="GoBackToStart" target="{start_location}"/> | ||
| </RetryUntilSuccessful> | ||
| </Sequence> | ||
| </Fallback> | ||
| </Sequence> | ||
| </RetryUntilSuccessful> | ||
| </Sequence> | ||
| </BehaviorTree> | ||
|
|
||
| <BehaviorTree ID="OpenRobustly"> | ||
| <Sequence> | ||
| <GetCurrentLocation location="{start_location}"/> | ||
| <RetryUntilSuccessful num_attempts="10"> | ||
| <Sequence> | ||
| <Open name="OpenLocation" target="{target}"/> | ||
| <Fallback> | ||
| <IsBatteryFull full_threshold="30"/> | ||
| <Sequence> | ||
| <RetryUntilSuccessful num_attempts="10"> | ||
| <Navigate name="GoToCharger" target="charger"/> | ||
| </RetryUntilSuccessful> | ||
| <RetryUntilSuccessful num_attempts="10"> | ||
| <Navigate name="GoBackToStart" target="{start_location}"/> | ||
| </RetryUntilSuccessful> | ||
| </Sequence> | ||
| </Fallback> | ||
| </Sequence> | ||
| </RetryUntilSuccessful> | ||
| </Sequence> | ||
| </BehaviorTree> | ||
|
|
||
| <BehaviorTree ID="CloseRobustly"> | ||
| <Sequence> | ||
| <GetCurrentLocation location="{start_location}"/> | ||
| <RetryUntilSuccessful num_attempts="10"> | ||
| <Sequence> | ||
| <Close name="CloseLocation" target="{target}"/> | ||
| <Fallback> | ||
| <IsBatteryFull full_threshold="30"/> | ||
| <Sequence> | ||
| <RetryUntilSuccessful num_attempts="10"> | ||
| <Navigate name="GoToCharger" target="charger"/> | ||
| </RetryUntilSuccessful> | ||
| <RetryUntilSuccessful num_attempts="10"> | ||
| <Navigate name="GoBackToStart" target="{start_location}"/> | ||
| </RetryUntilSuccessful> | ||
| </Sequence> | ||
| </Fallback> | ||
| </Sequence> | ||
| </RetryUntilSuccessful> | ||
| </Sequence> | ||
| </BehaviorTree> | ||
|
|
||
| </root> |