int
hokuyo::Laser::sendCmd(const char* cmd, int timeout)
{
if (!portOpen())
HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
char buf[100];
//printf("sendreq: %s\n", cmd);
laserWrite(cmd);
laserWrite("\n");;
laserReadlineAfter(buf, 100, cmd, timeout);
laserReadline(buf,100,timeout);
//printf("chksum: %s",buf);
if (!checkSum(buf,4))
{
//printf("chksum error\n");
HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Checksum failed on status code.");
}
buf[2] = 0;
//printf("sendreq_end: %s\n", cmd);
if (buf[0] - '0' >= 0 && buf[0] - '0' <= 9 && buf[1] - '0' >= 0 && buf[1] - '0' <= 9)
return (buf[0] - '0')*10 + (buf[1] - '0');
else
HOKUYO_EXCEPT(hokuyo::Exception, "Hokuyo error code returned. Cmd: %s -- Error: %s", cmd, buf);
}
void
hokuyo::Laser::setToSCIP2()
{
if (!portOpen())
HOKUYO_EXCEPT(hokuyo::Exception, "Port not open.");
const char * cmd = "SCIP2.0";
char buf[100];
laserWrite(cmd);
laserWrite("\n");
laserReadline(buf, 100, 1000);
ROS_DEBUG("Laser comm protocol changed to %s \n", buf);
//printf ("Laser comm protocol changed to %s \n", buf);
}
