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
均值平滑处理路径|路径规划路径平滑

最简单的平滑算法，处理A星路径。效果如下
原始路径.png
平滑之后的路径.png


均值平滑的使用不宜太多，如图map坐标系的Y轴正向是朝左的，红色路径最左边的点，经过3次均值平滑，肯定会朝右平移，造成平滑后的路径靠近了障碍物

平均采样，获得anchor的index

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
/**
 * 平均分割区间 [start, end] 为num段，num+1 个点，存入sliced
 * start and end 为sliced的首尾元素
 */
template <typename T>
void uniform_slice(const T start, const T end, uint32_t num,
                   std::vector<T>* sliced)
{
  if (!sliced || num == 0)
        return;
  const T delta = (end - start) / num;
  sliced->resize(num + 1);
  T  s = start;
  for (uint32_t i = 0; i < num; ++i, s += delta)
  {
      sliced->at(i) = s;
  }
  sliced->at(num) = end;
}

补充

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
vector<Point2D> samplePoints(const vector<Point2D>& input_points, int interval)
{
    vector<Point2D> anchor_points;

    int sampling_num = std::max(2, static_cast<int>(input_points.size() / interval + 0.5)  );
    
    vector<double> anchor_id;
    Point2D temp_point;
    uniform_slice(0, input_points.size(), sampling_num - 1, &anchor_id);
    
    for(int i=0; i< anchor_id.size(); i++)
    {
        // cout << anchor_id.at(i) << "   ";
        temp_point = input_points.at(anchor_id.at(i) );
        // cout << "anchor X: " << temp_point.first <<endl;
        // cout << "anchor Y: " << temp_point.second <<endl;
        anchor_points.push_back(temp_point);
    }
    return anchor_points;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
/**
 * Check if two points u and v are the same point on XY dimension.
 * @param u one point that has member function x() and y().
 * @param v one point that has member function x() and y().
 * @return sqrt((u.x-v.x)^2 + (u.y-v.y)^2) < epsilon, i.e., the Euclid distance on XY dimension.
 */
template <typename U, typename V>
bool SamePointXY(const U& u, const V& v)
{
    static constexpr double kMathEpsilonSqr = 1e-8 * 1e-8;
    return (u.x() - v.x()) * (u.x() - v.x()) < kMathEpsilonSqr &&
         (u.y() - v.y()) * (u.y() - v.y()) < kMathEpsilonSqr;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
PathPoint GetWeightedAverageOfTwoPathPoints(const PathPoint& p1,
                                            const PathPoint& p2,
                                            const double w1, const double w2)
{
    PathPoint p;
    p.set_x(p1.x() * w1 + p2.x() * w2);
    p.set_y(p1.y() * w1 + p2.y() * w2);
    p.set_z(p1.z() * w1 + p2.z() * w2);
    p.set_theta(p1.theta() * w1 + p2.theta() * w2);
    p.set_kappa(p1.kappa() * w1 + p2.kappa() * w2);
    p.set_dkappa(p1.dkappa() * w1 + p2.dkappa() * w2);
    p.set_ddkappa(p1.ddkappa() * w1 + p2.ddkappa() * w2);
    p.set_s(p1.s() * w1 + p2.s() * w2);
    return p;
}
1
2
3
4
5
6
7
8
9
10
11
12
// Test whether two float or double numbers are equal.
template <typename T>
typename std::enable_if<!std::numeric_limits<T>::is_integer, bool>::type
IsFloatEqual(T x, T y, int ulp = 2)
{
  // the machine epsilon has to be scaled to the magnitude of the values used
  // and multiplied by the desired precision in ULPs (units in the last place)
  return std::fabs(x - y) <
             std::numeric_limits<T>::epsilon() * std::fabs(x + y) * ulp
         // unless the result is subnormal
         || std::fabs(x - y) < std::numeric_limits<T>::min();
}
相机点云无法从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树后，发现报警依然，应该还是时间戳问题，懒得再对齐了。