Shows how to setup a simulation loop to move objects around a conveyor belt.
#include <vector>
#include <sstream>
using namespace OpenRAVE;
using namespace std;
namespace cppexamples {
{
struct RegisteredBody
{
string filename;
dReal appearanceprobability;
};
struct InstancedBody
{
};
public:
{
__description = "Handles conveyor belt movement";
RegisterCommand("registerbody",boost::bind(&ConveyorBeltModule::RegisterBody,this,_1,_2),"registers a body to be put into the environment");
}
int main(
const string& cmd)
{
return 0;
}
bool RegisterBody(ostream& sout, istream& sinput)
{
EnvironmentMutex::scoped_lock lock(GetEnv()->GetMutex());
RegisteredBody body;
sinput >> body.filename >> body.appearanceprobability;
if( !sinput ) {
return false;
}
_listregistered.push_back(body);
return true;
}
bool SimulationStep(
dReal fElapsedTime)
{
for(list<RegisteredBody>::iterator it = _listregistered.begin(); it != _listregistered.end(); ++it) {
dReal appearanceprobability = 1-pow(1-it->appearanceprobability,fElapsedTime);
vector<dReal> vsample;
if( vsample.at(0) < appearanceprobability ) {
KinBodyPtr pbody = GetEnv()->ReadKinBodyXMLFile(it->filename);
GetEnv()->Add(pbody,true);
InstancedBody b;
for(int iter = 0; iter < 10; ++iter) {
t.rot = geometry::quatFromAxisAngle<dReal>(
Vector(0,0,1),vsample.at(1)*2*
PI);
t.trans = start +
Vector(vsample.at(2)-0.5,vsample.at(3)-0.5,0)*0.4;
pbody->SetTransform(t);
b.pbody = pbody;
break;
}
}
if( !b.pbody ) {
GetEnv()->Remove(pbody);
}
else {
b.timeleft = 4.0;
_listinstances.push_back(b);
}
}
}
list<InstancedBody>::iterator it = _listinstances.begin();
while(it != _listinstances.end() ) {
t.trans += fElapsedTime*movevel;
it->pbody->SetTransform(t);
it->timeleft -= fElapsedTime;
if( it->timeleft <= 0 ) {
GetEnv()->Remove(it->pbody);
it = _listinstances.erase(it);
}
else {
++it;
}
}
return false;
}
{
}
private:
list<RegisteredBody> _listregistered;
list<InstancedBody> _listinstances;
};
class ConveyorExample : public OpenRAVEExample
{
public:
virtual void demothread(int argc, char ** argv) {
string scenefilename = "robots/pr2-beta-static.zae";
penv->Load(scenefilename);
penv->Add(p,true,"");
stringstream sout, sin("registerbody data/mug1.kinbody.xml 0.6");
p->SendCommand(sout,sin);
sin.clear();
sin.str("registerbody data/ketchup.kinbody.xml 0.3");
p->SendCommand(sout,sin);
while(IsOk()) {
boost::this_thread::sleep(boost::posix_time::milliseconds(1));
}
}
};
}
int main(
int argc,
char ** argv)
{
return example.
main(argc,argv);
}