void
SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
/*
timeval t1,t2;
gettimeofday(&t1,NULL);*/
auto start = std::chrono::high_resolution_clock::now();
laser_count_++;
if ((laser_count_ % throttle_scans_) != 0)
return;
static ros::Time last_map_update(0,0);
// We can't initialize the mapper until we've got the first scan
if(!got_first_scan_)
{
if(!initMapper(*scan))
return;
got_first_scan_ = true;
}
GMapping::OrientedPoint odom_pose;
if(addScan(*scan, odom_pose))
{
ROS_DEBUG("scan processed");
GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;
ROS_DEBUG("new best pose: %.3f %.3f %.3f", mpose.x, mpose.y, mpose.theta);
ROS_DEBUG("odom pose: %.3f %.3f %.3f", odom_pose.x, odom_pose.y, odom_pose.theta);
ROS_DEBUG("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);
tf::Transform laser_to_map = tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose.theta), tf::Vector3(mpose.x, mpose.y, 0.0)).inverse();
tf::Transform odom_to_laser = tf::Transform(tf::createQuaternionFromRPY(0, 0, odom_pose.theta), tf::Vector3(odom_pose.x, odom_pose.y, 0.0));
map_to_odom_mutex_.lock();
map_to_odom_ = (odom_to_laser * laser_to_map).inverse();
map_to_odom_mutex_.unlock();
if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_)
{
updateMap(*scan);
last_map_update = scan->header.stamp;
ROS_DEBUG("Updated the map");
}
} else
ROS_DEBUG("cannot process scan");
/*
gettimeofday(&t2,NULL);
double ms = (double)(t2.tv_usec - t1.tv_usec)/1000 + (double)(t2.tv_sec - t1.tv_sec);*/
auto end = std::chrono::high_resolution_clock::now();
double ms = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count() / 1000;
//std::cout<<"No."<<++update_count<<"update process takes "<<ms<<"ms"<<std::endl;
//printf("No. %d update process takes %f ms \n", ++update_count, ms);
ROS_INFO("No. %d update process takes %f ms", update_count, ms);
}
void
SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
laser_count_++;
if ((laser_count_ % throttle_scans_) != 0)
return;
static ros::Time last_map_update(0,0);
// We can't initialize the mapper until we've got the first scan
if(!got_first_scan_)
{
if(!initMapper(*scan))
return;
got_first_scan_ = true;
}
GMapping::OrientedPoint odom_pose;
if(addScan(*scan, odom_pose))
{
ROS_DEBUG("scan processed");
GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;
ROS_DEBUG("new best pose: %.3f %.3f %.3f", mpose.x, mpose.y, mpose.theta);
ROS_DEBUG("odom pose: %.3f %.3f %.3f", odom_pose.x, odom_pose.y, odom_pose.theta);
ROS_DEBUG("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);
tf::Transform laser_to_map = tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose.theta), tf::Vector3(mpose.x, mpose.y, 0.0)).inverse();
tf::Transform odom_to_laser = tf::Transform(tf::createQuaternionFromRPY(0, 0, odom_pose.theta), tf::Vector3(odom_pose.x, odom_pose.y, 0.0));
map_to_odom_mutex_.lock();
map_to_odom_ = (odom_to_laser * laser_to_map).inverse();
map_to_odom_mutex_.unlock();
if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_)
{
updateMap(*scan);
last_map_update = scan->header.stamp;
ROS_DEBUG("Updated the map");
}
}
}
void FrmFrameDetails::setFrameDetails(const Mode mode, const Persistence persistence, Sample* sample,
QList<int>& blackList, const Options options)
{
qApp->setOverrideCursor( QCursor(Qt::BusyCursor ) );
if (treeView==0) return;
if (blackList.size()>0)
treeView->setBlackList(blackList);
treeView->setSupportNewItems(options & FrmFrameDetails::ALLOW_NEW);
m_sample=sample;
m_mode=mode;
m_persistence=persistence;
m_verified=false;
m_submitted=false;
//Now fix the UI
lbPersistence->setText
(persistence==FrmFrameDetails::PERMANENT?tr("Permanent"):tr("Temporary"));
pushVerify->setEnabled(true);
pushApply->setEnabled(!pushVerify->isEnabled());
pushUndo->setEnabled(!pushVerify->isEnabled());
pushVerify->setVisible(mode!=FrmFrameDetails::VIEW || persistence==FrmFrameDetails::TEMPORARY);
pushApply->setVisible(mode!=FrmFrameDetails::VIEW || persistence==FrmFrameDetails::TEMPORARY);
pushUndo->setVisible(mode!=FrmFrameDetails::VIEW || persistence==FrmFrameDetails::TEMPORARY);
pushBack->setVisible(true);
lineName->clear();
textComments->clear();
textDesc->clear();
if (!initModel(mode,sample,options)){
qApp->setOverrideCursor( QCursor(Qt::ArrowCursor ) );
emit showError(tr("Could not create frame view!"));
return;
}
if (mode==FrmFrameDetails::VIEW){// read-only on Permanent mode
groupBox->setEnabled(false);
setTreeReadOnly(true);
horizontalLayout->addWidget(pushBack);
persistence==FrmFrameDetails::PERMANENT? horizontalLayout->removeWidget(pushVerify):horizontalLayout->addWidget(pushVerify);
persistence==FrmFrameDetails::PERMANENT? horizontalLayout->removeWidget(pushApply): horizontalLayout->addWidget(pushApply);
persistence==FrmFrameDetails::PERMANENT? horizontalLayout->removeWidget(pushUndo): horizontalLayout->addWidget(pushUndo);
persistence==FrmFrameDetails::PERMANENT?setTreeReadOnly(true):setTreeReadOnly(false);
initMapper();//TODO: maybe throw an error here later?
modelInterface->tRefFrame->setFilter(tr("Fr_Frame.ID=") + QVariant(sample->frameId).toString());
mapper->toLast();
}else{
if (!modelInterface->insertNewRecord(modelInterface->tRefFrame)){
qApp->setOverrideCursor( QCursor(Qt::ArrowCursor ) );
QString strErrors;
if (modelInterface->getErrors(strErrors))
emit showError(strErrors);
else
emit showError(tr("Could not insert new record!"));
}else{
initMapper();//TODO: maybe throw an error here later?
mapper->toLast();
if (mode==FrmFrameDetails::EDIT){
//set Frame Name
QSqlQuery query;
query.prepare(tr("SELECT dbo.FR_Frame.Name FROM dbo.FR_Frame ") +
tr("WHERE (dbo.FR_Frame.ID = ?)"));
query.addBindValue(sample->frameId);
if (!query.exec() || query.numRowsAffected()<1){
qApp->setOverrideCursor( QCursor(Qt::ArrowCursor ) );
if (query.lastError().type()!=QSqlError::NoError)
emit showError(query.lastError().text());
else
emit showError(tr("Could not retrieve the type of the cloned frame!"));
return;
}
query.first();
this->cmbCloned->setCurrentIndex(this->cmbCloned->findText(
query.value(0).toString()));
//Set src ID
query.prepare(tr("SELECT dbo.Ref_Source.Name FROM dbo.FR_Frame INNER JOIN ") +
tr("dbo.Ref_Source ON dbo.FR_Frame.id_source = dbo.Ref_Source.ID ") +
tr("WHERE (dbo.FR_Frame.ID = ?)"));
query.addBindValue(sample->frameId);
if (!query.exec() || query.numRowsAffected()<1){
qApp->setOverrideCursor( QCursor(Qt::ArrowCursor ) );
if (query.lastError().type()!=QSqlError::NoError)
emit showError(query.lastError().text());
else
emit showError(tr("Could not retrieve the type of the cloned frame!"));
return;
}
query.first();
this->cmbType->setCurrentIndex(this->cmbType->findText(query.value(0).toString()));
}else if (mode==FrmFrameDetails::CREATE)
this->cmbCloned->setCurrentIndex(this->cmbCloned->findText(
qApp->translate("null_replacements", strNa)));
emit showStatus(tr("Record successfully initialized!"));
}
groupBox->setEnabled(true);
setTreeReadOnly(false);
horizontalLayout->addWidget(pushVerify);
horizontalLayout->addWidget(pushApply);
horizontalLayout->addWidget(pushUndo);
horizontalLayout->addWidget(pushBack);
}
qApp->setOverrideCursor( QCursor(Qt::ArrowCursor ) );
}
void
SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
//BOOKMARK: write scan to file
#ifdef LOGTOFILE
writeLaserMsgtoFile(*scan);
#endif
laser_count_++;
if ((laser_count_ % throttle_scans_) != 0)
return;
static ros::Time last_map_update(0,0);
// We can't initialize the mapper until we've got the first scan
if(!got_first_scan_)
{
if(!initMapper(*scan))
return;
got_first_scan_ = true;
}
GMapping::OrientedPoint odom_pose;
if(addScan(*scan, odom_pose))
{
ROS_DEBUG("scan processed");
GMapping::OrientedPoint mpose = gsp_->getParticles()[gsp_->getBestParticleIndex()].pose;
ROS_DEBUG("new best pose: %.3f %.3f %.3f", mpose.x, mpose.y, mpose.theta);
ROS_DEBUG("odom pose: %.3f %.3f %.3f", odom_pose.x, odom_pose.y, odom_pose.theta);
ROS_DEBUG("correction: %.3f %.3f %.3f", mpose.x - odom_pose.x, mpose.y - odom_pose.y, mpose.theta - odom_pose.theta);
tf::Stamped<tf::Pose> odom_to_map;
try
{
tf_.transformPose(odom_frame_,tf::Stamped<tf::Pose> (tf::Transform(tf::createQuaternionFromRPY(0, 0, mpose.theta),
tf::Vector3(mpose.x, mpose.y, 0.0)).inverse(),
scan->header.stamp, base_frame_),odom_to_map);
}
catch(tf::TransformException e){
ROS_ERROR("Transform from base_link to odom failed\n");
odom_to_map.setIdentity();
}
#ifdef LOGTOFILE
writePoseStamped(odom_to_map);
#endif
map_to_odom_mutex_.lock();
map_to_odom_ = tf::Transform(tf::Quaternion( odom_to_map.getRotation() ),
tf::Point( odom_to_map.getOrigin() ) ).inverse();
map_to_odom_mutex_.unlock();
if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_)
{
updateMap(*scan);
last_map_update = scan->header.stamp;
ROS_DEBUG("Updated the map");
}
}
}
