Exemplo n.º 1
0
void Sm_Approx::approximate(double jmax,double amax,double vmax,double sampTime, double ErrMax, 
			    int ExpTime, bool flagExport, std::string fileName, SM_TRAJ &traj)
{
  this->approximate(jmax, amax, vmax, sampTime,  ErrMax,  ExpTime, flagExport, fileName);
  traj.clear();


  return;
}
  int executeCB(const soft_move_base::SoftMoveBaseGoalConstPtr &goal)
  {
    ROS_INFO("Received new trajectory");
    SM_TRAJ tmpTraj;
    ros::Rate _loop_rate(1/_dt);
    _timeFromStart= 0;
    _tmpTimeFromStart= 0;
    _timer = 0;
    loadNewTraj(goal->traj); 
    double duration = 0.0;
    duration =  _newTraj.getDuration();


    limits.clear();
    for(int i=0; i<6; ++i)
    {
      limit.maxJerk = _newTraj.jmax[i] * 1.1;
      limit.maxAcc =  _newTraj.amax[i] * 1.1;
      limit.maxVel =  _newTraj.vmax[i] * 1.1;
      limits.push_back(limit);
    }

    //double t = 0;
    //FILE* file;
    //file  = fopen("baseExperiment.dat", "w");


   bool cont = true;
   do {
     // end if goal canceled
     if( as_.isPreemptRequested())
		break;

     getNextRobotCond(_timeFromStart + _dt);
     //printCond(_nextRobotCond_w);

     getPoseInRobotFrame(_nextRobotCond_w);
     //printCond(_nextRobotCond_r);

     _currRobotCond_r[0].x = 0;
     _currRobotCond_r[0].v = _lastVel[0];
     _currRobotCond_r[0].a = _lastAcc[0];
     _currRobotCond_r[1].x = 0;
     _currRobotCond_r[1].v = _lastVel[1];
     _currRobotCond_r[1].a = _lastAcc[1];
     _currRobotCond_r[5].x = 0;
     _currRobotCond_r[5].v = _lastVel[5];
     _currRobotCond_r[5].a = _lastAcc[5];
     //printCond(_currRobotCond_r);

     ROS_INFO("TMP TIME:%f", _tmpTimeFromStart);

     if( _timeFromStart == 0.0) 
     {
       //correction
       tmpTraj.clear(); 

       boundsCond(_currRobotCond_r);
       boundsCond(_nextRobotCond_r);
       tmpTraj.computeTraj(_currRobotCond_r, _nextRobotCond_r, limits, SM_TRAJ::SM_INDEPENDANT);
  
       _tmpTimeFromStart = _dt;
       //printCond( _newRobotCond_r);
     }
     else if(_timer - 0.5 >= 0.0001 
            || _tmpTimeFromStart > tmpTraj.getDuration())
     {
       _timer = 0;

       //correction
       tmpTraj.clear(); 
       tmpTraj.computeTraj(_currRobotCond_r, _nextRobotCond_r, limits, SM_TRAJ::SM_INDEPENDANT);
  
       _tmpTimeFromStart = _dt;
     }

     tmpTraj.getMotionCond(_tmpTimeFromStart,_newRobotCond_r);
     _lastVel[0]= _newRobotCond_r[0].v;
     _lastVel[1]= _newRobotCond_r[1].v;
     _lastVel[5]= _newRobotCond_r[5].v;
     _lastAcc[0]= _newRobotCond_r[0].a;
     _lastAcc[1]= _newRobotCond_r[1].a;
     _lastAcc[5]= _newRobotCond_r[5].a;

     //printCond(_newRobotCond_r);
    
     publishNewCond(_newRobotCond_r);

     _timeFromStart += _dt;
     _tmpTimeFromStart += _dt;
     _timer += _dt;
     if(_timeFromStart > _newTraj.getDuration())
       _timeFromStart = _newTraj.getDuration();

     _loop_rate.sleep();

     if(_timeFromStart >= _newTraj.getDuration())
     {
       cont = fabs(_newRobotCond_r[0].x) > _tolerance[0]
              || fabs(_newRobotCond_r[1].x) > _tolerance[1]
              || fabs(_newRobotCond_r[5].x) > _tolerance[2];
     }
     
     ros::spinOnce();
     
   } while(cont);

    soft_move_base::SoftMoveBaseResult result;
    result.result = true;
    as_.setSucceeded(result);

    return 0;
  }