避障后未能继续原路径|那些卡我很久的bug
abstract Welcome to my blog, enter password to read.
Read more
使用GDB解决算法切换导致的多线程问题 (二)|那些卡我很久的bug
abstract Welcome to my blog, enter password to read.
Read more
gprof在ROS中的配置|ROSROS Kinetic知识

在CMakeLists.txt中的catkin_package()之后，添加

1
2
add_compile_options(-pg)
set(catkin_LIBRARIES ${catkin_LIBRARIES} -pg)

默认情况，一个launch的所有节点都会把gprof输出到同一个文件。在<node>之前添加<env>部分，GMON_OUT_PREFIX环境变量表示了gprof的前缀

1
2
3
4
5
<env name="GMON_OUT_PREFIX" value="listener" />
<node name="listener" pkg="examples" type="listener" output="screen" /> 
                                                                       
<env name="GMON_OUT_PREFIX" value="talker" />
<node name="talker" pkg="examples" type="talker" output="screen" />

gprof文件会在~/.ros生成。

参考: gprof

tmux的使用|常用工具

第一个启动的 Tmux 窗口，编号是0，第二个窗口的编号是1，以此类推。

使用编号区分会话，不太直观，更好的方法是为会话起名 tmux new -s <session-name>

tmux ls命令可以查看当前所有的 Tmux 会话。

tmux attach命令用于重新接入某个已存在的会话。

1
2
3
4
5
# 使用会话编号
tmux attach -t 0

# 使用会话名称
tmux attach -t <session-name>

杀死某个会话

1
2
3
4
5
# 使用会话编号
tmux kill-session -t 0

# 使用会话名称
tmux kill-session -t <session-name>

tmux switch命令用于切换会话

1
2
3
4
5
# 使用会话编号
tmux switch -t 0

# 使用会话名称
tmux switch -t <session-name>

lsof命令解析进程|Linux基础
1
2
3
4
5
6
7
8
9
10
11
12
13
lsof abc.txt             //显示开启文件abc.txt的进程

lsof -c abc              //显示abc进程现在打开的文件

lsof -c -p 1234          //列出进程号为1234的进程所打开的文件

lsof +d /usr/local/      //显示目录下被进程开启的文件

lsof +D /usr/local/      //同上，但是会搜索目录下的目录，时间较长

lsof -i                  //用以显示符合条件的进程情况
lsof -i[46] [protocol][@hostname|hostaddr][:service|port]

使用后发现对文本文件、图片这样的文件不适用，对可执行文件适用。

Quaternion has length close to zero... discarding as navigation goal|那些卡我很久的bug
abstract Welcome to my blog, enter password to read.
Read more
move_base received signal SIGABRT|那些卡我很久的bug
abstract Welcome to my blog, enter password to read.
Read more
抛出错误 std__bad_alloc|那些卡我很久的bug
abstract Welcome to my blog, enter password to read.
Read more
相机点云无法从costmap清除障碍，甚至无法生成障碍|路径规划代价地图

测试的要求: 不能把地面加入costmap，但是稍高于地面的物体能加入。

不断调整 max_obstacle_height, min_obstacle_height, obstacle_range, raytrace_range 四个参数，但是costmap发现生成的障碍总是清除不掉。

换了另一个相机和雷达，发现没有这个问题. 后来发现有时甚至不能生成障碍了。

清除障碍的函数重点是ObservationBuffer::purgeStaleObservations()其中的 observation_list_，它又来自bufferCloud函数中的observation_cloud，后者又来自 global_frame_cloud。

相机发布的点云是在相机坐标系，在bufferCloud函数里用pcl_ros::transformPointCloud转换到代价地图的全局坐标系(也就是yaml中指定的global_frame，一般关注的是local costmap。) 得到 global_frame_cloud。然后按如下条件筛选出 observation_cloud

1
2
3
4
5
6
7
8
for (unsigned int i = 0; i < cloud_size; ++i)
{
  if (global_frame_cloud.points[i].z <= max_obstacle_height_
      && global_frame_cloud.points[i].z >= min_obstacle_height_)
  {
    observation_cloud.points[point_count++] = global_frame_cloud.points[i];
  }
}

因此 min_obstacle_height 和 max_obstacle_height是在代价地图全局坐标系下的值。

在bufferCloud函数中加入代码，把observation_cloud发布出来

1
2
3
sensor_msgs::PointCloud2 observation_ros_cloud;
pcl::toROSMsg(observation_cloud, observation_ros_cloud);
observation_cloud_pub.publish( observation_ros_cloud );

在构造函数里加入
1
2
3
ros::NodeHandle n;
n.param("publish_observation_cloud",  pub_observation_cloud_,  false );
observation_cloud_pub = n.advertise < sensor_msgs::PointCloud2 > ("observation_cloud", 2);

这样可以观察最终生成的障碍来自什么样的点云。比如下面两个点云的对比

最后查啊查啊，终于发现和其他相机的区别不在参数，而在于我之前修改相机驱动时的滤波，滤得太狠了。于是修改驱动，y和z方向的点云不能太少，终于可以清除成功了。

问题的根源在于滤波后的点太少而且稀疏， 导致raytrace机制不能清除障碍。 点云滤波不能直接滤到自己需要的范围，比如即使你实际需要1m的距离，驱动里也不能只给1m，竖直y方向也不能太小，体素滤波的体素不能太大，一般取0.01，这个对点云数量的影响也很大。

vector删除指定索引的元素|C++C++ 模板与STL
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
std::vector<int> v;
v.push_back(0);
v.push_back(1);
v.push_back(2);
v.push_back(3);
v.push_back(4);
v.push_back(5);
v.push_back(6);
v.push_back(7);
v.push_back(8);
v.push_back(9);

unsigned int j = 0;
int size = v.size();
for(unsigned int i=0; i< size; i++)
{
    if(i%2 == 0)
    {
        v.erase(v.begin() + i - j);
        j++;
    }
}
cout << "after remove"<< endl ;
for(unsigned int i=0; i < v.size(); i++)
    cout << v[i] <<"   ";

结果是 1 3 5 7 9


常见iterator自增，如果多增加，可能报错

1
2
3
4
5
6
7
8
9
10
11
12
std::vector<int> v;
v.push_back(0);
v.push_back(1);
v.push_back(2);
v.push_back(3);
v.push_back(4);

for(vector<int>::iterator it=v.begin(); it != v.end(); it++)
{
    it = it + 5;
        cout << *it << endl;
}

这样的程序是错的，会越界。 应该改成这样
1
2
3
4
5
6
for(vector<int>::iterator it=v.begin(); it != v.end(); it++)
{
    int step = v.end() - it;
    it = it+step/5;
    cout << *it << endl;
}