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;
            }
        }
    }
}
Beispiel #3
0
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");
		}

     }
}