Added get_group_joints BT wrapper

include/tiago_arm_module/tiago_arm_bt_module.h

@@ -285,6 +285,24 @@ class CTiagoArmModuleBT
       */
     BT::NodeStatus get_tiago_arm_group_name(BT::TreeNode& self);
 
+    /**
+      * \brief Synchronized get_group_joints TIAGo arm function.
+      *
+      * This function calls get_group_joints of tiago_arm_module.
+      *
+      * It has the following output ports:
+      *
+      *   joint_names (std::vector<std::string>): The group joints' names.
+      *
+      * \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_arm_group_joints(BT::TreeNode& self);
+
     /**
       * \brief Synchronized set_max_planning_attempts TIAGo arm function.
       *

src/tiago_arm_bt_module.cpp

@@ -379,6 +379,17 @@ BT::NodeStatus CTiagoArmModuleBT::get_tiago_arm_group_name(BT::TreeNode& self)
   return BT::NodeStatus::SUCCESS;
 }
 
+BT::NodeStatus CTiagoArmModuleBT::get_tiago_arm_group_joints(BT::TreeNode& self)
+{
+  ROS_DEBUG("CTiagoArmModuleBT::get_tiago_arm_group_joints-> get_tiago_arm_group_joints");
+
+  std::vector<std::string> joint_names;
+  joint_names = this->tiago_arm_module.get_group_joints();
+
+  self.setOutput("joint_names", joint_names);
+  return BT::NodeStatus::SUCCESS;
+}
+
 BT::NodeStatus CTiagoArmModuleBT::set_tiago_arm_max_planning_attempts(BT::TreeNode& self)
 {
   ROS_DEBUG("CTiagoArmModuleBT::set_tiago_arm_max_planning_attempts-> set_tiago_arm_max_planning_attempts");

src/xml/bt_test.xml

@@ -70,6 +70,9 @@
         <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>