Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add BT.CPP solutions to Problems 2, 3, and 4 #72

Merged
merged 4 commits into from
Oct 20, 2024
Merged

Add BT.CPP solutions to Problems 2, 3, and 4 #72

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
ca4153d
Add simple BT.CPP solutions to Problems 2 and 3
sea-bass Oct 17, 2024
6809fcc
WIP problem 4
sea-bass Oct 17, 2024
34b8299
Add Problem 4 solution
sea-bass Oct 19, 2024
5077938
Merge branch 'main' into btcpp-solutions-2-and-3
sea-bass Oct 19, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions technologies/BehaviorTree.CPP/README.md
View file
Open in desktop
sea-bass marked this conversation as resolved.
Show resolved Hide resolved
sea-bass marked this conversation as resolved.
Show resolved Hide resolved
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down
41 changes: 41 additions & 0 deletions ...aviorTree.CPP/pyrobosim_btcpp/include/pyrobosim_btcpp/nodes/get_current_location_node.hpp
View file
Open in desktop
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
2 changes: 2 additions & 0 deletions technologies/BehaviorTree.CPP/pyrobosim_btcpp/src/btcpp_executor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -72,6 +73,7 @@ int main(int argc, char** argv)

factory.registerNodeType<BT::IsBatteryLow>("IsBatteryLow", nh->get_logger());
factory.registerNodeType<BT::IsBatteryFull>("IsBatteryFull", nh->get_logger());
factory.registerNodeType<BT::GetCurrentLocation>("GetCurrentLocation", nh->get_logger());
factory.registerNodeType<BT::CloseAction>("Close", params);
factory.registerNodeType<BT::DetectObject>("DetectObject", params);
factory.registerNodeType<BT::NavigateAction>("Navigate", params);
Expand Down
18 changes: 0 additions & 18 deletions technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/problem2.xml
View file
Open in desktop

This file was deleted.

3 changes: 3 additions & 0 deletions technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/pyrobosim_btcpp_model.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,9 @@
<Condition ID="IsBatteryLow">
<input_port name="low_threshold" type="double" default="20.000000">Return SUCCESS if {@battery_level} below this value</input_port>
</Condition>
<Action ID="GetCurrentLocation"/>
<output_port name="location" type="std::string"/>
</Action>
<Action ID="Navigate">
<input_port name="target" type="std::string"/>
<input_port name="robotID" type="std::string" default="robot">Robot name</input_port>
Expand Down
0 ...ee.CPP/pyrobosim_btcpp/trees/problem1.xml → ...robosim_btcpp/trees/solution/problem1.xml
View file
Open in desktop
File renamed without changes.
36 changes: 36 additions & 0 deletions technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem2.xml
View file
Open in desktop
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 technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem3.xml
View file
Open in desktop
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 technologies/BehaviorTree.CPP/pyrobosim_btcpp/trees/solution/problem4.xml
View file
Open in desktop
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>