本文共 8964 字，大约阅读时间需要 29 分钟。

4.2.4 costmap_2d工作过程

4.2.0节描述了MoveBase启动过程中创建和初始化global planner的costmap的过程。

planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);

planner_costmap_ros_->pause();  //先暂定运行

costmap的动态库位于 /opt/ros/kinetic/lib/libcostmap_2d.so

costmap会启动一个线程处理工作流程：costmap_2d::Costmap2DROS::mapUpdateLoop(double)

4.2.4.1 原理

costmap就是一张代价地图，地图数据是一个unsigned char数组，每一个点的值表示该点对应的实际坐标的通行性。 costmap坐标系的坐标是整数，世界坐标系的坐标是双浮点数，它们之间通过分辨率以及costmap左下角坐标进行转换。costmap对应的类为costmap_2d::Costmap2D。

ros利用插件机制实现了分层的costmap，每一层保存一张由该层对应的数据源生成的costmap，由各自的数据源实时更新，最后，各层的数据上下叠加在一起，就生成了最终的costmap。每一层的costmap为Costmap2D类型，最后叠加在一起的为LayeredCostmap类型，LayeredCostmap类里包含一个Costmap2D类型的成员变量。

4.2.4.2 类图

costmap_2d\include\costmap_2d\costmap_2d_ros.h

class Costmap2DROS

{

    LayeredCostmap* layered_costmap_;

}

class LayeredCostmap

{

Costmap2D costmap_;

std::vector<boost::shared_ptr<Layer> > plugins_;

}

class Costmap2D

{

    unsigned char* costmap_

}

class Layer

{

    LayeredCostmap* layered_costmap_

}

class VoxelLayer : public ObstacleLayer{}

class ObstacleLayer : public CostmapLayer{}

class StaticLayer : public CostmapLayer{}

class CostmapLayer : public Layer, public Costmap2D{}

class InflationLayer : public Layer{}

4.2.4.3 Costmap2DROS初始化过程

初始化过程在构造函数中完成：costmap_2d\src\costmap_2d_ros.cpp

Costmap2DROS::Costmap2DROS(std::string name, tf::TransformListener& tf)

主要工作有：

1） 初始化old_pose_

       tf::Stamped<tf::Pose> old_pose_

      old_pose_.setIdentity();

      old_pose_.setOrigin(tf::Vector3(1e30, 1e30, 1e30));

 2） 读取frame参数global_frame和robot_base_frame

        global_frame_=/map

        robot_base_frame_=/base_footprint

变量名	变量类型	参数名	值
global_frame_	std::string	global_frame	/map
robot_base_frame_	std::string	robot_base_frame	/base_footprint
rolling_window	bool	rolling_window	false
track_unknown_space	bool	track_unknown_space	false
always_send_full_costmap	bool	always_send_full_costmap	false

3）等待，直到有global_frame_到robot_base_frame_的TF变换

      tf_.waitForTransform(global_frame_, robot_base_frame_, ros::Time(), ros::Duration(0.1), ros::Duration(0.01), &tf_error)

4） 创建LayeredCostmap对象

       LayeredCostmap* layered_costmap_；

       layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);//参数都为false

5） 加载plugins

       读取plugins参数中指定的costmap plugin， 加载并初始化

       这里配置的为：

               Costmap_2d::StaticLayer

               Costmap_2d::VoxelLayer

               Costmap_2d::InflationLayer

       

  

• addPluin()会把layer plugin加到vector  plugins_中。

       std::vector<boost::shared_ptr<Layer> > plugins_

              <class StaticLayer>

              <class VoxelLayer>

              <class InflationLayer >

• initialize()会调用相应layer的onInitialize()函数

            每一个layer对象也有一个LayeredCostmap* layered_costmap_指针。会把此指针赋值为

            第4)步Costmap2DROS中的LayeredCostmap* layered_costmap_对象

           StaticLayer和VoxelLayer有自己的costmap。 InflationLayer没有自己的costmap，

            它直接访问LayeredCostmap中的costmap

       具体的每一个layer的初始化过程见4.2.4.4节

   6)  订阅和发布topic /move_base/global_costmap/footprint

       footprint_sub_ = private_nh.subscribe(topic, 1, &Costmap2DROS::setUnpaddedRobotFootprintPolygon, this);

       footprint_pub_ = private_nh.advertise<geometry_msgs::PolygonStamped>("footprint", 1);

    

    7） 设置robot的footprint

           setUnpaddedRobotFootprint(makeFootprintFromParams(private_nh));

           从配置参数footprint或者robot_radius中读取

    8） 创建Costmap2DPublisher

           publisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(), global_frame_,

                                                                            "costmap",always_send_full_costmap);

            此对象用于发布/move_base/global_costmap/costmap

                                    /move_base/global_costmap/costmap_updates

    9）创建定时器，每隔0.1秒判断robot是否处于运动中

          // Create a time r to check if the robot is moving

          robot_stopped_ = false;

          timer_ = private_nh.createTimer(ros::Duration(.1), &Costmap2DROS::movementCB, this);

   10）创建mapUpdateLoop线程

           在Costmap2DROS::reconfigureCB（）会创建线程。

           map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROS::mapUpdateLoop, this,

                                                                                map_update_frequency));

4.2.4.4 每层layer的初始化

Costmap2DROS调用父类Layer的initialize()进行初始化。Initialize()最终调用每一个子类的onInitialzie() 函数完成每一个layer自己的初始化。

plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);void Layer::initialize(LayeredCostmap* parent, std::string name, tf::TransformListener *tf){  layered_costmap_ = parent;  name_ = name;  tf_ = tf;  onInitialize();}

4.2.4.4.1 StaticLayer

StaticLayer的初始化是在函数void StaticLayer::onInitialize()中

costmap_2d\plugins\static_layer.cpp

初始化过程为：

1)  读取配置参数

变量名字	类型	参数名字	值
map_topic	std::string	map_topic	/map
first_map_only_	bool	first_map_only	false
subscribe_to_updates_	bool	subscribe_to_updates	false
track_unknown_space_	bool	track_unknown_space	true
use_maximum_	bool	use_maximum	false
temp_lethal_threshold	int	lethal_cost_threshold	100
temp_unknown_cost_value	int	unknown_cost_value	-1
trinary_costmap_	bool	trinary_costmap	true

 2)  订阅topic /map

       /map topic是map_server发布。会一直在等待，直到有topic数据才继续后面的初始化工作。

       map_sub_ = g_nh.subscribe(map_topic, 1, &StaticLayer::incomingMap, this);

              

        >  回调函数 StaticLayer::incomingMap()的工作：

            /map topic的消息数据格式如下所示：   

rosmsg show  nav_msgs/OccupancyGridstd_msgs/Header header  uint32 seq  time stamp  string frame_idnav_msgs/MapMetaData info  time map_load_time  float32 resolution  uint32 width  uint32 height  geometry_msgs/Pose origin    geometry_msgs/Point position      float64 x      float64 y      float64 z    geometry_msgs/Quaternion orientation      float64 x      float64 y      float64 z      float64 wint8[] data

         

          >首先读取地图的宽度，高度和分辨率。

          >与layered_costmap_的宽，高，分辨率，初始位置(x,y)比较，是否一致，如果不一致，则：

                调用 layered_costmap_->resizeMap()重新初始化地图数据（根据size重新分配内存，并初始化）

                调用各个layer的(*plugin)->matchSize()：

                     （staticlayer：根据master map的尺寸重新分配内存并初始化

                         VoxelLayer：根据master map的尺寸重新分配内存并初始化，voxelgrid 初始化

                        InflationLayer：重新分配cache cost和distance内存，并计算值

                     ）

                如果一致，则只resizeMap() static layer自己的地图.

              

         > 根据配置参数，转化每个像素的值到规定值

             具体的转换规则定义在函数StaticLayer::interpretValue(unsigned char value)中。

             入参value是map_server发布的地图数据值。

               如果value等于unknown_cost_value_（-1），则返回255（因为track_unknown_space为true）

               如果value大于等于lethal_cost_threshold（100）， 则返回254

               如果value小于lethal_cost_threshold（100），则返回0

               

                

                 

   

4.2.4.4.2 VoxelLayer

Voxel layer继承自Obstacle layer. 他们从传感器接收PointClouds or LaserScans格式的数据。区别在于Obstacle layer处理二维数据， Voxel layer处理三维数据。

VoxelLayer 的初始化过程是在函数VoxelLayer::onInitialize()中完成的。它首先会先调用ObstacleLayer::onInitialize()来完成二维层面的初始化。

costmap_2d\plugins\obstacle_layer.cpp

costmap_2d\plugins\voxel_layer.cpp

 

ObstacleLayer::onInitialize()初始化过程：

1）读取track_unknown_space参数，初始化默认值

track_unknown_space参数决定了地图初始化的初始值，如果为true，则初始化默认值为NO_INFORMATION(255), 如果为false，则初始化默认值为FREE（0）。

  2）分配costmap地图内存并初始化。参数来自master（layered_costmap_） 的costmap。master的costmap在staticlayer初始化时完成的。

     ObstacleLayer::matchSize();

      

   

   3）读取参数transform_tolerance，置为0.2

   4）读取参数observation_sources，这里配置为：laser_scan_sensor sonar_scan_sensor camera_depth

        指定了使用的传感器数据

        下面对每一项都有详细配置。会依次解析配置参数,下面是laser scan的配置：

变量名	变量类型	参数名	值
topic	std::string	topic	/scan
sensor_frame	std::string	sensor_frame	“”
observation_keep_time	double	observation_persistence	0.0
expected_update_rate	double	expected_update_rate	0
data_type	std::string	data_type	LaserScan
min_obstacle_height	double	min_obstacle_height	0.21
max_obstacle_height	double	max_obstacle_height	0.30
inf_is_valid	bool	inf_is_valid	false
clearing	bool	clearing	true
marking	bool	marking	true
obstacle_range	double	obstacle_range	2.0
raytrace_range	double	raytrace_range	5.0

  5)  根据配置依次分配observations

       std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > observation_buffers_;

       std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > marking_buffers_; 

       std::vector<boost::shared_ptr<costmap_2d::ObservationBuffer> > clearing_buffers_;

       订阅相应的topic，并注册callback

       filter->registerCallback(boost::bind(&ObstacleLayer::laserScanCallback, this, _1, observation_buffers_.back()));

       雷达数据使用ObstacleLayer::laserScanCallback回调函数。将/map发布的数据转换为pointcloud格式，放入ObservationBuffer的list std::list<Observation> observation_list_中的成员 pcl::PointCloud<pcl::PointXYZ>* cloud_

       最终数据时点云格式。

   

6）设置参数动态配置的回调函数

      setupDynamicReconfigure(nh);

 

      至此，ObstacleLayer::onInitialize()部分初始化完成。

      下面是VoxelLayer::onInitialize()的剩余部分。

 7）发布topic

     如果publish_voxel_map为true，则发布topic /move_base/global_costmap/obstacle_layer/voxel_grid

     发布topic /move_base/global_costmap/obstacle_layer/clearing_endpoints

     voxel_pub_ = private_nh.advertise < costmap_2d::VoxelGrid > ("voxel_grid", 1);

      clearing_endpoints_pub_ = private_nh.advertise<sensor_msgs::PointCloud>("clearing_endpoints", 1);

  

4.2.4.4.3 InflationLayer

InflationLayer的初始化是在InflationLayer::onInitialize()函数中完成的。

costmap_2d\plugins\inflation_layer.cpp

具体过程为:

1. 注册reconfigureCB函数
2. 调用matchSize()初始化

         为cached_distances_和cached_costs_分配内存并初始化

         为seen_分配内存

4.2.4.5 mapUpdateLoop线程工作过程

Costmap2DROS初始化完成，启动mapUpdateLoop线程更新地图

Costmap2DROS::mapUpdateLoop(double frequency)

costmap_2d\src\costmap_2d_ros.cpp

具体过程如下：

 1)  updateMap

      Costmap2DROS::updateMap()

      >首先获取机器人的当前全局位姿(x, y, yaw)

      >调用layered_costmap_->updateMap(x, y, yaw);

              》遍历每一个layer plugin，调用updateBounds（）

                  (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);

             》遍历每一个layer plugin， 调用updateCosts()

                 (*plugin)->updateCosts(costmap_, x0, y0, xn, yn);

      > 发布机器人的footprint

  2） publisher_->updateBounds(x0, xn, y0, yn);

  3） 发布costmap到/move_base/global_costmap/costmap和/move_base/global_costmap/costmap_updates

        publisher_->publishCostmap();

   此线程在一直循环上面的过程。

 

 

 

 

转载地址：https://blog.csdn.net/lclfans1983/article/details/106796545 如侵犯您的版权，请留言回复原文章的地址，我们会给您删除此文章，给您带来不便请您谅解！