Allow customization in BT factory (ros-industrial-consortium#86)

* Created virtual function in SNP widget for configuring BT factory such that inherited classes can add additional custom BT nodes

* Changed section from private to protected

* Export dependencies of snp_application

* Nest headers in snp_application subdirectory

* Remove unnecessary directory include

* Removed unnecessary Q_OBJECT macros

* Updated BT configuration method to include ROS timeouts

* Moved template functions to header for use by other packages

snp_application/CMakeLists.txt

@@ -14,10 +14,6 @@ set(CMAKE_AUTOMOC ON)
 set(CMAKE_AUTORCC ON)
 set(CMAKE_AUTOUIC ON)
 
-if(CMAKE_VERSION VERSION_LESS "3.7.0")
-  set(CMAKE_INCLUDE_CURRENT_DIR ON)
-endif()
-
 find_package(Eigen3 REQUIRED)
 find_package(Qt5 COMPONENTS Widgets REQUIRED)
 find_package(behaviortree_cpp REQUIRED)
@@ -40,31 +36,25 @@ find_package(rviz_common REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(trajectory_preview REQUIRED)
 
-include_directories(include ${EIGEN3_INCLUDE_DIRS})
-
 add_library(
   ${PROJECT_NAME}_widget SHARED
   src/snp_widget.ui
-  include/snp_widget.h
   src/snp_widget.cpp
   # BT
-  include/bt/utils.h
-  include/bt/bt_thread.h
   src/bt/bt_thread.cpp
-  include/bt/button_approval_node.h
   src/bt/button_approval_node.cpp
-  include/bt/button_monitor_node.h
   src/bt/button_monitor_node.cpp
-  include/bt/progress_decorator_node.h
   src/bt/progress_decorator_node.cpp
-  include/bt/set_page_decorator_node.h
   src/bt/set_page_decorator_node.cpp
-  include/bt/snp_bt_ros_nodes.h
   src/bt/snp_bt_ros_nodes.cpp
-  include/bt/snp_sequence_with_memory_node.h
   src/bt/snp_sequence_with_memory_node.cpp
-  include/bt/text_edit_logger.h
   src/bt/text_edit_logger.cpp)
+target_include_directories(
+  ${PROJECT_NAME}_widget
+  PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+         "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>"
+         "$<INSTALL_INTERFACE:include>"
+         ${EIGEN3_INCLUDE_DIRS})
 target_link_libraries(${PROJECT_NAME}_widget Qt5::Widgets)
 ament_target_dependencies(
   ${PROJECT_NAME}_widget
@@ -82,7 +72,6 @@ ament_target_dependencies(
   tf2_eigen
   trajectory_msgs
   trajectory_preview)
-target_compile_definitions(${PROJECT_NAME}_widget PUBLIC SNP_TPP_GUI_PLUGINS="${PROJECT_NAME}_plugins")
 
 add_library(${PROJECT_NAME}_panel SHARED src/snp_panel.cpp)
 target_link_libraries(${PROJECT_NAME}_panel ${PROJECT_NAME}_widget)
@@ -92,6 +81,9 @@ add_executable(roscon_app src/main.cpp)
 target_link_libraries(roscon_app Qt5::Widgets ${PROJECT_NAME}_widget)
 ament_target_dependencies(roscon_app rclcpp)
 
+# Install the headers
+install(DIRECTORY include/${PROJECT_NAME} DESTINATION include/)
+
 # Install the library(ies)
 install(TARGETS ${PROJECT_NAME}_widget ${PROJECT_NAME}_panel EXPORT ${PROJECT_NAME}-targets DESTINATION lib)
 ament_export_targets(${PROJECT_NAME}-targets HAS_LIBRARY_TARGET)
@@ -101,4 +93,21 @@ install(TARGETS roscon_app DESTINATION lib/${PROJECT_NAME}/)
 install(DIRECTORY config DESTINATION share/${PROJECT_NAME}/)
 
 pluginlib_export_plugin_description_file(rviz_common plugin_description.xml)
+ament_export_dependencies(
+  behaviortree_cpp
+  behaviortree_ros2
+  industrial_reconstruction_msgs
+  rclcpp_action
+  rclcpp_components
+  control_msgs
+  sensor_msgs
+  geometry_msgs
+  snp_msgs
+  snp_tpp
+  std_srvs
+  tf2_eigen
+  trajectory_msgs
+  trajectory_preview
+  rviz_common
+  pluginlib)
 ament_package()

snp_application/include/bt/bt_thread.h → snp_application/include/snp_application/bt/bt_thread.h

@@ -1,6 +1,5 @@
 #pragma once
 
-#include <memory>
 #include <QThread>
 #include <behaviortree_cpp/bt_factory.h>
 
@@ -11,7 +10,6 @@ namespace snp_application
  */
 class BTThread : public QThread
 {
-  Q_OBJECT
 public:
   BTThread(QObject* parent = nullptr);
   BTThread(BT::Tree tree, QObject* parent = nullptr);

snp_application/include/bt/snp_bt_ros_nodes.h → snp_application/include/snp_application/bt/snp_bt_ros_nodes.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#include <snp_application/bt/utils.h>
+
 #include <behaviortree_ros2/bt_action_node.hpp>
 #include <behaviortree_ros2/bt_service_node.hpp>
 #include <behaviortree_ros2/bt_topic_pub_node.hpp>
@@ -57,15 +59,75 @@ class SnpRosServiceNode : public BT::RosServiceNode<T>
 {
 public:
   using BT::RosServiceNode<T>::RosServiceNode;
-  BT::NodeStatus onFailure(BT::ServiceNodeErrorCode error) override;
+
+  inline BT::NodeStatus onFailure(BT::ServiceNodeErrorCode error) override
+  {
+    std::stringstream ss;
+    ss << "Service '" << BT::RosServiceNode<T>::prev_service_name_ << "'";
+
+    switch (error)
+    {
+      case BT::SERVICE_UNREACHABLE:
+        ss << " is unreachable";
+        break;
+      case BT::SERVICE_TIMEOUT:
+        ss << " timed out";
+        break;
+      case BT::INVALID_REQUEST:
+        ss << " was sent an invalid request";
+        break;
+      case BT::SERVICE_ABORTED:
+        ss << " was aborted";
+        break;
+      default:
+        break;
+    }
+
+    this->config().blackboard->set(ERROR_MESSAGE_KEY, ss.str());
+
+    return BT::NodeStatus::FAILURE;
+  }
 };
 
 template <typename T>
 class SnpRosActionNode : public BT::RosActionNode<T>
 {
 public:
   using BT::RosActionNode<T>::RosActionNode;
-  BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override;
+
+  inline BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override
+  {
+    std::stringstream ss;
+    ss << "Action '" << BT::RosActionNode<T>::prev_action_name_ << "' failed: '";
+
+    switch (error)
+    {
+      case BT::SERVER_UNREACHABLE:
+        ss << "server unreachable'";
+        break;
+      case BT::SEND_GOAL_TIMEOUT:
+        ss << "goal timed out'";
+        break;
+      case BT::GOAL_REJECTED_BY_SERVER:
+        ss << "goal rejected by server'";
+        break;
+      case BT::ACTION_ABORTED:
+        ss << "action aborted'";
+        break;
+      case BT::ACTION_CANCELLED:
+        ss << "action cancelled'";
+        break;
+      case BT::INVALID_GOAL:
+        ss << "invalid goal'";
+        break;
+      default:
+        break;
+    }
+
+    this->config().blackboard->set(ERROR_MESSAGE_KEY, ss.str());
+
+    return BT::NodeStatus::FAILURE;
+  }
 };
 
 class TriggerServiceNode : public SnpRosServiceNode<std_srvs::srv::Trigger>

snp_application/include/snp_widget.h → snp_application/include/snp_application/snp_widget.h

@@ -16,14 +16,14 @@ namespace snp_application
 {
 class SNPWidget : public QWidget
 {
-  Q_OBJECT
-
 public:
   explicit SNPWidget(rclcpp::Node::SharedPtr rviz_node, QWidget* parent = nullptr);
 
-private:
+protected:
   void runTreeWithThread();
 
+  virtual BT::BehaviorTreeFactory createBTFactory(int ros_short_timeout, int ros_long_timeout);
+
   rclcpp::Node::SharedPtr bt_node_;
   rclcpp::Node::SharedPtr tpp_node_;
   rclcpp::executors::SingleThreadedExecutor tpp_node_executor_;

snp_application/src/bt/bt_thread.cpp

@@ -1,4 +1,4 @@
-#include "bt/bt_thread.h"
+#include <snp_application/bt/bt_thread.h>
 
 namespace snp_application
 {

snp_application/src/bt/button_approval_node.cpp

@@ -1,5 +1,5 @@
-#include "bt/button_approval_node.h"
-#include "bt/utils.h"
+#include <snp_application/bt/button_approval_node.h>
+#include <snp_application/bt/utils.h>
 
 #include <QAbstractButton>
 

snp_application/src/bt/button_monitor_node.cpp

@@ -1,5 +1,5 @@
-#include "bt/button_monitor_node.h"
-#include "bt/utils.h"
+#include <snp_application/bt/button_monitor_node.h>
+#include <snp_application/bt/utils.h>
 
 #include <QAbstractButton>
 

snp_application/src/bt/progress_decorator_node.cpp

@@ -1,5 +1,5 @@
-#include "bt/progress_decorator_node.h"
-#include "bt/utils.h"
+#include <snp_application/bt/progress_decorator_node.h>
+#include <snp_application/bt/utils.h>
 
 #include <QProgressBar>
 

snp_application/src/bt/set_page_decorator_node.cpp

@@ -1,5 +1,5 @@
-#include "bt/set_page_decorator_node.h"
-#include "bt/utils.h"
+#include <snp_application/bt/set_page_decorator_node.h>
+#include <snp_application/bt/utils.h>
 
 #include <QStackedWidget>
 

snp_application/src/bt/snp_bt_ros_nodes.cpp

@@ -1,74 +1,9 @@
-#include "bt/snp_bt_ros_nodes.h"
-#include "bt/utils.h"
+#include <snp_application/bt/snp_bt_ros_nodes.h>
 
 #include <geometry_msgs/msg/pose_array.hpp>
 
 namespace snp_application
 {
-template <typename T>
-BT::NodeStatus SnpRosServiceNode<T>::onFailure(BT::ServiceNodeErrorCode error)
-{
-  std::stringstream ss;
-  ss << "Service '" << BT::RosServiceNode<T>::prev_service_name_ << "'";
-
-  switch (error)
-  {
-    case BT::SERVICE_UNREACHABLE:
-      ss << " is unreachable";
-      break;
-    case BT::SERVICE_TIMEOUT:
-      ss << " timed out";
-      break;
-    case BT::INVALID_REQUEST:
-      ss << " was sent an invalid request";
-      break;
-    case BT::SERVICE_ABORTED:
-      ss << " was aborted";
-      break;
-    default:
-      break;
-  }
-
-  this->config().blackboard->set(ERROR_MESSAGE_KEY, ss.str());
-
-  return BT::NodeStatus::FAILURE;
-}
-
-template <typename T>
-BT::NodeStatus SnpRosActionNode<T>::onFailure(BT::ActionNodeErrorCode error)
-{
-  std::stringstream ss;
-  ss << "Action '" << BT::RosActionNode<T>::prev_action_name_ << "' failed: '";
-
-  switch (error)
-  {
-    case BT::SERVER_UNREACHABLE:
-      ss << "server unreachable'";
-      break;
-    case BT::SEND_GOAL_TIMEOUT:
-      ss << "goal timed out'";
-      break;
-    case BT::GOAL_REJECTED_BY_SERVER:
-      ss << "goal rejected by server'";
-      break;
-    case BT::ACTION_ABORTED:
-      ss << "action aborted'";
-      break;
-    case BT::ACTION_CANCELLED:
-      ss << "action cancelled'";
-      break;
-    case BT::INVALID_GOAL:
-      ss << "invalid goal'";
-      break;
-    default:
-      break;
-  }
-
-  this->config().blackboard->set(ERROR_MESSAGE_KEY, ss.str());
-
-  return BT::NodeStatus::FAILURE;
-}
-
 bool TriggerServiceNode::setRequest(typename Request::SharedPtr& /*request*/)
 {
   return true;

snp_application/src/bt/snp_sequence_with_memory_node.cpp

@@ -14,7 +14,7 @@
  * SOFTWARE.
  */
 
-#include "bt/snp_sequence_with_memory_node.h"
+#include <snp_application/bt/snp_sequence_with_memory_node.h>
 
 namespace snp_application
 {

snp_application/src/bt/text_edit_logger.cpp

@@ -1,5 +1,5 @@
-#include "bt/text_edit_logger.h"
-#include "bt/utils.h"
+#include <snp_application/bt/text_edit_logger.h>
+#include <snp_application/bt/utils.h>
 
 #include <QTextCursor>
 #include <QTextEdit>

snp_application/src/main.cpp

@@ -1,4 +1,4 @@
-#include "snp_widget.h"
+#include <snp_application/snp_widget.h>
 #include <QApplication>
 
 #include <rclcpp/rclcpp.hpp>

snp_application/src/snp_panel.cpp

@@ -1,4 +1,4 @@
-#include "snp_widget.h"
+#include <snp_application/snp_widget.h>
 
 #include <QMessageBox>
 #include <QVBoxLayout>