Hardcoding values on a Behaviour Tree

[estebanatwork]

Hi everyone,

Do you know if its possible to hardcode values on a Behaviour Tree? I would like to specify a pose directly instead of using {goal} in <ComputePathToPose> for example.

I tried things like: <ComputePathToPose goal=“{x: 2.0, y: 3.0, yaw: 0.0}” path=“{path}”/>

But it didn’t work. Any ideas? Thanks in advance.

[rzegers]

Hi @estebanatwork,

Not 100% sure, but you could try to send a hardcoded pose value as a goal by creating a custom Action node that receives the pose directly as input and then uses it as the goal for the <ComputePathToPose> action.

I would suggest to first create a custom Action that can accept a pose as input. In the Behavior Tree XML notation, it would look like this:

<BehaviorTree>
  <Root>
    <Sequence>
      <HardcodedGoalActionNode pose="{x: 2.0, y: 3.0, yaw: 0.0}" goal="{goal}" />
      <ComputePathToPose goal="{goal}" path="{path}" />
    </Sequence>
  </Root>
</BehaviorTree>

Next try to implement the custom Action node in C++, something like this:

// hardcode_goal_action_node.hpp
#pragma once

#include <behaviortree_cpp_v3/behavior_tree.h>
#include <geometry_msgs/msg/pose_stamped.hpp>

class HardcodedGoalActionNode : public BT::SyncActionNode
{
public:
  HardcodedGoalActionNode(const std::string& name, const BT::NodeConfiguration& config)
    : BT::SyncActionNode(name, config)
  {
  }

  static BT::PortsList providedPorts()
  {
    return { BT::InputPort<geometry_msgs::msg::PoseStamped>("pose") };
  }

  // This method will be executed when the node is triggered
  BT::NodeStatus tick() override
  {
    geometry_msgs::msg::PoseStamped pose;
    if (!getInput("pose", pose))
    {
      throw BT::RuntimeError("Missing parameter [pose] in HardcodedGoalActionNode");
    }

    // Forward the pose value as the goal
    setOutput("goal", pose);
    return BT::NodeStatus::SUCCESS;
  }
};

Finally you could create a ROS2 Node to instantiate the Behavior Tree and register the custom Action node:

// main.cpp
#include <behaviortree_cpp_v3/bt_factory.h>
#include <behaviortree_cpp_v3/loggers/bt_cout_logger.h>
#include <rclcpp/rclcpp.hpp>
#include "hardcode_goal_action_node.hpp"

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  auto node = std::make_shared<rclcpp::Node>("bt_ros2_node");

  // Register custom action node
  BT::BehaviorTreeFactory factory;
  factory.registerNodeType<HardcodedGoalActionNode>("HardcodedGoalAction");

  // Load the Behavior Tree from file
  BT::Tree tree = factory.createTreeFromFile("path/to/your/behavior_tree.xml");

  // Print the Behavior Tree graph
  BT::StdCoutLogger logger_cout(tree);

  rclcpp::Rate loop_rate(10);
  while (rclcpp::ok())
  {
    // Execute one tick of the Behavior Tree
    tree.rootNode()->executeTick();

    rclcpp::spin_some(node);
    loop_rate.sleep();
  }

  rclcpp::shutdown();
  return 0;
}

The above ROS2 Node should execute Behavior Tree and trigger the custom node to set the pose as the goal. I haven’t tried this myself but you could give it a try and report back if it works. Good luck!

Roberto