Autoware感知瞎学笔记(二)roi_object_filter
目录
无人驾驶中,我们在对激光雷达数据进行聚类得到车身周边的目标,但是很多场景下我们其实并不关心道路以外(比如建筑物)的目标信息,所以在我们聚类前就应该想办法滤除掉额外点云。因此在雷达数据预处理的时候,最好是从高精地图拿到道路信息,将我们不关心的区域过滤掉。正好最近学习autoware,我们看下autoware是怎么做的。
代码分析:
打开autoware/core_perception/launch/lidar_euclidean_cluster_detect.launch:
可以看到,在这个欧式聚类节点的launch文件中,如果使用vector_map(autoware里的“高精地图”),则会启动一个叫做roi_object_filter的包,因此我们可以大胆猜测一下,这个应该是融合高精地图信息做的,不然它设立这个标志位干什么=、=。
<group if="$(arg use_vector_map)">
<node name="object_roi_filter_clustering" pkg="roi_object_filter" type="roi_object_filter"
output="screen" ns="/detection/lidar_detector">
<param name="objects_src_topic" value="/objects"/>
<param name="wayarea_gridmap_layer" value="$(arg wayarea_gridmap_layer)"/>
<param name="sync_topics" value="false"/>
<param name="exception_list" value="[person, bicycle]"/>
</node>
<node pkg="detected_objects_visualizer" type="visualize_detected_objects" name="cluster_detect_visualization_01"
output="screen" ns="/detection/lidar_detector">
<param name="objects_src_topic" value="/objects_filtered"/>
</node>
</group>
<group unless="$(arg use_vector_map)">
<node pkg="detected_objects_visualizer" type="visualize_detected_objects" name="cluster_detect_visualization_01"
output="screen" ns="/detection/lidar_detector">
<param name="objects_src_topic" value="/objects"/>
</node>
</group>
1. roi_object_filter_node.cpp
找到core_perception下面的roi_object_filter包,并且打开roi_object_filter_node.cpp。这里便是节点的入口,RosRoiObjectFilterApp 实例化一个类,并调用其Run()方法
#include "roi_object_filter/roi_object_filter.h"
int main(int argc, char **argv)
{
ros::init(argc, argv, __APP_NAME__);
RosRoiObjectFilterApp app;
app.Run();
return 0;
}
2. roi_object_filter.cpp
2.1 构造函数
初始化一些标志位,这里可以看到gridmap网格地图的字眼,会让人联想到occupancy grid map,这里先往下继续分析
RosRoiObjectFilterApp::RosRoiObjectFilterApp()
{
gridmap_ready_ = false;
detections_ready_ = false;
processing_ = false;
}
2.2 Run()函数
void RosRoiObjectFilterApp::Run() {
ros::NodeHandle private_node_handle("~");
tf::TransformListener transform_listener;
transform_listener_ = &transform_listener;
InitializeRosIo(private_node_handle);
ROS_INFO("[%s] Ready. Waiting for data...", __APP_NAME__);
ros::spin();
ROS_INFO("[%s] END", __APP_NAME__);
}
这里是声明一个局部tf的监听器变量,然后赋值给类内全局变量。tf监听器在类内声明的时候并没有初始化,因此在主线程开始前将其初始化一下赋予一个值。下面打开主要功能函数InitializeRosIo()
void RosRoiObjectFilterApp::InitializeRosIo(ros::NodeHandle &in_private_handle) {
std::string exception_list;
ros_namespace_ = ros::this_node::getNamespace();
if (ros_namespace_.substr(0, 2) == "//") {
ros_namespace_.erase(ros_namespace_.begin());
}
objects_src_topic_ = ros_namespace_ + "/objects";
wayarea_gridmap_topic_= "/grid_map_wayarea";
std::string roi_topic_str = ros_namespace_+ "/objects_filtered";
ROS_INFO("[%s] objects_src_topic: %s", __APP_NAME__, objects_src_topic_.c_str());
ROS_INFO("[%s] wayarea_gridmap_topic: %s", __APP_NAME__, wayarea_gridmap_topic_.c_str());
in_private_handle.param<bool>("sync_topics", sync_topics_, false);
ROS_INFO("[%s] sync_topics: %d", __APP_NAME__, sync_topics_);
in_private_handle.param<std::string>("wayarea_gridmap_layer", gridmap_layer_, "wayarea");
ROS_INFO("[%s] wayarea_gridmap_layer: %s", __APP_NAME__, gridmap_layer_.c_str());
in_private_handle.param<int>("wayarea_no_road_value", gridmap_no_road_value_, GRID_MAX_VALUE);
ROS_INFO("[%s] wayarea_no_road_value: %ds", __APP_NAME__, gridmap_no_road_value_);
in_private_handle.param<std::string>("exception_list", exception_list, "[person, bicycle]");
ROS_INFO("[%s] exception_list: %s", __APP_NAME__, exception_list.c_str());
YAML::Node exception_list_yaml;
try {
exception_list_yaml = YAML::Load(exception_list);
for (const auto &exception : exception_list_yaml) {
exception_list_.push_back(exception.as<std::string>());
}
} catch (const std::exception &e) {
ROS_INFO("[%s] Incorrect format for Exception List
(exception_list) param, using default.",__APP_NAME__);
exception_list_ = {"person", "bycicle"};
}
if (!sync_topics_) {
detections_range_subscriber_ = in_private_handle.subscribe(objects_src_topic_, 1,
&RosRoiObjectFilterApp::DetectionsCallback, this);
wayarea_gridmap_subscriber_ = in_private_handle.subscribe(wayarea_gridmap_topic_, 1,
&RosRoiObjectFilterApp::WayAreaGridMapCallback, this);
} else {
detections_filter_subscriber_ = new message_filters::Subscriber<
autoware_msgs::DetectedObjectArray(node_handle_, objects_src_topic_,1);
gridmap_filter_subscriber_ = new message_filters::Subscriber<
grid_map_msgs::GridMap>(node_handle_, wayarea_gridmap_topic_, 1);
detections_synchronizer_ = new message_filters::Synchronizer<SyncPolicyT>(SyncPolicyT(10),
*detections_filter_subscriber_, *gridmap_filter_subscriber_);
detections_synchronizer_->registerCallback(boost::bind(
&RosRoiObjectFilterApp::SyncedDetectionsCallback, this, _1, _2));
}
roi_objects_publisher_ = node_handle_.advertise<autoware_msgs::DetectedObjectArray>(roi_topic_str, 1);
ROS_INFO("[%s] Publishing filtered objects in %s", __APP_NAME__, roi_topic_str.c_str());
}
前面的部分都是ros通过参数服务器加载参数,我们直接看到后面,这里面有一个ros关于同步的知识点,如果不需要ros同步的话,则分别订阅两个topic,一个是关于雷达目标的topic,一个是关于wayarea_gridmap_topic。 同步的话,则利用ros自带的库message_filters::Synchronizer, 将多个topic数据放到一个callback回调函数里面处理,具体用法可以看ros.wiki页面
回到autoware这里,我们可以看到关于WayAreaGridMap的回调函数,将ros消息数据传到全局变量wayarea_gridmap_中,然后重置标志位。
void RosRoiObjectFilterApp::WayAreaGridMapCallback(const grid_map_msgs::GridMap::ConstPtr &in_message)
{
if (!processing_)
{
processing_ = true;
wayarea_gridmap_ = in_message;
processing_ = false;
}
}
再看关于雷达聚类检测出目标的回调函数:
void RosRoiObjectFilterApp::DetectionsCallback(const autoware_msgs::DetectedObjectArray::ConstPtr &in_detections)
{
if (!processing_ && !in_detections->objects.empty())
{
processing_ = true;
object_detections_ = in_detections;
SyncedDetectionsCallback(in_detections, wayarea_gridmap_);
processing_ = false;
}
}
这里先是将ros消息传入到全局变量,然后调用同步方式里的两个消息同步SyncedDetectioncallback来处理两个ros数据消息,那其实这里可以猜到雷达聚类目标的这个topic频率应该是高于wayarea_gridmap_的。
然后我们继续打开SyncedDetectionsCallback()同步callback:
void RosRoiObjectFilterApp::SyncedDetectionsCallback(
const autoware_msgs::DetectedObjectArray::ConstPtr &in_detections,
const grid_map_msgs::GridMap::ConstPtr &in_gridmap) {
if (nullptr == in_detections || nullptr == in_gridmap) {
ROS_INFO("[%s] Empty input messages, for details check the topics : %s and %s.", __APP_NAME__,
wayarea_gridmap_topic_.c_str(), objects_src_topic_.c_str());
return;
}
grid_map::GridMap current_grid;
grid_map::GridMapRosConverter::fromMessage(*in_gridmap, current_grid);
autoware_msgs::DetectedObjectArray in_roi_objects;
autoware_msgs::DetectedObjectArray out_roi_objects;
in_roi_objects.header = in_detections->header;
out_roi_objects.header = in_detections->header;
if (current_grid.exists(gridmap_layer_))
{
// check if centroids are inside the drivable area
cv::Mat grid_image;
grid_map::GridMapCvConverter::toImage<unsigned char, 1>(
current_grid, gridmap_layer_, CV_8UC1, GRID_MIN_VALUE, GRID_MAX_VALUE, grid_image);
#pragma omp for
for (unsigned int i = 0; i < in_detections->objects.size(); i++) {
geometry_msgs::Point original_centroid_point, final_centroid_point;
original_centroid_point = in_detections->objects[i].pose.position;
if (current_grid.getFrameId() != in_detections->header.frame_id) {
tf::StampedTransform grid_sensor_tf = FindTransform(
current_grid.getFrameId(), in_detections->header.frame_id);
final_centroid_point = TransformPoint(original_centroid_point, grid_sensor_tf);
} else {
final_centroid_point = original_centroid_point;
}
bool point_in_grid = CheckPointInGrid(current_grid, grid_image, final_centroid_point);
std::vector<std::string>::const_iterator found = std::find(exception_list_.begin(), exception_list_.end(),
in_detections->objects[i].label);
bool is_exception = (found != exception_list_.end());
if (is_exception || point_in_grid) {
in_roi_objects.objects.push_back(in_detections->objects[i]);
} else {
out_roi_objects.objects.push_back(in_detections->objects[i]);
}
}
} else {
ROS_INFO("[%s] %s layer not contained in the OccupancyGrid", __APP_NAME__, gridmap_layer_.c_str());
}
roi_objects_publisher_.publish(in_roi_objects);
wayarea_gridmap_ = nullptr;
object_detections_ = nullptr;
}
具体的函数流程如下:
- 第一步检测ros数据是否都有,没有就退出。这是一个非常好的编程习惯,即在函数入口处检查数据的正常性。我们可以在google开源的各种代码中看到这样的check机制。
- 第二步将gridmap消息转换成grid_map::GridMap,这里是用的ros自带的grid_map包
- 第三步判断grid_map中是否存在gridmap_layer这一层
- 先将gridmap转换成图像数据cv::Mat grid_image
- 使用openmp进行并行加速for循环
- 遍历检测到的目标:
- 拿到每一个目标的位置坐标值
- 判断当前占有格地图的frame_id 和检测目标的frame_id是否一致
- 如果不一致,则将目标转换到地图坐标系下,这样是便于比较两者的值
- 如果一致,则将目标点直接赋值给final_centroid_point
- 使用CheckPointInGrid方法来检测目标点是否在目标栅格地图gridmap中
- 通过将目标的标签来和期望目标类别(车子和行人)比较
- 如果目标是行人或者车子 || 目标点在gridmap中即在我们关心的网格区域内,就把对象存到in_roi_objects中,并且通过/objects_filtered发布出来。
以上流程中,FindTransform()和TransformPoint()方法比较简单,不在赘述。我们打开CheckPointInGrid方法来看一下:
bool RosRoiObjectFilterApp::CheckPointInGrid(const grid_map::GridMap &in_grid_map,
const cv::Mat &in_grid_image,
const geometry_msgs::Point &in_point)
{
// calculate out_grid_map position
grid_map::Position map_pos = in_grid_map.getPosition();
double origin_x_offset = in_grid_map.getLength().x() / 2.0 - map_pos.x();
double origin_y_offset = in_grid_map.getLength().y() / 2.0 - map_pos.y();
// coordinate conversion for cv image
double cv_x = (in_grid_map.getLength().y() - origin_y_offset - in_point.y) / in_grid_map.getResolution();
double cv_y = (in_grid_map.getLength().x() - origin_x_offset - in_point.x) / in_grid_map.getResolution();
// check coords are inside the gridmap
if (cv_x < 0 || cv_x > in_grid_image.cols || cv_y < 0 || cv_y > in_grid_image.rows)
{
return false;
}
//_gridmap_no_road_value if road
if (in_grid_image.at<uchar>(cv_y, cv_x) != gridmap_no_road_value_)
{
return true;
}
return false;
}
- 首先拿到in_grid_map的位置
- 拿到地图左下角x和y相对于地图中心的偏移,这里其实是负的偏移,因此下面会减去这个值
- 将输入点的坐标转换成图像坐标系下的像素坐标值,像素坐标系和地图坐标系这里是有区别的,同时x和y好像这里也是互相调换的。
- 拿到目标点在图像坐标系下的像素值,如果有的话,则证明目标点是处于grid_map内的,则返回true。反之返回false。
最后我们可以发现这个功能节点还有一个很重要的依赖输入topic便是/grid_map_wayarea,它到底是多长多宽,如何生成的,是动态变化的还是固定就是整个大地图场景,我们暂时都不知道,因此我们需要在后面继续学习这个topic是如何配置的。
总结
最后看下来,并不是和我想的一样,autoware是聚类加上使用占有格地图的方式来滤除我们行驶区域以外的目标。因此前面聚类的计算量依然是挺庞大的。