本文整理汇总了C++中ArSensorReading::resetSensorPosition方法的典型用法代码示例。如果您正苦于以下问题：C++ ArSensorReading::resetSensorPosition方法的具体用法？C++ ArSensorReading::resetSensorPosition怎么用？C++ ArSensorReading::resetSensorPosition使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类ArSensorReading的用法示例。
在下文中一共展示了ArSensorReading::resetSensorPosition方法的4个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: setParamsBySteps
AREXPORT bool ArUrg::setParamsBySteps(int startingStep, int endingStep,
int clusterCount, bool flipped)
{
if (startingStep < 0 || startingStep > 768 ||
endingStep < 0 || endingStep > 768 ||
startingStep > endingStep ||
clusterCount < 1)
{
ArLog::log(ArLog::Normal,
"%s::setParamsBySteps: Bad params (starting %d ending %d clusterCount %d)",
getName(), startingStep, endingStep, clusterCount);
return false;
}
myDataMutex.lock();
myStartingStep = startingStep;
myEndingStep = endingStep;
myClusterCount = clusterCount;
myFlipped = flipped;
sprintf(myRequestString, "G%03d%03d%02d", myStartingStep, endingStep,
clusterCount);
myClusterMiddleAngle = 0;
if (myClusterCount > 1)
myClusterMiddleAngle = myClusterCount * 0.3515625 / 2.0;
if (myRawReadings != NULL)
{
ArUtil::deleteSet(myRawReadings->begin(), myRawReadings->end());
myRawReadings->clear();
delete myRawReadings;
myRawReadings = NULL;
}
myRawReadings = new std::list<ArSensorReading *>;
ArSensorReading *reading;
int onStep;
double angle;
for (onStep = myStartingStep;
onStep < myEndingStep;
onStep += myClusterCount)
{
/// FLIPPED
if (!myFlipped)
angle = ArMath::subAngle(ArMath::subAngle(135, onStep * 0.3515625),
myClusterMiddleAngle);
else
angle = ArMath::addAngle(ArMath::addAngle(-135, onStep * 0.3515625),
myClusterMiddleAngle);
reading = new ArSensorReading;
reading->resetSensorPosition(ArMath::roundInt(mySensorPose.getX()),
ArMath::roundInt(mySensorPose.getY()),
ArMath::addAngle(angle,
mySensorPose.getTh()));
myRawReadings->push_back(reading);
//printf("%.1f ", reading->getSensorTh());
}
myDataMutex.unlock();
return true;
}示例2: sensorInterp
//.........这里部分代码省略.........
continue;
}
// If we don't have any sensor readings created at all, make 'em all
if (myRawReadings->size() == 0) {
for (j = 0; j < eachNumberData; j++) {
myRawReadings->push_back(new ArSensorReading);
}
}
if (eachNumberData > myRawReadings->size())
{
ArLog::log(ArLog::Terse,
"%s::sensorInterp() Bad data, in theory have %d readings but can only have 751... skipping this packet",
getName(), eachNumberData);
// PS 12/6/12 - unlock and delete before continuing
delete packet;
myDataMutex.unlock();
unlockDevice();
continue;
}
std::list<ArSensorReading *>::iterator it;
double atDeg;
int onReading;
double start;
double increment;
eachAngularStepWidth = .36;
if (myFlipped) {
start = mySensorPose.getTh() + 135;
increment = -eachAngularStepWidth;
} else {
start = -(mySensorPose.getTh() + 135);
increment = eachAngularStepWidth;
}
int readingIndex;
bool ignore = false;
for (atDeg = start,
it = myRawReadings->begin(),
readingIndex = 0,
onReading = 0;
onReading < eachNumberData;
atDeg += increment,
it++,
readingIndex++,
onReading++)
{
reading = (*it);
dist = (((buf[readingIndex * 2] & 0x3f)<< 8) | (buf[(readingIndex * 2) + 1]));
// note max distance is 16383 mm, if the measurement
// object is not there, distance will still be 16383
/*
ArLog::log(ArLog::Normal,
"reading %d first half = 0x%x, second half = 0x%x dist = %d",
readingIndex, buf[(readingIndex *2)+1], buf[readingIndex], 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
//printf("dist = %d, pose = %d, encoderPose = %d, transform = %d, counter = %d, time = %d, igore = %d",
// dist, pose, encoderPose, transform, counter,
// time, ignore);
}
/*
ArLog::log(ArLog::Normal,
"Received: %s %s scan %d numReadings %d",
packet->getCommandType(), packet->getCommandName(),
myScanCounter, onReading);
*/
myDataMutex.unlock();
/*
ArLog::log(
ArLog::Terse,
"%s::sensorInterp() Telegram number = %d ",
getName(), packet->getTelegramNumByte2());
*/
laserProcessReadings();
unlockDevice();
delete packet;
}
}示例3: params
AREXPORT bool ArUrg_2_0::setParamsBySteps(int startingStep, int endingStep,
int clusterCount, bool flipped)
{
if (startingStep > endingStep || clusterCount < 1)
{
ArLog::log(ArLog::Normal,
"%s::setParamsBySteps: Bad params (starting %d ending %d clusterCount %d)",
getName(), startingStep, endingStep, clusterCount);
return false;
}
if (startingStep < myAMin)
startingStep = myAMin;
if (endingStep > myAMax)
endingStep = myAMax;
myDataMutex.lock();
myStartingStep = startingStep;
myEndingStep = endingStep;
myClusterCount = clusterCount;
myFlipped = flipped;
//sprintf(myRequestString, "G%03d%03d%02d", myStartingStep, endingStep,
//clusterCount);
int baudRate = 0;
ArSerialConnection *serConn = NULL;
serConn = dynamic_cast<ArSerialConnection *>(myConn);
if (serConn != NULL)
baudRate = serConn->getBaud();
// only use the three data bytes if our range needs it, and if the baud rate can support it
if (myMaxRange > 4095 && (baudRate == 0 || baudRate > 57600))
{
myUseThreeDataBytes = true;
sprintf(myRequestString, "MD%04d%04d%02d%01d%02d",
myStartingStep, myEndingStep, myClusterCount,
0, // scan interval
0 // number of scans to send (forever)
);
}
else
{
myUseThreeDataBytes = false;
if (myMaxRange > 4094)
myMaxRange = 4094;
sprintf(myRequestString, "MS%04d%04d%02d%01d%02d",
myStartingStep, myEndingStep, myClusterCount,
0, // scan interval
0 // number of scans to send (forever)
);
}
myClusterMiddleAngle = 0;
if (myClusterCount > 1)
//myClusterMiddleAngle = myClusterCount * 0.3515625 / 2.0;
myClusterMiddleAngle = myClusterCount * myStepSize / 2.0;
if (myRawReadings != NULL)
{
ArUtil::deleteSet(myRawReadings->begin(), myRawReadings->end());
myRawReadings->clear();
delete myRawReadings;
myRawReadings = NULL;
}
myRawReadings = new std::list<ArSensorReading *>;
ArSensorReading *reading;
int onStep;
double angle;
for (onStep = myStartingStep;
onStep < myEndingStep;
onStep += myClusterCount)
{
/// FLIPPED
if (!myFlipped)
//angle = ArMath::subAngle(ArMath::subAngle(135,
// onStep * 0.3515625),
angle = ArMath::subAngle(ArMath::subAngle(myStepFirst,
onStep * myStepSize),
myClusterMiddleAngle);
else
//angle = ArMath::addAngle(ArMath::addAngle(-135,
// onStep * 0.3515625),
angle = ArMath::addAngle(ArMath::addAngle(-myStepFirst,
onStep * myStepSize),
myClusterMiddleAngle);
reading = new ArSensorReading;
reading->resetSensorPosition(ArMath::roundInt(mySensorPose.getX()),
ArMath::roundInt(mySensorPose.getY()),
ArMath::addAngle(angle,
mySensorPose.getTh()));
myRawReadings->push_back(reading);
//printf("%.1f ", reading->getSensorTh());
}
//.........这里部分代码省略.........
示例4: if
//.........这里部分代码省略.........
*/
/*
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;
}
}