本文整理汇总了C++中ArTime类的典型用法代码示例。如果您正苦于以下问题:C++ ArTime类的具体用法?C++ ArTime怎么用?C++ ArTime使用的例子?那么, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了ArTime类的15个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: getTimeRead
AREXPORT ArTime ArLogFileConnection::getTimeRead(int index)
{
ArTime now;
now.setToNow();
return now;
}
示例2: sendCommandAndRecvStatus
bool ArUrg_2_0::internalConnect(void)
{
bool ret = true;
char buf[1024];
ArSerialConnection *serConn = NULL;
serConn = dynamic_cast<ArSerialConnection *>(myConn);
bool alreadyAtAutobaud = false;
// empty the buffer...
/*
sendCommandAndRecvStatus(
"RS", "reset",
buf, sizeof(buf), 1000);
readLine(buf, sizeof(buf), 1, true, false);
sendCommandAndRecvStatus(
"SCIP2.0", "SCIP 2.0 request",
buf, sizeof(buf), 1000);
*/
writeLine("RS");
ArUtil::sleep(100);
writeLine("SCIP2.0");
ArUtil::sleep(100);
ArTime startedFlushing;
while (readLine(buf, sizeof(buf), 1, true, false) ||
startedFlushing.mSecSince() < 1000);
buf[0] = '\0';
if (!(ret = sendCommandAndRecvStatus(
"VV", "version request",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "00") != 0)
{
// if we didn't get it and have an autobaud, try it at what the autobaud rate is
if (serConn != NULL && atoi(getAutoBaudChoice()) > 0)
{
alreadyAtAutobaud = true;
serConn->setBaud(atoi(getAutoBaudChoice()));
ArUtil::sleep(100);
writeLine("RS");
ArUtil::sleep(100);
writeLine("SCIP2.0");
ArUtil::sleep(100);
startedFlushing.setToNow();
while (readLine(buf, sizeof(buf), 1, true, false) ||
startedFlushing.mSecSince() < 1000);
if (!(ret = sendCommandAndRecvStatus(
"VV", "version request after falling back to autobaudchoice",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "00") != 0)
{
if (ret && strcasecmp(buf, "00") != 0)
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad status on version response after falling back to autobaudchoice",
getName());
return false;
}
}
// if we don't have a serial port or no autobaud then we can't
// change the baud, so just fail
else
{
if (ret && strcasecmp(buf, "00") != 0)
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad status on version response (%s)",
getName(), buf);
return false;
}
}
// if we want to autobaud, then give it a whirl
if (!alreadyAtAutobaud && serConn != NULL && atoi(getAutoBaudChoice()) > 0)
{
// empty the buffer from the last version request
while (readLine(buf, sizeof(buf), 100, true, false));
// now change the baud...
sprintf(buf, "SS%06d", atoi(getAutoBaudChoice()));
if (!writeLine(buf))
return false;
ArUtil::sleep(100);
//serConn->setBaud(115200);
//.........这里部分代码省略.........
示例3: read
AREXPORT int ArLogFileConnection::read(const char *data, unsigned int size,
unsigned int msWait)
{
ArTime timeDone;
unsigned int bytesRead = 0;
int n;
if (getStatus() != STATUS_OPEN)
{
ArLog::log(ArLog::Terse,
"ArLogFileConnection::read: Attempt to use port that is not open.");
return -1;
}
timeDone.setToNow();
timeDone.addMSec(msWait);
if (stopAfter-- <= 0)
{
stopAfter= 1;
return 0;
}
if (myFD != NULL)
{
char line[1000];
if (fgets(line, 1000, myFD) == NULL) // done with file, close
{
close();
return -1;
}
// parse the line
int i=0;
n = 0;
while (line[i] != 0)
{
if (isdigit(line[i]))
{
if (isdigit(line[i+1]))
{
if (isdigit(line[i+2]))
{
const_cast<char *>(data)[n++] =
100 * (line[i]-'0') + 10*(line[i+1]-'0') + line[i+2]-'0';
i++;
}
else
const_cast<char *>(data)[n++] = 10*(line[i]-'0') + line[i+1]-'0';
i++;
}
else
const_cast<char *>(data)[n++] = line[i]-'0';
}
i++;
}
}
#if 0
if (n > 0) // add in checksum
{
int i;
unsigned char nn;
int c = 0;
i = 3;
nn = data[2] - 2;
while (nn > 1)
{
c += ((unsigned char)data[i]<<8) | (unsigned char)data[i+1];
c = c & 0xffff;
nn -= 2;
i += 2;
}
if (nn > 0)
c = c ^ (int)((unsigned char) data[i]);
const_cast<char *>(data)[n++] = (c << 8) & 0xff;
const_cast<char *>(data)[n++] = c & 0xff;
}
#endif
bytesRead = n;
return bytesRead;
}
示例4: main
int main(void)
{
int ret;
char bufWrite[1024];
char bufRead[1024];
bool verbose = false;
int i, n;
for (i = 0; i < 1024; i++)
bufWrite[i] = 0x66;
srand(time(NULL));
int bytes1 = 0;
int bytes2 = 0;
//int numToWrite = 1;
ArTime lastPrint;
ArSerialConnection ser1;
ser1.setPort(ArUtil::COM1);
//ser1.setBaud(115200);
if (!ser1.openSimple())
{
printf("Exiting since open failed\n");
exit(0);
}
printf("Port opened\n");
lastPrint.setToNow();
while (1)
{
ArUtil::sleep(1);
//ArUtil::sleep(500);
/*
bufWrite[0] = 0xfa;
bufWrite[1] = 0xfb;
bufWrite[2] = 0x3;
bufWrite[3] = 0x0;
bufWrite[4] = 0x0;
bufWrite[5] = 0x0;
ser1.write(bufWrite, 6);
*/
////ser1.write("a", 1);
if ((ret = ser1.read(bufRead, sizeof(bufRead))) < 0)
printf("Failed2 read\n");
else if (ret > 0)
{
bufRead[ret] = '\0';
if (verbose)
{
printf("%3d: ", ret);
for (i = 0; i < ret; i++)
printf("%c(%x) ", bufRead[i], (unsigned char)bufRead[i]);
printf("\n");
}
else
printf("%s", bufRead);
}
else
bufRead[0] = '\0';
//ser1.write("a", 1);
}
}
示例5: main
int main(int argc, char **argv)
{
std::string str;
int ret;
int dist;
ArTime start;
ArPose startPose;
bool vel2 = false;
// connection to the robot
ArSerialConnection con;
// the robot
ArRobot robot;
// the connection handler from above
ConnHandler ch(&robot);
// init area with a dedicated signal handling thread
Aria::init(Aria::SIGHANDLE_THREAD);
if (argc != 2 || (dist = atoi(argv[1])) == 0)
{
printf("Usage: %s <distInMM>\n", argv[0]);
exit(0);
}
if (dist < 1000)
{
printf("You must go at least a meter\n");
exit(0);
}
// open the connection with the defaults, exit if failed
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
// set the robots connection
robot.setDeviceConnection(&con);
// try to connect, if we fail, the connection handler should bail
if (!robot.blockingConnect())
{
// this should have been taken care of by the connection handler
// but just in case
printf(
"asyncConnect failed because robot is not running in its own thread.\n");
Aria::shutdown();
return 1;
}
// run the robot in its own thread, so it gets and processes packets and such
robot.runAsync(false);
// just a big long set of printfs, direct motion commands and sleeps,
// it should be self-explanatory
robot.lock();
/*
robot.setAbsoluteMaxTransVel(2000);
robot.setTransVelMax(2000);
robot.setTransAccel(1000);
robot.setTransDecel(1000);
robot.comInt(82, 30); // rotkp
robot.comInt(83, 200); // rotkv
robot.comInt(84, 0); // rotki
robot.comInt(85, 30); // transkp
robot.comInt(86, 450); // transkv
robot.comInt(87, 4); // transki
*/
printf("Driving %d mm (going full speed for that far minus a meter then stopping)\n", dist);
if (vel2)
robot.setVel2(2200, 2200);
else
robot.setVel(2200);
robot.unlock();
start.setToNow();
startPose = robot.getPose();
while (1)
{
robot.lock();
printf("\r vel: %.0f x: %.0f y: %.0f: dist: %.0f heading: %.2f",
robot.getVel(), robot.getX(), robot.getY(),
startPose.findDistanceTo(robot.getPose()),
robot.getTh());
if (startPose.findDistanceTo(robot.getPose()) > abs(dist) - 1000)
{
printf("\nFinished distance\n");
robot.setVel(0);
robot.unlock();
break;
}
if (start.mSecSince() > 10000)
{
printf("\nDistance timed out\n");
robot.setVel(0);
robot.unlock();
break;
}
//.........这里部分代码省略.........
示例6: if
ArLMS1XXPacket *ArLMS1XXPacketReceiver::receivePacket(unsigned int msWait,
bool scandataShortcut)
{
ArLMS1XXPacket *packet;
unsigned char c;
long timeToRunFor;
ArTime timeDone;
ArTime lastDataRead;
ArTime packetReceived;
int numRead;
int i;
if (myConn == NULL ||
myConn->getStatus() != ArDeviceConnection::STATUS_OPEN)
{
return NULL;
}
timeDone.setToNow();
timeDone.addMSec(msWait);
do
{
timeToRunFor = timeDone.mSecTo();
if (timeToRunFor < 0)
timeToRunFor = 0;
//printf("%x\n", c);
if (myState == STARTING)
{
if ((numRead = myConn->read((char *)&c, 1, timeToRunFor)) <= 0)
return NULL;
if (c == '\002')
{
myState = DATA;
myPacket.empty();
myPacket.setLength(0);
myPacket.rawCharToBuf(c);
myReadCount = 0;
packetReceived = myConn->getTimeRead(0);
myPacket.setTimeReceived(packetReceived);
}
else
{
ArLog::log(ArLog::Normal,
"ArLMS1XXPacketReceiver: Failed read (%d)",
numRead);
}
}
else if (myState == DATA)
{
numRead = myConn->read(&myReadBuf[myReadCount],
sizeof(myReadBuf) - myReadCount,
5);
// trap if we failed the read
if (numRead < 0)
{
ArLog::log(ArLog::Normal,
"ArLMS1XXPacketReceiver: Failed read (%d)",
numRead);
myState = STARTING;
return NULL;
}
// see if we found the end of the packet
for (i = myReadCount; i < myReadCount + numRead; i++)
{
if (myReadBuf[i] == '\002')
{
ArLog::log(ArLog::Verbose, "ArLMS1XXPacketReceiver: Data found start of new packet...",
myReadCount);
myPacket.empty();
myPacket.setLength(0);
memmove(myReadBuf, &myReadBuf[i], myReadCount + numRead - i);
numRead -= (i - myReadCount);
myReadCount -= i;
i = 0;
continue;
}
if (myReadBuf[i] == '\003')
{
myPacket.dataToBuf(myReadBuf, i + 1);
myPacket.resetRead();
packet = new ArLMS1XXPacket;
packet->duplicatePacket(&myPacket);
myPacket.empty();
myPacket.setLength(0);
// if it's the end of the data just go back to the beginning
if (i == myReadCount + numRead - 1)
{
//ArLog::log(ArLog::Verbose, "ArLMS1XXPacketReceiver: Starting again");
myState = STARTING;
}
// if it isn't move the data up and start again
else
{
memmove(myReadBuf, &myReadBuf[i+1], myReadCount + numRead - i - 1);
myReadCount = myReadCount + numRead - i - 1;
myState = REMAINDER;
ArLog::log(ArLog::Verbose, "ArLMS1XXPacketReceiver: Got remainder, %d bytes beyond one packet ...",
myReadCount);
//.........这里部分代码省略.........
示例7: main
int main(int argc, char **argv)
{
std::string str;
int ret;
ArTime start;
// connection to the robot
ArSerialConnection con;
// the robot
ArRobot robot;
// the connection handler from above
ConnHandler ch(&robot);
// init area with a dedicated signal handling thread
Aria::init(Aria::SIGHANDLE_THREAD);
// open the connection with the defaults, exit if failed
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
// set the robots connection
robot.setDeviceConnection(&con);
// try to connect, if we fail, the connection handler should bail
if (!robot.blockingConnect())
{
// this should have been taken care of by the connection handler
// but just in case
printf(
"asyncConnect failed because robot is not running in its own thread.\n");
Aria::shutdown();
return 1;
}
// run the robot in its own thread, so it gets and processes packets and such
robot.runAsync(false);
// just a big long set of printfs, direct motion commands and sleeps,
// it should be self-explanatory
printf("Telling the robot to go 300 mm for 5 seconds\n");
robot.lock();
robot.setVel(500);
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (start.mSecSince() > 5000)
{
robot.unlock();
break;
}
printf("Trans: %10g Rot: %10g\n", robot.getVel(), robot.getRotVel());
robot.unlock();
ArUtil::sleep(100);
}
printf("Telling the robot to turn at 50 deg/sec for 10 seconds\n");
robot.lock();
robot.setVel(0);
robot.setRotVel(50);
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (start.mSecSince() > 10000)
{
robot.unlock();
break;
}
printf("Trans: %10g Rot: %10g\n", robot.getVel(), robot.getRotVel());
robot.unlock();
ArUtil::sleep(100);
}
printf("Telling the robot to turn at 100 deg/sec for 10 seconds\n");
robot.lock();
robot.setVel(0);
robot.setRotVel(100);
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (start.mSecSince() > 10000)
{
robot.unlock();
break;
}
printf("Trans: %10g Rot: %10g\n", robot.getVel(), robot.getRotVel());
robot.unlock();
ArUtil::sleep(100);
}
printf("Done with tests, exiting\n");
robot.disconnect();
//.........这里部分代码省略.........
示例8: printf
void RosArnlNode::publish()
{
// todo could only publish if robot not stopped (unless arnl has TriggerTime
// set in which case it might update localization even ifnot moving), or
// use a callback from arnl for robot pose updates rather than every aria
// cycle. In particular, getting the covariance is a bit computational
// intensive and not everyone needs it.
ArTime tasktime;
// Note, this is called via SensorInterpTask callback (myPublishCB, named "ROSPublishingTask"). ArRobot object 'robot' sholud not be locked or unlocked.
ArPose pos = arnl.robot->getPose();
// convert mm and degrees to position meters and quaternion angle in ros pose
tf::poseTFToMsg(tf::Transform(tf::createQuaternionFromYaw(pos.getTh()*M_PI/180), tf::Vector3(pos.getX()/1000,
pos.getY()/1000, 0)), pose_msg.pose.pose);
pose_msg.header.frame_id = "map";
// ARIA/ARNL times are in reference to an arbitrary starting time, not OS
// clock, so find the time elapsed between now and last ARNL localization
// to adjust the time stamp in ROS time vs. now accordingly.
//pose_msg.header.stamp = ros::Time::now();
ArTime loctime = arnl.locTask->getLastLocaTime();
ArTime arianow;
const double dtsec = (double) loctime.mSecSince(arianow) / 1000.0;
//printf("localization was %f seconds ago\n", dtsec);
pose_msg.header.stamp = ros::Time(ros::Time::now().toSec() - dtsec);
// TODO if robot is stopped, ARNL won't re-localize (unless TriggerTime option is
// configured), so should we just use Time::now() in that case? or do users
// expect long ages for poses if robot stopped?
#if 0
{
printf("ros now is %12d sec + %12d nsec = %f seconds\n", ros::Time::now().sec, ros::Time::now().nsec, ros::Time::now().toSec());
ArTime t;
printf("aria now is %12lu sec + %12lu ms\n", t.getSec(), t.getMSec());
printf("arnl loc is %12lu sec + %12lu ms\n", loctime.getSec(), loctime.getMSec());
printf("pose stamp:= %12d sec + %12d nsec = %f seconds\n", pose_msg.header.stamp.sec, pose_msg.header.stamp.nsec, pose_msg.header.stamp.toSec());
double d = ros::Time::now().toSec() - pose_msg.header.stamp.toSec();
printf("diff is %12f sec, \n", d);
puts("----");
}
#endif
#ifndef ROS_ARNL_NO_COVARIANCE
ArMatrix var;
ArPose meanp;
if(arnl.locTask->findLocalizationMeanVar(meanp, var))
{
// ROS pose covariance is 6x6 with position and orientation in 3
// dimensions each x, y, z, roll, pitch, yaw (but placed all in one 1-d
// boost::array container)
//
// ARNL has x, y, yaw (aka theta):
// 0 1 2
// 0 x*x x*y x*yaw
// 1 y*x y*y y*yaw
// 2 yaw*x yaw*y yaw*yaw
//
// Also convert mm to m and degrees to radians.
//
// all elements in pose_msg.pose.covariance were initialized to -1 (invalid
// marker) in the RosArnlNode constructor, so just update elements that
// contain x, y and yaw.
pose_msg.pose.covariance[6*0 + 0] = var(0,0)/1000.0; // x/x
pose_msg.pose.covariance[6*0 + 1] = var(0,1)/1000.0; // x/y
pose_msg.pose.covariance[6*0 + 5] = ArMath::degToRad(var(0,2)/1000.0); //x/yaw
pose_msg.pose.covariance[6*1 + 0] = var(1,0)/1000.0; //y/x
pose_msg.pose.covariance[6*1 + 1] = var(1,1)/1000.0; // y/y
pose_msg.pose.covariance[6*1 + 5] = ArMath::degToRad(var(1,2)/1000.0); // y/yaw
pose_msg.pose.covariance[6*5 + 0] = ArMath::degToRad(var(2,0)/1000.0); //yaw/x
pose_msg.pose.covariance[6*5 + 1] = ArMath::degToRad(var(2,1)/1000.0); // yaw*y
pose_msg.pose.covariance[6*5 + 5] = ArMath::degToRad(var(2,2)); // yaw*yaw
}
#endif
pose_pub.publish(pose_msg);
if(action_executing)
{
move_base_msgs::MoveBaseFeedback feedback;
feedback.base_position.pose = pose_msg.pose.pose;
actionServer.publishFeedback(feedback);
}
// publishing transform map->base_link
map_trans.header.stamp = ros::Time::now();
map_trans.header.frame_id = frame_id_map;
map_trans.child_frame_id = frame_id_base_link;
map_trans.transform.translation.x = pos.getX()/1000;
map_trans.transform.translation.y = pos.getY()/1000;
map_trans.transform.translation.z = 0.0;
map_trans.transform.rotation = tf::createQuaternionMsgFromYaw(pos.getTh()*M_PI/180);
//.........这里部分代码省略.........
示例9: main
int main(int argc, char **argv)
{
// this is how long to wait after there's been no data to close the
// connection.. if its 0 and its using robot it'll set it to 5000 (5
// seconds), if its 0 and using laser, it'll set it to 60000 (60
// seconds, which is needed if the sick driver is controlling power
int timeout = 0;
// true will print out packets as they come and go, false won't
bool tracePackets = false;
// The socket objects
ArSocket masterSock, clientSock;
// The connections
ArTcpConnection clientConn;
ArSerialConnection robotConn;
// the receivers, first for the robot
ArRobotPacketReceiver clientRec(&clientConn);
ArRobotPacketReceiver robotRec(&robotConn);
// then for the laser
ArSickPacketReceiver clientSickRec(&clientConn, 0, false, true);
ArSickPacketReceiver robotSickRec(&robotConn);
// how about a packet
ArBasePacket *packet;
// our timer for how often we test the client
ArTime lastClientTest;
ArTime lastData;
// where we're forwarding from and to
int portNumber;
const char *portName;
// if we're using the robot or the laser
bool useRobot;
if (argc == 1)
{
printf("Using robot and port 8101 and serial connection %s, by default.\n", ArUtil::COM1);
useRobot = true;
portNumber = 8101;
portName = ArUtil::COM1;
}
else if (argc == 2)
{
// if laser isn't the last arg, somethings wrong
if (strcmp(argv[1], "laser") != 0)
{
usage(argv[0]);
return -1;
}
useRobot = false;
portNumber = 8102;
portName = ArUtil::COM3;
printf("Using laser and port %d and serial connection %s, by command line arguments.\n", portNumber, portName);
printf("(Note: Requests to change BAUD rate cannot be fulfilled; use 9600 rate only.)\n");
}
else if (argc == 3)
{
if ((portNumber = atoi(argv[1])) <= 0)
{
usage(argv[0]);
return -1;
}
portName = argv[2];
printf("Using robot and port %d and serial connection %s, by command line arguments.\n", portNumber, portName);
}
else if (argc == 4)
{
if ((portNumber = atoi(argv[1])) <= 0)
{
usage(argv[0]);
return -1;
}
// if laser isn't the last arg, somethings wrong
if (strcmp(argv[3], "laser") != 0)
{
usage(argv[0]);
return -1;
}
useRobot = false;
portName = argv[2];
printf("Using laser and port %d and serial connection %s, by command line arguments.\n", portNumber, portName);
printf("(Note: Requests to change BAUD rate cannot be fulfilled; use 9600 rate only.)\n");
}
else
{
usage(argv[0]);
return -1;
}
if (timeout == 0 && useRobot)
timeout = 5000;
else if (timeout == 0)
timeout = 60000;
// Initialize Aria. For Windows, this absolutely must be done. Because
// Windows does not initialize the socket layer for each program. Each
// program must initialize the sockets itself.
Aria::init(Aria::SIGHANDLE_NONE);
// Lets open the master socket
if (masterSock.open(portNumber, ArSocket::TCP))
printf("Opened the master port at %d\n", portNumber);
else
//.........这里部分代码省略.........
示例10: main
int main(void)
{
printf("\nTesting platform localtime (broken-down) struct:\n");
//struct tm t;
//ArUtil::localtime(&t);
struct tm t;
ArUtil::localtime(&t);
printf("ArUtil::localtime() returned: year=%d mon=%d mday=%d hour=%d min=%d sec=%d\n",
t.tm_year, t.tm_mon, t.tm_mday, t.tm_hour, t.tm_min, t.tm_sec);
time_t yesterday = time(NULL) - (24*60*60) ;
ArUtil::localtime(&yesterday, &t);
printf("ArUtil::localtime(time(NULL) - 24hours, struct tm*) returned: year=%d mon=%d mday=%d hour=%d min=%d sec=%d\n",
t.tm_year, t.tm_mon, t.tm_mday, t.tm_hour, t.tm_min, t.tm_sec);
printf("\nCurrent time as strings:\n");
char year[5], month[3], day[3], hour[3], min[3], sec[3];
ArUtil::putCurrentYearInString(year, 5);
ArUtil::putCurrentMonthInString(month, 3);
ArUtil::putCurrentDayInString(day, 3);
ArUtil::putCurrentHourInString(hour, 3);
ArUtil::putCurrentMinuteInString(min, 3);
ArUtil::putCurrentSecondInString(sec, 3);
printf(" Year:%s, Month:%s, Day:%s, Hour:%s, Min:%s, Sec:%s\n",
year, month, day, hour, min, sec);
printf("\nTesting ArTime:\n");
ArTime start, test;
printf("Setting an ArTime object \"start\" to now...\n");
start.setToNow();
start.log();
printf("Sleeping 4 secs\n");
ArUtil::sleep(4000);
printf("Setting an ArTime object \"test\" to now...\n");
test.setToNow();
test.log();
printf("ms of \"test\" since start %ld\n", test.mSecSince(start));
printf("seconds \"test\" since start %ld\n", test.secSince(start));
printf("ms of start since \"test\" %ld\n", start.mSecSince(test));
printf("seconds \"start\" test %ld\n", start.secSince(test));
printf("\"start\" is before \"test\"? %d\n", test.isBefore(start));
printf("\"start\" is after \"test\"? %d\n", test.isAfter(start));
printf("\"test\" is before \"start\"? %d\n", start.isBefore(test));
printf("\"test\" is after \"start\"? %d\n", start.isAfter(test));
printf("ms from \"start\" to now %ld\n", start.mSecTo());
printf("s from \"start\" to now %ld\n", start.secTo());
printf("ms since \"start\" %ld\n", start.mSecSince());
printf("s since \"start\" %ld\n", start.secSince());
printf("ms from \"test\" stamp to now %ld\n", test.mSecTo());
printf("s from \"test\" stamp to now %ld\n", test.secTo());
printf("Testing addMSec, adding 200 mSec\n");
test.addMSec(200);
printf("ms from \"test\" stamp to now %ld\n", test.mSecTo());
printf("Testing addMSec, subtracting 300 mSec\n");
test.addMSec(-300);
printf("ms from \"test\" stamp to now %ld\n", test.mSecTo());
printf("Testing addMSec, adding 20.999 seconds\n");
test.addMSec(20999);
printf("ms from \"test\" stamp to now %ld\n", test.mSecTo());
printf("Testing addMSec, subtracting 23.5 seconds\n");
test.addMSec(-23500);
printf("ms from \"test\" stamp to now %ld\n", test.mSecTo());
ArTime timeDone;
printf("Setting ArTime object \"done\" to now.\n");
timeDone.setToNow();
timeDone.addMSec(1000);
printf("Making sure the add works in the right direction, adding a second to a timestamp set now\n");
printf("Reading: %ld\n", timeDone.mSecTo());
printf("Sleeping 20 ms\n");
ArUtil::sleep(20);
printf("Reading: %ld\n", timeDone.mSecTo());
printf("Sleeping 2 seconds\n");
ArUtil::sleep(2000);
printf("Reading: %ld\n", timeDone.mSecTo());
puts("\nslamming ArUtil::localtime() from a bunch of threads with the same input time...");
time_t now = time(NULL);
class LocaltimeTestThread : public virtual ArASyncTask
{
private:
time_t time;
public:
LocaltimeTestThread(time_t t) : time(t) {}
virtual void *runThread(void *) {
struct tm t;
ArUtil::localtime(&time, &t);
printf("ArUtil::localtime() returned: year=%d mon=%d mday=%d hour=%d min=%d sec=%d\n", t.tm_year, t.tm_mon, t.tm_mday, t.tm_hour, t.tm_min, t.tm_sec);
return 0;
}
};
for(int i = 0; i < 200; ++i)
(new LocaltimeTestThread(now))->runAsync();
ArUtil::sleep(5000);
printf("test is done.\n");
//.........这里部分代码省略.........
示例11: threadStarted
AREXPORT void * ArSyncLoop::runThread(void *arg)
{
threadStarted();
long timeToSleep;
ArTime loopEndTime;
std::list<ArFunctor *> *runList;
std::list<ArFunctor *>::iterator iter;
ArTime lastLoop;
bool firstLoop = true;
bool warned = false;
if (!myRobot)
{
ArLog::log(ArLog::Terse, "ArSyncLoop::runThread: Trying to run the synchronous loop without a robot.");
return(0);
}
if (!myRobot->getSyncTaskRoot())
{
ArLog::log(ArLog::Terse, "ArSyncLoop::runThread: Can not run the synchronous loop without a task tree");
return(0);
}
while (myRunning)
{
myRobot->lock();
if (!firstLoop && !warned && !myRobot->getNoTimeWarningThisCycle() &&
myRobot->getCycleWarningTime() != 0 &&
myRobot->getCycleWarningTime() > 0 &&
lastLoop.mSecSince() > (signed int) myRobot->getCycleWarningTime())
{
ArLog::log(ArLog::Normal,
"Warning: ArRobot cycle took too long because the loop was waiting for lock.");
ArLog::log(ArLog::Normal,
"\tThe cycle took %u ms, (%u ms normal %u ms warning)",
lastLoop.mSecSince(), myRobot->getCycleTime(),
myRobot->getCycleWarningTime());
}
myRobot->setNoTimeWarningThisCycle(false);
firstLoop = false;
warned = false;
lastLoop.setToNow();
loopEndTime.setToNow();
loopEndTime.addMSec(myRobot->getCycleTime());
myRobot->incCounter();
myRobot->unlock();
// note that all the stuff beyond here should maybe have a lock
// but it should be okay because its just getting data
myInRun = true;
myRobot->getSyncTaskRoot()->run();
myInRun = false;
if (myStopRunIfNotConnected && !myRobot->isConnected())
{
if (myRunning)
ArLog::log(ArLog::Normal, "Exiting robot run because of lost connection.");
break;
}
timeToSleep = loopEndTime.mSecTo();
// if the cycles chained and we're connected the packet handler will be
// doing the timing for us
if (myRobot->isCycleChained() && myRobot->isConnected())
timeToSleep = 0;
if (!myRobot->getNoTimeWarningThisCycle() &&
myRobot->getCycleWarningTime() != 0 &&
myRobot->getCycleWarningTime() > 0 &&
lastLoop.mSecSince() > (signed int) myRobot->getCycleWarningTime())
{
ArLog::log(ArLog::Normal,
"Warning: ArRobot sync tasks too long at %u ms, (%u ms normal %u ms warning)",
lastLoop.mSecSince(), myRobot->getCycleTime(),
myRobot->getCycleWarningTime());
warned = true;
}
if (timeToSleep > 0)
ArUtil::sleep(timeToSleep);
}
myRobot->lock();
myRobot->wakeAllRunExitWaitingThreads();
myRobot->unlock();
myRobot->lock();
runList=myRobot->getRunExitListCopy();
myRobot->unlock();
for (iter=runList->begin();
iter != runList->end(); ++iter)
(*iter)->invoke();
delete runList;
threadFinished();
return(0);
}
示例12: _tWinMain
int APIENTRY _tWinMain(HINSTANCE hInstance,
HINSTANCE hPrevInstance,
LPTSTR lpCmdLine,
int nCmdShow)
{
UNREFERENCED_PARAMETER(hPrevInstance);
UNREFERENCED_PARAMETER(lpCmdLine);
//-------------- M A I N D E L P R O G R A M A D E L R O B O T------------//
//----------------------------------------------------------------------------------------------------------
//inicializaion de variables
Aria::init();
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArSimpleConnector simpleConnector(&parser);
ArRobot robot;
ArSonarDevice sonar;
ArAnalogGyro gyro(&robot);
robot.addRangeDevice(&sonar);
// presionar tecla escape para salir del programa
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
printf("You may press escape to exit\n");
// uso de sonares para evitar colisiones con las paredes u
// obstaculos grandes, mayores a 8cm de alto
ArActionLimiterForwards limiterAction("speed limiter near", 300, 600, 250);
ArActionLimiterForwards limiterFarAction("speed limiter far", 300, 1100, 400);
ArActionLimiterTableSensor tableLimiterAction;
robot.addAction(&tableLimiterAction, 100);
robot.addAction(&limiterAction, 95);
robot.addAction(&limiterFarAction, 90);
// Inicializon la funcion de goto
ArActionGoto gotoPoseAction("goto");
robot.addAction(&gotoPoseAction, 50);
// Finaliza el goto si es que no hace nada
ArActionStop stopAction("stop");
robot.addAction(&stopAction, 40);
// Parser del CLI
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
exit(1);
}
// Conexion del robot
if (!simpleConnector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::exit(1);
}
robot.runAsync(true);
// enciende motores, apaga sonidos
robot.enableMotors();
robot.comInt(ArCommands::SOUNDTOG, 0);
const int duration = 100000; //msec
ArLog::log(ArLog::Normal, "Completados los puntos en %d segundos", duration/1000);
bool first = true;
int horiz = 1800;
int vert = 380;
int goalNum = 0;
ArTime start;
start.setToNow();
while (Aria::getRunning())
{
robot.lock();
// inicia el primer punto
if (first || gotoPoseAction.haveAchievedGoal())
{
first = false;
goalNum++; //cambia de 0 a 1 el contador
if (goalNum > 7)
goalNum = 1;
//comienza la secuencia de puntos
if (goalNum == 1)
gotoPoseAction.setGoal(ArPose(horiz, vert*0));
else if (goalNum == 2)
gotoPoseAction.setGoal(ArPose(0, vert*1));
else if (goalNum == 3)
gotoPoseAction.setGoa l(ArPose(horiz, vert*2)+5);
else if (goalNum == 4)
gotoPoseAction.setGoal(ArPose(0, vert*3));
else if (goalNum == 5)
gotoPoseAction.setGoal(ArPose(horiz, vert*4+5));
else if (goalNum == 6)
gotoPoseAction.setGoal(ArPose(0, vert*5));
else if (goalNum == 7)
gotoPoseAction.setGoal(ArPose(0, vert*0));
//.........这里部分代码省略.........
开发者ID:eilo,项目名称:Evolucion-Artificial-y-Robotica-Autonoma-en-Robots-Pioneer-P3-DX,代码行数:101,代码来源:GoPos.cpp
示例13: while
/**
* moveRobotTo() rotates the robot by the mentioned theta and displaces the robot
* by the given distance in the new heading.
* @args:
* double r - the distance in Millimeter by which robot is to be moved. Default=1000.
* double th - the angle in degrees by which the robot is to be rotated. Default=45
* @return: None
*/
void BotConnector::moveRobot(double r, double th)
{
ArTime start;
robot.runAsync(true);
robot.enableMotors();
//rotating robot
// robot.lock();
// robot.setDeltaHeading(th);
// robot.unlock();
// ArUtil::sleep(10000);
if( th!=0 )
{
robot.lock();
robot.setHeading(th+robot.getTh());
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (robot.isHeadingDone(1))
{
printf("directMotionExample: Finished turn\n");
robot.unlock();
break;
}
if (start.mSecSince() > 15000)
{
printf("directMotionExample: Turn timed out\n");
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(100);
}
}
//moving robot
if( r!=0 )
{
robot.lock();
robot.move(r);
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (robot.isMoveDone())
{
printf("directMotionExample: Finished distance\n");
robot.unlock();
break;
}
if (start.mSecSince() > 15000)
{
printf("directMotionExample: Distance timed out\n");
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(50);
}
}
// ArLog::log(ArLog::Normal, "Going to four goals in turn for %d seconds, then cancelling goal and exiting.", duration/1000);
// ArTime start;
// start.setToNow();
// while(Aria::getRunning())
// {
// if(robot.isHeadingDone())
// {
// robot.unlock();
// printf("turned in %ld\n",start.mSecSince());
// break;
// }
// gotoPoseAction->setGoal(pos);
// if(gotoPoseAction->haveAchievedGoal())
// if(start.mSecSince() >= duration)
// {
// gotoPoseAction->cancelGoal();
// printf("time out :(\n");
// break;
// }
// else if(gotoPoseAction->haveAchievedGoal())
// {
// gotoPoseAction->cancelGoal();
// printf("task in time %ld\n",start.mSecSince());
// break;
// }
//.........这里部分代码省略.........
开发者ID:krishnatejakvs,项目名称:Velocity-Obstacle-Method-on-Differential-Drive-Robots,代码行数:101,代码来源:test.cpp
示例14: strcasecmp
bool ArUrg_2_0::internalGetReading(void)
{
ArTime readingRequested;
std::string reading;
char buf[1024];
reading = "";
/*
if (!writeLine(myRequestString))
{
ArLog::log(ArLog::Terse, "Could not send request distance reading to urg");
return false;
}
*/
ArTime firstByte;
if (!readLine(buf, sizeof(buf), 1000, true, false, &firstByte) ||
strcasecmp(buf, myRequestString) != 0)
{
ArLog::log(ArLog::Normal,
"%s: Did not get distance reading response (%s)",
getName(), buf);
return false;
}
// TODO this isn't the right time, but for most of what we do that
// won't matter
readingRequested.setToNow();
readingRequested.addMSec(-100);
if (!readLine(buf, sizeof(buf), 100, false, false) ||
strcasecmp(buf, "99") != 0)
{
ArLog::log(ArLog::Normal,
"%s: Bad status on distance reading response (%s)",
getName(), buf);
return false;
}
if (!readLine(buf, sizeof(buf), 100, false, false))
{
ArLog::log(ArLog::Normal,
"%s: Could not read timestamp in distance reading response (%s)",
getName(), buf);
return false;
}
while (readLine(buf, sizeof(buf), 100, false, false))
{
if (strlen(buf) == 0)
{
myReadingMutex.lock();
myReadingRequested = readingRequested;
myReading = reading;
myReadingMutex.unlock();
if (myRobot == NULL)
sensorInterp();
return true;
}
else
{
reading += buf;
}
}
return false;
}
示例15: main
int main(int argc, char **argv)
{
int ret;
std::string str;
ArSerialConnection con;
ArSickPacket sick;
ArSickPacket *packet;
ArSickPacketReceiver receiver(&con);
ArTime start;
unsigned int value;
int numReadings;
ArTime lastReading;
ArTime packetTime;
start.setToNow();
// open the connection, if it fails, exit
if ((ret = con.open()) != 0)
{
str = con.getOpenMessage(ret);
printf("Open failed: %s\n", str.c_str());
Aria::shutdown();
return 1;
}
start.setToNow();
printf("Waiting for laser to power on\n");
sick.empty();
sick.uByteToBuf(0x10);
sick.finalizePacket();
con.write(sick.getBuf(), sick.getLength());
while (start.secSince() < 70 &&
((packet = receiver.receivePacket(100)) == NULL
|| (packet->getID() != 0x90)));
if (packet != NULL)
printf("Laser powered on\n");
else
exit(1);
printf("Changing baud\n");
sick.empty();
sick.byteToBuf(0x20);
sick.byteToBuf(0x40);
sick.finalizePacket();
con.write(sick.getBuf(), sick.getLength());
ArUtil::sleep(10);
if (!con.setBaud(38400))
{
printf("Could not set baud, exiting\n");
}
/*packet = receiver.receivePacket(100);
if (packet != NULL)
packet->log();
*/
sick.empty();
sick.uByteToBuf(0x3B);
sick.uByte2ToBuf(180);
sick.uByte2ToBuf(100);
sick.finalizePacket();
con.write(sick.getBuf(), sick.getLength());
packet = receiver.receivePacket(100);
if (packet != NULL)
packet->log();
sick.empty();
sick.byteToBuf(0x20);
sick.byteToBuf(0x24);
sick.finalizePacket();
con.write(sick.getBuf(), sick.getLength());
packet = receiver.receivePacket(100);
if (packet != NULL)
packet->log();
printf("Starting to report back from port, it took %ld ms to get here:\n",
start.mSecSince());
start.setToNow();
while (start.secSince() < 6)
{
packetTime.setToNow();
packet = receiver.receivePacket();
if (packet != NULL)
printf("####### %ld ms was how long the packet took\n", packetTime.mSecSince());
if (packet != NULL)
{
if (packet->getLength() < 10)
packet->log();
else if (packet->getID() == 0x90)
{
char strBuf[512];
packet->log();
//printf("%x\n", packet->bufToUByte());
//.........这里部分代码省略.........