void ULaserDevice::send(char * msg)
{
int v;
char * s, *d;
//
//strncpy(sendStr, msg, MAX_SEND_MSG_LNG);
s = msg;
d = sendStr;
while (*s != '\0')
{
if (*s == ':')
{
v = strtol(&s[1], &s, 0);
if (*s == ':')
s++;
*d = char(v);
d++;
}
else
*d++ = *s++;
}
*d = '\0';
sendStrCnt = (d - sendStr);
sendNewData = true;
if (not isPortOpen())
start();
}
DynamixelDriver::DynamixelDriver(const char *port, unsigned int baudrate, DriverMode mode) {
_portHandler = dynamixel::PortHandler::getPortHandler(port);
if(isPortOpen()) {
_portHandler->setBaudRate(baudrate);
_portHandler->setPacketTimeout(10000.0);
switch (mode) {
case COMBINE:
_packetHandlerVer1 = dynamixel::PacketHandler::getPacketHandler(PROTOCOL1);
_packetHandlerVer2 = dynamixel::PacketHandler::getPacketHandler(PROTOCOL2);
break;
case PROTOCOL1:
_packetHandlerVer1 = dynamixel::PacketHandler::getPacketHandler(PROTOCOL1);
_packetHandlerVer2 = NULL;
break;
case PROTOCOL2:
_packetHandlerVer2 = dynamixel::PacketHandler::getPacketHandler(PROTOCOL2);
_packetHandlerVer1 = NULL;
break;
}
}
else ROS_INFO("[%s]: Port %s can't be open", ros::this_node::getName().c_str(), port);
}
const char * ULaserDevice::print(const char * preString, char * buff, int buffCnt)
{
snprintf(buff, buffCnt, "%s %s on %s open=%s w=%ddeg, "
"res=%4.2fdeg, rate=%.1f/s scan=%lu\n", preString,
getName(), devName, bool2str(isPortOpen()),
modeAngleScan, angleResolution,
getMsgRate(), getSerial());
return buff;
}
void ULaserDevice::threadRunLoop()
{
int STAT_INTERVAL = 1000;
//struct timeval t1, t2;
double dt;
bool gotData;
UTime t;
//
threadRunning = true;
loopCnt = 0;
t.Now();
while (not threadStop)
{
// send new data to device
if (sendNewData)
{ // debug
printf("ULaserDevice::threadRunLoop sending to device '%s'\n", sendStr);
// debug end
sendToDevice(sendStr, sendStrCnt);
sendNewData = false;
}
// receive data from device
// may be measurements or not
gotData = receiveData();
// do other things as appropriate
if (not isPortOpen())
Wait(0.1);
if (not gotData)
Wait(0.001); // changed to 1 ms
if(int(statGoodCnt + statBadCnt) == STAT_INTERVAL )
{
dt = t.getTimePassed();
t.Now();
if (verbose)
{
printf("Got %d Good and %d Bad packets (total %d) %3.1f%% good\n",
statGoodCnt, statBadCnt, statGoodCnt + statBadCnt,
(float)(statGoodCnt) /
(float)(statGoodCnt + statBadCnt) * 100.0);
printf("in %7.3f seconds or %6.2f scan/sec\n",
dt, (float)(statGoodCnt)/dt);
}
//
statMsgRate = double(statGoodCnt)/dt;
statGoodCnt = 0;
statBadCnt = 0;
}
loopCnt++;
}
threadRunning = false;
}
bool ULaserDevice::start()
{
bool result = true;
pthread_attr_t thAttr;
//
if ((not modeSimulated) and (not isPortOpen()))
{ // open port
result = openPort();
}
//
if ((isPortOpen() or modeSimulated) and not threadRunning)
{
pthread_attr_init(&thAttr);
//
threadStop = false;
// create socket server thread
result = (pthread_create(&threadHandle, &thAttr,
&threadRunSick, (void *)this) == 0);
pthread_attr_destroy(&thAttr);
}
return result;
}
void ULaserDevice::stop(bool justClosePort)
{
if (isPortOpen())
{ // stop contiious mode and close serial port
closePort();
}
if (threadRunning and not justClosePort)
{ // stop and join thread
threadStop = true;
pthread_join(threadHandle, NULL);
}
// close log too (if open)
datalog.closeLog();
}
const char * UHokuyo::getNameFromDevice()
{
int i;
//
if (isPortOpen())
{
closePort();
}
// open portand ensure that receive thread is running
start();
if (isPortOpen())
{ // wait for port to open ...
lock();
Wait(0.1);
printf("Name request [V\\n]...\n");
// request forst message - vill probably be lost
sendToDevice("V\n", 2);
printf("Waiting for reply ...\n");
Wait(0.1);
// request again
sendToDevice("V\n", 2);
// wait for reply
i = 0;
unlock();
while (strlen(getName()) < 10)
{ // wait for reply
Wait(0.1);
i++;
if (i > 20)
break;
}
printf("Finished after <= %g secs\n", 0.1 * i);
closePort();
}
return getName();
}
void ULaserDevice::createBaseVars()
{
double d;
if (vars != NULL)
{
var.maxRange = vars->addVar("maxRange", maxValidRange, "d", "(r) Max valid range for device");
var.name = vars->addVarA("name", name, "s", "(r) device name");
var.versionInfo = vars->addVarA("versionInfo", name, "s", "(r) version info from device (if available)");
var.scanres = vars->addVar("scanRes", angleResolution, "d", "(r) resoulution in degrees between scans.");
var.scanwidth = vars->addVar("scanWidth", modeAngleScan, "d", "(r) width of scan - in xy plane symmetric around x-axis");
var.serial = vars->addVar("scannumber", serial, "d", "(r) next unused scan number");
var.type = vars->addVarA("device", devName, "s", "(r) device source name");
var.pose = vars->addVarA("pose", "0 0 0 0 0 0", "6d", "(r/w) pose (x,y,z,a,p,k) relative to robot. 2D pose is (x,y,k)");
d = isPortOpen();
var.isOpen = vars->addVar("isOpen", d, "d", "(r) is device open (delivers data)");
var.framerate = vars->addVar("framerate", 1.0, "d", "(r) delivered framerate (Hz) - may not work for all devices");
var.scanDelay = vars->addVar("scanDelay", 0.0, "d", "(r/w) Delay time from laser scan to timestamp (approx 1/(2 * framerate))");
}
}
bool UHokuyo::receiveData()
{
bool gotData = false;
int n, i = 0;
char * dend = NULL;
UTime t;
bool gotRngData = false;
const int MSL = 500;
char s[MSL];
//
if (modeSimulated)
{
//gotData = receiveSimulatedData(&length);
Wait(0.1);
}
else
{
if (dataCnt > 0)
{ // test for old data
if (dataTime.getTimePassed() > 1.0)
{
printf("Discarded '%s'\n", dataBuf);
for (n = 0; n < dataCnt; n++)
printf("%02x,", dataBuf[n]);
printf("\n");
printf("Discarded %d bytes of old data\n", dataCnt);
statBadCnt++;
dataCnt = 0;
}
}
lock();
if (isPortOpen())
{
// set timeout to ~30 ms
n = receiveFromDevice(&dataBuf[dataCnt], MAX_DATA_LNG - dataCnt - 1, 0.015);
if (/*repeatGetScan and*/ (n > 0))
{ // request more data
requestMoreData();
}
//if (n > 0)
// printf("(%d bytes)", n);
}
else
n = 0;
unlock();
if (n > 0)
{
dataCnt += n;
dataBuf[dataCnt] = '\0';
dend = strstr(dataBuf, "\n\n");
// debug
/* if (verbose and (datalog != NULL))
{ // log the received part of the buffer
dataBuf[n] = '\0';
fprintf(datalog, "%4d (n=%3d) got:'%s'\n", dataCnt, n, dataBuf);
}*/
// debug end
gotData = (dend != NULL);
}
}
//
while (gotData)
{ // message length
badSeries++;
n = dend - dataBuf;
// debug
if (n > dataCnt)
{
printf("UHokuyo::receiveData message end found after buffer end? n=%d dataCnt=%d\n", n, dataCnt);
}
if (verbose)
{
snprintf(s, MSL, "In %d 'gotData' dataCnt=%d n=%d %c%c%c%c%c%c%c%c%c... after %g sec\n", i, dataCnt, n,
dataBuf[0], dataBuf[1], dataBuf[2],dataBuf[3], dataBuf[4], dataBuf[5], dataBuf[6], dataBuf[7], dataBuf[8],
dataTime.getTimePassed());
laslog.toLog("urg:", s);
}
//printf("msg:%s", dataBuf);
i++;
// debug end
switch (dataBuf[0])
{
case 'V':
dend[1] = '\0';
if (dataBuf[1] == '\n')
{ // some garbage may also start with a V
if (versionInfoCnt < 5 or verbose)
printf("Got version (%d) info ((grp %d) %d chars):'%s'\n", versionInfoCnt, i, dataCnt, dataBuf);
decodeName(dataBuf);
versionInfoCnt++;
if (verbose)
laslog.toLog(" - V:", &dataBuf[4]);
}
break;
case 'G':
if (dataBuf[10] != '0')
{ // must be '0 to be valid scandata
// printf("Bad data set laser may be off (bdSeries=%d) - send an on-command (\\nL1\\n)\n", badSeries);
sendToDevice("\nL1\n", 4);
break;
}
badSeries = 0;
if (lasData == NULL)
lasData = new ULaserData();
if (lasData != NULL)
{ // save data into decoded buffer
lasData->lock();
lasData->setDeviceNum(deviceNum);
gotRngData = decodeData(dataBuf, n, lasData);
lasData->setMaxValidRange(maxValidRange);
//if (repeatGetScan and not gotRngData)
//{ // error message - request new data
// // send request for more data right away
// lock();
// //sendToDevice("00038401\n", 9);
// //sendToDevice("00076801\n", 9);
// unlock();
//}
if (gotRngData)
statGoodCnt++;
lasData->setMirror(mirrorData);
lasData->unlock();
//
if (gotRngData)
// do pending push commands.
// No need to set data as locked, as
// a client scanGet may use data at the same time as a scanpush,
// as none of these will modify the 'lasData' structure.
gotNewScan(lasData);
if (verbose)
{ // debug logging
snprintf(s, MSL, "UHokuyo::receiveData: scan=%6s %drngs In %2d msgCnt=%4d/%4d : %c%c%c%c%c%c%c%c%c %c %c%c... after %g sec\n",
bool2str(gotRngData), lasData->getRangeCnt(), i, n, dataCnt,
dataBuf[0], dataBuf[1], dataBuf[2],dataBuf[3], dataBuf[4], dataBuf[5], dataBuf[6], dataBuf[7], dataBuf[8],
dataBuf[10], dataBuf[12], dataBuf[13],
dataTime.getTimePassed());
laslog.toLog(" - G:", s);
// to screen also
printf("%s", s);
}
dataTime.Now();
}
break;
default:
// got error
// discard to first newline
/* char * nl = strchr(dataBuf, '\n');
if (nl < dend and nl > dataBuf)
{ // newline found,
// set new data end (dend) to one earlier, as if it was
// a real "\n\n" command termination.
// this is to avoid a situation where OK scans
// are part of an error-respond.
dend = --nl;
}*/
// close string and restart
dend[1] = '\0';
// debug
if (verbose)
{
snprintf(s, MSL, "UHokuyo::receiveData: garbage msg %4d of %4d chars in buffer:'%c%c%c...'\n", n, dataCnt, dataBuf[0], dataBuf[1], dataBuf[2]);
laslog.toLog(" - ", s);
}
//printf("UHokuyo::receiveData: ignored garbage (%d chars):'%s'\n", dataCnt, dataBuf);
//rp = true; //repeatGetScan;
// no other than G???... should be
// received in repeat mode, well
// sometimes a LFLF is in the error message
//repeatGetScan = false;
/*closePort();
printf("Got unknown garbage:--------Closing port\n");
Wait(0.2);
if (rp)
{ // repeat is enabled - request dummy data
printf("-------------restarting in repeat mode\n");
getNewestData(NULL, 0, false);
//repeatGetScan = true;
}*/
statBadCnt++;
break;
}
// discard the used (or unknown) data
n = dataCnt - (dend - dataBuf) - 2;
memmove(dataBuf, dend + 2, n);
// reduce available data count
dataCnt = n;
dataBuf[dataCnt] = '\0';
// test for more data in buffer
gotData = dataCnt > 3;
if (gotData)
{ // is it a full message
dend = strstr(dataBuf, "\n\n");
gotData = (dend != NULL);
//printf("****Got more messages in one read (OK, but indicate latency)\n");
}
}
return gotRngData;
}
bool UHokuyo::getNewestData(ULaserData * dest,
unsigned long lastSerial,
int fake)
{
bool result = false;
UTime t;
bool isOK;
//
result = (dest != NULL);
if (result)
{
if (fake > 0)
getFakeScan( dest, lastSerial, fake);
else
{ // live data
if (not isPortOpen())
{ // open port (in get scan mode)
// and start read thread
start();
if (isPortOpen())
{ // wait for port to open
printf("Opening port for scan ...\n");
Wait(0.1);
lock();
sendToDevice("\nV\n", 3);
//isOK = sendToDevice("\nG04572501\n", 11);
unlock();
//printf("Waiting for reply 1 to [G38438501\\n] ...\n");
Wait(0.1);
lock();
isOK = sendToDevice("\nG04572501\n", 11);
unlock();
//printf("Waiting for reply 2 to [G38438501\\n] ...\n");
Wait(0.3);
if (isOK)
printf("Opening port for scan ... is OK\n");
badSeries = 0;
versionInfoCnt = 0;
}
}
if (isPortOpen())
{ // get a full scan of data
t = dataRxTime;
}
result = (lasData != NULL) and (dest != NULL);
if (result)
{
lasData->lock();
//printf("Last serial = %lu, got data fromserial %lu, valid=%s\n",
// lastSerial, lasData->getSerial(), bool2str(lasData->isValid()));
result = lasData->isValid() and // and ((lasData->getScanTime() - t) > 0.001);
(lasData->getSerial() > lastSerial);
if (result)
{
dest->copy(lasData);
// save to logfile (if open)
if (datalogUsedScans)
logThisScan(lasData);
}
else
dest->setValid(false);
lasData->unlock();
}
else if (dest != NULL)
dest->setValid(false);
}
if (result)
dest->setDeviceNum( deviceNum);
}
return result;
}
