diff --git a/humanoid_modules/include/humanoid_modules/joints_module.h b/humanoid_modules/include/humanoid_modules/joints_module.h
index 8bb367cf136ac206c6597b17df96d1b3cd420687..5028260056106d53a4d3730634846e4d3c608d86 100644
--- a/humanoid_modules/include/humanoid_modules/joints_module.h
+++ b/humanoid_modules/include/humanoid_modules/joints_module.h
@@ -17,13 +17,10 @@ typedef enum {JOINTS_MODULE_RUNNING,
JOINTS_MODULE_ABORTED,
JOINTS_MODULE_PREEMPTED} joints_module_status_t;
-//class CJointsModule : public CHumanoidModule<action_module::ActionModuleConfig>
class CJointsModule : public CHumanoidModule<joints_module::JointsModuleConfig>
{
private:
- //CModuleAction<humanoid_common_msgs::humanoid_motionAction> motion_action;
CModuleAction<control_msgs::JointTrajectoryAction> joints_trajectory;
- //action_module::ActionModuleConfig config;
joints_module::JointsModuleConfig config;
joints_module_state_t state;
joints_module_status_t status;
@@ -31,18 +28,13 @@ class CJointsModule : public CHumanoidModule<joints_module::JointsModuleConfig>
/* goal */
control_msgs::JointTrajectoryGoal goal;
bool new_trajectory;
+ void clear_goal(void);
std::vector<std::string> servos_name;
-
- //JOINT STATE
- ros::NodeHandle nh;
- std::vector<double> current_angle;
- //sensor_msgs::JointState joint_state;
ros::Subscriber joint_states_subscriber_;
void joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg);
- pthread_mutex_t joint_states_mutex_;
- void joint_states_mutex_enter(void);
- void joint_states_mutex_exit(void);
+ CMutex joint_state_access;
+ std::vector<double> current_angle;
protected:
void state_machine(void);
@@ -51,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, std::vector<double> &accels/* const std::vector<double> &accels=std::vector<double>()*/);
+ bool 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 17e52f7d883386cb158c30953fa9913d8716ab38..515d94d9789bfb4d3de5f30c4e5d35c26f1d01e9 100644
--- a/humanoid_modules/src/joints_module.cpp
+++ b/humanoid_modules/src/joints_module.cpp
@@ -1,39 +1,33 @@
#include <humanoid_modules/joints_module.h>
+#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),
- nh(ros::this_node::getName()+"/"+name)
+ joints_trajectory("joint_trajectory_action",name)
{
+ this->start_operation();
this->state=JOINTS_MODULE_IDLE;
this->status=JOINTS_MODULE_SUCCESS;
this->cancel_pending=false;
this->new_trajectory=false;
- //this->servo_id=-1;
-
this->goal.trajectory.points.resize(1);
- //this->joint_states_subscriber_ = this->public_node_handle_.subscribe("joint_states", 1, &SmQrSearchAlgNode::joint_states_callback, this);
- this->joint_states_subscriber_ = this->nh.subscribe("joint_states", 1, &CJointsModule::joint_states_callback, this);
- pthread_mutex_init(&this->joint_states_mutex_,NULL);
-
+ this->joint_states_subscriber_ = this->module_nh.subscribe("joint_states", 1, &CJointsModule::joint_states_callback, this);
+
}
void CJointsModule::state_machine(void)
{
- //humanoid_common_msgs::humanoid_motionGoal goal;
- //control_msgs::JointTrajectoryGoal goal;
-
switch(this->state)
{
case JOINTS_MODULE_IDLE: ROS_INFO("CJointsModule : state IDLE");
if(this->new_trajectory)
{
- ROS_INFO("New trajectory!");
+ ROS_INFO("New trajectory!");
this->new_trajectory=false;
this->state=JOINTS_MODULE_SET_MODULES;
this->status=JOINTS_MODULE_RUNNING;
@@ -57,25 +51,12 @@ void CJointsModule::state_machine(void)
}
break;
case JOINTS_MODULE_START: ROS_INFO("CJointsModule : state START");
- //goal.motion_id=this->action_id;
- /* for(i=0;i<goal.trajectory.joint_names.size();i++)
- {
- if(servo_names[this->servo_id]==goal.trajectory.joint_names[i])// update the current servo information
- {
- goal.trajectory.points[0].positions[i]=this->angle;
- goal.trajectory.points[0].velocities[i]=this->speed;
- goal.trajectory.points[0].accelerations[i]=this->acceleration;
- }
- } */
switch(this->joints_trajectory.make_request(goal))
{
case ACT_SRV_SUCCESS: this->state=JOINTS_MODULE_WAIT;
ROS_DEBUG("CJointsModule : goal start successfull");
- /*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 goal*/
+ this->clear_goal();
/* start timeout */
if(this->config.enable_timeout)
this->joints_trajectory.start_timeout(this->config.timeout_s);
@@ -153,29 +134,37 @@ 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, std::vector<double> &accels)
+bool 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;
this->lock();
- if(this->state!=JOINTS_MODULE_IDLE)// action is in progres
- this->cancel_pending=true;
-
- //Start joints trajectory
- this->new_trajectory=true;
-
-
- //Check if input vector have the same size
- if((servos_id.size()!=angles.size()) || (servos_id.size()!=speeds.size()) ||(servos_id.size()!=accels.size()))
- {
- ROS_ERROR("Diferent size of input vectors");
- this->new_trajectory=false;
- return false;
- }
- else
+ if(this->state==JOINTS_MODULE_IDLE)// trajectory is in progres
{
- servos_name.resize(servos_id.size());
+ /*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)
+ {
+ //Check if input vector have the same size
+ if((servos_id.size()!=angles.size()) || (servos_id.size()!=speeds.size()) || (servos_id.size()!=accels.size()))
+ throw CModuleException(_HERE_,"Diferent size of input vectors", this->get_name());
+ }
+ else
+ {
+ //goal.trajectory.points[0].accelerations.resize(servo_id.size());
+ //goal.trajectory.points[0].accelerations.push_back(accels[i]);
+ if((servos_id.size()!=angles.size()) || (servos_id.size()!=speeds.size()))
+ throw CModuleException(_HERE_,"Diferent size of input vectors", this->get_name());
+ }
+
+ this->servos_name.resize(servos_id.size());
//Check if some elements are repeated
for(i=0; i<servos_id.size(); i++)
{
@@ -190,100 +179,41 @@ bool CJointsModule::execute(std::vector<unsigned int> &servos_id, std::vector<do
//Save servo if the element is different from the rest
if(servo_ok)
{
- servos_name[i]=servo_names[servos_id[i]]; //Save the servo as string
-
- this->add_servo(servos_name[i]); //Assign servo to joints module
- //set servo to goal
- goal.trajectory.joint_names.push_back(servos_name[i]);
+ this->servos_name[i]=servo_names[servos_id[i]]; //Save the servo as string
+ this->add_servo(this->servos_name[i]); //Assign servo to joints module
+ //set servo to goal
+ goal.trajectory.joint_names.push_back(this->servos_name[i]);
goal.trajectory.points[0].positions.push_back(angles[i]);
goal.trajectory.points[0].velocities.push_back(speeds[i]);
- if(accels.size()>0)
+ if(accels.size()>0)
goal.trajectory.points[0].accelerations.push_back(accels[i]);
+ else
+ goal.trajectory.points[0].accelerations.push_back(SERVO_ACCEL); //Set default acceleration
}
else
servo_ok=true;
}
- return true;
+ //Start joints trajectory
+ this->new_trajectory=true;
}
-
-
this->unlock();
-}
-/*
-
- //Pass servos id to string
- for(i=0; i<servos_id.size(); i++)
- servos_name[i]=servo_names[servos_id[i]];
-
- //Check if input vector have the same size
- if(servos_name.size()!=angles.size()!=speeds.size()!=accels.size())
- {
- ROS_WARN("Diferent size of input vectors");
- this->new_trajectory=false;
- }
- else
- {
- //Add selected servos to module "joints"
- this->add_servos(servos_name);
- //Set joint trajectory goal
- goal.trajectory.joint_names=servos_name;
- goal.trajectory.points[0].positions=angles;
- goal.trajectory.points[0].velocities=speeds;
- goal.trajectory.points[0].accelerations=accels;
- }
- //Check if some elements are repeated
- for(i=0; i<servos_id.size(); i++)
- {
- for(j=i+1; j<servos_id.size(); j++)
- {
- if(servos_id[i]!=servos_id[j])
- {
- servos_name[i]=servo_names[servos_id[i]];
- // servos_name.erase(servos_name[j]);
- // angles.erase(angles[j]);
- // speeds.erase(speeds[j]);
- // accels.erase(accels[j]);
- ROS_WARN("Element %d of servos_id vector is repeated", j);
- this->new_trajectory=false;
- }
- }
- }
- */
-/*
-void CJointsModule::execute_goal(unsigned int servos, double angles,double speeds, double accel)
-{
- this->lock();
- if(this->state!=JOINTS_MODULE_IDLE)// action is in progres
- this->cancel_pending=true;
- this->servo_id=servo_id;
-// this->angle=angle;
-// this->speed=speed;
-// this->acceleration=acceleration;
- goal.trajectory.joint_names=servos;
- goal.trajectory.points[0].positions=angles;
- goal.trajectory.points[0].velocities=speeds;
- goal.trajectory.points[0].accelerations=accels;
-
- this->new_trajectory=true;
- this->unlock();
+ return true;
}
-*/
+
void CJointsModule::execute_goal(void)
{
this->lock();
- if(this->state!=JOINTS_MODULE_IDLE)// action is in progres
- this->cancel_pending=true;
- //Assign servos to joints module
- // this->add_servo(goal.trajectory.joint_names);
- this->new_trajectory=true;
+ if(this->state==JOINTS_MODULE_IDLE)// action isn't in progress
+ this->new_trajectory=true;
this->unlock();
}
void CJointsModule::add_goal(unsigned int servo_id, double angle, double speed, double acceleration)
{
-
unsigned int i;
+ this->lock();
+
for(i=0;i<goal.trajectory.joint_names.size();i++)
{
if(servo_names[servo_id]==goal.trajectory.joint_names[i])// update the current servo information
@@ -294,13 +224,18 @@ void CJointsModule::add_goal(unsigned int servo_id, double angle, double speed,
break;
}
}
- if(i==goal.trajectory.joint_names.size())// a new servo
+ if(i==goal.trajectory.joint_names.size()) // set new servo information
{
- goal.trajectory.joint_names.push_back(servo_names[servo_id]);
+ ROS_INFO("Setting servo info");
+ this->servos_name.push_back(servo_names[servo_id]); // save servo as string in vector
+ this->add_servo(this->servos_name[i]); // assign servo to "joints" module
+ // set servo goal
+ goal.trajectory.joint_names.push_back(this->servos_name[i]);
goal.trajectory.points[0].positions.push_back(angle);
goal.trajectory.points[0].velocities.push_back(speed);
goal.trajectory.points[0].accelerations.push_back(acceleration);
}
+ this->unlock();
}
void CJointsModule::stop(void)
@@ -316,7 +251,7 @@ joints_module_status_t CJointsModule::get_status(void)
bool CJointsModule::is_finished(void)
{
- if(this->state==JOINTS_MODULE_IDLE)
+ if(this->state==JOINTS_MODULE_IDLE && this->new_trajectory==false)
return true;
else
return false;
@@ -327,18 +262,26 @@ std::vector<double> CJointsModule::get_current_angle(void)
return this->current_angle;
}
+void CJointsModule::clear_goal(void)
+{
+ /* 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 */
+ this->servos_name.clear();
+}
+
void CJointsModule::joint_states_callback(const sensor_msgs::JointState::ConstPtr& msg)
{
- this->joint_states_mutex_enter();
+ this->joint_state_access.enter();
unsigned int i,j;
- //sensor_msgs::JointState::ConstPtr& joint_state;
- //this->joint_state=*msg;
-
- for (i=0; i<servos_name.size(); ++i)
+ for (i=0; i<this->servos_name.size(); ++i)
{
- current_angle.resize(servos_name.size());
+ current_angle.resize(this->servos_name.size());
for(j=0; j<msg->name.size(); j++)
{
if(this->servos_name[i] == msg->name[j])
@@ -346,20 +289,10 @@ void CJointsModule::joint_states_callback(const sensor_msgs::JointState::ConstPt
}
}
- this->joint_states_mutex_exit();
+ this->joint_state_access.exit();
}
-void CJointsModule::joint_states_mutex_enter(void)
-{
- pthread_mutex_lock(&this->joint_states_mutex_);
-}
-
-void CJointsModule::joint_states_mutex_exit(void)
-{
- pthread_mutex_unlock(&this->joint_states_mutex_);
-}
-
CJointsModule::~CJointsModule()
{
if(!this->joints_trajectory.is_finished())
diff --git a/joints_client/cfg/JointsClient.cfg b/joints_client/cfg/JointsClient.cfg
index c2531d155335d686bdef4715d1bba8270667e420..107be544f2e2e6758b330ea9f8ef59b4fbcf5b74 100755
--- a/joints_client/cfg/JointsClient.cfg
+++ b/joints_client/cfg/JointsClient.cfg
@@ -38,7 +38,6 @@ from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
# Name Type Reconfiguration level Description Default Min Max
-#gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
gen.add("servo_id", int_t, 0, "Servo identifier", 0, 0, 32)
gen.add("angle", double_t, 0, "Servo angle (rad)", 0, -3.14159, 3.14159)
gen.add("speed", double_t, 0, "Servo speed (rad/s)", 0, 0, 6.2832)
diff --git a/joints_client/src/joints_client_alg_node.cpp b/joints_client/src/joints_client_alg_node.cpp
index e487599ec4afe244a28b021298e2787f1403df3e..96c74d3c7187a1d64e6f520cd748ba9f14147a29 100644
--- a/joints_client/src/joints_client_alg_node.cpp
+++ b/joints_client/src/joints_client_alg_node.cpp
@@ -53,22 +53,27 @@ void JointsClientAlgNode::node_config_update(Config &config, uint32_t level)
this->alg_.lock();
if(config.start)
{
+ /* trajectory is in progress */
if(!this->joints_trajectory.is_finished())
ROS_WARN("Impossible to execute action because the previous action is not finished yet");
else
{
+ ROS_INFO("Start trajectory");
joints_trajectory.execute_goal();
}
config.start=false;
}
+ /* stop trajectory */
else if(config.stop)
{
if(!this->joints_trajectory.is_finished())
this->joints_trajectory.stop();
config.stop=false;
}
+ /* load servo info */
else if(config.load)
{
+ ROS_INFO("Loading servo info");
joints_trajectory.add_goal(config.servo_id, config.angle, config.speed, config.acceleration);
config.load=false;
}