void LaserScanMatcher::scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_msg)
{
// **** if first scan, cache the tf from base to the scanner
if (!initialized_)
{
createCache(scan_msg); // caches the sin and cos of all angles
// cache the static tf from base to laser
if (!getBaseToLaserTf(scan_msg->header.frame_id))
{
ROS_INFO("FRAME_ID : %s",scan_msg->header.frame_id.c_str());
ROS_WARN("Skipping laser scan");
return;
}
laserScanToLDP(scan_msg, prev_ldp_scan_);
last_icp_time_ = scan_msg->header.stamp;
initialized_ = true;
}
LDP curr_ldp_scan;
laserScanToLDP(scan_msg, curr_ldp_scan);
processScan(curr_ldp_scan, scan_msg->header.stamp);
}
void LaserScanMatcher::cloudCallback (const PointCloudT::ConstPtr& cloud)
{
// **** if first scan, cache the tf from base to the scanner
std_msgs::Header cloud_header = pcl_conversions::fromPCL(cloud->header);
if (!initialized_)
{
// cache the static tf from base to laser
if (!getBaseToLaserTf(cloud_header.frame_id))
{
ROS_WARN("Skipping cloud scan");
return;
}
PointCloudToLDP(cloud, prev_ldp_scan_);
last_icp_time_ = cloud_header.stamp;
initialized_ = true;
}
LDP curr_ldp_scan;
PointCloudToLDP(cloud, curr_ldp_scan);
processScan(curr_ldp_scan, cloud_header.stamp);
}
void image::decodeScan()
{
int errNo;
int srcCurPos;
FwiDecodeHuffmanState* pDecHuffState;
signed short int lastDc[4] = {0,0,0}; // we support only four components now...
int marker = 0;
bool eoiFound = false;
processScan();
errNo = fwiDecodeHuffmanStateInitAlloc_JPEG_8u(&pDecHuffState);
printError((errNo!=0),"\nerror: in fwiDecodeHuffmanStateInitAlloc_JPEG_8u\n");
srcCurPos = (int)image::curIndex;
int tableSelector = 0;
Fw16s pDataUnit[64];
int unSampledDstStep;
Fw8u *pTemporaryDst2[4];
// TODO: can find this at the time of reading the header....
int maxHorizontalSamplingFactor = 0;
int maxVerticalSamplingFactor = 0;
int noOfDataUnitsInMCU = 0;
int noOfMCUforComponent = 0;
noOfComponents = SOF0::noOfComponents;
samplesPerLine = SOF0::samplesPerLine;
noOfLines = SOF0::noOfLines;
for(int i=0; i<noOfComponents;i++)
{
componentData[i].horizontalSamplingFactor = SOF0::componentData[i].horizontalSamplingFactor;
componentData[i].verticalSamplingFactor = SOF0::componentData[i].verticalSamplingFactor;
componentData[i].componentId = SOF0::componentData[i].componentId;
componentData[i].DQTTableSelector = SOF0::componentData[i].DQTTableSelector;
componentData[i].noOfMCUsForComponent = componentData[i].horizontalSamplingFactor*
componentData[i].verticalSamplingFactor;
}
for(int i=0; i<noOfComponents;i++)
{
if(componentData[i].horizontalSamplingFactor > maxHorizontalSamplingFactor)
maxHorizontalSamplingFactor = componentData[i].horizontalSamplingFactor;
if(componentData[i].verticalSamplingFactor > maxVerticalSamplingFactor)
maxVerticalSamplingFactor = componentData[i].verticalSamplingFactor;
noOfDataUnitsInMCU = noOfDataUnitsInMCU + componentData[i].horizontalSamplingFactor * componentData[i].verticalSamplingFactor;
}
mcuWidth = 8 * maxHorizontalSamplingFactor; // right now we support only lossy JPEG ..so 8 pixels
mcuHeight = 8 * maxVerticalSamplingFactor;
//TODO: calculate xPadding and yPadding here...
noOfxMCU = (samplesPerLine + mcuWidth - 1)/ mcuWidth;
noOfyMCU = (noOfLines + mcuHeight - 1) / mcuHeight;
for(int i=0; i<noOfComponents;i++)
{
unSampledDstStep = noOfxMCU*componentData[i].horizontalSamplingFactor*8;
pMCUdst[i] = (Fw16s*)malloc(unSampledDstStep*componentData[i].verticalSamplingFactor*8 * noOfyMCU * 2/*sizeof(Fw16u)*/);
pTemporaryDst2[i] = (Fw8u*)malloc(unSampledDstStep*componentData[i].verticalSamplingFactor*8 * noOfyMCU * 1/*sizeof(Fw8u)*/);
}
/*Data for kernel processing */
imageWidth = noOfxMCU * mcuWidth;
imageHeight = noOfyMCU * mcuHeight;
// get all MCU's..
for(int yMCU=0;yMCU<noOfyMCU;yMCU++)
// get one MCU row..
{
for(int xMCU=0;xMCU<noOfxMCU;xMCU++)
// get one MCU..
{
for(int i=0;i<noOfComponents;i++)
// TODO: the way we are referring componentData with the componentID may be wrong.. change it
{
for(unsigned int j=0;j<componentData[(component[i].ComponentId-1)].verticalSamplingFactor;j++)
{
for(unsigned int k=0;k<componentData[component[i].ComponentId-1].horizontalSamplingFactor;k++)
{
// TODO: if restart interval is set then calculate nomber of restarts required and after that many number of MCU's reset the decoder
marker = 0;
errNo = fwiDecodeHuffman8x8_JPEG_1u16s_C1(image::rawImageBuffer, (int)image::fileLength - 1, &srcCurPos, pDataUnit, &(lastDc[i]), &marker, pHuffDcTable[component[i].DCTableSelector], pHuffAcTable[component[i].ACTableSelector],pDecHuffState);
//printError((marker!=0),"\nerror: marker found while decoding..\n");
if(marker!=0)
{
//printError(1,"\nerror: marker is not zero\n");
}
if(errNo !=0)
{
printf("\ncaught");
}
printError((errNo!=0),"\nError : errNo != 0 in fwiDecodeHuffman8x8_JPEG_1u16s_C1\n");
image::moveCurIndex(srcCurPos - image::curIndex);
//copy to pTempDst
noOfMCUforComponent = componentData[(component[i].ComponentId-1)].verticalSamplingFactor *
componentData[component[i].ComponentId-1].horizontalSamplingFactor ;
for (int l=0;l<64;l++)
{
pMCUdst[i][yMCU * noOfxMCU *(noOfMCUforComponent *64) + (xMCU * noOfMCUforComponent *64 ) + ((j * componentData[component[i].ComponentId-1].horizontalSamplingFactor * 64) + k *64) + l] = pDataUnit[l];
}
}
}
}
}
}
return;
}
void Scanner::singleScan(int frame, float rotation, float frameRotation,
LocationMapper& leftLocMapper, LocationMapper& rightLocMapper, TimingStats * timingStats)
{
double time1 = GetTimeInSeconds();
m_turnTable->rotate(frameRotation);
timingStats->rotationTime += GetTimeInSeconds() - time1;
// Make sure the laser is off
m_laser->turnOff(Laser::ALL_LASERS);
// Take a picture with the laser off
time1 = GetTimeInSeconds();
acquireImage(&m_image1);
timingStats->imageAcquisitionTime += GetTimeInSeconds() - time1;
// If this is the first image, save it as a thumbnail
if (frame == 0)
{
std::string thumbnail = m_filename + ".png";
PixelLocationWriter imageWriter;
imageWriter.writeImage(m_image1, 128, 96, thumbnail.c_str());
}
// Scan with the Right laser
if (m_laserSelection == Laser::RIGHT_LASER || m_laserSelection == Laser::ALL_LASERS)
{
bool goodResult = false;
for (int iTry = 0; iTry < m_maxNumFrameRetries && ! goodResult; iTry++)
{
// Turn on the right laser
time1 = GetTimeInSeconds();
m_laser->turnOn(Laser::RIGHT_LASER);
timingStats->laserTime += GetTimeInSeconds() - time1;
// Take a picture with the right laser on
time1 = GetTimeInSeconds();
acquireImage(&m_image2);
timingStats->imageAcquisitionTime += GetTimeInSeconds() - time1;
// Turn off the right laser
time1 = GetTimeInSeconds();
m_laser->turnOff(Laser::RIGHT_LASER);
timingStats->laserTime += GetTimeInSeconds() - time1;
// Process the right laser results
goodResult = processScan(m_rightLaserResults, frame, rotation, rightLocMapper, Laser::RIGHT_LASER, m_firstRowRightLaserCol, timingStats);
// If it didn't succeed, take the first image again
if (!goodResult)
{
// Take a picture with the laser off
time1 = GetTimeInSeconds();
acquireImage(&m_image1);
timingStats->imageAcquisitionTime += GetTimeInSeconds() - time1;
}
}
}
// Scan with the Left laser
if (m_laserSelection == Laser::LEFT_LASER || m_laserSelection == Laser::ALL_LASERS)
{
bool goodResult = false;
for (int iTry = 0; iTry < m_maxNumFrameRetries && ! goodResult; iTry++)
{
// Turn on the left laser
time1 = GetTimeInSeconds();
m_laser->turnOn(Laser::LEFT_LASER);
timingStats->laserTime += GetTimeInSeconds() - time1;
// Take a picture with the left laser on
time1 = GetTimeInSeconds();
acquireImage(&m_image2);
timingStats->imageAcquisitionTime += GetTimeInSeconds() - time1;
// Turn off the left laser
time1 = GetTimeInSeconds();
m_laser->turnOff(Laser::LEFT_LASER);
timingStats->laserTime += GetTimeInSeconds() - time1;
// Process the left laser results
goodResult = processScan(m_leftLaserResults, frame, rotation, leftLocMapper, Laser::LEFT_LASER, m_firstRowLeftLaserCol, timingStats);
// If it didn't succeed, take the first image again
if (!goodResult)
{
// Take a picture with the laser off
time1 = GetTimeInSeconds();
acquireImage(&m_image1);
timingStats->imageAcquisitionTime += GetTimeInSeconds() - time1;
}
}
}
}
