Removed iri_base_bt_client style

CMakeLists.txt

@@ -7,7 +7,7 @@ add_definitions(-std=c++11)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib trajectory_msgs std_srvs control_msgs iri_base_algorithm iri_base_bt_client iri_behaviortree)
+find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib trajectory_msgs std_srvs control_msgs iri_base_algorithm iri_behaviortree)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -106,7 +106,7 @@ set(module_bt_name tiago_gripper_module_bt)
 catkin_package(
  INCLUDE_DIRS include
  LIBRARIES ${module_name} ${module_bt_name}
- CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib trajectory_msgs std_srvs control_msgs iri_base_algorithm iri_base_bt_client iri_behaviortree
+ CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib trajectory_msgs std_srvs control_msgs iri_base_algorithm iri_behaviortree
 
 #  DEPENDS system_lib
 )
@@ -165,7 +165,7 @@ add_dependencies(${module_bt_name} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_
 
 #BT_client
 set(bt_client_name tiago_gripper_bt_client)
-add_executable(${bt_client_name} src/tiago_gripper_bt_client_alg_node.cpp)
+add_executable(${bt_client_name} src/tiago_gripper_bt_client_alg.cpp src/tiago_gripper_bt_client_alg_node.cpp)
 target_link_libraries(${bt_client_name} ${catkin_LIBRARIES})
 target_link_libraries(${bt_client_name} ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/lib${module_name}.so)
 target_link_libraries(${bt_client_name} ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_LIB_DESTINATION}/lib${module_bt_name}.so)

include/tiago_gripper_bt_client_alg.h

+// Copyright (C) 2010-2011 Institut de Robotica i Informatica Industrial, CSIC-UPC.
+// Author
+// All rights reserved.
+//
+// This file is part of iri-ros-pkg
+// iri-ros-pkg is free software: you can redistribute it and/or modify
+// it under the terms of the GNU Lesser General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU Lesser General Public License for more details.
+//
+// You should have received a copy of the GNU Lesser General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+//
+// IMPORTANT NOTE: This code has been generated through a script from the
+// iri_ros_scripts. Please do NOT delete any comments to guarantee the correctness
+// of the scripts. ROS topics can be easly add by using those scripts. Please
+// refer to the IRI wiki page for more information:
+// http://wikiri.upc.es/index.php/Robotics_Lab
+
+#ifndef _tiago_gripper_bt_client_alg_h_
+#define _tiago_gripper_bt_client_alg_h_
+
+#include <tiago_gripper_module/TiagoGripperBtClientConfig.h>
+
+//include tiago_gripper_client_alg main library
+
+/**
+ * \brief IRI ROS Specific Driver Class
+ *
+ *
+ */
+class TiagoGripperBtClientAlgorithm
+{
+  protected:
+   /**
+    * \brief define config type
+    *
+    * Define a Config type with the TiagoGripperBtClientConfig. All driver implementations
+    * will then use the same variable type Config.
+    */
+    pthread_mutex_t access_;
+
+    // private attributes and methods
+
+  public:
+   /**
+    * \brief define config type
+    *
+    * Define a Config type with the TiagoGripperBtClientConfig. All driver implementations
+    * will then use the same variable type Config.
+    */
+    typedef tiago_gripper_module::TiagoGripperBtClientConfig Config;
+
+   /**
+    * \brief config variable
+    *
+    * This variable has all the driver parameters defined in the cfg config file.
+    * Is updated everytime function config_update() is called.
+    */
+    Config config_;
+
+   /**
+    * \brief constructor
+    *
+    * In this constructor parameters related to the specific driver can be
+    * initalized. Those parameters can be also set in the openDriver() function.
+    * Attributes from the main node driver class IriBaseDriver such as loop_rate,
+    * may be also overload here.
+    */
+    TiagoGripperBtClientAlgorithm(void);
+
+   /**
+    * \brief Lock Algorithm
+    *
+    * Locks access to the Algorithm class
+    */
+    void lock(void) { pthread_mutex_lock(&this->access_); };
+
+   /**
+    * \brief Unlock Algorithm
+    *
+    * Unlocks access to the Algorithm class
+    */
+    void unlock(void) { pthread_mutex_unlock(&this->access_); };
+
+   /**
+    * \brief Tries Access to Algorithm
+    *
+    * Tries access to Algorithm
+    *
+    * \return true if the lock was adquired, false otherwise
+    */
+    bool try_enter(void)
+    {
+      if(pthread_mutex_trylock(&this->access_)==0)
+        return true;
+      else
+        return false;
+    };
+
+   /**
+    * \brief config update
+    *
+    * In this function the driver parameters must be updated with the input
+    * config variable. Then the new configuration state will be stored in the
+    * Config attribute.
+    *
+    * \param new_cfg the new driver configuration state
+    *
+    * \param level level in which the update is taken place
+    */
+    void config_update(Config& config, uint32_t level=0);
+
+    // here define all tiago_gripper_client_alg interface methods to retrieve and set
+    // the driver parameters
+
+   /**
+    * \brief Destructor
+    *
+    * This destructor is called when the object is about to be destroyed.
+    *
+    */
+    ~TiagoGripperBtClientAlgorithm(void);
+};
+
+#endif

include/tiago_gripper_bt_client_alg_node.h

@@ -25,10 +25,13 @@
 #ifndef _tiago_gripper_module_bt_client_alg_node_h_
 #define _tiago_gripper_module_bt_client_alg_node_h_
 
-#include <iri_base_bt_client/iri_base_bt_client.h>
-#include <tiago_gripper_module/TiagoGripperBtClientConfig.h>
-
+#include <iri_base_algorithm/iri_base_algorithm.h>
+#include "tiago_gripper_bt_client_alg.h"
 #include "tiago_gripper_module/tiago_gripper_module_bt.h"
+#include "iri_bt_factory.h"
+#include "behaviortree_cpp_v3/loggers/bt_cout_logger.h"
+#include "behaviortree_cpp_v3/loggers/bt_zmq_publisher.h"
+
 
 // [publisher subscriber headers]
 
@@ -40,12 +43,22 @@
  * \brief IRI ROS Specific Algorithm Class
  *
  */
-class TiagoGripperModuleBtClientAlgNode : public behaviortree_base::IriBaseBTClient<tiago_gripper_module::TiagoGripperBtClientConfig>
+class TiagoGripperModuleBtClientAlgNode : public algorithm_base::IriBaseAlgorithm<TiagoGripperBtClientAlgorithm>
 {
   private:
     // object of module
     CTiagoGripperModuleBT tiago_gripper_module_bt;
 
+    IriBehaviorTreeFactory factory;
+    BT::Tree tree;
+    BT::NodeStatus status;
+
+    BT::StdCoutLogger   *logger_cout;
+    BT::PublisherZMQ *publisher_zmq;
+    std::string path;
+    std::string tree_xml_file;
+    bool send_ticks;
+
     // Client actions and conditions (always return BT::NodeStatus::SUCCESS)
 
     // [publisher attributes]
@@ -78,6 +91,19 @@ class TiagoGripperModuleBtClientAlgNode : public behaviortree_base::IriBaseBTCli
     ~TiagoGripperModuleBtClientAlgNode(void);
 
   protected:
+    /**
+    * \brief main node thread
+    *
+    * This is the main thread node function. Code written here will be executed
+    * in every node loop while the algorithm is on running state. Loop frequency 
+    * can be tuned by modifying loop_rate attribute.
+    *
+    * Here data related to the process loop or to ROS topics (mainly data structs
+    * related to the MSG and SRV files) must be updated. ROS publisher objects 
+    * must publish their data in this process. ROS client servers may also
+    * request data to the corresponding server topics.
+    */
+    void mainNodeThread(void);
 
    /**
     * \brief dynamic reconfigure server callback
@@ -91,7 +117,7 @@ class TiagoGripperModuleBtClientAlgNode : public behaviortree_base::IriBaseBTCli
     * \param level  integer referring the level in which the configuration
     *               has been changed.
     */
-    void node_config_update(tiago_gripper_module::TiagoGripperBtClientConfig &config, uint32_t level);
+    void node_config_update(Config &config, uint32_t level);
 
    /**
     * \brief node add diagnostics

launch/tiago_gripper_bt_client.launch

 <?xml version="1.0" encoding="UTF-8"?>
 <launch>
   <arg name="node_name" default="gripper_client" />
-  <arg name="output" default="log" />
+  <arg name="output" default="screen" />
   <arg name="launch_prefix" default="" />
   <arg name="config_file" default="$(find tiago_gripper_module)/config/tiago_gripper_module_default.yaml" />
   <arg name="gripper_action" default="/gripper_controller/follow_joint_trajectory"/>

launch/tiago_gripper_bt_client_sim.launch

@@ -2,7 +2,7 @@
 <launch>
 
   <arg name="node_name" default="gripper_client" />
-  <arg name="output" default="log" />
+  <arg name="output" default="screen" />
   <arg name="launch_prefix" default="" />
   <arg name="config_file" default="$(find tiago_gripper_module)/config/tiago_gripper_module_default.yaml" />
   <arg name="gripper_action" default="/gripper_controller/follow_joint_trajectory"/>
@@ -17,7 +17,7 @@
     <arg name="robot" value="steel" />
   </include>
 
-  <include file="$(find tiago_gripper_module)/launch/tiago_gripper_client.launch">
+  <include file="$(find tiago_gripper_module)/launch/tiago_gripper_bt_client.launch">
     <arg name="node_name"       value="$(arg node_name)" />
     <arg name="output"          value="$(arg output)" />
     <arg name="launch_prefix"   value="$(arg launch_prefix)" />

package.xml

@@ -48,7 +48,6 @@
   <build_depend>std_srvs</build_depend>
   <build_depend>control_msgs</build_depend>
   <build_depend>iri_base_algorithm</build_depend>
-  <build_depend>iri_base_bt_client</build_depend>
   <build_depend>iri_behaviortree</build_depend>
   <!-- new dependencies -->
   <!--<build_depend>new build dependency</build_depend>-->
@@ -60,7 +59,6 @@
   <run_depend>std_srvs</run_depend>
   <run_depend>control_msgs</run_depend>
   <run_depend>iri_base_algorithm</run_depend>
-  <run_depend>iri_base_bt_client</run_depend>
   <run_depend>iri_behaviortree</run_depend>
   <!-- new dependencies -->
   <!--<run_depend>new run dependency</run_depend>-->

src/tiago_gripper_bt_client_alg.cpp

+#include "tiago_gripper_bt_client_alg.h"
+
+TiagoGripperBtClientAlgorithm::TiagoGripperBtClientAlgorithm(void)
+{
+  pthread_mutex_init(&this->access_,NULL);
+}
+
+TiagoGripperBtClientAlgorithm::~TiagoGripperBtClientAlgorithm(void)
+{
+  pthread_mutex_destroy(&this->access_);
+}
+
+void TiagoGripperBtClientAlgorithm::config_update(Config& config, uint32_t level)
+{
+  this->lock();
+
+  // save the current configuration
+  this->config_=config;
+
+  this->unlock();
+}
+
+// TiagoGripperBtClientAlgorithm Public API

src/tiago_gripper_bt_client_alg_node.cpp

 #include "tiago_gripper_bt_client_alg_node.h"
 
 TiagoGripperModuleBtClientAlgNode::TiagoGripperModuleBtClientAlgNode(void) :
-  behaviortree_base::IriBaseBTClient<tiago_gripper_module::TiagoGripperBtClientConfig>(),
+  algorithm_base::IriBaseAlgorithm<TiagoGripperBtClientAlgorithm>(),
   tiago_gripper_module_bt()
 {
+  this->setRate(1);
+
+  if(!this->private_node_handle_.getParam("path",this->path))
+    ROS_ERROR("TiagoGripperModuleBtClientAlgNode: need to define path parameter");
+  else
+    ROS_DEBUG("TiagoGripperModuleBtClientAlgNode: path set to: %s", this->path.c_str());
+
+  if(!this->private_node_handle_.getParam("tree_xml_file",this->tree_xml_file))
+    ROS_ERROR("TiagoGripperModuleBtClientAlgNode: need to define tree_xml_file parameter");
+  else
+    ROS_DEBUG("TiagoGripperModuleBtClientAlgNode: tree_xml_file set to: %s", this->tree_xml_file.c_str());
+
+  // std::cout << xml_path << "/" << xml_file << std::endl;
+  this->tiago_gripper_module_bt.init(this->factory);
+  try
+  {
+    std::string tree_string = this->path + "/" + this->tree_xml_file + ".xml";
+    this->tree = this->factory.createTreeFromFile(tree_string);
+  }
+  catch(BT::RuntimeError &e)
+  {
+    ROS_ERROR_STREAM("IriJointsClientBtAlgNode::Constructor -> Exception caught: " << e.what());
+  }
+
+  this->logger_cout      = new BT::StdCoutLogger(this->tree);
+  this->publisher_zmq    = new BT::PublisherZMQ(this->tree);
+  this->send_ticks = false;
   // [init publishers]
 
   // [init subscribers]
@@ -15,12 +42,45 @@ TiagoGripperModuleBtClientAlgNode::TiagoGripperModuleBtClientAlgNode(void) :
   // [init action servers]
 
   // [init action clients]
-  this->tiago_gripper_module_bt.init(factory);
 }
 
 TiagoGripperModuleBtClientAlgNode::~TiagoGripperModuleBtClientAlgNode(void)
 {
   // [free dynamic memory]
+  if (this->logger_cout != NULL)
+  {
+    delete this->logger_cout;
+    this->logger_cout = NULL;
+  }
+  if (this->publisher_zmq != NULL)
+  {
+    delete this->publisher_zmq;
+    this->publisher_zmq = NULL;
+  }
+}
+
+void TiagoGripperModuleBtClientAlgNode::mainNodeThread(void)
+{
+  // [fill msg structures]
+  
+  // [fill srv structure and make request to the server]
+
+  // [publish messages]
+  try
+  {
+    if (send_ticks)
+    {
+      ROS_INFO("---Tick---");
+      this->status = this->tree.tickRoot();
+      if (this->status != BT::NodeStatus::RUNNING)
+        this->send_ticks = false;
+      ROS_INFO_STREAM("Status: " << this->status);
+    }
+  }
+  catch(BT::LogicError &e)
+  {
+    ROS_ERROR_STREAM("TiagoGripperModuleBtClientAlgNode::mainNodeThread -> EXCEPTION caught:" << e.what());
+  }
 }
 
 /*  [subscriber callbacks] */
@@ -34,22 +94,22 @@ TiagoGripperModuleBtClientAlgNode::~TiagoGripperModuleBtClientAlgNode(void)
 
 void TiagoGripperModuleBtClientAlgNode::node_config_update(tiago_gripper_module::TiagoGripperBtClientConfig &config, uint32_t level)
 {
-  this->lock();
+  this->alg_.lock();
 
   if(config.START)
   {
-    this->core.start_tree();
+    this->send_ticks = true;
     config.START=false;
   }
 
   if(config.RESTART)
   {
-    this->core.stop_tree();
+    this->send_ticks = true;
     config.RESTART=false;
   }
 
-  this->config_=config;
-  this->unlock();
+  this->alg_.config_=config;
+  this->alg_.unlock();
 }
 
 void TiagoGripperModuleBtClientAlgNode::addNodeDiagnostics(void)
@@ -59,5 +119,5 @@ void TiagoGripperModuleBtClientAlgNode::addNodeDiagnostics(void)
 /* main function */
 int main(int argc,char *argv[])
 {
-  return behaviortree_base::main<TiagoGripperModuleBtClientAlgNode>(argc, argv, "tiago_gripper_module_bt_client_alg_node");
+  return algorithm_base::main<TiagoGripperModuleBtClientAlgNode>(argc, argv, "tiago_gripper_module_bt_client_alg_node");
 }

src/xml/bt_test.xml

@@ -3,7 +3,7 @@
     <!-- ////////// -->
     <BehaviorTree ID="BehaviorTree">
         <SequenceStar>
-            <SetBlackboard output_key="duration" value="0.5"/>
+            <SetBlackboard output_key="duration" value="2.0"/>
             <Action ID="async_close_tiago_gripper" duration="{duration}"/>
             <SetBlackboard output_key="distance" value="0.2"/>
             <Action ID="async_fingers_distance_tiago_gripper" distance="{distance}" duration="{duration}"/>
@@ -16,7 +16,7 @@
         <SequenceStar>
             <SetBlackboard output_key="left_finger" value="0.2"/>
             <SetBlackboard output_key="right_finger" value="0.1"/>
-            <SetBlackboard output_key="duration" value="0.8"/>
+            <SetBlackboard output_key="duration" value="3.0"/>
             <SetBlackboard output_key="new_goal" value="True"/>
         </SequenceStar>
     </BehaviorTree>