adlink-ros / bt_ros2

Behavior Tree that runs on ROS2

BT on ROS 2

BT_ros2 is a demo of how to use Behavior Tree to control AMR. All the examples are tested on our NeuronBot simulation. If you are interested in NeuronBot simulation, please go to see the tutorials: https://github.com/Adlink-ROS/neuronbot2#bring-up-in-simulation

Build

1. git clone the repo.

mkdir -p ~/bt_ros2_ws/src
cd ~/bt_ros2_ws/src
git clone https://github.com/Adlink-ROS/BT_ros2.git

2. Install dependencies

source /opt/ros/<ROS2_DISTRO>/setup.bash
cd ~/bt_ros2_ws
rosdep install --from-paths src --ignore-src -r -y

3. Build

colcon build --symlink-install
# If you want to build with OpenVINO support, add the define
# Remember to source OpenVINO before build
colcon build --symlink-install --cmake-args -DBUILD_OPENVINO=ON

Usage

We will run Gazebo with NeuronBot2 and show simple BT examples.

Basic Example

This example makes NeuronBot2 continuously moving between the pre-defined three goals in bt_nav_mememan.xml.

• Open 1st terminal and run mememan world.

source /opt/ros/<ROS2_DISTRO>/local_setup.bash
source ~/neuronbot2_ros2_ws/install/local_setup.bash
ros2 launch neuronbot2_gazebo neuronbot2_world.launch.py world_model:=mememan_world.model

• Open 2nd terminal and run navigation.

source /opt/ros/<ROS2_DISTRO>/local_setup.bash
source ~/neuronbot2_ros2_ws/install/local_setup.bash
ros2 launch neuronbot2_nav bringup_launch.py map:=$HOME/neuronbot2_ros2_ws/src/neuronbot2/neuronbot2_nav/map/mememan.yaml open_rviz:=true use_sim_time:=true

• Open 3rd termainal and run BT.

source /opt/ros/<ROS2_DISTRO>/local_setup.bash
source ~/bt_ros2_ws/install/local_setup.bash
ros2 launch bt_ros2 bt_ros2.launch.py

Advanced Example - Interrupt

In this example (refer to bt_nav_mememan_interrupt.xml), when BT receives /interrupt_event, which is gohome, then NeuronBot2 will move to Goal_c.

• Open 1st terminal and run mememan world.

source /opt/ros/<ROS2_DISTRO>/local_setup.bash
source ~/neuronbot2_ros2_ws/install/local_setup.bash
ros2 launch neuronbot2_gazebo neuronbot2_world.launch.py world_model:=mememan_world.model

• Open 2nd terminal and run navigation.

source /opt/ros/<ROS2_DISTRO>/local_setup.bash
source ~/neuronbot2_ros2_ws/install/local_setup.bash
ros2 launch neuronbot2_nav bringup_launch.py map:=$HOME/neuronbot2_ros2_ws/src/neuronbot2/neuronbot2_nav/map/mememan.yaml open_rviz:=true use_sim_time:=true

• Open 3rd termainal and run BT.

source /opt/ros/<ROS2_DISTRO>/local_setup.bash
source ~/bt_ros2_ws/install/local_setup.bash
ros2 run bt_ros2 bt_ros2 --ros-args -p bt_xml:=$HOME/bt_ros2_ws/src/BT_ros2/bt_xml/bt_nav_mememan_interrupt.xml

• Open 4th terminal and pub interrupt event.

source /opt/ros/<ROS2_DISTRO>/local_setup.bash
ros2 topic pub -1 /interrupt_event std_msgs/msg/String data:\ \'gohome\'

Advanced Example - Take a snapshot

The demo is to show how to take snapshots with BT example. You need to install OpenVINO and its ROS 2 wrapper before using the example.

• Open 1st terminal and run OpenVINO object detection

# Source ROS 2 environment
# Setup OpenVINO environment and source wrapper
ros2 launch dynamic_vino_sample pipeline_object.launch.py

• Open 2nd terminal and run image_saver
  • If you don't have image_view, please run sudo apt install ros-${ROS_DISTRO}-image-view

# Source ROS 2 environment
ros2 run image_view image_saver image:=ros2_openvino_toolkit/image_rviz

• Open 3rd terminal and run BT

# Source ROS 2 environment
source ~/bt_ros2_ws/install/local_setup.bash
ros2 run bt_ros2 bt_ros2 --ros-args -p bt_xml:=$HOME/bt_ros2_ws/src/BT_ros2/bt_xml/bt_snapshot.xml

Advanced Example - OpenVINO object detection

This demo shows an integrated application with object detection and robot navigation. With different signal triggered by OpenVINO, the robot will navigate to certain position. The objects in the BT file are animals, which are cat, dog, cow.

• Open 1st terminal and run mememan world.

# Source ROS 2 environment
source ~/neuronbot2_ros2_ws/install/local_setup.bash
ros2 launch neuronbot2_gazebo neuronbot2_world.launch.py world_model:=mememan_world.model

• Open 2nd terminal and run OpenVINO object detection

# Source ROS 2 environment
# Setup OpenVINO environment and source wrapper
ros2 launch dynamic_vino_sample pipeline_object.launch.py

• Open 3rd terminal and run navigation.

# Source ROS 2 environment
source ~/neuronbot2_ros2_ws/install/local_setup.bash
ros2 launch neuronbot2_nav bringup_launch.py map:=$HOME/neuronbot2_ros2_ws/src/neuronbot2/neuronbot2_nav/map/mememan.yaml open_rviz:=true use_sim_time:=true

• Open 4th terminal and run BT

# Source ROS 2 environment
# Setup OpenVINO environment and source wrapper
source ~/bt_ros2_ws/install/local_setup.bash
ros2 run bt_ros2 bt_ros2 --ros-args -p bt_xml:=$HOME/bt_ros2_ws/src/BT_ros2/bt_xml/bt_openvino.xml
# An additional bt_xml file is prepared, which switch the action after trigger from navigation to teleop command.
#ros2 run bt_ros2 bt_ros2 --ros-args -p bt_xml:=$HOME/bt_ros2_ws/src/BT_ros2/bt_xml/bt_openvino_teleop.xml

Note

If you want to get the coordinate for navigation, you can run navigation2 and listen to the topic /goal_pose or open rviz2 to monitor tf.

The position and orientation should be put into BT file and the orientation value we use here is Quaternion.

Record X, Y in positoin and Z, W in orientation.

Open xml file in bt_xml and modify robot checkpoint with " X ; Y ; Z ; W " format in SetBlackboard.

If you only have Euler Angle and want to transfer to Quaternion, please refer to online tools, such as https://quaternions.online/

bt_ros2's Issues

項目中使用的包已經被ros官方停用

您好，如果您還在維護這個項目，這裡有點小問題。
當我在執行下面這條命令時，colcon提示找不到object-msgs包。
colcon build --symlink-install --cmake-args -DBUILD_OPENVINO=ON

於是我像以往一樣使用sudo apt-get install ros-foxy-object-msgs來安裝這個包時，他提示：

當我懷著疑惑查找了ros官方中是否還在使用這個包，發現它已經被停用了。下面這個圖片來自這個網址：
https://robostack.github.io/foxy.html

我在項目中找到了該問題出在哪裡，這個頭文件使用了已經停用的包。

關羽已經被停用的包，我在github上也找到了它的鏈接，但是我不會使用它，在這裡也提供給您。
https://github.com/intel/object_msgs

當然，如果項目已經不再被維護，也感謝您的閱讀，謝謝！

Update for ROS 2 Galactic

Hello,

I would like to know if there are any plans to update the repository with the ROS Galaxy version?

Right now I'm having trouble building everything without any deeper changes.

Best regards
Łukasz Janiec

Subscription callback cannot work in my project

I write a Subscription callback of ros2 in my IsHomeCondition, but it didn't work. Though I send topic /localizaiton in terminal, it didn't output log, and other things, Could you please look my code, check what problem happend?

THIS IS MY CODE.

#include <string>
#include "somo_behavior_tree/plugins/condition/is_home_condition.hpp"

namespace behavior_tree
{
    IsHomeCondition::IsHomeCondition(
        const std::string &condition_name,
        const BT::NodeConfiguration &conf)
        : BT::ConditionNode(condition_name, conf),
        is_home_(false),
        localization_topic_("/localization")
        {
          node_ = node_ = rclcpp::Node::make_shared("IsHomeCondition");
          // node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
          localization_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>(
            localization_topic_,
            rclcpp::SystemDefaultsQoS(),
            std::bind(&IsHomeCondition::localization_cb, this, std::placeholders::_1));
          //parameters declare
          std::vector<std::double_t> home_position={0,0,0};
          node_->declare_parameter<std::vector<std::double_t>>("home_position",home_position);
          node_->declare_parameter<std::double_t>("check_threshold", 0.5);
          check_threshold_ = node_->get_parameter("check_threshold").as_double();

          home_position_.position.x = node_->get_parameter("home_position").as_double_array()[0];
          home_position_.position.y = node_->get_parameter("home_position").as_double_array()[1];
          std::cout << "Init completed" << std::endl;

        }
    IsHomeCondition::~IsHomeCondition()
    {
        RCLCPP_DEBUG(node_->get_logger(), "Shutting down IsHomeCondition BT node");

    }



    BT::NodeStatus IsHomeCondition::tick()
    {
      std::cout << "home flag is" <<is_home_ << std::endl;
      if (is_home_) {
      std::cout << "SOMO is in not home"<< std::endl;
        return BT::NodeStatus::SUCCESS;

      }   
      std::cout << "SOMO is in not home"<< std::endl;
      return BT::NodeStatus::FAILURE; 
    }


    void IsHomeCondition::localization_cb(geometry_msgs::msg::PoseStamped::SharedPtr msg)
    {
      std::cout << "In localization callback" << std::endl;
      current_position_ = msg->pose;

        is_home_ = isHome();

    }

    bool IsHomeCondition::isHome(){
      return utils->check_distance(current_position_,home_position_,check_threshold_);

    }
        
}


#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory)
{
  factory.registerNodeType<behavior_tree::IsHomeCondition>("IsHomeCondition");
}