在运行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() 用来构建的节点和边，便于做图优化
//time for tolerance on the published transform,
//basically defines how long a map->odom transform is good for
ros::Duration transform_tolerance_;
// ROS handles
ros::NodeHandle node_;
tf::TransformListener tf_;
tf::TransformBroadcaster* tfB_;
message_filters::Subscriber<sensor_msgs::LaserScan>* scan_filter_sub_;
tf::MessageFilter<sensor_msgs::LaserScan>* scan_filter_;
ros::Publisher sst_;
ros::Publisher marker_publisher_;
ros::Publisher sstm_;
ros::ServiceServer ss_;
// The map that will be published / send to service callers
nav_msgs::GetMap::Response map_;
// Storage for ROS parameters
std::string odom_frame_;
std::string map_frame_;
std::string base_frame_;
int throttle_scans_;
ros::Duration map_update_interval_;
double resolution_;
boost::mutex map_mutex_;
boost::mutex map_to_odom_mutex_;
// Karto bookkeeping
karto::Mapper* mapper_;
karto::Dataset* dataset_;
SpaSolver* solver_;
std::map<std::string, karto::LaserRangeFinder*> lasers_;
std::map<std::string, bool> lasers_inverted_;
// Internal state
bool got_map_;
int laser_count_;
boost::thread* transform_thread_;
tf::Transform map_to_odom_;
unsigned marker_count_;
bool inverted_laser_;
};
SlamKarto::SlamKarto() ://构造函数 初始化参数
got_map_(false),
laser_count_(0),
transform_thread_(NULL),
marker_count_(0)
{
map_to_odom_.setIdentity();
// Retrieve parameters
ros::NodeHandle private_nh_("~");
if(!private_nh_.getParam("odom_frame", odom_frame_))
odom_frame_ = "odom";
if(!private_nh_.getParam("map_frame", map_frame_))
map_frame_ = "map";
if(!private_nh_.getParam("base_frame", base_frame_))
base_frame_ = "base_link";
if(!private_nh_.getParam("throttle_scans", throttle_scans_))
throttle_scans_ = 1;
double tmp;
if(!private_nh_.getParam("map_update_interval", tmp))
tmp = 5.0;
map_update_interval_.fromSec(tmp);
double tmp_tol;
private_nh_.param("transform_tolerance", tmp_tol, 0.0);
transform_tolerance_.fromSec(tmp_tol);
if(!private_nh_.getParam("resolution", resolution_))
{
// Compatibility with slam_gmapping, which uses "delta" to mean
// resolution
if(!private_nh_.getParam("delta", resolution_))
resolution_ = 0.05;
}
double transform_publish_period;
private_nh_.param("transform_publish_period", transform_publish_period, 0.05);
// Set up advertisements and subscriptions
tfB_ = new tf::TransformBroadcaster();
sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);
ss_ = node_.advertiseService("dynamic_map", &SlamKarto::mapCallback, this);
scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, "scan", 5);
scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_, odom_frame_, 5);
scan_filter_->registerCallback(boost::bind(&SlamKarto::laserCallback, this, _1));
marker_publisher_ = node_.advertise<visualization_msgs::MarkerArray>("visualization_marker_array",1);
// Create a thread to periodically publish the latest map->odom
// transform; it needs to go out regularly, uninterrupted by potentially
// long periods of computation in our main loop.
transform_thread_ = new boost::thread(boost::bind(&SlamKarto::publishLoop, this, transform_publish_period));
// Initialize Karto structures
mapper_ = new karto::Mapper();
dataset_ = new karto::Dataset();
// Setting General Parameters from the Parameter Server
bool use_scan_matching;
if(private_nh_.getParam("use_scan_matching", use_scan_matching))
mapper_->setParamUseScanMatching(use_scan_matching);
bool use_scan_barycenter;
if(private_nh_.getParam("use_scan_barycenter", use_scan_barycenter))
mapper_->setParamUseScanBarycenter(use_scan_barycenter);
double minimum_time_interval;
if(private_nh_.getParam("minimum_time_interval", minimum_time_interval))
mapper_->setParamMinimumTimeInterval(minimum_time_interval);
double minimum_travel_distance;
if(private_nh_.getParam("minimum_travel_distance", minimum_travel_distance))
mapper_->setParamMinimumTravelDistance(minimum_travel_distance);
double minimum_travel_heading;
if(private_nh_.getParam("minimum_travel_heading", minimum_travel_heading))
mapper_->setParamMinimumTravelHeading(minimum_travel_heading);
int scan_buffer_size;
if(private_nh_.getParam("scan_buffer_size", scan_buffer_size))
mapper_->setParamScanBufferSize(scan_buffer_size);
double scan_buffer_maximum_scan_distance;
if(private_nh_.getParam("scan_buffer_maximum_scan_distance", scan_buffer_maximum_scan_distance))
mapper_->setParamScanBufferMaximumScanDistance(scan_buffer_maximum_scan_distance);
double link_match_minimum_response_fine;
if(private_nh_.getParam("link_match_minimum_response_fine", link_match_minimum_response_fine))
mapper_->setParamLinkMatchMinimumResponseFine(link_match_minimum_response_fine);
double link_scan_maximum_distance;
if(private_nh_.getParam("link_scan_maximum_distance", link_scan_maximum_distance))
mapper_->setParamLinkScanMaximumDistance(link_scan_maximum_distance);
double loop_search_maximum_distance;
if(private_nh_.getParam("loop_search_maximum_distance", loop_search_maximum_distance))
mapper_->setParamLoopSearchMaximumDistance(loop_search_maximum_distance);
bool do_loop_closing;
if(private_nh_.getParam("do_loop_closing", do_loop_closing))
mapper_->setParamDoLoopClosing(do_loop_closing);
int loop_match_minimum_chain_size;
if(private_nh_.getParam("loop_match_minimum_chain_size", loop_match_minimum_chain_size))
mapper_->setParamLoopMatchMinimumChainSize(loop_match_minimum_chain_size);
double loop_match_maximum_variance_coarse;
if(private_nh_.getParam("loop_match_maximum_variance_coarse", loop_match_maximum_variance_coarse))
mapper_->setParamLoopMatchMaximumVarianceCoarse(loop_match_maximum_variance_coarse);
double loop_match_minimum_response_coarse;
if(private_nh_.getParam("loop_match_minimum_response_coarse", loop_match_minimum_response_coarse))
mapper_->setParamLoopMatchMinimumResponseCoarse(loop_match_minimum_response_coarse);
double loop_match_minimum_response_fine;
if(private_nh_.getParam("loop_match_minimum_response_fine", loop_match_minimum_response_fine))
mapper_->setParamLoopMatchMinimumResponseFine(loop_match_minimum_response_fine);
// Setting Correlation Parameters from the Parameter Server
double correlation_search_space_dimension;
if(private_nh_.getParam("correlation_search_space_dimension", correlation_search_space_dimension))
mapper_->setParamCorrelationSearchSpaceDimension(correlation_search_space_dimension);
double correlation_search_space_resolution;
if(private_nh_.getParam("correlation_search_space_resolution", correlation_search_space_resolution))
mapper_->setParamCorrelationSearchSpaceResolution(correlation_search_space_resolution);
double correlation_search_space_smear_deviation;
if(private_nh_.getParam("correlation_search_space_smear_deviation", correlation_search_space_smear_deviation))
mapper_->setParamCorrelationSearchSpaceSmearDeviation(correlation_search_space_smear_deviation);
// Setting Correlation Parameters, Loop Closure Parameters from the Parameter Server
double loop_search_space_dimension;
if(private_nh_.getParam("loop_search_space_dimension", loop_search_space_dimension))
mapper_->setParamLoopSearchSpaceDimension(loop_search_space_dimension);
double loop_search_space_resolution;
if(private_nh_.getParam("loop_search_space_resolution", loop_search_space_resolution))
mapper_->setParamLoopSearchSpaceResolution(loop_search_space_resolution);
double loop_search_space_smear_deviation;
if(private_nh_.getParam("loop_search_space_smear_deviation", loop_search_space_smear_deviation))
mapper_->setParamLoopSearchSpaceSmearDeviation(loop_search_space_smear_deviation);
// Setting Scan Matcher Parameters from the Parameter Server
double distance_variance_penalty;
if(private_nh_.getParam("distance_variance_penalty", distance_variance_penalty))
mapper_->setParamDistanceVariancePenalty(distance_variance_penalty);
double angle_variance_penalty;
if(private_nh_.getParam("angle_variance_penalty", angle_variance_penalty))
mapper_->setParamAngleVariancePenalty(angle_variance_penalty);
double fine_search_angle_offset;
if(private_nh_.getParam("fine_search_angle_offset", fine_search_angle_offset))
mapper_->setParamFineSearchAngleOffset(fine_search_angle_offset);
double coarse_search_angle_offset;
if(private_nh_.getParam("coarse_search_angle_offset", coarse_search_angle_offset))
mapper_->setParamCoarseSearchAngleOffset(coarse_search_angle_offset);
double coarse_angle_resolution;
if(private_nh_.getParam("coarse_angle_resolution", coarse_angle_resolution))
mapper_->setParamCoarseAngleResolution(coarse_angle_resolution);
double minimum_angle_penalty;
if(private_nh_.getParam("minimum_angle_penalty", minimum_angle_penalty))
mapper_->setParamMinimumAnglePenalty(minimum_angle_penalty);
double minimum_distance_penalty;
if(private_nh_.getParam("minimum_distance_penalty", minimum_distance_penalty))
mapper_->setParamMinimumDistancePenalty(minimum_distance_penalty);
bool use_response_expansion;
if(private_nh_.getParam("use_response_expansion", use_response_expansion))
mapper_->setParamUseResponseExpansion(use_response_expansion);
// Set solver to be used in loop closure
solver_ = new SpaSolver();
mapper_->SetScanSolver(solver_);
}
SlamKarto::~SlamKarto()
{
if(transform_thread_)
{
transform_thread_->join();
delete transform_thread_;
}
if (scan_filter_)
delete scan_filter_;
if (scan_filter_sub_)
delete scan_filter_sub_;
if (solver_)
delete solver_;
if (mapper_)
delete mapper_;
if (dataset_)
delete dataset_;
// TODO: delete the pointers in the lasers_ map; not sure whether or not
// I'm supposed to do that.
}
void
SlamKarto::publishLoop(double transform_publish_period)
{
if(transform_publish_period == 0)
return;
ros::Rate r(1.0 / transform_publish_period);
while(ros::ok())
{
publishTransform();
r.sleep();
}
}
void
SlamKarto::publishTransform()
{
boost::mutex::scoped_lock lock(map_to_odom_mutex_);
ros::Time tf_expiration = ros::Time::now() + transform_tolerance_;
tfB_->sendTransform(tf::StampedTransform (map_to_odom_, tf_expiration, map_frame_, odom_frame_));
}
/*LaserRangeFinder
该类管理关于 laser的相关信息，包括:
laser名字 : 在tutorial1中为 laser0
laser的扫描范围:最大角度，最小角度等等
laser与车中心之间的相对关系 : SetOffsetPose()实现，tutorial1中为karto::Pose2(1.0, 0.0, 0.0)，意为laser相对于车的坐标为 (1,0)，同时相对车的转向角（karto称之为Heading）为 0,
laser 的分辨率，每两根laser射线之间的夹角，配合laser的范围，可以确定laser的射线数量laser 的分辨率，每两根laser射线之间的夹角，配合laser的范围，可以确定laser的射线数量
laser 的 RangeThreshold，用于栅格的大小。*/
karto::LaserRangeFinder*
SlamKarto::getLaser(const sensor_msgs::LaserScan::ConstPtr& scan)//检测scan是否重复，以及laser是否颠倒，以及得到Laser相对于base的位姿信息
{
// Check whether we know about this laser yet ：查看我们是否之前知道这帧数据
if(lasers_.find(scan->header.frame_id) == lasers_.end())
{
// New laser; need to create a Karto device for it.：新数据，需要使用karto处理（需要为它创建一个Karto设备 ）
// Get the laser's pose, relative to base.（得到激光的位姿，关于base的，我认为某一帧激光数据是有角度的，所以是得到激光的旋转角度
tf::Stamped<tf::Pose> ident;
tf::Stamped<tf::Transform> laser_pose;
ident.setIdentity();// Set this transformation to the identity
ident.frame_id_ = scan->header.frame_id;
ident.stamp_ = scan->header.stamp;
try
{
tf_.transformPose(base_frame_, ident, laser_pose);//得到baselink坐标系下laser的角度
}
catch(tf::TransformException e)
{
ROS_WARN("Failed to compute laser pose, aborting initialization (%s)",
e.what());
return NULL;
}
double yaw = tf::getYaw(laser_pose.getRotation());//Helper function for getting yaw from a Quaternion message.
ROS_INFO("laser %s's pose wrt base: %.3f %.3f %.3f",//With Regard To -wrt
scan->header.frame_id.c_str(),
laser_pose.getOrigin().x(),
laser_pose.getOrigin().y(),
yaw);
// To account for lasers that are mounted upside-down,
// we create a point 1m above the laser and transform it into the laser frame
// if the point's z-value is <=0, it is upside-down
//为了计算数据颠倒的laser，我们在激光上方1米处创建一个点，并将其转换为激光帧，如果点的z值<=0，则它是颠倒的
tf::Vector3 v;
v.setValue(0, 0, 1 + laser_pose.getOrigin().z());//x，y，z=1+laser_pose的
tf::Stamped<tf::Vector3> up(v, scan->header.stamp, base_frame_);
try
{
tf_.transformPoint(scan->header.frame_id, up, up);//将子laser的相对坐标转换为世界坐标放在up里
ROS_DEBUG("Z-Axis in sensor frame: %.3f", up.z());
}
catch (tf::TransformException& e)
{
ROS_WARN("Unable to determine orientation of laser: %s", e.what());
return NULL;
}
bool inverse = lasers_inverted_[scan->header.frame_id] = up.z() <= 0;//lasers_inverted_ map可以标记哪一个laser数据是颠倒的
if (inverse)
ROS_INFO("laser is mounted upside-down");
// Create a laser range finder device and copy in data from the first 创建laser range finder对象 并从复制数据
// scan
std::string name = scan->header.frame_id;
karto::LaserRangeFinder* laser =
karto::LaserRangeFinder::CreateLaserRangeFinder(karto::LaserRangeFinder_Custom, karto::Name(name));
laser->SetOffsetPose(karto::Pose2(laser_pose.getOrigin().x(),
laser_pose.getOrigin().y(),
yaw));//设置laser相对于baselink的偏移和角度
laser->SetMinimumRange(scan->range_min);
laser->SetMaximumRange(scan->range_max);
laser->SetMinimumAngle(scan->angle_min);
laser->SetMaximumAngle(scan->angle_max);
laser->SetAngularResolution(scan->angle_increment);
// TODO: expose this, and many other parameters
//laser_->SetRangeThreshold(12.0);
// Store this laser device for later
lasers_[scan->header.frame_id] = laser;
// Add it to the dataset, which seems to be necessary
dataset_->Add(laser);
}
return lasers_[scan->header.frame_id];
}
bool
SlamKarto::getOdomPose(karto::Pose2& karto_pose, const ros::Time& t)//得到t时间点下，机器人的odom pose；
{
// Get the robot's pose//通过时间戳，得到里程计的位姿信息；这里的位姿信息是相对于odom坐标系的，即fixed frame；
tf::Stamped<tf::Pose> ident (tf::Transform(tf::createQuaternionFromRPY(0,0,0),
tf::Vector3(0,0,0)), t, base_frame_);
tf::Stamped<tf::Transform> odom_pose;
try
{
//当机器人在移动过程中，tf会不断接收 base_link->odom 的位置关系信息，这些信息是根据时间不断变化并被记录下来的。当其它节点需要获取某个时间点上的 base_link的位置时就可以通过下面的方法查询：
tf_.transformPose(odom_frame_, ident, odom_pose);//把ident进行转换到odom_frame坐标系下，结果放在odom pose里面
//效果就是odom pose存储了base_link在当前地图上的位置
}
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());//四元素转换成yaw
karto_pose =
karto::Pose2(odom_pose.getOrigin().x(),
odom_pose.getOrigin().y(),
yaw);//
return true;
}
//publishGraphVisualization() 用来构建的节点和边，便于做图优化
void
SlamKarto::publishGraphVisualization()
{
std::vector<float> graph;
solver_->getGraph(graph);//slover是spa优化器
visualization_msgs::MarkerArray marray;
visualization_msgs::Marker m;
m.header.frame_id = "map";
m.header.stamp = ros::Time::now();
m.id = 0;
m.ns = "karto";
m.type = visualization_msgs::Marker::SPHERE;
m.pose.position.x = 0.0;
m.pose.position.y = 0.0;
m.pose.position.z = 0.0;
m.scale.x = 0.1;
m.scale.y = 0.1;
m.scale.z = 0.1;
m.color.r = 1.0;
m.color.g = 0.0;
m.color.b = 0.0;
m.color.a = 1.0;
m.lifetime = ros::Duration(0);
visualization_msgs::Marker edge;
edge.header.frame_id = "map";
edge.header.stamp = ros::Time::now();
edge.action = visualization_msgs::Marker::ADD;
edge.ns = "karto";
edge.id = 0;
edge.type = visualization_msgs::Marker::LINE_STRIP;
edge.scale.x = 0.1;
edge.scale.y = 0.1;
edge.scale.z = 0.1;
edge.color.a = 1.0;
edge.color.r = 0.0;
edge.color.g = 0.0;
edge.color.b = 1.0;
m.action = visualization_msgs::Marker::ADD;
uint id = 0;
for (uint i=0; i<graph.size()/2; i++)
{
m.id = id;
m.pose.position.x = graph[2*i];
m.pose.position.y = graph[2*i+1];
marray.markers.push_back(visualization_msgs::Marker(m));
id++;
if(i>0)
{
edge.points.clear();
geometry_msgs::Point p;
p.x = graph[2*(i-1)];
p.y = graph[2*(i-1)+1];
edge.points.push_back(p);
p.x = graph[2*i];
p.y = graph[2*i+1];
edge.points.push_back(p);
edge.id = id;
marray.markers.push_back(visualization_msgs::Marker(edge));
id++;
}
}
m.action = visualization_msgs::Marker::DELETE;
for (; id < marker_count_; id++)
{
m.id = id;
marray.markers.push_back(visualization_msgs::Marker(m));
}
marker_count_ = marray.markers.size();
marker_publisher_.publish(marray);
}
//后端优化的处理过程都写在了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);//上一次地图更新的时间 （不知道0,0参数的意义）
// Check whether we know about this laser yet
karto::LaserRangeFinder* laser = getLaser(scan);//检测scan是否重复，以及laser是否错误（laser构成scan），获取laser pose 并且放入dataset_存储
if(!laser)
{
ROS_WARN("Failed to create laser device for %s; discarding scan",
scan->header.frame_id.c_str());
return;
}
karto::Pose2 odom_pose;//得到laser之后
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");
}
}
}
}
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;
map_.map.info.origin.orientation.z = 0.0;
map_.map.info.origin.orientation.w = 1.0;
}
// Translate to ROS format
kt_int32s width = occ_grid->GetWidth();
kt_int32s height = occ_grid->GetHeight();
karto::Vector2<kt_double> offset = occ_grid->GetCoordinateConverter()->GetOffset();
if(map_.map.info.width != (unsigned int) width ||
map_.map.info.height != (unsigned int) height ||
map_.map.info.origin.position.x != offset.GetX() ||
map_.map.info.origin.position.y != offset.GetY())
{
map_.map.info.origin.position.x = offset.GetX();
map_.map.info.origin.position.y = offset.GetY();
map_.map.info.width = width;
map_.map.info.height = height;
map_.map.data.resize(map_.map.info.width * map_.map.info.height);
}
for (kt_int32s y=0; y<height; y++)
{
for (kt_int32s x=0; x<width; x++)
{
// Getting the value at position x,y
kt_int8u value = occ_grid->GetValue(karto::Vector2<kt_int32s>(x, y));
switch (value)
{
case karto::GridStates_Unknown:
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = -1;
break;
case karto::GridStates_Occupied:
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 100;
break;
case karto::GridStates_Free:
map_.map.data[MAP_IDX(map_.map.info.width, x, y)] = 0;
break;
default:
ROS_WARN("Encountered unknown cell value at %d, %d", x, y);
break;
}
}
}
// Set the header information on the map
map_.map.header.stamp = ros::Time::now();
map_.map.header.frame_id = map_frame_;
sst_.publish(map_.map);
sstm_.publish(map_.map.info);
delete occ_grid;
return true;
}
bool
SlamKarto::addScan(karto::LaserRangeFinder* laser,
const sensor_msgs::LaserScan::ConstPtr& scan,
karto::Pose2& karto_pose)//核心函数： karto_pose是一个待赋值的指针
/*1、对于激光laser的输入数据scan，添加局部的scan 数据(range_scan)
2、将局部数据range_scan 添加到地图中:processed = mapper_->Process(range_scan)
以达到矫正位姿的效果；
*/
{
if(!getOdomPose(karto_pose, scan->header.stamp))//得到当前时刻的odom pose 也就是baselink在地图上的位置
return false;//这里还是有点不明白 这个TF变换
// Create a vector of doubles for karto
std::vector<kt_double> readings;
if (lasers_inverted_[scan->header.frame_id]) {//如果说 laser颠倒了 进行处理
for(std::vector<float>::const_reverse_iterator it = scan->ranges.rbegin();
it != scan->ranges.rend();
++it)
{
readings.push_back(*it);
}
} else {
for(std::vector<float>::const_iterator it = scan->ranges.begin();
it != scan->ranges.end();
++it)
{
readings.push_back(*it);
}
}
// create localized range scan
karto::LocalizedRangeScan* range_scan =
new karto::LocalizedRangeScan(laser->GetName(), readings);
range_scan->SetOdometricPose(karto_pose);//根据odom数据设置
range_scan->SetCorrectedPose(karto_pose);
// Add the localized range scan to the mapper
bool processed;
if((processed = mapper_->Process(range_scan)))//addScan()调用了Process()函数
{
//std::cout << "Pose: " << range_scan->GetOdometricPose() << " Corrected Pose: " << range_scan->GetCorrectedPose() << std::endl;
karto::Pose2 corrected_pose = range_scan->GetCorrectedPose();
// Compute the map->odom transform ： Q：为什么说是compute呢 transformPose是坐标系转换，而不是得到数据！
tf::Stamped<tf::Pose> odom_to_map;
try
{
tf_.transformPose(odom_frame_,tf::Stamped<tf::Pose> (tf::Transform(tf::createQuaternionFromRPY(0, 0, corrected_pose.GetHeading()),
tf::Vector3(corrected_pose.GetX(), corrected_pose.GetY(), 0.0)).inverse(),
scan->header.stamp, base_frame_),odom_to_map);
}
catch(tf::TransformException e)
{
ROS_ERROR("Transform from base_link to odom failed\n");
odom_to_map.setIdentity();
}
map_to_odom_mutex_.lock();
map_to_odom_ = tf::Transform(tf::Quaternion( odom_to_map.getRotation() ),
tf::Point( odom_to_map.getOrigin() ) ).inverse();
map_to_odom_mutex_.unlock();
// Add the localized range scan to the dataset (for memory management)
dataset_->Add(range_scan);
}
else
delete range_scan;
return processed;
}
bool
SlamKarto::mapCallback(nav_msgs::GetMap::Request &req,
nav_msgs::GetMap::Response &res)
{
boost::mutex::scoped_lock lock(map_mutex_);
if(got_map_ && map_.map.info.width && map_.map.info.height)
{
res = map_;
return true;
}
else
return false;
}
int
main(int argc, char** argv)
{
ros::init(argc, argv, "slam_karto");
SlamKarto kn;
ros::spin();
return 0;
}
有一个疑问:
激光数据来了之后调用laserCallback
laserCallback创建了 karto::Pose2 odom_pose;然后调用addScan(laser, scan, odom_pose)
addScan通过if(!getOdomPose(karto_pose, scan->header.stamp))得到当前时刻的odom pose存储在karto_pose
getOdomPose的代码如下，到底是如何得到odom pose的呢?
我们来看一下ROS与C++入门教程-navigation-实现发布Odom话题
尽然rosrun找不到Odom pkg下面的可执行node。。。不知道哪里出了问题
如果使用tf::transform进行简单的不同frame间的pose转换
以及在书上我看到topic的订阅需要subscriber，我找了slam_karto里面的subscriber，只有
message_filters::Subscriber<sensor_msgs::LaserScan>* scan_filter_sub_;
和
scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(node_, “scan”, 5);
确确实实这里是没有获取odom的topic了
bool
SlamKarto::getOdomPose(karto::Pose2& karto_pose, const ros::Time& t)//得到t时间点下，机器人的odom pose；
{
// Get the robot's pose//通过时间戳，得到里程计的位姿信息；这里的位姿信息是相对于odom坐标系的，即fixed frame；
tf::Stamped<tf::Pose> ident (tf::Transform(tf::createQuaternionFromRPY(0,0,0),
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;
}
运行之后 rosrun tf tf_monitor发现
RESULTS: for all Frames
Frames:
Frame: base_footprint published by unknown_publisher Average Delay: 0.000484265 Max Delay: 0.000924223
Frame: **base_link** published by unknown_publisher(static) Average Delay: 0 Max Delay: 0
Frame: camera_depth_frame published by unknown_publisher(static) Average Delay: 0 Max Delay: 0
Frame: camera_depth_optical_frame published by unknown_publisher(static) Average Delay: 0 Max Delay: 0
Frame: camera_link published by unknown_publisher(static) Average Delay: 0 Max Delay: 0
Frame: camera_rgb_frame published by unknown_publisher(static) Average Delay: 0 Max Delay: 0
Frame: camera_rgb_optical_frame published by unknown_publisher(static) Average Delay: 0 Max Delay: 0
Frame: caster_back_link published by unknown_publisher(static) Average Delay: 0 Max Delay: 0
Frame: caster_front_link published by unknown_publisher(static) Average Delay: 0 Max Delay: 0
Frame: cliff_sensor_front_link published by unknown_publisher(static) Average Delay: 0 Max Delay: 0
Frame: cliff_sensor_left_link published by unknown_publisher(static) Average Delay: 0 Max Delay: 0
Frame: cliff_sensor_right_link published by unknown_publisher(static) Average Delay: 0 Max Delay: 0
Frame: gyro_link published by unknown_publisher(static) Average Delay: 0 Max Delay: 0
Frame: laser published by unknown_publisher Average Delay: -0.0996192 Max Delay: 0
Frame:**odom** published by unknown_publisher Average Delay: 0.000370893 Max Delay: 0.000783593
...
Frame: **wheel_left_link** published by unknown_publisher Average Delay: 0.000773169 Max Delay: 0.00110661
Frame: **wheel_right_link** published by unknown_publisher Average Delay: 0.000775364 Max Delay: 0.00110921
All Broadcasters:
Node: unknown_publisher 41.446 Hz, Average Delay: -0.0237617 Max Delay: 0.00110791
Node: unknown_publisher(static) 1e+08 Hz, Average Delay: 0 Max Delay: 0
而别人的.bag运行之后
RESULTS: for all Frames
Frames:
Frame: ** base_laser_link** published by unknown_publisher Average Delay: 1.57823e+09 Max Delay: 1.57823e+09
Frame: base_link published by unknown_publisher Average Delay: 1.57823e+09 Max Delay: 1.57823e+09
Frame: odom published by unknown_publisher Average Delay: 0.00041296 Max Delay: 0.000618173
All Broadcasters:
Node: unknown_publisher 32.189 Hz, Average Delay: 5.93414e+08 Max Delay: 1.57823e+09
单独运行.bag文件的topic是
asber@asber-X550VX:~/catkin_ws/src/slam_karto/script$ rostopic list
/clock
/rosout
/rosout_agg
/scan
/tf
单独运行的发布的tf
RESULTS: for all Frames
Frames:
Frame: base_laser_link published by unknown_publisher Average Delay: 1.57828e+09 Max Delay: 1.57828e+09
Frame: base_link published by unknown_publisher Average Delay: 1.57828e+09 Max Delay: 1.57828e+09
All Broadcasters:
Node: unknown_publisher 11.6853 Hz, Average Delay: 1.57828e+09 Max Delay: 1.57828e+09
单独跑slam_karto的tf
RESULTS: for all Frames
Frames:
Frame: odom published by unknown_publisher Average Delay: 0.000402493 Max Delay: 0.000565779
All Broadcasters:
Node: unknown_publisher 20.4436 Hz, Average Delay: 0.000402493 Max Delay: 0.000565779
单独跑slam_karto的topic
/map
/map_metadata
/rosout
/rosout_agg
/scan
/tf
/tf_static
/visualization_marker_array
所以这证明了，kartoslam的输入只需要scan和tf
现在知道了 TF是turterbot_bringup minimal.launch写好发送的
tf_.transformPose(odom_frame_, ident, odom_pose);//把ident进行转换到odom_frame坐标系下，结果放在odom pose里面
效果就是odom pose存储了base_link在当前地图上的位置
除了slam_karto之外的代码解析，比如open_karto的解析
参考链接:
- MIT：Karto_slam框架与代码解析
- Karto_slam源码框架分析
- Karto SLAM算法学习（草稿）
- Karto SLAM之open_karto代码学习笔记(一)
- Karto SLAM之open_karto代码学习笔记（二）
- kartoslam算法介绍
附:
除了MIT的
还有intel的实验室
rosbag info interRsearchLab.bag
path: interRsearchLab.bag
version: 2.0
duration: 44:51s (2691s)
start: Dec 06 2000 05:47:37.34 (976052857.34)
end: Dec 06 2000 06:32:28.63 (976055548.63)
size: 17.2 MB
messages: 54177
compression: none [23/23 chunks]
types: sensor_msgs/LaserScan [90c7ef2dc6895d81024acba2ac42f369]
tf2_msgs/TFMessage [94810edda583a504dfda3829e70d7eec]
topics: scan 13631 msgs : sensor_msgs/LaserScan
tf 40546 msgs : tf2_msgs/TFMessage
/clock
/rosout
/rosout_agg
/scan
/tf
scan看了一下貌似差不多 只有一个tf有差别
transforms:
-
header:
seq: 0
stamp:
secs: 1578378612
nsecs: 764702028
frame_id: "base_link"
child_frame_id: "laser"
transform:
translation:
x: 0.0
y: 0.0
z: 0.18
rotation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
transforms:
-
header:
seq: 0
stamp:
secs: 78
nsecs: 680800000
frame_id: "base_link"
child_frame_id: "base_laser_link"
transform:
translation:
x: 0.0
y: 0.0
z: 0.0
rotation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
---
transforms:
-
header:
seq: 0
stamp:
secs: 976052861
nsecs: 640506982
frame_id: "base_link"
child_frame_id: "base_laser_link"
transform:
translation:
x: 0.0
y: 0.0
z: 0.0
rotation:
x: 0.0
y: 0.0
z: 0.0
w: 1.0
---
别人都是baselink到base_laser_link
而我的tf却是 到 child_frame_id: “laser”
<launch>
<node pkg="tf" type="static_transform_publisher" name="base_to_laser" args="0.0 0.0 0.18 0 0.0 0.0 base_link base_laser_link 100"/>
<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>
修改之后 令人不解的地方来了!
asber@asber-X550VX:~$ rosrun slam_karto slam_karto
[ WARN] [1578392745.355315546]: MessageFilter [target=odom ]: Dropped 100.00% of messages so far. Please turn the [ros.slam_karto.message_notifier] rosconsole logger to DEBUG for more information.
炸了 ，说明也不是这里这个的问题
那就很说明，是激光雷达的原因了。
来源：CSDN
作者：专业渡劫修仙
链接：https://blog.csdn.net/kuizhao8951/article/details/103838157