Skip to content
Snippets Groups Projects
Commit e640fcd7 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Added BT client

parent 16242cb0
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 502 additions and 10 deletions
CMakeLists.txt
+ 11
− 2
View file @ e640fcd7
...@@ -7,7 +7,7 @@ add_definitions(-std=c++11) ...@@ -7,7 +7,7 @@ add_definitions(-std=c++11)
## Find catkin macros and libraries ## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages ## 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) find_package(pal_waypoint_msgs)
if(${pal_waypoint_msgs_FOUND}) if(${pal_waypoint_msgs_FOUND})
...@@ -95,6 +95,7 @@ find_package(iriutils REQUIRED) ...@@ -95,6 +95,7 @@ find_package(iriutils REQUIRED)
generate_dynamic_reconfigure_options( generate_dynamic_reconfigure_options(
cfg/TiagoNavModule.cfg cfg/TiagoNavModule.cfg
cfg/TiagoNavClient.cfg cfg/TiagoNavClient.cfg
cfg/TiagoNavBtClient.cfg
) )
################################### ###################################
...@@ -112,7 +113,7 @@ set(module_bt_name tiago_nav_module_bt) ...@@ -112,7 +113,7 @@ set(module_bt_name tiago_nav_module_bt)
catkin_package( catkin_package(
INCLUDE_DIRS include INCLUDE_DIRS include
LIBRARIES ${module_name} ${module_bt_name} 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 # DEPENDS system_lib
) )
...@@ -161,6 +162,14 @@ target_link_libraries(${module_bt_name} ${catkin_LIBRARIES}) ...@@ -161,6 +162,14 @@ target_link_libraries(${module_bt_name} ${catkin_LIBRARIES})
target_link_libraries(${module_bt_name} ${iriutils_LIBRARIES}) target_link_libraries(${module_bt_name} ${iriutils_LIBRARIES})
add_dependencies(${module_bt_name} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} ${module_name}) 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 ## ## Install ##
############# #############
......
cfg/TiagoNavBtClient.cfg 0 → 100755
+ 113
− 0
View file @ e640fcd7
#! /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"))
include/tiago_nav_bt_client_alg_node.h 0 → 100644
+ 161
− 0
View file @ e640fcd7
// 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
launch/ivo_nav_client.launch
+ 2
− 2
View file @ e640fcd7
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8"?>
<launch> <launch>
<arg name="node_name" default="head_client" /> <arg name="node_name" default="nav_client" />
<arg name="output" default="log" /> <arg name="output" default="screen" />
<arg name="launch_prefix" default="" /> <arg name="launch_prefix" default="" />
<arg name="config_file" default="$(find tiago_nav_module)/config/tiago_nav_module_default.yaml" /> <arg name="config_file" default="$(find tiago_nav_module)/config/tiago_nav_module_default.yaml" />
<arg name="move_base_action" default="/move_base"/> <arg name="move_base_action" default="/move_base"/>
......
launch/ivo_nav_client_sim.launch
+ 2
− 2
View file @ e640fcd7
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8"?>
<launch> <launch>
<arg name="node_name" default="head_client" /> <arg name="node_name" default="nav_client" />
<arg name="output" default="log" /> <arg name="output" default="screen" />
<arg name="launch_prefix" default="" /> <arg name="launch_prefix" default="" />
<arg name="config_file" default="$(find tiago_nav_module)/config/tiago_nav_module_default.yaml" /> <arg name="config_file" default="$(find tiago_nav_module)/config/tiago_nav_module_default.yaml" />
<arg name="move_base_action" default="/move_base"/> <arg name="move_base_action" default="/move_base"/>
......
launch/tiago_nav_client.launch
+ 2
− 2
View file @ e640fcd7
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8"?>
<launch> <launch>
<arg name="node_name" default="head_client" /> <arg name="node_name" default="nav_client" />
<arg name="output" default="log" /> <arg name="output" default="screen" />
<arg name="launch_prefix" default="" /> <arg name="launch_prefix" default="" />
<arg name="config_file" default="$(find tiago_nav_module)/config/tiago_nav_module_default.yaml" /> <arg name="config_file" default="$(find tiago_nav_module)/config/tiago_nav_module_default.yaml" />
<arg name="move_base_action" default="/move_base"/> <arg name="move_base_action" default="/move_base"/>
......
launch/tiago_nav_client_sim.launch
+ 2
− 2
View file @ e640fcd7
<?xml version="1.0" encoding="UTF-8"?> <?xml version="1.0" encoding="UTF-8"?>
<launch> <launch>
<arg name="node_name" default="head_client" /> <arg name="node_name" default="nav_client" />
<arg name="output" default="log" /> <arg name="output" default="screen" />
<arg name="launch_prefix" default="" /> <arg name="launch_prefix" default="" />
<arg name="config_file" default="$(find tiago_nav_module)/config/tiago_nav_module_default.yaml" /> <arg name="config_file" default="$(find tiago_nav_module)/config/tiago_nav_module_default.yaml" />
<arg name="move_base_action" default="/move_base"/> <arg name="move_base_action" default="/move_base"/>
......
package.xml
+ 2
− 0
View file @ e640fcd7
...@@ -51,6 +51,7 @@ ...@@ -51,6 +51,7 @@
<build_depend>pal_waypoint_msgs</build_depend> <build_depend>pal_waypoint_msgs</build_depend>
<build_depend>tf</build_depend> <build_depend>tf</build_depend>
<build_depend>iri_base_algorithm</build_depend> <build_depend>iri_base_algorithm</build_depend>
<build_depend>iri_base_bt_client</build_depend>
<build_depend>iri_behaviortree</build_depend> <build_depend>iri_behaviortree</build_depend>
<!-- new dependencies --> <!-- new dependencies -->
<!--<build_depend>new build dependency</build_depend>--> <!--<build_depend>new build dependency</build_depend>-->
...@@ -65,6 +66,7 @@ ...@@ -65,6 +66,7 @@
<run_depend>pal_waypoint_msgs</run_depend> <run_depend>pal_waypoint_msgs</run_depend>
<run_depend>tf</run_depend> <run_depend>tf</run_depend>
<run_depend>iri_base_algorithm</run_depend> <run_depend>iri_base_algorithm</run_depend>
<run_depend>iri_base_bt_client</run_depend>
<run_depend>iri_behaviortree</run_depend> <run_depend>iri_behaviortree</run_depend>
<!-- new dependencies --> <!-- new dependencies -->
<!--<run_depend>new run dependency</run_depend>--> <!--<run_depend>new run dependency</run_depend>-->
......
src/tiago_nav_bt_client_alg_node.cpp 0 → 100644
+ 156
− 0
View file @ e640fcd7
#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");
}
src/xml/bt_test.xml 0 → 100644
+ 51
− 0
View file @ e640fcd7
<?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>
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment