PlanSys2 · fmrico · Feb 25, 2024 · Feb 21, 2024
diff --git a/plansys2_bt_example/README.md b/plansys2_bt_example/README.md
@@ -8,6 +8,11 @@ This example shows how to use Behavior Trees to carry out the tasks that an acti
 
 See https://github.com/IntelligentRoboticsLabs/ros2_planning_system/pull/27 for details.
 
+## Prerequisites
+```bash
+sudo apt-get install ros-humble-turtlebot3*
+```
+
 ## How to run
 
 ### In terminal 1:
@@ -16,7 +21,7 @@ See https://github.com/IntelligentRoboticsLabs/ros2_planning_system/pull/27 for
 export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/humble/share/turtlebot3_gazebo/models
 export TURTLEBOT3_MODEL=waffle
 source /usr/share/gazebo/setup.sh
-source $HOME/dev_ws/install/setup.bash
+source install/setup.bash
 ros2 launch nav2_bringup tb3_simulation_launch.py headless:=False
 ```
 
@@ -32,7 +37,7 @@ ros2 topic pub --once /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "
 ### In terminal 2:
 
 ```bash
-source $HOME/dev_ws/install/setup.bash
+source install/setup.bash
 ros2 launch plansys2_bt_example plansys2_bt_example_launch.py
 ```
 
@@ -41,7 +46,7 @@ ros2 launch plansys2_bt_example plansys2_bt_example_launch.py
 There are two options in this example. First option is to use the plansys2 terminal. Secondly you can use the custom assemble controller. Both options are shown below.
 
 ```bash
-source $HOME/dev_ws/install/setup.bash
+source install/setup.bash
 
 # before you run the commands below, it seems the starting location must be different to prevent an error
 ros2 topic pub --times 3 /goal_pose geometry_msgs/PoseStamped "{header: {stamp: {sec: 0}, frame_id: 'map'}, pose: {position: {x: -0.4, y: 0.4, z: 0.01}, orientation: {w: 0.0}}}"
@@ -70,21 +75,23 @@ set predicate (piece_at steering_wheel_1 steering_wheels_zone)
 set goal (and(piece_at wheel_1 assembly_zone))
 run
 ```
-You can also paste commands above into a file (e.g. commands.txt), start the plansys2_terminal and enter the following:
+You can also paste the problem above into a file (e.g. problem.txt), start the plansys2_terminal and enter the following:
 
 ```text
-source commands.txt
+source problem.txt
 ```
 Or use a custom controller:
 
 ```bash
-source $HOME/dev_ws/install/setup.bash
+source install/setup.bash
 $HOME/dev_ws/build/plansys2_bt_example/assemble_controller_node
 ```
 NOTE: this problem domain is different from example entered in the plansys2 terminal.
 
 ### In terminal 4 (optional):
 
+NOTE: this is not working right now due to the upgrade to BT.CPP v4
+
 To monitor the behaviortree in Groot, type the following to connect it to the plansys2 executor:
 ```bash
 $HOME/dev_ws/build/groot/Groot --mode monitor --publisher_port 2666 --server_port 2667 --autoconnect

diff --git a/plansys2_bt_example/behavior_trees_xml/move.xml b/plansys2_bt_example/behavior_trees_xml/move.xml
@@ -1,7 +1,7 @@
-<root main_tree_to_execute = "MainTree" >
+<root BTCPP_format="4" main_tree_to_execute = "MainTree" >
     <BehaviorTree ID="MainTree">
        <Sequence name="root_sequence">
-           <Move    name="move" goal="${arg2}"/>
+           <Move    name="move" goal="{arg2}" />
        </Sequence>
     </BehaviorTree>
 </root>
diff --git a/plansys2_bt_example/behavior_trees_xml/recharge.xml b/plansys2_bt_example/behavior_trees_xml/recharge.xml
@@ -1,4 +1,4 @@
-<root main_tree_to_execute = "MainTree" >
+<root BTCPP_format="4" main_tree_to_execute = "MainTree" >
     <BehaviorTree ID="MainTree">
        <Sequence name="root_sequence">
            <Recharge    name="recharge"/>

diff --git a/plansys2_bt_example/behavior_trees_xml/transport.xml b/plansys2_bt_example/behavior_trees_xml/transport.xml
@@ -1,10 +1,10 @@
-<root main_tree_to_execute = "MainTree" >
+<root BTCPP_format="4" main_tree_to_execute = "MainTree" >
     <BehaviorTree ID="MainTree">
        <Sequence name="root_sequence">
            <OpenGripper    name="open_gripper"/>
            <ApproachObject name="approach_object"/>
            <CloseGripper   name="close_gripper"/>
-           <Move    name="move" goal="${arg3}"/>
+           <Move    name="move" goal="{arg3}"/>
            <OpenGripper    name="open_gripper"/>
        </Sequence>
     </BehaviorTree>

diff --git a/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/ApproachObject.hpp b/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/ApproachObject.hpp
@@ -17,8 +17,8 @@
 
 #include <string>
 
-#include "behaviortree_cpp_v3/behavior_tree.h"
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/behavior_tree.h"
+#include "behaviortree_cpp/bt_factory.h"
 
 namespace plansys2_bt_example
 {

diff --git a/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/CloseGripper.hpp b/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/CloseGripper.hpp
@@ -17,8 +17,8 @@
 
 #include <string>
 
-#include "behaviortree_cpp_v3/behavior_tree.h"
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/behavior_tree.h"
+#include "behaviortree_cpp/bt_factory.h"
 
 namespace plansys2_bt_example
 {

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
@@ -22,8 +22,8 @@
 #include "nav2_msgs/action/navigate_to_pose.hpp"
 
 #include "plansys2_bt_actions/BTActionNode.hpp"
-#include "behaviortree_cpp_v3/behavior_tree.h"
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/behavior_tree.h"
+#include "behaviortree_cpp/bt_factory.h"
 
 namespace plansys2_bt_tests
 {

diff --git a/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/OpenGripper.hpp b/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/OpenGripper.hpp
@@ -17,8 +17,8 @@
 
 #include <string>
 
-#include "behaviortree_cpp_v3/behavior_tree.h"
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/behavior_tree.h"
+#include "behaviortree_cpp/bt_factory.h"
 
 namespace plansys2_bt_example
 {

diff --git a/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/Recharge.hpp b/plansys2_bt_example/include/plansys2_bt_example/behavior_tree_nodes/Recharge.hpp
@@ -17,8 +17,8 @@
 
 #include <string>
 
-#include "behaviortree_cpp_v3/behavior_tree.h"
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/behavior_tree.h"
+#include "behaviortree_cpp/bt_factory.h"
 
 namespace plansys2_bt_example
 {

diff --git a/plansys2_bt_example/src/behavior_tree_nodes/ApproachObject.cpp b/plansys2_bt_example/src/behavior_tree_nodes/ApproachObject.cpp
@@ -17,7 +17,7 @@
 
 #include "plansys2_bt_example/behavior_tree_nodes/ApproachObject.hpp"
 
-#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp/behavior_tree.h"
 
 namespace plansys2_bt_example
 {
@@ -50,7 +50,7 @@ ApproachObject::tick()
 
 }  // namespace plansys2_bt_example
 
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/bt_factory.h"
 BT_REGISTER_NODES(factory)
 {
   factory.registerNodeType<plansys2_bt_example::ApproachObject>("ApproachObject");

diff --git a/plansys2_bt_example/src/behavior_tree_nodes/CloseGripper.cpp b/plansys2_bt_example/src/behavior_tree_nodes/CloseGripper.cpp
@@ -17,7 +17,7 @@
 
 #include "plansys2_bt_example/behavior_tree_nodes/CloseGripper.hpp"
 
-#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp/behavior_tree.h"
 
 namespace plansys2_bt_example
 {
@@ -50,7 +50,7 @@ CloseGripper::tick()
 
 }  // namespace plansys2_bt_example
 
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/bt_factory.h"
 BT_REGISTER_NODES(factory)
 {
   factory.registerNodeType<plansys2_bt_example::CloseGripper>("CloseGripper");

diff --git a/plansys2_bt_example/src/behavior_tree_nodes/Move.cpp b/plansys2_bt_example/src/behavior_tree_nodes/Move.cpp
@@ -22,7 +22,7 @@
 #include "geometry_msgs/msg/pose2_d.hpp"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 
-#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp/behavior_tree.h"
 
 #include "rclcpp_lifecycle/lifecycle_node.hpp"
 
@@ -36,7 +36,9 @@ Move::Move(
 : plansys2::BtActionNode<nav2_msgs::action::NavigateToPose>(xml_tag_name, action_name, conf)
 {
   rclcpp_lifecycle::LifecycleNode::SharedPtr node;
-  config().blackboard->get("node", node);
+  if (!config().blackboard->get("node", node)) {
+    RCLCPP_ERROR(node_->get_logger(), "Failed to get 'node' from the blackboard");
+  }
 
   try {
     node->declare_parameter<std::vector<std::string>>("waypoints");
@@ -76,7 +78,9 @@ Move::on_tick()
 {
   if (status() == BT::NodeStatus::IDLE) {
     rclcpp_lifecycle::LifecycleNode::SharedPtr node;
-    config().blackboard->get("node", node);
+    if (!config().blackboard->get("node", node)) {
+      RCLCPP_ERROR(node_->get_logger(), "Failed to get 'node' from the blackboard");
+    }
 
     std::string goal;
     getInput<std::string>("goal", goal);
@@ -112,7 +116,7 @@ Move::on_success()
 
 }  // namespace plansys2_bt_tests
 
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/bt_factory.h"
 BT_REGISTER_NODES(factory)
 {
   BT::NodeBuilder builder =

diff --git a/plansys2_bt_example/src/behavior_tree_nodes/OpenGripper.cpp b/plansys2_bt_example/src/behavior_tree_nodes/OpenGripper.cpp
@@ -17,7 +17,7 @@
 
 #include "plansys2_bt_example/behavior_tree_nodes/OpenGripper.hpp"
 
-#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp/behavior_tree.h"
 
 namespace plansys2_bt_example
 {
@@ -50,7 +50,7 @@ OpenGripper::tick()
 
 }  // namespace plansys2_bt_example
 
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/bt_factory.h"
 BT_REGISTER_NODES(factory)
 {
   factory.registerNodeType<plansys2_bt_example::OpenGripper>("OpenGripper");

diff --git a/plansys2_bt_example/src/behavior_tree_nodes/Recharge.cpp b/plansys2_bt_example/src/behavior_tree_nodes/Recharge.cpp
@@ -17,7 +17,7 @@
 
 #include "plansys2_bt_example/behavior_tree_nodes/Recharge.hpp"
 
-#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp/behavior_tree.h"
 
 namespace plansys2_bt_example
 {
@@ -50,7 +50,7 @@ Recharge::tick()
 
 }  // namespace plansys2_bt_example
 
-#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp/bt_factory.h"
 BT_REGISTER_NODES(factory)
 {
   factory.registerNodeType<plansys2_bt_example::Recharge>("Recharge");