navigatoin 调试 -3- 解决costmap动态障碍不更新问题

机器人导航过程中，有两个问题

1.在行进过程中，当扫描到动态障碍后，将障碍记录到地图信息中，如果这块区域不再扫描到，则这块动态障碍区域永远不再消失。如果指定目的地为这块区域时，算法会显示该地方不可到达

2.很多人围着机器人，地图中会出现很多动态障碍，当人群散开不再围着机器人时，costmap中会残留很多动态障碍，并且长时间不消失。通常来说是扫描范围中最左边和最右边的区域里动态障碍不会消失，怀疑跟光线收敛有关。

初步解决方案：

在导航命令下发前，将地图上所有的动态障碍清除掉。代码如下：

  void MoveBase::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal){
    ROS_DEBUG_NAMED("move_base","In ROS goal callback, wrapping the PoseStamped in the action message and re-sending to the server.");
    move_base_msgs::MoveBaseActionGoal action_goal;
    action_goal.header.stamp = ros::Time::now();
    action_goal.goal.target_pose = *goal;

    #if 1
    ROS_ERROR("-sunyoop- clean all the o -before-");
    planner_costmap_ros_->resetLayers(); //消除地图数据和非costmap中的动态障碍
    controller_costmap_ros_->resetLayers();//消除costmap中的动态障碍
    ROS_ERROR("-sunyoop- clean all the o -end-");

    ros::Duration(0.5).sleep();//延迟0.5秒，为了将地图和动态障碍再次刷新上，之后再下发目标位置。
    #endif
    
    action_goal_pub_.publish(action_goal);
  }

延时0.5秒还有个作用：如果没有延迟，第一次规划路径时很大概率出现规划的路径是一条直线，因为当时地图上什么也没有，是空的，而机器人第一秒内就沿着这条直线前进，有可能出现碰撞事件。

根本原因：怀疑是离散点无法收敛问题。