karto探秘之slam_karto

最新推荐文章于 2022-12-08 14:48:13 发布

原创

最新推荐文章于 2022-12-08 14:48:13 发布 · 3.2k 阅读

标签

#slam #karto

本文深入解析slam_karto雷达SLAM系统的工作原理，重点介绍了laserCallback、updateMap、getOdomPose、addScan等关键函数的作用与实现细节。探讨了雷达数据处理流程，包括雷达坐标系校正、地图更新机制及雷达数据融合，适用于理解机器人定位与地图构建。

slam_karto是open_karto的ROS的封装。

1 laserCallback()

直接看雷达消息的回调函数，调用了 addScan（）与 updateMap()。

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

  // addScan()根据当前的雷达数据，返回当前的base_link在odom下的坐标 odom_pose
  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");
      }
    }
  }
}

2 updateMap()

updateMap()方法就是更新ros的地图，每个栅格都需要重新生成。

通过mapper中所有的雷达数据进行karto的占用栅格地图的构建，这个是每次雷达数据的到来都会重新生成一遍这个地图。

之后将karto的栅格地图转换成ros的地图，转换时分别填入 -1，0, 100 三种数值。

bool
SlamKarto::updateMap()
{
  boost::mutex::scoped_lock lock(map_mutex_);

  karto::OccupancyGrid* occ_grid = 
          karto::OccupancyGrid::CreateFromScans(mapper_->GetAllProcessedScans(), resolution_);

  if(!occ_grid)
    return false;

  if(!got_map_) {
    map_.map.info.resolution = resolution_;
    map_.map.info.origin.position.x = 0.0;
    map_.map.info.origin.position.y = 0.0;
    map_.map.info.origin.position.z = 0.0;
    map_.map.info.origin.orientation.x = 0.0;
    map_.map.info.origin.orientation.y = 0.0;
    ma

最低0.47元/天解锁文章