diff --git a/CMakeLists.txt b/CMakeLists.txt
index e87f5798a6554b17709f70f419453bf560aec946..343e1643b8af14f77c31f8f322484e692ce83300 100644
--- a/CMakeLists.txt
+++ b/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 play_motion_msgs iri_base_algorithm iri_behaviortree)
+find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib play_motion_msgs iri_base_algorithm iri_base_bt_client iri_behaviortree)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -88,6 +88,7 @@ find_package(iriutils REQUIRED)
generate_dynamic_reconfigure_options(
cfg/TiagoPlayMotionModule.cfg
cfg/TiagoPlayMotionClient.cfg
+ cfg/TiagoPlayMotionBtClient.cfg
)
###################################
@@ -104,7 +105,7 @@ set(module_bt_name tiago_play_motion_module_bt)
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${module_name} ${module_bt_name}
- CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib play_motion_msgs iri_base_algorithm iri_behaviortree
+ CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib play_motion_msgs iri_base_algorithm iri_base_bt_client iri_behaviortree
# DEPENDS system_lib
)
@@ -158,6 +159,14 @@ target_link_libraries(${module_bt_name} ${catkin_LIBRARIES})
target_link_libraries(${module_bt_name} ${iriutils_LIBRARIES})
add_dependencies(${module_bt_name} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${module_name})
+#BT_client
+set(bt_client_name tiago_play_motion_bt_client)
+add_executable(${bt_client_name} src/tiago_play_motion_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)
+add_dependencies(${bt_client_name} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${module_name} ${module_bt_name})
+
#############
## Install ##
#############
diff --git a/cfg/TiagoPlayMotionBtClient.cfg b/cfg/TiagoPlayMotionBtClient.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..8f480c977966d487d2cabca61e36a55cde9dabd4
--- /dev/null
+++ b/cfg/TiagoPlayMotionBtClient.cfg
@@ -0,0 +1,50 @@
+#! /usr/bin/env python
+#* All rights reserved.
+#*
+#* 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 the Willow Garage 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.
+#***********************************************************
+
+# Author:
+
+PACKAGE='tiago_play_motion_module'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+from iri_base_bt_client.submodule import add_bt_client_params
+
+gen = ParameterGenerator()
+
+# Name Type Reconfiguration level Description Default Min Max
+#gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
+add_bt_client_params(gen)
+gen.add("START", bool_t, 0, "synchronous start", False)
+gen.add("RESTART", bool_t, 0, "asynchronous restart of BT", False)
+gen.add("motion_name", str_t, 0, "Motion name to execute", "")
+gen.add("set_motion", bool_t, 0, "Execute selected motion", False)
+# gen.add("cancel_motion", bool_t, 0, "Cancel current motion", False)
+
+exit(gen.generate(PACKAGE, "TiagoPlayMotionModuleBtClientAlgNode", "TiagoPlayMotionBtClient"))
diff --git a/include/tiago_play_motion_bt_client_alg_node.h b/include/tiago_play_motion_bt_client_alg_node.h
new file mode 100644
index 0000000000000000000000000000000000000000..8ee14650b8f5a4c9de3d203a6f1cb0f1f9fbced1
--- /dev/null
+++ b/include/tiago_play_motion_bt_client_alg_node.h
@@ -0,0 +1,132 @@
+// Copyright (C) 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_play_motion_module_bt_client_alg_node_h_
+#define _tiago_play_motion_module_bt_client_alg_node_h_
+
+#include <iri_base_bt_client/iri_base_bt_client.h>
+#include <tiago_play_motion_module/TiagoPlayMotionBtClientConfig.h>
+
+#include "tiago_play_motion_module/tiago_play_motion_bt_module.h"
+#include <sensor_msgs/JointState.h>
+#include <geometry_msgs/PoseStamped.h>
+
+// [publisher subscriber headers]
+
+// [service client headers]
+
+// [action server client headers]
+
+/**
+ * \brief IRI ROS Specific Algorithm Class
+ *
+ */
+class TiagoPlayMotionModuleBtClientAlgNode : public behaviortree_base::IriBaseBTClient<tiago_play_motion_module::TiagoPlayMotionBtClientConfig>
+{
+ private:
+ // object of module
+ CTiagoPlayMotionModuleBT tiago_play_motion_module_bt;
+
+ // Client actions and conditions (always return BT::NodeStatus::SUCCESS)
+
+ // [publisher attributes]
+
+ // [subscriber attributes]
+
+ // [service attributes]
+
+ // [client attributes]
+
+ // [action server attributes]
+
+ // [action client attributes]
+
+ bool set_motion;
+ /**
+ * \brief Async function to set the tiago_play_motion_execute_motion input ports.
+ *
+ * It takes the values from Dynamic Reconfigure.
+ *
+ * It has the following output ports:
+ *
+ * motion_name (std::string): The motion name to be executed.
+ *
+ * new_goal (bool): If it's a new_goal.
+ *
+ * \param self Self node with the required ports:
+ *
+ * \return a BT:NodeStatus indicating whether the request has been
+ * successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
+ * If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
+ *
+ */
+ BT::NodeStatus set_tiago_play_motion_execute_motion_inputs(BT::TreeNode& self);
+
+ public:
+ /**
+ * \brief Constructor
+ *
+ * This constructor initializes specific class attributes and all ROS
+ * communications variables to enable message exchange.
+ */
+ TiagoPlayMotionModuleBtClientAlgNode(void);
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor frees all necessary dynamic memory allocated within this
+ * this class.
+ */
+ ~TiagoPlayMotionModuleBtClientAlgNode(void);
+
+ protected:
+
+ /**
+ * \brief dynamic reconfigure server callback
+ *
+ * This method is called whenever a new configuration is received through
+ * the dynamic reconfigure. The derivated generic algorithm class must
+ * implement it.
+ *
+ * \param config an object with new configuration from all algorithm
+ * parameters defined in the config file.
+ * \param level integer referring the level in which the configuration
+ * has been changed.
+ */
+ void node_config_update(tiago_play_motion_module::TiagoPlayMotionBtClientConfig &config, uint32_t level);
+
+ /**
+ * \brief node add diagnostics
+ *
+ * In this abstract function additional ROS diagnostics applied to the
+ * specific algorithms may be added.
+ */
+ void addNodeDiagnostics(void);
+
+ // [diagnostic functions]
+
+ // [test functions]
+};
+
+#endif
\ No newline at end of file
diff --git a/launch/tiago_play_motion_bt_client.launch b/launch/tiago_play_motion_bt_client.launch
new file mode 100644
index 0000000000000000000000000000000000000000..385897bb284947e5f65f2971a1ecc03bab1336c1
--- /dev/null
+++ b/launch/tiago_play_motion_bt_client.launch
@@ -0,0 +1,28 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+
+ <arg name="tree_path" default="$(find tiago_play_motion_module)/src/xml" />
+ <arg name="tree_file" default="bt_test" />
+ <arg name="bt_client_rate" default="10" />
+
+ <!-- launch the play motion client node -->
+ <node name="tiago_play_motion_client"
+ pkg="tiago_play_motion_module"
+ type="tiago_play_motion_bt_client"
+ output="screen"
+ ns="/tiago">
+ <remap from="~/tiago_play_motion_module/play_motion"
+ to="/play_motion"/>
+
+ <rosparam file="$(find tiago_play_motion_module)/config/tiago_play_motion_module_default.yaml" command="load" ns="tiago_play_motion_module" />
+ <param name="path" value="$(arg tree_path)"/>
+ <param name="tree_xml_file" value="$(arg tree_file)"/>
+ <param name="bt_client_rate" value="$(arg bt_client_rate)"/>
+ </node>
+
+ <!-- launch dynamic reconfigure -->
+ <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
+ output="screen"/>
+
+</launch>
+
diff --git a/launch/tiago_play_motion_bt_client_sim.launch b/launch/tiago_play_motion_bt_client_sim.launch
new file mode 100644
index 0000000000000000000000000000000000000000..2010ebebe7dc22b6ed5b11febe956f0cf74dad11
--- /dev/null
+++ b/launch/tiago_play_motion_bt_client_sim.launch
@@ -0,0 +1,20 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+
+ <arg name="tree_path" default="$(find tiago_play_motion_module)/src/xml" />
+ <arg name="tree_file" default="bt_test" />
+ <arg name="bt_client_rate" default="10" />
+
+ <include file="$(find tiago_gazebo)/launch/tiago_gazebo.launch">
+ <arg name="public_sim" value="true" />
+ <arg name="robot" value="steel" />
+ </include>
+
+ <include file="$(find tiago_play_motion_module)/launch/tiago_play_motion_bt_client.launch">
+ <arg name="tree_path" value="$(arg tree_path)"/>
+ <arg name="tree_file" value="$(arg tree_file)"/>
+ <arg name="bt_client_rate" value="$(arg bt_client_rate)"/>
+ </include>
+
+</launch>
+
diff --git a/launch/tiago_play_motion_client.launch b/launch/tiago_play_motion_client.launch
index 738b3d122741b58720af04eabac611186d9f6cb8..2e191e503b18b2ac9523a85d63ac85acb094385d 100644
--- a/launch/tiago_play_motion_client.launch
+++ b/launch/tiago_play_motion_client.launch
@@ -1,3 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
<launch>
<!-- launch the play motion client node -->
diff --git a/launch/tiago_play_motion_client_sim.launch b/launch/tiago_play_motion_client_sim.launch
index f1531f952bbe720d3cd947869281eb86f6e8af56..5ddfe129ba45ad6a09c92391a10ea2d4f08efaca 100644
--- a/launch/tiago_play_motion_client_sim.launch
+++ b/launch/tiago_play_motion_client_sim.launch
@@ -1,3 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
<launch>
<include file="$(find tiago_gazebo)/launch/tiago_gazebo.launch">
diff --git a/package.xml b/package.xml
index 6bf92af93aade2e6a330dbea17892c73be69ab05..26cb188efa247bcc51dd8ef5aaec6a36369ef6e3 100644
--- a/package.xml
+++ b/package.xml
@@ -46,6 +46,7 @@
<build_depend>actionlib</build_depend>
<build_depend>play_motion_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>-->
@@ -55,6 +56,7 @@
<run_depend>actionlib</run_depend>
<run_depend>play_motion_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>-->
diff --git a/src/tiago_play_motion_bt_client_alg_node.cpp b/src/tiago_play_motion_bt_client_alg_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f79cb95eca4c7202f718c9d9482e3d1dfc11d000
--- /dev/null
+++ b/src/tiago_play_motion_bt_client_alg_node.cpp
@@ -0,0 +1,88 @@
+#include "tiago_play_motion_bt_client_alg_node.h"
+
+TiagoPlayMotionModuleBtClientAlgNode::TiagoPlayMotionModuleBtClientAlgNode(void) :
+ behaviortree_base::IriBaseBTClient<tiago_play_motion_module::TiagoPlayMotionBtClientConfig>(),
+ tiago_play_motion_module_bt()
+{
+ // [init publishers]
+
+ // [init subscribers]
+
+ // [init services]
+
+ // [init clients]
+
+ // [init action servers]
+
+ // [init action clients]
+
+ this->set_motion = false;
+ this->tiago_play_motion_module_bt.init(this->factory);
+
+ // Init local functions
+ BT::PortsList async_execute_motion_ports = {BT::OutputPort<std::string>("motion_name"), BT::OutputPort<bool>("new_goal")};
+
+ this->factory.registerIriAsyncAction("set_tiago_play_motion_execute_motion_inputs", std::bind(&TiagoPlayMotionModuleBtClientAlgNode::set_tiago_play_motion_execute_motion_inputs, this, std::placeholders::_1), async_execute_motion_ports);
+}
+
+TiagoPlayMotionModuleBtClientAlgNode::~TiagoPlayMotionModuleBtClientAlgNode(void)
+{
+ // [free dynamic memory]
+}
+
+/* [subscriber callbacks] */
+
+/* [service callbacks] */
+
+/* [action callbacks] */
+
+/* [action requests] */
+
+
+void TiagoPlayMotionModuleBtClientAlgNode::node_config_update(tiago_play_motion_module::TiagoPlayMotionBtClientConfig &config, uint32_t level)
+{
+ this->lock();
+
+ if(config.START)
+ {
+ this->core.start_tree();
+ config.START=false;
+ }
+
+ if(config.RESTART)
+ {
+ this->core.stop_tree();
+ config.RESTART=false;
+ }
+
+ if (config.set_motion)
+ {
+ this->set_motion = true;
+ config.set_motion = false;
+ }
+
+ this->config_=config;
+ this->unlock();
+}
+
+BT::NodeStatus TiagoPlayMotionModuleBtClientAlgNode::set_tiago_play_motion_execute_motion_inputs(BT::TreeNode& self)
+{
+ if (this->set_motion)
+ {
+ this->set_motion = false;
+ self.setOutput("motion_name", this->config_.motion_name);
+ self.setOutput("new_goal", true);
+ return BT::NodeStatus::SUCCESS;
+ }
+ return BT::NodeStatus::RUNNING;
+}
+
+void TiagoPlayMotionModuleBtClientAlgNode::addNodeDiagnostics(void)
+{
+}
+
+/* main function */
+int main(int argc,char *argv[])
+{
+ return behaviortree_base::main<TiagoPlayMotionModuleBtClientAlgNode>(argc, argv, "tiago_play_motion_module_bt_client_alg_node");
+}
diff --git a/src/tiago_play_motion_bt_module.cpp b/src/tiago_play_motion_bt_module.cpp
index a2bf1f069f5acc0f8354633ed84d4d34bdfe3926..36d75f55f43519e2ed02bb494fda1e7cac4674bb 100644
--- a/src/tiago_play_motion_bt_module.cpp
+++ b/src/tiago_play_motion_bt_module.cpp
@@ -41,7 +41,6 @@ void CTiagoPlayMotionModuleBT::init(IriBehaviorTreeFactory &factory)
factory.registerSimpleAction("sync_tiago_play_motion_execute_motion", std::bind(&CTiagoPlayMotionModuleBT::sync_tiago_play_motion_execute_motion, this, std::placeholders::_1), sync_execute_motion_ports);
factory.registerIriAsyncAction("async_tiago_play_motion_execute_motion", std::bind(&CTiagoPlayMotionModuleBT::async_tiago_play_motion_execute_motion, this, std::placeholders::_1), async_execute_motion_ports);
-
}
CTiagoPlayMotionModuleBT::~CTiagoPlayMotionModuleBT(void)
diff --git a/src/xml/bt_test.xml b/src/xml/bt_test.xml
new file mode 100644
index 0000000000000000000000000000000000000000..06253839fbece29130fccd1579e8c3c196860a7c
--- /dev/null
+++ b/src/xml/bt_test.xml
@@ -0,0 +1,61 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="BehaviorTree">
+ <!-- ////////// -->
+ <BehaviorTree ID="BehaviorTree">
+ <SequenceStar>
+ <Action ID="async_is_start_tree"/>
+ <SubTree ID="execute_motion"/>
+ </SequenceStar>
+ </BehaviorTree>
+ <!-- ////////// -->
+ <BehaviorTree ID="execute_motion">
+ <SequenceStar>
+ <Action ID="set_tiago_play_motion_execute_motion_inputs" motion_name="{motion_name}" new_goal="{new_goal}"/>
+ <Action ID="async_tiago_play_motion_execute_motion" motion_name="{motion_name}" new_goal="{new_goal}"/>
+ </SequenceStar>
+ </BehaviorTree>
+ <!-- ////////// -->
+ <TreeNodesModel>
+ <!-- basic_nodes -->
+ <Action ID="NOP"/>
+ <Action ID="is_not_start_tree"/>
+ <Condition ID="async_is_start_tree"/>
+ <Action ID="is_not_stop_tree"/>
+ <Condition ID="async_is_stop_tree"/>
+ <Action ID="set_start_tree"/>
+ <Action ID="set_stop_tree"/>
+ <Action ID="reset_start_tree"/>
+ <Action ID="reset_stop_tree"/>
+ <!-- tiago_play_motion_module -->
+ <Action ID="sync_cancel_tiago_play_motion_action"/>
+ <Action ID="async_cancel_tiago_play_motion_action"/>
+ <Action ID="async_is_tiago_play_motion_finished"/>
+ <Action ID="sync_tiago_play_motion_execute_motion">
+ <input_port name="motion_name"> desired motion to be executed</input_port>
+ </Action>
+ <Action ID="async_tiago_play_motion_execute_motion">
+ <input_port name="motion_name"> desired motion to be executed</input_port>
+ <input_port default="True" name="new_goal"> If it's a new_goal</input_port>
+ </Action>
+ <Condition ID="is_tiago_play_motion_finished"/>
+ <Condition ID="is_tiago_play_motion_module_running"/>
+ <Condition ID="is_tiago_play_motion_module_success"/>
+ <Condition ID="is_tiago_play_motion_module_action_start_fail"/>
+ <Condition ID="is_tiago_play_motion_module_timeout"/>
+ <Condition ID="is_tiago_play_motion_module_fb_watchdog"/>
+ <Condition ID="is_tiago_play_motion_module_aborted"/>
+ <Condition ID="is_tiago_play_motion_module_preempted"/>
+ <Condition ID="is_tiago_play_motion_module_rejected"/>
+ <Condition ID="is_tiago_play_motion_module_invalid_id"/>
+ <!-- Client -->
+ <Action ID="set_tiago_play_motion_execute_motion_inputs">
+ <output_port name="motion_name"> desired motion to be executed</output_port>
+ <output_port default="True" name="new_goal"> If it's a new_goal</output_port>
+ </Action>
+ <!-- Tree -->
+ <SubTree ID="execute_motion"/>
+ </TreeNodesModel>
+ <!-- ////////// -->
+</root>
+
+