Skip to content
Snippets Groups Projects
Commit 3694892a authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Merge branch 'cherry-pick-3c226877' into 'smart_charger'

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

See merge request !1
parents ed9a9c65 2bdd6de2
No related branches found
No related tags found
2 merge requests!4Filtered localization,!1Modified the add_servo() function to check whether the new servo has already been assigned or not.
...@@ -96,12 +96,17 @@ CHumanoidModule<Module>::CHumanoidModule(const std::string &name,motion_module_t ...@@ -96,12 +96,17 @@ CHumanoidModule<Module>::CHumanoidModule(const std::string &name,motion_module_t
template<class Module> template<class Module>
void CHumanoidModule<Module>::add_servo(const std::string &name) 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++) for(i=0;i<NUM_SERVOS;i++)
{ {
if(servo_names[i]==name) 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); this->assigned_servos.push_back(name);
return; return;
} }
......
...@@ -9,6 +9,7 @@ CActionModule::CActionModule(const std::string &name) : CHumanoidModule(name,ACT ...@@ -9,6 +9,7 @@ CActionModule::CActionModule(const std::string &name) : CHumanoidModule(name,ACT
this->cancel_pending=false; this->cancel_pending=false;
this->new_action=false; this->new_action=false;
this->action_id=-1; this->action_id=-1;
this->add_whole_body();
} }
void CActionModule::state_machine(void) void CActionModule::state_machine(void)
......
...@@ -20,6 +20,8 @@ CWalkModule::CWalkModule(const std::string &name) : CHumanoidModule(name,WALK_MO ...@@ -20,6 +20,8 @@ CWalkModule::CWalkModule(const std::string &name) : CHumanoidModule(name,WALK_MO
this->cmd_vel_timer.stop(); this->cmd_vel_timer.stop();
/* joint states subscriber */ /* joint states subscriber */
this->joint_state_subscriber=this->module_nh.subscribe("joint_states",1,&CWalkModule::joint_state_callback,this); 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) void CWalkModule::state_machine(void)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment