kartoSLAM代码解读

在运行kartoslam的代码的时候，通过这里的教程
我们通过roslaunch turtlebot_navigation rplidar_karto_demo.launch运行，但是运行之后发生了什么呢?

---

rplidar_karto_demo.launch内容如下

<launch>
  <!-- Define laser type-->
  <arg name="laser_type" default="rplidar" />
  <!-- laser driver -->
  <include file="$(find turtlebot_navigation)/laser/driver/$(arg laser_type)_laser.launch" />
  <!-- karto.launch-->
  <arg name="custom_karto_launch_file" default="$(find turtlebot_navigation)/launch/includes/karto/$(arg laser_type)_karto.launch.xml"/>
  <include file="$(arg custom_karto_launch_file)"/>
  <!-- Move base -->
  <include file="$(find turtlebot_navigation)/launch/includes/move_base.launch.xml"/>
</launch>

可以知道我们定义参数laser_type为rplidar，这是因为我们使用的激光雷达就是rplidar，所以我们启动了rplidar_laser.launch这个node(驱动node)。
先不关心Movebase(导航用的node)是干嘛的，主要看一下karto.launch.xml

<launch>
  <node pkg="slam_karto" type="slam_karto" name="slam_karto" output="screen">
    <remap from="scan" to="scan"/>
    <param name="odom_frame" value="odom"/>
    <param name="map_update_interval" value="25"/>
    <param name="resolution" value="0.025"/>
  </node>
</launch>

可以看到，这里只启动了一个slam_karto的pkg(功能包)
slam_karto的pkg结构

asber@asber-X550VX:~/catkin_ws/src/slam_karto$ tree
.
├── CHANGELOG.rst
├── CMakeLists.txt
├── config
│   ├── base_local_planner_params.yaml
│   ├── costmap_common_params.yaml
│   ├── explore_costmap_params.yaml
│   ├── global_costmap_params.yaml
│   ├── local_costmap_params.yaml
│   ├── mapper_params.yaml
│   ├── move_base.xml
│   └── recovery_behaviors.yaml
├── launch
│   ├── build_map_w_params.launch
│   ├── karto_slam.launch
│   └── karto_stage.launch
├── maps
│   └── willow-full-0.05.pgm
├── package.xml
 |
├── src
│   ├── slam_karto.cpp
│   ├── spa_solver.cpp
│   └── spa_solver.h
└── worlds
    └── willow-pr2-5cm.world

其实slam_karto的pkg结构比较简单，主要就是src下的3个文件
我们先看slam_karto.cpp，这个文件里面定义了一个大类SlamKarto

class SlamKarto
{
  public:
    SlamKarto();
    ~SlamKarto();

    void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan);//得到一帧激光数据之后的回调函数
    bool mapCallback(nav_msgs::GetMap::Request  &req,//请求map地图的回调函数
                     nav_msgs::GetMap::Response &res);

  private:
    bool getOdomPose(karto::Pose2& karto_pose, const ros::Time& t);//处理odom的(应该是得到TF广播的odom)
    //检测scan是否重复，以及laser是否颠倒，计算laser pose（laser构成scan）
    karto::LaserRangeFinder* getLaser(const sensor_msgs::LaserScan::ConstPtr& scan);
    /*1、对于激光laser的输入数据scan，添加局部的scan 数据(range_scan)
      2、将局部数据range_scan 添加到地图中:processed = mapper_->Process(range_scan)以达到矫正位姿的效果；*/
    bool addScan(karto::LaserRangeFinder* laser,
                 const sensor_msgs::LaserScan::ConstPtr& scan,
                 karto::Pose2& karto_pose);
    bool updateMap();
    void publishTransform();//以互斥锁的形式发布TF变换
    void publishLoop(double transform_publish_period);//不断调用publishTransform发布TF变换的线程
    void publishGraphVisualization();//发布visualization_msgs::Marker的函数

大部分的函数定义就是这些
这里补充一点关于TF坐标系转换的

void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const

参数名字的定义对功能的说明还是很明显的，target_frame就是你要把源pose转换成哪个frame上的pose。假如你的源pose的frame_id是"odom"，你想转到"map"上，那么target_frame写成“map”就可以了。stamped_in就是源pose，而stamped_out就是目标数据了，也就是转换完成的数据。需要注意的是，从参数上来看，转换时是不需要指定源frame_id的，这是因为它已经包含在了stamped_in中，换句话说，就是这个函数一个隐含的使用条件是，stamped_in中必须指明源pose属于哪个frame。
我们经常看到会需要监听TFtopic得到坐标转换，以下代码就是这样

    std::string odom_frame_;
 初始化中出现     if(!private_nh_.getParam("odom_frame", odom_frame_))
    odom_frame_ = "odom";
    ---
bool
SlamKarto::getOdomPose(karto::Pose2& karto_pose, const ros::Time& t)
{
  // Get the robot's pose
  tf::Stamped<tf::Pose> ident (tf::Transform(tf::createQuaternionFromRPY(0,0,0),//首先建立一个初始零TF
                                           tf::Vector3(0,0,0)), t, base_frame_);
  tf::Stamped<tf::Transform> odom_pose;//
  try
  {
    tf_.transformPose(odom_frame_, ident, odom_pose);
  }
  catch(tf::TransformException e)
  {
    ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
    return false;
  }
  double yaw = tf::getYaw(odom_pose.getRotation());

  karto_pose = 
          karto::Pose2(odom_pose.getOrigin().x(),
                       odom_pose.getOrigin().y(),
                       yaw);
  return true;
}

那么我们就可以很清楚的知道
tf_.transformPose(odom_frame_, ident, odom_pose);
的意思就是获取需要将ident这个零向量转换到odom坐标系下，这里的话，就是不断的得到base_link->odom 的位置关系信息，即某时刻机器人在地图上的位置?
我感觉貌似我没有自己写一个node发送odom，虽然minimal.launch文件里面有odom的topic。(注意询问)
参考链接:ROS代码经验系列-- tf进行位置查询变换

---

以下是我注释的slam_karto.cpp
参考:阅读karto_slam源码笔记
参考中说到:

map:地图坐标系，顾名思义，一般设该坐标系为固定坐标系（fixed frame）
odom frame是对于机器人全局位姿的粗略估计。取名来源于odometry（里程计），一般这个坐标系的数据也是来源于里程计。如果你的odom计算没有错误，那么map–>odom的tf就是0.
个人理解：所以，map frame 和 odom frame 的误差可以看成是真实三维坐标系下和里程计测量之间的误差。
base_link: 一般位于tf tree的最根部，物理语义原点一般为表示机器人中心，为相对机器人的本体的坐标系。
————————————————
版权声明：本文为CSDN博主「Coolguyinhust」的原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/SoP_KaiXinHaHa/article/details/89606257

所以我认为这里的odom frame是根据其他非odom的topic计算出来的

/*
 * slam_karto
 * Copyright (c) 2008, Willow Garage, Inc.
 *
 * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
 * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
 * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
 * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
 *
 * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
 * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
 * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
 * CONDITIONS.
 *
 */

/* Author: Brian Gerkey */

/**

@mainpage karto_gmapping

@htmlinclude manifest.html

*/

#include "ros/ros.h"
#include "ros/console.h"
#include "message_filters/subscriber.h"//消息同步的头文件
#include "tf/transform_broadcaster.h"
#include "tf/transform_listener.h"
#include "tf/message_filter.h"
#include "visualization_msgs/MarkerArray.h"

#include "nav_msgs/MapMetaData.h"
#include "sensor_msgs/LaserScan.h"
#include "nav_msgs/GetMap.h"

#include "open_karto/Mapper.h"//重要

#include "spa_solver.h"//spa优化算法头文件

#include <boost/thread.hpp>

#include <string>
#include <map>
#include <vector>

// compute linear index for given map coords
#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))

class SlamKarto
{
  public:
    SlamKarto();
    ~SlamKarto();

    void laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan);//得到一帧激光数据之后的回调函数
    bool mapCallback(nav_msgs::GetMap::Request  &req,//请求map地图的回调函数
                     nav_msgs::GetMap::Response &res);

  private:
    bool getOdomPose(karto::Pose2& karto_pose, const ros::Time& t);//得到估计位姿
    karto::LaserRangeFinder* getLaser(const sensor_msgs::LaserScan::ConstPtr& scan);
    bool addScan(karto::LaserRangeFinder* laser,
                 const sensor_msgs::LaserScan::ConstPtr& scan,
                 karto::Pose2& karto_pose);
    bool updateMap();
    void publishTransform();
    void publishLoop(double transform_publish_period);
    void publishGraphVisualization();//publishGraphVisualization() 用来构建的节点和边，便于做图优