Add roseus_bt package

.gitmodules

@@ -1,3 +1,6 @@
 [submodule ".travis"]
 	path = .travis
 	url = https://github.com/jsk-ros-pkg/jsk_travis
+[submodule "roseus_bt/include/rosbridgecpp"]
+	path = roseus_bt/include/rosbridgecpp
+	url = https://github.com/ppianpak/rosbridgecpp

roseus_bt/CMakeLists.txt

@@ -0,0 +1,49 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(roseus_bt)
+
+SET(Boost_USE_STATIC_LIBS ON)
+
+find_package(catkin REQUIRED COMPONENTS
+  cmake_modules
+  roscpp
+  behaviortree_ros
+)
+
+find_package(TinyXML2 REQUIRED)
+find_package(Boost REQUIRED COMPONENTS log)
+find_package(fmt)
+
+catkin_package(
+ INCLUDE_DIRS include
+ LIBRARIES
+ CATKIN_DEPENDS
+ DEPENDS TinyXML2 fmt
+)
+
+# git submodule update
+# only when not in bloom build
+if (NOT $ENV{DH_OPTIONS})
+  find_package(Git QUIET)
+  option(GIT_SUBMODULE "Check submodules during build" ON)
+  if(Git_FOUND AND GIT_SUBMODULE)
+    message(STATUS "Submodule update")
+    execute_process(COMMAND ${GIT_EXECUTABLE} submodule update --init --remote include/rosbridgecpp
+      WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+      RESULT_VARIABLE GIT_SUBMOD_RESULT)
+    if(NOT GIT_SUBMOD_RESULT EQUAL "0")
+      message(WARNING "git submodule update failed with ${GIT_SUBMOD_RESULT}")
+      message(WARNING "please update submodules manually with: git submodule update --init --remote include/rosbridgecpp")
+    endif()
+  endif()
+endif()
+
+include_directories( include ${catkin_INCLUDE_DIRS} ${TinyXML2_INCUDE_DIRS})
+
+
+add_executable(create_bt_package src/create_bt_package.cpp)
+add_dependencies(create_bt_package ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(create_bt_package ${catkin_LIBRARIES} ${TinyXML2_LIBRARIES} ${Boost_LIBRARIES} fmt::fmt)
+
+add_executable(create_bt_tutorials src/create_bt_tutorials.cpp)
+add_dependencies(create_bt_tutorials ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+target_link_libraries(create_bt_tutorials ${catkin_LIBRARIES} ${TinyXML2_LIBRARIES} ${Boost_LIBRARIES} fmt::fmt)

roseus_bt/README.md

@@ -0,0 +1,49 @@
+# roseus_bt
+
+Generate glue code for connecting your roseus projects to [BehaviorTree.CPP](https://github.com/BehaviorTree/BehaviorTree.CPP), [BehaviorTree.ROS](https://github.com/BehaviorTree/BehaviorTree.ROS) and [Groot](https://github.com/BehaviorTree/Groot).
+
+## What are behavior trees
+
+Behavior Trees are a control structure used to better organize and design the logical flow of an application. When compared to State Machines they tend to display increased modularity (because there are no direct connections between the execution nodes) and reactivity (because the tree formulation includes implicit transitions).
+
+BehaviorTree.CPP documentation page: [https://www.behaviortree.dev/bt_basics](https://www.behaviortree.dev/bt_basics)
+
+Behavior Tree in Robotics and AI: [https://arxiv.org/pdf/1709.00084.pdf](https://arxiv.org/pdf/1709.00084.pdf)
+
+## Quick Setup
+
+Clone related directories
+```bash
+mkdir ~/catkin_ws/src -p
+cd ~/catkin_ws/src
+wget https://raw.githubusercontent.com/Affonso-Gui/jsk_roseus/roseus_bt/roseus_bt/roseus_bt.rosinstall -O .rosinstall
+wstool update
+```
+
+Install dependencies
+```bash
+rosdep install -yr --ignore-src --from-paths BehaviorTree.ROS Groot jsk_roseus/roseus_bt
+```
+
+Build & source
+```bash
+cd ~/catkin_ws/
+catkin build roseus_bt groot
+source ~/catkin_ws/devel/setup.bash
+```
+
+## Run
+
+The following command creates a new ros package with all the necessary glue code for running roseus code on the BehaviorTree.CPP engine.
+
+```bash
+cd ~/catkin_ws/src
+rosrun roseus_bt create_bt_package my_package path/to/model.xml
+catkin build my_package
+```
+
+For more information on how to compose the model file and use the package check the [tutorials](https://github.com/Affonso-Gui/jsk_roseus/tree/roseus_bt/roseus_bt/sample) and the [BehaviorTree.CPP documentation](https://www.behaviortree.dev/bt_basics).
+
+## Samples
+
+Follow instructions at the [tutorial page](https://github.com/Affonso-Gui/jsk_roseus/tree/roseus_bt/roseus_bt/sample).

roseus_bt/euslisp/nodes.l

@@ -0,0 +1,140 @@
+(unless (find-package "ROSEUS_BT")
+  (make-package "ROSEUS_BT" :nicknames (list "ROSEUS-BT")))
+
+(in-package "ROSEUS_BT")
+
+(export '(action-node condition-node spin-once spin))
+;; import relevant ros::simple-action-server symbols
+(import '(ros::name-space ros::status ros::execute-cb
+          ros::goal ros::goal-id))
+
+(defvar *action-list*)
+(defvar *condition-list*)
+
+
+;; utility
+(defun get-fn-sym (fn)
+  (when (functionp fn)
+    (cond
+      ((symbolp fn) fn)
+      ((and (listp fn) (memq (car fn) '(lambda lambda-closure)))
+       (cadr fn))
+      ((derivedp fn compiled-code)
+       (send fn :name))
+      (t nil))))
+
+(defun check-fn-closure (fn)
+  (when (and (consp fn)
+             (eql (car fn) 'lambda-closure)
+             (or (not (zerop (third fn)))
+                 (not (zerop (fourth fn)))))
+    (let ((name (get-fn-sym fn)))
+      (if name
+          (ros::ros-warn
+           (format nil "Possibly harmful lambda-closure in #'~A! Try using '~A instead."
+                   name
+                   name))
+          (ros::ros-warn
+           (format nil "Possibly harmful lambda context in ~A! Reseting to 0..."
+                   (append (subseq fn 0 5) '(...)))
+           (setf (third fn) 0)
+           (setf (fourth fn) 0))))))
+
+
+;; classes
+(defclass action-node :super ros::simple-action-server :slots (monitor-groupname))
+(defmethod action-node
+  (:init (ns spec &key execute-cb preempt-cb accept-cb groupname
+             ((:monitor-groupname monitor-gn)))
+      (check-fn-closure execute-cb)
+      (check-fn-closure preempt-cb)
+      (check-fn-closure accept-cb)
+
+      (send-super :init ns spec
+              :execute-cb execute-cb
+              :preempt-cb preempt-cb
+              :accept-cb accept-cb
+              :groupname groupname)
+      (when monitor-gn
+        (setq monitor-groupname monitor-gn)
+        (ros::create-nodehandle monitor-groupname)
+        ;; resubscribe /cancel on separate handler
+        (ros::subscribe (format nil "~A/cancel" ns)
+                        actionlib_msgs::GoalID #'send self :cancel-callback 50
+                        :groupname monitor-groupname))
+
+      ;; Publishing the status topic is required to make a
+      ;; connection with the action client, but doing so on
+      ;; high frequencies is slow. Throttle it to 5Hz.
+      (ros::create-timer 0.2 #'(lambda (event) (send self :publish-status))
+                         :groupname groupname)
+
+      (push self *action-list*)
+      (if *condition-list*
+          (ros::ros-warn "Actions and Conditions detected in the same node! Start two separate nodes when reactivity is required."))
+      self)
+  (:publish-status ()
+    (let ((msg (instance actionlib_msgs::GoalStatusArray :init)))
+      (when goal-id
+        (send msg :status_list
+              (list (instance actionlib_msgs::goalstatus :init
+                              :goal_id goal-id
+                              :status status
+                              :text (ros::goal-status-to-string status)))))
+      (send msg :header :seq (send self :next-seq-id))
+      (send msg :header :stamp (ros::time-now))
+      (ros::publish (format nil "~A/status" name-space) msg)))
+  (:execute-cb ()
+    (when (and goal execute-cb)
+      (let ((res (funcall execute-cb self goal)))
+        (when (send self :is-active)
+          (send self :set-succeeded
+                (send self :result :success (not (not res))))))))
+  (:set-output (name val)
+    (let ((msg (send self :feedback (intern (string-upcase name) *keyword-package*) val)))
+      (send msg :feedback :update_field_name name)
+      (send self :publish-feedback msg)))
+  (:ok ()
+     (send self :spin-monitor)
+     (not (send self :is-preempt-requested)))
+  (:spin-monitor ()
+    (if monitor-groupname
+        (ros::spin-once monitor-groupname)
+        (send self :spin-once))))
+
+
+(defclass condition-node :super propertied-object :slots (execute-cb groupname))
+(defmethod condition-node
+  (:init (ns spec &key ((:execute-cb exec-cb)) ((:groupname gn)))
+    (check-fn-closure exec-cb)
+    (setq execute-cb exec-cb)
+    (setq groupname gn)
+    (if groupname
+        (ros::advertise-service ns spec #'send self :request-cb :groupname groupname)
+        (ros::advertise-service ns spec #'send self :request-cb))
+    (push self *condition-list*)
+    (if *action-list*
+        (ros::ros-warn "Actions and Conditions detected in the same node! Start two separate nodes when reacitivity is required."))
+    self)
+  (:request-cb (msg)
+    (let ((response (send msg :response)))
+      (send response :success (not (not (funcall execute-cb self msg))))
+      response))
+  (:spin-once ()
+    (if groupname
+        (ros::spin-once groupname)
+        (ros::spin-once))))
+
+
+;; functions
+(defun spin-once ()
+  (dolist (cc *condition-list*)
+    (send cc :spin-once))
+  (dolist (ac *action-list*)
+    (send ac :spin-once)
+    (send ac :execute-cb)))
+
+(defun spin ()
+  (while t
+    (spin-once)
+    (ros::sleep)))

roseus_bt/include/roseus_bt/basic_types.cpp

@@ -0,0 +1,54 @@
+#include "roseus_bt/basic_types.h"
+
+namespace roseus_bt
+{
+
+std::string toStr(NodeType type)
+{
+    switch (type)
+    {
+        case NodeType::ACTION:
+            return "Action";
+        case NodeType::CONDITION:
+            return "Condition";
+        case NodeType::DECORATOR:
+            return "Decorator";
+        case NodeType::CONTROL:
+            return "Control";
+        case NodeType::SUBTREE:
+            return "SubTree";
+        case NodeType::SUBSCRIBER:
+            return "Subscriber";
+        case NodeType::REMOTE_SUBSCRIBER:
+            return "RemoteSubscriber";
+        case NodeType::REMOTE_ACTION:
+            return "RemoteAction";
+        case NodeType::REMOTE_CONDITION:
+            return "RemoteCondition";
+        default:
+            return "Undefined";
+    }
+}
+
+NodeType convertFromString(BT::StringView str)
+{
+    if( str == "Action" )    return NodeType::ACTION;
+    if( str == "Condition" ) return NodeType::CONDITION;
+    if( str == "Control" )   return NodeType::CONTROL;
+    if( str == "Decorator" ) return NodeType::DECORATOR;
+    if( str == "SubTree" || str == "SubTreePlus" ) return NodeType::SUBTREE;
+    if( str == "Subscriber" )      return NodeType::SUBSCRIBER;
+    if( str == "RemoteAction" )    return NodeType::REMOTE_ACTION;
+    if( str == "RemoteCondition" ) return NodeType::REMOTE_CONDITION;
+    if( str == "RemoteSubscriber" )  return NodeType::REMOTE_SUBSCRIBER;
+    return NodeType::UNDEFINED;
+}
+
+std::ostream& operator<<(std::ostream& os, const NodeType& type)
+{
+    os << toStr(type);
+    return os;
+}
+
+
+}  // namespace roseus_bt

roseus_bt/include/roseus_bt/basic_types.h

@@ -0,0 +1,30 @@
+#ifndef BEHAVIOR_TREE_ROSEUS_BT_EUS_NODE_TYPE_HPP_
+#define BEHAVIOR_TREE_ROSEUS_BT_EUS_NODE_TYPE_HPP_
+
+#include <behaviortree_cpp_v3/basic_types.h>
+
+
+namespace roseus_bt
+{
+
+enum class NodeType
+{
+    UNDEFINED = 0,
+    ACTION,
+    CONDITION,
+    CONTROL,
+    DECORATOR,
+    SUBTREE,
+    SUBSCRIBER,
+    REMOTE_ACTION,
+    REMOTE_CONDITION,
+    REMOTE_SUBSCRIBER,
+};
+
+std::string toStr(NodeType type);
+NodeType convertFromString(BT::StringView str);
+std::ostream& operator<<(std::ostream& os, const NodeType& type);
+
+}  // namespace roseus_bt
+
+#endif  // BEHAVIOR_TREE_ROSEUS_BT_EUS_NODE_TYPE_HPP_

roseus_bt/include/roseus_bt/bt_exceptions.h

@@ -0,0 +1,41 @@
+#ifndef BEHAVIOR_TREE_ROSEUS_BT_BT_EXCEPTIONS_
+#define BEHAVIOR_TREE_ROSEUS_BT_BT_EXCEPTIONS_
+
+#include <string>
+#include <stdexcept>
+#include <fmt/format.h>
+
+namespace RoseusBT
+{
+
+class BTError: public std::exception {
+public:
+  BTError(std::string message):  message(message) {}
+
+  const char* what() const noexcept {
+    return message.c_str();
+  }
+
+  std::string message;
+};
+
+class InvalidOutputPort: public BTError {
+public:
+  InvalidOutputPort() : BTError("Condition Node only accepts input ports!") {};
+};
+
+class InvalidPackageName: public BTError {
+public:
+  InvalidPackageName(std::string name) :
+    BTError(fmt::format("Package name {} does not follow naming conventions", name)) {};
+};
+
+class InvalidInput: public BTError {
+public:
+  InvalidInput() : BTError("Invalid Input") {};
+};
+
+
+}  // namespace RoseusBT
+
+#endif  // BEHAVIOR_TREE_ROSEUS_BT_BT_EXCEPTIONS_