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;
            }
        }
    }
}
Пример #2
0
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");
            }
        }
    }
}