diff --git a/cfg/TiagoArmBtClient.cfg b/cfg/TiagoArmBtClient.cfg
index 1925d3ed05cc57bf420cc05f775f87af7530d981..98c3e8728e1a6f89238f9cfbcecb419dcda70f6b 100755
--- a/cfg/TiagoArmBtClient.cfg
+++ b/cfg/TiagoArmBtClient.cfg
@@ -48,12 +48,7 @@ add_bt_client_params(gen)
gen.add("START", bool_t, 0, "synchronous start", False)
gen.add("RESTART", bool_t, 0, "asynchronous restart of BT", False)
-arm_to_move_enum = gen.enum([ gen.const("tiago_arm", int_t, 0, "Tiago Arm"),
- gen.const("ivo_left_arm", int_t, 1, "Ivo left arm"),
- gen.const("tiago_dual_left_arm", int_t, 2, "Tiago dual left arm")],
- "The posibles arms to control")
move_joints.add("set_joints", bool_t, 0, "Set selected angles", False)
-move_joints.add("arm_to_move", int_t, 0, "The arm to move", 0, 0, 1, edit_method=arm_to_move_enum)
move_joints.add("torso", double_t, 0, "Target angle for torso", 0.15, 0.0, 0.35)
move_joints.add("joint1", double_t, 0, "Target angle for joint 1", 0.2, -3.14159, 3.14159)
move_joints.add("joint2", double_t, 0, "Target angle for joint 2", -1.3, -3.14159, 3.14159)
diff --git a/config/ari_arm_module_default.yaml b/config/ari_arm_module_default.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..ba28a4c7c40889df7159dd967bd4c263f5baaaab
--- /dev/null
+++ b/config/ari_arm_module_default.yaml
@@ -0,0 +1,26 @@
+arm_module_rate_hz: 1.0
+
+move_num_retries: 1
+move_feedback_watchdog_time_s: 1.0
+move_enable_watchdog: False
+move_timeout_s: 10.0
+move_enable_timeout: True
+move_enabled: True
+
+#joint states watchdog time
+js_watchdog_time_s: 1.0
+
+planner_id: "RRTkConfigDefault"
+group_name: "arm_left"
+max_plan_attempts: 5
+max_plan_time: 10.0
+max_vel_scale: 1.0
+end_effector_tool: "arm_left_grasping_frame"
+
+ws_frame_id: "/base_footprint"
+ws_x_min: -1.0
+ws_y_min: -1.0
+ws_z_min: -1.0
+ws_x_max: 1.0
+ws_y_max: 1.0
+ws_z_max: 1.0
diff --git a/config/ari_move_groups.yaml b/config/ari_move_groups.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..c38d0a514f42e3e0eedb4c84d3f605f774bb84d1
--- /dev/null
+++ b/config/ari_move_groups.yaml
@@ -0,0 +1,13 @@
+move_groups:
+ - arm_left
+ - arm_right
+arm_left:
+ - arm_left_1_joint
+ - arm_left_2_joint
+ - arm_left_3_joint
+ - arm_left_4_joint
+arm_right:
+ - arm_right_1_joint
+ - arm_right_2_joint
+ - arm_right_3_joint
+ - arm_right_4_joint
diff --git a/include/tiago_arm_module/tiago_arm_bt_module.h b/include/tiago_arm_module/tiago_arm_bt_module.h
index 417bd3136ec0463a4140f0b7d7351e0f6d93996b..8debac86c60e4884879a6c2cc5ce2273028ad0a8 100644
--- a/include/tiago_arm_module/tiago_arm_bt_module.h
+++ b/include/tiago_arm_module/tiago_arm_bt_module.h
@@ -83,7 +83,7 @@ class CTiagoArmModuleBT
*
* It has the following input ports:
*
- * joint_targets (sensor_msgs::JointState): The target JointState
+ * target_joints (sensor_msgs::JointState): The target JointState
*
* joint_tol (double): [Optional] Joints tolerance.
*
@@ -105,7 +105,7 @@ class CTiagoArmModuleBT
*
* It has the following input ports:
*
- * joint_targets (sensor_msgs::JointState): The target JointState
+ * target_joints (sensor_msgs::JointState): The target JointState
*
* joint_tol (double): [Optional] Joints tolerance.
*
@@ -419,7 +419,7 @@ class CTiagoArmModuleBT
*
* It has the following input ports:
*
- * name (std::string): The object name.
+ * obj_name (std::string): The object name.
*
* pose (geometry_msgs::PoseStamped): The object pose.
*
@@ -443,7 +443,7 @@ class CTiagoArmModuleBT
*
* It has the following input ports:
*
- * name (std::string): The object name.
+ * obj_name (std::string): The object name.
*
* pose (geometry_msgs::PoseStamped): The new object pose.
*
@@ -461,7 +461,7 @@ class CTiagoArmModuleBT
*
* It has the following input ports:
*
- * name (std::string): The object name.
+ * obj_name (std::string): The object name.
*
* width (double): The object width.
*
@@ -483,7 +483,7 @@ class CTiagoArmModuleBT
*
* It has the following input ports:
*
- * name (std::string): The object name.
+ * obj_name (std::string): The object name.
*
* \return a BT:NodeStatus indicating whether the request has been
* successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
@@ -499,7 +499,7 @@ class CTiagoArmModuleBT
*
* It has the following input ports:
*
- * name (std::string): The object name.
+ * obj_name (std::string): The object name.
*
* \return a BT:NodeStatus indicating whether the request has been
* successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
@@ -515,7 +515,7 @@ class CTiagoArmModuleBT
*
* It has the following input ports:
*
- * name (std::string): The object name.
+ * obj_name (std::string): The object name.
*
* \return a BT:NodeStatus indicating whether the request has been
* successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
@@ -531,7 +531,7 @@ class CTiagoArmModuleBT
*
* It has the following input ports:
*
- * name (std::string): The object name.
+ * obj_name (std::string): The object name.
*
* link (std::string): [Optional] The gripper link.
*
@@ -549,7 +549,7 @@ class CTiagoArmModuleBT
*
* It has the following input ports:
*
- * name (std::string): The object name.
+ * obj_name (std::string): The object name.
*
* \return a BT:NodeStatus indicating whether the request has been
* successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
@@ -565,7 +565,7 @@ class CTiagoArmModuleBT
*
* It has the following input ports:
*
- * name (std::string): The object name.
+ * obj_name (std::string): The object name.
*
* \return a BT:NodeStatus indicating whether the request has been
* successfull (BT:NodeStatus::SUCCESS) or not (BT:NodeStatus::FAILURE).
diff --git a/launch/ari_arm_bt_client.launch b/launch/ari_arm_bt_client.launch
new file mode 100644
index 0000000000000000000000000000000000000000..989a98214f6e6abc74ab603ce31524b795c3ea52
--- /dev/null
+++ b/launch/ari_arm_bt_client.launch
@@ -0,0 +1,43 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+ <arg name="node_name" default="arm_client" />
+ <arg name="output" default="screen" />
+ <arg name="launch_prefix" default="" />
+ <arg name="config_file" default="$(find tiago_arm_module)/config/ari_arm_module_default.yaml" />
+ <arg name="move_groups_file" default="$(find tiago_arm_module)/config/ari_move_groups.yaml" />
+ <arg name="joint_states_topic" default="/joint_states"/>
+ <arg name="arm_action" default="/move_group"/>
+ <arg name="planning_scene_topic" default="/planning_scene"/>
+ <arg name="get_planning_scene_service" default="/get_planning_scene"/>
+
+ <arg name="tree_path" default="$(find tiago_arm_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_arm_module"
+ type="tiago_arm_bt_client"
+ output="$(arg output)"
+ launch-prefix="$(arg launch_prefix)">
+ <remap from="~/arm_module/joint_states"
+ to="$(arg joint_states_topic)"/>
+ <remap from="~/arm_module/arm"
+ to="$(arg arm_action)"/>
+ <remap from="~/arm_module/planning_scene"
+ to="$(arg planning_scene_topic)"/>
+ <remap from="~/arm_module/get_planning_scene"
+ to="$(arg get_planning_scene_service)"/>
+ <rosparam file="$(arg config_file)" command="load" ns="arm_module" />
+ <rosparam file="$(arg move_groups_file)" command="load" ns="arm_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/ari_arm_client.launch b/launch/ari_arm_client.launch
new file mode 100644
index 0000000000000000000000000000000000000000..04f117840ad172b1e5045d4a1fae2712e772633a
--- /dev/null
+++ b/launch/ari_arm_client.launch
@@ -0,0 +1,36 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+ <arg name="node_name" default="arm_client" />
+ <arg name="output" default="screen" />
+ <arg name="launch_prefix" default="" />
+ <arg name="config_file" default="$(find tiago_arm_module)/config/ari_arm_module_default.yaml" />
+ <arg name="move_groups_file" default="$(find tiago_arm_module)/config/ari_move_groups.yaml" />
+ <arg name="joint_states_topic" default="/joint_states"/>
+ <arg name="arm_action" default="/move_group"/>
+ <arg name="planning_scene_topic" default="/planning_scene"/>
+ <arg name="get_planning_scene_service" default="/get_planning_scene"/>
+
+ <!-- launch the play motion client node -->
+ <node name="$(arg node_name)"
+ pkg="tiago_arm_module"
+ type="tiago_arm_client"
+ output="$(arg output)"
+ launch-prefix="$(arg launch_prefix)">
+ <remap from="~/arm_module/joint_states"
+ to="$(arg joint_states_topic)"/>
+ <remap from="~/arm_module/arm"
+ to="$(arg arm_action)"/>
+ <remap from="~/arm_module/planning_scene"
+ to="$(arg planning_scene_topic)"/>
+ <remap from="~/arm_module/get_planning_scene"
+ to="$(arg get_planning_scene_service)"/>
+ <rosparam file="$(arg config_file)" command="load" ns="arm_module" />
+ <rosparam file="$(arg move_groups_file)" command="load" ns="arm_module" />
+ </node>
+
+ <!-- launch dynamic reconfigure -->
+ <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
+ output="screen"/>
+
+</launch>
+
diff --git a/src/tiago_arm_bt_client_alg_node.cpp b/src/tiago_arm_bt_client_alg_node.cpp
index 0512a8ca7a0e6c0628d3b6d2dfad2f1cad046efa..46214478d9feeda1a757764f6b14784e58bbc5da 100644
--- a/src/tiago_arm_bt_client_alg_node.cpp
+++ b/src/tiago_arm_bt_client_alg_node.cpp
@@ -47,6 +47,7 @@ TiagoArmModuleBtClientAlgNode::~TiagoArmModuleBtClientAlgNode(void)
void TiagoArmModuleBtClientAlgNode::node_config_update(tiago_arm_module::TiagoArmBtClientConfig &config, uint32_t level)
{
+ std::string move_group;
this->lock();
if(config.START)
@@ -68,36 +69,37 @@ void TiagoArmModuleBtClientAlgNode::node_config_update(tiago_arm_module::TiagoAr
config.set_joints = false;
std::vector<std::string>().swap(this->target_joint_states.name);
std::vector<double>().swap(this->target_joint_states.position);
- if (config.arm_to_move == 0)//tiago_arm
+ move_group=this->tiago_arm_module.get_group_name();
+ this->target_joint_states.name=this->tiago_arm_module.get_group_joints();
+ this->target_joint_states.position.resize(this->target_joint_states.name.size());
+ if(move_group.find("torso")!=std::string::npos) // Tiago with torso
{
- this->target_joint_states.name.push_back("torso_lift_joint");
- this->target_joint_states.name.push_back("arm_1_joint");
- this->target_joint_states.name.push_back("arm_2_joint");
- this->target_joint_states.name.push_back("arm_3_joint");
- this->target_joint_states.name.push_back("arm_4_joint");
- this->target_joint_states.name.push_back("arm_5_joint");
- this->target_joint_states.name.push_back("arm_6_joint");
- this->target_joint_states.name.push_back("arm_7_joint");
+ this->target_joint_states.position[0]=config.torso;
+ this->target_joint_states.position[1]=config.joint1;
+ this->target_joint_states.position[2]=config.joint2;
+ this->target_joint_states.position[3]=config.joint3;
+ this->target_joint_states.position[4]=config.joint4;
+ this->target_joint_states.position[5]=config.joint5;
+ this->target_joint_states.position[6]=config.joint6;
+ this->target_joint_states.position[7]=config.joint7;
}
- else if (config.arm_to_move == 1 || config.arm_to_move == 2)//ivo_left_arm or tiago_dual_left_arm
+ else if (this->target_joint_states.position.size() == 7) //Tiago
{
- this->target_joint_states.name.push_back("torso_lift_joint");
- this->target_joint_states.name.push_back("arm_left_1_joint");
- this->target_joint_states.name.push_back("arm_left_2_joint");
- this->target_joint_states.name.push_back("arm_left_3_joint");
- this->target_joint_states.name.push_back("arm_left_4_joint");
- this->target_joint_states.name.push_back("arm_left_5_joint");
- this->target_joint_states.name.push_back("arm_left_6_joint");
- this->target_joint_states.name.push_back("arm_left_7_joint");
+ this->target_joint_states.position[0]=config.joint1;
+ this->target_joint_states.position[1]=config.joint2;
+ this->target_joint_states.position[2]=config.joint3;
+ this->target_joint_states.position[3]=config.joint4;
+ this->target_joint_states.position[4]=config.joint5;
+ this->target_joint_states.position[5]=config.joint6;
+ this->target_joint_states.position[6]=config.joint7;
+ }
+ else if (this->target_joint_states.position.size() == 4) //ari
+ {
+ this->target_joint_states.position[0]=config.joint1;
+ this->target_joint_states.position[1]=config.joint2;
+ this->target_joint_states.position[2]=config.joint3;
+ this->target_joint_states.position[3]=config.joint4;
}
- 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)
diff --git a/src/tiago_arm_client_alg_node.cpp b/src/tiago_arm_client_alg_node.cpp
index d34a58e827581f378f7fbaa7b8e2e6e2978951a1..7a6dcb848a44d65817fa08114d7d8c815a882383 100644
--- a/src/tiago_arm_client_alg_node.cpp
+++ b/src/tiago_arm_client_alg_node.cpp
@@ -59,7 +59,7 @@ void TiagoArmClientAlgNode::node_config_update(Config &config, uint32_t level)
move_group=this->arm.get_group_name();
joint_states.name=this->arm.get_group_joints();
joint_states.position.resize(joint_states.name.size());
- if(move_group.find("torso")!=std::string::npos)
+ if(move_group.find("torso")!=std::string::npos) // Tiago with torso
{
joint_states.position[0]=config.torso;
joint_states.position[1]=config.joint1;
@@ -70,7 +70,7 @@ void TiagoArmClientAlgNode::node_config_update(Config &config, uint32_t level)
joint_states.position[6]=config.joint6;
joint_states.position[7]=config.joint7;
}
- else
+ else if (joint_states.position.size() == 7) //Tiago
{
joint_states.position[0]=config.joint1;
joint_states.position[1]=config.joint2;
@@ -80,6 +80,15 @@ void TiagoArmClientAlgNode::node_config_update(Config &config, uint32_t level)
joint_states.position[5]=config.joint6;
joint_states.position[6]=config.joint7;
}
+ else if (joint_states.position.size() == 4) //ari
+ {
+ joint_states.position[0]=config.joint1;
+ joint_states.position[1]=config.joint2;
+ joint_states.position[2]=config.joint3;
+ joint_states.position[3]=config.joint4;
+ }
+ else
+ ROS_ERROR("TiagoArmClientAlgNode -> Number of joints not recognized");
this->arm.move_to(joint_states,config.joint_tol);
config.start_joints=false;
}
diff --git a/src/xml/bt_definitions.xml b/src/xml/bt_definitions.xml
new file mode 100644
index 0000000000000000000000000000000000000000..8a1cdd15abac17e8f75c0047e8ec7086d1164169
--- /dev/null
+++ b/src/xml/bt_definitions.xml
@@ -0,0 +1,155 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="BehaviorTree">
+ <!-- ////////// -->
+ <BehaviorTree ID="BehaviorTree">
+ </BehaviorTree>
+ <!-- ////////// -->
+ <TreeNodesModel>
+ <!-- tiago_arm_module -->
+ <Action ID="sync_cancel_tiago_arm_action"/>
+ <Action ID="async_cancel_tiago_arm_action"/>
+ <Action ID="async_is_tiago_arm_finished"/>
+ <Action ID="set_tiago_arm_workspace">
+ <input_port name="frame_id"> Workspace's frame ID</input_port>
+ <input_port name="x_min"> Workspace's x min</input_port>
+ <input_port name="y_min"> Workspace's y min</input_port>
+ <input_port name="z_min"> Workspace's z min</input_port>
+ <input_port name="x_max"> Workspace's x max</input_port>
+ <input_port name="y_max"> Workspace's y max</input_port>
+ <input_port name="z_max"> Workspace's z max</input_port>
+ </Action>
+ <Action ID="sync_tiago_arm_move_to_joints">
+ <input_port name="target_joints"> The target JointState</input_port>
+ <input_port name="joint_tol"> Joints tolerance</input_port>
+ </Action>
+ <Action ID="async_tiago_arm_move_to_joints">
+ <input_port name="target_joints"> The target JointState</input_port>
+ <input_port name="joint_tol"> Joints tolerance</input_port>
+ <input_port name="new_goal"> If it's a new_goal</input_port>
+ </Action>
+ <Action ID="get_tiago_arm_current_joint_angles">
+ <output_port name="joint_states"> The current JointState</output_port>
+ </Action>
+ <Action ID="sync_tiago_arm_move_to_pose">
+ <input_port name="pose"> The target pose</input_port>
+ <input_port name="position_tol"> Position tolerance</input_port>
+ <input_port name="orientation_tol"> Orientation tolerance</input_port>
+ </Action>
+ <Action ID="async_tiago_arm_move_to_pose">
+ <input_port name="pose"> The target pose</input_port>
+ <input_port name="position_tol"> Position tolerance</input_port>
+ <input_port name="orientation_tol"> Orientation tolerance</input_port>
+ <input_port name="new_goal"> If it's a new_goal</input_port>
+ </Action>
+ <Action ID="get_tiago_arm_current_pose">
+ <output_port name="pose"> The current pose</output_port>
+ </Action>
+ <Action ID="set_tiago_arm_planner">
+ <input_port name="planner_id"> The planner id</input_port>
+ </Action>
+ <Action ID="get_tiago_arm_planner">
+ <output_port name="planner_id"> The planner id</output_port>
+ </Action>
+ <Action ID="set_tiago_arm_group_name">
+ <input_port name="group_id"> The group id</input_port>
+ </Action>
+ <Action ID="get_tiago_arm_group_name">
+ <output_port name="group_id"> The group id</output_port>
+ </Action>
+ <Action ID="get_tiago_arm_group_joints">
+ <output_port name="joint_names"> The group joints' names</output_port>
+ </Action>
+ <Action ID="set_tiago_arm_max_planning_attempts">
+ <input_port name="num"> The num of maximum planning attempts</input_port>
+ </Action>
+ <Action ID="get_tiago_arm_max_planning_attempts">
+ <output_port name="num"> The num of maximum planning attempts</output_port>
+ </Action>
+ <Action ID="set_tiago_arm_max_planning_time">
+ <input_port name="time"> The maximum planning time</input_port>
+ </Action>
+ <Action ID="get_tiago_arm_max_planning_time">
+ <output_port name="time"> The maximum planning time</output_port>
+ </Action>
+ <Action ID="set_tiago_arm_velocity_scale_factor">
+ <input_port name="scale"> The velocity scale factor</input_port>
+ </Action>
+ <Action ID="get_tiago_arm_velocity_scale_factor">
+ <output_port name="scale"> The velocity scale factor</output_port>
+ </Action>
+ <Action ID="tiago_arm_add_object">
+ <input_port name="obj_name"> The object name</input_port>
+ <input_port name="pose"> The object pose</input_port>
+ <input_port name="width"> The object width</input_port>
+ <input_port name="length"> The object length</input_port>
+ <input_port name="height"> The object height</input_port>
+ </Action>
+ <Action ID="tiago_arm_update_object_pose">
+ <input_port name="obj_name"> The object name</input_port>
+ <input_port name="pose"> The object pose</input_port>
+ </Action>
+ <Action ID="tiago_arm_update_object_size">
+ <input_port name="obj_name"> The object name</input_port>
+ <input_port name="width"> The object width</input_port>
+ <input_port name="length"> The object length</input_port>
+ <input_port name="height"> The object height</input_port>
+ </Action>
+ <Action ID="tiago_arm_remove_object">
+ <input_port name="obj_name"> The object name</input_port>
+ </Action>
+ <Action ID="tiago_arm_add_object_to_environment">
+ <input_port name="obj_name"> The object name</input_port>
+ </Action>
+ <Action ID="tiago_arm_remove_object_from_environment">
+ <input_port name="obj_name"> The object name</input_port>
+ </Action>
+ <Action ID="tiago_arm_attach_object_to_robot">
+ <input_port name="obj_name"> The object name</input_port>
+ <input_port name="link"> The gripper link</input_port>
+ </Action>
+ <Action ID="tiago_arm_dettach_object_from_robot">
+ <input_port name="obj_name"> The object name</input_port>
+ </Action>
+ <Action ID="tiago_arm_disable_collision">
+ <input_port name="obj_name"> The object name</input_port>
+ </Action>
+ <Action ID="tiago_arm_get_current_planning_scene">
+ <input_port name="component"> Planing scene component</input_port>
+ </Action>
+ <Action ID="tiago_arm_clear_path_constraints"/>
+ <Action ID="tiago_arm_add_orientation_path_constraint">
+ <input_port name="quat"> The orientation constraint</input_port>
+ <input_port name="orientation_tol"> The orientation constraint tolerance</input_port>
+ </Action>
+ <Action ID="tiago_arm_add_plane_path_constraint">
+ <input_port name="plane"> The plane definition</input_port>
+ <input_port name="frame_id"> The plane frame_id</input_port>
+ <input_port name="offset"> The plane offset</input_port>
+ <input_port name="position_tol"> The position tolerance</input_port>
+ </Action>
+ <Action ID="tiago_arm_add_plane_path_constraint_normal">
+ <input_port name="normal"> The plane definition</input_port>
+ <input_port name="frame_id"> The plane frame_id</input_port>
+ <input_port name="offset"> The plane offset</input_port>
+ <input_port name="position_tol"> The position tolerance</input_port>
+ </Action>
+ <Action ID="tiago_arm_add_joint_goal_constraint">
+ <input_port name="joint_names"> The joint names</input_port>
+ <input_port name="position"> The joint positions</input_port>
+ <input_port name="tol"> The joints tolerance</input_port>
+ </Action>
+ <Condition ID="is_tiago_arm_finished"/>
+ <Condition ID="is_tiago_arm_module_running"/>
+ <Condition ID="is_tiago_arm_module_success"/>
+ <Condition ID="is_tiago_arm_module_action_start_fail"/>
+ <Condition ID="is_tiago_arm_module_timeout"/>
+ <Condition ID="is_tiago_arm_module_fb_watchdog"/>
+ <Condition ID="is_tiago_arm_module_aborted"/>
+ <Condition ID="is_tiago_arm_module_preempted"/>
+ <Condition ID="is_tiago_arm_module_rejected"/>
+ <Condition ID="is_tiago_arm_module_no_joint_states"/>
+ </TreeNodesModel>
+ <!-- ////////// -->
+</root>
+
+
diff --git a/src/xml/bt_test.xml b/src/xml/bt_test.xml
index 55d3aa789711f170ecafacc1e6eab69bc9bdfb41..a7c2f8180f3f0014cd503e023585ec2eb7f87101 100644
--- a/src/xml/bt_test.xml
+++ b/src/xml/bt_test.xml
@@ -95,40 +95,40 @@
<output_port name="scale"> The velocity scale factor</output_port>
</Action>
<Action ID="tiago_arm_add_object">
- <input_port name="name"> The object name</input_port>
+ <input_port name="obj_name"> The object name</input_port>
<input_port name="pose"> The object pose</input_port>
<input_port name="width"> The object width</input_port>
<input_port name="length"> The object length</input_port>
<input_port name="height"> The object height</input_port>
</Action>
<Action ID="tiago_arm_update_object_pose">
- <input_port name="name"> The object name</input_port>
+ <input_port name="obj_name"> The object name</input_port>
<input_port name="pose"> The object pose</input_port>
</Action>
<Action ID="tiago_arm_update_object_size">
- <input_port name="name"> The object name</input_port>
+ <input_port name="obj_name"> The object name</input_port>
<input_port name="width"> The object width</input_port>
<input_port name="length"> The object length</input_port>
<input_port name="height"> The object height</input_port>
</Action>
<Action ID="tiago_arm_remove_object">
- <input_port name="name"> The object name</input_port>
+ <input_port name="obj_name"> The object name</input_port>
</Action>
<Action ID="tiago_arm_add_object_to_environment">
- <input_port name="name"> The object name</input_port>
+ <input_port name="obj_name"> The object name</input_port>
</Action>
<Action ID="tiago_arm_remove_object_from_environment">
- <input_port name="name"> The object name</input_port>
+ <input_port name="obj_name"> The object name</input_port>
</Action>
<Action ID="tiago_arm_attach_object_to_robot">
- <input_port name="name"> The object name</input_port>
+ <input_port name="obj_name"> The object name</input_port>
<input_port name="link"> The gripper link</input_port>
</Action>
<Action ID="tiago_arm_dettach_object_from_robot">
- <input_port name="name"> The object name</input_port>
+ <input_port name="obj_name"> The object name</input_port>
</Action>
<Action ID="tiago_arm_disable_collision">
- <input_port name="name"> The object name</input_port>
+ <input_port name="obj_name"> The object name</input_port>
</Action>
<Action ID="tiago_arm_get_current_planning_scene">
<input_port name="component"> Planing scene component</input_port>