diff --git a/humanoid_modules/include/humanoid_modules/joints_module.h b/humanoid_modules/include/humanoid_modules/joints_module.h index 5028260056106d53a4d3730634846e4d3c608d86..7efd9e5332b7c311098e115f5138d5eedb6637ec 100644 --- a/humanoid_modules/include/humanoid_modules/joints_module.h +++ b/humanoid_modules/include/humanoid_modules/joints_module.h @@ -43,7 +43,7 @@ class CJointsModule : public CHumanoidModule<joints_module::JointsModuleConfig> CJointsModule(const std::string &name); /* control functions */ //execute joints trajectory of a vector of servos id - bool execute(std::vector<unsigned int> &servos_id, std::vector<double> &angles, std::vector<double> &speeds, const std::vector<double> &accels=std::vector<double>()); + void execute(std::vector<unsigned int> &servos_id, std::vector<double> &angles, std::vector<double> &speeds, const std::vector<double> &accels=std::vector<double>()); //Add servo goal void add_goal(unsigned int servo_id, double angle, double speed, double acceleration); //Execute action with servos added (add_servo) diff --git a/humanoid_modules/src/joints_module.cpp b/humanoid_modules/src/joints_module.cpp index 515d94d9789bfb4d3de5f30c4e5d35c26f1d01e9..b56f64f277a34844f416830ecd8b092abd8c1e52 100644 --- a/humanoid_modules/src/joints_module.cpp +++ b/humanoid_modules/src/joints_module.cpp @@ -2,9 +2,6 @@ #define SERVO_ACCEL 0.3 -/** - * cambiar funcion is-finished para que tmb tenga en cuenta el flag new_trajectory - */ CJointsModule::CJointsModule(const std::string &name) : CHumanoidModule(name,JOINTS_MODULE), joints_trajectory("joint_trajectory_action",name) { @@ -134,7 +131,7 @@ void CJointsModule::reconfigure_callback(joints_module::JointsModuleConfig &conf this->unlock(); } -bool CJointsModule::execute(std::vector<unsigned int> &servos_id, std::vector<double> &angles, std::vector<double> &speeds,const std::vector<double> &accels) +void CJointsModule::execute(std::vector<unsigned int> &servos_id, std::vector<double> &angles, std::vector<double> &speeds,const std::vector<double> &accels) { unsigned int i,j; bool servo_ok=true; @@ -142,13 +139,6 @@ bool CJointsModule::execute(std::vector<unsigned int> &servos_id, std::vector<do this->lock(); if(this->state==JOINTS_MODULE_IDLE)// trajectory is in progres { - /*Clear goal*/ -/* goal.trajectory.joint_names.clear(); - goal.trajectory.points[0].positions.clear(); - goal.trajectory.points[0].velocities.clear(); - goal.trajectory.points[0].accelerations.clear()*/; - /* clear vector */ -// servos_name.clear(); if(accels.size()>0) { @@ -197,8 +187,7 @@ bool CJointsModule::execute(std::vector<unsigned int> &servos_id, std::vector<do this->new_trajectory=true; } this->unlock(); - - return true; + } void CJointsModule::execute_goal(void)