bool REIXSRIXSScanConfigurationView::dropScanURLs(const QList<QUrl>& urls) {
if( !urls.count() )
return false;
bool accepted = false;
foreach(QUrl url, urls) {
bool isScanning;
QString scanName;
AMScan* scan = AMScan::createFromDatabaseUrl(url, false, &isScanning, &scanName);
// See if this scan is acquiring, and refuse to create a new instance if so.
/// \todo With the new AMScan memory management model, we could actually open the existing AMScan* instance in multiple editors if desired... But propagation of changes under editing might be a problem, all editors currently assuming they are the only ones modifying the scan.
if(isScanning) {
QMessageBox stillScanningEnquiry;
stillScanningEnquiry.setWindowTitle("This scan is still acquiring.");
stillScanningEnquiry.setText(QString("The scan '%1' is currently still acquiring data, so you can't open multiple copies of it yet.")
.arg(scanName));
stillScanningEnquiry.setIcon(QMessageBox::Information);
stillScanningEnquiry.addButton(QMessageBox::Ok);
stillScanningEnquiry.exec();
continue;
}
if(!scan)
continue;
if(scan->type().compare("AMXASScan")) //.compare() returns 0 if same
continue;
// success!
addScan(scan);
accepted = true;
}
void
SlamKarto::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
laser_count_++;
if ((laser_count_ % throttle_scans_) != 0) //1 激光频率问题
return;
static ros::Time last_map_update(0,0);
// Check whether we know about this laser yet // if New laser, need to create a Karto device for it.
karto::LaserRangeFinder* laser = getLaser(scan);
if(!laser)
{
ROS_WARN("Failed to create laser device for %s; discarding scan",
scan->header.frame_id.c_str());
return;
}
if(debug_print_flag_)
debugPrint_<<"1.0 laser_count_: "<<laser_count_<<" addScan "<<" "<<endl;
karto::Pose2 odom_pose;
if(addScan(laser, scan, odom_pose))
{
ROS_DEBUG("added scan at pose: %.3f %.3f %.3f",
odom_pose.GetX(),
odom_pose.GetY(),
odom_pose.GetHeading());
ROS_INFO("added scan at pose: %.3f %.3f %.3f",
odom_pose.GetX(),
odom_pose.GetY(),
odom_pose.GetHeading());
geometry_msgs::Point debug_pose;
debug_pose.x = odom_pose.GetX();
debug_pose.y = odom_pose.GetY();
debug_pose.z = odom_pose.GetHeading();
link_node_publisher_.publish(debug_pose);
if(debug_print_flag_)
debugPrint_<<"2.0 odom_pose: "<<odom_pose.GetX()<<" "<<odom_pose.GetY()<<" "<<odom_pose.GetHeading()<<endl;
publishGraphVisualization();
if(!got_map_ ||
(scan->header.stamp - last_map_update) > map_update_interval_)
{
if(updateMap())
{
last_map_update = scan->header.stamp;
got_map_ = true;
ROS_DEBUG("Updated the map");
if(debug_print_flag_)
debugPrint_<<"3.0 updateMap: "<<endl;
}
}
}
}
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
SlamKarto::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
laser_count_++;
if ((laser_count_ % throttle_scans_) != 0)
return;
static ros::Time last_map_update(0,0);
// Check whether we know about this laser yet
karto::LaserRangeFinder* laser = getLaser(scan);
if(!laser)
{
ROS_WARN("Failed to create laser device for %s; discarding scan",
scan->header.frame_id.c_str());
return;
}
karto::Pose2 odom_pose;
if(addScan(laser, scan, odom_pose))
{
ROS_DEBUG("added scan at pose: %.3f %.3f %.3f",
odom_pose.GetX(),
odom_pose.GetY(),
odom_pose.GetHeading());
publishGraphVisualization();
if(!got_map_ ||
(scan->header.stamp - last_map_update) > map_update_interval_)
{
if(updateMap())
{
last_map_update = scan->header.stamp;
got_map_ = true;
ROS_DEBUG("Updated the map");
}
}
}
}
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");
}
}
}
