diff --git a/bt_bumpgo/CMakeLists.txt b/bt_bumpgo/CMakeLists.txt
new file mode 100644
index 0000000..8c0d7c8
--- /dev/null
+++ b/bt_bumpgo/CMakeLists.txt
@@ -0,0 +1,88 @@
+cmake_minimum_required(VERSION 3.5)
+project(bt_bumpgo)
+
+set(CMAKE_CONFIG_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_LIST_DIR}/cmake")
+list(APPEND CMAKE_MODULE_PATH "${CMAKE_CONFIG_PATH}")
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(behaviortree_cpp_v3 REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(ament_index_cpp REQUIRED)
+
+find_package(ZMQ)
+if(ZMQ_FOUND)
+ message(STATUS "ZeroMQ found.")
+ add_definitions(-DZMQ_FOUND)
+else()
+ message(WARNING "ZeroMQ NOT found. Not including PublisherZMQ.")
+endif()
+
+set(CMAKE_CXX_STANDARD 17)
+
+set(dependencies
+ rclcpp
+ behaviortree_cpp_v3
+ sensor_msgs
+ geometry_msgs
+ ament_index_cpp
+)
+
+include_directories(include ${ZMQ_INCLUDE_DIRS})
+
+add_library(forward_bt_node SHARED src/bt_bumpgo/Forward.cpp)
+add_library(back_bt_node SHARED src/bt_bumpgo/Back.cpp)
+add_library(turn_bt_node SHARED src/bt_bumpgo/Turn.cpp)
+add_library(is_obstacle_bt_node SHARED src/bt_bumpgo/IsObstacle.cpp)
+
+list(APPEND plugin_libs
+ forward_bt_node
+ back_bt_node
+ turn_bt_node
+ is_obstacle_bt_node
+)
+
+foreach(bt_plugin ${plugin_libs})
+ ament_target_dependencies(${bt_plugin} ${dependencies})
+ target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT)
+endforeach()
+
+add_executable(bt_bumpgo src/bt_bumpgo_main.cpp)
+ament_target_dependencies(bt_bumpgo ${dependencies})
+target_link_libraries(bt_bumpgo ${ZMQ_LIBRARIES})
+
+install(TARGETS
+ ${plugin_libs}
+ bt_bumpgo
+ ARCHIVE DESTINATION lib
+ LIBRARY DESTINATION lib
+ RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+ DESTINATION include/
+)
+
+install(DIRECTORY behavior_tree_xml
+ launch
+ DESTINATION share/${PROJECT_NAME}
+)
+
+
+if(BUILD_TESTING)
+ find_package(ament_lint_auto REQUIRED)
+ ament_lint_auto_find_test_dependencies()
+
+ set(ament_cmake_cpplint_FOUND TRUE)
+ ament_lint_auto_find_test_dependencies()
+
+ find_package(ament_cmake_gtest REQUIRED)
+
+ add_subdirectory(tests)
+endif()
+
+ament_export_include_directories(include)
+ament_export_dependencies(${dependencies})
+
+ament_package()
diff --git a/bt_bumpgo/behavior_tree_xml/bumpgo.xml b/bt_bumpgo/behavior_tree_xml/bumpgo.xml
new file mode 100644
index 0000000..1efe004
--- /dev/null
+++ b/bt_bumpgo/behavior_tree_xml/bumpgo.xml
@@ -0,0 +1,29 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ Distance to consider obstacle
+
+
+
+
+
+
diff --git a/bt_bumpgo/cmake/FindZMQ.cmake b/bt_bumpgo/cmake/FindZMQ.cmake
new file mode 100644
index 0000000..a7ac39e
--- /dev/null
+++ b/bt_bumpgo/cmake/FindZMQ.cmake
@@ -0,0 +1,86 @@
+# - Try to find ZMQ
+# Once done this will define
+#
+# ZMQ_FOUND - system has ZMQ
+# ZMQ_INCLUDE_DIRS - the ZMQ include directory
+# ZMQ_LIBRARIES - Link these to use ZMQ
+# ZMQ_DEFINITIONS - Compiler switches required for using ZMQ
+#
+# Copyright (c) 2011 Lee Hambley
+#
+# All rights reserved.
+#
+# Software License Agreement (BSD License 2.0)
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of {copyright_holder} nor the names of its
+# contributors may be used to endorse or promote products derived
+# from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+
+if(ZMQ_LIBRARIES AND ZMQ_INCLUDE_DIRS)
+ # in cache already
+ set(ZMQ_FOUND TRUE)
+else()
+
+ find_path(ZMQ_INCLUDE_DIR
+ NAMES
+ zmq.h
+ PATHS
+ /usr/include
+ /usr/local/include
+ /opt/local/include
+ /sw/include
+ )
+
+ find_library(ZMQ_LIBRARY
+ NAMES
+ zmq
+ PATHS
+ /usr/lib
+ /usr/local/lib
+ /opt/local/lib
+ /sw/lib
+ )
+
+ set(ZMQ_INCLUDE_DIRS
+ ${ZMQ_INCLUDE_DIR}
+ )
+
+ if(ZMQ_LIBRARY)
+ set(ZMQ_LIBRARIES
+ ${ZMQ_LIBRARIES}
+ ${ZMQ_LIBRARY}
+ )
+ endif()
+
+ include(FindPackageHandleStandardArgs)
+ find_package_handle_standard_args(ZMQ DEFAULT_MSG ZMQ_LIBRARIES ZMQ_INCLUDE_DIRS)
+
+ # show the ZMQ_INCLUDE_DIRS and ZMQ_LIBRARIES variables only in the advanced view
+ mark_as_advanced(ZMQ_INCLUDE_DIRS ZMQ_LIBRARIES)
+
+endif()
+
diff --git a/bt_bumpgo/include/bt_bumpgo/Back.hpp b/bt_bumpgo/include/bt_bumpgo/Back.hpp
new file mode 100644
index 0000000..b3c60ff
--- /dev/null
+++ b/bt_bumpgo/include/bt_bumpgo/Back.hpp
@@ -0,0 +1,52 @@
+// Copyright 2021 Intelligent Robotics Lab
+//
+// 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 BT_BUMPGO__BACK_HPP_
+#define BT_BUMPGO__BACK_HPP_
+
+#include
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "geometry_msgs/msg/twist.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace bt_bumpgo
+{
+
+class Back : public BT::ActionNodeBase
+{
+public:
+ explicit Back(
+ const std::string & xml_tag_name,
+ const BT::NodeConfiguration & conf);
+
+ void halt();
+ BT::NodeStatus tick();
+
+ static BT::PortsList providedPorts()
+ {
+ return BT::PortsList({});
+ }
+
+private:
+ rclcpp::Node::SharedPtr node_;
+ rclcpp::Time start_time_;
+ rclcpp::Publisher::SharedPtr vel_pub_;
+};
+
+} // namespace bt_bumpgo
+
+#endif // BT_BUMPGO__BACK_HPP_
diff --git a/bt_bumpgo/include/bt_bumpgo/Forward.hpp b/bt_bumpgo/include/bt_bumpgo/Forward.hpp
new file mode 100644
index 0000000..d17c92f
--- /dev/null
+++ b/bt_bumpgo/include/bt_bumpgo/Forward.hpp
@@ -0,0 +1,51 @@
+// Copyright 2021 Intelligent Robotics Lab
+//
+// 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 BT_BUMPGO__FORWARD_HPP_
+#define BT_BUMPGO__FORWARD_HPP_
+
+#include
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "geometry_msgs/msg/twist.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace bt_bumpgo
+{
+
+class Forward : public BT::ActionNodeBase
+{
+public:
+ explicit Forward(
+ const std::string & xml_tag_name,
+ const BT::NodeConfiguration & conf);
+
+ void halt() {}
+ BT::NodeStatus tick();
+
+ static BT::PortsList providedPorts()
+ {
+ return BT::PortsList({});
+ }
+
+private:
+ rclcpp::Node::SharedPtr node_;
+ rclcpp::Publisher::SharedPtr vel_pub_;
+};
+
+} // namespace bt_bumpgo
+
+#endif // BT_BUMPGO__FORWARD_HPP_
diff --git a/bt_bumpgo/include/bt_bumpgo/IsObstacle.hpp b/bt_bumpgo/include/bt_bumpgo/IsObstacle.hpp
new file mode 100644
index 0000000..6ffdbd9
--- /dev/null
+++ b/bt_bumpgo/include/bt_bumpgo/IsObstacle.hpp
@@ -0,0 +1,57 @@
+// Copyright 2021 Intelligent Robotics Lab
+//
+// 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 BT_BUMPGO__ISOBSTACLE_HPP_
+#define BT_BUMPGO__ISOBSTACLE_HPP_
+
+#include
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "sensor_msgs/msg/laser_scan.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace bt_bumpgo
+{
+
+class IsObstacle : public BT::ConditionNode
+{
+public:
+ explicit IsObstacle(
+ const std::string & xml_tag_name,
+ const BT::NodeConfiguration & conf);
+
+ BT::NodeStatus tick();
+
+ static BT::PortsList providedPorts()
+ {
+ return BT::PortsList(
+ {
+ BT::InputPort("distance")
+ });
+ }
+
+ void laser_callback(sensor_msgs::msg::LaserScan::UniquePtr msg);
+
+private:
+ rclcpp::Node::SharedPtr node_;
+ rclcpp::Time last_reading_time_;
+ rclcpp::Subscription::SharedPtr laser_sub_;
+ sensor_msgs::msg::LaserScan::UniquePtr last_scan_;
+};
+
+} // namespace bt_bumpgo
+
+#endif // BT_BUMPGO__ISOBSTACLE_HPP_
diff --git a/bt_bumpgo/include/bt_bumpgo/Turn.hpp b/bt_bumpgo/include/bt_bumpgo/Turn.hpp
new file mode 100644
index 0000000..25c3c43
--- /dev/null
+++ b/bt_bumpgo/include/bt_bumpgo/Turn.hpp
@@ -0,0 +1,52 @@
+// Copyright 2021 Intelligent Robotics Lab
+//
+// 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 BT_BUMPGO__TURN_HPP_
+#define BT_BUMPGO__TURN_HPP_
+
+#include
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "geometry_msgs/msg/twist.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace bt_bumpgo
+{
+
+class Turn : public BT::ActionNodeBase
+{
+public:
+ explicit Turn(
+ const std::string & xml_tag_name,
+ const BT::NodeConfiguration & conf);
+
+ void halt();
+ BT::NodeStatus tick();
+
+ static BT::PortsList providedPorts()
+ {
+ return BT::PortsList({});
+ }
+
+private:
+ rclcpp::Node::SharedPtr node_;
+ rclcpp::Time start_time_;
+ rclcpp::Publisher::SharedPtr vel_pub_;
+};
+
+} // namespace bt_bumpgo
+
+#endif // BT_BUMPGO__TURN_HPP_
diff --git a/bt_bumpgo/launch/bumpgo.launch.py b/bt_bumpgo/launch/bumpgo.launch.py
new file mode 100644
index 0000000..d7538c9
--- /dev/null
+++ b/bt_bumpgo/launch/bumpgo.launch.py
@@ -0,0 +1,37 @@
+# Copyright 2024 Intelligent Robotics Lab
+#
+# 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.
+
+
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+
+ bumpgo_cmd = Node(
+ package='bt_bumpgo',
+ executable='bt_bumpgo',
+ name='bt_bumpgo',
+ output='screen',
+ remappings=[
+ ('/output_vel', '/cmd_vel'),
+ ('/input_scan', '/scan')
+ ],
+ parameters=[
+ {'use_sim_time': True}
+ ])
+
+ ld = LaunchDescription()
+ ld.add_action(bumpgo_cmd)
+ return ld
diff --git a/bt_bumpgo/package.xml b/bt_bumpgo/package.xml
new file mode 100644
index 0000000..acb71e6
--- /dev/null
+++ b/bt_bumpgo/package.xml
@@ -0,0 +1,26 @@
+
+
+
+ bt_bumpgo
+ 0.2.0
+ Mastering Behavior Trees in ROS2
+ fmrico
+ Apache License, Version 2.0
+
+ ament_cmake
+
+ rclcpp
+ behaviortree_cpp_v3
+ sensor_msgs
+ geometry_msgs
+ libzmq3-dev
+ ament_index_cpp
+
+ ament_lint_auto
+ ament_lint_common
+ ament_cmake_gtest
+
+
+ ament_cmake
+
+
diff --git a/bt_bumpgo/src/bt_bumpgo/Back.cpp b/bt_bumpgo/src/bt_bumpgo/Back.cpp
new file mode 100644
index 0000000..256125c
--- /dev/null
+++ b/bt_bumpgo/src/bt_bumpgo/Back.cpp
@@ -0,0 +1,72 @@
+// Copyright 2021 Intelligent Robotics Lab
+//
+// 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.
+
+#include
+#include
+
+#include "bt_bumpgo/Back.hpp"
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+
+#include "geometry_msgs/msg/twist.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace bt_bumpgo
+{
+
+using namespace std::chrono_literals;
+
+Back::Back(
+ const std::string & xml_tag_name,
+ const BT::NodeConfiguration & conf)
+: BT::ActionNodeBase(xml_tag_name, conf)
+{
+ config().blackboard->get("node", node_);
+
+ vel_pub_ = node_->create_publisher("/output_vel", 100);
+}
+
+void
+Back::halt()
+{
+}
+
+BT::NodeStatus
+Back::tick()
+{
+ if (status() == BT::NodeStatus::IDLE) {
+ start_time_ = node_->now();
+ }
+
+ geometry_msgs::msg::Twist vel_msgs;
+ vel_msgs.linear.x = -0.3;
+ vel_pub_->publish(vel_msgs);
+
+ auto elapsed = node_->now() - start_time_;
+
+ if (elapsed < 3s) {
+ RCLCPP_INFO(node_->get_logger(), "Moving back");
+ return BT::NodeStatus::RUNNING;
+ } else {
+ return BT::NodeStatus::SUCCESS;
+ }
+}
+
+} // namespace bt_bumpgo
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+ factory.registerNodeType("Back");
+}
diff --git a/bt_bumpgo/src/bt_bumpgo/Forward.cpp b/bt_bumpgo/src/bt_bumpgo/Forward.cpp
new file mode 100644
index 0000000..576e233
--- /dev/null
+++ b/bt_bumpgo/src/bt_bumpgo/Forward.cpp
@@ -0,0 +1,57 @@
+// Copyright 2021 Intelligent Robotics Lab
+//
+// 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.
+
+#include
+#include
+
+#include "bt_bumpgo/Forward.hpp"
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+
+#include "geometry_msgs/msg/twist.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace bt_bumpgo
+{
+
+using namespace std::chrono_literals;
+
+Forward::Forward(
+ const std::string & xml_tag_name,
+ const BT::NodeConfiguration & conf)
+: BT::ActionNodeBase(xml_tag_name, conf)
+{
+ config().blackboard->get("node", node_);
+
+ vel_pub_ = node_->create_publisher("/output_vel", 100);
+}
+
+BT::NodeStatus
+Forward::tick()
+{
+ geometry_msgs::msg::Twist vel_msgs;
+ vel_msgs.linear.x = 0.3;
+ vel_pub_->publish(vel_msgs);
+
+ RCLCPP_INFO(node_->get_logger(), "Moving forward");
+ return BT::NodeStatus::RUNNING;
+}
+
+} // namespace bt_bumpgo
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+ factory.registerNodeType("Forward");
+}
diff --git a/bt_bumpgo/src/bt_bumpgo/IsObstacle.cpp b/bt_bumpgo/src/bt_bumpgo/IsObstacle.cpp
new file mode 100644
index 0000000..96c894a
--- /dev/null
+++ b/bt_bumpgo/src/bt_bumpgo/IsObstacle.cpp
@@ -0,0 +1,89 @@
+// Copyright 2021 Intelligent Robotics Lab
+//
+// 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.
+
+#include
+#include
+
+#include "bt_bumpgo/IsObstacle.hpp"
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+
+#include "sensor_msgs/msg/laser_scan.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace bt_bumpgo
+{
+
+using namespace std::chrono_literals;
+using namespace std::placeholders;
+
+IsObstacle::IsObstacle(
+ const std::string & xml_tag_name,
+ const BT::NodeConfiguration & conf)
+: BT::ConditionNode(xml_tag_name, conf)
+{
+ config().blackboard->get("node", node_);
+
+ laser_sub_ = node_->create_subscription(
+ "/input_scan", 100, std::bind(&IsObstacle::laser_callback, this, _1));
+
+ last_reading_time_ = node_->now();
+}
+
+void
+IsObstacle::laser_callback(sensor_msgs::msg::LaserScan::UniquePtr msg)
+{
+ last_scan_ = std::move(msg);
+}
+
+BT::NodeStatus
+IsObstacle::tick()
+{
+ if (last_scan_ == nullptr) {
+ return BT::NodeStatus::FAILURE;
+ }
+
+ double distance = 1.0;
+ getInput("distance", distance);
+ for (size_t pos = last_scan_->ranges.size() / 2 - 50;
+ pos < last_scan_->ranges.size() / 2 + 50;
+ pos++)
+ {
+ if (last_scan_->ranges[pos] < distance) {
+ RCLCPP_INFO(
+ node_->get_logger(), "Obstacle detected at %.2f meters (dist=%.2f, pos=%zu)",
+ last_scan_->ranges[pos], distance, pos);
+ return BT::NodeStatus::SUCCESS;
+ }
+ }
+ RCLCPP_INFO(node_->get_logger(), "No obstacle detected");
+ return BT::NodeStatus::FAILURE;
+ // if (last_scan_->ranges[last_scan_->ranges.size() / 2] < distance) {
+ // RCLCPP_INFO(node_->get_logger(), "Obstacle detected at %f meters",
+ // last_scan_->ranges[last_scan_->ranges.size() / 2]);
+ // return BT::NodeStatus::SUCCESS;
+ // } else {
+ // RCLCPP_INFO(node_->get_logger(), "Closest obstacle is at %f meters",
+ // last_scan_->ranges[last_scan_->ranges.size() / 2]);
+ // return BT::NodeStatus::FAILURE;
+ // }
+}
+
+} // namespace bt_bumpgo
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+ factory.registerNodeType("IsObstacle");
+}
diff --git a/bt_bumpgo/src/bt_bumpgo/Turn.cpp b/bt_bumpgo/src/bt_bumpgo/Turn.cpp
new file mode 100644
index 0000000..28bd2dc
--- /dev/null
+++ b/bt_bumpgo/src/bt_bumpgo/Turn.cpp
@@ -0,0 +1,72 @@
+// Copyright 2021 Intelligent Robotics Lab
+//
+// 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.
+
+#include
+#include
+
+#include "bt_bumpgo/Turn.hpp"
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+
+#include "geometry_msgs/msg/twist.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace bt_bumpgo
+{
+
+using namespace std::chrono_literals;
+
+Turn::Turn(
+ const std::string & xml_tag_name,
+ const BT::NodeConfiguration & conf)
+: BT::ActionNodeBase(xml_tag_name, conf)
+{
+ config().blackboard->get("node", node_);
+
+ vel_pub_ = node_->create_publisher("/output_vel", 100);
+}
+
+void
+Turn::halt()
+{
+}
+
+BT::NodeStatus
+Turn::tick()
+{
+ if (status() == BT::NodeStatus::IDLE) {
+ start_time_ = node_->now();
+ }
+
+ geometry_msgs::msg::Twist vel_msgs;
+ vel_msgs.angular.z = 0.5;
+ vel_pub_->publish(vel_msgs);
+
+ auto elapsed = node_->now() - start_time_;
+
+ if (elapsed < 3s) {
+ RCLCPP_INFO(node_->get_logger(), "Turning");
+ return BT::NodeStatus::RUNNING;
+ } else {
+ return BT::NodeStatus::SUCCESS;
+ }
+}
+
+} // namespace bt_bumpgo
+
+#include "behaviortree_cpp_v3/bt_factory.h"
+BT_REGISTER_NODES(factory)
+{
+ factory.registerNodeType("Turn");
+}
diff --git a/bt_bumpgo/src/bt_bumpgo_main.cpp b/bt_bumpgo/src/bt_bumpgo_main.cpp
new file mode 100644
index 0000000..afd706f
--- /dev/null
+++ b/bt_bumpgo/src/bt_bumpgo_main.cpp
@@ -0,0 +1,63 @@
+// Copyright 2021 Intelligent Robotics Lab
+//
+// 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.
+
+#include
+#include
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp_v3/utils/shared_library.h"
+#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"
+
+#include "ament_index_cpp/get_package_share_directory.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+
+
+int main(int argc, char * argv[])
+{
+ rclcpp::init(argc, argv);
+
+ auto node = rclcpp::Node::make_shared("patrolling_node");
+
+ BT::BehaviorTreeFactory factory;
+ BT::SharedLibrary loader;
+
+ factory.registerFromPlugin(loader.getOSName("forward_bt_node"));
+ factory.registerFromPlugin(loader.getOSName("back_bt_node"));
+ factory.registerFromPlugin(loader.getOSName("turn_bt_node"));
+ factory.registerFromPlugin(loader.getOSName("is_obstacle_bt_node"));
+
+ std::string pkgpath = ament_index_cpp::get_package_share_directory("bt_bumpgo");
+ std::string xml_file = pkgpath + "/behavior_tree_xml/bumpgo.xml";
+
+ auto blackboard = BT::Blackboard::create();
+ blackboard->set("node", node);
+ BT::Tree tree = factory.createTreeFromFile(xml_file, blackboard);
+
+ auto publisher_zmq = std::make_shared(tree, 10, 1666, 1667);
+
+ rclcpp::Rate rate(10);
+
+ bool finish = false;
+ while (!finish && rclcpp::ok()) {
+ finish = tree.rootNode()->executeTick() != BT::NodeStatus::RUNNING;
+
+ rclcpp::spin_some(node);
+ rate.sleep();
+ }
+
+ rclcpp::shutdown();
+ return 0;
+}
diff --git a/bt_bumpgo/tests/CMakeLists.txt b/bt_bumpgo/tests/CMakeLists.txt
new file mode 100644
index 0000000..80b377d
--- /dev/null
+++ b/bt_bumpgo/tests/CMakeLists.txt
@@ -0,0 +1,14 @@
+
+ament_add_gtest(bt_action_test bt_action_test.cpp)
+ament_target_dependencies(bt_action_test ${dependencies})
+
+add_executable(bt_forward bt_forward_main.cpp)
+ament_target_dependencies(bt_forward ${dependencies})
+target_link_libraries(bt_forward ${ZMQ_LIBRARIES})
+
+install(TARGETS
+ bt_forward
+ ARCHIVE DESTINATION lib
+ LIBRARY DESTINATION lib
+ RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
diff --git a/bt_bumpgo/tests/bt_action_test.cpp b/bt_bumpgo/tests/bt_action_test.cpp
new file mode 100644
index 0000000..f4849b0
--- /dev/null
+++ b/bt_bumpgo/tests/bt_action_test.cpp
@@ -0,0 +1,261 @@
+// Copyright 2021 Intelligent Robotics Lab
+//
+// 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.
+
+#include
+#include
+#include
+#include
+#include
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp_v3/utils/shared_library.h"
+
+#include "ament_index_cpp/get_package_share_directory.hpp"
+
+#include "geometry_msgs/msg/twist.hpp"
+#include "sensor_msgs/msg/laser_scan.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "gtest/gtest.h"
+
+using namespace std::placeholders;
+using namespace std::chrono_literals;
+
+
+class VelocitySinkNode : public rclcpp::Node
+{
+public:
+ VelocitySinkNode()
+ : Node("VelocitySink")
+ {
+ vel_sub_ = create_subscription(
+ "/output_vel", 100, std::bind(&VelocitySinkNode::vel_callback, this, _1));
+ }
+
+ void vel_callback(geometry_msgs::msg::Twist::SharedPtr msg)
+ {
+ vel_msgs_.push_back(*msg);
+ }
+
+ std::list vel_msgs_;
+
+private:
+ rclcpp::Subscription::SharedPtr vel_sub_;
+};
+
+
+TEST(bt_action, turn_btn)
+{
+ auto node = rclcpp::Node::make_shared("turn_btn_node");
+ auto node_sink = std::make_shared();
+
+ BT::BehaviorTreeFactory factory;
+ BT::SharedLibrary loader;
+
+ factory.registerFromPlugin(loader.getOSName("turn_bt_node"));
+
+ std::string xml_bt =
+ R"(
+
+
+
+
+ )";
+
+ auto blackboard = BT::Blackboard::create();
+ blackboard->set("node", node);
+ BT::Tree tree = factory.createTreeFromText(xml_bt, blackboard);
+
+ rclcpp::Rate rate(10);
+ bool finish = false;
+ while (!finish && rclcpp::ok()) {
+ finish = tree.rootNode()->executeTick() == BT::NodeStatus::SUCCESS;
+ rclcpp::spin_some(node_sink);
+ rate.sleep();
+ }
+
+ ASSERT_FALSE(node_sink->vel_msgs_.empty());
+ ASSERT_NEAR(node_sink->vel_msgs_.size(), 30, 1);
+
+ geometry_msgs::msg::Twist & one_twist = node_sink->vel_msgs_.front();
+
+ ASSERT_GT(one_twist.angular.z, 0.1);
+ ASSERT_NEAR(one_twist.linear.x, 0.0, 0.0000001);
+}
+
+TEST(bt_action, back_btn)
+{
+ auto node = rclcpp::Node::make_shared("back_btn_node");
+ auto node_sink = std::make_shared();
+
+ BT::BehaviorTreeFactory factory;
+ BT::SharedLibrary loader;
+
+ factory.registerFromPlugin(loader.getOSName("back_bt_node"));
+
+ std::string xml_bt =
+ R"(
+
+
+
+
+ )";
+
+ auto blackboard = BT::Blackboard::create();
+ blackboard->set("node", node);
+ BT::Tree tree = factory.createTreeFromText(xml_bt, blackboard);
+
+ rclcpp::Rate rate(10);
+ bool finish = false;
+ while (!finish && rclcpp::ok()) {
+ finish = tree.rootNode()->executeTick() == BT::NodeStatus::SUCCESS;
+ rclcpp::spin_some(node_sink);
+ rate.sleep();
+ }
+
+ ASSERT_FALSE(node_sink->vel_msgs_.empty());
+ ASSERT_NEAR(node_sink->vel_msgs_.size(), 30, 1);
+
+ geometry_msgs::msg::Twist & one_twist = node_sink->vel_msgs_.front();
+
+ ASSERT_LT(one_twist.linear.x, -0.1);
+ ASSERT_NEAR(one_twist.angular.z, 0.0, 0.0000001);
+}
+
+TEST(bt_action, forward_btn)
+{
+ auto node = rclcpp::Node::make_shared("forward_btn_node");
+ auto node_sink = std::make_shared();
+
+ BT::BehaviorTreeFactory factory;
+ BT::SharedLibrary loader;
+
+ factory.registerFromPlugin(loader.getOSName("forward_bt_node"));
+
+ std::string xml_bt =
+ R"(
+
+
+
+
+ )";
+
+ auto blackboard = BT::Blackboard::create();
+ blackboard->set("node", node);
+ BT::Tree tree = factory.createTreeFromText(xml_bt, blackboard);
+
+ rclcpp::Rate rate(10);
+ auto current_status = BT::NodeStatus::FAILURE;
+ int counter = 0;
+ while (counter++ < 30 && rclcpp::ok()) {
+ current_status = tree.rootNode()->executeTick();
+ rclcpp::spin_some(node_sink);
+ rate.sleep();
+ }
+
+ ASSERT_EQ(current_status, BT::NodeStatus::RUNNING);
+ ASSERT_FALSE(node_sink->vel_msgs_.empty());
+ ASSERT_NEAR(node_sink->vel_msgs_.size(), 30, 1);
+
+ geometry_msgs::msg::Twist & one_twist = node_sink->vel_msgs_.front();
+
+ ASSERT_GT(one_twist.linear.x, 0.1);
+ ASSERT_NEAR(one_twist.angular.z, 0.0, 0.0000001);
+}
+
+TEST(bt_action, is_obstacle_btn)
+{
+ auto node = rclcpp::Node::make_shared("is_obstacle_btn_node");
+ auto scan_pub = node->create_publisher("input_scan", 1);
+
+ BT::BehaviorTreeFactory factory;
+ BT::SharedLibrary loader;
+
+ factory.registerFromPlugin(loader.getOSName("is_obstacle_bt_node"));
+
+ std::string xml_bt =
+ R"(
+
+
+
+
+ )";
+
+ auto blackboard = BT::Blackboard::create();
+ blackboard->set("node", node);
+ BT::Tree tree = factory.createTreeFromText(xml_bt, blackboard);
+
+ rclcpp::Rate rate(10);
+
+ sensor_msgs::msg::LaserScan scan;
+ scan.ranges.push_back(2.0);
+ for (int i = 0; i < 10; i++) {
+ scan_pub->publish(scan);
+ rclcpp::spin_some(node);
+ rate.sleep();
+ }
+
+ BT::NodeStatus current_status = tree.rootNode()->executeTick();
+ ASSERT_EQ(current_status, BT::NodeStatus::FAILURE);
+
+ scan.ranges.resize(360, 100.0);
+ scan.ranges[180] = 0.3f;
+ for (int i = 0; i < 10; i++) {
+ scan_pub->publish(scan);
+ rclcpp::spin_some(node);
+ rate.sleep();
+ }
+
+ current_status = tree.rootNode()->executeTick();
+ ASSERT_EQ(current_status, BT::NodeStatus::SUCCESS);
+
+ xml_bt =
+ R"(
+
+
+
+
+ )";
+ tree = factory.createTreeFromText(xml_bt, blackboard);
+
+ scan.ranges[180] = 0.3f;
+ for (int i = 0; i < 10; i++) {
+ scan_pub->publish(scan);
+ rclcpp::spin_some(node);
+ rate.sleep();
+ }
+
+ current_status = tree.rootNode()->executeTick();
+ ASSERT_EQ(current_status, BT::NodeStatus::SUCCESS);
+
+ scan.ranges[180] = 0.6f;
+ for (int i = 0; i < 10; i++) {
+ scan_pub->publish(scan);
+ rclcpp::spin_some(node);
+ rate.sleep();
+ }
+
+ current_status = tree.rootNode()->executeTick();
+ ASSERT_EQ(current_status, BT::NodeStatus::FAILURE);
+}
+
+int main(int argc, char ** argv)
+{
+ rclcpp::init(argc, argv);
+
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/bt_bumpgo/tests/bt_forward_main.cpp b/bt_bumpgo/tests/bt_forward_main.cpp
new file mode 100644
index 0000000..3daf074
--- /dev/null
+++ b/bt_bumpgo/tests/bt_forward_main.cpp
@@ -0,0 +1,62 @@
+// Copyright 2021 Intelligent Robotics Lab
+//
+// 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.
+
+#include
+#include
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+#include "behaviortree_cpp_v3/utils/shared_library.h"
+#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"
+
+#include "ament_index_cpp/get_package_share_directory.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+
+
+int main(int argc, char * argv[])
+{
+ rclcpp::init(argc, argv);
+
+ auto node = rclcpp::Node::make_shared("forward_node");
+
+ BT::BehaviorTreeFactory factory;
+ BT::SharedLibrary loader;
+
+ factory.registerFromPlugin(loader.getOSName("forward_bt_node"));
+
+ std::string xml_bt =
+ R"(
+
+
+
+
+ )";
+
+ auto blackboard = BT::Blackboard::create();
+ blackboard->set("node", node);
+ BT::Tree tree = factory.createTreeFromText(xml_bt, blackboard);
+
+ rclcpp::Rate rate(10);
+ bool finish = false;
+ while (!finish && rclcpp::ok()) {
+ finish = tree.rootNode()->executeTick() != BT::NodeStatus::RUNNING;
+
+ rclcpp::spin_some(node);
+ rate.sleep();
+ }
+
+ rclcpp::shutdown();
+ return 0;
+}