关于ego-planner里面的GridMap

浙大这套开源的代码写得很nice 很值得借鉴 ， 对于 GridMap 类的实现。该类通过智能指针的封装简化了 GridMap 实例的创建和管理过程。一旦通过 GridMap::initMap(ros::NodeHandle &nh) 方法初始化，就可以方便地调用 GridMap 及其所有相关功能

它主要订阅了d435i的深度话题以及odom来实现的建图

 typedef std::shared_ptr<GridMap> Ptr;

单独写一个node试一下

#include <ros/ros.h>
#include "plan_env/grid_map.h"


GridMap::Ptr   grid_map_test_ ; 

int main(int argc, char** argv)
{
   
   
    ros::init(argc, argv, "gridmap_test_node");
    ros::NodeHandle nh_("~");

    grid_map_test_.reset(new GridMap) ;
    grid_map_test_->initMap(nh_) ; 

    ros::spin();
  
    return 0;
}

然后再在原来的基础上搞一个launch

<launch>

<node pkg="ego_planner" name="gridmap_test_node" type="gridmap_test_node" output="screen">

    <remap from="~grid_map/odom" to="/mavros/local_position/odom"/>
    <remap from="~grid_map/cloud" to="nouse2"/>
    <remap from="~grid_map/pose"   to = "nouse1"/> 
    <remap from="~grid_map/depth" to = "/iris/realsense/depth_camera/depth/image_raw"/>

    <param name="grid_map/resolution"      value="0.15" /> 
    <param name="grid_map/map_size_x"   value="50" /> 
    <param name="grid_map/map_size_y"   value="25" /> 
    <param name="grid_map/map_size_z"   value="3.0" /> 

    <param name="grid_map/local_update_range_x"  value="5.5" /> 
    <param name="grid_map/local_update_range_y"  value="5.5" /> 
    <param name="grid_map/local_update_range_z"  value="4.5" /> 
    <param name="grid_map/obstacles_inflation"     value="0.299" /> 
    <param name="grid_map/local_map_margin" value="10"/>
    <param name="grid_map/ground_height"        value="-0.01"/>
    <!-- camera parameter -->

    <param name="grid_map/cx" value="376.0"/>
    <param name="grid_map/cy" value="240.0"/>
    <param name="grid_map/fx" value="319.9988245765257"/>
    <param name="grid_map/fy" value="319.9988245765257"/>
    
    <!-- depth filter -->
    <param name="grid_map/use_depth_filter" value="true"/>
    <param name="grid_map/depth_filter_tolerance" value="0.15"/>
    <param name="grid_map/depth_filter_maxdist"   value="5.0"/>
    <param name="grid_map/depth_filter_mindist"   value="0.2"/>
    <param name="grid_map/depth_filter_margin"    value="2"/>
    <param name="grid_map/k_depth_scaling_factor" value="1000.0"/>
    <param name="grid_map/skip_pixel" value="2"/>
    <!-- local fusion -->
    <param name="grid_map/p_hit"  value="0.65"/>
    <param name="grid_map/p_miss" value="0.35"/>
    <param name="grid_map/p_min"  value="0.12"/>
    <param name="grid_map/p_max"  value="0.90"/>
    <param name="grid_map/p_occ"  value="0.80"/>
    <param name="grid_map/min_ray_length" value="0.3"/>
    <param name="grid_map/max_ray_length" value="5.0"/>

    <param name="grid_map/visualization_truncate_height"   value="1.8"/>
    <param name="grid_map/show_occ_time"  value="false"/>
    <param name="grid_map/pose_type"     value="2"/>  
    <param name="grid_map/frame_id"      value="world"/>
	
 </node>

<node pkg="odom_visualization" name="drone_odom_visualization" type="odom_visualization" output="screen">
    <remap from="~odom" to="/mavros/local_position/odom"/>
    <param name="color/a" value="1.0"/>    
    <param name="color/r" value="0.0"/>        
    <param name="color/g" value="0.0"/>        
    <param name="color/b" value="0.0"/>       
    <param name="covariance_scale" value="100.0"/>       
    <param name="robot_scale" value="1.0"/>
    <param name="tf45" value="false"/>
    <param name="drone_id" value="0"/>
  </node>

<node name="rviz" pkg="rviz" type="rviz" args="-d $(find ego_planner)/launch/gridmap_test.rviz" required="true" />

</launch>

最后用gazebo进行仿真

1. 在整个ego中调用的地方

// 位于/home/Fast-Drone-250/src/planner/plan_manage/src/planner_manager.cpp下的29行 
    grid_map_.reset(new GridMap);
    //GridMap::Ptr grid_map_; 在h文件声明 ， 这里使用了c++ 智能指针 ， 将grid_map_ 指向一个新的对象 原来的对象（如果有的话）将被销毁。
    grid_map_->initMap(nh);//传入nh 开始初始化Gridmap
    
    bspline_optimizer_->a_star_.reset(new AStar);
    bspline_optimizer_->a_star_->initGridMap(grid_map_, Eigen::Vector3i(100, 100, 100));//将该指针传入到a_star_->initGridMap
    

2. 初始化栅格地图GridMap::initMap(ros::NodeHandle &nh)

初始化地图的相关参数(mp_) 以及数据结构(md_) ， 以及ros的回调函数以及消息发布器

void GridMap::initMap(ros::NodeHandle &nh)
{
   
   
  node_ = nh;

  /* get parameter */
  double x_size, y_size, z_size;
  node_.param("grid_map/resolution", mp_.resolution_, -1.0);
  node_.param("grid_map/map_size_x", x_size, -1.0);
  node_.param("grid_map/map_size_y", y_size, -1.0);
  node_.param("grid_map/map_size_z", z_size, -1.0);
  node_.param("grid_map/local_update_range_x", mp_.local_update_range_(0), -1.0);
  node_.param("grid_map/local_update_range_y", mp_.local_update_range_(1), -1.0);
  node_.param("grid_map/local_update_range_z", mp_.local_update_range_(2), -1.0);
  node_.param("grid_map/obstacles_inflation", mp_.obstacles_inflation_, -1.0);

  node_.param("grid_map/fx", mp_.fx_, -1.0);
  node_.param("grid_map/fy", mp_.fy_, -1.0);
  node_.param("grid_map/cx", mp_.cx_, -1.0);
  node_.param("grid_map/cy", mp_.cy_, -1.0);

  node_.param("grid_map/use_depth_filter", mp_.use_depth_filter_, true);
  node_.param(</