diff --git a/CMakeLists.txt b/CMakeLists.txt
index be57aab126864ef4eebfb3cb64fd1e0596fc4062..302fbee3db76401f0b3f4c8c3e60e67df0bac391 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 std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs iri_base_algorithm iri_behaviortree)
+find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs iri_base_algorithm iri_base_bt_client iri_behaviortree)
find_package(pal_waypoint_msgs)
if(${pal_waypoint_msgs_FOUND})
@@ -95,6 +95,7 @@ find_package(iriutils REQUIRED)
generate_dynamic_reconfigure_options(
cfg/TiagoNavModule.cfg
cfg/TiagoNavClient.cfg
+ cfg/TiagoNavBtClient.cfg
)
###################################
@@ -112,7 +113,7 @@ set(module_bt_name tiago_nav_module_bt)
catkin_package(
INCLUDE_DIRS include
LIBRARIES ${module_name} ${module_bt_name}
- CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs ${pal_waypoint_msgs_catkin_depends} iri_base_algorithm iri_behaviortree
+ CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib std_srvs move_base_msgs tf nav_msgs pal_navigation_msgs ${pal_waypoint_msgs_catkin_depends} iri_base_algorithm iri_base_bt_client iri_behaviortree
# DEPENDS system_lib
)
@@ -161,6 +162,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_nav_bt_client)
+add_executable(${bt_client_name} src/tiago_nav_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/TiagoNavBtClient.cfg b/cfg/TiagoNavBtClient.cfg
new file mode 100755
index 0000000000000000000000000000000000000000..7a94317cc79aaf401f16d7423a7d2a15d7de4769
--- /dev/null
+++ b/cfg/TiagoNavBtClient.cfg
@@ -0,0 +1,113 @@
+#! /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_nav_module'
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+from iri_base_bt_client.submodule import add_bt_client_params
+
+gen = ParameterGenerator()
+move_joints = gen.add_group("Move nav in joints space")
+move_pose = gen.add_group("Move nav in cartesian space")
+# objects = gen.add_group("Objects in the environment")
+# const = gen.add_group("Path constraints")
+
+# 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)
+
+nav_to_move_enum = gen.enum([ gen.const("tiago_nav", int_t, 0, "Tiago Nav"),
+ gen.const("ivo_left_nav", int_t, 1, "Ivo left nav")],
+ "The posibles navs to control")
+move_joints.add("set_joints", bool_t, 0, "Set selected angles", False)
+move_joints.add("nav_to_move", int_t, 0, "The nav to move", 0, 0, 1, edit_method=nav_to_move_enum)
+move_joints.add("torso", double_t, 0, "Target angle for torso", 0.15, 0.0, 0.35)
+move_joints.add("joint1", double_t, 0, "Target angle for joint 1", 0.2, -3.14159, 3.14159)
+move_joints.add("joint2", double_t, 0, "Target angle for joint 2", -1.3, -3.14159, 3.14159)
+move_joints.add("joint3", double_t, 0, "Target angle for joint 3", -0.2, -3.14159, 3.14159)
+move_joints.add("joint4", double_t, 0, "Target angle for joint 4", 2.0, -3.14159, 3.14159)
+move_joints.add("joint5", double_t, 0, "Target angle for joint 5", -1.5, -3.14159, 3.14159)
+move_joints.add("joint6", double_t, 0, "Target angle for joint 6", 1.4, -3.14159, 3.14159)
+move_joints.add("joint7", double_t, 0, "Target angle for joint 7", 0.0, -3.14159, 3.14159)
+move_joints.add("joint_tol", double_t, 0, "Target angle tolerance", 0.1, -3.14159, 3.14159)
+
+move_pose.add("set_pose", bool_t, 0, "Set selected pose", False)
+move_pose.add("plan_frame_id", str_t, 0, "Target pose reference frame", "/base_footprint")
+move_pose.add("x_pos", double_t, 0, "Target X position", 0.5, -2.0, 2.0)
+move_pose.add("y_pos", double_t, 0, "Target Y position", 0.0, -2.0, 2.0)
+move_pose.add("z_pos", double_t, 0, "Target Z position", 0.6, -2.0, 2.0)
+move_pose.add("x_orientation", double_t, 0, "Target X orientation", 0.0, -3.14159,3.14159)
+move_pose.add("y_orientation", double_t, 0, "Target Y orientation", 0.0, -3.14159,3.14159)
+move_pose.add("z_orientation", double_t, 0, "Target Z orientation", 0.0, -3.14159,3.14159)
+move_pose.add("w_orientation", double_t, 0, "Target W orientation", 1.0, -3.14159,3.14159)
+move_pose.add("position_tol", double_t, 0, "Target position tolerance", 0.1, 0, 0.5)
+move_pose.add("orientation_tol", double_t, 0, "Target orientation tolerance", 0.1, 0, 3.14159)
+
+# objects.add("obj_name", str_t, 0, "Identifier of the object", "object")
+# objects.add("object_frame_id", str_t, 0, "Object pose reference frame", "/base_footprint")
+# objects.add("obj_x_pos", double_t, 0, "Object X position", 0.0, -2.0, 2.0)
+# objects.add("obj_y_pos", double_t, 0, "Object Y position", 0.0, -2.0, 2.0)
+# objects.add("obj_z_pos", double_t, 0, "Object Y position", 0.0, -2.0, 2.0)
+# objects.add("obj_x_orientation", double_t, 0, "Object X orientation", 0.0, -3.14159,3.14159)
+# objects.add("obj_y_orientation", double_t, 0, "Object Y orientation", 0.0, -3.14159,3.14159)
+# objects.add("obj_z_orientation", double_t, 0, "Object Z orientation", 0.0, -3.14159,3.14159)
+# objects.add("obj_w_orientation", double_t, 0, "Object W orientation", 0.0, -3.14159,3.14159)
+# objects.add("obj_width", double_t, 0, "Object width", 0.0, -2.0, 2.0)
+# objects.add("obj_length", double_t, 0, "Object length", 0.0, -2.0, 2.0)
+# objects.add("obj_height", double_t, 0, "Object height", 0.0, -2.0, 2.0)
+# objects.add("add_object", bool_t, 0, "Add the desired object", False)
+# objects.add("add_env", bool_t, 0, "Add object to environment", False)
+# objects.add("dis_col", bool_t, 0, "Disable collision of this object", False)
+# objects.add("remove_env", bool_t, 0, "Remove object from environment", False)
+
+# const.add("const_frame_id", str_t, 0, "Constraint reference frame", "/base_footprint")
+# const.add("const_x_orientation", double_t, 0, "Constraint X orientation", 0.0, -3.14159,3.14159)
+# const.add("const_y_orientation", double_t, 0, "Constraint Y orientation", 0.0, -3.14159,3.14159)
+# const.add("const_z_orientation", double_t, 0, "Constraint Z orientation", 0.0, -3.14159,3.14159)
+# const.add("const_w_orientation", double_t, 0, "Constraint W orientation", 0.0, -3.14159,3.14159)
+# const.add("add_orientation_const", bool_t, 0, "Add orientation constraint", False)
+# const.add("x_plane_const", bool_t, 0, "Plane on the X axis", False)
+# const.add("y_plane_const", bool_t, 0, "Plane on the Y axis", False)
+# const.add("z_plane_const", bool_t, 0, "Plane on the Z axis", False)
+# const.add("plane_offset", double_t, 0, "Plane offset position", 0.0, -2.0, 2.0)
+# const.add("plane_tol", double_t, 0, "Plane tolerance", 0.1, 0.001, 0.5)
+# const.add("add_plane_const", bool_t, 0, "Add plane constraint", False)
+# const.add("joint_name", str_t, 0, "Joint name", "")
+# const.add("joint_position", double_t, 0, "Joint position", 0.25, -3.14159,3.14159)
+# const.add("joint_tolerance", double_t, 0, "Joint tolerance", 0.1, -3.14159,3.14159)
+# const.add("add_joint_const", bool_t, 0, "Add joint constraint", False)
+# const.add("clear_const", bool_t, 0, "Clear all constraints", False)
+
+exit(gen.generate(PACKAGE, "TiagoNavModuleBtClientAlgNode", "TiagoNavBtClient"))
diff --git a/include/tiago_nav_bt_client_alg_node.h b/include/tiago_nav_bt_client_alg_node.h
new file mode 100644
index 0000000000000000000000000000000000000000..522b74a126671bd58ab964fd4e108ae937838f2d
--- /dev/null
+++ b/include/tiago_nav_bt_client_alg_node.h
@@ -0,0 +1,161 @@
+// 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_nav_module_bt_client_alg_node_h_
+#define _tiago_nav_module_bt_client_alg_node_h_
+
+#include <iri_base_bt_client/iri_base_bt_client.h>
+#include <tiago_nav_module/TiagoNavBtClientConfig.h>
+
+#include "tiago_nav_module/tiago_nav_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 TiagoNavModuleBtClientAlgNode : public behaviortree_base::IriBaseBTClient<tiago_nav_module::TiagoNavBtClientConfig>
+{
+ private:
+ // object of module
+ CTiagoNavModuleBT tiago_nav_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_move_to_joints;
+ sensor_msgs::JointState target_joint_states;
+ bool set_move_to_pose;
+ geometry_msgs::PoseStamped target_pose;
+ /**
+ * \brief Async function to set the tiago_nav_move_to_joints input ports.
+ *
+ * It takes the values from Dynamic Reconfigure.
+ *
+ * It has the following output ports:
+ *
+ * target_joints (sensor_msgs::JointState): The target JointState
+ *
+ * joint_tol (double): Joints tolerance.
+ *
+ * new_goal (bool): If it's a new_goal (Only used by async_move_to function).
+ *
+ * \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_nav_move_to_joints_input(BT::TreeNode& self);
+
+ /**
+ * \brief Async function to set the tiago_nav_move_to_pose input ports.
+ *
+ * It takes the values from Dynamic Reconfigure.
+ *
+ * It has the following output ports:
+ *
+ * pose (geometry_msgs::PoseStamped): The target pose
+ *
+ * position_tol (double): Joints postion tolerance.
+ *
+ * orientation_tol (double): Joints orientation tolerance.
+ *
+ * new_goal (bool): If it's a new_goal (Only used by async_move_to function).
+ *
+ * \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_nav_move_to_pose_input(BT::TreeNode& self);
+
+ public:
+ /**
+ * \brief Constructor
+ *
+ * This constructor initializes specific class attributes and all ROS
+ * communications variables to enable message exchange.
+ */
+ TiagoNavModuleBtClientAlgNode(void);
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor frees all necessary dynamic memory allocated within this
+ * this class.
+ */
+ ~TiagoNavModuleBtClientAlgNode(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_nav_module::TiagoNavBtClientConfig &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/ivo_nav_client.launch b/launch/ivo_nav_client.launch
index 15ff1723317470f2b90dc5fa06191864fc8a03e8..32acbb0273355eb6ad0185e3ef79528b07994f0e 100644
--- a/launch/ivo_nav_client.launch
+++ b/launch/ivo_nav_client.launch
@@ -1,7 +1,7 @@
<?xml version="1.0" encoding="UTF-8"?>
<launch>
- <arg name="node_name" default="head_client" />
- <arg name="output" default="log" />
+ <arg name="node_name" default="nav_client" />
+ <arg name="output" default="screen" />
<arg name="launch_prefix" default="" />
<arg name="config_file" default="$(find tiago_nav_module)/config/tiago_nav_module_default.yaml" />
<arg name="move_base_action" default="/move_base"/>
diff --git a/launch/ivo_nav_client_sim.launch b/launch/ivo_nav_client_sim.launch
index 22db9fc9dc5ca5bc1a213e611f25df619de487fe..69bb18e8456fff165e098820942212bcdb210afb 100644
--- a/launch/ivo_nav_client_sim.launch
+++ b/launch/ivo_nav_client_sim.launch
@@ -1,7 +1,7 @@
<?xml version="1.0" encoding="UTF-8"?>
<launch>
- <arg name="node_name" default="head_client" />
- <arg name="output" default="log" />
+ <arg name="node_name" default="nav_client" />
+ <arg name="output" default="screen" />
<arg name="launch_prefix" default="" />
<arg name="config_file" default="$(find tiago_nav_module)/config/tiago_nav_module_default.yaml" />
<arg name="move_base_action" default="/move_base"/>
diff --git a/launch/tiago_nav_client.launch b/launch/tiago_nav_client.launch
index 721cd4fbb9b7bab6cea2b077ae1448cbf8464040..0402ef114d8173e4b507adb9df5cfaa9268a8755 100644
--- a/launch/tiago_nav_client.launch
+++ b/launch/tiago_nav_client.launch
@@ -1,7 +1,7 @@
<?xml version="1.0" encoding="UTF-8"?>
<launch>
- <arg name="node_name" default="head_client" />
- <arg name="output" default="log" />
+ <arg name="node_name" default="nav_client" />
+ <arg name="output" default="screen" />
<arg name="launch_prefix" default="" />
<arg name="config_file" default="$(find tiago_nav_module)/config/tiago_nav_module_default.yaml" />
<arg name="move_base_action" default="/move_base"/>
diff --git a/launch/tiago_nav_client_sim.launch b/launch/tiago_nav_client_sim.launch
index 22db9fc9dc5ca5bc1a213e611f25df619de487fe..69bb18e8456fff165e098820942212bcdb210afb 100644
--- a/launch/tiago_nav_client_sim.launch
+++ b/launch/tiago_nav_client_sim.launch
@@ -1,7 +1,7 @@
<?xml version="1.0" encoding="UTF-8"?>
<launch>
- <arg name="node_name" default="head_client" />
- <arg name="output" default="log" />
+ <arg name="node_name" default="nav_client" />
+ <arg name="output" default="screen" />
<arg name="launch_prefix" default="" />
<arg name="config_file" default="$(find tiago_nav_module)/config/tiago_nav_module_default.yaml" />
<arg name="move_base_action" default="/move_base"/>
diff --git a/package.xml b/package.xml
index 0ad29cccdb388ef1cae171abece98e6e3bf621f2..bcde9b0821e7678cc22fe0da551993c345d75998 100644
--- a/package.xml
+++ b/package.xml
@@ -51,6 +51,7 @@
<build_depend>pal_waypoint_msgs</build_depend>
<build_depend>tf</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>-->
@@ -65,6 +66,7 @@
<run_depend>pal_waypoint_msgs</run_depend>
<run_depend>tf</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_nav_bt_client_alg_node.cpp b/src/tiago_nav_bt_client_alg_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3e968e4719c43aa6726c7ba1fa9775306fac0f8d
--- /dev/null
+++ b/src/tiago_nav_bt_client_alg_node.cpp
@@ -0,0 +1,156 @@
+#include "tiago_nav_bt_client_alg_node.h"
+
+TiagoNavModuleBtClientAlgNode::TiagoNavModuleBtClientAlgNode(void) :
+ behaviortree_base::IriBaseBTClient<tiago_nav_module::TiagoNavBtClientConfig>(),
+ tiago_nav_module_bt()
+{
+ // [init publishers]
+
+ // [init subscribers]
+
+ // [init services]
+
+ // [init clients]
+
+ // [init action servers]
+
+ // [init action clients]
+
+ this->set_move_to_joints = false;
+ this->set_move_to_pose = false;
+ this->tiago_nav_module_bt.init(this->factory);
+
+ // Init local functions
+ BT::PortsList set_tiago_nav_move_to_joints_input_ports = {BT::OutputPort<sensor_msgs::JointState>("target_joints"), BT::OutputPort<double>("joint_tol"), BT::OutputPort<bool>("new_goal")};
+
+ BT::PortsList set_tiago_nav_move_to_pose_input_ports = {BT::OutputPort<geometry_msgs::PoseStamped>("pose"), BT::OutputPort<double>("position_tol"), BT::OutputPort<double>("orientation_tol"), BT::OutputPort<bool>("new_goal")};
+
+ this->factory.registerIriAsyncAction("set_tiago_nav_move_to_joints_input", std::bind(&TiagoNavModuleBtClientAlgNode::set_tiago_nav_move_to_joints_input, this, std::placeholders::_1), set_tiago_nav_move_to_joints_input_ports);
+ this->factory.registerIriAsyncAction("set_tiago_nav_move_to_pose_input", std::bind(&TiagoNavModuleBtClientAlgNode::set_tiago_nav_move_to_pose_input, this, std::placeholders::_1), set_tiago_nav_move_to_pose_input_ports);
+
+}
+
+TiagoNavModuleBtClientAlgNode::~TiagoNavModuleBtClientAlgNode(void)
+{
+ // [free dynamic memory]
+}
+
+/* [subscriber callbacks] */
+
+/* [service callbacks] */
+
+/* [action callbacks] */
+
+/* [action requests] */
+
+
+void TiagoNavModuleBtClientAlgNode::node_config_update(tiago_nav_module::TiagoNavBtClientConfig &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_joints)
+ {
+ ROS_WARN("TiagoNavModuleBtClientAlgNode: To change the move_group modify it on module dynamic Reconfigure");
+ this->set_move_to_joints = true;
+ config.set_joints = false;
+ std::vector<std::string>().swap(this->target_joint_states.name);
+ std::vector<double>().swap(this->target_joint_states.position);
+ if (config.nav_to_move == 0)//tiago_nav
+ {
+ this->target_joint_states.name.push_back("torso_lift_joint");
+ this->target_joint_states.name.push_back("nav_1_joint");
+ this->target_joint_states.name.push_back("nav_2_joint");
+ this->target_joint_states.name.push_back("nav_3_joint");
+ this->target_joint_states.name.push_back("nav_4_joint");
+ this->target_joint_states.name.push_back("nav_5_joint");
+ this->target_joint_states.name.push_back("nav_6_joint");
+ this->target_joint_states.name.push_back("nav_7_joint");
+ }
+ else if (config.nav_to_move == 1)//ivo_left_nav
+ {
+ this->target_joint_states.name.push_back("torso_lift_joint");
+ this->target_joint_states.name.push_back("nav_left_1_joint");
+ this->target_joint_states.name.push_back("nav_left_2_joint");
+ this->target_joint_states.name.push_back("nav_left_3_joint");
+ this->target_joint_states.name.push_back("nav_left_4_joint");
+ this->target_joint_states.name.push_back("nav_left_5_joint");
+ this->target_joint_states.name.push_back("nav_left_6_joint");
+ this->target_joint_states.name.push_back("nav_left_7_joint");
+ }
+ this->target_joint_states.position.push_back(config.torso);
+ this->target_joint_states.position.push_back(config.joint1);
+ this->target_joint_states.position.push_back(config.joint2);
+ this->target_joint_states.position.push_back(config.joint3);
+ this->target_joint_states.position.push_back(config.joint4);
+ this->target_joint_states.position.push_back(config.joint5);
+ this->target_joint_states.position.push_back(config.joint6);
+ this->target_joint_states.position.push_back(config.joint7);
+ }
+
+ if (config.set_pose)
+ {
+ ROS_WARN("TiagoNavModuleBtClientAlgNode: To change the move_group modify it on module dynamic Reconfigure");
+ this->set_move_to_pose = true;
+ config.set_pose = false;
+ this->target_pose.header.stamp=ros::Time::now();
+ this->target_pose.header.frame_id=config.plan_frame_id;
+ this->target_pose.pose.position.x=config.x_pos;
+ this->target_pose.pose.position.y=config.y_pos;
+ this->target_pose.pose.position.z=config.z_pos;
+ this->target_pose.pose.orientation.x=config.x_orientation;
+ this->target_pose.pose.orientation.y=config.y_orientation;
+ this->target_pose.pose.orientation.z=config.z_orientation;
+ this->target_pose.pose.orientation.w=config.w_orientation;
+ }
+ this->config_=config;
+ this->unlock();
+}
+
+BT::NodeStatus TiagoNavModuleBtClientAlgNode::set_tiago_nav_move_to_joints_input(BT::TreeNode& self)
+{
+ if (this->set_move_to_joints)
+ {
+ this->set_move_to_joints = false;
+ self.setOutput("target_joints", this->target_joint_states);
+ self.setOutput("joint_tol", this->config_.joint_tol);
+ self.setOutput("new_goal", true);
+ return BT::NodeStatus::SUCCESS;
+ }
+ return BT::NodeStatus::RUNNING;
+}
+
+BT::NodeStatus TiagoNavModuleBtClientAlgNode::set_tiago_nav_move_to_pose_input(BT::TreeNode& self)
+{
+ if (this->set_move_to_pose)
+ {
+ this->set_move_to_pose = false;
+ self.setOutput("pose", this->target_pose);
+ self.setOutput("position_tol", this->config_.position_tol);
+ self.setOutput("orientation_tol", this->config_.orientation_tol);
+ self.setOutput("new_goal", true);
+ return BT::NodeStatus::SUCCESS;
+ }
+ return BT::NodeStatus::RUNNING;
+}
+
+void TiagoNavModuleBtClientAlgNode::addNodeDiagnostics(void)
+{
+}
+
+/* main function */
+int main(int argc,char *argv[])
+{
+ return behaviortree_base::main<TiagoNavModuleBtClientAlgNode>(argc, argv, "tiago_nav_module_bt_client_alg_node");
+}
diff --git a/src/xml/bt_test.xml b/src/xml/bt_test.xml
new file mode 100644
index 0000000000000000000000000000000000000000..832f3f8e6762b93bc4b37c6ec7448b55e48b1522
--- /dev/null
+++ b/src/xml/bt_test.xml
@@ -0,0 +1,51 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="BehaviorTree">
+ <!-- ////////// -->
+ <BehaviorTree ID="BehaviorTree">
+ <SequenceStar>
+ <Action ID="async_is_start_tree"/>
+ <Action ID="set_tiago_arm_move_to_joints_input" target_joints="{target_joints}" joint_tol="{joint_tol}" new_goal="{new_goal}"/>
+ <Action ID="async_tiago_arm_move_to_joints" target_joints="{target_joints}" joint_tol="{joint_tol}" new_goal="{new_goal}"/>
+ <Action ID="set_tiago_arm_move_to_pose_input" pose="{pose}" position_tol="{position_tol}" orientation_tol="{orientation_tol}" new_goal="{new_goal}"/>
+ <Action ID="async_tiago_arm_move_to_pose" pose="{pose}" position_tol="{position_tol}" orientation_tol="{orientation_tol}" new_goal="{new_goal}"/>
+ </SequenceStar>
+ </BehaviorTree>
+ <!-- ////////// -->
+ <BehaviorTree ID="set_fingers_pos_blackboard">
+ <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="new_goal" value="True"/>
+ </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_nav_module -->
+ <!-- Client -->
+ <Action ID="set_tiago_arm_move_to_joints_input">
+ <output_port name="target_joints"> The target JointState</output_port>
+ <output_port name="joint_tol"> Joints tolerance</output_port>
+ <output_port name="new_goal"> If it's a new_goal (Only used by async_move_to function)</output_port>
+ </Action>
+ <Action ID="set_tiago_arm_move_to_pose_input">
+ <output_port name="pose"> The target pose</output_port>
+ <output_port name="position_tol"> Joints postion tolerance</output_port>
+ <output_port name="orientation_tol"> Joints orientation tolerance</output_port>
+ <output_port name="new_goal"> If it's a new_goal (Only used by async_move_to function)</output_port>
+ </Action>
+ </TreeNodesModel>
+ <!-- ////////// -->
+</root>
+
+