AREXPORT void ArLaser::laserConnect(void)
{
// figure out how many readings we can have and set the current
// buffer size to that
double degrees;
myLastReading.setToNow();
if (canSetDegrees())
{
//degrees = fabs(ArMath::subAngle(getStartDegrees(), getEndDegrees()));
degrees = fabs(getStartDegrees() - getEndDegrees());
ArLog::log(myInfoLogLevel,
"%s: Using degrees settings of %g to %g for %g degrees",
getName(), getStartDegrees(), getEndDegrees(),
degrees);
}
else if (canChooseDegrees())
{
degrees = getDegreesChoiceDouble();
ArLog::log(myInfoLogLevel, "%s: Using choice of %g degrees",
getName(), degrees);
}
else
{
degrees = 360;
ArLog::log(ArLog::Terse, "%s: Don't have any settings for degrees, arbitrarily using 360", getName());
}
double increment;
if (canSetIncrement())
{
increment = getIncrement();
ArLog::log(myInfoLogLevel, "%s: Using increment setting of %g degrees",
getName(), increment);
}
else if (canChooseIncrement())
{
increment = getIncrementChoiceDouble();
ArLog::log(myInfoLogLevel, "%s: Using increment setting of %g degrees",
getName(), increment);
}
else
{
// PS 10/20/11 - This was missing causing buffer size to be very large
// set this to the lowest, note both the SZ and S3 are setting the buffer
// size but it's being overriden by this procedure - do we want to fix
// this or just leave it at the max value 360/.25=1440???
increment = .25;
ArLog::log(ArLog::Terse, "%s: Don't have any settings for increment, arbitrarily using .25", getName());
}
int size = (int)ceil(degrees / increment) + 1;
ArLog::log(myInfoLogLevel, "%s: Setting current buffer size to %d",
getName(), size);
setCurrentBufferSize(size);
ArLog::log(myInfoLogLevel, "%s: Connected", getName());
myConnectCBList.invoke();
}
AREXPORT ArSick::ArSick(
size_t currentBufferSize, size_t cumulativeBufferSize,
const char *name, bool addAriaExitCB, bool isSecondLaser) :
ArLMS2xx(!isSecondLaser ? 1 : 2, name, !isSecondLaser ? false : true)
{
setCurrentBufferSize(currentBufferSize);
setCumulativeBufferSize(cumulativeBufferSize);
}
AREXPORT void ArLaser::laserConnect(void)
{
// figure out how many readings we can have and set the current
// buffer size to that
double degrees;
myLastReading.setToNow();
if (canSetDegrees())
{
degrees = fabs(getStartDegrees() - getEndDegrees());
ArLog::log(myInfoLogLevel,
"%s: Using degrees settings of %g to %g for %g degrees",
getName(), getStartDegrees(), getEndDegrees(),
degrees);
}
else if (canChooseDegrees())
{
degrees = getDegreesChoiceDouble();
ArLog::log(myInfoLogLevel, "%s: Using choice of %g degrees",
getName(), degrees);
}
else
{
degrees = 360;
ArLog::log(ArLog::Terse, "%s: Don't have any settings for degrees, arbitrarily using 360", getName());
}
double increment;
if (canSetIncrement())
{
increment = getIncrement();
ArLog::log(myInfoLogLevel, "%s: Using increment setting of %g degrees",
getName(), increment);
}
else if (canChooseIncrement())
{
increment = getIncrementChoiceDouble();
ArLog::log(myInfoLogLevel, "%s: Using increment setting of %g degrees",
getName(), increment);
}
else
{
ArLog::log(ArLog::Terse, "%s: Don't have any settings for degrees, arbitrarily using 1000 for current buffer size", getName());
}
int size = (int)ceil(degrees / increment) + 1;
ArLog::log(myInfoLogLevel, "%s: Setting current buffer size to %d",
getName(), size);
setCurrentBufferSize(size);
ArLog::log(myInfoLogLevel, "%s: Connected", getName());
myConnectCBList.invoke();
}
AREXPORT bool ArSZSeries::blockingConnect(void) {
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;
}
// myPrevSensorIntTime = myConn->getTimeRead(0);
// PS 9/9/11 - moved this here to fix issue with setting baud in mt400.p
laserPullUnsetParamsFromRobot();
laserCheckParams();
// PS 9/9/11 - add setting baud
ArSerialConnection *serConn = NULL;
serConn = dynamic_cast<ArSerialConnection *>(myConn);
if (serConn != NULL)
serConn->setBaud(atoi(getStartingBaudChoice()));
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;
}
// PS - set logging level and laser type in packet receiver class
myReceiver.setmyInfoLogLevel(myInfoLogLevel);
myReceiver.setmyName(getName());
myReceiver.setDeviceConnection(myConn);
myConnMutex.unlock();
lockDevice();
myTryingToConnect = true;
unlockDevice();
// PS 9/9/11 - moved up top
//laserPullUnsetParamsFromRobot();
//laserCheckParams();
int size = ArMath::roundInt((270/.3) + 1);
ArLog::log(myInfoLogLevel,
"%s::blockingConnect() Setting current buffer size to %d",
getName(), size);
setCurrentBufferSize(size);
ArTime timeDone;
if (myPowerControlled)
{
if (!timeDone.addMSec(60 * 1000))
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect() error adding msecs (60 * 1000)",
getName());
}
}
else
{
if (!timeDone.addMSec(30 * 1000))
{
ArLog::log(ArLog::Normal,
"%s::blockingConnect() error adding msecs (30 * 1000)",
getName());
}
}
ArSZSeriesPacket *packet;
ArSZSeriesPacket sendPacket;
#if 0
sendPacket.empty();
sendPacket.uByteToBuf(0xA0); // stop continous sending
sendPacket.uByteToBuf(0x00);
sendPacket.uByteToBuf(0x1D);
sendPacket.uByteToBuf(0x7E);
sendPacket.finalizePacket();
if ((myConn->write(sendPacket.getBuf(), sendPacket.getLength())) == -1)
{
ArLog::log(ArLog::Terse,
"%s::blockingConnect() Could not send Stop Continuous mode to laser", getName());
failedToConnect();
return false;
}
#endif
// Build the Start Continuous sending packet and set it
// once we get a response, then we are connected, note
// the response needs to be a real reading
sendPacket.empty();
// command id = 0x91
//sendPacket.uByteToBuf(145);
sendPacket.uByteToBuf(0x91);
// note communication ID default is 0
// this value is set via the SZ Configurator
// ???? not sure what to do if it fails
// and put in CRC - from manual, this is
// specific to the communication ID = 0
sendPacket.uByteToBuf(0);
sendPacket.uByteToBuf(43);
sendPacket.uByteToBuf(218);
unsigned short crc = myReceiver.CRC16((unsigned char *)sendPacket.getBuf(), 2);
#if 0
// other communications IDs and CRC
// communication ID =1
sendPacket.uByteToBuf(1);
sendPacket.uByteToBuf(59);
sendPacket.uByteToBuf(251);
// communication ID =2
sendPacket.uByteToBuf(2);
sendPacket.uByteToBuf(11);
sendPacket.uByteToBuf(152);
// communication ID =3
sendPacket.uByteToBuf(3);
sendPacket.uByteToBuf(27);
sendPacket.uByteToBuf(185);
#endif
sendPacket.finalizePacket();
IFDEBUG(
int i;
char x[100000];
printf("buffer with len = %d: ",sendPacket.getLength());
for (i = 0;i < sendPacket.getLength();i++)
{
printf("0x%x ",sendPacket.getBuf()[i] & 0xff);
//sprintf(&x[i], "%2x", (char *)sendPacket.getBuf()[i]);
}
printf("\n");
//ArLog::log(ArLog::Terse,
// "%s::blockingConnect() write Buffer = %s", getName(), x);
); // end IFDEBUG
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;
}
AREXPORT ArLaserFilter::ArLaserFilter(
ArLaser *laser, const char *name) :
ArLaser(laser->getLaserNumber(),
name != NULL && name[0] != '\0' ? name : laser->getName(),
laser->getAbsoluteMaxRange(),
laser->isLocationDependent(),
false),
myProcessCB(this, &ArLaserFilter::processReadings)
{
myLaser = laser;
if (name == NULL || name[0] == '\0')
{
std::string filteredName;
filteredName = "filtered_";
filteredName += laser->getName();
laserSetName(filteredName.c_str());
}
myRawReadings = new std::list<ArSensorReading *>;
char buf[1024];
sprintf(buf, "%sProcessCB", getName());
myProcessCB.setName(buf);
myAngleToCheck = 1;
myAnyFactor = -1;
myAllFactor = -1;
myAnyMinRange = -1;
myAnyMinRangeLessThanAngle = -180;
myAnyMinRangeGreaterThanAngle = 180;
setCurrentDrawingData(
new ArDrawingData(*(myLaser->getCurrentDrawingData())),
true);
setCumulativeDrawingData(
new ArDrawingData(*(myLaser->getCumulativeDrawingData())),
true);
// laser parameters
setInfoLogLevel(myLaser->getInfoLogLevel());
setConnectionTimeoutSeconds(myLaser->getConnectionTimeoutSeconds());
setCurrentBufferSize(myLaser->getCurrentBufferSize());
setCumulativeBufferSize(myLaser->getCumulativeBufferSize());
setCumulativeCleanDist(myLaser->getCumulativeCleanDist());
setCumulativeCleanInterval(myLaser->getCumulativeCleanInterval());
setCumulativeCleanOffset(myLaser->getCumulativeCleanOffset());
setSensorPosition(myLaser->getSensorPosition(),
myLaser->getSensorPositionZ());
laserSetAbsoluteMaxRange(myLaser->getAbsoluteMaxRange());
// set our max range to the laser we're filtering... then set the
// max range on the laser we're filtering to 0, so that it's over
// max range values don't get set to ignore, since then we can't
// clear cumulatives beyond that value
setMaxRange(myLaser->getMaxRange());
myLaser->setMaxRange(0);
// base range device parameters
setMaxSecondsToKeepCurrent(myLaser->getMaxSecondsToKeepCurrent());
setMinDistBetweenCurrent(getMinDistBetweenCurrent());
setMaxSecondsToKeepCumulative(myLaser->getMaxSecondsToKeepCumulative());
setMaxDistToKeepCumulative(myLaser->getMaxDistToKeepCumulative());
setMinDistBetweenCumulative(myLaser->getMinDistBetweenCumulative());
setMaxInsertDistCumulative(myLaser->getMaxInsertDistCumulative());
setCurrentDrawingData(myLaser->getCurrentDrawingData(), false);
setCumulativeDrawingData(myLaser->getCumulativeDrawingData(), false);
// turn off the cumulative buffer on the original to save CPU
myLaser->setCumulativeBufferSize(0);
// now all the specific laser settings (this should already be taken
// care of when this is created, but the code existed for the
// simulated laser so I put it here too)
if (myLaser->canSetDegrees())
laserAllowSetDegrees(
myLaser->getStartDegrees(), myLaser->getStartDegreesMin(),
myLaser->getStartDegreesMax(), myLaser->getEndDegrees(),
myLaser->getEndDegreesMin(), myLaser->getEndDegreesMax());
if (myLaser->canChooseDegrees())
laserAllowDegreesChoices(myLaser->getDegreesChoice(),
myLaser->getDegreesChoicesMap());
if (myLaser->canSetIncrement())
laserAllowSetIncrement(myLaser->getIncrement(),
myLaser->getIncrementMin(),
myLaser->getIncrementMax());
if (myLaser->canChooseIncrement())
laserAllowIncrementChoices(myLaser->getIncrementChoice(),
myLaser->getIncrementChoicesMap());
if (myLaser->canChooseUnits())
laserAllowUnitsChoices(myLaser->getUnitsChoice(),
myLaser->getUnitsChoices());
if (myLaser->canChooseReflectorBits())
laserAllowReflectorBitsChoices(myLaser->getReflectorBitsChoice(),
myLaser->getReflectorBitsChoices());
if (canSetPowerControlled())
laserAllowSetPowerControlled(myLaser->getPowerControlled());
if (myLaser->canChooseStartingBaud())
laserAllowStartingBaudChoices(myLaser->getStartingBaudChoice(),
myLaser->getStartingBaudChoices());
if (myLaser->canChooseAutoBaud())
laserAllowAutoBaudChoices(myLaser->getAutoBaudChoice(),
myLaser->getAutoBaudChoices());
laserSetDefaultTcpPort(myLaser->getDefaultTcpPort());
laserSetDefaultPortType(myLaser->getDefaultPortType());
}