目前所用的机器人模型是圆形, 在通用代价地图里定义:robot_radius: 0.26, 而不是footprint参数. 但是注意TEB里用的是Point类型.

指定footprint的数组元素时, 点如果太多, 可以换行。 机器人的轮廓可能是不规则的polygon, 但它的运动中心永远是(0, 0), 顺时针和逆时针规范都支持。 这应该是局部坐标系决定的

现在分析源码中对footprint参数的处理, Costmap2DROS的构造函数里有一句： setUnpaddedRobotFootprint(makeFootprintFromParams(private_nh));, 先看括号里的函数

makeFootprintFromParams

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40

std::vector<geometry_msgs::Point> makeFootprintFromParams(ros::NodeHandle& nh)
{
  std::string full_param_name;
  std::string full_radius_param_name;
  std::vector<geometry_msgs::Point> points;
  // 优先读footprint参数的值
  if (nh.searchParam("footprint",  full_param_name))
  {
    XmlRpc::XmlRpcValue footprint_xmlrpc;
    nh.getParam(full_param_name,  footprint_xmlrpc);
    if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString &&
        footprint_xmlrpc != "" && footprint_xmlrpc != "[]")
    {
      if (makeFootprintFromString(std::string(footprint_xmlrpc),  points))
      {
        writeFootprintToParam(nh,  points);
        return points;
      }
    }
    // 一般是这个, 因为我们定义的是数组
    else if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray)
    {
      points = makeFootprintFromXMLRPC(footprint_xmlrpc,  full_param_name);
      writeFootprintToParam(nh,  points);
      return points;
    }
  }
  // 没有 footprint再读robot_radius
  if (nh.searchParam("robot_radius",  full_radius_param_name))
  {
    double robot_radius;
    nh.param(full_radius_param_name,  robot_radius,  1.234);
    points = makeFootprintFromRadius(robot_radius);
    nh.setParam("robot_radius",  robot_radius);
  }
  // Else neither param was found anywhere this knows about,  so
  // defaults will come from dynamic_reconfigure stuff,  set in
  // cfg/Costmap2D.cfg and read in this file in reconfigureCB().
  return points;
}

函数检查通用代价地图的yaml里是否定义了footprint和robot_radius参数。 前者优先, 如果footprint已经定义了, 就把它做轮廓, 不再处理robot_radius. 我目前用的是robot_radius, 所以再看makeFootprintFromRadius

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16

std::vector<geometry_msgs::Point> makeFootprintFromRadius(double radius)
{
  std::vector<geometry_msgs::Point> points;
  // Loop over 16 angles around a circle making a point each time
  int N = 16;
  geometry_msgs::Point pt;
  for (int i = 0; i < N; ++i)
  {
    double angle = i * 2 * M_PI / N;
    pt.x = cos(angle) * radius;
    pt.y = sin(angle) * radius;

    points.push_back(pt);
  }
  return points;
}

所以对于圆形轮廓机器人, 内接圆和外接圆的半径是不同的, 因为正十六边形的外接圆半径是内接圆半径的1.01959倍

setUnpaddedRobotFootprint

1
2
3
4
5
6
7
8

void Costmap2DROS::setUnpaddedRobotFootprint(const std::vector<geometry_msgs::Point>& points)
{
  unpadded_footprint_ = points;
  padded_footprint_ = points;
  padFootprint(padded_footprint_,  footprint_padding_);

  layered_costmap_->setFootprint(padded_footprint_);
}

setUnpaddedRobotFootprint主要是对padded_footprint_赋值, 和setFootprint函数

footprint_padding_只用于reconfigureCB, 可以不看. padFootprint就是处理padding的情况, 但我们的footprint_padding参数为0, 所以也不看了.

当话题上收到footprint时, 回调函数会将接收到的footprint根据参数footprint_padding_的值进行膨胀, 得到膨胀后的 padded_footprint_, 传递给各级地图。

调用了这个函数的还有setUnpaddedRobotFootprintRadius回调函数 和 setUnpaddedRobotFootprintPolygon回调函数, 前者对应footprint_radius的参数值做的话题, 后者对应footprint_topic的参数值做的话题

1
2
3
4
5
6
7
8
9
10
11

void LayeredCostmap::setFootprint(const std::vector<geometry_msgs::Point>& footprint_spec)
{
  footprint_ = footprint_spec;
  costmap_2d::calculateMinAndMaxDistances(footprint_spec,  inscribed_radius_,  circumscribed_radius_);

  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); 
    plugin != plugins_.end();  ++plugin)
  {
    (*plugin)->onFootprintChanged();
  }
}

calculateMinAndMaxDistances很重要, 它可求得内外圆的半径。footprint 无论是局部代价地图还是全局代价地图得到，两个半径的计算结果都一样。 对于固定的机器人，干脆将两个半径设置为固定值，省得计算。

这里的plugin就是全局和局部代价地图里定义的plugins成员, 别忘了MoveBase里有两个代价地图, 对plugin的添加在Costmap2DROS的构造函数里.

每一层都调用了onFootprintChanged, 但是只有膨胀层覆盖了基类函数, 也就是InflationLayer::onFootprintChanged(), 这个在另一篇文章分析. 其它层没有覆盖, 还是调用的Layer类的空函数virtual void onFootprintChanged() {}

机器人轮廓之所以能在导航时也行走, 原因在updateMap

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

void Costmap2DROS::updateMap()
{
  if (!stop_updates_)
  {
    // get pose in global frame of costmap
    tf::Stamped < tf::Pose > pose;
    if (getRobotPose (pose))
    {
      double x = pose.getOrigin().x(), 
             y = pose.getOrigin().y(), 
             yaw = tf::getYaw(pose.getRotation());

      layered_costmap_->updateMap(x,  y,  yaw);

      geometry_msgs::PolygonStamped footprint;
      footprint.header.frame_id = global_frame_;
      footprint.header.stamp = ros::Time::now();
      transformFootprint(x,  y,  yaw,  padded_footprint_,  footprint);
      footprint_pub_.publish(footprint);

      initialized_ = true;
    }
  }
}

footprint_pub_发布的话题就是/move_base/local_costmap/footprint

相关话题 (不是参数)

footprint, 类型geometry_msgs/Polygon, 由move_base订阅, 但没有发布者。

/move_base/global_costmap/footprint和/move_base/local_costmap/footprint话题是机器人轮廓, 可以直接获得。

Padding机制

Padding机制为机器人和障碍物之间提供了额外的距离, 打开rqt_reconfigure调整全局代价地图的参数footprint_padding, 膨胀层和障碍层会变化, 效果和调整参数inflation_radius和cost_scaling_factor相同。

调整局部代价地图的footprint_padding, 机器人轮廓会成比例放大和缩小。 如果确实需要改变这个参数, 两个代价地图的都要改变。

调整的过程

话题 footprint_radius 和 polygon_footprint

这个和padding是不同的机制, 用话题的方式在线更改机器人的轮廓, 适用于机器人更换货架而导致轮廓改变的情况。
原因在源码中的这一段, footprint_radius是自己加的, 注意全局和局部代价地图的相应话题都只发布一次

所以可以看到move_base(实际在costmap_2d) 订阅/footprint_radius 和 footprint，但没有发布者

1
2
3
4
5
6
7

private_nh.param(topic_param, topic, std::string("footprint"));

footprint_sub_ = private_nh.subscribe(topic, 1, &Costmap2DROS::setUnpaddedRobotFootprintPolygon,  this);

private_nh.param("footprint_radius", footprint_radius_topic_, std::string("footprint_radius"));

radius_sub_ = private_nh.subscribe(footprint_radius_topic_, 1, &Costmap2DROS::setUnpaddedRobotFootprintRadius,  this);

两个回调函数都是调用函数setUnpaddedRobotFootprint, 对于radius的情况, 也就是圆形底盘, 还是把圆按正十六边形处理。

可以用命令测试，现实中由调度端实现：

1
2
3
4
5

rostopic pub -1 /move_base/local_costmap/footprint_radius std_msgs/Float32 '0.34'
rostopic pub -1 /move_base/global_costmap/footprint_radius std_msgs/Float32 '0.34'

rostopic pub -1 /move_base/local_costmap/polygon_footprint geometry_msgs/Polygon '[ [-0.3,  0.3],  [0.3, 0.3],  [0.3, -0.3],  [-0.3, -0.3] ]'
rostopic pub -1 /move_base/global_costmap/polygon_footprint geometry_msgs/Polygon '[ [-0.3,  0.3],  [0.3, 0.3],  [0.3, -0.3],  [-0.3, -0.3] ]'

TEB中的处理

footprint是多边形, 每增加一条边, 就会增加TEB计算时间。设置和通用代价地图里相同

1
2

type: "polygon"   
vertices: [[-0.26,0], [-0.240209, 0.099498], [-0.183848,0.240209], [0,0.26], [0.099498,0.240209], [0.5662,0.240209], [0.5662,-0.240209], [0.099498,-0.240209], [0,-0.26], [-0.183848,-0.240209], [-0.240209, -0.099498] ]

内切圆半径和外切圆半径的计算原理

计算footprint的内切圆半径和外切圆半径, 用到点到线段的距离, 计算方法是两个向量的点积等于一个向量在另一个向量的投影乘以另一个向量的模。看函数distanceToLine

点到线段的三种情况如下:

1
2
3
4
5
6

/*  作用：计算点到线段的距离
*  参数：pX, pY 是中心点坐标
*  x0, y0, x1, y2 线段两端的坐标, 求点到线段的距离
*  用向量点乘 来计算
*/
double distanceToLine(double pX,  double pY,  double x0,  double y0,  double x1,  double y1)

注意devel/lib里的so文件权限是rwxrwxr-x，install/lib里的so文件权限是rw-r--r--

节点move_base由很多库链接,一部分是/opt/ros/kinetic目录的ROS基本库文件,另一部分是/usr/lib/x86_64-linux-gnu的系统库文件,除此之外的重要库文件只有:

• libmove_base.so
• libbase_local_planner.so
• libcostmap_2d.so
• libvoxel_grid.so
• libteb_local_planner.so

同理,libmove_base.so的重要链接库:

• libbase_local_planner.so
• libcostmap_2d.so
• libvoxel_grid.so

依次检查其他库文件,由于base_local_planner一般不修改,所以重要的其实就是libmove_base.so, libcostmap_2d.so, libvoxel_grid.so

主要的库文件，比较大的几个

1
2
3
4
5
6

11M   libmove_base.so
8.5M  liblayers.so

3.1M  libcostmap_2d.so
2.3M  libteb_local_planner.so
2.0M  move_base/move_base

libteb_local_planner.so

1
2
3
4
5
6
7
8
9
10
11
12

add_library( teb_local_planner

   src/timed_elastic_band.cpp
   src/optimal_planner.cpp
   src/obstacles.cpp
   src/visualization.cpp
   src/recovery_behaviors.cpp
   src/teb_config.cpp
   src/homotopy_class_planner.cpp
   src/teb_local_planner_ros.cpp
   src/graph_search.cpp
)

更新teb算法的so文件，不能只更新libteb_local_planner.so，而是包含以下文件：

1

libbase_local_planner.so libmove_base.so libcostmap_2d.so libvoxel_grid.so liblayers.so libteb_local_planner.so

libcostmap_2d.so：

1
2
3
4
5
6
7
8
9
10
11
12
13

add_library(  costmap_2d

  src/array_parser.cpp
  src/costmap_2d.cpp
  src/observation_buffer.cpp
  src/layer.cpp
  src/layered_costmap.cpp
  src/costmap_2d_ros.cpp
  src/costmap_2d_publisher.cpp
  src/costmap_math.cpp
  src/footprint.cpp
  src/costmap_layer.cpp
)

liblayers.so:

1
2
3
4
5
6
7
8

add_library( layers  

  plugins/inflation_layer.cpp
  plugins/obstacle_layer.cpp
  plugins/static_layer.cpp
  plugins/voxel_layer.cpp
  src/observation_buffer.cpp
)

问题

move_base运行时出错，可能是安装问题，更可能是链接问题，确保libtf.so是/opt/ros/的那个，它还会链接libtf2_ros.so 和 libtf2.so，它们也应该是/opt/ros/的

costmap中，机器人需要避开区域包括3类：

1. 静态地图中的障碍物，适合描述整个静态场景的静止障碍物信息，如墙面、固定不动的桌柜等，对应StaticLayer，一旦在costmap中加入static_map,即使地图中的障碍物后来移走了，该区域在costmap中依然存在，机器人仍然会避开该区域。

2. 传感器观察到的障碍物，适合描述运动的障碍物，如行人、小车、移动位置的桌椅等。通过传感器激光实时扫描到的障碍物ObstacleLayer。这一块障碍物信息是不断变化的，对于运动小车的实时避障是很有帮助的。

3. 自定义障碍物，禁止行驶区域

二维costmap地图示例如图上所示。红色单元表示代价地图中的障碍物，蓝色单元表示由机器人内切半径来计算膨胀的障碍物，红色多边形表示机器人的垂直投影（footprint），浅灰色代表已知且未被占用的区域，深灰色代表未知区域。为了避免机器人与障碍碰撞，footprint不能与红色相交，机器人的中心不能穿过蓝色

定义了5个具体的值衡量机器人的状态

上图共分为五个部分:（下面的红色框图是机器人的轮廓，旁边的黑框是上图的映射位置）

1. Lethal（致命的）:机器人的中心与该网格的中心重合，此时机器人必然与障碍物冲突。枚举值254
2. Inscribed（内切）：网格的外切圆与机器人的轮廓内切，此时机器人也必然与障碍物冲突。 枚举值253
3. Possibly circumscribed（外切）：网格的外切圆与机器人的轮廓外切，此时机器人相当于靠在障碍物附近，所以不一定冲突。
4. Freespace（自由空间）：没有障碍物的空间。枚举值0
5. Unknown（未知）：未知的空间。 枚举值255

代价地图采用网格的形式，分为三种状态：占用（有障碍物）、无用（空闲的）、未知。每个网格的值如下：

1
2
3
4

NO_INFORMATION = 255;
LETHAL_OBSTACLE = 254;
INSCRIBED_INFLATED_OBSTACLE = 253;
FREE_SPACE = 0;

• cost_lethal 代价值254，机器人中心在一个cell里，这肯定发生碰撞

• cost_inscribed 代价值253，机器人中心所在的cell与障碍物的距离小于机器人的内切圆半径，还是肯定碰撞，cell的代价值大于等于inscribed cost

• cost_possibly_circumscribed 使用机器人的外接圆半径作为 cutoff distance. 机器人中心所在cell到障碍物距离小于机器人外切圆半径，但是大于内切圆半径。 如果机器人中心所在的cell大于等于这个值，是否碰撞要取决于机器人的朝向

possibly这个词表示不一定真是一个obstacle cell，而是用户自定义一个代价值的情况。比如，如果用户想让机器人应对避开一段特定的区域，应当向那段区域的代价地图插入自定义的代价值，而不考虑任何障碍物。

尽管128也在上图中，但真实的代价值受内切圆半径和外接圆半径的影响，对于源码在InflationLayer::computeCost:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

/** @brief  Given a distance, compute a cost.
 * @param  distance The distance from an obstacle in cells
 * @return A cost value for the distance */
inline unsigned char computeCost(double distance) const
{
  unsigned char cost = 0;
  if (distance == 0)
    cost = LETHAL_OBSTACLE;
  else if (distance * resolution_ <= inscribed_radius_)
    cost = INSCRIBED_INFLATED_OBSTACLE;
  else
  {
    // make sure cost falls off by Euclidean distance
    double euclidean_distance = distance * resolution_;
    double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_));
    cost = (unsigned char)((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
  }
  return cost;
}

障碍层 膨胀层

Obstacle Map Layer：障碍层，用于动态的记录传感器感知到的障碍物信息。

Inflation Layer：膨胀层，在以上两层地图上进行膨胀（向外扩张），以避免机器人的撞上障碍物。膨胀是一个从占用cell代价值从内切障碍过渡到FREE的过程，距离cell越远代价越小，膨胀层会把障碍物代价膨胀直到该半径为止。

到实际障碍物距离dist，$内切半径 < dist < 内切半径+膨胀半径$. 在内切圆半径到膨胀半径之间的所有cell，可以使用如下公式来计算膨胀代价：

如果cost_scaling_factor默认10，膨胀半径默认0.55，那么cost最小约为1

• cost_scaling_factor: 膨胀过程中应用到代价值的比例因子，默认10。从上面公式可以看出，增大比例因子会降低代价。10对应的代价值降低速度很快

• inflation_radius:膨胀半径，默认0.55. 膨胀层会把障碍物代价膨胀直到该半径为止，一般将该值设置为机器人底盘的直径大小。如果太小，车就会贴着障碍走，或者说容易撞到障碍物就需要增大该值，若经常无法通过狭窄地方就减小该值。

减小cost_scaling_factor，使代价值增大，rviz里的膨胀层变为粉色。如果这个参数很大很大，代价值趋近于0，也就是FREE。如果很小，代价值趋近于253，相当于放大了内切障碍值。cost_scaling_factor越小，膨胀层越趋近粉色，反之趋近蓝色。

查看话题/move_base/local_costmap/costmap_updates，会发现一些数值变大

全局代价地图一般膨胀较多，使机器人转弯顺利。 但是局部代价可以不用膨胀层，这是因为TEB已有障碍物膨胀功能。 增大cost_scaling_factor和减小膨胀半径可以使机器人容易进窄通道，但一般是调整TEB，这两个参数不修改。

添加了静态层、障碍层、膨胀层的代价地图是这样的：

如果代价地图不添加膨胀层，结果是这样：

但是加不加障碍层，看上去没有变化， 目前无法在rviz里直接可视化每一层，只能添加或去除后观察

对于画图函数，如果用plot函数参数没有用符号标记参数的话，会自动把离散的点连接起来，

1
2
3
4

plot(x, y)
# 指定x y的坐标范围
xlim([-15 15])
ylim([-12 12])

画离散点

如果设置了点的符号标记参数，就可以使用plot函数画离散的点图：

1
2
3

x=1:10;
y=1:10;
plot(x,y,'.');

将多个函数曲线在同一张图显示

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

x=0:0.01:4;  % x从1到4，绘图采样间隔为0.01
y1=252*exp(-10*(x-radius));
y1=10*x+3;
y2=exp(2*x);
y3=sin(x)

plot(x,y1,'o-','color','r');   %画出图形
hold on;  	%继续画图
plot(x,y2,'s-','color','k');
hold on;  	%继续画图
plot(x,y3,'*-','color','y');

grid on;     % 添加网格，这是用默认的最小标度画出的网格
axis([-5 10 -10 50]);     % x轴的范围是[-5, 10], y轴范围[-10,50]

xlabel('时间');     %设置X轴含义：时间
ylabel('目标');   %设置X轴含义：零件数

legend('y1','y2','y3');

一个图中画多个曲线记得使用hold on，否则之前画的曲线将会被覆盖掉！

标识号曲线有两种方法：(1)通过不同的颜色；(2)通过不同的线形；

1
2
3
4
5
6
7
8

颜色          定义符  

红色          R（red）
兰色          b(blue)
青色          c(cyan)
黄色          y(yellow)
黑色          k(black)
白色          w(white)

1
2
3
4
5

线形            符号

实线             -
圈圈             o
正方形           s

TF发布的话题一般有两种: tf 和 tf_static

tf是只要有节点发布就会有消息， tf_static是在已知的坐标发生变化或有新的节点订阅的时候才会发布新的消息

tf 话题

录制数据集时一定要录这个话题

tf_static 话题

tf_static是静态转换， 和tf的格式一样， any transform on this topic can be considered true for all time. Internally any query for a static transform will return true.

tf_static的发布者使用latched topics, the tf2_ros static_transform_publisher does this correctly. Note: avoid multiple latched static transform publishers on /tf_static in the same process, because with multiple latched publishers within the same node only one will latch correctly.

比如这样的，echo只有这一条结果

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38

transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1652186247
        nsecs:  41763935
      frame_id: "base_link"
    child_frame_id: "move_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
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1652186247
        nsecs:  41846600
      frame_id: "base_link"
    child_frame_id: "shelf_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
---

1. rosrun tf view_frames

当启动ROS程序后，如果想查看存在的所有坐标系转换关系，可以使用命令rosrun tf view_frames，结果会在当前目录生成pdf文件：

顺便说一下，打开pdf的命令为evince frames.pdf

1. rosrun rqt_tf_tree rqt_tf_tree

跟上一个命令差不多，区别是直接打开一个pdf文件，但不会保存

在仿真环境下，使用rostopic hz tf，发现tf的频率是40Hz左右，这个数值是各个tf变化叠加的结果，使用rqt_tf_tree可以看到

有些转换可能显示average rate有数值，但是buffer 和 recent transform都是0，这种其实没有发生tf，所以不能算数。

左上角还有一个clear buffer按钮，Buffer length会清零
Buffer length表示tf buffer中有几秒的数据可使用，rqt_tf_tree中是10秒，view frames大约5秒

1. tf_echo

本命令用来查看两个坐标系之间的实时转换，需要5秒钟：

1

rosrun tf tf_echo /turtle1 /turtle2  　//turtle1是parent坐标系, turtle2是child

1. rqt_tf_tree

rosrun rqt_tf_tree rqt_tf_tree显示一个带界面的tf图显示工具，不必等待，然后可以另存为svg,png等格式
结果发现刷新按钮不好用，当tf树改变后，刷新有时没有改变显示，有时工具无法响应了

1. launch文件中的应用

如果要让两个坐标系建立转换关系，常常在launch文件中添加

1

<node args="-0.24 0 0.295 1.571 0 -1.571 /base_link /camera 10" pkg="tf" name="base_link_to_camera" type="static_transform_publisher"/>

为了支持多个“相似”的机器人，tf使用tf_prefix参数。没有tf_prefix参数，帧名base_link将解析为/base_link

如果tf_prefix参数设置为robot1，base_link将解析为/robot1/base_link。这在运行多机器人时是最有用的，否则这些机器人将在其frame_ids中具有名称冲突。

源码其实很简单:

1
2
3
4
5
6
7
8
9
10

inline std::string getPrefixParam(ros::NodeHandle & nh)\
{
   std::string param; 
   if (!nh.searchParam("tf_prefix", param)) 
     return ""; 
   
   std::string return_val;
   nh.getParam(param, return_val);
   return return_val;
 }

1
2
3
4

<group ns="robot_1">
  <param name="tf_prefix" value="robot_1"/>
  ......
</group>

tf2不再支持tf_prefix. my_frame和/my_frame是不同的坐标系名称, tf2不接受以/开头的坐标系名称,否则会报tf2::InvalidArgument exception

对浮点数进行四舍五入

1
2
3
4

int getEstimate(float a)
{
	return int(a + 0.5);
}

double 只取小数点后两位

1
2

double d = 0.2500000500001;
double b = floor(d * 10000.000f + 0.5) / 10000.000f;

回调函数的降频功能

这是从gmapping里学的，cartographer里也有，但是更复杂了

1
2
3
4
5
6
7
8
9

unsigned int laser_count_ = 0;
int throttle_scans_ = 5;  // 自定义的值
SlamGMapping::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
  laser_count_++;
  if ((laser_count_ % throttle_scans_) != 0)  // 判断是否降频
    return;
  // ... 主要代码 ...
}

在终端显示带颜色和风格的文字

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

// color指定颜色BLACK, RED, GREEN, YELLOW, BLUE, WHITE, option指定文字风格BOLD, REGULAR, UNDERLINE.
const std::string RESET = "\033[0m";
const std::string BLACK = "0m";
const std::string RED = "1m";
const std::string GREEN = "2m";
const std::string YELLOW = "3m";
const std::string BLUE = "4m";
const std::string WHITE = "7m";

const std::string BOLD = "\033[1;3";
const std::string REGULAR = "\033[0;3";
const std::string UNDERLINE = "\033[4;3";
const std::string BACKGROUND = "\033[4";

std::string colouredString(std::string str, std::string colour, std::string option)
{
  double time_now = ros::Time::now().toSec();
  std::string time_string = std::to_string(time_now);
  return "[" + time_string + "]: " + option + colour + str + RESET;
}

生成27个三维向量，每个可取-1,0,1，一共27种组合

1
2
3
4
5
6

for (i = 0; i < 3 * 3 * 3; i++)
{
	int a = (i / 9) - 1;
    int b = ((i % 9) / 3) - 1;
    int c = ((i % 9) % 3) - 1;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70

int killNode(std::string nodeName)
{
  pid_t status;
  string cmd = "rosnode kill "+nodeName;
  status = system(cmd.data());
  if (-1 == status)
  {
    return -1;  // system func error
  }
  else
  {
    if (WIFEXITED(status))    //返回一个非零值, 正常退出
    {
      ROS_INFO("child process exit done: %d", WEXITSTATUS(status) );
      return WEXITSTATUS(status);   // rosnode kill done, exit status value
    }
    else
    {
      ROS_WARN("child process exit abnormally");
      return -3;
    }
  }
}

string getCmdResult(const string &strCmd)
{
  char buf[10240] = {0};
  FILE *pf = NULL;
  if( (pf = popen(strCmd.c_str(), "r")) == NULL )
  {
    return "";
  }
  string strResult;
  while(fgets(buf, sizeof buf, pf))
  {
    strResult += buf;
  }
  pclose(pf);
 
  unsigned int iSize =  strResult.size();
  if(iSize > 0 && strResult[iSize - 1] == '\n')  // linux
  {
    strResult = strResult.substr(0, iSize - 1);
  }
  return strResult;
}

// ros::Time 转为年月日的string
std::string stampToString(const ros::Time& stamp, const std::string format="%Y-%m-%d %H:%M:%S")
{
  const int output_size = 100;
  char output[output_size];
  std::time_t raw_time = static_cast<time_t>(stamp.sec);
  struct tm* timeinfo = localtime(&raw_time);
  std::strftime(output, output_size, format.c_str(), timeinfo);
  std::stringstream ss; 
  ss << std::setw(9) << std::setfill('0') << stamp.nsec;  
  const size_t fractional_second_digits = 4;
  return std::string(output) + "." + ss.str().substr(0, fractional_second_digits);
}

tf2_ros::Buffer buff;
tf2_ros::TransformListener listener(buff);

geometry_msgs::PoseStamped  point_in, point_out;
point_in.header.frame_id = "laser_link";
point_in.header.stamp = ros::Time(0);
point_in.pose =    //赋值
// buff跟上面的使用一样， 返回的point_out是point_in在 parent坐标系 的坐标
point_out = buff.transform(point_in, point_out, "map", ros::Duration(0.3) );

获得Linux命令的结果

比如输入ls，返回当前目录的文件名

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

string getCmdResult(const string &strCmd)
{
  char buf[10240] = {0};
  FILE *pf = NULL;
  if( (pf = popen(strCmd.c_str(), "r")) == NULL )
  {
    return "";
  }
  string strResult;
  while(fgets(buf, sizeof buf, pf))
  {
    strResult += buf;
  }
  pclose(pf);
 
  unsigned int iSize =  strResult.size();
  if(iSize > 0 && strResult[iSize - 1] == '\n')  // linux
  {
    strResult = strResult.substr(0, iSize - 1);
  }
  return strResult;
}

使用json配置realsense的参数(非rosparam)

今天使用realsense时，发现有个参数需要修改

这个参数可以修正因反光而导致的错误，也就是把稍微凸起的地面当做障碍物，适当减小这个参数可以实现。

这个界面在realsense viewer中，但问题是如何在ROS中做到，它在rosparam中没有对应的参数。查来查去，在github的issue里发现了解决方法。realsense已经提供了一种visual preset文件，其实是个json文件，可以对很多参数设置，而且realsense的launch文件中已经定义了json文件的路径，比如rs_camera.launch:

1

<arg name="json_file_path"      default=""/>

realsense提供的所有json文件在这里，我们需要的参数在DefaultPreset_D435.json，也就是第一个参数："aux-param-autoexposure-setpoint": "400"，修改它的值后，把json文件的路径填到上面launch参数那里，启动launch就可以实现了。不过不能动态调整参数。

配置点云的滤波器

rs_camera.launch的参数是比较全的，demo_pointcloud.launch比较少。

现在介绍filters这个参数，在launch中没有赋值:<arg name="filters" default=""/>

pointcloud: 点云话题是/camera/depth/color/points. 点云的texture可以在rqt_reconfigure里修改，或者用参数pointcloud_texture_stream和pointcloud_texture_index修改

The depth FOV and the texture FOV are not similar. By default, pointcloud is limited to the section of depth containing the texture. You can have a full depth to pointcloud, coloring the regions beyond the texture with zeros, by setting allow_no_texture_points to true.

可以配置的滤波器如下:

• disparity, convert depth to disparity before applying other filters and back.
• spatial，filter the depth image spatially.
• temporal，filter the depth image temporally.
• hole_filling，apply hole-filling filter.
• decimation，reduces depth scene complexity.

所有滤波器的说明在这里

给参数filters赋值，必须有pointcloud，再增加滤波器，用逗号隔开。比如 disparity,spatial,pointcloud

滤波器的相关的源码在base_realsense_node.cpp

• BaseRealSenseNode::setupFilters(), 读取参数filters，把所有滤波器名称都插入容器_filters

• BaseRealSenseNode::publishPointCloud(), 发布滤波后的点云

参考:
realsense-ros 滤波器
CSDN Realsense D435 Post-processing filters

ROS版本不兼容产生的TransformListenerWrapper问题

AmclNode类中有这样一段：

1
2
3
4
5

// Use a child class to get access to tf2::Buffer class inside of tf_
struct TransformListenerWrapper : public tf::TransformListener
{
   inline tf2_ros::Buffer &getBuffer() {return tf2_buffer_;}
};

这是Kinetic的版本，但是在Melodic里没有这一段了，这是ROS版本兼容问题，在Kinetic里用不了这个结构体了。搜索发现getBuffer只有在AmclNode::runFromBag里调用，因为平时不用bag仿真时测试AMCL，所以这句话可以注释掉。

“map” passed to lookupTransform argument target_frame does not exist

解决方法： 先加上listener.waitForTransform，再lookupTransform

The listener’s buffer, which carries all information about recent transformation, is literally empty. Therefore, any transform which looks-up the buffer does not find the frames it needs. It is good practice to wait for some time after the listener has been created so that the buffer can fill up. But instead of just sleeping, tf use waitForTransform

参考:

机器人SLAM问题中涉及到平移和旋转，平移是在欧氏空间中，连续的平移变换只需要向量相加。 可是旋转是在非欧空间了，连续的旋转变换就是矩阵连续右乘

右手坐标系：x向前(roll)，y向左(pitch)，z向上(yaw)

用矩阵表示绕某轴旋转

旋转矩阵

坐标系A变换到B，从坐标系B到坐标系A的旋转矩阵的每一列，都是{B}的坐标轴单位向量在{A}中的表示

旋转矩阵R是一个行列式为1的正交矩阵，而且行向量和列向量的长度都为1，所以它的转置就是它的逆，而逆矩阵就可以将坐标系旋转回来。
R的每一行就是{A}的坐标轴单位向量在{B}中的表示。
SO(n)是特殊正交群，这个集合由n维空间的旋转矩阵组成。 SO(3)指三维空间的旋转。

变换矩阵

左上角为旋转矩阵，右侧为平移向量，这种矩阵又称为特殊欧氏群

逆矩阵的形式 $\left[ \begin{matrix} R & t \\ 0 & 1 \\ \end{matrix} \right]^{-1} \ = \left[ \begin{matrix} R^T & -R^Tt \\ 0 & 1 \\ \end{matrix} \right]$

如果一个点的坐标是 $P(x,y,z)$，那么点作T变换后的坐标就写作 $R*P + t$，按齐次矩阵和坐标形式计算就可得到。

Censi 公式

我习惯用Censi论文的公式，还可以用Grisetti派系的

$\quad\quad\quad\quad\quad\quad\quad\Downarrow$

其实这里就是 $Rp + t$ ，只是在二维平面，R就是绕Z轴旋转的旋转矩阵形式。

左乘和右乘

左乘是行变换，右乘是列变换。这是因为习惯上来说，空间中的向量用列向量表示，用矩阵左乘列向量，就是把它在空间当中变换。

若绕静坐标系（世界坐标系）旋转，则左乘，也是变换矩阵 x 坐标矩阵。 在固定坐标系下，一个点 $p$ 经过一个旋转矩阵$R_1$，再经过一个旋转 $R_2$ ，总共旋转 $R_2 * R_1 * p$，这是左乘。 向量旋转中的右乘本没必要，甚至会引起人误解。

若是绕动态坐标系旋转（自身建立一个坐标系），则右乘，也就是坐标矩阵 x 变换矩阵。

即左乘是相对于坐标值所在的坐标系（世界坐标系）下的三个坐标轴进行旋转变换，右乘则是以当前点为旋转中心，进行旋转变换。

具体去看文章： 欧拉角，旋转矩阵，旋转向量，四元数

图优化里的相对位姿是由右乘获得的，即节点 $X_i$ 和 $X_j$ 之间的相对位姿是

常用的公式 $P_c=R_c∗P_w+t_c$ 是把点P从$w$坐标系转换到 $c$ 坐标系，由于旋转时，两个坐标系原点还是重合的，我们说经过了平移后，平移向量 $t_c$ 是从坐标系 $w$坐标系指向 $c$

参考：
cartographer中公式的推导