17 using namespace OpenRAVE;
20 namespace cppexamples {
28 dReal appearanceprobability;
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");
43 start =
Vector(0.5,-1,0.6);
47 int main(
const string& cmd)
52 bool RegisterBody(ostream& sout, istream& sinput)
54 EnvironmentMutex::scoped_lock lock(GetEnv()->GetMutex());
56 sinput >> body.filename >> body.appearanceprobability;
60 _listregistered.push_back(body);
64 bool SimulationStep(
dReal fElapsedTime)
66 for(list<RegisteredBody>::iterator it = _listregistered.begin(); it != _listregistered.end(); ++it) {
68 dReal appearanceprobability = 1-pow(1-it->appearanceprobability,fElapsedTime);
69 vector<dReal> vsample;
71 if( vsample.at(0) < appearanceprobability ) {
72 KinBodyPtr pbody = GetEnv()->ReadKinBodyXMLFile(it->filename);
73 GetEnv()->Add(pbody,
true);
75 for(
int iter = 0; iter < 10; ++iter) {
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);
87 GetEnv()->Remove(pbody);
91 _listinstances.push_back(b);
96 list<InstancedBody>::iterator it = _listinstances.begin();
97 while(it != _listinstances.end() ) {
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);
120 list<RegisteredBody> _listregistered;
121 list<InstancedBody> _listinstances;
131 string scenefilename =
"robots/pr2-beta-static.zae";
132 penv->Load(scenefilename);
136 penv->Add(p,
true,
"");
137 stringstream sout, sin(
"registerbody data/mug1.kinbody.xml 0.6");
138 p->SendCommand(sout,sin);
140 sin.str(
"registerbody data/ketchup.kinbody.xml 0.3");
141 p->SendCommand(sout,sin);
143 boost::this_thread::sleep(boost::posix_time::milliseconds(1));
150 int main(
int argc,
char ** argv)
153 return example.
main(argc,argv);