Modified the add_servo() function to check whether the new servo has already been assigned or not.

Modified the walk and action modules to assign the basic necessary at construction time.

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[i]==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)