void ThreadLocalize::eventLoop(void)
{
_sleepCond.wait(_sleepMutex);
while(_stayActive)
{
if(!_laserData.size())
{
_sleepCond.wait(_sleepMutex);
}
_dataMutex.lock();
vector<float> ranges = _laserData.front()->ranges;
if(_reverseScan)
std::reverse(ranges.begin(),ranges.end());
_stampLaserOld = _stampLaser;
_stampLaser = _laserData.front()->header.stamp;
_sensor->setRealMeasurementData(ranges);
_sensor->setStandardMask();
for(std::deque<sensor_msgs::LaserScan*>::iterator iter = _laserData.begin(); iter < _laserData.end(); iter++)
delete *iter;
_laserData.clear();
_dataMutex.unlock();
//odom rescue
// if(_useOdomRescue)
// _odomAnalyzer->odomRescueUpdate();
//std::cout << __PRETTY_FUNCTION__ << "_odomTfIsValid = " << _odomTfIsValid << std::endl;
const unsigned int measurementSize = _sensor->getRealMeasurementSize();
if(!_scene) //first call, initialize buffers
{
_scene = new double[measurementSize * 2];
_maskS = new bool[measurementSize];
_modelCoords = new double[measurementSize * 2];
_modelNormals = new double[measurementSize * 2];
_maskM = new bool[measurementSize];
*_lastPose = _sensor->getTransformation();
}
//reconstruction
unsigned int validModelPoints = _rayCaster->calcCoordsFromCurrentViewMask(&_grid, _sensor, _modelCoords, _modelNormals, _maskM);
if(validModelPoints == 0)
{
ROS_ERROR_STREAM("Localizer (" << _nameSpace << ") error! Raycasting found no coordinates! \n");
continue;
}
//get current scan
const unsigned int validScenePoints = _sensor->dataToCartesianVectorMask(_scene, _maskS);
/**
* Create Point Matrices with structure [x1 y1; x2 y2; ..]
* M, N and S are matrices that preserve the ray model of a laser scanner
* Xvalid matrices are matrices that do not preserve the ray model but contain only valid points (mask applied)
* T transformation
*/
obvious::Matrix M(measurementSize, 2, _modelCoords);
obvious::Matrix N(measurementSize, 2, _modelNormals);
obvious::Matrix S(measurementSize, 2, _scene);
obvious::Matrix Mvalid = maskMatrix(&M, _maskM, measurementSize, validModelPoints);
obvious::Matrix Nvalid = maskMatrix(&N, _maskM, measurementSize, validModelPoints);
obvious::Matrix Svalid = maskMatrix(&S, _maskS, measurementSize, validScenePoints);
obvious::Matrix T(3,3);
T = doRegistration(_sensor, &M, &Mvalid, &N, &Nvalid, &S, &Svalid);
//analyze registration result
_tf.stamp_ = ros::Time::now();
const bool regErrorT = isRegistrationError(&T, _trnsMax, _rotMax);
if(regErrorT)
{
ROS_ERROR_STREAM("Localizer(" << _nameSpace << ") registration error! \n");
sendNanTransform();
}
else //transformation valid -> transform sensor and publish new sensor pose
{
// obvious::Matrix Tbla(3, 3);
// Tbla = this->tfToObviouslyMatrix3x3(_tfFrameSensorMount);
// std::cout << __PRETTY_FUNCTION__ << "tbla" << std::endl;
// Tbla.print();
// std::cout << std::endl;
//Tbla.invert();
//T = Tbla * T;
_sensor->transform(&T);
//_sensor->transform(&Tbla);
obvious::Matrix curPose = _sensor->getTransformation();
sendTransform(&curPose);
//update map if necessary
if(this->isPoseChangeSignificant(_lastPose, &curPose))
{
*_lastPose = curPose;
_mapper.queuePush(_sensor);
}
}
}
}
void odomCallback(nav_msgs::Odometry const &odometry) {
sendTransform(odometry.pose.pose, odometry.header, odometry.child_frame_id);
}
void poseCallback(geometry_msgs::PoseStamped const &pose) {
sendTransform(pose.pose, pose.header);
}
void TransformBroadcaster::sendTransform(const geometry_msgs::TransformStamped & msgtf)
{
std::vector<geometry_msgs::TransformStamped> v1;
v1.push_back(msgtf);
sendTransform(v1);
}
