action_client/src/action_client_alg_node.cpp

@@ -19,8 +19,6 @@ ActionClientAlgNode::ActionClientAlgNode(void) :
   // [init action servers]
   
   // [init action clients]
-
-  this->action.add_whole_body();
 }
 
 ActionClientAlgNode::~ActionClientAlgNode(void)

humanoid_modules/include/humanoid_modules/humanoid_module.h

@@ -96,12 +96,17 @@ CHumanoidModule<Module>::CHumanoidModule(const std::string &name,motion_module_t
 template<class Module>
 void CHumanoidModule<Module>::add_servo(const std::string &name)
 {
-  unsigned int i=0;
+  unsigned int i=0,j=0;
 
   for(i=0;i<NUM_SERVOS;i++)
   {
     if(servo_names[i]==name)
     {
+      for(j=0;j<this->assigned_servos.size();j++)
+      {
+        if(this->assigned_servos[j]==name)
+          return;// servo already assigned to this module
+      }
       this->assigned_servos.push_back(name);
       return;
     }

humanoid_modules/src/action_module.cpp

@@ -9,6 +9,7 @@ CActionModule::CActionModule(const std::string &name) : CHumanoidModule(name,ACT
   this->cancel_pending=false;
   this->new_action=false;
   this->action_id=-1;
+  this->add_whole_body();
 }
 
 void CActionModule::state_machine(void)

humanoid_modules/src/walk_module.cpp

@@ -20,6 +20,8 @@ CWalkModule::CWalkModule(const std::string &name) : CHumanoidModule(name,WALK_MO
   this->cmd_vel_timer.stop();
   /* joint states subscriber */
   this->joint_state_subscriber=this->module_nh.subscribe("joint_states",1,&CWalkModule::joint_state_callback,this);
+  this->add_left_leg();
+  this->add_right_leg();
 }
 
 void CWalkModule::state_machine(void)

walk_client/src/walk_client_alg_node.cpp

@@ -20,7 +20,6 @@ WalkClientAlgNode::WalkClientAlgNode(void) :
   // [init action servers]
   
   // [init action clients]
-  this->walk.add_whole_body();
 }
 
 WalkClientAlgNode::~WalkClientAlgNode(void)