C++ EnvironmentBasePtr::GetMutex方法代码示例

void GetTaskTime(EnvironmentBasePtr _penv, std::vector<dReal> &vsolutionA, std::vector<dReal> &vsolutionB, TrajectoryBasePtr ptraj){

	//thread::id get_id();
	std::cout << "this thread ::" << boost::this_thread::get_id() << std::endl;
	PlannerBasePtr planner = RaveCreatePlanner(_penv,"birrt");
	//std::vector<dReal> vinitialconfig,vgoalconfig;
	ptraj = RaveCreateTrajectory(_penv,"");
	PlannerBase::PlannerParametersPtr params(new PlannerBase::PlannerParameters());
	RobotBasePtr probot = _penv->GetRobot(robotname);
	params->_nMaxIterations = 4000; // max iterations before failure

	GraphHandlePtr pgraph;
	{
		EnvironmentMutex::scoped_lock lock(_penv->GetMutex()); // lock environment
		params->SetRobotActiveJoints(probot); // set planning configuration space to current active dofs

		//initial config
		probot->SetActiveDOFValues(vsolutionA);
		params->vinitialconfig.resize(probot->GetActiveDOF());
		probot->GetActiveDOFValues(params->vinitialconfig);

		//goal config	
		probot->SetActiveDOFValues(vsolutionB);
		params->vgoalconfig.resize(probot->GetActiveDOF());
		probot->GetActiveDOFValues(params->vgoalconfig);

		//setting limits
		vector<dReal> vlower,vupper;
		probot->GetActiveDOFLimits(vlower,vupper);

		//RAVELOG_INFO("starting to plan\n");
		
		if( !planner->InitPlan(probot,params) ) {
			//return ptraj;
		}
		// create a new output trajectory
		
		if( !planner->PlanPath(ptraj) ) {
			RAVELOG_WARN("plan failed \n");
			//return NULL;
			
		}

		
	}
	
	
	////RAVELOG_INFO(ss.str());
	if (!! ptraj){
		//std::cout << ptraj->GetDuration() << std::endl;	
		writeTrajectory(ptraj);
	}
	std::cout << "this thread ::" << boost::this_thread::get_id() << " has completed its work" << std::endl;
	mtx__.lock();
	completed_thread = boost::this_thread::get_id();
	mtx__.unlock();
	//return  ptraj;


}

int main(int argc, char ** argv)
{
    if( argc < 3 ) {
        RAVELOG_INFO("ikloader robot iktype\n");
        return 1;
    }

    string robotname = argv[1];
    string iktype = argv[2];
    RaveInitialize(true); // start openrave core

    EnvironmentBasePtr penv = RaveCreateEnvironment(); // create the main environment

    {
        // lock the environment to prevent changes
        EnvironmentMutex::scoped_lock lock(penv->GetMutex());
        // load the scene
        RobotBasePtr probot = penv->ReadRobotXMLFile(robotname);
        if( !probot ) {
            penv->Destroy();
            return 2;
        }
        penv->Add(probot);

        ModuleBasePtr pikfast = RaveCreateModule(penv,"ikfast");
        penv->Add(pikfast,true,"");
        stringstream ssin,ssout;
        ssin << "LoadIKFastSolver " << probot->GetName() << " " << iktype;
        // if necessary, add free inc for degrees of freedom
        //ssin << " " << 0.04f;
        // get the active manipulator
        RobotBase::ManipulatorPtr pmanip = probot->GetActiveManipulator();
        if( !pikfast->SendCommand(ssout,ssin) ) {
            RAVELOG_ERROR("failed to load iksolver\n");
            penv->Destroy();
            return 1;
        }

        RAVELOG_INFO("testing random ik\n");
        while(1) {
            Transform trans;
            trans.rot = quatFromAxisAngle(Vector(RaveRandomFloat()-0.5,RaveRandomFloat()-0.5,RaveRandomFloat()-0.5));
            trans.trans = Vector(RaveRandomFloat()-0.5,RaveRandomFloat()-0.5,RaveRandomFloat()-0.5)*2;

            vector<dReal> vsolution;
            if( pmanip->FindIKSolution(IkParameterization(trans),vsolution,IKFO_CheckEnvCollisions) ) {
                stringstream ss; ss << "solution is: ";
                for(size_t i = 0; i < vsolution.size(); ++i) {
                    ss << vsolution[i] << " ";
                }
                ss << endl;
                RAVELOG_INFO(ss.str());
            }
            else {
                // could fail due to collisions, etc
            }
        }
    }

    RaveDestroy(); // destroy
    return 0;
}

//.........这里部分代码省略.........
    return 2;
  }

  // Wait until the robot model appears
  usleep(1000000);
	
  /* timing */
  struct timespec t;
	
  /* hubo ACH Channel */
  struct hubo_ref H;
  memset( &H,   0, sizeof(H));
  int r;
	
  if(fflag){
    r = ach_open(&chan_num, HUBO_CHAN_REF_FILTER_NAME, NULL);
  }
  else{
    r = ach_open(&chan_num, HUBO_CHAN_REF_NAME, NULL);
  }

  size_t fs;


  /* read first set of data */
  r = ach_get( &chan_num, &H, sizeof(H), &fs, NULL, ACH_O_LAST );
  assert( sizeof(H) == fs );

  int contactpoints = 0;
  bool runflag = true;
  while(runflag) {
  {
    // lock the environment to prevent data from changing
    EnvironmentMutex::scoped_lock lock(penv->GetMutex());
    
    //vector<KinBodyPtr> vbodies;
    vector<RobotBasePtr> vbodies;
    //penv->GetBodies(vbodies);
    penv->GetRobots(vbodies);
    // get the first body
    if( vbodies.size() == 0 ) {
      RAVELOG_ERROR("no bodies loaded\n");
      return -3;
    }
    
    //KinBodyPtr pbody = vbodies.at(0);
    RobotBasePtr pbody = vbodies.at(0);
    vector<dReal> values;
    pbody->GetDOFValues(values);
    
    // set new values
    for(int i = 0; i < (int)vsetvalues.size() && i < (int)values.size(); ++i) {
      values[i] = vsetvalues[i];
    }
    
    pbody->Enable(true);
    //pbody->SetVisible(true);
    //CollisionReportPtr report(new CollisionReport());
    //bool runflag = true;
    //while(runflag) {
	
      /* Wait until next shot */
      clock_nanosleep(0,TIMER_ABSTIME,&t, NULL);

      /* Get updated joint info here */
      r = ach_get( &chan_num, &H, sizeof(H), &fs, NULL, ACH_O_LAST );

int main(int argc, char ** argv)
{
    string scenefilename = "data/diffdrive_arm.env.xml";
    string viewername = "qtcoin";
    RaveInitialize(true);
    EnvironmentBasePtr penv = RaveCreateEnvironment();
    penv->SetDebugLevel(Level_Debug);

    boost::thread thviewer(boost::bind(SetViewer,penv,viewername)); // create the viewer
    usleep(400000); // wait for the viewer to init

    penv->Load(scenefilename);

    // attach a physics engine
    penv->SetPhysicsEngine(RaveCreatePhysicsEngine(penv,"ode"));
    penv->GetPhysicsEngine()->SetGravity(Vector(0,0,-9.8));

    vector<RobotBasePtr> vrobots;
    penv->GetRobots(vrobots);
    RobotBasePtr probot = vrobots.at(0);
    std::vector<dReal> q;

    vector<int> wheelindices, restindices;
    ControllerBasePtr wheelcontroller, armcontroller;
    // create the controllers, make sure to lock environment!
    {
        EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment

        MultiControllerPtr multi(new MultiController(penv));
        vector<int> dofindices(probot->GetDOF());
        for(int i = 0; i < probot->GetDOF(); ++i) {
            dofindices[i] = i;
        }
        probot->SetController(multi,dofindices,1); // control everything
        // set the velocity controller on all joints that have 'wheel' in their description
        for(std::vector<KinBody::JointPtr>::const_iterator itjoint = probot->GetJoints().begin(); itjoint != probot->GetJoints().end(); ++itjoint) {
            if( (*itjoint)->GetName().find("wheel") != string::npos ) {
                for(int i = 0; i < (*itjoint)->GetDOF(); ++i) {
                    wheelindices.push_back((*itjoint)->GetDOFIndex()+i);
                }
            }
            else {
                for(int i = 0; i < (*itjoint)->GetDOF(); ++i) {
                    restindices.push_back((*itjoint)->GetDOFIndex()+i);
                }
            }
        }

        if(wheelindices.size() > 0 ) {
            wheelcontroller = RaveCreateController(penv,"odevelocity");
            multi->AttachController(wheelcontroller,wheelindices,0);
        }

        if( restindices.size() > 0 ) {
            armcontroller = RaveCreateController(penv,"idealcontroller");
            multi->AttachController(armcontroller,restindices,0);
        }
        else {
            RAVELOG_WARN("robot needs to have wheels and arm for demo to work\n");
        }
    }

    while(1) {
        {
            EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment

            if( !!armcontroller ) {
                // set a trajectory on the arm and velocity on the wheels
                TrajectoryBasePtr traj = RaveCreateTrajectory(penv,"");
                probot->SetActiveDOFs(restindices);
                ConfigurationSpecification spec = probot->GetActiveConfigurationSpecification();
                int timeoffset = spec.AddDeltaTime();
                traj->Init(spec);
                probot->GetActiveDOFValues(q); // get current values
                vector<dReal> vdata(spec.GetDOF(),0);
                std::copy(q.begin(),q.end(),vdata.begin());
                traj->Insert(0,vdata);
                for(int i = 0; i < 4; ++i) {
                    q.at(RaveRandomInt()%restindices.size()) += RaveRandomFloat()-0.5; // move a random axis
                }

                // check for collisions
                {
                    RobotBase::RobotStateSaver saver(probot); // add a state saver so robot is not moved permenantly
                    probot->SetActiveDOFValues(q);
                    if( probot->CheckSelfCollision() ) { // don't check env collisions since we have physics enabled
                        continue; // robot in collision at final point, so reject
                    }
                }

                std::copy(q.begin(),q.end(),vdata.begin());
                vdata.at(timeoffset) = 2; // trajectory takes 2s
                traj->Insert(1,vdata);
                planningutils::RetimeActiveDOFTrajectory(traj,probot,true);
                armcontroller->SetPath(traj);
            }

            if( !!wheelcontroller ) {
                stringstream sout,ss; ss << "setvelocity ";
                for(size_t i = 0; i < wheelindices.size(); ++i) {
//.........这里部分代码省略.........