为了支持多个“相似”的机器人，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;
}

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中公式的推导

这个就是最常用的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()，这里只是阻塞，跟订阅无关)，订阅者仍然能收到阻塞之前发布的消息。

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

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

协方差可以表示两个变量的协同关系， 变化趋势是否一致。 相关性就是用X、Y的协方差除以X的标准差和Y的标准差。

我们机器人用到的向量是(x,y,z, roll,pitch,yaw)，协方差就是一个6x6的矩阵，对角线的元素为各个随机变量的方差，非对角线的元素就是两两随机变量之间的协方差。AMCL中所用的协方差矩阵，由于z,pitch,roll都是0，矩阵的第1,8和最后的元素，分别是x,y,yaw的方差。另外还有x和y的协方差

如果协方差矩阵是单位矩阵，那么就是标准的多元高斯分布。由于协方差矩阵是对称矩阵，所以可以做特征分解

其实可以变成

因此任意一个协方差矩阵可以看做三维变换的结果，概率密度函数是由协方差矩阵的特征向量控制旋转，特征值控制尺度。 协方差矩阵就是根据这两项对标准高斯分布进行变换。

AMCL中的pf_ran_gaussian函数是以对角线元素为标准差，生成一个高斯分布。

要想在地图中添加障碍，用virtual_layer和costmap_prohibition_layer都可以，效果一样，区别就是前者的yaml里需要加上朝向，其实是没必要的。下面以virtual_layer为例向代价地图加入新的障碍。

进入costmap_2d这个包，查看costmap_2d\costmap_plugins.xml文件，可以看到目前所拥有的层，基类都是costmap_2d::Layer。

配置代价地图

launch里加入

1
2

<rosparam file="$(find navigation2d)/param/virtual_layer.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find navigation2d)/param/virtual_layer.yaml" command="load" ns="local_costmap" />

最后是设置禁止区域坐标参数，首先需要在参数配置文件夹（global_costmap_params.yaml所在的文件夹）中创建新的文档，命名为prohibition_areas.yaml，然后输入:

1
2
3
4
5
6
7

costmap_prohibition_layer:
 prohibition_areas:
  - [[-1.3, -2.98],[-1.3, -16.77]]
  - [[13.31, -26.77],[2.88, -3.45]]
  - [[-4.96, -37.78],[-32.67, -37.90]]
  - [[-23.39, -33.43],[-11.89, -33.20]]
  - [[0.0, 6.89],[0.0, 38.05]]

1
2
3
4
5
6

virtual_layer:
    zone_topics: ["/zone"]
    obstacle_topics: ["/obstacle"]
    static_forms: [[[-18.46,-12.18,0.0],[-13.21,-12.18,0.0],[-13.21,-13.4,0.0],[-18.46,-13.4,0.0]],
                   [[-13.99,-0.17,0.0],[-11.87,-0.14,0.0]],
                   [[-13.18,-11.15,0.0],[-11.81,-11.15,0.0]]]

查来查去，怎么也找不到原因，最后是在两个代价地图的yaml里，把- {name: virtual_layer, type: "virtual_costmap_layer::VirtualLayer"} 放到膨胀层之前 ，问题解决，看来膨胀层的源码不完善。

注意事项：

1. 一定要严格按照上述格式来设置坐标，格式错误导致不能识别禁止区域坐标情形有: (1)坐标前的短横线没对齐 (2) 定义禁止区域或者禁止线，两坐标之间缺少了逗号
2. 可以同时定义多个点障碍／线障碍／障碍区域。对于3个及以上的点，形成的是封闭的区域障碍。
3. 禁止区域的坐标是map坐标系的
4. 由于Bresenham算法，形成的线可能看起来不够直
   

costmap_prohibition_layer

源码在costmap_prohibition_layer

CMakeLists里可以看到add_library(costmap_prohibition_layer src/costmap_prohibition_layer.cpp)， 结果在lib/libcostmap_prohibition_layer.so

看文件package.xml，最后有一句：

1
2
3

<export>
      <costmap_2d plugin="${prefix}/costmap_plugins.xml"/>
</export>

costmap_plugins.xml如下：

1
2
3
4
5

<library path="lib/libcostmap_prohibition_layer">
  <class type="costmap_prohibition_layer_namespace::CostmapProhibitionLayer" base_class_type="costmap_2d::Layer">
    <description>ROS-Package that implements a costmap layer to add prohibited areas to the costmap-2D by a user configuration.</description>
  </class>
</library>

以上几项缺一不可，放到工作空间编译后，在终端中输入rospack plugins —attrib=plugin costmap_2d ，这个命令是显示当前costmap_2d能接受的所有插件，如果出现

1

costmap_prohibition_layer /home/user/plan_ws/src/costmap_prohibition_layer/costmap_plugins.xml

参考：
在Costmap_2d里面插入Prohibition_layer

之前看depthimage_to_laserscan包，发现它的订阅发布采用的是所谓 lazy模式。今天研究了一下，发现并不复杂。先看advertise函数的一个原型：

1
2
3
4
5
6
7

Publisher ros::NodeHandle::advertise(const std::string& topic,
	uint32_t 	queue_size,
	const  SubscriberStatusCallback & 	connect_cb,
	const  SubscriberStatusCallback & 	disconnect_cb = SubscriberStatusCallback(),
	const  VoidConstPtr & 	tracked_object = VoidConstPtr(),
	bool   latch = false 
)

话题有订阅者时，发布者会触发connect_cb函数，停止订阅时又触发disconnect_cb函数。如果让它们做类成员函数，就使用Boost::bind形式，tracked_object暂时不用。所以程序可以这样写：

1
2
3
4
5
6
7
8
9
10

void connectCb(const ros::SingleSubscriberPublisher& singlePub)
{
    ROS_INFO("Subscriber Name: %s", singlePub.getSubscriberName().c_str());
}
void disconnectCb(const ros::SingleSubscriberPublisher& singlePub)
{
    ROS_INFO("disconnectCb");
}
......
  ros::Publisher pub = nh.advertise<std_msgs::Int8>("topic", 50, connectCb,disconnectCb);

再来看depthimage_to_laserscan的程序，构造函数里是这样的：

1
2
3
4

pub_ = n.advertise<sensor_msgs::LaserScan>("scan", 10, 
	boost::bind(&DepthImageToLaserScanROS::connectCb, this, _1), 
	boost::bind(&DepthImageToLaserScanROS::disconnectCb, this, _1)
	);

1
2
3
4
5
6

if (!sub_ && pub_.getNumSubscribers() > 0)
{
	ROS_DEBUG("Connecting to depth topic.");
	image_transport::TransportHints hints("raw", ros::TransportHints(), pnh_);
	sub_ = it_.subscribeCamera("/camera/depth/image_raw", 10, &DepthImageToLaserScanROS::depthCb, this, hints);
}

而depthCb是：

1
2

sensor_msgs::LaserScanPtr scan_msg = dtl_.convert_msg(depth_msg, info_msg);
    pub_.publish(scan_msg);

disconnectCb里，如果另一个节点不再订阅scan话题，本节点就会sub_.shutdown();；如果再有订阅scan，又可以通过connectCb使用sub_，设计确实很巧妙

参考：
Publisher advertise
SingleSubscriberPublisher

参考：这篇博客，意外的是我配置rplidar时发现标志号码和博客中是一样的，还以为像产品序列号那样。我设置了底盘的USB和思岚雷达的USB

realsense的安装包里面包含了udev的文件，即realsense udev的deb。所以使用过realsense的系统，自己没编辑，也会有一个99-realsense-libusb.rules，不必自己设置

70-snap.core.rules是系统自带的

• rosed: 可以直接编辑某个package当中的文件，格式是rosed package file，还可以用tab补全名称，如果文件名在package里不是唯一的，那么会呈现出一个列表，让你选择编辑哪一个文件。

• roswtf: 检查ROS各节点联系是否有错；检查环节变量等系统设置

rostopic

• rostopic echo -b BAGFILE -a, 显示bag文件的所有消息。但是所有消息一次显示，没什么意义。

• rostopic echo -n1， 只echo一段消息

• rostopic echo -p --nostr --noarr /topic_name， 不显示话题中的string和array部分

• rostopic echo /odom/pose/pose/position， 直接显示odom话题中的position部分

• rostopic bw topic_name： 用于显示主题所使用的带宽。

• rostopic delay topic_name : 显示ROS节点通信延迟

以下命令可以组合使用：

• rostopic list -p, 只显示发布的话题

• rostopic list -s, 只显示订阅的话题

• rostopic list -v, 显示话题细节，有几个发布者和订阅者

• rostopic list --host, 根据host名分组

有一次使用rostopic pub一个std_msgs/Int8的消息，结果报错 Inbound TCP/IP connection failed: field data must be an integer type 。 确实echo没有结果，另开一个终端，source 工作空间后成功。

rosmsg rosnode

• rosmsg package [package_name], 显示所有在这个包里面的消息

• rosmsg md5 [message], 显示某个消息的md5sum

• rosnode cleanup， 清除僵尸节点, 也就是清除那些实际关闭但能用rosnode list查看到的节点

• rosnode machine <hostname>： 用于列出指定计算机上运行的节点

• rospack find turtlesim, 输出指定package的路径, 输出/opt/ros/kinetic/share/turtlesim

• rospack depends turtlesim, 查看package的依赖项

roslaunch

• --wait, 延迟launch直到检测到roscore

roslaunch启动的时候，会尝试获得已有的rosmaster的id，如果没有获得，将会创建一个自己的。假如同时也有一个roslaunch和roscore启动，可能会导致run_id冲突。比如

可以使用roslaunch --wait rplidar_ros a2.launch，它会输出这样的信息：

1
2

roscore/master is not yet running, will wait for it to start
master has started, initiating launch

• --screen, 输出所有节点的日志，用于debugging.

• --dump-params, 以YAML格式输出launch文件中的所有参数

参考：roslaunch Commandline Tools