agv_ros_ws/costmap_2d/plugins/obstacle_layer.cpp

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, 2013, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Eitan Marder-Eppstein
 *         David V. Lu!!
 *********************************************************************/
#include <costmap_2d/obstacle_layer.h>
#include <costmap_2d/costmap_math.h>
#include <tf2_ros/message_filter.h>

#include <pluginlib/class_list_macros.hpp>
#include <sensor_msgs/point_cloud2_iterator.h>

PLUGINLIB_EXPORT_CLASS(costmap_2d::ObstacleLayer, costmap_2d::Layer)

using costmap_2d::FREE_SPACE;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::NO_INFORMATION;

using costmap_2d::Observation;
using costmap_2d::ObservationBuffer;

namespace costmap_2d
{

  // 初始化时被调用，用于设置参数、订阅传感器话题、创建观察缓冲区等
  void ObstacleLayer::onInitialize()
  {
    ros::NodeHandle nh("~/" + name_), g_nh;
    rolling_window_ = layered_costmap_->isRolling();

    // 未知空间的代价值设置
    bool track_unknown_space;
    nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown());
    if (track_unknown_space)
      default_value_ = NO_INFORMATION; // no information 的代价值是 255
    else
      default_value_ = FREE_SPACE; // 空闲代价 0

    ObstacleLayer::matchSize(); // 调整障碍层的大小以匹配代价地图的大小
    current_ = true;            // 设置当前状态为真，表示层是最新的

    global_frame_ = layered_costmap_->getGlobalFrameID(); // 获取全局坐标ID
    double transform_tolerance;
    nh.param("transform_tolerance", transform_tolerance, 0.2);

    std::string topics_string;
    // get the topics that we'll subscribe to from the parameter server
    nh.param("observation_sources", topics_string, std::string("")); // 这里就一个雷达数据的话题
    ROS_INFO("    Subscribed to Topics: %s", topics_string.c_str());

    // now we need to split the topics based on whitespace which we can use a stringstream for
    // 现在我们需要根据空格分割主题，我们可以使用字符串流
    std::stringstream ss(topics_string); // 使用std::stringstream分割话题字符串，为每个话题创建观察缓冲区和订阅器

    std::string source;
    while (ss >> source)
    {
      std::cout << "========================  while (ss >> source)  ==========================" << std::endl; 

      ros::NodeHandle source_node(nh, source);

      // get the parameters for the specific topic  获得具体话题的一些参数
      double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height;
      std::string topic, sensor_frame, data_type;
      bool inf_is_valid, clearing, marking;

      source_node.param("topic", topic, source);
      source_node.param("sensor_frame", sensor_frame, std::string(""));
      source_node.param("observation_persistence", observation_keep_time, 0.0); // 这个是干嘛用的?
      source_node.param("expected_update_rate", expected_update_rate, 0.0);
      source_node.param("data_type", data_type, std::string("PointCloud"));
      source_node.param("min_obstacle_height", min_obstacle_height, 0.0);
      source_node.param("max_obstacle_height", max_obstacle_height, 2.0);
      source_node.param("inf_is_valid", inf_is_valid, false);
      source_node.param("clearing", clearing, true);
      source_node.param("marking", marking, true);

      if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan"))
      {
        ROS_FATAL("Only topics that use point clouds or laser scans are currently supported");
        throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported");
      }

      std::string raytrace_range_param_name, obstacle_range_param_name;

      // get the obstacle range for the sensor    获取传感器的障碍物范围
      double obstacle_range = 2.5;
      if (source_node.searchParam("obstacle_range", obstacle_range_param_name))
      {
        source_node.getParam(obstacle_range_param_name, obstacle_range);
      }

      // get the raytrace range for the sensor    获取传感器的光线追踪范围
      double raytrace_range = 3.0;
      if (source_node.searchParam("raytrace_range", raytrace_range_param_name))
      {
        source_node.getParam(raytrace_range_param_name, raytrace_range);
      }

      ROS_DEBUG("Creating an observation buffer for source %s, topic %s, frame %s", source.c_str(), topic.c_str(), sensor_frame.c_str());

      // create an observation buffer   创建观察缓冲区
      observation_buffers_.push_back(
          boost::shared_ptr<ObservationBuffer>(new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,
                                                                     max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_,
                                                                     sensor_frame, transform_tolerance)));

      // check if we'll add this buffer to our marking observation buffers
      // 检查我们是否要将此缓冲区添加到标记观察缓冲区中
      if (marking)
        marking_buffers_.push_back(observation_buffers_.back());

      // check if we'll also add this buffer to our clearing observation buffers
      std::cout << "clearing = " << clearing << std::endl;
      if (clearing)
        clearing_buffers_.push_back(observation_buffers_.back());

      ROS_DEBUG(
          "Created an observation buffer for source %s, topic %s, global frame: %s, "
          "expected update rate: %.2f, observation persistence: %.2f",
          source.c_str(), topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);

      // create a callback for the topic    根据话题创建回调函数
      if (data_type == "LaserScan")
      {
        boost::shared_ptr<message_filters::Subscriber<sensor_msgs::LaserScan>> sub(new message_filters::Subscriber<sensor_msgs::LaserScan>(g_nh, topic, 50));

        boost::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::LaserScan>> filter(
            new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*sub, *tf_, global_frame_, 50, g_nh));

        if (inf_is_valid)
        {
          filter->registerCallback([this, buffer = observation_buffers_.back()](auto &msg)
                                   { laserScanValidInfCallback(msg, buffer); });
          //  当有新的LaserScan消息到达并且时间戳检查通过时，会自动调用laserScanValidInfCallback函数，并将消息和最后一个ObservationBuffer对象的引用作为参数传递
        }
        else
        {
          filter->registerCallback([this, buffer = observation_buffers_.back()](auto &msg)
                                   { laserScanCallback(msg, buffer); });
        }

        observation_subscribers_.push_back(sub);
        observation_notifiers_.push_back(filter);

        observation_notifiers_.back()->setTolerance(ros::Duration(0.05));
      }
      else if (data_type == "PointCloud")
      {
        boost::shared_ptr<message_filters::Subscriber<sensor_msgs::PointCloud>> sub(new message_filters::Subscriber<sensor_msgs::PointCloud>(g_nh, topic, 50));

        if (inf_is_valid)
        {
          ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
        }

        boost::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::PointCloud>> filter(new tf2_ros::MessageFilter<sensor_msgs::PointCloud>(*sub, *tf_, global_frame_, 50, g_nh));
        filter->registerCallback([this, buffer = observation_buffers_.back()](auto &msg)
                                 { pointCloudCallback(msg, buffer); });

        observation_subscribers_.push_back(sub);
        observation_notifiers_.push_back(filter);
      }
      else
      {
        boost::shared_ptr<message_filters::Subscriber<sensor_msgs::PointCloud2>> sub(new message_filters::Subscriber<sensor_msgs::PointCloud2>(g_nh, topic, 50));

        if (inf_is_valid)
        {
          ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
        }

        boost::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::PointCloud2>> filter(new tf2_ros::MessageFilter<sensor_msgs::PointCloud2>(*sub, *tf_, global_frame_, 50, g_nh));
        filter->registerCallback([this, buffer = observation_buffers_.back()](auto &msg)
                                 { pointCloud2Callback(msg, buffer); });

        observation_subscribers_.push_back(sub);
        observation_notifiers_.push_back(filter);
      }

      if (sensor_frame != "")
      {
        std::vector<std::string> target_frames;
        target_frames.push_back(global_frame_);
        target_frames.push_back(sensor_frame);
        observation_notifiers_.back()->setTargetFrames(target_frames);
      }
    }

    dsrv_ = NULL;
    setupDynamicReconfigure(nh); // 动态重配置的设置
  }

  // 设置动态重配置，允许在运行时修改配置参数
  void ObstacleLayer::setupDynamicReconfigure(ros::NodeHandle &nh)
  {
    dsrv_ = new dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>(nh);
    dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>::CallbackType cb =
        [this](auto &config, auto level)
    { reconfigureCB(config, level); };
    dsrv_->setCallback(cb);
  }

  // 析构函数
  ObstacleLayer::~ObstacleLayer()
  {
    if (dsrv_)
      delete dsrv_;
  }

  // 动态重配置的回调函数，用于更新配置参数
  void ObstacleLayer::reconfigureCB(costmap_2d::ObstaclePluginConfig &config, uint32_t level)
  {
    enabled_ = config.enabled;
    footprint_clearing_enabled_ = config.footprint_clearing_enabled;
    max_obstacle_height_ = config.max_obstacle_height;
    combination_method_ = config.combination_method;
  }

  // 处理激光扫描（LaserScan）消息的回调函数，将激光扫描转换为点云并存储在观察缓冲区中
  void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScanConstPtr &message,
                                        const boost::shared_ptr<ObservationBuffer> &buffer)
  {
    // project the laser into a point cloud
    sensor_msgs::PointCloud2 cloud;
    cloud.header = message->header;

    // project the scan into a point cloud   将扫描投影到点云中
    try
    {
      projector_.transformLaserScanToPointCloud(message->header.frame_id, *message, cloud, *tf_);
    }
    catch (tf2::TransformException &ex)
    {
      ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", global_frame_.c_str(), ex.what());
      projector_.projectLaser(*message, cloud);
    }
    catch (std::runtime_error &ex)
    {
      ROS_WARN("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s", ex.what());
      return; // ignore this message
    }

    // buffer the point cloud   缓冲点云
    buffer->lock();
    buffer->bufferCloud(cloud);
    buffer->unlock();
  }

  // 处理激光扫描消息的回调函数，与laserScanCallback类似，但会将正无穷大的距离替换为最大范围值
  void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr &raw_message,
                                                const boost::shared_ptr<ObservationBuffer> &buffer)
  {
    // Filter positive infinities ("Inf"s) to max_range.
    float epsilon = 0.0001; // a tenth of a millimeter
    sensor_msgs::LaserScan message = *raw_message;
    for (size_t i = 0; i < message.ranges.size(); i++)
    {
      float range = message.ranges[i];
      if (!std::isfinite(range) && range > 0)
      {
        message.ranges[i] = message.range_max - epsilon;
      }
    }

    // project the laser into a point cloud
    sensor_msgs::PointCloud2 cloud;
    cloud.header = message.header;

    // project the scan into a point cloud
    try
    {
      projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_);
    }
    catch (tf2::TransformException &ex)
    {
      ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s",
               global_frame_.c_str(), ex.what());
      projector_.projectLaser(message, cloud);
    }
    catch (std::runtime_error &ex)
    {
      ROS_WARN("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s", ex.what());
      return; // ignore this message
    }

    // buffer the point cloud
    buffer->lock();
    buffer->bufferCloud(cloud);
    buffer->unlock();
  }

  // 处理点云（PointCloud）消息的回调函数，将PointCloud转换为PointCloud2并存储在观察缓冲区中
  void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloudConstPtr &message,
                                         const boost::shared_ptr<ObservationBuffer> &buffer)
  {
    sensor_msgs::PointCloud2 cloud2;

    if (!sensor_msgs::convertPointCloudToPointCloud2(*message, cloud2))
    {
      ROS_ERROR("Failed to convert a PointCloud to a PointCloud2, dropping message");
      return;
    }

    // buffer the point cloud
    buffer->lock();
    buffer->bufferCloud(cloud2);
    buffer->unlock();
  }

  // 处理点云2（PointCloud2）消息的回调函数，直接将PointCloud2存储在观察缓冲区中
  void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr &message,
                                          const boost::shared_ptr<ObservationBuffer> &buffer)
  {
    // buffer the point cloud
    buffer->lock();
    buffer->bufferCloud(*message);
    buffer->unlock();
  }

  // 更新障碍层的边界，处理观察数据，并将障碍物添加到代价地图中
  void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double *min_x,
                                   double *min_y, double *max_x, double *max_y)
  {
    // std::cout << "get in updateBounds========================================" << std::endl;
    if (rolling_window_)
      updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
    useExtraBounds(min_x, min_y, max_x, max_y);

    bool current = true;
    std::vector<Observation> observations, clearing_observations;

    // get the marking observations
    current = current && getMarkingObservations(observations);

    // get the clearing observations
    current = current && getClearingObservations(clearing_observations);

    // update the global current status
    current_ = current;

    // raytrace freespace
    for (unsigned int i = 0; i < clearing_observations.size(); ++i)
    {
      raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
    }

    // place the new obstacles into a priority queue... each with a priority of zero to begin with
    for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it)
    {
      const Observation &obs = *it;

      const sensor_msgs::PointCloud2 &cloud = *(obs.cloud_);

      double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;

      sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
      sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
      sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");

      for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z)
      {
        double px = *iter_x, py = *iter_y, pz = *iter_z;

        // if the obstacle is too high or too far away from the robot we won't add it
        if (pz > max_obstacle_height_)
        {
          ROS_DEBUG("The point is too high");
          continue;
        }

        // compute the squared distance from the hitpoint to the pointcloud's origin
        double sq_dist = (px - obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y) + (pz - obs.origin_.z) * (pz - obs.origin_.z);

        // if the point is far enough away... we won't consider it
        if (sq_dist >= sq_obstacle_range)
        {
          ROS_DEBUG("The point is too far away");
          continue;
        }

        // now we need to compute the map coordinates for the observation
        unsigned int mx, my;
        if (!worldToMap(px, py, mx, my))
        {
          ROS_DEBUG("Computing map coords failed");
          continue;
        }

        unsigned int index = getIndex(mx, my);
        costmap_[index] = LETHAL_OBSTACLE;
        touch(px, py, min_x, min_y, max_x, max_y);
      }
    }

    updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
  }

  // 更新机器人足印（footprint）在代价地图中的表示
  void ObstacleLayer::updateFootprint(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y,
                                      double *max_x, double *max_y)
  {
    if (!footprint_clearing_enabled_)
      return;
    transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_);

    for (unsigned int i = 0; i < transformed_footprint_.size(); i++)
    {
      touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y);
    }
  }

  // 更新代价地图的成本值
  void ObstacleLayer::updateCosts(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
  {

    // std::vector<Observation> clearing_observations;

    // // get the clearing observations
    // getClearingObservations(clearing_observations);

    // // double *min_x, *min_y, *max_x, *max_y;

    // double *min_x = new double(1e30);
    // double *min_y = new double(1e30);
    // double *max_x = new double(-1e30);
    // double *max_y = new double(-1e30);
    // // raytrace freespace
    // // std::cout << clearing_observations.size() << std::endl;

    // for (unsigned int i = 0; i < clearing_observations.size(); ++i)
    // {
    //   raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);
    // }

    // std::cout << "get in updateCosts" << std::endl;
    if (footprint_clearing_enabled_)
    {
      setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE);
      // 如果启用了足印清除，调用setConvexPolygonCost函数，将机器人的当前区域设置为 FREE_SPACE
    }

    switch (combination_method_)
    {
    case 0:                                                         // Overwrite    覆盖
      updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); // 这个的效果有点类似是: 移动机器人附近的障碍物才会膨胀,膨胀后的不会消除
      break;
    case 1:                                                   // Maximum      最大值
      updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); // 一般是调用这个 updateWithMax             updateWithAddition     updateWithTrueOverwrite
      break;
    default: // Nothing
      break;
    }
  }

  // 添加静态观察数据，可以是标记障碍物或清除障碍物
  void ObstacleLayer::addStaticObservation(costmap_2d::Observation &obs, bool marking, bool clearing)
  {
    if (marking)
      static_marking_observations_.push_back(obs);
    if (clearing)
      static_clearing_observations_.push_back(obs);
  }

  // 清除静态观察数据
  void ObstacleLayer::clearStaticObservations(bool marking, bool clearing)
  {
    if (marking)
      static_marking_observations_.clear();
    if (clearing)
      static_clearing_observations_.clear();
  }

  // 获取标记障碍物的观察数据
  bool ObstacleLayer::getMarkingObservations(std::vector<Observation> &marking_observations) const
  {
    bool current = true;
    // get the marking observations
    for (unsigned int i = 0; i < marking_buffers_.size(); ++i)
    {
      marking_buffers_[i]->lock();
      marking_buffers_[i]->getObservations(marking_observations);
      current = marking_buffers_[i]->isCurrent() && current;
      marking_buffers_[i]->unlock();
    }
    marking_observations.insert(marking_observations.end(),
                                static_marking_observations_.begin(), static_marking_observations_.end());
    return current;
  }

  // 获取清除障碍物的观察数据
  bool ObstacleLayer::getClearingObservations(std::vector<Observation> &clearing_observations) const
  {
    bool current = true;
    // get the clearing observations

    // std::cout << "clearing_buffers_.size() = " << clearing_buffers_.size() << std::endl;

    for (unsigned int i = 0; i < clearing_buffers_.size(); ++i)
    {
      clearing_buffers_[i]->lock();
      clearing_buffers_[i]->getObservations(clearing_observations);
      current = clearing_buffers_[i]->isCurrent() && current;
      clearing_buffers_[i]->unlock();
    }
    clearing_observations.insert(clearing_observations.end(),
                                 static_clearing_observations_.begin(), static_clearing_observations_.end());
    return current;
  }

  // 执行射线追踪以清除代价地图中的障碍物
  void ObstacleLayer::raytraceFreespace(const Observation &clearing_observation, double *min_x, double *min_y, double *max_x, double *max_y)
  {
    std::cout << "raytraceFreespace ==================================" << std::endl;
    double ox = clearing_observation.origin_.x;
    double oy = clearing_observation.origin_.y;
    const sensor_msgs::PointCloud2 &cloud = *(clearing_observation.cloud_);

    // get the map coordinates of the origin of the sensor
    unsigned int x0, y0;
    if (!worldToMap(ox, oy, x0, y0))
    {
      ROS_WARN_THROTTLE(
          1.0, "The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.",
          ox, oy);
      return;
    }

    // we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
    double origin_x = origin_x_, origin_y = origin_y_;
    double map_end_x = origin_x + size_x_ * resolution_;
    double map_end_y = origin_y + size_y_ * resolution_;

    touch(ox, oy, min_x, min_y, max_x, max_y);

    // for each point in the cloud, we want to trace a line from the origin and clear obstacles along it
    sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
    sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");

    for (; iter_x != iter_x.end(); ++iter_x, ++iter_y)
    {
      double wx = *iter_x;
      double wy = *iter_y;

      // now we also need to make sure that the enpoint we're raytracing
      // to isn't off the costmap and scale if necessary
      double a = wx - ox;
      double b = wy - oy;

      // the minimum value to raytrace from is the origin
      if (wx < origin_x)
      {
        double t = (origin_x - ox) / a;
        wx = origin_x;
        wy = oy + b * t;
      }
      if (wy < origin_y)
      {
        double t = (origin_y - oy) / b;
        wx = ox + a * t;
        wy = origin_y;
      }

      // the maximum value to raytrace to is the end of the map
      if (wx > map_end_x)
      {
        double t = (map_end_x - ox) / a;
        wx = map_end_x - .001;
        wy = oy + b * t;
      }
      if (wy > map_end_y)
      {
        double t = (map_end_y - oy) / b;
        wx = ox + a * t;
        wy = map_end_y - .001;
      }

      // now that the vector is scaled correctly... we'll get the map coordinates of its endpoint
      unsigned int x1, y1;

      // check for legality just in case
      if (!worldToMap(wx, wy, x1, y1))
        continue;

      unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
      MarkCell marker(costmap_, FREE_SPACE);
      // and finally... we can execute our trace to clear obstacles along that line
      raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range);

      updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
    }
  }

  // 激活障碍层，重新订阅话题
  void ObstacleLayer::activate()
  {
    // if we're stopped we need to re-subscribe to topics
    for (unsigned int i = 0; i < observation_subscribers_.size(); ++i)
    {
      if (observation_subscribers_[i] != NULL)
        observation_subscribers_[i]->subscribe();
    }

    for (unsigned int i = 0; i < observation_buffers_.size(); ++i)
    {
      if (observation_buffers_[i])
        observation_buffers_[i]->resetLastUpdated();
    }
  }

  // 禁用障碍层，取消订阅话题
  void ObstacleLayer::deactivate()
  {
    for (unsigned int i = 0; i < observation_subscribers_.size(); ++i)
    {
      if (observation_subscribers_[i] != NULL)
        observation_subscribers_[i]->unsubscribe();
    }
  }

  // 更新射线追踪的边界
  void ObstacleLayer::updateRaytraceBounds(double ox, double oy, double wx, double wy, double range,
                                           double *min_x, double *min_y, double *max_x, double *max_y)
  {
    double dx = wx - ox, dy = wy - oy;
    double full_distance = hypot(dx, dy);
    double scale = std::min(1.0, range / full_distance);
    double ex = ox + dx * scale, ey = oy + dy * scale;
    touch(ex, ey, min_x, min_y, max_x, max_y);
  }

  void ObstacleLayer::reset()
  {
    deactivate();
    resetMaps();
    current_ = true;
    activate();
  }

} // namespace costmap_2d