diff --git a/CMakeLists.txt b/CMakeLists.txt index 6299d1cc6828c2059ac0514d2902453afa16021e..ea2e1c9be28e546fad5d8beb037fefdae9d7a7b1 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 geometry_msgs moveit_ros_planning_interface sensor_msgs iri_base_algorithm iri_behaviortree) +find_package(catkin REQUIRED COMPONENTS roscpp dynamic_reconfigure iri_ros_tools actionlib geometry_msgs moveit_ros_planning_interface sensor_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/TiagoArmModule.cfg cfg/TiagoArmClient.cfg + cfg/TiagoArmBtClient.cfg ) ################################### @@ -102,7 +103,7 @@ generate_dynamic_reconfigure_options( catkin_package( INCLUDE_DIRS include LIBRARIES tiago_arm_module - CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib geometry_msgs moveit_ros_planning_interface sensor_msgs iri_base_algorithm iri_behaviortree + CATKIN_DEPENDS roscpp dynamic_reconfigure iri_ros_tools actionlib geometry_msgs moveit_ros_planning_interface sensor_msgs iri_base_algorithm iri_base_bt_client iri_behaviortree # DEPENDS system_lib ) @@ -154,6 +155,13 @@ 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}) +set(bt_client_name tiago_arm_bt_client) +add_executable(${bt_client_name} src/tiago_arm_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/TiagoArmBtClient.cfg b/cfg/TiagoArmBtClient.cfg new file mode 100755 index 0000000000000000000000000000000000000000..9c3b869f38bf37f784299a91369f07525b1d5a83 --- /dev/null +++ b/cfg/TiagoArmBtClient.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_arm_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 arm in joints space") +move_pose = gen.add_group("Move arm 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) + +arm_to_move_enum = gen.enum([ gen.const("tiago_arm", int_t, 0, "Tiago Arm"), + gen.const("ivo_left_arm", int_t, 1, "Ivo left arm")], + "The posibles arms to control") +move_joints.add("set_joints", bool_t, 0, "Set selected angles", False) +move_joints.add("arm_to_move", int_t, 0, "The arm to move", 0, 0, 1, edit_method=arm_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.0, -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, "TiagoArmModuleBtClientAlgNode", "TiagoArmBtClient")) diff --git a/include/tiago_arm_bt_client_alg_node.h b/include/tiago_arm_bt_client_alg_node.h new file mode 100644 index 0000000000000000000000000000000000000000..3b65ab45136bd5bdc6a0f77d70cc5e3d387932ed --- /dev/null +++ b/include/tiago_arm_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_arm_module_bt_client_alg_node_h_ +#define _tiago_arm_module_bt_client_alg_node_h_ + +#include <iri_base_bt_client/iri_base_bt_client.h> +#include <tiago_arm_module/TiagoArmBtClientConfig.h> + +#include "tiago_arm_module/tiago_arm_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 TiagoArmModuleBtClientAlgNode : public behaviortree_base::IriBaseBTClient<tiago_arm_module::TiagoArmBtClientConfig> +{ + private: + // object of module + CTiagoArmModuleBT tiago_arm_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_arm_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_arm_move_to_joints_input(BT::TreeNode& self); + + /** + * \brief Async function to set the tiago_arm_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_arm_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. + */ + TiagoArmModuleBtClientAlgNode(void); + + /** + * \brief Destructor + * + * This destructor frees all necessary dynamic memory allocated within this + * this class. + */ + ~TiagoArmModuleBtClientAlgNode(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_arm_module::TiagoArmBtClientConfig &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/package.xml b/package.xml index 450034b7c9404f38ed91e4444e1c2989ea496c11..d08c71a7536065b6daeec6a7ee0c8e864fdecec7 100644 --- a/package.xml +++ b/package.xml @@ -49,6 +49,7 @@ <build_depend>moveit_ros_planning_interface</build_depend> <build_depend>iri_base_algorithm</build_depend> <build_depend>iri_behaviortree</build_depend> + <build_depend>iri_base_bt_client</build_depend> <!-- new dependencies --> <!--<build_depend>new build dependency</build_depend>--> <run_depend>iri_ros_tools</run_depend> @@ -60,6 +61,7 @@ <run_depend>moveit_ros_planning_interface</run_depend> <run_depend>iri_base_algorithm</run_depend> <run_depend>iri_behaviortree</run_depend> + <run_depend>iri_base_bt_client</run_depend> <!-- new dependencies --> <!--<run_depend>new run dependency</run_depend>--> diff --git a/src/tiago_arm_bt_client_alg_node.cpp b/src/tiago_arm_bt_client_alg_node.cpp new file mode 100644 index 0000000000000000000000000000000000000000..b5cf6541b3445011e32521bc3d6579738cfe38ce --- /dev/null +++ b/src/tiago_arm_bt_client_alg_node.cpp @@ -0,0 +1,155 @@ +#include "tiago_arm_bt_client_alg_node.h" + +TiagoArmModuleBtClientAlgNode::TiagoArmModuleBtClientAlgNode(void) : + behaviortree_base::IriBaseBTClient<tiago_arm_module::TiagoArmBtClientConfig>(), + tiago_arm_module_bt() +{ + // [init publishers] + + // [init subscribers] + + // [init services] + + // [init clients] + + // [init action servers] + + // [init action clients] + + this->set_move_to_joints = false; + this->tiago_arm_module_bt.init(this->factory); + + // Init local functions + BT::PortsList set_tiago_arm_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_arm_move_to_pose_input_ports = {BT::InputPort<geometry_msgs::PoseStamped>("pose"), BT::InputPort<double>("position_tol"), BT::InputPort<double>("orientation_tol"), BT::BidirectionalPort<bool>("new_goal")}; + + this->factory.registerIriAsyncAction("set_tiago_arm_move_to_joints_input", std::bind(&TiagoArmModuleBtClientAlgNode::set_tiago_arm_move_to_joints_input, this, std::placeholders::_1), set_tiago_arm_move_to_joints_input_ports); + this->factory.registerIriAsyncAction("set_tiago_arm_move_to_pose_input", std::bind(&TiagoArmModuleBtClientAlgNode::set_tiago_arm_move_to_pose_input, this, std::placeholders::_1), set_tiago_arm_move_to_pose_input_ports); + +} + +TiagoArmModuleBtClientAlgNode::~TiagoArmModuleBtClientAlgNode(void) +{ + // [free dynamic memory] +} + +/* [subscriber callbacks] */ + +/* [service callbacks] */ + +/* [action callbacks] */ + +/* [action requests] */ + + +void TiagoArmModuleBtClientAlgNode::node_config_update(tiago_arm_module::TiagoArmBtClientConfig &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("TiagoArmModuleBtClientAlgNode: 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.arm_to_move == 0)//tiago_arm + { + this->target_joint_states.name.push_back("torso_lift_joint"); + this->target_joint_states.name.push_back("arm_1_joint"); + this->target_joint_states.name.push_back("arm_2_joint"); + this->target_joint_states.name.push_back("arm_3_joint"); + this->target_joint_states.name.push_back("arm_4_joint"); + this->target_joint_states.name.push_back("arm_5_joint"); + this->target_joint_states.name.push_back("arm_6_joint"); + this->target_joint_states.name.push_back("arm_7_joint"); + } + else if (config.arm_to_move == 1)//ivo_left_arm + { + this->target_joint_states.name.push_back("torso_lift_joint"); + this->target_joint_states.name.push_back("arm_left_1_joint"); + this->target_joint_states.name.push_back("arm_left_2_joint"); + this->target_joint_states.name.push_back("arm_left_3_joint"); + this->target_joint_states.name.push_back("arm_left_4_joint"); + this->target_joint_states.name.push_back("arm_left_5_joint"); + this->target_joint_states.name.push_back("arm_left_6_joint"); + this->target_joint_states.name.push_back("arm_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("TiagoArmModuleBtClientAlgNode: 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 TiagoArmModuleBtClientAlgNode::set_tiago_arm_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 TiagoArmModuleBtClientAlgNode::set_tiago_arm_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 TiagoArmModuleBtClientAlgNode::addNodeDiagnostics(void) +{ +} + +/* main function */ +int main(int argc,char *argv[]) +{ + return behaviortree_base::main<TiagoArmModuleBtClientAlgNode>(argc, argv, "tiago_arm_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..b0beceafb38b40a2a2587348dd647830678ed649 --- /dev/null +++ b/src/xml/bt_test.xml @@ -0,0 +1,116 @@ +<?xml version="1.0"?> +<root main_tree_to_execute="BehaviorTree"> + <!-- ////////// --> + <BehaviorTree ID="BehaviorTree"> + <SequenceStar> + <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> + <Action ID="syn_cancel_tiago_arm_action"/> + <Action ID="asyn_cancel_tiago_arm_action"/> + <Action ID="async_is_tiago_arm_finished"/> + <Action ID="set_tiago_arm_workspace"> + <input_port name="frame_id"> Workspace's frame ID</input_port> + <input_port name="x_min"> Workspace's x min</input_port> + <input_port name="y_min"> Workspace's y min</input_port> + <input_port name="z_min"> Workspace's z min</input_port> + <input_port name="x_max"> Workspace's x max</input_port> + <input_port name="y_max"> Workspace's y max</input_port> + <input_port name="z_max"> Workspace's z max</input_port> + </Action> + <Action ID="sync_tiago_arm_move_to_joints"> + <input_port name="target_joints"> The target JointState</input_port> + <input_port default="0.1" name="joint_tol"> Joints tolerance</input_port> + </Action> + <Action ID="async_tiago_arm_move_to_joints"> + <input_port name="target_joints"> The target JointState</input_port> + <input_port default="0.1" name="joint_tol"> Joints tolerance</input_port> + <input_port default="True" name="new_goal"> If it's a new_goal</input_port> + </Action> + <Action ID="get_tiago_arm_current_joint_angles"> + <output_port name="joint_states"> The current JointState</output_port> + </Action> + <Action ID="sync_tiago_arm_move_to_pose"> + <input_port name="pose"> The target pose</input_port> + <input_port default="0.01" name="position_tol"> Position tolerance</input_port> + <input_port default="0.1" name="orientation_tol"> Orientation tolerance</input_port> + </Action> + <Action ID="async_tiago_arm_move_to_pose"> + <input_port name="pose"> The target pose</input_port> + <input_port default="0.01" name="position_tol"> Position tolerance</input_port> + <input_port default="0.1" name="orientation_tol"> Orientation tolerance</input_port> + <input_port default="True" name="new_goal"> If it's a new_goal</input_port> + </Action> + <Action ID="get_tiago_arm_current_pose"> + <output_port name="pose"> The current pose</output_port> + </Action> + <Action ID="set_tiago_arm_planner"> + <input_port name="planner_id"> The planner id</input_port> + </Action> + <Action ID="get_tiago_arm_planner"> + <output_port name="planner_id"> The planner id</output_port> + </Action> + <Action ID="set_tiago_arm_group_name"> + <input_port name="group_id"> The group id</input_port> + </Action> + <Action ID="get_tiago_arm_group_name"> + <output_port name="group_id"> The group id</output_port> + </Action> + <Action ID="set_tiago_arm_max_planning_attempts"> + <input_port name="num"> The num of maximum planning attempts</input_port> + </Action> + <Action ID="get_tiago_arm_max_planning_attempts"> + <output_port name="num"> The num of maximum planning attempts</output_port> + </Action> + <Action ID="set_tiago_arm_max_planning_time"> + <input_port name="time"> The maximum planning time</input_port> + </Action> + <Action ID="get_tiago_arm_max_planning_time"> + <output_port name="time"> The maximum planning time</output_port> + </Action> + <Action ID="set_tiago_arm_velocity_scale_factor"> + <input_port name="scale"> The velocity scale factor</input_port> + </Action> + <Action ID="get_tiago_arm_velocity_scale_factor"> + <output_port name="scale"> The velocity scale factor</output_port> + </Action> + <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> + <Condition ID="is_tiago_arm_finished"/> + <Condition ID="is_tiago_arm_module_running"/> + <Condition ID="is_tiago_arm_module_success"/> + <Condition ID="is_tiago_arm_module_action_start_fail"/> + <Condition ID="is_tiago_arm_module_timeout"/> + <Condition ID="is_tiago_arm_module_fb_watchdog"/> + <Condition ID="is_tiago_arm_module_aborted"/> + <Condition ID="is_tiago_arm_module_preempted"/> + <Condition ID="is_tiago_arm_module_rejected"/> + <Condition ID="is_tiago_arm_module_no_joint_states"/> + </TreeNodesModel> + <!-- ////////// --> +</root> + +