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;
}
