相机点云无法从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;
}

prune_plan机制及相关问题|路径规划纯跟踪算法
abstract Welcome to my blog, enter password to read.
Read more
第二次规划全局路径导致move_base退出|那些卡我很久的bug
abstract Welcome to my blog, enter password to read.
Read more
不使用传感器时的代价地图和move_base报警|路径规划代价地图

有一次机器人没有装雷达和相机，打算随便跑跑。于是在通用代价地图的障碍层，不设置传感器数据来源，运行move_base出现频繁报警

1
The /scan observation buffer has not been updated for 22.06 seconds, and it should be updated every 5.00 seconds.

来源在
1
2
3
4
5
6
7
8
9
10
11
12
13
14
bool ObservationBuffer::isCurrent() const
{
  if (expected_update_rate_ == ros::Duration(0.0))
    	return true;
  // last_updated_ 没有赋值
  bool current = (ros::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();
  if (!current)
  {
    ROS_WARN(
        "The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.",
        topic_name_.c_str(), (ros::Time::now() - last_updated_).toSec(), expected_update_rate_.toSec());
  }
  return current;
}

此时发导航命令，又有报警

1
[/move_base]:Sensor data is out of date, we're not going to allow commanding of the base for safety

因为没有可靠的传感器数据，move_base不允许车跑起来。来源在MoveBase::executeCycle
1
2
3
4
5
6
if(!controller_costmap_ros_->isCurrent())
{
  ROS_WARN("[%s]:Sensor data is out of date, we're not going to allow commanding of the base for safety",ros::this_node::getName().c_str());
  publishZeroVelocity();
  return false;
}

也就是函数
1
2
3
4
5
6
7
8
9
bool LayeredCostmap::isCurrent()
{
  current_ = true;
  for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins_.begin(); plugin != plugins_.end();
      ++plugin)
  {
    current_ = current_ && (*plugin)->isCurrent();
  }
}

显然是因为障碍层不符合isCurrent，导致代价地图也不符合isCurrent。如果希望车照样跑，就把MoveBase::executeCycle那段注释掉，把ObservationBuffer::isCurrent()的报警也注释掉，否则没完没了。

从网上下载一个包含雷达数据的bag，又设置了通用代价地图和tf树后，发现报警依然，应该还是时间戳问题，懒得再对齐了。

ROS编程规范|ROSROS程序和源码分析

  • Packages, Topics / Services, 文件、库的命名都采用 under_scored

  • Classes / Types的命名采用 CamelCased，比如class ExampleClass, class HokuyoURGLaser。 函数命名采用 camelCased，比如int exampleMethod(int example_arg)

  • 普通变量和成员变量命名、命名空间都使用 under_scored，循环中的临时变量使用i,j,k，i on the outer loop, j on the next inner loop

  • 常量命名采用 ALL_CAPITALS

  • 全局变量采用 g_ 开头的 under_scored

  • 每行最多120个字符

  • 所有头文件要包含#ifndef，比如:

1
2
3
4
#ifndef PACKAGE_PATH_FILE_H
#define PACKAGE_PATH_FILE_H
	......
#endif

这部分应该在license之后，#endif在头文件的最后

  • 不使用宏

预处理所用的宏，比如

1
2
3
#if DEBUG
        temporary_debugger_break();
#endif

  • 函数的输出参数使用指针，而不是引用: int exampleMethod(FooThing input, BarThing* output);

  • 头文件里不要用using namespace关键字，在源文件里可使用using，但不要using namespace std;，而是使用using std::list;, using std::vector;，否则引入了std所有内容。

  • 建议使用Exceptions，而不是returning integer error codes。 析构函数不要抛出异常。 不直接触发的回调函数，不要抛出异常。

  • 保证代码是 exception-safe: When your code can be interrupted by exceptions, you must ensure that resources you hold will be deallocated when stack variables go out of scope. In particular, mutexes must be released, and heap-allocated memory must be freed. Accomplish this safety by using the following mutex guards and smart pointers

枚举

1
2
3
4
5
6
7
8
9
10
namespace Choices
{
  enum Choice
  {
     Choice1,
     Choice2,
     Choice3
  };
}
typedef Choices::Choice Choice;

This prevents enums from polluting the namespace they’re inside. Individual items within the enum are referenced by: Choices::Choice1, but the typedef still allows declaration of the Choice enum without the namespace.

  • If you are using C++11 and above, you can use scoped enumeration
1
2
3
4
5
6
7
enum class Choice
{
    Choice1,
    Choice2,
    Choice3
};
Choice c = Choice::Choice1;

  • 不建议使用全局变量和函数，尤其前者。更不能在多线程中使用。大多数变量和函数都该在类的体内，其他应当在命名空间里。

  • 类不要使用静态变量。

  • 只在 well-defined exit point 调用exit()函数

  • 使用assertions之类的比条件判断语句好，比如ROS_ASSERT, ROS_ASSERT_MSG, ROS_ASSERT_CMD, ROS_BREADK

Depending on compilation settings, the assertion may not be executed.

It is typical to develop software with assertion-checking enabled, in order to catch violations. When nearing software completion and when assertions are found to always be true in the face of extensive testing, you build with a flag that removes assertions from compilation, so they take up no space or time.

The following option to catkin_make will define the NDEBUG macro for all your ROS packages, and thereby remove assertion checks.
catkin_make -DCMAKE_CXX_FLAGS:STRING="-DNDEBUG"

Note: cmake will rebuild all your software when you run it with this command, and will remember the setting through subsequent catkin_make runs until you delete your build and devel directories and rebuild.

判断CPU 架构和操作系统类型|C++C++ 基础
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
#include<iostream>
  
int main()
{
  #if defined __linux__
      std::cout<<"linux system"<<std::endl;
  #elif defined _WIN32
      std::cout<<"windows system"<<std::endl;
  #endif
   
  #if defined  __aarch64__
      std::cout<<"this is arm cpu"<<std::endl;
  #elif defined __x86_64__
      std::cout<<"this id x86 cpu"<<std::endl;
  #endif
      return 0;
}

cmake 中判断CPU 架构，操作系统类型

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
cmake_minimum_required(VERSION 3.10.0)
 
message(${CMAKE_HOST_SYSTEM_NAME})
message(${CMAKE_HOST_SYSTEM_PROCESSOR})
 
if(CMAKE_HOST_SYSTEM_NAME MATCHES "Linux")
    message("this is Linux")
elseif(CMAKE_HOST_SYSTEM_NAME MATCHES "Windows")
  message("this is Windows")
endif()
if(CMAKE_HOST_SYSTEM_PROCESSOR MATCHES "aarch64")
    message("this is aarch64 cpu")
elseif(CMAKE_HOST_SYSTEM_PROCESSOR MATCHES "x86_64")
  message("this is x86_64 cpu")
endif()

根据编译器的情况，有下面的宏用于调试程序:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
//判断是不是C++环境，需要注意的是ROS环境中这里为否
#ifdef _cplusplus
  printf("C++\n");
#endif
//判断是不是C环境
#ifdef __STDC__
  printf("C\n");
#endif
//输出语句所在函数，行，文件等参数
printf("function %s: Line 25\n",__func__);   //或者用__FUNCTION__
printf("pretty function %s: Line 25\n",__PRETTY_FUNCTION__);   //函数声明，包括了参数
printf("line: %d\n",__LINE__);
printf("current file: %s\n",__FILE__);
printf("date: %s\n",__DATE__);
printf("time: %s\n",__TIME__);

Costmap2DROS的函数getRobotPose|路径规划代价地图

源码其实并不复杂

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
bool Costmap2DROS::getRobotPose(tf::Stamped<tf::Pose>& global_pose) const
{
  global_pose.setIdentity();
  tf::Stamped < tf::Pose > robot_pose;
  robot_pose.setIdentity();
  robot_pose.frame_id_ = robot_base_frame_;
  robot_pose.stamp_ = ros::Time();
  ros::Time current_time = ros::Time::now();  // save time for checking tf delay later
  try
  {
    tf_.transformPose(global_frame_, robot_pose, global_pose);
  }
  catch (tf::LookupException& ex)
  {
    ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
    return false;
  }
  catch (tf::ConnectivityException& ex)
  {
    ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
    return false;
  }
  catch (tf::ExtrapolationException& ex)
  {
    ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
    return false;
  }
  // check global_pose timeout
  if (current_time.toSec() - global_pose.stamp_.toSec() > transform_tolerance_)
  {
    ROS_WARN_THROTTLE(1.0,
                      "Costmap2DROS transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f",
                      current_time.toSec(), global_pose.stamp_.toSec(), transform_tolerance_);
    return false;
  }
  return true;
}

核心是函数 transformPose，tf_在这里是tf::TransformListener，但函数实际是继承自基类tf::Transformer，有两个重载，常用的是第一个

1
2
3
4
5
6
7
8
9
10
11
void Transformer::transformPose(const std::string&   target_frame,
                                const Stamped< tf::Pose >&   stamped_in,
                                Stamped< tf::Pose >&   stamped_out 
) const

void Transformer::transformPose(const std::string&   target_frame,
                                const ros::Time&   target_time,
                                const Stamped< tf::Pose >&   stamped_in,
                                const std::string&   fixed_frame,
                                Stamped< tf::Pose >&   stamped_out 
) const

第二个重载会调用函数lookupTransform
1
2
3
4
5
6
7
8
// Get the transform between two frames by frame ID assuming fixed frame.
void Transformer::lookupTransform ( const std::string &   target_frame,
	const ros::Time &   target_time,
	const std::string &   source_frame,
	const ros::Time &   source_time,
	const std::string &   fixed_frame,
	StampedTransform &  transform 
)   const

  • target_frame The frame to which data should be transformed
  • target_time The time to which the data should be transformed. (0 will get the latest)
  • source_frame The frame where the data originated
  • source_time The time at which the source_frame should be evaluated. (0 will get the latest)
  • fixed_frame The frame in which to assume the transform is constant in time.
  • transform The transform reference to fill.

可能抛出的异常

1
2
3
4
5
6
7
8
namespace tf{
  // Pass through exceptions from tf2
  typedef tf2::TransformException TransformException;
  typedef tf2::LookupException LookupException;
  typedef tf2::ConnectivityException ConnectivityException;
  typedef tf2::ExtrapolationException ExtrapolationException;
  typedef tf2::InvalidArgumentException InvalidArgument; 
 }

报警 Costmap2DROS transform timeout

报警对应的transform_tolerance_是在构造函数里写死的0.3，一开始我以为这个数太小了，于是改为2，结果没有改善。

以下报警的根源都是Costmap2DROS::getRobotPose

获取不到开始位姿，无法生成全局路径

尝试的改善措施(全都无效):

  • Costmap2DROS构造函数中设置: transform_tolerance_(2)
  • timer_ = private_nh.createTimer(ros::Duration(.1), &Costmap2DROS::movementCB, this); 0.1秒间隔太短了。增大到1.5，稍有改善，但未解决根本问题，反而降低了代价地图的性能。
  • 降低两个代价地图的update_frequency

所有报警基本来自Costmap2DROS::getRobotPose，或者说map—->base_link的tf变换不及时。

  1. 如果不启动VSLAM，启动诠视相机的xv_sdk.launch，使用static_transform_republisher建立假的TF树，启动导航框架，一个报警也没有。因此问题还在VSLAM，怀疑是CPU而不是网络问题。
  2. 启动诠视相机的xv_sdk.launch和VSLAM，但不发布tf，使用假的tf树，启动导航框架，仍然没有报警。
  3. 继续缩小问题范围，发布假的tf: world—->map—->odom—-> base_link，导航框架不报警。但是如果由VSLAM发布map—->odom，就有报警了。

所以问题的原因：

  1. VSLAM的map—->odom—-> base_link变换。
  2. Nano的CPU没有使用到极致，还有多余算力

在此情况下规划路径时，全局路径可生成，因为全局规划频率是0，虽然有很多报警，总能成功规划一次。但局部路径就不同了，需要频繁规划。
只看TF的频率和tf echo有没有数据还不够，应该看Rviz里，base_link坐标系在map中的坐标是否能随着机器人的移动实时更新。先满足这一点，再运行导航框架。

另一种原因是网络带宽不足
unique_lock|C++多线程

unique_lock是管理具有唯一所有权的互斥对象的对象。类似智能指针unique_ptr。在构造时（或通过移动分配给它），对象获得一个互斥对象，负责该对象的锁定和解锁操作。该对象支持两种状态：锁定和解锁。此类保证在销毁时将管理的互斥对象解锁（即使未显式调用解锁函数）。因此，它能够保证互斥对象在抛出异常的情况下正确解锁，克服了mutex使用中的缺点。但是，unique_lock对象不以任何方式管理互斥对象的生命周期：这要求互斥对象的持续时间应至少延长到管理它的unique_lock销毁。

unique_lock是一个模板类，需要使用Mutex类型作为模板参数。

  • try_lock初始化: 新创建的unique_lock对象管理Mutex对象m，并尝试调用m.lock()对Mutex对象进行上锁，如果此时另外某个unique_lock对象已经管理了该Mutex对象m，则当前线程将会被阻塞。

  • try-locking初始化: 新创建的unique_lock对象管理Mutex对象m，并尝试调用m.try_lock()对Mutex对象进行上锁，但如果上锁不成功，并不会阻塞当前线程

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
boost::mutex mut;

void bar()
{
   boost::unique_lock<boost::mutex> lock(mut);
   std::cout << "This is bar!" << std::endl;

   if( lock.owns_lock() )
       cout << "owns lock" << endl;
   else
      lock.lock();
}

void foo()
{
    boost::unique_lock<boost::mutex> lock(mut);
    std::cout << "This is foo!" << std::endl;
}
递归锁 recursive_mutex|C++多线程

如果一个mutex被许多模块和类共享，就使用boost::recursive_mutex，如果只被一个模块和类使用，而且不需要re-entrant特性， 就使用boost::mutex

特点:

  1. 递归锁是互斥锁的一种，同一线程对其多次加锁不会产生死锁。
  2. 递归锁会使用引用计数机制，以便可以从同一线程多次加锁、解锁，当加锁、解锁次数相等时，锁才可以被其他线程获取。
  3. 能用普通互斥锁就不要用递归锁，会增大开销
  4. 无法定义lock递归锁的次数，当然也不是无限的，次数太多会有报错
  5. 递归锁也可以在循环中使用
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
#include <ros/ros.h>
#include <ros/console.h>
#include <boost/thread.hpp>
#include <iostream>
using namespace std;

boost::mutex  mtx;

void bar()
{
    // ! dead-locked here
    boost::mutex::scoped_lock lLock(mtx); 
    std::cout << "bar" << std::endl;
}

void foo()
{
    boost::mutex::scoped_lock lLock(mtx);
    std::cout << "foo" << std::endl;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "test_curve");
  ros::NodeHandle n;

  foo();
  bar();
  return 0; 
}

这种情况下，线程正常运行，不会死锁。

但是这种情况下，线程就死锁了，只能运行foo，因为mutex不可重入，foo这个线程没有运行完

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
#include <ros/ros.h>
#include <ros/console.h>
#include <boost/thread.hpp>
#include <iostream>
using namespace std;

boost::mutex  mtx;

void bar()
{
    boost::mutex::scoped_lock lLock(mtx); // dead-locked here
    std::cout << "bar" << std::endl;
}

void foo()
{
    boost::mutex::scoped_lock lLock(mtx);
    std::cout << "foo" << std::endl;
    bar();
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "test_curve");
  ros::NodeHandle n;

  foo();
  return 0; 
}

如果把mutex换成recursive_mutex，又能正常运行了。

recursive_mutex类可多次进入锁，这样在递归时可以避免一次死锁的几率，这是原本BOOST中所体现的一种思想。递归锁可多次锁，同样的需要多次解锁，那么问题就来了，如果真正或可能造成这次死锁的位置在某个已经进入死锁的地方，那么死锁的位置可能并不是你需要的位置，简单的说，如果A锁了3次，B锁一次，C锁一次，这样就锁出现死锁，排错之困难可想而知。


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
#include <ros/ros.h>
#include <boost/thread.hpp>
#include "boost/thread/mutex.hpp"
using namespace std;

boost::recursive_mutex  mtx;
int sum = 10;

void thread_1(int n)
{
  mtx.lock();
  mtx.lock();
  mtx.lock();
  sum = sum * n;
  cout<<"thread 1, sum: "<< sum <<endl;
  mtx.unlock();
//   mtx.unlock();
//   mtx.unlock();
}

void thread_2(int n)
{
  mtx.lock();
  sum = sum * 7 * n;
  cout<<"thread 2, sum: "<< sum <<endl;
  mtx.unlock();
}

int main()
{
    unsigned int n = 2;
    boost::thread th_1 = boost::thread(boost::bind(&thread_1,  n) );
    boost::thread th_2 = boost::thread(boost::bind(&thread_2,  n) );

    th_1.join();
    th_2.join();
    return 0;
}

thread_1里如果lock3次，unlock1次，运行时会阻塞在thread_1。只有3次unlock才会正常运行两个线程。