Autoware感知瞎学笔记(三)object_map / wayarea2grid
目录
我们在Autoware瞎学系列第二节中瞎学了roi_object_filter利用/grid_map_wayarea滤除掉行驶区域外的目标,但是关于这个topic的参数和来源我们一无所知,因此我们就在此节瞎学一下,一探究竟。话不多说,Let’s go
代码分析:
1. CMakeLists.txt
可以看到节点的入口位于nodes/wayarea2grid/wayarea2grid_node.cpp
add_library(wayarea2grid_lib
nodes/wayarea2grid/wayarea2grid.h
include/object_map/object_map_utils.hpp
nodes/wayarea2grid/wayarea2grid.cpp
)
target_link_libraries(wayarea2grid_lib
${catkin_LIBRARIES}
object_map_utils_lib
)
add_executable(wayarea2grid
nodes/wayarea2grid/wayarea2grid.h
nodes/wayarea2grid/wayarea2grid_node.cpp
)
target_link_libraries(wayarea2grid
${catkin_LIBRARIES}
wayarea2grid_lib
)
2. wayarea2grid_node.cpp
实例化一个WayareaToGrid类,并且调用其Run()方法
#include <ros/ros.h>
#include "wayarea2grid.h"
int main(int argc, char **argv)
{
ros::init(argc, argv, "wayarea2grid");
object_map::WayareaToGrid wayarea2grid;
wayarea2grid.Run();
return 0;
}
3. wayarea2grid.h
这里面主要调用了ros的自带package grid_map::GridMap
class WayareaToGrid {
public:
WayareaToGrid();
void Run();
private:
// handle
ros::NodeHandle node_handle_;
ros::NodeHandle private_node_handle_;
ros::Publisher publisher_grid_map_;
ros::Publisher publisher_occupancy_;
grid_map::GridMap gridmap_;
std::string sensor_frame_;
std::string map_frame_;
const std::string grid_layer_name_ = "wayarea";
double grid_resolution_;
double grid_length_x_;
double grid_length_y_;
double grid_position_x_;
double grid_position_y_;
double grid_position_z_;
tf::TransformListener tf_listener_;
int OCCUPANCY_ROAD = 128;
int OCCUPANCY_NO_ROAD = 255;
const int grid_min_value_ = 0;
const int grid_max_value_ = 255;
std::vector<std::vector<geometry_msgs::Point>> area_points_;
/*!
* Initializes ROS Publisher, Subscribers and sets the configuration parameters
*/
void InitializeROSIo();
};
4. wayarea2grid.cpp
4.1 WayareaToGrid() 构造函数
WayareaToGrid::WayareaToGrid() : private_node_handle_("~") {
InitializeROSIo();
LoadRoadAreasFromVectorMap(private_node_handle_, area_points_);
}
调用InitializeROSIo()方法初始化 和 LoadRoadAreasFromVectorMap 方法加载VectorMap到area_points_
4.2 InitializeROSIo() 初始化方法
通过ros参数服务器加载参数
void WayareaToGrid::InitializeROSIo()
{
private_node_handle_.param<std::string>("sensor_frame", sensor_frame_, "velodyne");
private_node_handle_.param<std::string>("map_frame", map_frame_, "map");
private_node_handle_.param<double>("grid_resolution", grid_resolution_, 0.2);
private_node_handle_.param<double>("grid_length_x", grid_length_x_, 80);
private_node_handle_.param<double>("grid_length_y", grid_length_y_, 30);
private_node_handle_.param<double>("grid_position_x", grid_position_x_, 20);
private_node_handle_.param<double>("grid_position_y", grid_position_y_, 0);
private_node_handle_.param<double>("grid_position_z", grid_position_z_, -2.f);
publisher_grid_map_ = node_handle_.advertise<grid_map_msgs::GridMap>("grid_map_wayarea", 1, true);
publisher_occupancy_ = node_handle_.advertise<nav_msgs::OccupancyGrid>("occupancy_wayarea", 1, true);
}
4.3 LoadRoadAreasFromVectorMap() 加载VectorMap地图
void LoadRoadAreasFromVectorMap(ros::NodeHandle& in_private_node_handle,
std::vector<std::vector<geometry_msgs::Point>>& out_area_points) {
// 声明一个VectorMap类型的vmap
vector_map::VectorMap vmap;
// 调用其内置的subscribe方法来监听point,line,area,category类型的数据
vmap.subscribe(in_private_node_handle,
vector_map::Category::POINT | vector_map::Category::LINE | vector_map::Category::AREA |
vector_map::Category::WAY_AREA, 10);
// 使用仿函数的方式传入到vmap.findByFilter方法中拿到way_area类型的数据,放到vector中
std::vector<vector_map_msgs::WayArea> way_areas =
vmap.findByFilter([](const vector_map_msgs::WayArea &way_area) { return true;});
if (way_areas.empty())
{
ROS_WARN_STREAM("No WayArea...");
return;
}
// 根据前面拿到的way_area里的area id来拿到area,这里way_area和area是不一样的
// 然后使用SearchAreaPoints方法找到area外边界点,并且压入到out_area_points中
for (const auto &way_area : way_areas)
{
vector_map_msgs::Area area = vmap.findByKey(vector_map::Key<vector_map::Area>(way_area.aid));
out_area_points.emplace_back(SearchAreaPoints(area, vmap));
}
}
4.4 SearchAreaPoints()方法
std::vector<geometry_msgs::Point>
SearchAreaPoints(const vector_map::Area &in_area, const vector_map::VectorMap &in_vectormap)
{
std::vector<geometry_msgs::Point> area_points;
std::vector<geometry_msgs::Point> area_points_empty;
// 如果传入的in_area没有aid,返回空的边界点
if (in_area.aid == 0)
return area_points_empty;
// 使用findByKey方法根据in_area里面的线slid来找到line数据
vector_map_msgs::Line line = in_vectormap.findByKey(vector_map::Key<vector_map_msgs::Line>(in_area.slid));
// must set beginning line
if (line.lid == 0 || line.blid != 0)
return area_points_empty;
// Search all lines in in_area
while (line.flid != 0)
{
// 找到line的起点
vector_map_msgs::Point bp = in_vectormap.findByKey(vector_map::Key<vector_map_msgs::Point>(line.bpid));
if (bp.pid == 0)
return area_points_empty;
// 找到line的终点
vector_map_msgs::Point fp = in_vectormap.findByKey(vector_map::Key<vector_map_msgs::Point>(line.fpid));
if (fp.pid == 0)
return area_points_empty;
// 2 points of line
area_points.push_back(vector_map::convertPointToGeomPoint(bp));
area_points.push_back(vector_map::convertPointToGeomPoint(fp));
// 根据flid找到后面一条线
line = in_vectormap.findByKey(vector_map::Key<vector_map_msgs::Line>(line.flid));
if (line.lid == 0)
return area_points_empty;
}
vector_map_msgs::Point bp = in_vectormap.findByKey(vector_map::Key<vector_map_msgs::Point>(line.bpid));
vector_map_msgs::Point fp = in_vectormap.findByKey(vector_map::Key<vector_map_msgs::Point>(line.fpid));
if (bp.pid == 0 || fp.pid == 0)
return area_points_empty;
// 将线起点终点压入到area_points中,返回出去,即得到了边界点
area_points.push_back(vector_map::convertPointToGeomPoint(bp));
area_points.push_back(vector_map::convertPointToGeomPoint(fp));
return area_points;
}
4.5 Run()方法
void WayareaToGrid::Run() {
bool set_map = false;
ros::Rate loop_rate(10);
while (ros::ok())
{
if (!set_map)
{
// 初始化gridmap_
gridmap_.add(grid_layer_name_);
gridmap_.setFrameId(sensor_frame_);
gridmap_.setGeometry(grid_map::Length(grid_length_x_, grid_length_y_),
grid_resolution_,
grid_map::Position(grid_position_x_, grid_position_y_));
set_map = true;
}
// timer start
//auto start = std::chrono::system_clock::now();
// 如果拿到了边界点
if (!area_points_.empty())
{
// 此cpp的重点函数,根据边界点的位置将占有和非占有网格值填入到grid_map_中
FillPolygonAreas(gridmap_, area_points_, grid_layer_name_, OCCUPANCY_NO_ROAD, OCCUPANCY_ROAD, grid_min_value_,
grid_max_value_, sensor_frame_, map_frame_,
tf_listener_);
// 发布grid_map_msgs::GridMap消息类型的grid_map_wayarea topic
PublishGridMap(gridmap_, publisher_grid_map_);
// 发布nav_msgs::OccupancyGrid消息类型的occupancy_wayarea topic
PublishOccupancyGrid(gridmap_, publisher_occupancy_, grid_layer_name_, grid_min_value_, grid_max_value_, grid_position_z_);
}
// timer end
//auto end = std::chrono::system_clock::now();
//auto usec = std::chrono::duration_cast<std::chrono::microseconds>(end - start).count();
//std::cout << "time: " << usec / 1000.0 << " [msec]" << std::endl;
loop_rate.sleep();
}
}
4.6 FillPolygonAreas()方法
/*!
* Projects the in_area_points forming the road, stores the result in out_grid_map.
* @param[out] out_grid_map GridMap object to add the road grid
* @param[in] in_area_points Array of points containing the wayareas
* @param[in] in_grid_layer_name Name to assign to the layer
* @param[in] in_layer_background_value Empty state value
* @param[in] in_fill_color Value to fill on wayareas
* @param[in] in_layer_min_value Minimum value in the layer
* @param[in] in_layer_max_value Maximum value in the later
* @param[in] in_tf_target_frame Target frame to transform the wayarea points
* @param[in] in_tf_source_frame Source frame, where the points are located
* @param[in] in_tf_listener Valid listener to obtain the transformation
*/
void FillPolygonAreas(grid_map::GridMap &out_grid_map,
const std::vector<std::vector<geometry_msgs::Point>> &in_area_points,
const std::string &in_grid_layer_name,
const int in_layer_background_value,
const int in_fill_color,
const int in_layer_min_value,
const int in_layer_max_value,
const std::string &in_tf_target_frame,
const std::string &in_tf_source_frame,
const tf::TransformListener &in_tf_listener);
void FillPolygonAreas(grid_map::GridMap &out_grid_map, const std::vector<std::vector<geometry_msgs::Point>> &in_area_points,
const std::string &in_grid_layer_name, const int in_layer_background_value,
const int in_layer_min_value, const int in_fill_color, const int in_layer_max_value,
const std::string &in_tf_target_frame, const std::string &in_tf_source_frame,
const tf::TransformListener &in_tf_listener)
{
if(!out_grid_map.exists(in_grid_layer_name))
{
// 增加in_grid_layer_name
out_grid_map.add(in_grid_layer_name);
}
// 设置out_grid_map默认的空值
out_grid_map[in_grid_layer_name].setConstant(in_layer_background_value);
cv::Mat original_image;
// 转换成图片的格式
grid_map::GridMapCvConverter::toImage<unsigned char, 1>(out_grid_map,
in_grid_layer_name,
CV_8UC1,
in_layer_min_value,
in_layer_max_value,
original_image);
// 拷贝到filled_image
cv::Mat filled_image = original_image.clone();
// 找到source_frame 和 target_frame之间的transform变换
tf::StampedTransform tf = FindTransform(in_tf_target_frame, in_tf_source_frame, in_tf_listener);
// calculate out_grid_map position
grid_map::Position map_pos = out_grid_map.getPosition();
// 找到起点
double origin_x_offset = out_grid_map.getLength().x() / 2.0 - map_pos.x();
double origin_y_offset = out_grid_map.getLength().y() / 2.0 - map_pos.y();
// 遍历边界点vector
for (const auto &points : in_area_points)
{
std::vector<cv::Point> cv_points;
// 遍历边界点
for (const auto &p : points)
{
// transform to GridMap coordinate
// 根据transform转换到gridmap坐标系下
geometry_msgs::Point tf_point = TransformPoint(p, tf);
// coordinate conversion for cv image
// 算出其二位平面图像坐标
double cv_x = (out_grid_map.getLength().y() - origin_y_offset - tf_point.y) / out_grid_map.getResolution();
double cv_y = (out_grid_map.getLength().x() - origin_x_offset - tf_point.x) / out_grid_map.getResolution();
// 压入到cv_points中
cv_points.emplace_back(cv::Point(cv_x, cv_y));
}
// 调用cv中的fillConvexPoly方法来构建Convex,并且在其内部填入对应的颜色像素值
cv::fillConvexPoly(filled_image, cv_points.data(), cv_points.size(), cv::Scalar(in_fill_color));
}
// convert to ROS msg
// 调用grid_map内建方法来将filled_image转换成地图格式out_grid_map
grid_map::GridMapCvConverter::addLayerFromImage<unsigned char, 1>(filled_image,
in_grid_layer_name,
out_grid_map,
in_layer_min_value,
in_layer_max_value);
}
4.7 PublishGridMap()方法
通过ros将数据发布出去,topic:grid_map_wayarea
void PublishGridMap(const grid_map::GridMap &in_gridmap, const ros::Publisher &in_publisher)
{
grid_map_msgs::GridMap message;
grid_map::GridMapRosConverter::toMessage(in_gridmap, message);
in_publisher.publish(message);
}
4.8 PublishOccupancyGrid()方法
通过grid_map::GridMapRosConverter::toOccupancyGrid方法将其转换成nav_msgs::OccupancyGrid占有格消息形式发布出去,topic:occupancy_wayarea
void PublishOccupancyGrid(const grid_map::GridMap &in_gridmap,
const ros::Publisher &in_publisher,
const std::string& in_layer,
double in_min_value,
double in_max_value,
double in_height)
{
nav_msgs::OccupancyGrid message;
grid_map::GridMapRosConverter::toOccupancyGrid(in_gridmap, in_layer, in_min_value, in_max_value, message );
message.info.origin.position.z = in_height;
in_publisher.publish(message);
}
总结
因此grid_map_msgs::GridMap和nav_msgs::OccupancyGrid两个格式的地图信息,就是通过找到全局地图中的边界点,然后巧妙的通过图像的方式利用fillConvexPoly方法来构建可行驶和占有格地图,这点和apollo不同。