Added a function to return the name of the group joints in the module.

The client reads the group joint names from the module to initialize the joints motion.

include/tiago_arm_module/tiago_arm_module.h

@@ -87,6 +87,7 @@ class CTiagoArmModule : public CModule<tiago_arm_module::TiagoArmModuleConfig>
     std::string get_planner(void);
     bool set_group_name(std::string &group_id);
     std::string get_group_name(void);
+    std::vector<std::string> get_group_joints(void);
     void set_max_planning_attempts(unsigned int num);
     unsigned int get_max_planning_attempts(void);
     void set_max_planning_time(double time);

src/tiago_arm_client_alg_node.cpp

@@ -51,28 +51,35 @@ void TiagoArmClientAlgNode::node_config_update(Config &config, uint32_t level)
   std::vector< std::string > joint_names;
   std::vector<double> joint_pos;
   std::vector<double> joint_tol; 
+  std::string move_group;
 
   this->alg_.lock();
   if(config.start_joints)
   {
-    joint_states.name.resize(8);
-    joint_states.name[0]="torso_lift_joint";
-    joint_states.name[1]="arm_1_joint";
-    joint_states.name[2]="arm_2_joint";
-    joint_states.name[3]="arm_3_joint";
-    joint_states.name[4]="arm_4_joint";
-    joint_states.name[5]="arm_5_joint";
-    joint_states.name[6]="arm_6_joint";
-    joint_states.name[7]="arm_7_joint";
-    joint_states.position.resize(8);
-    joint_states.position[0]=config.torso;
-    joint_states.position[1]=config.joint1;
-    joint_states.position[2]=config.joint2;
-    joint_states.position[3]=config.joint3;
-    joint_states.position[4]=config.joint4;
-    joint_states.position[5]=config.joint5;
-    joint_states.position[6]=config.joint6;
-    joint_states.position[7]=config.joint7;
+    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)
+    {
+      joint_states.position[0]=config.torso;
+      joint_states.position[1]=config.joint1;
+      joint_states.position[2]=config.joint2;
+      joint_states.position[3]=config.joint3;
+      joint_states.position[4]=config.joint4;
+      joint_states.position[5]=config.joint5;
+      joint_states.position[6]=config.joint6;
+      joint_states.position[7]=config.joint7;
+    }
+    else
+    {
+      joint_states.position[0]=config.joint1;
+      joint_states.position[1]=config.joint2;
+      joint_states.position[2]=config.joint3;
+      joint_states.position[3]=config.joint4;
+      joint_states.position[4]=config.joint5;
+      joint_states.position[5]=config.joint6;
+      joint_states.position[6]=config.joint7;
+    }
     this->arm.move_to(joint_states,config.joint_tol);
     config.start_joints=false;
   }

src/tiago_arm_module.cpp

@@ -229,6 +229,14 @@ std::string CTiagoArmModule::get_group_name(void)
   return this->goal.request.group_name;
 }
 
+std::vector<std::string> CTiagoArmModule::get_group_joints(void)
+{
+  if(this->current_move_group!=-1)
+    return this->move_group_joints[this->current_move_group];
+  else
+    return std::vector<std::string>();
+}
+
 void CTiagoArmModule::set_max_planning_attempts(unsigned int num)
 {
   this->lock();