openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
orconveyormovement.cpp
Go to the documentation of this file.
1 
11 #include <openrave-core.h>
12 #include <vector>
13 #include <sstream>
14 
15 #include "orexample.h"
16 
17 using namespace OpenRAVE;
18 using namespace std;
19 
20 namespace cppexamples {
21 
24 {
25  struct RegisteredBody
26  {
27  string filename;
28  dReal appearanceprobability; // probably of appearance in 1 second
29  };
30 
31  struct InstancedBody
32  {
33  KinBodyPtr pbody;
34  dReal timeleft;
35  };
36  SpaceSamplerBasePtr _psampler;
37 public:
38  ConveyorBeltModule(EnvironmentBasePtr penv, std::istream& is) : ModuleBase(penv)
39  {
40  __description = "Handles conveyor belt movement";
41  RegisterCommand("registerbody",boost::bind(&ConveyorBeltModule::RegisterBody,this,_1,_2),"registers a body to be put into the environment");
42  movevel = Vector(0,0.4,0);
43  start = Vector(0.5,-1,0.6);
44  _psampler = RaveCreateSpaceSampler(penv,"mt19937");
45  }
46 
47  int main(const string& cmd)
48  {
49  return 0;
50  }
51 
52  bool RegisterBody(ostream& sout, istream& sinput)
53  {
54  EnvironmentMutex::scoped_lock lock(GetEnv()->GetMutex());
55  RegisteredBody body;
56  sinput >> body.filename >> body.appearanceprobability;
57  if( !sinput ) {
58  return false;
59  }
60  _listregistered.push_back(body);
61  return true;
62  }
63 
64  bool SimulationStep(dReal fElapsedTime)
65  {
66  for(list<RegisteredBody>::iterator it = _listregistered.begin(); it != _listregistered.end(); ++it) {
67  // appearanceprobabiliy is in seconds, so have to transform
68  dReal appearanceprobability = 1-pow(1-it->appearanceprobability,fElapsedTime);
69  vector<dReal> vsample;
70  _psampler->SampleSequence(vsample,4,IT_OpenStart);
71  if( vsample.at(0) < appearanceprobability ) {
72  KinBodyPtr pbody = GetEnv()->ReadKinBodyXMLFile(it->filename);
73  GetEnv()->Add(pbody,true);
74  InstancedBody b;
75  for(int iter = 0; iter < 10; ++iter) {
76  Transform t;
77  t.rot = geometry::quatFromAxisAngle<dReal>(Vector(0,0,1),vsample.at(1)*2*PI);
78  t.trans = start + Vector(vsample.at(2)-0.5,vsample.at(3)-0.5,0)*0.4;
79  pbody->SetTransform(t);
80  if( !GetEnv()->CheckCollision(KinBodyConstPtr(pbody)) ) {
81  b.pbody = pbody;
82  break;
83  }
84  }
85 
86  if( !b.pbody ) {
87  GetEnv()->Remove(pbody);
88  }
89  else {
90  b.timeleft = 4.0;
91  _listinstances.push_back(b);
92  }
93  }
94  }
95 
96  list<InstancedBody>::iterator it = _listinstances.begin();
97  while(it != _listinstances.end() ) {
98  Transform t = it->pbody->GetTransform();
99  t.trans += fElapsedTime*movevel;
100  it->pbody->SetTransform(t);
101  it->timeleft -= fElapsedTime;
102  if( it->timeleft <= 0 ) {
103  GetEnv()->Remove(it->pbody);
104  it = _listinstances.erase(it);
105  }
106  else {
107  ++it;
108  }
109  }
110  return false;
111  }
112 
113  static InterfaceBasePtr create(EnvironmentBasePtr penv, std::istream& is)
114  {
115  return InterfaceBasePtr(new ConveyorBeltModule(penv,is));
116  }
117 
118 private:
119  Vector start, movevel;
120  list<RegisteredBody> _listregistered;
121  list<InstancedBody> _listinstances;
122 };
123 
125 {
126 public:
127  virtual void demothread(int argc, char ** argv) {
128  boost::shared_ptr<void> handle = RaveRegisterInterface(PT_Module,"conveyorbelt",OPENRAVE_MODULE_HASH,OPENRAVE_ENVIRONMENT_HASH,ConveyorBeltModule::create);
129 
130  // load the environment
131  string scenefilename = "robots/pr2-beta-static.zae";
132  penv->Load(scenefilename);
133 
134  // create the conveyor module and add a couple of bodies for simulation
135  ModuleBasePtr p = RaveCreateModule(penv,"conveyorbelt");
136  penv->Add(p,true,"");
137  stringstream sout, sin("registerbody data/mug1.kinbody.xml 0.6");
138  p->SendCommand(sout,sin);
139  sin.clear();
140  sin.str("registerbody data/ketchup.kinbody.xml 0.3");
141  p->SendCommand(sout,sin);
142  while(IsOk()) {
143  boost::this_thread::sleep(boost::posix_time::milliseconds(1));
144  }
145  }
146 };
147 
148 } // end namespace cppexamples
149 
150 int main(int argc, char ** argv)
151 {
153  return example.main(argc,argv);
154 }