AREXPORT ArTime ArLogFileConnection::getTimeRead(int index)
{
ArTime now;
now.setToNow();
return now;
}
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);
serConn->setBaud(atoi(getAutoBaudChoice()));
// wait a second for the baud to change...
ArUtil::sleep(100);
// empty the buffer from the baud change
while (readLine(buf, sizeof(buf), 100, true, false));
if (!(ret = sendCommandAndRecvStatus(
"VV", "version request after switching 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 switching to autobaudchoice",
getName());
return false;
}
ArLog::log(ArLog::Verbose, "%s: Switched to %s baud rate",
getName(), getAutoBaudChoice());
}
while (readLine(buf, sizeof(buf), 10000, false, true))
{
if (strlen(buf) == 0)
break;
if (strncasecmp(buf, "VEND:", strlen("VEND:")) == 0)
myVendor = &buf[5];
else if (strncasecmp(buf, "PROD:", strlen("PROD:")) == 0)
myProduct = &buf[5];
else if (strncasecmp(buf, "FIRM:", strlen("FIRM:")) == 0)
myFirmwareVersion = &buf[5];
else if (strncasecmp(buf, "PROT:", strlen("PROT:")) == 0)
myProtocolVersion = &buf[5];
else if (strncasecmp(buf, "SERI:", strlen("SERI:")) == 0)
mySerialNumber = &buf[5];
else if (strncasecmp(buf, "STAT:", strlen("STAT:")) == 0)
myStat = &buf[5];
}
if (myVendor.empty() || myProduct.empty() || myFirmwareVersion.empty() ||
myProtocolVersion.empty() || mySerialNumber.empty())
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Missing information in version response",
getName());
return false;
}
if (!(ret = sendCommandAndRecvStatus(
"PP", "parameter info request",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "00") != 0)
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad response to parameter info request",
getName());
return false;
}
while (readLine(buf, sizeof(buf), 10000, false, true))
{
if (strlen(buf) == 0)
break;
if (strncasecmp(buf, "MODL:", strlen("MODL:")) == 0)
myModel = &buf[5];
else if (strncasecmp(buf, "DMIN:", strlen("DMIN:")) == 0)
myDMin = atoi(&buf[5]);
else if (strncasecmp(buf, "DMAX:", strlen("DMAX:")) == 0)
myDMax = atoi(&buf[5]);
else if (strncasecmp(buf, "ARES:", strlen("ARES:")) == 0)
myARes = atoi(&buf[5]);
else if (strncasecmp(buf, "AMIN:", strlen("AMIN:")) == 0)
myAMin = atoi(&buf[5]);
else if (strncasecmp(buf, "AMAX:", strlen("AMAX:")) == 0)
myAMax = atoi(&buf[5]);
else if (strncasecmp(buf, "AFRT:", strlen("AFRT:")) == 0)
myAFront = atoi(&buf[5]);
else if (strncasecmp(buf, "SCAN:", strlen("SCAN:")) == 0)
myScan = atoi(&buf[5]);
}
if (myModel.empty() || myDMin == 0 || myDMax == 0 || myARes == 0 ||
myAMin == 0 || myAMax == 0 || myAFront == 0 || myScan == 0)
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Missing information in parameter info response",
getName());
return false;
}
myStepSize = 360.0 / myARes;
myStepFirst = myAFront * myStepSize;
if (myMaxRange > myDMax)
setMaxRange(myDMax);
//log();
setParams(getStartDegrees(), getEndDegrees(), getIncrement(), getFlipped());
//myLogMore = true;
// myLogMore = false;
ArUtil::sleep(100);
//printf("myRequestString %s\n", myRequestString);
if (!(ret = sendCommandAndRecvStatus(
myRequestString, "request distance reading",
buf, sizeof(buf), 10000)) ||
strcasecmp(buf, "00") != 0)
{
if (ret && strcasecmp(buf, "00") != 0)
ArLog::log(ArLog::Normal,
"%s::blockingConnect: Bad status on distance reading response (%s)",
getName(), buf);
return false;
}
//myLogMore = false;
ArTime started;
started.setToNow();
while (started.secSince() < 10 &&
readLine(buf, sizeof(buf), 10000, true, false))
{
if (strlen(buf) == 0)
return true;
}
ArLog::log(ArLog::Normal, "%s::blockingConnect: Did not get distance reading back",
getName());
return false;
}
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;
}
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);
}
}
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;
}
robot.unlock();
ArUtil::sleep(50);
}
if (vel2)
robot.setVel2(0, 0);
else
robot.setVel(0);
start.setToNow();
while (1)
{
robot.lock();
if (vel2)
robot.setVel2(0, 0);
else
robot.setVel(0);
if (fabs(robot.getVel()) < 20)
{
printf("Stopped\n");
robot.unlock();
break;
}
if (start.mSecSince() > 2000)
{
printf("\nStop timed out\n");
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(50);
}
robot.lock();
robot.disconnect();
robot.unlock();
// shutdown and ge tout
Aria::shutdown();
return 0;
}
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);
}
return packet;
}
}
myReadCount += numRead;
ArLog::log(ArLog::Verbose, "ArLMS1XXPacketReceiver: Got %d bytes (but not end char), up to %d",
numRead, myReadCount);
}
else if (myState == REMAINDER)
{
//printf("In remainder ('%c' %d) \n", myReadBuf[0], myReadBuf[0]);
if (myReadBuf[0] != '\002')
{
ArLog::log(ArLog::Verbose,
"ArLMS1XXPacketReceiver: Remainder didn't start with \\002, starting over...");
myState = STARTING;
continue;
}
// see if we found the end of the packet
for (i = 0; i < myReadCount - 1; i++)
{
//printf("%03d '%c' %d\n", i, myReadBuf[i], myReadBuf[i]);
if (myReadBuf[i] == '\002' && i != 0)
{
ArLog::log(ArLog::Verbose, "ArLMS1XXPacketReceiver: Remainder found start of new packet...",
myReadCount);
myPacket.empty();
myPacket.setLength(0);
memmove(myReadBuf, &myReadBuf[i], myReadCount + i);
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 - 1)
{
myState = STARTING;
ArLog::log(ArLog::Verbose,
"ArLMS1XXPacketReceiver: Remainder was one packet...");
}
// if it isn't move the data up and start again
else
{
if (myReadCount - i < 50)
printf("read buf (%d %d) %s\n", myReadCount, i, myReadBuf);
memmove(myReadBuf, &myReadBuf[i+1], myReadCount - i);
myReadCount = myReadCount - i - 1;
myState = REMAINDER;
ArLog::log(ArLog::Verbose,
"ArLMS1XXPacketReceiver: Remainder was more than one packet... (%d %d)", myReadCount, i);
}
return packet;
}
}
// if we didn't find the end of the packet, then get the rest of the data
myState = DATA;
ArLog::log(ArLog::Verbose,
"ArLMS1XXPacketReceiver: Remainder didn't contain a whole packet...");
continue;
}
else
{
ArLog::log(ArLog::Normal, "ArLMS1XXPacketReceiver: Bad state (%d)",
myState);
myState = STARTING;
}
} while (timeDone.mSecTo() >= 0); // || !myStarting)
return NULL;
}
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();
// shutdown and ge tout
Aria::shutdown();
return 0;
}
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);
map_broadcaster.sendTransform(map_trans);
// publish motors state if changed
bool e = arnl.robot->areMotorsEnabled();
if(e != motors_state.data || !published_motors_state)
{
ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: publishing new motors state: %s.", e?"yes":"no");
motors_state.data = e;
motors_state_pub.publish(motors_state);
published_motors_state = true;
}
const char *s = arnl.getServerStatus();
if(s != NULL && lastServerStatus != s)
{
ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: publishing new server status: %s", s);
lastServerStatus = s;
std_msgs::String msg;
msg.data = lastServerStatus;
arnl_server_status_pub.publish(msg);
}
const char *m = arnl.getServerMode();
if(m != NULL && lastServerMode != m)
{
ROS_INFO_NAMED("rosarnl_node", "rosarnl_node: publishing now server mode: %s", m);
lastServerMode = m;
std_msgs::String msg;
msg.data = lastServerMode;
arnl_server_mode_pub.publish(msg);
}
ROS_WARN_COND_NAMED((tasktime.mSecSince() > 20), "rosarnl_node", "rosarnl_node: publish aria task took %ld ms", tasktime.mSecSince());
}
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
{
printf("Failed to open the master port at %d: %s\n",
portNumber, masterSock.getErrorStr().c_str());
return -1;
}
// just go forever
while (1)
{
// Lets wait for the client to connect to us.
if (masterSock.accept(&clientSock))
printf("Client has connected\n");
else
printf("Error in accepting a connection from the client: %s\n",
masterSock.getErrorStr().c_str());
// now set up our connection so our packet receivers work
clientConn.setSocket(&clientSock);
clientConn.setStatus(ArDeviceConnection::STATUS_OPEN);
lastClientTest.setToNow();
lastData.setToNow();
// open up the robot port
if (robotConn.open(portName) != 0)
{
printf("Could not open robot port %s.\n", portName);
return -1;
}
// while we're open, just read from one port and write to the other
while (clientSock.getFD() >= 0)
{
// get our packet
if (useRobot)
packet = clientRec.receivePacket(1);
else
packet = clientSickRec.receivePacket(1);
// see if we had one
if (packet != NULL)
{
if (tracePackets)
{
printf("Client ");
packet->log();
}
robotConn.writePacket(packet);
lastData.setToNow();
}
// get our packet
if (useRobot)
packet = robotRec.receivePacket(1);
else
packet = robotSickRec.receivePacket(1);
// see if we had one
if (packet != NULL)
{
if (tracePackets)
{
printf("Robot ");
packet->log();
}
clientConn.writePacket(packet);
lastData.setToNow();
}
ArUtil::sleep(1);
// If no datas gone by in timeout ms assume our connection is broken
if (lastData.mSecSince() > timeout)
{
printf("No data received in %d milliseconds, closing connection.\n",
timeout);
clientConn.close();
}
}
// Now lets close the connection to the client
clientConn.close();
printf("Socket to client closed\n");
robotConn.close();
}
// And lets close the master port
masterSock.close();
printf("Master socket closed and program exiting\n");
// Uninitialize Aria
Aria::uninit();
// All done
return(0);
}
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");
}
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);
}
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));
ArLog::log(ArLog::Normal, "Siguiente punto en %.0f %.0f",
gotoPoseAction.getGoal().getX(), gotoPoseAction.getGoal().getY());
}
if(start.mSecSince() >= duration) {
ArLog::log(ArLog::Normal, "%d seconds have elapsed. Cancelling current goal, waiting 3 seconds, and exiting.", duration/1000);
gotoPoseAction.cancelGoal();
robot.unlock();
ArUtil::sleep(3000);
break;
}
robot.unlock();
ArUtil::sleep(100);
}
// Robot desconectado al terminal el sleep
Aria::shutdown();
return 0;
//----------------------------------------------------------------------------------------------------------
// TODO: Place code here.
MSG msg;
HACCEL hAccelTable;
// Initialize global strings
LoadString(hInstance, IDS_APP_TITLE, szTitle, MAX_LOADSTRING);
LoadString(hInstance, IDC_VENTANAROBOT, szWindowClass, MAX_LOADSTRING);
MyRegisterClass(hInstance);
// Perform application initialization:
if (!InitInstance (hInstance, nCmdShow))
{
return FALSE;
}
hAccelTable = LoadAccelerators(hInstance, MAKEINTRESOURCE(IDC_VENTANAROBOT));
// Main message loop:
while (GetMessage(&msg, NULL, 0, 0))
{
if (!TranslateAccelerator(msg.hwnd, hAccelTable, &msg))
{
TranslateMessage(&msg);
DispatchMessage(&msg);
}
}
return (int) msg.wParam;
}
/**
* 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;
// }
// }
printf("Movement Done\n");
}
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;
}
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());
packet->bufToStr(strBuf, 512);
printf("0x%x %s\n", packet->getID(), strBuf);
sick.empty();
sick.uByteToBuf(0x3B);
sick.uByte2ToBuf(180);
sick.uByte2ToBuf(100);
sick.finalizePacket();
con.write(sick.getBuf(), sick.getLength());
packet = receiver.receivePacket(100);
sick.empty();
sick.uByteToBuf(0x20);
sick.uByteToBuf(0x24);
sick.finalizePacket();
con.write(sick.getBuf(), sick.getLength());
}
else
{
value = packet->bufToUByte2();
numReadings = value & 0x3ff;
printf("%ld ms after last reading.\n", lastReading.mSecSince());
/*
printf("Reading number %d, complete %d, unit: %d %d:\n", value & 0x3ff, !(bool)(value & ArUtil::BIT13), (bool)(value & ArUtil::BIT14), (bool)(value & ArUtil::BIT15));
for (i = 0; i < numReadings; i++)
{
value = packet->bufToUByte2();
if (value & ArUtil::BIT13)
printf("D");
printf("%d ", value & 0x1fff);
}
printf("\n");
*/
lastReading.setToNow();
}
}
else
{
//printf("No packet\n");
}
}
}
bool tryport(const char *port, bool is422, int baud, char **argv, int argc, int maxread, int timeout)
{
ArSerialConnection ser1(is422);
ser1.setPort(port);
ser1.setBaud(baud);
printf("Trying %s port %s at baud rate %d:\n", is422?"RS-422":"RS-232", port, baud);
if (!ser1.openSimple())
{
ArLog::logErrorFromOS(ArLog::Terse, "Error opening %s.", port);
return false;
}
if (argc > 0)
{
printf("-> %3d bytes: ", argc);
fflush(stdout);
}
for (int i = 0; i < argc; ++i)
{
int d = strtol(argv[i], NULL, 0);
if (d == 0 && errno != 0)
{
ArLog::logErrorFromOS(ArLog::Terse, "error parsing command argument %d (\"%s\"). Must be decimal, hexidecimal, or octal number.", i, argv[i]);
Aria::exit(3);
}
unsigned char c = (unsigned char)d;
printf("0x%.2X %c ", (unsigned char) c, (c >= ' ' && c <= '~') ? c : ' ');
fflush(stdout);
ser1.write((char*)&c, 1);
}
if (argc > 0)
puts("");
char buf[256];
int n = 0;
ArTime t;
while (1)
{
ArUtil::sleep(1);
int r = 0;
if ((r = ser1.read(buf, sizeof(buf))) < 0)
{
puts("Error reading data from serial port.");
return false;
}
else if (r > 0)
{
printf("<- %3d bytes: ", r);
for (int i = 0; i < r; ++i)
{
//printf("[%d] ", i); fflush(stdout);
char c = buf[i];
printf("0x%.2X %c ", (unsigned char)c, (c >= ' ' && c <= '~') ? c : ' ');
if ((i+1) % 8 == 0)
printf("\n ");
}
printf("\n");
}
if ((n += r) >= maxread)
{
puts("max");
return true;
}
if (t.secSince() > timeout)
{
puts("timeout");
return false;
}
}
return true;
}
AREXPORT bool ArLMS1XX::blockingConnect(void)
{
char buf[1024];
if (!getRunning())
runAsync();
myConnMutex.lock();
if (myConn == NULL)
{
ArLog::log(ArLog::Terse,
"%s: Could not connect because there is no connection defined",
getName());
myConnMutex.unlock();
failedToConnect();
return false;
}
if (myConn->getStatus() != ArDeviceConnection::STATUS_OPEN &&
!myConn->openSimple())
{
ArLog::log(ArLog::Terse,
"%s: Could not connect because the connection was not open and could not open it", getName());
myConnMutex.unlock();
failedToConnect();
return false;
}
myReceiver.setDeviceConnection(myConn);
myConnMutex.unlock();
lockDevice();
myTryingToConnect = true;
unlockDevice();
laserPullUnsetParamsFromRobot();
laserCheckParams();
int size = (270 / .25 + 1);
ArLog::log(myInfoLogLevel, "%s: Setting current buffer size to %d",
getName(), size);
setCurrentBufferSize(size);
ArTime timeDone;
if (myPowerControlled)
timeDone.addMSec(60 * 1000);
else
timeDone.addMSec(30 * 1000);
ArLMS1XXPacket *packet;
ArLMS1XXPacket sendPacket;
sendPacket.empty();
sendPacket.strToBuf("sMN");
sendPacket.strToBuf("SetAccessMode");
sendPacket.uByteToBuf(0x3); // level
sendPacket.strToBuf("F4724744"); // hashed password
sendPacket.finalizePacket();
if ((packet = sendAndRecv(timeDone, &sendPacket, "SetAccessMode")) != NULL)
{
int val;
val = packet->bufToUByte();
delete packet;
packet = NULL;
if (val == 1)
{
ArLog::log(myLogLevel, "%s: Changed access mode (%d)",
getName(), val);
}
else
{
ArLog::log(ArLog::Terse,
"%s: Could not change access mode (%d)", getName(), val);
failedToConnect();
return false;
}
}
else
{
failedToConnect();
return false;
}
sendPacket.empty();
sendPacket.strToBuf("sMN");
sendPacket.strToBuf("mLMPsetscancfg");
sendPacket.byte4ToBuf(5000); // scanning freq
sendPacket.byte2ToBuf(1); // number segments
sendPacket.byte4ToBuf(getIncrementChoiceDouble() * 10000); // angle resolution
//sendPacket.byte4ToBuf((getStartDegrees() + 90) * 10000); // starting angle
sendPacket.byte4ToBuf(-45 * 10000); // can't change starting angle
//sendPacket.byte4ToBuf((getEndDegrees() + 90) * 10000); // ending angle
sendPacket.byte4ToBuf(225 * 10000); // can't change ending angle
sendPacket.finalizePacket();
ArLog::log(myLogLevel, "%s: setscancfg: %s", getName(),
sendPacket.getBuf());
if ((packet = sendAndRecv(timeDone, &sendPacket, "mLMPsetscancfg")) != NULL)
{
int val;
val = packet->bufToUByte();
delete packet;
packet = NULL;
if (val == 0)
{
ArLog::log(myLogLevel, "%s: setscancfg succeeded (%d)",
getName(), val);
}
else
{
ArLog::log(ArLog::Terse,
"%s: Setscancfg failed (%d)", getName(), val);
failedToConnect();
return false;
}
}
else
{
failedToConnect();
return false;
}
sendPacket.empty();
sendPacket.strToBuf("sWN");
sendPacket.strToBuf("LMDscandatacfg");
sendPacket.uByte2ToBuf(0x1); // output channel
sendPacket.uByteToBuf(0x0); // remission
sendPacket.uByteToBuf(0x0); // remission resolution
sendPacket.uByteToBuf(0x0); // unit
sendPacket.uByte2ToBuf(0x0); // encoder
sendPacket.uByteToBuf(0x0); // position
sendPacket.uByteToBuf(0x0); // device name
sendPacket.uByteToBuf(0x0); // comment
sendPacket.uByteToBuf(0x0); // time
sendPacket.byteToBuf(5); // which scan
//sendPacket.byteToBuf(1); // which scan
sendPacket.finalizePacket();
ArLog::log(myLogLevel, "%s: scandatacfg: %s", getName(), sendPacket.getBuf());
if ((packet = sendAndRecv(timeDone, &sendPacket, "LMDscandatacfg")) != NULL)
{
ArLog::log(myLogLevel, "%s: scandatacfg succeeded", getName());
delete packet;
packet = NULL;
}
else
{
failedToConnect();
return false;
}
sendPacket.empty();
sendPacket.strToBuf("sMN");
sendPacket.strToBuf("Run");
sendPacket.finalizePacket();
if ((packet = sendAndRecv(timeDone, &sendPacket, "Run")) != NULL)
{
int val;
val = packet->bufToUByte();
delete packet;
packet = NULL;
if (val == 1)
{
ArLog::log(myLogLevel, "%s: Run succeeded (%d)",
getName(), val);
}
else
{
ArLog::log(ArLog::Terse,
"%s: Could not run (%d)", getName(), val);
failedToConnect();
return false;
}
}
else
{
failedToConnect();
return false;
}
/* when asking one at a time
sendPacket.empty();
sendPacket.strToBuf("sRN");
sendPacket.strToBuf("LMDscandata");
sendPacket.finalizePacket();
if ((packet = sendAndRecv(timeDone, &sendPacket, "LMDscandata")) != NULL)
{
ArLog::log(myLogLevel, "%s: Got %s scan data %d", getName(),
packet->getCommandType(), packet->getLength());
myPacketsMutex.lock();
myPackets.push_back(packet);
myPacketsMutex.unlock();
sensorInterp();
ArLog::log(myLogLevel, "%s: Processed scan data", getName());
}
else
{
failedToConnect();
return false;
}
*/
sendPacket.empty();
sendPacket.strToBuf("sEN");
sendPacket.strToBuf("LMDscandata");
sendPacket.uByteToBuf(1);
sendPacket.finalizePacket();
//printf("(%s)\n", sendPacket.getBuf());
// just ask for continuous data
if (!myConn->write(sendPacket.getBuf(), sendPacket.getLength()))
{
ArLog::log(ArLog::Terse,
"%s: Could not send %s to laser", getName(), "LMDscandata");
failedToConnect();
return false;
}
while (timeDone.mSecTo())
{
packet = myReceiver.receivePacket(1000);
if (packet != NULL &&
strcasecmp(packet->getCommandType(), "sSN") == 0 &&
strcasecmp(packet->getCommandName(), "LMDscandata") == 0)
{
delete packet;
packet = NULL;
lockDevice();
myIsConnected = true;
myTryingToConnect = false;
unlockDevice();
ArLog::log(ArLog::Normal, "%s: Connected to laser", getName());
laserConnect();
return true;
}
else if (packet != NULL)
{
ArLog::log(myLogLevel, "%s: Got %s %s (%d long)", getName(),
packet->getCommandType(), packet->getCommandName(),
packet->getLength());
delete packet;
packet = NULL;
}
}
ArLog::log(ArLog::Terse,
"%s: Did not get scandata back from laser", getName());
failedToConnect();
return false;
}
int main(int argc, char **argv)
{
Aria::init();
argc = 3;
argv[0] = "";
argv[1] = "-rp";
argv[2] = "/dev/ttyUSB0";
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArRobot robot;
ArAnalogGyro gyro(&robot);
ArSonarDevice sonar;
ArRobotConnector robotConnector(&parser, &robot);
if(!robotConnector.connectRobot())
{
if(parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
Aria::exit(1);
}
}
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
Aria::exit(1);
}
imgdb.setRobot(&robot);
int Rc;
pthread_t slam_thread;
Rc = pthread_create(&slam_thread, NULL, slam_event, NULL);
if (Rc) { printf("Create slam thread failed!\n"); exit(-1); }
ImageList* current = NULL;
while (!current) current = imgdb.getEdge();
// current->person_point
robot.addRangeDevice(&sonar);
robot.runAsync(true);
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
printf("You may press escape to exit\n");
robot.setAbsoluteMaxRotVel(30);
ArActionStallRecover recover;
ArActionBumpers bumpers;
double minDistance=10000;
// ArPose endPoint(point.x, point.z);
// current->collect_x
// current->collect_y
// robot.getX()
// robot.getY()
// current = imgdb.getEdge();
// for (int i = 0; i < current->edge.size(); i++) {
// for (int j = 0; j < current->edge[i].size() - 1; j++) {
// double dis = sqrt(pow(current->edge[i][j].z,2)+(pow(current->edge[i][j].x,2)));
// if(dis<minDistance){
// minDistance=dis;
// }
// }
// }
// 引入避障action,前方安全距离为500mm,侧边安全距离为340mm
Avoid avoidSide("Avoid side", ArPose(0, 0), 800, 100, 1000*minDistance);
robot.addAction(&recover, 100);
robot.addAction(&bumpers, 95);
robot.addAction(&avoidSide, 80);
ArActionStop stopAction("stop");
robot.addAction(&stopAction, 50);
robot.enableMotors();
robot.comInt(ArCommands::SOUNDTOG, 0);
const int duration = 500000;
bool first = true;
int goalNum = 0;
ArTime start;
start.setToNow();
while (Aria::getRunning())
{
current = imgdb.getEdge();
for (int i = 0; i < current->edge.size(); i++) {
for (int j = 0; j < current->edge[i].size() - 1; j++) {
// printf("i=%d\n",i);
double dis = sqrt(pow(current->edge[i][j].z,2)+(pow(current->edge[i][j].x,2)));
// printf("dis=%f,mindis=%f\n",dis,minDistance);
if(dis<minDistance){
minDistance=dis;
printf("min=%f\n",minDistance);
}
}
}
//引入避障action,前方安全距离为500mm,侧边安全距离为340mm
// Avoid avoidSide("Avoid side", ArPose(0, 0), 800, 200, minDistance);
// robot.addAction(&avoidSide,80);
robot.lock();
if (first || avoidSide.haveAchievedGoal())
{
first = false;
goalNum++;
printf("count goalNum = %d\n", goalNum);
if (goalNum > 2){
goalNum = 1; // start again at goal #1
}
// avoidSide.setGoal(ArPose(10000,0));
if(goalNum==1){
printf("zuobiaox=%f,zuobiaoy=%f\n",-current->person_point.z,current->person_point.x);
avoidSide.setGoal(ArPose(1000*current->person_point.z, -1000*current->person_point.x));
printf("goalNum == 1\n\n");
}
else if(goalNum==2){
printf("zuobiaox=%f,zuobiaoy=%f\n",-current->person_point.z,current->person_point.x);
avoidSide.setGoal(ArPose(1000*current->person_point.z, -1000*current->person_point.x));
printf("goalNum == 2\n\n");
}
}
if(start.mSecSince() >= duration) {
ArLog::log(ArLog::Normal, "%d seconds have elapsed. Cancelling current goal, waiting 3 seconds, and exiting.", duration/1000);
avoidSide.cancelGoal();
robot.unlock();
ArUtil::sleep(3000);
break;
}
robot.unlock();
ArUtil::sleep(100);
}
Aria::exit(0);
return 0;
}
void ArLMS1XX::sensorInterp(void)
{
ArLMS1XXPacket *packet;
while (1)
{
myPacketsMutex.lock();
if (myPackets.empty())
{
myPacketsMutex.unlock();
return;
}
packet = myPackets.front();
myPackets.pop_front();
myPacketsMutex.unlock();
// if its not a reading packet just skip it
if (strcasecmp(packet->getCommandName(), "LMDscandata") != 0)
{
delete packet;
continue;
}
//set up the times and poses
ArTime time = packet->getTimeReceived();
ArPose pose;
int ret;
int retEncoder;
ArPose encoderPose;
// this value should be found more empirically... but we used 1/75
// hz for the lms2xx and it was fine, so here we'll use 1/50 hz for now
time.addMSec(-20);
if (myRobot == NULL || !myRobot->isConnected())
{
pose.setPose(0, 0, 0);
encoderPose.setPose(0, 0, 0);
}
else if ((ret = myRobot->getPoseInterpPosition(time, &pose)) < 0 ||
(retEncoder =
myRobot->getEncoderPoseInterpPosition(time, &encoderPose)) < 0)
{
ArLog::log(ArLog::Normal, "%s: reading too old to process", getName());
delete packet;
continue;
}
ArTransform transform;
transform.setTransform(pose);
unsigned int counter = 0;
if (myRobot != NULL)
counter = myRobot->getCounter();
lockDevice();
myDataMutex.lock();
int i;
int dist;
//int onStep;
std::list<ArSensorReading *>::reverse_iterator it;
ArSensorReading *reading;
// read the extra stuff
myVersionNumber = packet->bufToUByte2();
myDeviceNumber = packet->bufToUByte2();
mySerialNumber = packet->bufToUByte4();
myDeviceStatus1 = packet->bufToUByte();
myDeviceStatus2 = packet->bufToUByte();
myMessageCounter = packet->bufToUByte2();
myScanCounter = packet->bufToUByte2();
myPowerUpDuration = packet->bufToUByte4();
myTransmissionDuration = packet->bufToUByte4();
myInputStatus1 = packet->bufToUByte();
myInputStatus2 = packet->bufToUByte();
myOutputStatus1 = packet->bufToUByte();
myOutputStatus2 = packet->bufToUByte();
myReserved = packet->bufToUByte2();
myScanningFreq = packet->bufToUByte4();
myMeasurementFreq = packet->bufToUByte4();
if (myDeviceStatus1 != 0 || myDeviceStatus2 != 0)
ArLog::log(myLogLevel, "%s: DeviceStatus %d %d",
myDeviceStatus1, myDeviceStatus2);
/*
printf("Received: %s %s ver %d devNum %d serNum %d scan %d sf %d mf %d\n",
packet->getCommandType(), packet->getCommandName(),
myVersionNumber, myDeviceNumber,
mySerialNumber, myScanCounter, myScanningFreq, myMeasurementFreq);
*/
myNumberEncoders = packet->bufToUByte2();
//printf("\tencoders %d\n", myNumberEncoders);
if (myNumberEncoders > 0)
ArLog::log(myLogLevel, "%s: Encoders %d", getName(), myNumberEncoders);
for (i = 0; i < myNumberEncoders; i++)
{
packet->bufToUByte4();
packet->bufToUByte2();
//printf("\t\t%d\t%d %d\n", i, eachEncoderPosition, eachEncoderSpeed);
}
myNumChans = packet->bufToUByte2();
if (myNumChans > 1)
ArLog::log(myLogLevel, "%s: NumChans %d", getName(), myNumChans);
//printf("\tnumchans %d\n", myNumChans);
char eachChanMeasured[1024];
int eachScalingFactor;
int eachScalingOffset;
double eachStartingAngle;
double eachAngularStepWidth;
int eachNumberData;
for (i = 0; i < myNumChans; i++)
{
eachChanMeasured[0] = '\0';
packet->bufToStr(eachChanMeasured, sizeof(eachChanMeasured));
// if this isn't the data we want then skip it
if (strcasecmp(eachChanMeasured, "DIST1") != 0 &&
strcasecmp(eachChanMeasured, "DIST2") != 0)
continue;
eachScalingFactor = packet->bufToUByte4(); // FIX should be real
eachScalingOffset = packet->bufToUByte4(); // FIX should be real
eachStartingAngle = packet->bufToByte4() / 10000.0;
eachAngularStepWidth = packet->bufToUByte2() / 10000.0;
eachNumberData = packet->bufToUByte2();
/*
ArLog::log(myLogLevel, "%s: %s start %.1f step %.2f numReadings %d",
getName(), eachChanMeasured,
eachStartingAngle, eachAngularStepWidth, eachNumberData);
*/
/*
printf("\t\t%s\tscl %d %d ang %g %g num %d\n",
eachChanMeasured,
eachScalingFactor, eachScalingOffset,
eachStartingAngle, eachAngularStepWidth,
eachNumberData);
*/
// If we don't have any sensor readings created at all, make 'em all
if (myRawReadings->size() == 0)
for (i = 0; i < eachNumberData; i++)
myRawReadings->push_back(new ArSensorReading);
if (eachNumberData > myRawReadings->size())
{
ArLog::log(ArLog::Terse, "%s: Bad data, in theory have %d readings but can only have 541... skipping this packet\n", getName(), eachNumberData);
printf("%s\n", packet->getBuf());
continue;
}
std::list<ArSensorReading *>::iterator it;
double atDeg;
int onReading;
double start;
double increment;
if (myFlipped)
{
start = mySensorPose.getTh() + eachStartingAngle - 90.0 + eachAngularStepWidth * eachNumberData;
increment = -eachAngularStepWidth;
}
else
{
start = mySensorPose.getTh() + eachStartingAngle - 90.0;
increment = eachAngularStepWidth;
}
bool ignore;
for (//atDeg = mySensorPose.getTh() + eachStartingAngle - 90.0,
//atDeg = mySensorPose.getTh() + eachStartingAngle - 90.0 + eachAngularStepWidth * eachNumberData,
atDeg = start,
it = myRawReadings->begin(),
onReading = 0;
onReading < eachNumberData;
//atDeg += eachAngularStepWidth,
//atDeg -= eachAngularStepWidth,
atDeg += increment,
it++,
onReading++)
{
ignore = false;
if (atDeg < getStartDegrees() || atDeg > getEndDegrees())
ignore = true;
reading = (*it);
dist = packet->bufToUByte2();
if (dist == 0)
{
ignore = true;
}
/*
else if (!ignore && dist < 150)
{
//ignore = true;
ArLog::log(ArLog::Normal, "%s: Reading at %.1f %s is %d (not ignoring, just warning)",
getName(), atDeg,
eachChanMeasured, dist);
}
*/
reading->resetSensorPosition(ArMath::roundInt(mySensorPose.getX()),
ArMath::roundInt(mySensorPose.getY()),
atDeg);
reading->newData(dist, pose, encoderPose, transform, counter,
time, ignore, 0); // no reflector yet
}
/*
ArLog::log(ArLog::Normal,
"Received: %s %s scan %d numReadings %d",
packet->getCommandType(), packet->getCommandName(),
myScanCounter, onReading);
*/
}
myDataMutex.unlock();
laserProcessReadings();
unlockDevice();
delete packet;
}
}
ArActionDesired *PickUp::fire(ArActionDesired currentDesired)
{
ArPose pose;
ArACTSBlob blob;
bool blobSeen = false;
double xRel, yRel;
double dist;
// this if the stuff we want to do if we're not going to just drive forward
// and home in on the color, ie the pickup-specific stuff
if (myState == STATE_START_LOOKING)
{
myGripper->gripOpen();
myGripper->liftDown();
mySentLiftDown.setToNow();
mySony->panTilt(0, -15);
myPointedDown = false;
myState = STATE_LOOKING;
myLastSeen.setToNow();
myTried = false;
myLastMoved.setToNow();
myLastPose = myRobot->getPose();
}
// we want to sit still until the lift is down or for a second and a half
if (!((mySentLiftDown.mSecSince() > 200 && myGripper->isLiftMaxed()) ||
mySentLiftDown.mSecSince() > 1500))
{
myDesired.setVel(0);
myDesired.setDeltaHeading(0);
myLastMoved.setToNow();
return &myDesired;
}
if (myState == STATE_SUCCEEDED)
{
//printf("PickUp: Succeeded\n");
return NULL;
}
else if (myState == STATE_FAILED)
{
//printf("PickUp: Failed\n");
return NULL;
}
pose = myRobot->getPose();
dist = myLastPose.findDistanceTo(pose);
if (dist < 10 && myLastMoved.mSecSince() > 1500)
{
printf("PickUp: Failed, no movement in the last 1500 msec.\n");
myState = STATE_FAILED;
return NULL;
}
else if (dist > 10)
{
myLastMoved.setToNow();
}
if (myActs->getNumBlobs(myChannel) == 0 ||
!(blobSeen = myActs->getBlob(myChannel, 1, &blob)))
{
if (((!myPointedDown && myLastSeen.mSecSince() > 1500) ||
(myPointedDown && myLastSeen.mSecSince() > 4000)) &&
myGripper->getBreakBeamState() == 0)
{
printf("PickUp: Lost the blob, failed, last saw it %ld msec ago.\n",
myLastSeen.mSecSince());
myState = STATE_FAILED;
return NULL;
}
}
else
myLastSeen.setToNow();
if (blobSeen)
{
xRel = (double)(blob.getXCG() - WIDTH/2.0) / (double)WIDTH;
yRel = (double)(blob.getYCG() - HEIGHT/2.0) / (double)HEIGHT;
//printf("xRel %.3f yRel %.3f:\n", xRel, yRel);
}
else
{
//printf("No blob: ");
}
myDesired.reset();
if (myGripper->getBreakBeamState() != 0)
{
if (myGripper->getGripState() == 2)
{
printf("PickUp: Succeeded, have block.\n");
myState = STATE_SUCCEEDED;
}
else if (!myTried)
{
myGripper->gripClose();
printf("PickUp: Trying to pick up.\n");
myTried = true;
}
myDesired.setVel(0);
myDesired.setDeltaHeading(0);
return &myDesired;
}
// this means that the grippers are closed, but we don't have anything in
// them, ie that we failed to get the block
else if (myTried && myGripper->getGripState() == 2)
{
myState = STATE_FAILED;
printf("PickUp: Grippers closed, didn't get a block, failed.\n");
return NULL;
}
if (blobSeen && yRel < 0.2 && !myPointedDown)
{
printf("PickUp: Pointing the camera down!!!\n");
mySony->panTilt(0, -ArSonyPTZ::MAX_TILT);
myPointedDown = true;
}
if (!blobSeen || ArMath::fabs(xRel) < .10)
{
//printf("Going straight ahead\n");
myDesired.setDeltaHeading(0);
}
else
{
//printf("Turning %.2f\n", -xRel * 10);
myDesired.setDeltaHeading(-xRel * 10);
}
myDesired.setVel(150);
return &myDesired;
}
int main(int argc, char **argv)
{
// robot
ArRobot robot;
// the laser
ArSick sick;
// sonar, must be added to the robot
//ArSonarDevice sonar;
// the actions we'll use to wander
// recover from stalls
//ArActionStallRecover recover;
// react to bumpers
//ArActionBumpers bumpers;
// limiter for close obstacles
ArActionLimiterForwards limiter("speed limiter near", 1600, 0, 0, 1.3);
// limiter for far away obstacles
//ArActionLimiterForwards limiterFar("speed limiter near", 300, 1000, 450, 1.1);
//ArActionLimiterForwards limiterFar("speed limiter far", 300, 1100, 600, 1.1);
// limiter for the table sensors
//ArActionLimiterTableSensor tableLimiter;
// actually move the robot
ArActionConstantVelocity constantVelocity("Constant Velocity", 1500);
// turn the orbot if its slowed down
ArActionTurn turn;
// mandatory init
Aria::init();
// Parse all our args
ArSimpleConnector connector(&argc, argv);
if (!connector.parseArgs() || argc > 1)
{
connector.logOptions();
exit(1);
}
// add the sonar to the robot
//robot.addRangeDevice(&sonar);
// add the laser to the robot
robot.addRangeDevice(&sick);
// try to connect, if we fail exit
if (!connector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::shutdown();
return 1;
}
robot.comInt(ArCommands::SONAR, 0);
// turn on the motors, turn off amigobot sounds
//robot.comInt(ArCommands::SONAR, 0);
robot.comInt(ArCommands::SOUNDTOG, 0);
// add the actions
//robot.addAction(&recover, 100);
//robot.addAction(&bumpers, 75);
robot.addAction(&limiter, 49);
//robot.addAction(&limiter, 48);
//robot.addAction(&tableLimiter, 50);
robot.addAction(&turn, 30);
robot.addAction(&constantVelocity, 20);
robot.setStateReflectionRefreshTime(50);
limiter.activate();
turn.activate();
constantVelocity.activate();
robot.clearDirectMotion();
//robot.setStateReflectionRefreshTime(50);
robot.setRotVelMax(50);
robot.setTransAccel(1500);
robot.setTransDecel(100);
// start the robot running, true so that if we lose connection the run stops
robot.runAsync(true);
connector.setupLaser(&sick);
// now that we're connected to the robot, connect to the laser
sick.runAsync();
if (!sick.blockingConnect())
{
printf("Could not connect to SICK laser... exiting\n");
Aria::shutdown();
return 1;
}
sick.lockDevice();
sick.setMinRange(250);
sick.unlockDevice();
robot.lock();
ArGlobalFunctor1<ArRobot *> userTaskCB(&userTask, &robot);
robot.addUserTask("iotest", 100, &userTaskCB);
requestTime.setToNow();
robot.comInt(ArCommands::IOREQUEST, 1);
robot.comInt(ArCommands::ENABLE, 1);
robot.unlock();
robot.waitForRunExit();
// now exit
Aria::shutdown();
return 0;
}
void TakeBlockToWall::handler(void)
{
switch (myState)
{
case STATE_START:
setState(STATE_ACQUIRE_BLOCK);
printf("Started state handling!\n");
handler();
return;
break;
case STATE_ACQUIRE_BLOCK:
if (myNewState)
{
myNewState = false;
mySony->panTilt(0, -10);
myAcquire->activate();
myAcquire->setChannel(COLOR_BLOCK);
myPickUp->deactivate();
myDriveTo->deactivate();
myBackup->deactivate();
}
if (myAcquire->getState() == Acquire::STATE_FAILED)
{
printf("## AcqiureBlock: failed\n");
setState(STATE_FAILED);
handler();
return;
}
else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
{
printf("## AcquireBlock: successful\n");
setState(STATE_PICKUP_BLOCK);
handler();
return;
}
break;
case STATE_PICKUP_BLOCK:
if (myNewState)
{
myNewState = false;
myAcquire->deactivate();
myPickUp->activate();
myPickUp->setChannel(COLOR_BLOCK);
myDriveTo->deactivate();
myBackup->deactivate();
}
if (myPickUp->getState() == PickUp::STATE_FAILED)
{
printf("## PickUpBlock: failed\n");
setState(STATE_BACKUP);
handler();
return;
}
else if (myPickUp->getState() == PickUp::STATE_SUCCEEDED)
{
printf("## PickUpBlock: successful\n");
setState(STATE_ACQUIRE_WALL);
myGripper->liftUp();
handler();
return;
}
break;
case STATE_BACKUP:
if (myNewState)
{
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->deactivate();
myBackup->activate();
}
if (myStateStart.mSecSince() > 2000)
{
printf("## Backup: done\n");
setState(STATE_PICKUP_BLOCK2);
handler();
return;
}
break;
case STATE_PICKUP_BLOCK2:
if (myNewState)
{
myNewState = false;
myAcquire->deactivate();
myPickUp->activate();
myPickUp->setChannel(COLOR_BLOCK);
myDriveTo->deactivate();
myBackup->deactivate();
}
if (myPickUp->getState() == PickUp::STATE_FAILED)
{
printf("## PickUpBlock2: failed\n");
setState(STATE_FAILED);
handler();
return;
}
else if (myPickUp->getState() == PickUp::STATE_SUCCEEDED)
{
printf("## PickUpBlock2: successful\n");
setState(STATE_ACQUIRE_WALL);
myGripper->liftUp();
handler();
return;
}
break;
case STATE_ACQUIRE_WALL:
if (myNewState)
{
myNewState = false;
mySony->panTilt(0, -5);
myAcquire->activate();
myAcquire->setChannel(COLOR_FIRST_WALL);
myPickUp->deactivate();
myDriveTo->deactivate();
myBackup->deactivate();
}
if (myAcquire->getState() == Acquire::STATE_FAILED)
{
printf("## AcquireWall:: failed\n");
setState(STATE_FAILED);
handler();
return;
}
else if (myAcquire->getState() == Acquire::STATE_SUCCEEDED)
{
printf("## AcquireWall: successful\n");
setState(STATE_DRIVETO_WALL);
handler();
return;
}
break;
case STATE_DRIVETO_WALL:
if (myNewState)
{
myNewState = false;
myAcquire->deactivate();
myPickUp->deactivate();
myDriveTo->activate();
myDriveTo->setChannel(COLOR_FIRST_WALL);
myBackup->deactivate();
}
if (myDriveTo->getState() == DriveTo::STATE_FAILED)
{
printf("## DriveToWall: failed\n");
setState(STATE_FAILED);
handler();
return;
}
else if (myDriveTo->getState() == DriveTo::STATE_SUCCEEDED)
{
printf("## DriveToWall: succesful\n");
setState(STATE_DROP);
handler();
return;
}
break;
case STATE_DROP:
if (myNewState)
{
myGripper->liftDown();
myNewState = false;
}
if (myStateStart.mSecSince() > 3500)
{
myGripper->gripOpen();
}
if (myStateStart.mSecSince() > 5500)
{
printf("## Drop: success\n");
setState(STATE_SUCCEEDED);
handler();
return;
}
break;
case STATE_SUCCEEDED:
printf("Succeeded at the task!\n");
Aria::shutdown();
myRobot->disconnect();
myRobot->stopRunning();
return;
case STATE_FAILED:
printf("Failed to complete the task!\n");
Aria::shutdown();
myRobot->disconnect();
myRobot->stopRunning();
return;
default:
printf("TakeBlockToWall::handler: Unknown state!\n");
}
}
/// This should be its own thread here
void *ArCentralManager::runThread(void *arg)
{
std::list<ArSocket *>::iterator sIt;
std::list<std::string>::iterator nIt;
std::list<ArCentralForwarder *>::iterator fIt;
ArSocket *socket;
std::string robotName;
ArCentralForwarder *forwarder;
ArNetPacket *packet;
std::list<ArNetPacket *> addPackets;
std::list<ArNetPacket *> remPackets;
threadStarted();
while (getRunning())
{
myTimeChecker.start();
int numForwarders = 0;
int numClients = 0;
myDataMutex.lock();
myCycleCBList.invoke();
myTimeChecker.check("lock");
// this is where the original code to add forwarders was before we
// changed the unique behavior to drop old ones...
std::list<ArCentralForwarder *> connectedRemoveList;
std::list<ArCentralForwarder *> unconnectedRemoveList;
for (fIt = myForwarders.begin(); fIt != myForwarders.end(); fIt++)
{
forwarder = (*fIt);
numForwarders++;
if (forwarder->getServer() != NULL)
numClients += forwarder->getServer()->getNumClients();
bool connected = forwarder->isConnected();
bool removed = false;
if (!forwarder->callOnce(myHeartbeatTimeout, myUdpHeartbeatTimeout,
myRobotBackupTimeout, myClientBackupTimeout))
{
if (connected)
{
ArLog::log(ArLog::Normal, "Will remove forwarder from %s",
forwarder->getRobotName());
connectedRemoveList.push_back(forwarder);
removed = true;
}
else
{
ArLog::log(ArLog::Normal, "Failed to connect to forwarder from %s",
forwarder->getRobotName());
unconnectedRemoveList.push_back(forwarder);
removed = true;
}
}
if (!connected && !removed && forwarder->isConnected())
{
ArTime *newTime = new ArTime;
newTime->setSec(0);
myUsedPorts[forwarder->getPort()] = newTime;
forwarderAdded(forwarder);
/*
ArLog::log(ArLog::Normal, "Adding forwarder %s",
forwarder->getRobotName());
std::multimap<int, ArFunctor1<ArCentralForwarder *> *>::iterator it;
for (it = myForwarderAddedCBList.begin();
it != myForwarderAddedCBList.end();
it++)
{
if ((*it).second->getName() == NULL ||
(*it).second->getName()[0] == '\0')
ArLog::log(ArLog::Normal, "Calling unnamed add functor at %d",
-(*it).first);
else
ArLog::log(ArLog::Normal, "Calling %s add functor at %d",
(*it).second->getName(), -(*it).first);
(*it).second->invoke(forwarder);
}
ArLog::log(ArLog::Normal, "Added forwarder %s",
forwarder->getRobotName());
*/
ArNetPacket *sendPacket = new ArNetPacket;
sendPacket->strToBuf("");
sendPacket->uByte2ToBuf(forwarder->getPort());
sendPacket->strToBuf(forwarder->getRobotName());
sendPacket->strToBuf("");
sendPacket->strToBuf(
forwarder->getClient()->getTcpSocket()->getIPString());
addPackets.push_back(sendPacket);
// MPL added this at the same time as the changes for the deadlock that happened down below
//myClientServer->broadcastPacketTcp(&sendPacket, "clientAdded");
}
}
myTimeChecker.check("processingForwarders");
while ((fIt = connectedRemoveList.begin()) != connectedRemoveList.end())
{
forwarder = (*fIt);
forwarderRemoved(forwarder);
/*
ArLog::log(ArLog::Normal, "Removing forwarder %s",
forwarder->getRobotName());
std::multimap<int, ArFunctor1<ArCentralForwarder *> *>::iterator it;
for (it = myForwarderRemovedCBList.begin();
it != myForwarderRemovedCBList.end();
it++)
{
if ((*it).second->getName() == NULL ||
(*it).second->getName()[0] == '\0')
ArLog::log(ArLog::Normal, "Calling unnamed remove functor at %d",
-(*it).first);
else
ArLog::log(ArLog::Normal, "Calling %s remove functor at %d",
(*it).second->getName(), -(*it).first);
(*it).second->invoke(forwarder);
}
ArLog::log(ArLog::Normal, "Called forwarder removed for %s",
forwarder->getRobotName());
*/
ArNetPacket *sendPacket = new ArNetPacket;
sendPacket->strToBuf("");
sendPacket->uByte2ToBuf(forwarder->getPort());
sendPacket->strToBuf(forwarder->getRobotName());
sendPacket->strToBuf("");
sendPacket->strToBuf(
forwarder->getClient()->getTcpSocket()->getIPString());
// MPL making this just push packets into a list to broadcast at the end since this was deadlocking
//myClientServer->broadcastPacketTcp(&sendPacket, "clientRemoved");
remPackets.push_back(sendPacket);
if (myUsedPorts.find(forwarder->getPort()) != myUsedPorts.end() &&
myUsedPorts[forwarder->getPort()] != NULL)
myUsedPorts[forwarder->getPort()]->setToNow();
myForwarders.remove(forwarder);
delete forwarder;
connectedRemoveList.pop_front();
ArLog::log(ArLog::Normal, "Removed forwarder");
}
myTimeChecker.check("removingForwarders");
while ((fIt = unconnectedRemoveList.begin()) !=
unconnectedRemoveList.end())
{
forwarder = (*fIt);
ArLog::log(ArLog::Normal, "Removing unconnected forwarder %s",
forwarder->getRobotName());
if (myUsedPorts.find(forwarder->getPort()) != myUsedPorts.end() &&
myUsedPorts[forwarder->getPort()] != NULL)
myUsedPorts[forwarder->getPort()]->setToNow();
myForwarders.remove(forwarder);
delete forwarder;
unconnectedRemoveList.pop_front();
ArLog::log(ArLog::Normal, "Removed unconnected forwarder");
}
myTimeChecker.check("removingUnconnectedForwarders");
// this code was up above just after the lock before we changed
// the behavior for unique names
while ((sIt = myClientSockets.begin()) != myClientSockets.end() &&
(nIt = myClientNames.begin()) != myClientNames.end())
{
socket = (*sIt);
robotName = (*nIt);
myClientSockets.pop_front();
myClientNames.pop_front();
ArLog::log(ArLog::Normal,
"New forwarder for socket from %s with name %s",
socket->getIPString(), robotName.c_str());
forwarder = new ArCentralForwarder(myClientServer, socket,
robotName.c_str(),
myClientServer->getTcpPort() + 1,
&myUsedPorts,
&myForwarderServerClientRemovedCB,
myEnforceProtocolVersion.c_str(),
myEnforceType);
myForwarders.push_back(forwarder);
}
myTimeChecker.check("creatingForwarders");
numClients += myRobotServer->getNumClients();
if (myRobotServer != myClientServer)
numClients += myClientServer->getNumClients();
// end of code moved for change in unique behavior
if (numClients > myMostClients)
{
ArLog::log(ArLog::Normal, "CentralManager: New most clients: %d",
numClients);
myMostClients = numClients;
}
if (numForwarders > myMostForwarders)
myMostForwarders = numForwarders;
if (numClients > myMostClients)
{
ArLog::log(ArLog::Normal, "CentralManager: New most forwarders: %d",
numForwarders);
myMostForwarders = numForwarders;
}
myRobotServer->internalSetNumClients(numForwarders +
myClientSockets.size());
myDataMutex.unlock();
while (addPackets.begin() != addPackets.end())
{
packet = addPackets.front();
myClientServer->broadcastPacketTcp(packet, "clientAdded");
addPackets.pop_front();
delete packet;
}
while (remPackets.begin() != remPackets.end())
{
packet = remPackets.front();
myClientServer->broadcastPacketTcp(packet, "clientRemoved");
remPackets.pop_front();
delete packet;
}
myTimeChecker.check("sendingPackets");
myTimeChecker.finish();
//ArUtil::sleep(1);
ArUtil::sleep(myLoopMSecs);
//make this a REALLY long sleep to test the duplicate pending
//connection code
//ArUtil::sleep(20000);
}
threadFinished();
return NULL;
}
ArActionDesired *DriveTo::fire(ArActionDesired currentDesired)
{
ArACTSBlob blob;
double xRel, yRel;
if (myState == STATE_START_LOOKING)
{
myGripper->gripClose();
myGripper->liftUp();
mySony->panTilt(0, -5);
myState = STATE_LOOKING;
myLastSeen.setToNow();
}
if (myActs->getNumBlobs(myChannel) == 0 ||
!myActs->getBlob(myChannel, 1, &blob))
{
if (myLastSeen.mSecSince() > 1000)
{
printf("DriveTo: Lost the blob, failed.\n");
myState = STATE_FAILED;
return NULL;
}
}
else
{
myLastSeen.setToNow();
}
if (myState == STATE_SUCCEEDED || myState == STATE_FAILED)
{
return NULL;
}
xRel = (double)(blob.getXCG() - WIDTH/2.0) / (double)WIDTH;
yRel = (double)(blob.getYCG() - HEIGHT/2.0) / (double)HEIGHT;
//printf("xRel %.3f yRel %.3f\n", xRel, yRel);
myDesired.reset();
// this if the stuff we want to do if we're not going to just drive forward
// and home in on the color, ie the pickup-specific stuff
if (currentDesired.getMaxVelStrength() > 0 &&
currentDesired.getMaxVel() < 50)
{
printf("DriveTo: Close to a wall of some sort, succeeded.\n");
myState = STATE_SUCCEEDED;
myDesired.setVel(0);
myDesired.setDeltaHeading(0);
return &myDesired;
}
if (ArMath::fabs(xRel) < .10)
{
//printf("Going straight ahead\n");
myDesired.setDeltaHeading(0);
}
else
{
//printf("Turning %.2f\n", -xRel * 10);
myDesired.setDeltaHeading(-xRel * 10);
}
myDesired.setVel(300);
return &myDesired;
}
int main(int argc, char **argv)
{
Aria::init();
ArArgumentParser argParser(&argc, argv);
ArSimpleConnector con(&argParser);
ArRobot robot;
// the connection handler from above
ConnHandler ch(&robot);
if(!Aria::parseArgs())
{
Aria::logOptions();
Aria::shutdown();
return 1;
}
if(!con.connectRobot(&robot))
{
ArLog::log(ArLog::Normal, "directMotionExample: Could not connect to the robot. Exiting.");
return 1;
}
ArLog::log(ArLog::Normal, "directMotionExample: Connected.");
// Run the robot processing cycle in its own thread. Note that after starting this
// thread, we must lock and unlock the ArRobot object before and after
// accessing it.
robot.runAsync(false);
// Send the robot a series of motion commands directly, sleeping for a
// few seconds afterwards to give the robot time to execute them.
printf("directMotionExample: Setting rot velocity to 100 deg/sec then sleeping 3 seconds\n");
robot.lock();
robot.setRotVel(100);
robot.unlock();
ArUtil::sleep(3*1000);
printf("Stopping\n");
robot.lock();
robot.setRotVel(0);
robot.unlock();
ArUtil::sleep(200);
printf("directMotionExample: Telling the robot to go 300 mm on left wheel and 100 mm on right wheel for 5 seconds\n");
robot.lock();
robot.setVel2(300, 100);
robot.unlock();
ArTime start;
start.setToNow();
while (1)
{
robot.lock();
if (start.mSecSince() > 5000)
{
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(50);
}
printf("directMotionExample: Telling the robot to move forwards one meter, then sleeping 5 seconds\n");
robot.lock();
robot.move(1000);
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (robot.isMoveDone())
{
printf("directMotionExample: Finished distance\n");
robot.unlock();
break;
}
if (start.mSecSince() > 5000)
{
printf("directMotionExample: Distance timed out\n");
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(50);
}
printf("directMotionExample: Telling the robot to move backwards one meter, then sleeping 5 seconds\n");
robot.lock();
robot.move(-1000);
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (robot.isMoveDone())
{
printf("directMotionExample: Finished distance\n");
robot.unlock();
break;
}
if (start.mSecSince() > 10000)
{
printf("directMotionExample: Distance timed out\n");
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(50);
}
printf("directMotionExample: Telling the robot to turn to 180, then sleeping 4 seconds\n");
robot.lock();
robot.setHeading(180);
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (robot.isHeadingDone(5))
{
printf("directMotionExample: Finished turn\n");
robot.unlock();
break;
}
if (start.mSecSince() > 5000)
{
printf("directMotionExample: Turn timed out\n");
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(100);
}
printf("directMotionExample: Telling the robot to turn to 90, then sleeping 2 seconds\n");
robot.lock();
robot.setHeading(90);
robot.unlock();
start.setToNow();
while (1)
{
robot.lock();
if (robot.isHeadingDone(5))
{
printf("directMotionExample: Finished turn\n");
robot.unlock();
break;
}
if (start.mSecSince() > 5000)
{
printf("directMotionExample: turn timed out\n");
robot.unlock();
break;
}
robot.unlock();
ArUtil::sleep(100);
}
printf("directMotionExample: Setting vel2 to 200 mm/sec on both wheels, then sleeping 3 seconds\n");
robot.lock();
robot.setVel2(200, 200);
robot.unlock();
ArUtil::sleep(3000);
printf("directMotionExample: Stopping the robot, then sleeping for 2 seconds\n");
robot.lock();
robot.stop();
robot.unlock();
ArUtil::sleep(2000);
printf("directMotionExample: Setting velocity to 200 mm/sec then sleeping 3 seconds\n");
robot.lock();
robot.setVel(200);
robot.unlock();
ArUtil::sleep(3000);
printf("directMotionExample: Stopping the robot, then sleeping for 2 seconds\n");
robot.lock();
robot.stop();
robot.unlock();
ArUtil::sleep(2000);
printf("directMotionExample: Setting vel2 with 0 on left wheel, 200 mm/sec on right, then sleeping 5 seconds\n");
robot.lock();
robot.setVel2(0, 200);
robot.unlock();
ArUtil::sleep(5000);
printf("directMotionExample: Telling the robot to rotate at 50 deg/sec then sleeping 5 seconds\n");
robot.lock();
robot.setRotVel(50);
robot.unlock();
ArUtil::sleep(5000);
printf("directMotionExample: Telling the robot to rotate at -50 deg/sec then sleeping 5 seconds\n");
robot.lock();
robot.setRotVel(-50);
robot.unlock();
ArUtil::sleep(5000);
printf("directMotionExample: Setting vel2 with 0 on both wheels, then sleeping 3 seconds\n");
robot.lock();
robot.setVel2(0, 0);
robot.unlock();
ArUtil::sleep(3000);
printf("directMotionExample: Now having the robot change heading by -125 degrees, then sleeping for 6 seconds\n");
robot.lock();
robot.setDeltaHeading(-125);
robot.unlock();
ArUtil::sleep(6000);
printf("directMotionExample: Now having the robot change heading by 45 degrees, then sleeping for 6 seconds\n");
robot.lock();
robot.setDeltaHeading(45);
robot.unlock();
ArUtil::sleep(6000);
printf("directMotionExample: Setting vel2 with 200 on left wheel, 0 on right wheel, then sleeping 5 seconds\n");
robot.lock();
robot.setVel2(200, 0);
robot.unlock();
ArUtil::sleep(5000);
printf("directMotionExample: Done, shutting down Aria and exiting.\n");
Aria::shutdown();
return 0;
}
/**
@param buf the buffer to put the data into
@param size the size of the buffer
@param msWait how long to wait for the data
@param noCheckSum whether there is a checksum on this data or not
(there isn't on command echos)
@param stripLastSemicolon Some responses (to VV and PP) have a
semicolon to separate the string from the checksum... but that
semicolon is NOT included in the checksum, and shouldn't be
included in the string...
**/
bool ArUrg_2_0::readLine(char *buf, unsigned int size,
unsigned int msWait, bool noChecksum,
bool stripLastSemicolon, ArTime *firstByte)
{
if (myConn == NULL)
{
ArLog::log(ArLog::Terse,
"%s: Attempt to read line from null connection", getName());
return false;
}
ArTime started;
started.setToNow();
buf[0] = '\0';
unsigned int onChar = 0;
int ret;
//long int rawCheckSum = 0;
unsigned char rawCheckSum = 0;
char checkSum;
unsigned int i;
unsigned int iMax;
myConnMutex.lock();
while ((msWait == 0 || started.mSecSince() < (int)msWait) &&
onChar < size)
{
if ((ret = myConn->read(&buf[onChar], 1, 0)) > 0)
{
if (onChar == 0 && firstByte != NULL)
firstByte->setToNow();
if (buf[onChar] == '\n' ||
buf[onChar] == '\r')
{
//buf[onChar+1] = '\0';
buf[onChar] = '\0';
if (!noChecksum && onChar >= 1)
{
if (stripLastSemicolon &&
onChar > 2 && buf[onChar - 2] == ';')
iMax = onChar - 2;
else
iMax = onChar - 1;
// find the checksum
for (i = 0; i < iMax; i++)
rawCheckSum += buf[i];
// see if it matches onChar - 1, then NULL out onchar -1
checkSum = (rawCheckSum & 0x3f) + 0x30;
if ((checkSum) != buf[onChar - 1])
{
ArLog::log(ArLog::Normal,
"%s: Bad checksum on '%s' it should be %c",
getName(), buf, checkSum);
myConnMutex.unlock();
return false;
}
// null out the checksum so it doesn't mess up other parsing
buf[onChar - 1] = '\0';
if (stripLastSemicolon &&
onChar >= 2 && buf[onChar - 2] == ';')
{
buf[onChar - 2] = '\0';
}
}
if (myLogMore)
ArLog::log(ArLog::Normal, "%s: '%s'", getName(), buf);
myConnMutex.unlock();
return true;
}
onChar++;
}
if (ret < 0)
{
ArLog::log(ArLog::Normal, "%s: bad ret", getName());
myConnMutex.unlock();
return false;
}
if (ret == 0)
ArUtil::sleep(1);
}
myConnMutex.unlock();
return false;
}
