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不同。