openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
ormultithreadedplanning.cpp
Go to the documentation of this file.
1 
8 #include <openrave-core.h>
9 #include <vector>
10 #include <sstream>
11 #include <boost/thread/thread.hpp>
12 #include <boost/bind.hpp>
13 
14 #include "orexample.h"
15 
16 using namespace OpenRAVE;
17 using namespace std;
18 
19 namespace cppexamples {
20 
22 {
23 public:
25  }
26 
27  void _PlanningThread(const std::string& robotname)
28  {
29  EnvironmentBasePtr pclondedenv = penv->CloneSelf(Clone_Bodies);
30  RobotBasePtr probot = pclondedenv->GetRobot(robotname);
31  RobotBase::ManipulatorPtr pmanip = probot->GetActiveManipulator();
32  if( !pmanip->GetIkSolver()) {
33  throw OPENRAVE_EXCEPTION_FORMAT0("need ik solver",ORE_Assert);
34  }
35 
36  ModuleBasePtr pbasemanip = RaveCreateModule(pclondedenv,"basemanipulation"); // create the module
37  pclondedenv->Add(pbasemanip,true,probot->GetName()); // load the module
38 
39  TrajectoryBasePtr ptraj = RaveCreateTrajectory(pclondedenv,"");
40 
41 
42  while(IsOk()) {
43  EnvironmentMutex::scoped_lock lock(pclondedenv->GetMutex()); // lock environment
44 
45  // find a new manipulator position and feed that into the planner. If valid, robot will move to it safely.
46  Transform t = pmanip->GetEndEffectorTransform();
47  t.trans += Vector(RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f);
48  t.rot = quatMultiply(t.rot,quatFromAxisAngle(Vector(RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f,RaveRandomFloat()-0.5f)*0.2f));
49 
50  stringstream ssin,ssout;
51  ssin << "MoveToHandPosition execute 0 outputtraj pose " << t;
52  // start the planner and run the robot
53  if( !pbasemanip->SendCommand(ssout,ssin) ) {
54  continue;
55  }
56 
57  ptraj->deserialize(ssout);
58  RAVELOG_INFO("trajectory duration %fs\n",ptraj->GetDuration());
59  }
60 
61  RAVELOG_INFO("destroying cloned environment...\n");
62  pclondedenv->Destroy();
63  }
64 
65  virtual void demothread(int argc, char ** argv) {
66  string scenefilename = "data/pa10grasp2.env.xml";
67  penv->Load(scenefilename);
68 
69  vector<RobotBasePtr> vrobots;
70  penv->GetRobots(vrobots);
71  RobotBasePtr probot = vrobots.at(0);
72 
73  // find a manipulator chain to move
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]);
77  break;
78  }
79  }
80  RobotBase::ManipulatorPtr pmanip = probot->GetActiveManipulator();
81 
82  // load inverse kinematics using ikfast
83  ModuleBasePtr pikfast = RaveCreateModule(penv,"ikfast");
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) ) {
89  RAVELOG_ERROR("failed to load iksolver\n");
90  }
91  if( !pmanip->GetIkSolver()) {
92  throw OPENRAVE_EXCEPTION_FORMAT0("need ik solver",ORE_Assert);
93  }
94 
95  int numthreads = 2;
96 
97  // start worker threads
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())));
101  }
102 
103  while(IsOk()) {
104  boost::this_thread::sleep(boost::posix_time::milliseconds(1));
105  }
106 
107  RAVELOG_INFO("wait for threads to finish\n");
108  for(size_t i = 0; i < vthreads.size(); ++i) {
109  vthreads[i]->join();
110  }
111  RAVELOG_INFO("threads finished\n");
112  }
113 };
114 
115 } // end namespace cppexamples
116 
117 int main(int argc, char ** argv)
118 {
120  return example.main(argc,argv);
121 }