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

Added BT client example

parent a226884d
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 343 additions and 170 deletions
cfg/TiagoNavBtClient.cfg
+ 28
− 60
View file @ da6306b8
......@@ -37,8 +37,11 @@ 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")
move_base = gen.add_group("Move base action")
# costmaps = gen.add_group("Costmap")
# map = gen.add_group("Map")
# poi = gen.add_group("Got to point of interest action")
# waypoint = gen.add_group("Got to waypoints action")
# objects = gen.add_group("Objects in the environment")
# const = gen.add_group("Path constraints")
......@@ -48,66 +51,31 @@ 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_base.add("set_go_to_input", bool_t, 0, "Set go to parameters", False)
move_base.add("mb_x_pos", double_t, 0, "Target X position", 0.0, -100.0, 100.0)
move_base.add("mb_y_pos", double_t, 0, "Target Y position", 0.0, -100.0, 100.0)
move_base.add("mb_xy_tol", double_t, 0, "Target XY tolerance", -1.0, -1.0, 10.0)
move_base.add("mb_yaw", double_t, 0, "Target Yaw angle", 0.0, -3.14159,3.14159)
move_base.add("mb_yaw_tol", double_t, 0, "Target Yaw tolerance", -1.0, -1.0, 1.0)
move_base.add("mb_frame_id", str_t, 0, "Target pose frame identifier", "map")
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)
# map.add("map_name", str_t, 0, "Name of the map to use", "iri_map")
# map.add("map_change", bool_t, 0, "Update the desired map", False)
# 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)
# costmaps.add("cm_enable_auto_clear",bool_t, 0, "Enable costmaps autoclear", False)
# costmaps.add("cm_disable_auto_clear",bool_t, 0, "Disable costmaps autoclear", False)
# costmaps.add("cm_auto_clear_rate_hz",double_t,0, "Costmap autoclear rate", 0.1, 0.01, 1.0)
# costmaps.add("cm_clear_costmaps", bool_t, 0, "Clear costmaps", 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)
# poi.add("start_poi", bool_t, 0, "Start the POI navigation", False)
# poi.add("stop_poi", bool_t, 0, "Stop the POI navigation", False)
# poi.add("poi_name", str_t, 0, "Name of the POI", "")
# waypoint.add("start_wp", bool_t, 0, "Start the waypoint navigation", False)
# waypoint.add("stop_wp", bool_t, 0, "Stop the waypoint navigation", False)
# waypoint.add("wp_group", str_t, 0, "Name of the waypoint group", "")
# waypoint.add("wp_first", int_t, 0, "Index of the first waypoint", 0, 0, 100)
# waypoint.add("wp_num", int_t, 0, "Number of waypoint to execute", 0, 0, 100)
# waypoint.add("wp_continue_on_error",bool_t, 0, "Continue on error", False)
exit(gen.generate(PACKAGE, "TiagoNavModuleBtClientAlgNode", "TiagoNavBtClient"))
include/tiago_nav_bt_client_alg_node.h
+ 17
− 19
View file @ da6306b8
......@@ -62,10 +62,7 @@ class TiagoNavModuleBtClientAlgNode : public behaviortree_base::IriBaseBTClient<
// [action client attributes]
bool set_move_to_joints;
sensor_msgs::JointState target_joint_states;
bool set_move_to_pose;
geometry_msgs::PoseStamped target_pose;
bool set_go_to_pose;
/**
* \brief Async function to set the tiago_nav_move_to_joints input ports.
*
......@@ -73,11 +70,18 @@ class TiagoNavModuleBtClientAlgNode : public behaviortree_base::IriBaseBTClient<
*
* It has the following output ports:
*
* target_joints (sensor_msgs::JointState): The target JointState
* x (double): desired X positon with respect to the goal frame set by the
* set_goal_frame() function.
*
* joint_tol (double): Joints tolerance.
* y (double): desired Y positon with respect to the goal frame set by the
* set_goal_frame() function.
*
* new_goal (bool): If it's a new_goal (Only used by async_move_to function).
* yaw (double): desired orientation with respect to the goal frame set by the
* set_goal_frame() function.
*
* heading_tol (double): heading tolerance for the goal.
*
* x_y_pos_tol (double): position tolerance for the goal.
*
* \param self Self node with the required ports:
*
......@@ -86,22 +90,16 @@ class TiagoNavModuleBtClientAlgNode : public behaviortree_base::IriBaseBTClient<
* If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
*
*/
BT::NodeStatus set_tiago_nav_move_to_joints_input(BT::TreeNode& self);
BT::NodeStatus set_tiago_nav_go_to_pose_inputs(BT::TreeNode& self);
/**
* \brief Async function to set the tiago_nav_move_to_pose input ports.
*
* It takes the values from Dynamic Reconfigure.
* \brief Async function to set the set_tiago_nav_goal_frame input ports
*
* 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).
* frame_id (std::string): name of the new reference frame for the future goals.
* This frame can be any that exist inside the TF tree
* of the robot.
*
* \param self Self node with the required ports:
*
......@@ -110,7 +108,7 @@ class TiagoNavModuleBtClientAlgNode : public behaviortree_base::IriBaseBTClient<
* If inputs are missing or incorrect it also returns BT:NodeStatus::FAILURE.
*
*/
BT::NodeStatus set_tiago_nav_move_to_pose_input(BT::TreeNode& self);
BT::NodeStatus set_tiago_nav_goal_frame_inputs(BT::TreeNode& self);
public:
/**
......
include/tiago_nav_module/tiago_nav_bt_module.h
+ 20
− 2
View file @ da6306b8
......@@ -341,9 +341,9 @@ class CTiagoNavModuleBT
BT::NodeStatus async_tiago_nav_go_to_waypoints(BT::TreeNode& self);
/**
* \brief Synchronized set_workspace TIAGo nav function.
* \brief Synchronized get_current_waypoint TIAGo nav function.
*
* This function calls set_workspace of tiago_nav_module.
* This function calls get_current_waypoint of tiago_nav_module.
*
* It has the following output ports:
*
......@@ -540,6 +540,24 @@ class CTiagoNavModuleBT
*/
BT::NodeStatus tiago_nav_set_double_parameter(BT::TreeNode& self);
/**
* \brief Synchronized getCurrentPose TIAGo nav function.
*
* This function calls getCurrentPose of tiago_nav_module.
*
* It has the following output ports:
*
* pose (geometry_msgs::Pose): current pose.
*
* \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 get_tiago_nav_current_pose(BT::TreeNode& self);
/**
* \brief Synchronized stop TIAGo nav function.
*
......
launch/tiago_nav_bt_client.launch 0 → 100644
+ 54
− 0
View file @ da6306b8
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<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"/>
<arg name="move_poi_action" default="/poi_navigation_server/go_to_poi"/>
<arg name="move_waypoint_action" default="/pal_waypoint/navigate"/>
<arg name="odom_topic" default="/mobile_base_controller/odom"/>
<arg name="clear_costmaps_service" default="/move_base/clear_costmaps"/>
<arg name="change_map_service" default="/pal_map_manager/change_map"/>
<arg name="set_op_mode_service" default="/pal_navigation_sm"/>
<arg name="move_base_dyn_reconf" default="/move_base/PalLocalPlanner/set_parameters"/>
<arg name="tree_path" default="$(find tiago_nav_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="$(arg node_name)"
pkg="tiago_nav_module"
type="tiago_nav_bt_client"
output="$(arg output)"
launch-prefix="$(arg launch_prefix)">
<remap from="~/tiago_nav_module/move_base"
to="$(arg move_base_action)"/>
<remap from="~/tiago_nav_module/move_poi"
to="$(arg move_poi_action)"/>
<remap from="~/tiago_nav_module/move_waypoint"
to="$(arg move_waypoint_action)"/>
<remap from="~/tiago_nav_module/odom"
to="$(arg odom_topic)"/>
<remap from="~/tiago_nav_module/clear_costmaps"
to="$(arg clear_costmaps_service)"/>
<remap from="~/tiago_nav_module/change_map"
to="$(arg change_map_service)"/>
<remap from="~/tiago_nav_module/set_op_mode"
to="$(arg set_op_mode_service)"/>
<remap from="~/tiago_nav_module/move_base_reconf"
to="$(arg move_base_dyn_reconf)"/>
<rosparam file="$(arg config_file)" command="load" ns="tiago_nav_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>
launch/tiago_nav_bt_client_sim.launch 0 → 100644
+ 44
− 0
View file @ da6306b8
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<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"/>
<arg name="move_poi_action" default="/poi_navigation_server/go_to_poi"/>
<arg name="move_waypoint_action" default="/pal_waypoint/navigate"/>
<arg name="odom_topic" default="/mobile_base_controller/odom"/>
<arg name="clear_costmaps_service" default="/move_base/clear_costmaps"/>
<arg name="change_map_service" default="/pal_map_manager/change_map"/>
<arg name="set_op_mode_service" default="/pal_navigation_sm"/>
<arg name="move_base_dyn_reconf" default="/move_base/PalLocalPlanner/set_parameters"/>
<arg name="tree_path" default="$(find tiago_nav_module)/src/xml" />
<arg name="tree_file" default="bt_test" />
<arg name="bt_client_rate" default="10" />
<include file="$(find tiago_2dnav_gazebo)/launch/tiago_navigation.launch">
<arg name="public_sim" value="true" />
<arg name="robot" value="steel" />
</include>
<include file="$(find tiago_nav_module)/launch/tiago_nav_bt_client.launch">
<arg name="node_name" value="$(arg node_name)" />
<arg name="output" value="$(arg output)" />
<arg name="launch_prefix" value="$(arg launch_prefix)" />
<arg name="config_file" value="$(arg config_file)" />
<arg name="move_base_action" value="$(arg move_base_action)"/>
<arg name="move_poi_action" value="$(arg move_poi_action)"/>
<arg name="move_waypoint_action" value="$(arg move_waypoint_action)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
<arg name="clear_costmaps_service" value="$(arg clear_costmaps_service)"/>
<arg name="change_map_service" value="$(arg change_map_service)"/>
<arg name="set_op_mode_service" value="$(arg set_op_mode_service)"/>
<arg name="move_base_dyn_reconf" value="$(arg move_base_dyn_reconf)"/>
<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>
src/tiago_nav_bt_client_alg_node.cpp
+ 20
− 71
View file @ da6306b8
......@@ -16,17 +16,15 @@ TiagoNavModuleBtClientAlgNode::TiagoNavModuleBtClientAlgNode(void) :
// [init action clients]
this->set_move_to_joints = false;
this->set_move_to_pose = false;
this->set_go_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 async_go_to_pose_inputs_ports = {BT::OutputPort<double>("x"), BT::OutputPort<double>("y"), BT::OutputPort<double>("yaw"), BT::OutputPort<double>("heading_tol"), BT::OutputPort<double>("x_y_pos_tol"), BT::BidirectionalPort<bool>("new_goal")};
BT::PortsList set_goal_frame_inputs_ports = {BT::OutputPort<std::string>("frame_id")};
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);
this->factory.registerIriAsyncAction("set_tiago_nav_go_to_pose_inputs", std::bind(&TiagoNavModuleBtClientAlgNode::set_tiago_nav_go_to_pose_inputs, this, std::placeholders::_1), async_go_to_pose_inputs_ports);
this->factory.registerIriAsyncAction("set_tiago_nav_goal_frame_inputs", std::bind(&TiagoNavModuleBtClientAlgNode::set_tiago_nav_goal_frame_inputs, this, std::placeholders::_1), set_goal_frame_inputs_ports);
}
......@@ -60,86 +58,37 @@ void TiagoNavModuleBtClientAlgNode::node_config_update(tiago_nav_module::TiagoNa
config.RESTART=false;
}
if (config.set_joints)
if (config.set_go_to_input)
{
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);
this->set_go_to_pose = true;
config.set_go_to_input = false;
}
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)
BT::NodeStatus TiagoNavModuleBtClientAlgNode::set_tiago_nav_go_to_pose_inputs(BT::TreeNode& self)
{
if (this->set_move_to_joints)
if (this->set_go_to_pose)
{
this->set_move_to_joints = false;
self.setOutput("target_joints", this->target_joint_states);
self.setOutput("joint_tol", this->config_.joint_tol);
this->set_go_to_pose = false;
self.setOutput("x", this->config_.mb_x_pos);
self.setOutput("y", this->config_.mb_y_pos);
self.setOutput("yaw", this->config_.mb_yaw);
self.setOutput("heading_tol", this->config_.mb_xy_tol);
self.setOutput("x_y_pos_tol", this->config_.mb_yaw_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)
BT::NodeStatus TiagoNavModuleBtClientAlgNode::set_tiago_nav_goal_frame_inputs(BT::TreeNode& self)
{
if (this->set_move_to_pose)
if (this->set_go_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);
// this->set_go_to_pose = false;
self.setOutput("frame_id", this->config_.mb_frame_id);
return BT::NodeStatus::SUCCESS;
}
return BT::NodeStatus::RUNNING;
......
src/tiago_nav_bt_module.cpp
+ 12
− 0
View file @ da6306b8
......@@ -32,6 +32,7 @@ void CTiagoNavModuleBT::init(IriBehaviorTreeFactory &factory)
BT::PortsList set_int_parameter_ports = {BT::InputPort<std::string>("name_space"), BT::InputPort<std::string>("name"), BT::InputPort<int>("value")};
BT::PortsList set_string_parameter_ports = {BT::InputPort<std::string>("name_space"), BT::InputPort<std::string>("name"), BT::InputPort<std::string>("value")};
BT::PortsList set_double_parameter_ports = {BT::InputPort<std::string>("name_space"), BT::InputPort<std::string>("name"), BT::InputPort<double>("value")};
BT::PortsList current_pose_ports = {BT::OutputPort<geometry_msgs::Pose>("pose")};
//Registration of conditions
......@@ -70,6 +71,7 @@ void CTiagoNavModuleBT::init(IriBehaviorTreeFactory &factory)
factory.registerSimpleAction("tiago_nav_set_int_parameter", std::bind(&CTiagoNavModuleBT::tiago_nav_set_int_parameter, this, std::placeholders::_1), set_int_parameter_ports);
factory.registerSimpleAction("tiago_nav_set_string_parameter", std::bind(&CTiagoNavModuleBT::tiago_nav_set_string_parameter, this, std::placeholders::_1), set_string_parameter_ports);
factory.registerSimpleAction("tiago_nav_set_double_parameter", std::bind(&CTiagoNavModuleBT::tiago_nav_set_double_parameter, this, std::placeholders::_1), set_double_parameter_ports);
factory.registerSimpleAction("get_tiago_nav_current_pose", std::bind(&CTiagoNavModuleBT::get_tiago_nav_current_pose, this, std::placeholders::_1), current_pose_ports);
#ifdef HAVE_WAYPOINTS
BT::PortsList sync_go_to_waypoints_ports = {BT::InputPort<std::string>("group"), BT::InputPort<unsigned int>("first_index"), BT::InputPort<unsigned int>("num"), BT::InputPort<bool>("continue_on_error")};
......@@ -664,6 +666,16 @@ BT::NodeStatus CTiagoNavModuleBT::tiago_nav_set_double_parameter(BT::TreeNode& s
return BT::NodeStatus::FAILURE;
}
BT::NodeStatus CTiagoNavModuleBT::get_tiago_nav_current_pose(BT::TreeNode& self)
{
ROS_DEBUG("CTiagoNavModuleBT::get_tiago_nav_current_pose-> get_tiago_nav_current_pose");
geometry_msgs::Pose pose_out = this->tiago_nav_module.getCurrentPose();
self.setOutput("pose", pose_out);
return BT::NodeStatus::SUCCESS;
}
BT::NodeStatus CTiagoNavModuleBT::sync_cancel_tiago_nav_action(void)
{
ROS_DEBUG("CTiagoNavModuleBT::sync_cancel_tiago_nav_action-> sync_cancel_tiago_nav_action");
......
src/xml/bt_test.xml
+ 148
− 18
View file @ da6306b8
......@@ -4,19 +4,36 @@
<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}"/>
<SubTree ID="move_to_pose"/>
<SubTree ID="move_to_position"/>
<SubTree ID="move_to_orientation"/>
</SequenceStar>
</BehaviorTree>
<!-- ////////// -->
<BehaviorTree ID="set_fingers_pos_blackboard">
<BehaviorTree ID="move_to_pose">
<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"/>
<Action ID="set_tiago_nav_goal_frame_inputs" frame_id="{frame_id}"/>
<Action ID="set_tiago_nav_goal_frame" frame_id="{frame_id}"/>
<Action ID="set_tiago_nav_go_to_pose_inputs" x="{x}" y="{y}" yaw="{yaw}" heading_tol="{heading_tol}" x_y_pos_tol="{x_y_pos_tol}" new_goal="{new_goal}"/>
<Action ID="async_tiago_nav_go_to_pose" x="{x}" y="{y}" yaw="{yaw}" heading_tol="{heading_tol}" x_y_pos_tol="{x_y_pos_tol}" new_goal="{new_goal}"/>
</SequenceStar>
</BehaviorTree>
<!-- ////////// -->
<BehaviorTree ID="move_to_position">
<SequenceStar>
<Action ID="set_tiago_nav_goal_frame_inputs" frame_id="{frame_id}"/>
<Action ID="set_tiago_nav_goal_frame" frame_id="{frame_id}"/>
<Action ID="set_tiago_nav_go_to_pose_inputs" x="{x}" y="{y}" yaw="{yaw}" heading_tol="{heading_tol}" x_y_pos_tol="{x_y_pos_tol}" new_goal="{new_goal}"/>
<Action ID="async_tiago_nav_go_to_position" x="{x}" y="{y}" x_y_pos_tol="{x_y_pos_tol}" new_goal="{new_goal}"/>
</SequenceStar>
</BehaviorTree>
<!-- ////////// -->
<BehaviorTree ID="move_to_orientation">
<SequenceStar>
<Action ID="set_tiago_nav_goal_frame_inputs" frame_id="{frame_id}"/>
<Action ID="set_tiago_nav_goal_frame" frame_id="{frame_id}"/>
<Action ID="set_tiago_nav_go_to_pose_inputs" x="{x}" y="{y}" yaw="{yaw}" heading_tol="{heading_tol}" x_y_pos_tol="{x_y_pos_tol}" new_goal="{new_goal}"/>
<Action ID="async_tiago_nav_go_to_orientation" yaw="{yaw}" heading_tol="{heading_tol}" new_goal="{new_goal}"/>
</SequenceStar>
</BehaviorTree>
<!-- ////////// -->
......@@ -32,18 +49,131 @@
<Action ID="reset_start_tree"/>
<Action ID="reset_stop_tree"/>
<!-- tiago_nav_module -->
<Action ID="sync_cancel_tiago_nav_action"/>
<Action ID="async_cancel_tiago_nav_action"/>
<Action ID="async_is_tiago_nav_finished"/>
<Action ID="sync_tiago_nav_go_to_pose">
<input_port name="x"> desired X positon with respect to the goal frame</input_port>
<input_port name="y"> desired Y positon with respect to the goal frame</input_port>
<input_port name="yaw"> desired orientation with respect to the goal frame</input_port>
<input_port default="-1.0" name="heading_tol"> heading tolerance for the goal</input_port>
<input_port default="-1.0" name="x_y_pos_tol"> position tolerance for the goal</input_port>
</Action>
<Action ID="async_tiago_nav_go_to_pose">
<input_port name="x"> desired X positon with respect to the goal frame</input_port>
<input_port name="y"> desired Y positon with respect to the goal frame</input_port>
<input_port name="yaw"> desired orientation with respect to the goal frame</input_port>
<input_port default="-1.0" name="heading_tol"> heading tolerance for the goal</input_port>
<input_port default="-1.0" name="x_y_pos_tol"> position tolerance for the goal</input_port>
<input_port default="True" name="new_goal"> If it's a new_goal</input_port>
</Action>
<Action ID="sync_tiago_nav_go_to_orientation">
<input_port name="yaw"> desired orientation with respect to the goal frame</input_port>
<input_port default="-1.0" name="heading_tol"> heading tolerance for the goal</input_port>
</Action>
<Action ID="async_tiago_nav_go_to_orientation">
<input_port name="yaw"> desired orientation with respect to the goal frame</input_port>
<input_port default="-1.0" name="heading_tol"> heading tolerance for the goal</input_port>
<input_port default="True" name="new_goal"> If it's a new_goal</input_port>
</Action>
<Action ID="sync_tiago_nav_go_to_position">
<input_port name="x"> desired X positon with respect to the goal frame</input_port>
<input_port name="y"> desired Y positon with respect to the goal frame</input_port>
<input_port default="-1.0" name="x_y_pos_tol"> position tolerance for the goal</input_port>
</Action>
<Action ID="async_tiago_nav_go_to_position">
<input_port name="x"> desired X positon with respect to the goal frame</input_port>
<input_port name="y"> desired Y positon with respect to the goal frame</input_port>
<input_port default="-1.0" name="x_y_pos_tol"> position tolerance for the goal</input_port>
<input_port default="True" name="new_goal"> If it's a new_goal</input_port>
</Action>
<Action ID="sync_tiago_nav_go_to_poi">
<input_port name="poi"> name of the desired point of interest to move to</input_port>
</Action>
<Action ID="async_tiago_nav_go_to_poi">
<input_port name="poi"> name of the desired point of interest to move to</input_port>
</Action>
<Action ID="sync_tiago_nav_go_to_waypoints">
<input_port name="group"> Name of the desired group of points of interest</input_port>
<input_port name="first_index"> The index of the first waypoint to execute inside the list of interest points inside the group</input_port>
<input_port name="num"> The number of waypoints to execute. If 0 is provided, all of them will be executed</input_port>
<input_port default="False" name="continue_on_error"> Whether to contiune on the next waypoint when the current one can not be reached (true) or not (false). By default it is set to not continue</input_port>
</Action>
<Action ID="async_tiago_nav_go_to_waypoints">
<input_port name="group"> Name of the desired group of points of interest</input_port>
<input_port name="first_index"> The index of the first waypoint to execute inside the list of interest points inside the group</input_port>
<input_port name="num"> The number of waypoints to execute. If 0 is provided, all of them will be executed</input_port>
<input_port default="False" name="continue_on_error"> Whether to contiune on the next waypoint when the current one can not be reached (true) or not (false). By default it is set to not continue</input_port>
<input_port default="True" name="new_goal"> If it's a new_goal</input_port>
</Action>
<Action ID="get_tiago_nav_current_waypoint">
<output_port name="waypoint"> The index of the current waypoint being executed</output_port>
</Action>
<Action ID="set_tiago_nav_goal_frame">
<input_port name="frame_id"> The name of the new reference frame for the future goals</input_port>
</Action>
<Action ID="tiago_nav_map_change">
<input_port name="map_name"> name of the desired map</input_port>
</Action>
<Action ID="tiago_nav_costmaps_clear"/>
<Action ID="tiago_nav_costmaps_enable_auto_clear">
<input_port name="rate_hz"> the desired clearing rate in Hz. This value should be less than 1 Hz, normally in the range of 0.1 to 0.01 Hz</input_port>
</Action>
<Action ID="tiago_nav_costmaps_disable_auto_clear"/>
<Action ID="tiago_nav_set_bool_parameter">
<input_port name="name_space"> String with the parameter name space</input_port>
<input_port name="name"> String with the name of the parameter to change</input_port>
<input_port name="value"> the new value for the parameter</input_port>
</Action>
<Action ID="tiago_nav_set_int_parameter">
<input_port name="name_space"> String with the parameter name space</input_port>
<input_port name="name"> String with the name of the parameter to change</input_port>
<input_port name="value"> the new value for the parameter</input_port>
</Action>
<Action ID="tiago_nav_set_string_parameter">
<input_port name="name_space"> String with the parameter name space</input_port>
<input_port name="name"> String with the name of the parameter to change</input_port>
<input_port name="value"> the new value for the parameter</input_port>
</Action>
<Action ID="tiago_nav_set_double_parameter">
<input_port name="name_space"> String with the parameter name space</input_port>
<input_port name="name"> String with the name of the parameter to change</input_port>
<input_port name="value"> the new value for the parameter</input_port>
</Action>
<Action ID="get_tiago_nav_current_pose">
<output_port name="pose"> current pose</output_port>
</Action>
<Condition ID="is_tiago_nav_finished"/>
<Condition ID="is_tiago_nav_module_running"/>
<Condition ID="is_tiago_nav_module_success"/>
<Condition ID="is_tiago_nav_module_action_start_fail"/>
<Condition ID="is_tiago_nav_module_timeout"/>
<Condition ID="is_tiago_nav_module_fb_watchdog"/>
<Condition ID="is_tiago_nav_module_aborted"/>
<Condition ID="is_tiago_nav_module_preempted"/>
<Condition ID="is_tiago_nav_module_rejected"/>
<Condition ID="is_tiago_nav_module_set_param_fail"/>
<Condition ID="is_tiago_nav_module_param_not_present"/>
<Condition ID="is_tiago_nav_module_no_odom"/>
<Condition ID="is_tiago_nav_module_no_transform"/>
<Condition ID="is_tiago_nav_module_unknown_map"/>
<Condition ID="is_tiago_nav_module_invalid_mode"/>
<!-- 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 ID="set_tiago_nav_go_to_pose_inputs">
<output_port name="x"> desired X positon with respect to the goal frame</output_port>
<output_port name="y"> desired Y positon with respect to the goal frame</output_port>
<output_port name="yaw"> desired orientation with respect to the goal frame</output_port>
<output_port default="-1.0" name="heading_tol"> heading tolerance for the goal</output_port>
<output_port default="-1.0" name="x_y_pos_tol"> position tolerance for the goal</output_port>
<output_port default="True" name="new_goal"> If it's a new_goal</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 ID="set_tiago_nav_goal_frame_inputs">
<output_port name="frame_id"> The name of the new reference frame for the future goals</output_port>
</Action>
<!-- Tree -->
<SubTree ID="move_to_pose"/>
<SubTree ID="move_to_position"/>
<SubTree ID="move_to_posorientation"/>
</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