WIP: BT Implementation

docker/world_modeling/world_modeling.Dockerfile

@@ -15,6 +15,11 @@ COPY src/wato_msgs/world_modeling_msgs world_modeling_msgs
 
 RUN git clone --depth 1 https://github.com/carla-simulator/ros-carla-msgs.git --branch 1.3.0 carla_msgs
 
+# Install behaviortree_ros2 from source
+RUN git clone --branch humble --single-branch https://github.com/BehaviorTree/BehaviorTree.ROS2.git && \
+  cd BehaviorTree.ROS2 && \
+  git checkout 7a6ace6424adaa81737b95d20d81e1e8c0782ed7
+
 # Update CONTRIBUTING.md to pass copyright test
 COPY src/wato_msgs/simulation/mit_contributing.txt ${AMENT_WS}/src/carla_msgs/CONTRIBUTING.md
 

src/wato_msgs/world_modeling_msgs/srv/BehaviourTreeInfo.srv

@@ -3,4 +3,4 @@ geometry_msgs/Point current_point
 geometry_msgs/Point goal_point
 Lanelet current_lanelet
 Lanelet goal_lanelet
-Lanelet route_list
+LaneletPath route_list

src/world_modeling/behaviour/CMakeLists.txt

@@ -0,0 +1,85 @@
+# Copyright (c) 2025-present WATonomous. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+cmake_minimum_required(VERSION 3.10)
+project(behaviour)
+
+# Set compiler to use C++ 17 standard
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Search for dependencies required for building this package
+find_package(ament_cmake REQUIRED) # ROS2 build tool
+find_package(rclcpp REQUIRED)      # ROS2 C++ package
+find_package(ament_index_cpp REQUIRED) # ROS2 package index helpers
+find_package(std_msgs REQUIRED)    # ROS2 Messages
+find_package(std_srvs REQUIRED)    # ROS2 Services
+find_package(rclcpp_action REQUIRED) # ROS2 Action Library
+find_package(behaviortree_cpp_v3 REQUIRED) # Core BehaviorTree.CPP library
+find_package(behaviortree_ros2 REQUIRED) # ROS2 BehaviorTree Library
+find_package(btcpp_ros2_interfaces REQUIRED) # ROS2 BehaviorTree Messages
+
+# Compiles source files into a library
+add_library(behaviour_lib
+  src/behaviour_core.cpp
+  src/bt_node_utils.cpp
+  src/bt_nodes.cpp
+)
+# Indicate to compiler where to search for header files
+target_include_directories(behaviour_lib
+  PUBLIC
+  include)
+
+ament_target_dependencies(behaviour_lib
+  rclcpp
+  std_msgs
+  std_srvs
+  rclcpp_action
+  behaviortree_cpp_v3
+  behaviortree_ros2
+  btcpp_ros2_interfaces
+)
+
+# Create ROS2 node executable from source files
+add_executable(behaviour_node src/behaviour_node.cpp)
+
+# Link to the previously built library to access behaviour classes and methods
+target_link_libraries(behaviour_node behaviour_lib)
+ament_target_dependencies(behaviour_node
+  rclcpp
+  ament_index_cpp
+  std_msgs
+  std_srvs
+  rclcpp_action
+  behaviortree_cpp_v3
+  behaviortree_ros2
+  btcpp_ros2_interfaces
+)
+
+# Copy executable to installation location
+install(TARGETS
+  behaviour_node
+  DESTINATION lib/${PROJECT_NAME})
+
+# Copy launch and config files to installation location
+install(DIRECTORY
+  config
+  launch
+  DESTINATION share/${PROJECT_NAME})
+
+ament_package()

src/world_modeling/behaviour/bt/xml/main_tree.xml

@@ -0,0 +1,50 @@
+<root main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <!--
+      High level behaviour tree:
+      1) Query HD map for current & goal
+      2) If we already have a valid trajectory -> Execute
+      3) Otherwise: request route, wait for it, run planner, validate, then execute
+      Blackboard keys used:
+        current_point, goal_point, planned_path, have_valid_trajectory, trajectory_ok
+    -->
+
+    <Sequence name="main_sequence">
+      <!-- Populate current and goal on the blackboard -->
+      <GetHDMapInfo current_point="{current_point}" goal_point="{goal_point}" />
+
+      <!-- Monitor traffic lights / dynamic reg elems in parallel while planning/executing -->
+      <Parallel name="monitor_and_control" success_threshold="1" failure_threshold="1">
+        <!-- optional monitor node (implement later) -->
+        <MonitorTrafficLights state="{traffic_light_state}" />
+
+        <!-- Plan or follow: try to follow existing trajectory, else plan -->
+        <Fallback name="plan_or_follow">
+          <!-- If we already have a valid trajectory, execute it -->
+          <Sequence name="follow_sequence">
+            <CheckHaveValidTrajectory have_valid="{have_valid_trajectory}" />
+            <ExecuteTrajectory path="{planned_path}" />
+          </Sequence>
+
+          <!-- Otherwise plan a new trajectory -->
+          <Sequence name="plan_sequence">
+            <!-- Request route from HD map -->
+            <RequestRoute goal_point="{goal_point}" />
+
+            <!-- Wait for the hd_map node to publish the route -->
+            <WaitForRoute timeout_ms="2000" />
+
+            <!-- Run the local planner (start=current_point, goal=goal_point) -->
+            <RunPlanner start="{current_point}" goal="{goal_point}" planned_path="{planned_path}" />
+
+            <!-- Validate path against occupancy/traffic rules -->
+            <ValidateTrajectory path="{planned_path}" ok="{trajectory_ok}" />
+
+            <!-- If valid, execute -->
+            <ExecuteTrajectory path="{planned_path}" />
+          </Sequence>
+        </Fallback>
+      </Parallel>
+    </Sequence>
+  </BehaviorTree>
+</root>

src/world_modeling/behaviour/config/params.yaml

@@ -0,0 +1,2 @@
+---
+

src/world_modeling/behaviour/include/behaviour/behaviour_core.hpp

@@ -0,0 +1,39 @@
+// Copyright (c) 2025-present WATonomous. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEHAVIOUR__BEHAVIOUR_CORE_HPP_
+#define BEHAVIOUR__BEHAVIOUR_CORE_HPP_
+
+namespace wato::world_modeling::behaviour
+{
+
+/**
+ * @class BehaviourCore
+ * @brief A boilerplate class template for BehaviourCore.
+ */
+class BehaviourCore
+{
+public:
+  /**
+   * @brief Constructor.
+   */
+  explicit BehaviourCore();
+
+private:
+  // Add private members and methods here.
+};
+
+}  // namespace wato::world_modeling::behaviour
+
+#endif  // BEHAVIOUR__BEHAVIOUR_CORE_HPP_

src/world_modeling/behaviour/include/behaviour/behaviour_node.hpp

@@ -0,0 +1,36 @@
+// Copyright (c) 2025-present WATonomous. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEHAVIOUR__BEHAVIOUR_NODE_HPP_
+#define BEHAVIOUR__BEHAVIOUR_NODE_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+
+/**
+ * @class BehaviourNode
+ * @brief A boilerplate class template for BehaviourNode.
+ */
+class BehaviourNode : public rclcpp::Node
+{
+public:
+  /**
+   * @brief Constructor.
+   */
+  explicit BehaviourNode();
+
+private:
+  // Add private members and methods here.
+};
+
+#endif  // BEHAVIOUR__BEHAVIOUR_NODE_HPP_

src/world_modeling/behaviour/include/behaviour/bt_node_utils.hpp

@@ -0,0 +1,32 @@
+// Copyright (c) 2025-present WATonomous. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEHAVIOUR__BT_NODE_UTILS_HPP_
+#define BEHAVIOUR__BT_NODE_UTILS_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+#include <memory>
+
+namespace wato::world_modeling::behaviour {
+
+// Set a shared rclcpp::Node instance that BT leaf nodes can access.
+// Call set_shared_node() once (from main) before BT nodes are created.
+void set_shared_node(const rclcpp::Node::SharedPtr &node);
+
+// Get the shared rclcpp::Node previously set. Returns nullptr if not set.
+rclcpp::Node::SharedPtr get_shared_node();
+
+} // namespace wato::world_modeling::behaviour
+
+#endif // BEHAVIOUR__BT_NODE_UTILS_HPP_

src/world_modeling/behaviour/include/behaviour/bt_nodes.hpp

@@ -0,0 +1,52 @@
+// Copyright (c) 2025-present WATonomous. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef BEHAVIOUR__BT_NODES_HPP_
+#define BEHAVIOUR__BT_NODES_HPP_
+
+#include <behaviortree_cpp_v3/action_node.h>
+#include <behaviortree_cpp_v3/async_action_node.h>
+#include "geometry_msgs/msg/point.hpp"
+
+namespace wato::world_modeling::behaviour
+{
+
+class GetHDMapInfo : public BT::SyncActionNode
+{
+public:
+  GetHDMapInfo(const std::string & name, const BT::NodeConfiguration & config);
+  static BT::PortsList providedPorts();
+  BT::NodeStatus tick() override;
+};
+
+class RequestRoute : public BT::SyncActionNode
+{
+public:
+  RequestRoute(const std::string & name, const BT::NodeConfiguration & config);
+  static BT::PortsList providedPorts();
+  BT::NodeStatus tick() override;
+};
+
+class WaitForRoute : public BT::AsyncActionNode
+{
+public:
+  WaitForRoute(const std::string & name, const BT::NodeConfiguration & config);
+  static BT::PortsList providedPorts();
+  BT::NodeStatus tick() override;
+  void halt() override;
+};
+
+}  // namespace wato::world_modeling::behaviour
+
+#endif  // BEHAVIOUR__BT_NODES_HPP_

src/world_modeling/behaviour/launch/behaviour.launch.py

@@ -0,0 +1,44 @@
+# Copyright (c) 2025-present WATonomous. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    """Launch behaviour node."""
+    behaviour_pkg_prefix = get_package_share_directory("behaviour")
+    behaviour_param_file = os.path.join(
+        behaviour_pkg_prefix, "config", "params.yaml"
+    )
+
+    behaviour_param = DeclareLaunchArgument(
+        "behaviour_param_file",
+        default_value=behaviour_param_file,
+        description="Path to config file for behaviour node",
+    )
+
+    config_file = LaunchConfiguration("behaviour_param_file")
+
+    behaviour = Node(
+        package="behaviour",
+        executable="behaviour_node",
+        parameters=[config_file],
+    )
+
+    return LaunchDescription([behaviour_param, behaviour])

src/world_modeling/behaviour/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>behaviour</name>
+  <version>0.0.0</version>
+  <description>ROS2 Package for Behaviour Module</description>
+
+  <maintainer email="[email protected]">WATonomous</maintainer>
+  <license>Apache 2.0</license>
+
+  <!--https://www.ros.org/reps/rep-0149.html#dependency-tags-->
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>behaviortree_ros2</depend>
+  <depend>btcpp_ros2_interfaces</depend>
+
+  <test_depend>ament_cmake_gtest</test_depend>
+
+  <!--https://www.ros.org/reps/rep-0149.html#export-->
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>