int main(int argc, char **argv)
{
ros::init(argc, argv, "trolley");
boost::thread spin_thread(&spinThread);
ros::NodeHandle nh;
ros::Publisher ledPublisher = nh.advertise<kinect_motor::LED>("led", 10);
ros::Publisher motor = nh.advertise<kinect_motor::angle>("angle", 10);
cout << "Start" << endl;
// Prostredi.
Environment * environment = new Environment();
// Nacteni prostredi ze souboru.
environment->Load("configuration.xml");
// Rizeni pohybu robota.
Control control(environment);
while (ros::ok()) {
// Pozice osoby. NITE.
Nullable<XnPoint3D> userPosition = User::getInstance().UserPosition();
// Rizeni pohybu robota.
control.Position(userPosition);
if (userPosition.HasValue()) {
// Osoba byla detekovana.
// Rozsvit celni diodu.
kinect_motor::LED msg;
msg.ledColor = 2;
ledPublisher.publish(msg);
}
else {
// Zhasni celni diodu.
kinect_motor::LED msg;
msg.ledColor = 0;
ledPublisher.publish(msg);
}
}
cout << "Finished" << endl;
spin_thread.join();
delete environment;
return 0;
}
int RobotWorld::LoadTerrain(const string& fn)
{
Environment* t = new Environment;
if(!t->Load(fn.c_str())) {
delete t;
return -1;
}
const char* justfn = GetFileName(fn.c_str());
char* buf = new char[strlen(justfn)+1];
strcpy(buf,justfn);
StripExtension(buf);
string name=buf;
delete [] buf;
int i = AddTerrain(name,t);
return i;
}