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