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
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");
}
}
}
}