예제 #1
0
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 );
  }