Source completed and lintered

Signed-off-by: Francisco Martín Rico <[email protected]>

br2_bt_patrolling/CMakeLists.txt

@@ -0,0 +1,100 @@
+cmake_minimum_required(VERSION 3.5)
+project(br2_bt_patrolling)
+
+set(CMAKE_BUILD_TYPE Debug)
+
+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(rclcpp_lifecycle REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(behaviortree_cpp_v3 REQUIRED)
+find_package(action_msgs REQUIRED)
+find_package(lifecycle_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav2_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
+    rclcpp_lifecycle
+    rclcpp_action
+    behaviortree_cpp_v3
+    action_msgs
+    lifecycle_msgs
+    geometry_msgs
+    nav2_msgs
+    ament_index_cpp
+)
+
+include_directories(include ${ZMQ_INCLUDE_DIRS})
+
+add_library(br2_recharge_bt_node SHARED src/br2_bt_patrolling/Recharge.cpp)
+add_library(br2_patrol_bt_node SHARED src/br2_bt_patrolling/Patrol.cpp)
+add_library(br2_move_bt_node SHARED src/br2_bt_patrolling/Move.cpp)
+add_library(br2_get_waypoint_bt_node SHARED src/br2_bt_patrolling/GetWaypoint.cpp)
+add_library(br2_battery_checker_bt_node SHARED src/br2_bt_patrolling/BatteryChecker.cpp)
+add_library(br2_track_objects_bt_node SHARED src/br2_bt_patrolling/TrackObjects.cpp)
+list(APPEND plugin_libs
+  br2_recharge_bt_node
+  br2_patrol_bt_node
+  br2_move_bt_node
+  br2_get_waypoint_bt_node
+  br2_battery_checker_bt_node
+  br2_track_objects_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(patrolling_main src/patrolling_main.cpp)
+ament_target_dependencies(patrolling_main ${dependencies})
+target_link_libraries(patrolling_main ${ZMQ_LIBRARIES})
+
+install(TARGETS
+  ${plugin_libs}
+  patrolling_main
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION lib/${PROJECT_NAME}
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+install(DIRECTORY behavior_tree_xml config 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()

br2_bt_patrolling/behavior_tree_xml/patrolling.xml

@@ -0,0 +1,42 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="BehaviorTree">
+    <!-- ////////// -->
+    <BehaviorTree ID="BehaviorTree">
+        <KeepRunningUntilFailure>
+            <ReactiveSequence>
+                <Fallback>
+                    <Action ID="BatteryChecker"/>
+                    <Sequence>
+                        <Action ID="GetWaypoint" waypoint="{recharge_wp}" wp_id="recharge"/>
+                        <Action ID="Move" goal="{recharge_wp}"/>
+                        <Action ID="Recharge"/>
+                    </Sequence>
+                </Fallback>
+                <Sequence>
+                    <Action ID="GetWaypoint" waypoint="{wp}" wp_id="next"/>
+                        <Parallel success_threshold="1" failure_threshold="1">
+                          <Action ID="TrackObjects"/>
+                          <Action ID="Move" goal="{wp}"/>
+                        </Parallel>
+                    <Action ID="Patrol"/>
+                </Sequence>
+            </ReactiveSequence>
+        </KeepRunningUntilFailure>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <TreeNodesModel>
+        <Action ID="BatteryChecker"/>
+        <Action ID="GetWaypoint">
+            <output_port name="waypoint"/>
+            <input_port name="wp_id"/>
+        </Action>
+        <Action ID="Move">
+            <input_port name="goal"/>
+        </Action>
+        <Action ID="Patrol"/>
+        <Action ID="Recharge"/>
+        <Action ID="TrackObjects"/>
+    </TreeNodesModel>
+    <!-- ////////// -->
+</root>
+

br2_bt_patrolling/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 <[email protected]>
+#
+#  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()
+

br2_bt_patrolling/config/test_waypoints.yaml

@@ -0,0 +1,7 @@
+patrolling_node:
+  ros__parameters:
+    waypoints: [recharge, wp1, wp2, wp3]
+    recharge: [3.67, -0.24]
+    wp1: [1.07, -12.38]
+    wp2: [-5.32, -8.85]
+    wp3: [-0.56, 0.24]

br2_bt_patrolling/include/br2_bt_patrolling/BatteryChecker.hpp

@@ -0,0 +1,59 @@
+// 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 BR2_BT_PATROLLING__BATTERYCHECKER_HPP_
+#define BR2_BT_PATROLLING__BATTERYCHECKER_HPP_
+
+#include <string>
+#include <vector>
+
+#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 br2_bt_patrolling
+{
+
+class BatteryChecker : public BT::ConditionNode
+{
+public:
+  explicit BatteryChecker(
+    const std::string & xml_tag_name,
+    const BT::NodeConfiguration & conf);
+
+  BT::NodeStatus tick();
+
+  static BT::PortsList providedPorts()
+  {
+    return BT::PortsList({});
+  }
+
+  void vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg);
+
+  const float DECAY_LEVEL = 0.5;  // 0.5 * |vel| * dt
+  const float EPSILON = 0.01;  // 0.001 * dt
+  const float MIN_LEVEL = 10.0;
+
+private:
+  rclcpp::Node::SharedPtr node_;
+  rclcpp::Time last_reading_time_;
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr vel_sub_;
+};
+
+}  // namespace br2_bt_patrolling
+
+#endif  // BR2_BT_PATROLLING__BATTERYCHECKER_HPP_

br2_bt_patrolling/include/br2_bt_patrolling/GetWaypoint.hpp

@@ -0,0 +1,56 @@
+// 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 BR2_BT_PATROLLING__GETWAYPOINT_HPP_
+#define BR2_BT_PATROLLING__GETWAYPOINT_HPP_
+
+#include <string>
+#include <vector>
+
+#include "behaviortree_cpp_v3/behavior_tree.h"
+#include "behaviortree_cpp_v3/bt_factory.h"
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+namespace br2_bt_patrolling
+{
+
+class GetWaypoint : public BT::ActionNodeBase
+{
+public:
+  explicit GetWaypoint(
+    const std::string & xml_tag_name,
+    const BT::NodeConfiguration & conf);
+
+  void halt();
+  BT::NodeStatus tick();
+
+  static BT::PortsList providedPorts()
+  {
+    return BT::PortsList(
+      {
+        BT::InputPort<std::string>("wp_id"),
+        BT::OutputPort<geometry_msgs::msg::PoseStamped>("waypoint")
+      });
+  }
+
+private:
+  geometry_msgs::msg::PoseStamped recharge_point_;
+  std::vector<geometry_msgs::msg::PoseStamped> waypoints_;
+  static int current_;
+};
+
+}  // namespace br2_bt_patrolling
+
+#endif  // BR2_BT_PATROLLING__GETWAYPOINT_HPP_