int main(void) { int ret; std::string str; CBTest cbTest; ArFunctorC<CBTest> connectCB(&cbTest, &CBTest::connected); ArFunctorC<CBTest> failedConnectCB(&cbTest, &CBTest::failedConnect); ArFunctorC<CBTest> disconnectCB(&cbTest, &CBTest::disconnected); ArFunctorC<CBTest> disconnectErrorCB(&cbTest, &CBTest::disconnectedError); ArSerialConnection con; ArRobot robot; printf("If a robot is attached to your port you should see:\n"); printf("Failed connect, Connected, Disconnected Error, Connected, Disconnected\n"); printf("If no robot is attached you should see:\n"); printf("Failed connect, Failed connect, Failed connect\n"); printf("-------------------------------------------------------\n"); ArLog::init(ArLog::None, ArLog::Terse); srand(time(NULL)); robot.setDeviceConnection(&con); robot.addConnectCB(&connectCB, ArListPos::FIRST); robot.addFailedConnectCB(&failedConnectCB, ArListPos::FIRST); robot.addDisconnectNormallyCB(&disconnectCB, ArListPos::FIRST); robot.addDisconnectOnErrorCB(&disconnectErrorCB, ArListPos::FIRST); // this should fail since there isn't an open port yet robot.blockingConnect(); if ((ret = con.open()) != 0) { str = con.getOpenMessage(ret); printf("Open failed: %s\n", str.c_str()); exit(0); } robot.blockingConnect(); con.close(); robot.loopOnce(); if ((ret = con.open()) != 0) { str = con.getOpenMessage(ret); printf("Open failed: %s\n", str.c_str()); exit(0); } robot.blockingConnect(); robot.disconnect(); exit(0); }
int main(int argc, char **argv) { int ret; std::string str; ArSerialConnection con; double dist, angle; std::list<ArPoseWithTime *> *readings; std::list<ArPoseWithTime *>::iterator it; double farDist, farAngle; bool found; ArGlobalFunctor failedConnectCB(&failedConnect); Aria::init(); sick = new ArSick; // open the connection, if it fails, exit if ((ret = con.open("/dev/ttyS2")) != 0) { str = con.getOpenMessage(ret); printf("Open failed: %s\n", str.c_str()); Aria::shutdown(); return 1; } sick->configure(false); sick->setDeviceConnection(&con); sick->setFilterNearDist(0); sick->setMinRange(0); sick->addFailedConnectCB(&failedConnectCB, ArListPos::FIRST); sick->runAsync(); ArUtil::sleep(100); sick->lockDevice(); sick->asyncConnect(); sick->unlockDevice(); while (!sick->isConnected()) ArUtil::sleep(100); printf("Connected\n"); // while (sick->isConnected()) int times = 0; while (sick->getRunning()) { //dist = sick->getCurrentBuffer().getClosestPolar(-90, 90, ArPose(0, 0), 30000, &angle); sick->lockDevice(); dist = sick->currentReadingPolar(-90, 90, &angle); if (dist < sick->getMaxRange()) printf("Closest reading %.2f mm away at %.2f degrees\n", dist, angle); else printf("No close reading.\n"); readings = sick->getCurrentBuffer(); int i = 0; for (it = readings->begin(), found = false; it != readings->end(); it++) { i++; dist = (*it)->findDistanceTo(ArPose(0, 0)); angle = (*it)->findAngleTo(ArPose(0, 0)); if (!found || dist > farDist) { found = true; farDist = dist; farAngle = angle; } } if (found) printf("Furthest reading %.2f mm away at %.2f degrees\n", farDist, farAngle); else printf("No far reading found.\n"); printf("%d readings\n\n", i); sick->unlockDevice(); ArUtil::sleep(100); } sick->lockDevice(); sick->stopRunning(); sick->disconnect(); sick->unlockDevice(); system("echo 'succeeded' >> results"); return 0; }
int main(int argc, char **argv) { int ret; std::string str; ArSerialConnection con; const std::list<ArSensorReading *> *readings; std::list<ArSensorReading *>::const_iterator it; int i; ArGlobalFunctor failedConnectCB(&failedConnect); Aria::init(); sick = new ArSick; // open the connection, if it fails, exit if ((ret = con.open("/dev/ttyS2")) != 0) { str = con.getOpenMessage(ret); printf("Open failed: %s\n", str.c_str()); Aria::shutdown(); return 1; } sick->configure(false); sick->setDeviceConnection(&con); sick->addFailedConnectCB(&failedConnectCB, ArListPos::FIRST); sick->runAsync(); ArUtil::sleep(100); sick->lockDevice(); sick->asyncConnect(); sick->unlockDevice(); while (!sick->isConnected()) ArUtil::sleep(100); printf("Connected\n"); // while (sick->isConnected()) while (1) { //dist = sick->getCurrentBuffer().getClosestPolar(-90, 90, ArPose(0, 0), 30000, &angle); sick->lockDevice(); if (!sick->getRunning() || !sick->isConnected()) { break; } printf("\r"); readings = sick->getRawReadings(); if (readings != NULL) { for (i = 0, it = readings->begin(); it != readings->end(); it++, i++) { if (abs(i - readings->size()/2) < 3) printf("(%.2f %d) ", (*it)->getSensorTh(), (*it)->getRange()); } } // switch the commenting in out of the fflush and the // printf("\n"); if you want to print on the same line or have it // scrolling fflush(stdout); //printf("\n"); sick->unlockDevice(); ArUtil::sleep(100); } sick->lockDevice(); sick->stopRunning(); sick->disconnect(); sick->unlockDevice(); return 0; }