diff --git a/cfg/TiagoNavBtClient.cfg b/cfg/TiagoNavBtClient.cfg
index 7a94317cc79aaf401f16d7423a7d2a15d7de4769..69caa8416fd2868909ac04c040a4dbd5fef25365 100755
--- a/cfg/TiagoNavBtClient.cfg
+++ b/cfg/TiagoNavBtClient.cfg
@@ -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"))
diff --git a/include/tiago_nav_bt_client_alg_node.h b/include/tiago_nav_bt_client_alg_node.h
index 522b74a126671bd58ab964fd4e108ae937838f2d..c032793e843e779c5f30280c6100df9583ebc117 100644
--- a/include/tiago_nav_bt_client_alg_node.h
+++ b/include/tiago_nav_bt_client_alg_node.h
@@ -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:
/**
diff --git a/include/tiago_nav_module/tiago_nav_bt_module.h b/include/tiago_nav_module/tiago_nav_bt_module.h
index 8f9914dc15d677c3e8d2374f6c7ceb899ae9b3f1..376343aec6823b833599d3fadd0b2fe842e1961b 100644
--- a/include/tiago_nav_module/tiago_nav_bt_module.h
+++ b/include/tiago_nav_module/tiago_nav_bt_module.h
@@ -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.
*
diff --git a/launch/tiago_nav_bt_client.launch b/launch/tiago_nav_bt_client.launch
new file mode 100644
index 0000000000000000000000000000000000000000..60c0d7c7f2ad18ea8e8d5f870ddd1946fa2a15f3
--- /dev/null
+++ b/launch/tiago_nav_bt_client.launch
@@ -0,0 +1,54 @@
+<?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>
+
diff --git a/launch/tiago_nav_bt_client_sim.launch b/launch/tiago_nav_bt_client_sim.launch
new file mode 100644
index 0000000000000000000000000000000000000000..7ab7b5600ab0a20a623ed717e7929dc45f2b3f44
--- /dev/null
+++ b/launch/tiago_nav_bt_client_sim.launch
@@ -0,0 +1,44 @@
+<?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>
+
diff --git a/src/tiago_nav_bt_client_alg_node.cpp b/src/tiago_nav_bt_client_alg_node.cpp
index 3e968e4719c43aa6726c7ba1fa9775306fac0f8d..22bbb5911cb00a442ebc19d1bc29d5e51c3462be 100644
--- a/src/tiago_nav_bt_client_alg_node.cpp
+++ b/src/tiago_nav_bt_client_alg_node.cpp
@@ -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;
diff --git a/src/tiago_nav_bt_module.cpp b/src/tiago_nav_bt_module.cpp
index 30b2d06a77522119e27f66150ae4a7fd3cfcda2a..40f12f8416372c136dfc217f4b05c1de783769d0 100644
--- a/src/tiago_nav_bt_module.cpp
+++ b/src/tiago_nav_bt_module.cpp
@@ -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");
diff --git a/src/xml/bt_test.xml b/src/xml/bt_test.xml
index 832f3f8e6762b93bc4b37c6ec7448b55e48b1522..14097279c1f378fa98c8e1f9247d27c19baec479 100644
--- a/src/xml/bt_test.xml
+++ b/src/xml/bt_test.xml
@@ -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>