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

int main()
{
   
    traj.clear();
    unsigned int mainthreadsleft = numThreads;
    
   // boost::shared_ptr<boost::thread> ( new boost::thread(boost::bind( &track_threads )));
    Ta.trans = transA;
    Ta.rot = quatA;
    Tb.trans = transB;
    Tb.rot = quatB;
    std::string scenefilename = "scenes/test6dof.mujin.zae";
    std::string viewername = "qtcoin";
    RaveInitialize(true); // start openrave core
    EnvironmentBasePtr penv = RaveCreateEnvironment(); // create the main environment
    Transform robot_t;
    RaveVector< dReal > transR(c, d, 0);
    robot_t.trans = transR;

    //boost::thread thviewer(boost::bind(SetViewer,penv,viewername));
    penv->Load(scenefilename);
    RobotBasePtr probot = penv->GetRobot("RV-4F");

    //removing floor for collision checking
    EnvironmentBasePtr pclondedenv = penv->CloneSelf(Clone_Bodies);
    pclondedenv->Remove( pclondedenv->GetKinBody("floor"));
    RobotBasePtr probot_clone = pclondedenv->GetRobot("RV-4F");

    unsigned int tot = ((( abs(a) + abs(c) )/discretization_x )+1) * (((( abs(b) + abs(d) )/discretization_y )+1) * (( abs( z )/discretization_z )+1));
    unsigned int tot_o = tot;
   
    for (unsigned int i = 0 ; i <= (( abs(a) + abs(c) )/discretization_x );i++) {
	for (unsigned int j = 0 ; j <= (( abs(b) + abs(d) )/discretization_y ); j++) {
		for (unsigned int k = 0 ; k <= ( abs( z )/discretization_z ) ; k++) {
			////std::cout << transR[0] << ";" << transR[1] << ";" << transR[2] << std::endl;
			
			//boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
			robot_t.trans = transR;
			probot->SetTransform(robot_t);
                        probot_clone->SetTransform(robot_t);
			if( pclondedenv->CheckCollision(RobotBaseConstPtr(probot_clone)) ){
				//std::cout << "Robot in collision with the environment" << std::endl;
			}
			else {	
				
				do_task(Ta, Tb, penv,3);
			}
			tot -= 1;
			std::cout << tot << "/" << tot_o << std::endl;
			transR[2] = transR[2]+ discretization_z;
		}
		transR[2] = 0;
		transR[1] = transR[1] + discretization_y;
		robot_t.trans = transR;
	}
	transR[2] = 0;
	transR[1] = c;
	transR[0] = transR[0] + discretization_x;
	robot_t.trans = transR;
    }

 
    //thviewer.join(); // wait for the viewer thread to exit	
    RaveDestroy(); // make sure to destroy the OpenRAVE runtime
    penv->Destroy(); // destroy
    return 0;
}

void do_task(Transform Ta, Transform Tb, EnvironmentBasePtr _penv, unsigned int thread_count_left)
{
	std::vector< std::vector< dReal > > vsolutionsA, vsolutionsB;
	dReal time_seconds;
	
	TrajectoryBasePtr ptraj;
	EnvironmentBasePtr pEnv = _penv->CloneSelf(Clone_Bodies);
	
	if (!!(GetIKSolutions(_penv, Tb, vsolutionsB) && GetIKSolutions(_penv, Ta,vsolutionsA))){
		//RAVELOG_INFO("Solution found for both source and goal\n");
		
	} 
	else {

		////RAVELOG_INFO("No solution found either for source or goal");
	}
	k_threads = 1;
	unsigned int _combinations = vsolutionsA.size() * vsolutionsB.size() , combinations = 0;
	std::cout << "Found " << _combinations << " combinations" << std::endl;
	if (!! _combinations){
	vector<boost::shared_ptr<boost::thread> > vthreads(_combinations);
	for(size_t i = 0; i < vsolutionsA.size(); ++i) {
		//RAVELOG_INFO("Planning for every Pose A \n");
			for(size_t j = 0; j < vsolutionsB.size(); ++j) {
				//RAVELOG_INFO("to every Pose B \n");
				std::cout << "i " << i << "j " <<j << std::endl;
				combinations +=1;
				//check if all threads are already running
				 if (k_threads == thread_count_left || combinations == _combinations){
					 for(size_t k = 0; k < k_threads-1; ++k) {
					        std::cout << "Retrieved thread to be complete " << completed_thread << std::endl;
						vthreads[k]->join();
						//k_threads -= 1;
						RAVELOG_INFO("joined ........................\n");
					  }


					 /*for(size_t k = 0; k < k_threads-1; ++k) {
					        std::cout << "Retrieved thread to be complete " << completed_thread << std::endl;
						if (vthreads[k]->get_id() == completed_thread){
								vthreads[k]->join();
								k_threads -= 1;
								RAVELOG_INFO("joined \n");

						}
						
					  }*/
				 }
	
				 else if(k_threads <= thread_count_left){
					//std::cout << k_threads-1 << std::endl;
					
				 	vthreads[k_threads-1].reset(new boost::thread(boost::bind(&GetTaskTime, _penv, vsolutionsA[i], vsolutionsB[j],ptraj)));
					//vthreads[k_threads].get_id();
					k_threads += 1;
					continue;
					
				 }
				  
			          //RAVELOG_INFO("wait for threads to finish\n");
				  //std::cout << vthreads.size() << std::endl;
				  //boost::this_thread::sleep(boost::posix_time::milliseconds(10));
				 
				 
				   
				   ////RAVELOG_INFO("threads finished\n");
				   //vthreads.resize(0);
			}
	}
	
 	
	
	}

}