11 #include <boost/thread/thread.hpp>
12 #include <boost/bind.hpp>
16 using namespace OpenRAVE;
19 namespace cppexamples {
27 void _PlanningThread(
const std::string& robotname)
32 if( !pmanip->GetIkSolver()) {
37 pclondedenv->Add(pbasemanip,
true,probot->GetName());
43 EnvironmentMutex::scoped_lock lock(pclondedenv->GetMutex());
46 Transform t = pmanip->GetEndEffectorTransform();
50 stringstream ssin,ssout;
51 ssin <<
"MoveToHandPosition execute 0 outputtraj pose " << t;
53 if( !pbasemanip->SendCommand(ssout,ssin) ) {
57 ptraj->deserialize(ssout);
58 RAVELOG_INFO(
"trajectory duration %fs\n",ptraj->GetDuration());
62 pclondedenv->Destroy();
66 string scenefilename =
"data/pa10grasp2.env.xml";
67 penv->Load(scenefilename);
69 vector<RobotBasePtr> vrobots;
70 penv->GetRobots(vrobots);
74 for(
size_t i = 0; i < probot->GetManipulators().size(); ++i) {
75 if( probot->GetManipulators()[i]->GetName().find(
"arm") != string::npos ) {
76 probot->SetActiveManipulator(probot->GetManipulators()[i]);
84 penv->Add(pikfast,
true,
"");
85 stringstream ssin,ssout;
86 vector<dReal> vsolution;
87 ssin <<
"LoadIKFastSolver " << probot->GetName() <<
" " << (int)
IKP_Transform6D;
88 if( !pikfast->SendCommand(ssout,ssin) ) {
91 if( !pmanip->GetIkSolver()) {
98 vector<boost::shared_ptr<boost::thread> > vthreads(numthreads);
99 for(
size_t i = 0; i < vthreads.size(); ++i) {
100 vthreads[i].reset(
new boost::thread(boost::bind(&MultithreadedPlanningExample::_PlanningThread,
this,probot->GetName())));
104 boost::this_thread::sleep(boost::posix_time::milliseconds(1));
108 for(
size_t i = 0; i < vthreads.size(); ++i) {
117 int main(
int argc,
char ** argv)
120 return example.
main(argc,argv);