void PosterReader::softmotionPlotTraj()
{
if( _softmotionPoster == NULL )
{
cout << "softmotion : NULL Poster" << endl;
return;
}
//_softmotionPoster->update();
SM_TRAJ smTraj;
if(_softmotionPoster->getPosterStuct((char *)(&_softmotionPosterStruct)) == false) {
cout << " PosterReader::softmotionPlotTraj() ERROR " << endl;
}
smTraj.importFromSM_TRAJ_STR(&_softmotionPosterStruct);
if( smTraj.getDuration() <= 0) {
// QMessageBox toto(QMessageBox::Information, ,QMessageBox::Close, NULL,Qt::Dialog);
QMessageBox::about (NULL, QString("SoftMotion info"), QString("Trajectory is empty"));
return;
}
cout << "duration " << smTraj.getDuration() << endl;
smTraj.plot();
return;
}
void loadNewTraj(const pr2_soft_controller::SM_TRAJ_STR_ROS& msg)
{
SM_TRAJ_STR tmpTraj;
//copy of the msg in a softmotion structure
tmpTraj.trajId= msg.trajId;
tmpTraj.nbAxis= msg.nbAxis;
//tmpTraj.timePreserved= msg.timePreserved;
for(size_t i=0; i<(unsigned int)msg.nbAxis; ++i){
tmpTraj.qStart[i] = msg.qStart[i];
tmpTraj.qGoal[i] = msg.qGoal[i];
tmpTraj.jmax[i] = msg.jmax[i];
tmpTraj.amax[i] = msg.amax[i];
tmpTraj.vmax[i] = msg.vmax[i];
tmpTraj.traj[i].nbSeg= msg.traj[i].nbSeg;
tmpTraj.traj[i].unsused= msg.traj[i].unsused;
for(size_t j=0; j<(unsigned int)msg.traj[i].nbSeg; ++j){
tmpTraj.traj[i].seg[j].lpId= msg.traj[i].seg[j].lpId;
tmpTraj.traj[i].seg[j].unused= msg.traj[i].seg[j].unused;
tmpTraj.traj[i].seg[j].timeOnTraj= msg.traj[i].seg[j].timeOnTraj;
tmpTraj.traj[i].seg[j].time= msg.traj[i].seg[j].time;
tmpTraj.traj[i].seg[j].ic_a= msg.traj[i].seg[j].ic_a;
tmpTraj.traj[i].seg[j].ic_v= msg.traj[i].seg[j].ic_v;
tmpTraj.traj[i].seg[j].ic_x= msg.traj[i].seg[j].ic_x;
tmpTraj.traj[i].seg[j].jerk= msg.traj[i].seg[j].jerk;
}
}
_newTraj.importFromSM_TRAJ_STR( &tmpTraj );
}