障碍层 (二) 初始化和 pointCloud2Callback

onInitialize

从参数服务器加载参数,并调用matchSize函数,根据主地图的参数来设置障碍层地图。

依据track_unknown_space参数,对代价地图取不同的默认值:

1
2
3
4
if (track_unknown_space)
default_value_ = NO_INFORMATION;
else
default_value_ = FREE_SPACE;

expected_update_rate参数

topics_string记录的观测源逐个输出到source字符串,并找到对应每个观测源的参数。

为每个观测源创建一个buffer,并将其指针存放进observation_buffers_中进行管理。并根据标志位确定是否将其添加到marking_buffers(标记障碍的观测数据) 与 clearing_buffers(用于清除小车与障碍间cost的观测数据):

1
2
3
4
5
6
7
8
9
10
11
12
// create an observation buffer
observation_buffers_.push_back(
boost::shared_ptr < ObservationBuffer>(new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height,
obstacle_range, raytrace_range, *tf_, global_frame_,
sensor_frame, transform_tolerance) ) );

// check if we'll add this buffer to our marking observation buffers
if (marking)
marking_buffers_.push_back(observation_buffers_.back());
// check if we'll also add this buffer to our clearing observation buffers
if (clearing)
clearing_buffers_.push_back(observation_buffers_.back());

然后分别针对不同的sensor类型(LaserScan、PointCloud、PointCloud2)注册不同的回调函数,相应的订阅者和filter还要插入相应的容器

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
if (data_type == "LaserScan")
......
else if (data_type == "PointCloud")
......
else // PointCloud2
{
boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud2>
> sub(new message_filters::Subscriber<sensor_msgs::PointCloud2>(g_nh, topic, 50));
if (inf_is_valid)
ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");

boost::shared_ptr < tf::MessageFilter<sensor_msgs::PointCloud2>
> filter(new tf::MessageFilter<sensor_msgs::PointCloud2>(*sub, *tf_, global_frame_, 50) );
filter->registerCallback(
boost::bind(&ObstacleLayer::pointCloud2Callback, this, _1, observation_buffers_.back() ) );
observation_subscribers_.push_back(sub); // 传感器数据订阅者
observation_notifiers_.push_back(filter); // 保证变换可行
}

ObservationBuffer类是专门用于存储观测数据的类,它是ObstacleLayer的类成员。这里关注一下它的bufferCloud函数

对三种传感器数据,都是先将收到的message转换为sensor_msgs::PointCloud2,再调用ObservationBuffer::bufferCloud,将点云数据存到buffer中,它实际又把点云消息转换为pcl::PointCloud < pcl::PointXYZ >格式后,再调用bufferCloud的重载函数。

这就是一个巨大的缺陷,深度相机和雷达获取的障碍都按PointCloud2类型放到buffer中,对两种障碍只能用同样的costmap参数,比如膨胀半径,影响了导航的调试

ObstacleLayer::pointCloud2Callback

1
2
3
4
5
6
7
void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& message,
const boost::shared_ptr<ObservationBuffer>& buffer)
{
buffer->lock();
buffer->bufferCloud(*message);
buffer->unlock();
}

bufferCloud

非常重要的observation_list_就是在这里赋值的

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
71
72
73
74
75
76
77
78
79
80
void ObservationBuffer::bufferCloud(const pcl::PointCloud<pcl::PointXYZ>& cloud)
{
Stamped < tf::Vector3 > global_origin;

// std::list<Observation> observation_list_;
// create a new observation on the list to be populated
observation_list_.push_front(Observation());

// check whether the origin frame has been set explicitly or
// whether we should get it from the cloud
// yaml里可以不设置sensor_frame,在这里取点云的坐标系名称
string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_;

try
{
// given these observations come from sensors, we store the origin pt of the sensor
Stamped < tf::Vector3 > local_origin(tf::Vector3(0, 0, 0),
pcl_conversions::fromPCL(cloud.header).stamp, origin_frame);

// global_frame_ 来源是obstacle_layer.cpp 中的 layered_costmap_->getGlobalFrameID();
// 转换 global_frame_ 和 origin_frame坐标系
tf_.waitForTransform(global_frame_, local_origin.frame_id_,
local_origin.stamp_, ros::Duration(0.5) );
// local_origin 转换到 global_frame_坐标系,变成global_origin
tf_.transformPoint(global_frame_, local_origin, global_origin);
observation_list_.front().origin_.x = global_origin.getX();
observation_list_.front().origin_.y = global_origin.getY();
observation_list_.front().origin_.z = global_origin.getZ();

// pass on the raytrace/obstacle range of the observation buffer to the observations
observation_list_.front().raytrace_range_ = raytrace_range_;
observation_list_.front().obstacle_range_ = obstacle_range_;

pcl::PointCloud < pcl::PointXYZ > global_frame_cloud;

// 把参数 cloud 转到global_frame_坐标系
pcl_ros::transformPointCloud(global_frame_, cloud, global_frame_cloud, tf_);
global_frame_cloud.header.stamp = cloud.header.stamp;

// now we need to remove observations from the cloud that are
// below or above our height thresholds

// 新的变量 observation_cloud,指向observation_list_的成员 cloud_
pcl::PointCloud < pcl::PointXYZ > &observation_cloud = *(observation_list_.front().cloud_);
unsigned int cloud_size = global_frame_cloud.points.size();
observation_cloud.points.resize(cloud_size);
unsigned int point_count = 0;

// copy over the points that are within our height bounds
// 从 global_frame_cloud中取出符合 z坐标的点,放到 observation_cloud
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];
}
}

// resize the cloud for the number of legal points
observation_cloud.points.resize(point_count);
// 时间戳和坐标系也复制过来
observation_cloud.header.stamp = cloud.header.stamp;
observation_cloud.header.frame_id = global_frame_cloud.header.frame_id;
}
catch (TransformException& ex)
{
// if an exception occurs, we need to remove the empty observation from the list
observation_list_.pop_front();
ROS_ERROR("TF Exception that should never happen for sensor frame: %s,
cloud frame: %s, %s", sensor_frame_.c_str(),
cloud.header.frame_id.c_str(), ex.what());
return;
}
// if the update was successful, we want to update the last updated time
last_updated_ = ros::Time::now();

// remove any stale observations from the list
purgeStaleObservations();
}


服务 clear_costmaps

服务/move_base/clear_costmaps,类型std_srvs/Empty

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
// 定义在 MoveBase构造函数
clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);

// 回调函数,其实就是两个代价地图重置所有层
bool MoveBase::clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
{
//clear the costmaps
boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock_controller(*(controller_costmap_ros_->getCostmap()->getMutex()));
controller_costmap_ros_->resetLayers();

boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock_planner( *(planner_costmap_ros_->getCostmap()->getMutex()) );
planner_costmap_ros_->resetLayers();
return true;
}

// 主要是 Costmap2D::resetMap 和 Layer::reset()
void Costmap2DROS::resetLayers()
{
Costmap2D* top = layered_costmap_->getCostmap();
top->resetMap(0, 0, top->getSizeInCellsX(), top->getSizeInCellsY());

std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin();
plugin != plugins->end(); ++plugin)
{
(*plugin)->reset(); //每一层覆盖Layer的虚函数reset()
}
}

先看重置代价地图,这个简单,就是把地图的char值重置为默认值0 FREE_SPACE

1
2
3
4
5
6
7
void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
{
boost::unique_lock<mutex_t> lock(*(access_) );
unsigned int len = xn - x0;
for (unsigned int y = y0 * size_x_ + x0; y < yn * size_x_ + x0; y += size_x_)
memset(costmap_ + y, default_value_, len * sizeof(unsigned char));
}

reset 所有层

三个层的reset()函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
void InflationLayer::reset() { onInitialize(); }

void ObstacleLayer::reset()
{
deactivate();
resetMaps();
current_ = true;
activate();
}

void StaticLayer::reset()
{
if (first_map_only_)
{
has_updated_data_ = true;
}
else
{
onInitialize();
}
}

这里障碍层的把栅格设置为默认值,取决于参数track_unknown_space,但这个参数一般设置为true,所以栅格的默认值是 NO_INFORMATION

最好不要高频率call clear_costmaps服务。一般是周期性调用,防止代价地图上有太多的garbage


costmap_converter产生的障碍比实际更大

12-16__171652.png

参数cluster_max_distance不能调大,否则线障碍更多,比如这样:
12-17__143426.png


雷达扫描到不存在的障碍

雷达默认参数

  1. 机器人的杂物挡住了雷达.png
    这是常见的一种情况
  1. 起初经过观察,发现在一些地方容易出现,而且是都在雷达的左侧出现,于是怀疑是雷达自身问题,最后把angle_min的绝对值改小,不再出现这个问题。
    雷达扫描到不存在的障碍

后来又出现这种现象,而且不分左右,有时看不出在哪边
不存在的障碍导致全局路径改变


根据全局路径的目标和多个机器人间距优化速度
abstract Welcome to my blog, enter password to read.
Read more
(十七) 可视化
1
2
3
4
5
6
planner_->visualize();
visualization_->publishObstacles(obstacles_);
// 很简单,显示 via_point, 坐标来源是 via_points_
visualization_->publishViaPoints(via_points_);
// 很简单,实际调用的是 base_local_planner::publishPlan
visualization_->publishGlobalPlan(global_plan_);

TebOptimalPlanner::visualize

1
2
3
4
5
6
7
8
9
10
11
12
void TebOptimalPlanner::visualize()
{
if (!visualization_)
return;

visualization_->publishLocalPlanAndPoses(teb_);
if (teb_.sizePoses() > 0)
visualization_->publishRobotFootprintModel(teb_.Pose(0), *robot_model_);

if (cfg_->trajectory.publish_feedback)
visualization_->publishFeedbackMessage(*this, *obstacles_);
}

最后部分是发布feedback消息 (single trajectory overload),话题teb_feedback. 需要设置publish_feedback为true,默认为false

feedback message包括规划的轨迹,速度和时间信息(temporal information),也包括 active obstacles

publishLocalPlanAndPoses很简单,需要注意的是

1
2
3
4
// 不明白z为什么这样赋值 ???
pose.pose.position.z = cfg_->hcp.visualize_with_time_as_z_axis_scale *
teb.getSumOfTimeDiffsUpToIdx(i);
// getSumOfTimeDiffsUpToIdx 就是从起点到当前位姿i的所有deltaT的总和

publishRobotFootprintModel也很简单,机器人在局部路径第一个点的时候,可视化轮廓,就是一些对visualization_msgs::Marker的操作。不过对不同的轮廓类型,调用的函数不同

publishFeedbackMessage

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
void TebVisualization::publishFeedbackMessage(const TebOptimalPlanner& teb_planner, const ObstContainer& obstacles)
{
FeedbackMsg msg;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = cfg_->map_frame;
msg.selected_trajectory_idx = 0;
msg.trajectories.resize(1);
msg.trajectories.front().header = msg.header;

teb_planner.getFullTrajectory(msg.trajectories.front().trajectory);
// add obstacles
msg.obstacles_msg.obstacles.resize(obstacles.size());
for (std::size_t i=0; i<obstacles.size(); ++i)
{
msg.obstacles_msg.header = msg.header;
// copy polygon
msg.obstacles_msg.obstacles[i].header = msg.header;
obstacles[i]->toPolygonMsg(msg.obstacles_msg.obstacles[i].polygon);
// copy id
msg.obstacles_msg.obstacles[i].id = i; // TODO: we do not have any id stored yet

// orientation
//msg.obstacles_msg.obstacles[i].orientation =; // TODO
// copy velocities
obstacles[i]->toTwistWithCovarianceMsg(msg.obstacles_msg.obstacles[i].velocities);
}
feedback_pub_.publish(msg);
}

publishObstacles

这里需要注意的是点障碍的可视化

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
visualization_msgs::Marker marker;
marker.header.frame_id = cfg_->map_frame;
marker.header.stamp = ros::Time::now();
marker.ns = "PointObstacles"; marker.id = 0;
marker.type = visualization_msgs::Marker::POINTS;
marker.action = visualization_msgs::Marker::ADD;
marker.lifetime = ros::Duration(2.0);

for (ObstContainer::const_iterator obst = obstacles.begin(); obst != obstacles.end(); ++obst)
{
boost::shared_ptr<PointObstacle> pobst = boost::dynamic_pointer_cast<PointObstacle>(*obst);
if (!pobst) continue;

if (cfg_->hcp.visualize_with_time_as_z_axis_scale < 0.001)
{
geometry_msgs::Point point;
point.x = pobst->x();
point.y = pobst->y();
point.z = 0;
marker.points.push_back(point);
}
else // Spatiotemporally point obstacles become a line
{
marker.type = visualization_msgs::Marker::LINE_LIST;
geometry_msgs::Point start;
start.x = pobst->x(); start.y = pobst->y();
start.z = 0;
marker.points.push_back(start);

geometry_msgs::Point end;
double t = 20;
Eigen::Vector2d pred;
pobst->predictCentroidConstantVelocity(t, pred);
end.x = pred[0];
end.y = pred[1];
end.z = cfg_->hcp.visualize_with_time_as_z_axis_scale * t;
marker.points.push_back(end);
}
}
// 省略 marker的着色
teb_marker_pub_.publish( marker );

HomotopyClassPlanner::visualize

visualization_的赋值在HomotopyClassPlanner::initialize 最后的 setVisualization(visual);

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
if (visualization_)
{
// Visualize graph
if (cfg_->hcp.visualize_hc_graph && graph_search_)
visualization_->publishGraph(graph_search_->graph_);

// Visualize active tebs as marker
visualization_->publishTebContainer(tebs_);

// Visualize best teb and feedback message if desired
TebOptimalPlannerConstPtr best_teb = bestTeb();
if (best_teb)
{
// 只发布best_teb的局部路径,类型是nav_msgs::Path 和 geometry_msgs::PoseArray
// 对应话题 local_plan 和 teb_poses
// 位姿点的z = cfg_->hcp.visualize_with_time_as_z_axis_scale *
// teb.getSumOfTimeDiffsUpToIdx(i)

visualization_->publishLocalPlanAndPoses(best_teb->teb() );
if (best_teb->teb().sizePoses() > 0) // 路径的位姿点个数
visualization_->publishRobotFootprintModel(best_teb->teb().Pose(0), *robot_model_);

if (cfg_->trajectory.publish_feedback) // feedback message
{
int best_idx = bestTebIdx();
if (best_idx>=0)
visualization_->publishFeedbackMessage(tebs_,
(unsigned int) best_idx, *obstacles_ );
}
}
}
else ROS_DEBUG("Ignoring HomotopyClassPlanner::visualize() call, since no visualization class was instantiated before.");


ROS点云消息的解析和点云类型的转换

消息类型

sensor_msgs/PointCloud2类型如下:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
# N维的点云,点的数据以二进制blob形式保存,其layout用fields数组描述

# The point cloud data may be organized 2d (image-like) or 1d
# (unordered). Point clouds organized as 2d images may be produced by
# camera depth sensors such as stereo or time-of-flight.

Header header

点云的2D结构,如果点云无序,height是1,width就是点云的length
uint32 height
uint32 width

# 用二进制数据blob的形式,描述channels 及其layout
PointField[] fields

bool is_bigendian
uint32 point_step # Length of a point in bytes
uint32 row_step # Length of a row in bytes
uint8[] data # 点的数据,size = row_step * height
bool is_dense # True if there are no invalid points

PointCloud2的data是序列化后的数据,直接看不到物理意义。只能转为点云1类型

sensor_msgs/PointCloud类型如下:

1
2
3
4
5
6
7
8
9
Header header

# 在header坐标系中的3D点云
geometry_msgs/Point32[] points

# Each channel should have the same number of elements as points array,
# and the data in each channel should correspond 1:1 with each point.
# Channel names in common practice are listed in ChannelFloat32.msg.
ChannelFloat32[] channels

坐标系和点云的位置关系如下,我们感兴趣的是z,也就是相机坐标系到点云的距离

sensor_msgs::PointCloud2类型转为sensor_msgs::PointCloud,而且获知坐标

1
2
3
4
5
6
7
8
9
10
11
12
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>

sensor_msgs::PointCloud2 in_pcl;
sensor_msgs::PointCloud out_pcl;
sensor_msgs::convertPointCloud2ToPointCloud(in_pcl, out_pcl);
for (int i=0; i < out_pcl.points.size(); i++)
{
ROS_INFO("out_pcl x: %f, y: %f, z: %f",
out_pcl.points[i].x, out_pcl.points[i].y, out_pcl.points[i].z);
}

消息的发布

意外发现ROS可以直接发布pcl的类型,不必转为ROS的消息类型

1
2
typedef pcl::PointCloud<PointT> PointCloudT;
PCL2_publisher_ = nh.advertise<PointCloudT>("pointcloud2", 1, this);

ros在pcl_ros中做了Publisher的多态。使得ros的标准Publisher可以接收pcl::PointCloud<T>类型的消息,并自动转换成sensor_msgs::PointCloud2的消息类型,再发布出去。

几种点云类型的转换

点云格式主要有四种,sensor_msgs::PointCloud,也就是点云1已经弃用,主要用点云2,但是点云1有时还有用。比如点云2转为点云1消息,可看到包含多少个点,rostopic echo --noarr pcl1/points,结果可以是<array type: geometry_msgs/Point32, length: 256000>,可看到每个点的坐标值。

  • sensor_msgs::PointCloud

  • sensor_msgs::PointCloud2

  • pcl::PCLPointCloud2 — PCL data structure mostly for compatibility with ROS (I think)

  • pcl::PointCloud — standard PCL data structure

相互转换关系
find_package里添加sensor_msgs, pcl_ros, pcl_conversions

ROS PointCloud2 和 PointCloud的转换

1
2
sensor_msgs::PointCloud out_pcl;
sensor_msgs::convertPointCloud2ToPointCloud(*pcl_dist_msg, out_pcl);

反过来就用sensor_msgs::convertPointCloudToPointCloud2

可以使用point_cloud_converter在点云1和点云2 之间转换

1
2
PointCloudConverter initialized to transform from PointCloud (/points_in) to PointCloud2 (/points2_out).
PointCloudConverter initialized to transform from PointCloud2 (/camera_/depth/points) to PointCloud (/pcl)

比如 rosrun point_cloud_converter point_cloud_converter_node points2_in:=/camera/depth/points points_out:=/pcl1

PointCloud2 转 pcl::PCLPointCloud2

1
2
3
4
sensor_msgs::PointCloud2 cloud;
pcl::PCLPointCloud2 pcl_pc2;

pcl_conversions::toPCL(cloud, pcl_pc2

pcl::PCLPointCloud2 转 pcl::PointCloud < pcl::PointXYZ >

1
2
3
4
pcl::PCLPointCloud2  pcl_pc2;
pcl::PointCloud < pcl::PointXYZ > pcl_cloud;

pcl::fromPCLPointCloud2(pcl_pc2, pcl_cloud);

pcl::PointCloud < pcl::PointXYZ > 转 sensor_msgs::PointCloud2

1
2
3
4
5
#include <pcl_conversions/pcl_conversions.h>

pcl::PointCloud<pcl::PointXYZ>::Ptr inCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
sensor_msgs::PointCloud2 cloud;
pcl::toROSMsg(*inCloud, cloud);

反过来就是fromROSMsg(const sensor_msgs::PointCloud2 &, pcl::PointCloud<T>&)

#include <sensor_msgs/point_cloud2_iterator.h>

参考: pcl_conversions


膨胀层

InflationLayer的构造及初始化方法和StaticLayer类似。该层是基于已有地图进行膨胀,因此不需要有缺省值。由Layer类单独派生出InflationLayer类. 用于执行每个cell的障碍物膨胀,由于它的父类中不含Costmap2D类,所以其实膨胀层自身没有栅格地图要维护,这一点和另外两层有区别。

  • inflation_radius (double, default: 0.55) 障碍物在地图中向外扩展的膨胀区的半径
  • cost_scaling_factor (double, default: 10.0) 膨胀过程中应用到代价值的比例因子。这个值越大,代价衰减越严重

膨胀层加载过程.png
膨胀层所用的参数:inflation params

可以修改膨胀层代码 inflation_layer.h, 添加函数double getInflationRadius() constdouble getInscribedRadius() const

每个周期传感器数据进来后,都要在代价地图底层占用结构上执行标记和清除障碍操作,并且这种结构会被投影到代价地图附上相应代价值。 这完成之后,对代价赋值为costmap_2d::LETHAL_OBSTACLE的每个cell执行障碍物的膨胀操作,即从每个代价cell向外传播代价值,直到用户定义的膨胀半径为止。这里确实只需要对状态为LETHAL_OBSTACLE的cell进行膨胀操作即可。


ldd, nm, c++filt, ldconfig命令

ldd

命令的作用是打印共享库的依赖关系

  1. ldd不是一个可执行程序,而只是一个shell脚本。ldd能够显示可执行模块的 dependency,其原理是通过设置一系列的环境变量,如下:LD_TRACE_LOADED_OBJECTS、LD_WARN、LD_BIND_NOW、LD_LIBRARY_VERSION、 LD_VERBOSE等。

  2. ldd的工作原理,其实质是通过ld-linux.so(elf动态库的装载器)来实现的。ld-linux.so模块会先于executable模块程序工作,并获得控制权,因此当上述的那些环境变量被设置时,ld-linux.so选择了显示可执行模块的dependency。
    实际上可以直接执行ld-linux.so模块,如:/lib/ld-linux.so.2 --list program(这相当于ldd program)

1
2
/usr/bin/ld: skipping incompatible /home/user/lib/libtinyalsa.so when searching for -ltinyalsa
/usr/bin/ld: cannot find -ltinyalsa: No such file or directory

so库的架构不匹配,可以通过file name.so查看so库的架构,也可以使用readelf命令查看so文件架构信息的示例: readelf -h libexample.so。输出so文件的头部信息,其中包括Machine字段,即架构信息。Machine字段的值对应于不同的架构,比如x86、aarch64、MIPS等。


编译时常常会出错: undefine reference XXX,显然是不识别函数,比如 _ZN9fast_gicp7NDTCudaIN3pcl9PointXYZIES2_EC1Ev,但是这个名称看得不清晰,看不出原来的函数原型。

先执行nm -D libhdl_localization_nodelet.so | grep NDTCuda | grep EC1Ev,输出

1
U  _ZN9fast_gicp7NDTCudaIN3pcl9PointXYZIES2_EC1Ev

再执行命令 c++filt _ZN9fast_gicp7NDTCudaIN3pcl9PointXYZIES2_EC1Ev (去掉前面的 U), 输出函数原型

1
fast_gicp::NDTCuda<pcl::PointXYZI, pcl::PointXYZI>::NDTCuda()

c++filt命令确实太好用了。

ldconfig

有时装完某个库后,需要使用命令sudo ldconfig -v,否则在程序运行后会出现找不到动态库的问题: error while loading shared libraries: liblog4cpp.so.5: cannot open shared object file: No such file or directory

ldconfig是一个动态链接库管理命令,为了让动态链接库为系统所共享,还需运行动态链接库的管理命令

ldconfig通常在系统启动时运行,而当用户安装了一个新的动态链接库时,就需要手工运行这个命令.主要是在默认搜寻目录(/lib/usr/lib)以及动态库配置文件/etc/ld.so.conf内所列的目录下,搜索出可共享的动态链接库(格式如lib*.so*),进而创建出动态装入程序(ld.so)所需的连接和缓存文件.

ldconfig与运行程序时有关,跟编译时一点关系都没有。 不管做了什么动态库的变动后,最好都ldconfig一下

问题 error while loading shared libraries … file too short

这个错误应该是从别处拷贝so文件过来时出现了软链接错误。删除,重新创建这个软连接即可。
比如A,B,C的链接关系: A --> B, B --> C。先把C重命名,删掉A,B。重新建立软链接

1
2
ln -s C B
ln -s B A


move_base 所有参数
  • base_global_planner: 指定全局规划器

  • base_local_planner: 指定局部规划器

  • recovery_behaviors: true 是否允许恢复行为,恢复行为为原地旋转。 有时候不规则的机器人做恢复行为是比较危险的,容易发生碰撞,圆形机器人不会有这个问题

  • shutdown_costmaps: false 当 move_base进入inactive状态时候,决定是否停用节点的costmap (bool, default: false)

  • controller_frequency: 20.0 控制器频率,即向基座发送速度命令的频率

  • planner_patience: 5.0 在空间清理操作执行前,路径规划器等待多长时间(秒)用来找出一个有效规划 (double, default: 5.0)

  • controller_patience: 20.0 在空间清理操作执行前,控制器会等待多长时间(秒)用来找出一个有效控制 (double, default: 15.0)

  • planner_frequency: 5.0 全局路径规划的更新速率,可以是0. 当机器在避障表现较好时,此参数较小即可。 如果local planner问题很大,再频繁更新全局路径,可能会离原先的最优路径越来越远,直到陷入局部障碍物

  • oscillation_timeout: 10.0 执行修复操作之前,允许的震荡时间是几秒

  • oscillation_distance: 0.5 机器人需要移动多少距离才算作没有震荡

  • conservative_reset_dist: 3.0 当在地图中清理出空间时候,距离机器人几米远的障碍将会从costmap清除。 实验中发现局部地图经常会有移动障碍物遗留,并且不会去除,只有在移动后才能清除,部分只能在恢复行为后清除,所以有一个插件是专门用来做local障碍物清除的
    剩余参数用于无法行动之后的恢复行为,震荡表示机器人在很短的距离反复运动,可以设置距离、时间来判断是否为震荡以及多久后执行恢复操作

move_base有很多unknown type的订阅

1
2
3
4
5
* /footprint [unknown type]
* /footprint_radius [unknown type]
* /move_base/cancel [unknown type]
* /move_base/goal [unknown type]
* /move_base_simple/goal [unknown type]

但是用rostopic info topic_name查看,没有出现 unknown type,怀疑是ROS的bug