Initial Commit

mg_bt/CMakeLists.txt

@@ -0,0 +1,68 @@
+cmake_minimum_required(VERSION 3.8)
+project(mg_bt)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(behaviortree_cpp REQUIRED)
+find_package(behaviortree_ros2 REQUIRED)
+find_package(btcpp_ros2_interfaces REQUIRED)
+find_package(mg_msgs REQUIRED)
+
+set(PACKAGE_DEPS
+  rclcpp
+  behaviortree_cpp
+  behaviortree_ros2
+  btcpp_ros2_interfaces
+  mg_msgs
+)
+
+
+add_executable(mg_bt_executor 
+  src/mg_bt_executor.cpp
+  src/MoveStraightBehavior.cpp)
+ament_target_dependencies(mg_bt_executor ${PACKAGE_DEPS})
+
+target_include_directories(
+  mg_bt_executor
+  PRIVATE
+  include
+)
+
+install( TARGETS
+  mg_bt_executor
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY
+  behavior_tree_nodes
+  behavior_trees
+  config
+  launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+
+ament_export_dependencies(behaviortree_ros2 btcpp_ros2_interfaces)
+
+ament_package()

mg_bt/behavior_tree_nodes/nodes.xml

@@ -0,0 +1,16 @@
+<root BTCPP_format="4">
+    <TreeNodesModel>
+        <Action ID="MoveStraightAction">
+            <input_port name="distance" type="double"/>
+            <input_port name="tolerance" type="double" default="0.100000"/>
+            <input_port name="action_name" type="std::string" default="">Action server name</input_port>
+        </Action>
+        <Action ID="SimpleBehavior">
+            <input_port name="message" type="std::string"/>
+        </Action>
+        <Action ID="SleepAction">
+            <input_port name="msec" type="unsigned int"/>
+            <input_port name="action_name" type="std::string" default="">Action server name</input_port>
+        </Action>
+    </TreeNodesModel>
+</root>

mg_bt/behavior_trees/mg_bt.btproj

@@ -0,0 +1,19 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root BTCPP_format="4" project_name="Testing">
+    <include path="untitled_1.xml"/>
+    <!-- Description of Node Models (used by Groot) -->
+    <TreeNodesModel>
+        <Action ID="MoveStraightAction">
+            <input_port name="distance" type="double"/>
+            <input_port name="tolerance" default="0.100000" type="double"/>
+            <input_port name="action_name" type="std::string">Action server name</input_port>
+        </Action>
+        <Action ID="SimpleBehavior">
+            <input_port name="message" type="std::string"/>
+        </Action>
+        <Action ID="SleepAction">
+            <input_port name="msec" type="unsigned int"/>
+            <input_port name="action_name" type="std::string">Action server name</input_port>
+        </Action>
+    </TreeNodesModel>
+</root>

mg_bt/behavior_trees/untitled_1.xml

@@ -0,0 +1,36 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<root BTCPP_format="4"
+      main_tree_to_execute="Main_strat">
+  <BehaviorTree ID="Main_strat">
+    <Repeat num_cycles="25">
+      <Sequence>
+        <MoveStraightAction distance="5"
+                            tolerance="0.001"
+                            action_name="/MoveStraight"/>
+        <Sleep msec="1000"/>
+        <SimpleBehavior message="YoYoYo Mr. White, what up?"/>
+        <MoveStraightAction distance="-5"
+                            tolerance="0.001"
+                            action_name="/MoveStraight"/>
+      </Sequence>
+    </Repeat>
+  </BehaviorTree>
+
+  <!-- Description of Node Models (used by Groot) -->
+  <TreeNodesModel>
+    <Action ID="MoveStraightAction">
+      <input_port name="distance"
+                  type="double"/>
+      <input_port name="tolerance"
+                  default="0.100000"
+                  type="double"/>
+      <input_port name="action_name"
+                  type="std::string">Action server name</input_port>
+    </Action>
+    <Action ID="SimpleBehavior">
+      <input_port name="message"
+                  type="std::string"/>
+    </Action>
+  </TreeNodesModel>
+
+</root>

mg_bt/config/mg_bt_executor_config.yaml

@@ -0,0 +1,14 @@
+bt_action_server:
+  ros__parameters:
+    action_name: "mg_bt_action_server" # Optional (defaults to `bt_action_server`)
+    tick_frequency: 100 # Optional (defaults to 100 Hz)
+    groot2_port: 8420 # Optional (defaults to 1667)
+    ros_plugins_timeout: 1000  # Optional (defaults 1000 ms)
+
+    plugins:
+      # - behaviortree_cpp/bt_plugins
+      - btcpp_ros2_samples/bt_plugins
+
+    behavior_trees:
+      - mg_bt/behavior_trees
+

mg_bt/include/mg_bt/MoveStraightBehavior.hpp

@@ -0,0 +1,30 @@
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "mg_msgs/action/move_straight.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+
+
+class MoveStraightAction : public BT::RosActionNode<mg_msgs::action::MoveStraight>
+{
+public:
+  MoveStraightAction(const std::string& name, const BT::NodeConfig& conf,
+              const BT::RosNodeParams& params)
+    : BT::RosActionNode<mg_msgs::action::MoveStraight>(name, conf, params)
+  {}
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({ 
+      BT::InputPort<double>("distance"),
+      BT::InputPort<double>("tolerance", 0.1, {}) 
+      });
+  }
+
+  bool setGoal(Goal& goal) override;
+
+  void onHalt() override;
+
+  BT::NodeStatus onResultReceived(const WrappedResult& wr) override;
+
+  virtual BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override;
+};

mg_bt/launch/mg_bt_launch.py

@@ -0,0 +1,25 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, RegisterEventHandler, TimerAction
+from launch.event_handlers import OnProcessStart
+from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+from pathlib import Path
+
+def generate_launch_description():
+
+    basedir = FindPackageShare("mg_bt")
+
+    yamlBT = PathJoinSubstitution([basedir, "config/mg_bt_executor_config.yaml"])
+
+    return LaunchDescription([
+        Node(
+            package='mg_bt',
+            executable='mg_bt_executor',
+            output="screen",
+            parameters=[yamlBT]
+        ),
+    ])
+    
+

mg_bt/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>mg_bt</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="82343504+Pimpest@users.noreply.github.com">petar</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>behaviortree_cpp</depend>
+  <depend>behaviortree_ros2</depend>
+  <depend>btcpp_ros2_interfaces</depend>
+  <depend>mg_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

mg_bt/src/MoveStraightBehavior.cpp

@@ -0,0 +1,26 @@
+#include "mg_bt/MoveStraightBehavior.hpp"
+
+
+bool MoveStraightAction::setGoal(Goal& goal) {
+    auto dist = getInput<double>("distance");
+    auto tolerance = getInput<double>("tolerance");
+    goal.distance = dist.value();
+    goal.tolerance = tolerance.value();
+    return true;
+}
+
+BT::NodeStatus MoveStraightAction::onResultReceived(const RosActionNode::WrappedResult& wr)
+{
+  return (!wr.result->error) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
+}
+
+BT::NodeStatus MoveStraightAction::onFailure(BT::ActionNodeErrorCode error)
+{
+  RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
+  return BT::NodeStatus::FAILURE;
+}
+
+void MoveStraightAction::onHalt()
+{
+  RCLCPP_INFO(logger(), "%s: onHalt", name().c_str());
+}

mg_bt/src/mg_bt_executor.cpp

@@ -0,0 +1,95 @@
+#include <behaviortree_ros2/tree_execution_server.hpp>
+#include <behaviortree_cpp/loggers/bt_cout_logger.h>
+#include <behaviortree_cpp/xml_parsing.h>
+#include <behaviortree_cpp/bt_factory.h>
+
+
+#include <memory>
+#include <iostream>
+
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "mg_bt/MoveStraightBehavior.hpp"
+
+class MgSimpleBehavior : public BT::SyncActionNode {
+public:
+    MgSimpleBehavior( const std::string& name, const BT::NodeConfig& config ) 
+        : BT::SyncActionNode(name, config) {
+    }
+
+    static BT::PortsList providedPorts()
+    {
+        return { BT::InputPort<std::string>("message") };
+    }
+    BT::NodeStatus tick() override{
+
+        std::cout << "This is an example action called " << this->name() << std::endl;
+
+        auto msg = getInput<std::string>("message");
+
+        if(!msg) {
+            std::cout << "message was not provided" << std::endl;
+        }
+        else {
+            std::cout << "The message is " << msg.value() << std::endl;
+        }
+    
+        return BT::NodeStatus::SUCCESS;
+    }
+
+};
+
+
+class MgBtExecutionServer : public BT::TreeExecutionServer {
+public:
+    MgBtExecutionServer(const rclcpp::NodeOptions options) 
+        : TreeExecutionServer(options) {
+            executeRegistration();
+            std::cout <<  BT::writeTreeNodesModelXML(factory())  << std::endl;
+        }
+
+    void registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) override {
+        factory.registerNodeType<MgSimpleBehavior>("SimpleBehavior");
+        factory.registerNodeType<MoveStraightAction>("MoveStraightAction", node());
+
+    }
+
+    void onTreeCreated(BT::Tree&  tree) override {
+        logger_cout_ = std::make_shared<BT::StdCoutLogger>(tree);
+    }
+
+    std::optional<std::string>
+    onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) override {
+        (void)status;
+        logger_cout_.reset();
+
+        if (was_cancelled) {
+            std::cout << "The tree execution was cancled" << std::endl;
+        }
+
+        return  std::nullopt;
+    }
+
+private:
+    std::shared_ptr<BT::StdCoutLogger> logger_cout_;
+};
+
+int main(int argc, const char** argv) {
+    rclcpp::init(argc,argv);   
+
+
+    rclcpp::NodeOptions options;
+    auto bt_server = std::make_shared<MgBtExecutionServer>(options);
+
+    
+    rclcpp::executors::MultiThreadedExecutor exec(rclcpp::ExecutorOptions(), 0, false, std::chrono::milliseconds(250));
+
+
+    exec.add_node(bt_server->node());
+    exec.spin();
+    exec.remove_node(bt_server->node());
+
+    rclcpp::shutdown();
+    return 0;
+}