From 00e92d3a3a7e707748123990aea7f6a72a50dba7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu>
Date: Thu, 18 Jun 2015 15:27:54 +0000
Subject: [PATCH] Removed the static variables from some functions. Implemented
the thread in a non-static function. Tested the execution of several
CTrajectory modules at the same time.
---
src/examples/trajectory_test.cpp | 16 +-
src/trajectory.cpp | 2 -
src/trajectory.h | 299 ++++++++++++++++++-------------
3 files changed, 182 insertions(+), 135 deletions(-)
diff --git a/src/examples/trajectory_test.cpp b/src/examples/trajectory_test.cpp
index cebc53c..dc30eaf 100644
--- a/src/examples/trajectory_test.cpp
+++ b/src/examples/trajectory_test.cpp
@@ -46,16 +46,20 @@ int main(int argc, char *argv[])
{
CMotorTest motor;
CEventServer *event_server=CEventServer::instance();
- CTrajectory<CMotorTest> traj("trajectory",&motor,&CMotorTest::stop,&CMotorTest::get_current_angles,&CMotorTest::move_absolute_angles,
- &CMotorTest::move_relative_angles,&CMotorTest::get_num_motors);
+ CTrajectory<CMotorTest> traj1("trajectory1",&motor,&CMotorTest::stop,&CMotorTest::get_current_angles,&CMotorTest::move_absolute_angles,
+ &CMotorTest::move_relative_angles,&CMotorTest::get_num_motors);
+ CTrajectory<CMotorTest> traj2("trajectory2",&motor,&CMotorTest::stop,&CMotorTest::get_current_angles,&CMotorTest::move_absolute_angles,
+ &CMotorTest::move_relative_angles,&CMotorTest::get_num_motors);
try{
#ifdef _HAVE_XSD
- traj.load_trajectory(sequence_filename);
- traj.start_trajectory();
- while(!event_server->event_is_set(traj.get_trajectory_complete_event_id()))
+ traj1.load_trajectory(sequence_filename);
+ traj1.start_trajectory();
+ traj2.load_trajectory(sequence_filename);
+ traj2.start_trajectory();
+ while(!event_server->event_is_set(traj1.get_trajectory_complete_event_id()))
{
- std::cout << traj.get_trajectory_completed_percentage() << "% of sequence completed" << std::endl;
+ std::cout << traj1.get_trajectory_completed_percentage() << "% of sequence completed" << std::endl;
usleep(200000);
}
#endif
diff --git a/src/trajectory.cpp b/src/trajectory.cpp
index 47f45a2..a0293de 100644
--- a/src/trajectory.cpp
+++ b/src/trajectory.cpp
@@ -1,3 +1 @@
#include "trajectory.h"
-
-
diff --git a/src/trajectory.h b/src/trajectory.h
index 063df34..f8ac075 100644
--- a/src/trajectory.h
+++ b/src/trajectory.h
@@ -32,6 +32,11 @@ template <class T>
class CTrajectory
{
private:
+ /**
+ * \brief
+ *
+ */
+ std::string traj_id;
/**
* \brief
*
@@ -135,6 +140,27 @@ class CTrajectory
*
*/
get_num_motors_fnct_ptr get_num_motors_fnct;
+ /**
+ * \brief
+ *
+ */
+ std::vector<int> time;
+ /**
+ * \brief
+ *
+ */
+ std::vector<double> current_position;
+ /**
+ * \brief
+ *
+ */
+ std::vector<double> last_speeds;
+ /**
+ * \brief
+ *
+ */
+ std::vector<int> dir;
+ traj_t *int_traj;
protected:
/**
* \brief
@@ -163,96 +189,102 @@ class CTrajectory
static void *trajectory_thread(void *param)
{
CTrajectory *mtn_seq=(CTrajectory *)param;
+
+ mtn_seq->do_trajectory();
+
+ pthread_exit(NULL);
+ };
+
+ void do_trajectory(void)
+ {
std::list<std::string> events;
int event_id=0,to=-1;
bool end=false;
// initialize the motion events
- events.push_back(mtn_seq->finish_thread_event_id);
- events.push_back(mtn_seq->start_trajectory_event_id);
- events.push_back(mtn_seq->stop_trajectory_event_id);
- events.push_back(mtn_seq->resume_trajectory_event_id);
+ events.push_back(this->finish_thread_event_id);
+ events.push_back(this->start_trajectory_event_id);
+ events.push_back(this->stop_trajectory_event_id);
+ events.push_back(this->resume_trajectory_event_id);
while(!end)
{
- try{
- event_id=mtn_seq->event_server->wait_first(events,to);
- mtn_seq->motion_access.enter();
- switch(event_id)
- {
- case -1: mtn_seq->trajectory_current_step++;
- to=mtn_seq->load_motion(mtn_seq->trajectory_current_step);
- break;
- case 0: /* finish the thread */
- end=true;
- break;
- case 1: /* start trajectory */
- to=mtn_seq->load_motion(mtn_seq->trajectory_current_step);
- break;
- case 2: /* stop trajectory */
- if(mtn_seq->stop_fnct!=NULL)
- (mtn_seq->motor_control->*mtn_seq->stop_fnct)();
- else
- throw CTrajectoryException(_HERE_,"Stop function not defined");
- mtn_seq->trajectory_error_msg="Motion trajectory stopped by user";
- mtn_seq->trajectory_current_step=0;
- to=-1;
- if(!mtn_seq->event_server->event_is_set(mtn_seq->trajectory_complete_event_id))
- mtn_seq->event_server->set_event(mtn_seq->trajectory_complete_event_id);
- break;
- case 3: /* resume trajectory */
- mtn_seq->trajectory_current_step++;
- to=mtn_seq->load_motion(mtn_seq->trajectory_current_step);
- break;
- }
- mtn_seq->motion_access.exit();
- }catch(CEventTimeoutException &e){
- if(!mtn_seq->event_server->event_is_set(mtn_seq->pause_trajectory_event_id))
- {
- /* load and execute the next step */
- mtn_seq->motion_access.enter();
- mtn_seq->trajectory_current_step++;
- try{
- to=mtn_seq->load_motion(mtn_seq->trajectory_current_step);
- }catch(CException &e){
+ try{
+ event_id=this->event_server->wait_first(events,to);
+ this->motion_access.enter();
+ switch(event_id)
+ {
+ case -1: this->trajectory_current_step++;
+ to=this->load_motion(this->trajectory_current_step);
+ break;
+ case 0: /* finish the thread */
+ end=true;
+ break;
+ case 1: /* start trajectory */
+ to=this->load_motion(this->trajectory_current_step);
+ break;
+ case 2: /* stop trajectory */
+ if(this->stop_fnct!=NULL)
+ (this->motor_control->*this->stop_fnct)();
+ else
+ throw CTrajectoryException(_HERE_,"Stop function not defined");
+ this->trajectory_error_msg="Motion trajectory stopped by user";
+ this->trajectory_current_step=0;
+ to=-1;
+ if(!this->event_server->event_is_set(this->trajectory_complete_event_id))
+ this->event_server->set_event(this->trajectory_complete_event_id);
+ break;
+ case 3: /* resume trajectory */
+ this->trajectory_current_step++;
+ to=this->load_motion(this->trajectory_current_step);
+ break;
+ }
+ this->motion_access.exit();
+ }catch(CEventTimeoutException &e){
+ if(!this->event_server->event_is_set(this->pause_trajectory_event_id))
+ {
+ /* load and execute the next step */
+ this->motion_access.enter();
+ this->trajectory_current_step++;
+ try{
+ to=this->load_motion(this->trajectory_current_step);
+ }catch(CException &e){
std::cout << e.what() << std::endl;
- to=-1;
- if(mtn_seq->stop_fnct!=NULL)
- (mtn_seq->motor_control->*mtn_seq->stop_fnct)();
- else
- throw CTrajectoryException(_HERE_,"Stop function not defined");
- mtn_seq->trajectory_state=mtn_loaded;
- mtn_seq->trajectory_error_msg=e.what();
- // signal the user
- if(!mtn_seq->event_server->event_is_set(mtn_seq->trajectory_error_event_id))
- mtn_seq->event_server->set_event(mtn_seq->trajectory_error_event_id);
- if(!mtn_seq->event_server->event_is_set(mtn_seq->trajectory_complete_event_id))
- mtn_seq->event_server->set_event(mtn_seq->trajectory_complete_event_id);
- }
- mtn_seq->motion_access.exit();
- }
- else
- {
- mtn_seq->event_server->reset_event(mtn_seq->pause_trajectory_event_id);
- to=-1;
- }
- }catch(CException &e){
+ to=-1;
+ if(this->stop_fnct!=NULL)
+ (this->motor_control->*this->stop_fnct)();
+ else
+ throw CTrajectoryException(_HERE_,"Stop function not defined");
+ this->trajectory_state=mtn_loaded;
+ this->trajectory_error_msg=e.what();
+ // signal the user
+ if(!this->event_server->event_is_set(this->trajectory_error_event_id))
+ this->event_server->set_event(this->trajectory_error_event_id);
+ if(!this->event_server->event_is_set(this->trajectory_complete_event_id))
+ this->event_server->set_event(this->trajectory_complete_event_id);
+ }
+ this->motion_access.exit();
+ }
+ else
+ {
+ this->event_server->reset_event(this->pause_trajectory_event_id);
+ to=-1;
+ }
+ }catch(CException &e){
std::cout << e.what() << std::endl;
- to=-1;
- mtn_seq->motion_access.exit();
- if(mtn_seq->stop_fnct!=NULL)
- (mtn_seq->motor_control->*mtn_seq->stop_fnct)();
- mtn_seq->trajectory_state=mtn_loaded;
- mtn_seq->trajectory_error_msg=e.what();
- // signal the user
- if(!mtn_seq->event_server->event_is_set(mtn_seq->trajectory_error_event_id))
- mtn_seq->event_server->set_event(mtn_seq->trajectory_error_event_id);
- if(!mtn_seq->event_server->event_is_set(mtn_seq->trajectory_complete_event_id))
- mtn_seq->event_server->set_event(mtn_seq->trajectory_complete_event_id);
- }
+ to=-1;
+ this->motion_access.exit();
+ if(this->stop_fnct!=NULL)
+ (this->motor_control->*this->stop_fnct)();
+ this->trajectory_state=mtn_loaded;
+ this->trajectory_error_msg=e.what();
+ // signal the user
+ if(!this->event_server->event_is_set(this->trajectory_error_event_id))
+ this->event_server->set_event(this->trajectory_error_event_id);
+ if(!this->event_server->event_is_set(this->trajectory_complete_event_id))
+ this->event_server->set_event(this->trajectory_complete_event_id);
+ }
}
-
- pthread_exit(NULL);
};
/**
* \brief
@@ -299,11 +331,6 @@ class CTrajectory
*/
int load_motion(unsigned int step_id)
{
- static std::vector<int> time((this->motor_control->*this->get_num_motors_fnct)(),0);
- static std::vector<double> current_position((this->motor_control->*this->get_num_motors_fnct)(),0.0);
- static std::vector<double> last_speeds((this->motor_control->*this->get_num_motors_fnct)(),0.0);
- static std::vector<int> dir((this->motor_control->*this->get_num_motors_fnct)(),1);
-
std::vector<double> position((this->motor_control->*this->get_num_motors_fnct)());
std::vector<double> velocity((this->motor_control->*this->get_num_motors_fnct)());
std::vector<double> acceleration((this->motor_control->*this->get_num_motors_fnct)());
@@ -328,7 +355,7 @@ class CTrajectory
else
{
if(this->get_current_angles_fnct!=NULL)
- (this->motor_control->*this->get_current_angles_fnct)(current_position);
+ (this->motor_control->*this->get_current_angles_fnct)(this->current_position);
else
throw CTrajectoryException(_HERE_,"Get_current_angles function not defined");
if(step_id==0)// first segment
@@ -336,43 +363,43 @@ class CTrajectory
for(i=0;i<(this->motor_control->*this->get_num_motors_fnct)();i++)
{
if(this->seq[step_id].absolute_motion)
- delta_angle=this->seq[step_id].position[i]-current_position[i];
+ delta_angle=this->seq[step_id].position[i]-this->current_position[i];
else
delta_angle=this->seq[step_id].position[i];
- if(fabs(delta_angle)>0.0)
+ if(fabs(delta_angle)>(pow(this->seq[step_id].velocity[i],2)/this->seq[step_id].acceleration[i]))
{
if(this->seq[step_id].position[i]>=0)
- dir[i]=1;
+ this->dir[i]=1;
else
- dir[i]=-1;
+ this->dir[i]=-1;
t_acc=this->seq[step_id].velocity[i]/this->seq[step_id].acceleration[i];
t_seg=2.0*t_acc+(fabs(delta_angle)-pow(this->seq[step_id].velocity[i],2)/(this->seq[step_id].acceleration[i]))/this->seq[step_id].velocity[i];
if(t_seg<2.0*t_acc)
throw CTrajectoryException(_HERE_,"Invalid motion commands");
- if(this->zero_speed(step_id,i,&dir[i]))// it is the last command or there is a delay
+ if(this->zero_speed(step_id,i,&this->dir[i]))// it is the last command or there is a delay
{
position[i]=this->seq[step_id].position[i];
velocity[i]=this->seq[step_id].velocity[i];
acceleration[i]=this->seq[step_id].acceleration[i];
- time[i]=t_seg*1000+this->seq[step_id].delay;
- last_speeds[i]=0.0;
+ this->time[i]=t_seg*1000+this->seq[step_id].delay;
+ this->last_speeds[i]=0.0;
}
else
{
velocity[i]=delta_angle/(t_seg-t_acc/2.0);
acceleration[i]=velocity[i]/t_acc;
- position[i]=this->seq[step_id].position[i]+dir[i]*velocity[i]*t_acc/2.0;
- time[i]=t_seg*1000;
- last_speeds[i]=velocity[i];
+ position[i]=this->seq[step_id].position[i]+this->dir[i]*velocity[i]*t_acc/2.0;
+ this->time[i]=t_seg*1000;
+ this->last_speeds[i]=velocity[i];
}
}
else
{
position[i]=this->seq[step_id].position[i];
- velocity[i]=0.0;
+ velocity[i]=this->seq[step_id].velocity[i];
acceleration[i]=this->seq[step_id].acceleration[i];
- time[i]=0.0;
- last_speeds[i]=0.0;
+ this->time[i]=0.0;
+ this->last_speeds[i]=0.0;
}
}
}
@@ -381,66 +408,66 @@ class CTrajectory
for(i=0;i<(this->motor_control->*this->get_num_motors_fnct)();i++)
{
if(this->seq[step_id].absolute_motion)
- delta_angle=this->seq[step_id].position[i]-current_position[i];//this->seq[step_id-1].position[i];
+ delta_angle=this->seq[step_id].position[i]-this->current_position[i];//this->seq[step_id-1].position[i];
else
delta_angle=this->seq[step_id].position[i];
- if(fabs(delta_angle)>0.0)
+ if(fabs(delta_angle)>(pow(this->seq[step_id].velocity[i],2)/this->seq[step_id].acceleration[i]))
{
t_acc=this->seq[step_id].velocity[i]/this->seq[step_id].acceleration[i];
t_seg=2.0*t_acc+(fabs(delta_angle)-pow(this->seq[step_id].velocity[i],2)/(this->seq[step_id].acceleration[i]))/this->seq[step_id].velocity[i];
if(t_seg<2.0*t_acc)
throw CTrajectoryException(_HERE_,"Invalid motion commands");
- if(last_speeds[i]==0.0)
+ if(this->last_speeds[i]==0.0)
{
- if(this->zero_speed(step_id,i,&dir[i]))// it is the last command or there is a change in direction or there is a delay
+ if(this->zero_speed(step_id,i,&this->dir[i]))// it is the last command or there is a change in direction or there is a delay
{
position[i]=this->seq[step_id].position[i];
velocity[i]=this->seq[step_id].velocity[i];
acceleration[i]=this->seq[step_id].acceleration[i];
- time[i]=t_seg*1000+this->seq[step_id].delay;
- last_speeds[i]=0.0;
+ this->time[i]=t_seg*1000+this->seq[step_id].delay;
+ this->last_speeds[i]=0.0;
}
else
{
velocity[i]=(fabs(delta_angle))/(t_seg-(t_acc/2.0));
acceleration[i]=velocity[i]/t_acc;
- position[i]=this->seq[step_id].position[i]+dir[i]*velocity[i]*t_acc/2.0;
- time[i]=t_seg*1000;
- last_speeds[i]=velocity[i];
+ position[i]=this->seq[step_id].position[i]+this->dir[i]*velocity[i]*t_acc/2.0;
+ this->time[i]=t_seg*1000;
+ this->last_speeds[i]=velocity[i];
}
}
else
{
- if(this->zero_speed(step_id,i,&dir[i]))// it is the last command or there is a change in direction or there is a delay
+ if(this->zero_speed(step_id,i,&this->dir[i]))// it is the last command or there is a change in direction or there is a delay
{
a=1.0;
b=2.0*this->seq[step_id].acceleration[i]*(t_seg-3.0*t_acc/2.0);
- c=(last_speeds[i]*t_acc/2.0-fabs(delta_angle))*2.0*this->seq[step_id].acceleration[i];
+ c=(this->last_speeds[i]*t_acc/2.0-fabs(delta_angle))*2.0*this->seq[step_id].acceleration[i];
if((b*b-4*a*c)<0)
throw CTrajectoryException(_HERE_,"Invalid motion commands");
velocity[i]=(-b+sqrt(b*b-4.0*a*c))/2.0;
- acceleration[i]=fabs(velocity[i]-last_speeds[i])/t_acc;
+ acceleration[i]=fabs(velocity[i]-this->last_speeds[i])/t_acc;
position[i]=this->seq[step_id].position[i];
- time[i]=t_seg*1000+this->seq[step_id].delay;
- last_speeds[i]=0.0;
+ this->time[i]=t_seg*1000+this->seq[step_id].delay;
+ this->last_speeds[i]=0.0;
}
else
{
- velocity[i]=(fabs(delta_angle)-last_speeds[i]*t_acc/2.0)/(t_seg-t_acc/2.0);
- acceleration[i]=fabs(velocity[i]-last_speeds[i])/t_acc;
- position[i]=this->seq[step_id].position[i]+dir[i]*velocity[i]*t_acc/2.0;
- time[i]=t_seg*1000;
- last_speeds[i]=velocity[i];
+ velocity[i]=(fabs(delta_angle)-this->last_speeds[i]*t_acc/2.0)/(t_seg-t_acc/2.0);
+ acceleration[i]=fabs(velocity[i]-this->last_speeds[i])/t_acc;
+ position[i]=this->seq[step_id].position[i]+this->dir[i]*velocity[i]*t_acc/2.0;
+ this->time[i]=t_seg*1000;
+ this->last_speeds[i]=velocity[i];
}
}
}
else
{
position[i]=this->seq[step_id].position[i];
- velocity[i]=0.0;
+ velocity[i]=this->seq[step_id].velocity[i];
acceleration[i]=this->seq[step_id].acceleration[i];
- time[i]=0.0;
- last_speeds[i]=0.0;
+ this->time[i]=0.0;
+ this->last_speeds[i]=0.0;
}
}
}
@@ -462,10 +489,10 @@ class CTrajectory
else
throw CTrajectoryException(_HERE_,"move_relative_angles function not defined");
}
- max=time[0];
+ max=this->time[0];
for(i=1;i<(this->motor_control->*this->get_num_motors_fnct)();i++)
- if(time[i]>max)
- max=time[i];
+ if(this->time[i]>max)
+ max=this->time[i];
return max;
}
};
@@ -505,7 +532,6 @@ class CTrajectory
this->trajectory_thread_id=traj_id+"_trajectory_thread";
this->thread_server->create_thread(this->trajectory_thread_id);
this->thread_server->attach_thread(this->trajectory_thread_id,this->trajectory_thread,this);
- this->thread_server->start_thread(this->trajectory_thread_id);
// initialize motion controller parameters
this->motor_control=motor_control;
this->set_stop_function(stop_fnct);
@@ -513,6 +539,12 @@ class CTrajectory
this->set_move_absolute_angles_function(move_absolute_angles_fnct);
this->set_move_relative_angles_function(move_relative_angles_fnct);
this->set_get_num_motors_function(get_num_motors_fnct);
+ this->time=std::vector<int>((this->motor_control->*this->get_num_motors_fnct)(),0);
+ this->current_position=std::vector<double>((this->motor_control->*this->get_num_motors_fnct)(),0.0);
+ this->last_speeds=std::vector<double>((this->motor_control->*this->get_num_motors_fnct)(),0.0);
+ this->dir=std::vector<int>((this->motor_control->*this->get_num_motors_fnct)(),1);
+ this->thread_server->start_thread(this->trajectory_thread_id);
+ this->traj_id=traj_id;
};
CTrajectory(const std::string &traj_id,T *motor_control,stop_fnct_ptr stop_fnct=NULL,get_current_angles_fnct_ptr get_current_angles_fnct=NULL,
move_absolute_angles_no_accel_fnct_ptr move_absolute_angles_fnct=NULL,move_relative_angles_no_accel_fnct_ptr move_relative_angles_fnct=NULL,
@@ -541,7 +573,6 @@ class CTrajectory
this->trajectory_thread_id=traj_id+"_trajectory_thread";
this->thread_server->create_thread(this->trajectory_thread_id);
this->thread_server->attach_thread(this->trajectory_thread_id,this->trajectory_thread,this);
- this->thread_server->start_thread(this->trajectory_thread_id);
// initialize motion controller parameters
this->motor_control=motor_control;
this->set_stop_function(stop_fnct);
@@ -549,6 +580,12 @@ class CTrajectory
this->set_move_absolute_angles_function(move_absolute_angles_fnct);
this->set_move_relative_angles_function(move_relative_angles_fnct);
this->set_get_num_motors_function(get_num_motors_fnct);
+ this->time=std::vector<int>((this->motor_control->*this->get_num_motors_fnct)(),0);
+ this->current_position=std::vector<double>((this->motor_control->*this->get_num_motors_fnct)(),0.0);
+ this->last_speeds=std::vector<double>((this->motor_control->*this->get_num_motors_fnct)(),0.0);
+ this->dir=std::vector<int>((this->motor_control->*this->get_num_motors_fnct)(),1);
+ this->thread_server->start_thread(this->trajectory_thread_id);
+ this->traj_id=traj_id;
};
/**
@@ -569,6 +606,7 @@ class CTrajectory
traj_t::step_iterator iterator;
TTrajectoryStep step;
struct stat buffer;
+ unsigned int i=0;
if(stat(filename.c_str(),&buffer)==0)
{
@@ -584,6 +622,7 @@ class CTrajectory
this->trajectory_state=mtn_loaded;
}
// load the new trajectory
+ std::cout << "read file " << filename << std::endl;
std::auto_ptr<traj_t> traj(trajectory(filename.c_str(),xml_schema::flags::dont_validate));
if(traj->num_motors()!=(this->motor_control->*this->get_num_motors_fnct)())
{
@@ -595,7 +634,7 @@ class CTrajectory
{
// erase the previous trajectory
this->seq.clear();
- for(iterator=traj->step().begin();iterator!=traj->step().end();iterator++)
+ for(iterator=traj->step().begin(),i=0;iterator!=traj->step().end();iterator++,i++)
{
if(iterator->mode()=="absolute")
step.absolute_motion=true;
@@ -615,6 +654,7 @@ class CTrajectory
this->trajectory_state=mtn_loaded;
this->trajectory_current_step=0;
}
+ std::cout << "done reading file " << filename << std::endl;
this->motion_access.exit();
}catch (const xml_schema::exception& e){
this->trajectory_state=mtn_empty;
@@ -622,11 +662,16 @@ class CTrajectory
/* handle exceptions */
std::ostringstream os;
os << e;
+ std::cout << os.str() << std::endl;
throw CTrajectoryException(_HERE_,os.str());
- }
+ }catch(...){
+ std::cout << "Unknown exception" << std::endl;
+ }
}
else
+ {
CTrajectoryException(_HERE_,"The motion trajectory file does not exist");
+ }
};
/**
* \brief
--
GitLab