bt_bumpgo

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()

bt_bumpgo/behavior_tree_xml/bumpgo.xml

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="BehaviorTree">
+    <!-- ////////// -->
+    <BehaviorTree ID="BehaviorTree">
+        <ReactiveSequence>
+            <Fallback>
+                <Inverter>
+                    <Condition ID="IsObstacle" distance="1.0"/>
+                </Inverter>
+                <Sequence>
+                    <Action ID="Back"/>
+                    <Action ID="Turn"/>
+                </Sequence>
+            </Fallback>
+            <Action ID="Forward"/>
+        </ReactiveSequence>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <TreeNodesModel>
+        <Action ID="Back"/>
+        <Action ID="Forward"/>
+        <Condition ID="IsObstacle">
+            <input_port default="1.0" name="distance">Distance to consider obstacle</input_port>
+        </Condition>
+        <Action ID="Turn"/>
+    </TreeNodesModel>
+    <!-- ////////// -->
+</root>
+

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 <lee.hambley@gmail.com>
+#
+#  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()
+

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 <string>
+
+#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<geometry_msgs::msg::Twist>::SharedPtr vel_pub_;
+};
+
+}  // namespace bt_bumpgo
+
+#endif  // BT_BUMPGO__BACK_HPP_

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 <string>
+
+#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<geometry_msgs::msg::Twist>::SharedPtr vel_pub_;
+};
+
+}  // namespace bt_bumpgo
+
+#endif  // BT_BUMPGO__FORWARD_HPP_

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 <string>
+
+#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<double>("distance")
+      });
+  }
+
+  void laser_callback(sensor_msgs::msg::LaserScan::UniquePtr msg);
+
+private:
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::Time last_reading_time_;
+  rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr laser_sub_;
+  sensor_msgs::msg::LaserScan::UniquePtr last_scan_;
+};
+
+}  // namespace bt_bumpgo
+
+#endif  // BT_BUMPGO__ISOBSTACLE_HPP_