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); } } } }