If you wish for peace, prepare for war

tf_prefix和多机器人的应用 2020-11-15|ROSROS Kinetic知识

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

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

源码其实很简单:

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

一般用法是:

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

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

动态调整机制 2020-11-15|ROSROS Kinetic知识

params.cfg文件

#!/usr/bin/env python

PACKAGE = "yocs_velocity_smoother"

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

gen.add("speed_lim_v", double_t, 0, "Maximum linear velocity", 1.0, 0.0, 100.0)
gen.add("speed_lim_w", double_t, 0, "Maximum angular velocity", 5.0, 0.0, 100.0)
gen.add("accel_lim_v", double_t, 0,
        "Maximum linear acceleration", 0.5, 0.0, 100.0)
gen.add("accel_lim_w", double_t, 0,
        "Maximum angular acceleration", 2.5, 0.0, 100.0)
gen.add("decel_factor", double_t, 0,
        "Deceleration to acceleration ratio", 1.0, 0.0, 10.0)
# Second arg is node name it will run in (doc purposes only), third is generated filename prefix
exit(gen.generate(PACKAGE, "velocity_smoother_configure", "params"))

这是一个基于python的模块，首先创建一个ParameterGenerator对象，然后调用其add()函数将参数添加到参数列表中。add()的参数含义分别是：参数名，参数类型，级别，描述，缺省值，最小值，最大值。

ddynamic_reconfigure声称不使用cfg文件就能动态调整参数，但是并不好用，我运行示例没有出现想要的效果。目前只发现在编译realsense驱动时候有使用，网上没发现其他应用，可以不研究。

编程技巧 2020-11-09|C++C++ 基础

对浮点数进行四舍五入

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里也有，但是更复杂了

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;
  // ... 主要代码 ...
}

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

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

生成极值

C++11中，std::numeric_limits为模板类，在库编译平台提供基础算术类型的极值等属性信息，取代传统C语言，所采用的预处理常数。比较常用的使用是对于给定的基础类型用来判断在当前系统上的最大值、最小值。需包含头文件。

std::numeric_limits<double>::max();的结果是一个很大的数字

数学函数

hypot()用来求三角形的斜边长，其原型为：double hypot(double x, double y);，需要#include <stdio.h>
fabs函数是求绝对值的函数，函数原型是extern float fabs(float x)，需要#include <math.h>

对double/float数据，一定要使用fabs函数。如果用了abs，就会出现bug，因为返回也是int

反正切函数求角度

atan2返回给定的 X 及 Y 坐标值的反正切值。反正切的角度值等于 X 轴与通过原点和给定坐标点 (Y坐标, X坐标) 的直线之间的夹角。结果以弧度表示并介于-pi到pi之间（不包括-pi）。

而atan(a/b)的取值范围介于-pi/2到pi/2之间，不包括±pi/2

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

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

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，返回当前目录的文件名

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配置realsense的参数 2020-10-18|相机

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

今天使用realsense时，发现有个参数需要修改
Set Point.png
这个参数可以修正因反光而导致的错误，也就是把稍微凸起的地面当做障碍物，适当减小这个参数可以实现。

这个界面在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

tf的疑难问题 2020-10-18|ROSROS Kinetic知识

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

AmclNode类中有这样一段：

// 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

参考:

csm包的源码解读 (二) 2020-09-25|激光SLAMICP和NDT

smart algorithm.png

坐标系空间转换 2020-09-20|数学基础

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

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

用矩阵表示绕某轴旋转

旋转矩阵

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

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

变换矩阵

$T =\left[ \begin{matrix} R & t \\ 0 & 1 \\ \end{matrix} \right]$

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

$SE(3) = \left \{T =\left[ \begin{matrix} R & t \\ 0 & 1 \\ \end{matrix} \right] \in \mathbb{R}^{4\times4} \ | R\in SO(3), t\in \mathbb{R}^3 \right \}$

逆矩阵的形式 $\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派系的
坐标系示意图.jpg
圆加和圆减.jpg

$\quad\quad\quad\quad\quad\quad \vec{a} \oplus \vec{b} = \vec{c}$

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

$\begin{pmatrix} a_x \\ a_y \\ a_{\theta} \end{pmatrix} + \left[ \begin{matrix} cos_{a_\theta} & -sin_{a_\theta} & 0 \\ sin_{a_\theta} & cos_{a_\theta} & 0 \\ 0 & 0 & 1\end{matrix} \right] * \begin{pmatrix} b_x \\ b_y \\ b_{\theta} \end{pmatrix} = \begin{pmatrix} c_x \\ c_y \\ c_{\theta} \end{pmatrix}$

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

左乘和右乘

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

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

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

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

车在全局坐标系中运动，这可以看做绕固定轴旋转的过程，是左乘矩阵

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

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

$Z_{ij} = X_i^{-1}*X_j$

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

参考：
cartographer中公式的推导

laser_scan_matcher 源码解读 2020-09-19|激光SLAMICP和NDT

主要是LaserScanMatcher的构造函数：

ros::init(argc, argv, "LaserScanMatcher");
ros::NodeHandle nh;
ros::NodeHandle nh_private("~");
scan_tools::LaserScanMatcher laser_scan_matcher(nh, nh_private);
ros::spin();

构造函数里初始化一系列参数，重点就是回调函数scanCallback

// 第一帧scan, cache the tf from base to the scanner
if (!initialized_)
{
  createCache(scan_msg);    // caches the sin and cos of all angles
  // cache the static tf from base to laser
  if (!getBaseToLaserTf(scan_msg->header.frame_id))
  {
    ROS_WARN("Skipping scan");
    return;
  }
  // 保存scan数据到  LDP  prev_ldp_scan_;
  laserScanToLDP(scan_msg, prev_ldp_scan_);
  last_icp_time_ = scan_msg->header.stamp;
  initialized_ = true;
}
// 第二帧及以后的scan
index++;		// static unsigned int index;
ROS_INFO("scan index: %d", index);
LDP  curr_ldp_scan;
laserScanToLDP(scan_msg, curr_ldp_scan);
processScan(curr_ldp_scan, scan_msg->header.stamp);

流程.png

公式计算

newKeyframeNeeded

if (fabs(tf::getYaw(d.getRotation())) > kf_dist_angular_)
   return   true;
double x = d.getOrigin().getX();
double y = d.getOrigin().getY();
if (x*x + y*y > kf_dist_linear_sq_)
   return  true;

return false;

优先判断相对旋转

调用是这样：

// 交换新旧frame, key frame 的意思应该是当前配准时的参考帧
// 这个在机器人一直走直线或原地旋转时很难发生
if (newKeyframeNeeded(corr_ch))
{
	// generate  a  keyframe
	ld_free(prev_ldp_scan_);
	prev_ldp_scan_ = curr_ldp_scan;
	// 更新下一个时间段的 f2b_kf
	f2b_kf_ = f2b_;
}

prev_ldp_scan_之前是第一帧scan的数据，只有需要 key frame时，才会更新它，但在processScan开头又会把估计位姿和真值初始化为0。如果没有达到运动阈值，prev_ldp_scan_还是第一帧scan，时间也是第一帧的时间，仍以第一帧为基准进行ICP配准

深入探究虚函数 (二) 虚函数表vtbl和虚表指针vptr 2020-09-08|C++C++ 面向对象

虚函数表是通过一块内存来存储虚函数的地址，它实际是一个函数指针数组，每一个元素都是虚函数的指针，虚函数表的最后一个元素是一个空指针。假如虚函数类型为int，函数指针就是int*类型．虚函数表将被该类的所有对象共享，每个对象内部包含了一个虚表指针，指向虚函数表

有虚函数的类的最开始部分就是虚指针，它指向虚函数表起始地址，类型为int**（如果虚函数类型int），表中存放虚函数的地址。通过这个指针可以获取到该类对象的所有虚函数，包括父类的。

在编译期，编译器完成了虚表的创建，而虚指针在构造函数期间被初始化

因为派生类会继承基类的虚函数表，所以通过虚函数表，我们就可以知道该类对象的父类，在转换的时候就可以用来判断对象有无继承关系。派生类中增加的虚函数，如果覆盖了基类的虚函数，虚函数表中会替换相应的基类虚函数，地址换成派生类的；如果没有覆盖基类的虚函数，就添加到原虚函数表后面，以空指针结尾．所以说派生类的虚函数表中的函数地址不是连续的，基类的是连续的。

类Base的虚表如下图：

如果派生类Derived没有覆盖基类的虚函数，它的虚函数表如下图：

如果覆盖vFunc1，则替换Base的vFunc1；如果还定义了一个虚函数vFunc3，那么继续往虚函数表之后填，最后一个数组成员还是空指针

更详细的说明

当类有虚函数时候，类的第一个成员变量是一个虚函数指针，而this指针的值和第一个成员变量的地址相同（this指针指向第一个成员变量）。因此当有虚函数时，this指针的值等于虚函数指针的地址 this==&_vptr;

参考：
C++ 虚函数表
 深入C++对象模型&虚函数表

发布者的latch机制 2020-09-03|ROSROS程序和源码分析

这个就是最常用的advertise函数的最后一个参数，原型： advertise (const std::string &topic, uint32_t queue_size, bool latch=false)

是否锁存。某些话题并不是会以某个频率发布，比如/map这个topic，只有在初次订阅或者地图更新这两种情况下，才会向/map发布消息，这里就用到了锁存。

锁存的作用就是，将发布者最后一次发布的消息保存下来，然后把它自动发送给后来的订阅者。这种方式非常适合变动较慢、相对固定的数据（例如地图），然后只发布一次，相比于同样的消息不定的发布，锁存的方式既可以减少通信中对带宽的占用，也可以减少消息资源维护的开销。

查看gmapping中的函数SlamGMapping::startLiveSlam()，发现三个话题都是 latched：

1
2
3

entropy_publisher_ = private_nh_.advertise<std_msgs::Float64>("entropy", 1, true);
sst_ = node_.advertise<nav_msgs::OccupancyGrid>("map", 1, true);
sstm_ = node_.advertise<nav_msgs::MapMetaData>("map_metadata", 1, true);

latch=true，发布消息一次或几次，然后设法阻塞程序(常常用spin()，这里只是阻塞，跟订阅无关)，订阅者仍然能收到阻塞之前发布的消息。

ros::init(argc,argv, "Pub");
ros::NodeHandle nh;
ros::Publisher pub_latch_true = nh.advertise<std_msgs::Int8>("topic_latch_true", 50, true);
ros::Publisher pub_latch_false = nh.advertise<std_msgs::Int8>("topic_latch_false", 50, false);
ros::Rate rate(1);

std_msgs::Int8 msg;
msg.data = 2;
pub_latch_true.publish(msg);
pub_latch_false.publish(msg);
while(ros::ok())
{
  ros::spinOnce();
  rate.sleep();
}
return 0;

运行程序后等一会，运行rostopic echo topic_latch_false，什么也没有。运行rostopic echo topic_latch_true，会收到data=2的消息。即使先运行两个echo，再运行发布者，结果也是如此

bool Publisher::isLatched () const 用于判断是否latch