Shows how to execute different planners simultaneously on different threads using environment cloning.
#include <vector>
#include <sstream>
#include <boost/thread/thread.hpp>
#include <boost/bind.hpp>
using namespace OpenRAVE;
using namespace std;
namespace cppexamples {
class MultithreadedPlanningExample : public OpenRAVEExample
{
public:
MultithreadedPlanningExample() : OpenRAVEExample("") {
}
void _PlanningThread(const std::string& robotname)
{
if( !pmanip->GetIkSolver()) {
}
pclondedenv->Add(pbasemanip,true,probot->GetName());
while(IsOk()) {
EnvironmentMutex::scoped_lock lock(pclondedenv->GetMutex());
Transform t = pmanip->GetEndEffectorTransform();
stringstream ssin,ssout;
ssin << "MoveToHandPosition execute 0 outputtraj pose " << t;
if( !pbasemanip->SendCommand(ssout,ssin) ) {
continue;
}
ptraj->deserialize(ssout);
RAVELOG_INFO(
"trajectory duration %fs\n",ptraj->GetDuration());
}
pclondedenv->Destroy();
}
virtual void demothread(int argc, char ** argv) {
string scenefilename = "data/pa10grasp2.env.xml";
penv->Load(scenefilename);
vector<RobotBasePtr> vrobots;
penv->GetRobots(vrobots);
for(size_t i = 0; i < probot->GetManipulators().size(); ++i) {
if( probot->GetManipulators()[i]->GetName().find("arm") != string::npos ) {
probot->SetActiveManipulator(probot->GetManipulators()[i]);
break;
}
}
penv->Add(pikfast,true,"");
stringstream ssin,ssout;
vector<dReal> vsolution;
ssin <<
"LoadIKFastSolver " << probot->GetName() <<
" " << (int)
IKP_Transform6D;
if( !pikfast->SendCommand(ssout,ssin) ) {
}
if( !pmanip->GetIkSolver()) {
}
int numthreads = 2;
vector<boost::shared_ptr<boost::thread> > vthreads(numthreads);
for(size_t i = 0; i < vthreads.size(); ++i) {
vthreads[i].reset(new boost::thread(boost::bind(&MultithreadedPlanningExample::_PlanningThread,this,probot->GetName())));
}
while(IsOk()) {
boost::this_thread::sleep(boost::posix_time::milliseconds(1));
}
for(size_t i = 0; i < vthreads.size(); ++i) {
vthreads[i]->join();
}
}
};
}
int main(
int argc,
char ** argv)
{
return example.
main(argc,argv);
}