From 3c2268777933e451692ed52ae2819c6ec46e9446 Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 31 May 2017 08:36:21 +0200
Subject: [PATCH] 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.
---
.../include/humanoid_modules/humanoid_module.h | 7 ++++++-
humanoid_modules/src/action_module.cpp | 1 +
humanoid_modules/src/walk_module.cpp | 2 ++
3 files changed, 9 insertions(+), 1 deletion(-)
diff --git a/humanoid_modules/include/humanoid_modules/humanoid_module.h b/humanoid_modules/include/humanoid_modules/humanoid_module.h
index c943a29..b9345d4 100644
--- a/humanoid_modules/include/humanoid_modules/humanoid_module.h
+++ b/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;
}
diff --git a/humanoid_modules/src/action_module.cpp b/humanoid_modules/src/action_module.cpp
index aec971a..20af838 100644
--- a/humanoid_modules/src/action_module.cpp
+++ b/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)
diff --git a/humanoid_modules/src/walk_module.cpp b/humanoid_modules/src/walk_module.cpp
index 11de797..1682b70 100644
--- a/humanoid_modules/src/walk_module.cpp
+++ b/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)
--
GitLab