/********************************************************************* * * 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 #include #include #include #include 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(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> sub(new message_filters::Subscriber(g_nh, topic, 50)); boost::shared_ptr> filter( new tf2_ros::MessageFilter(*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> sub(new message_filters::Subscriber(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> filter(new tf2_ros::MessageFilter(*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> sub(new message_filters::Subscriber(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> filter(new tf2_ros::MessageFilter(*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 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(nh); dynamic_reconfigure::Server::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 &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 &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 &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 &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 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::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 iter_x(cloud, "x"); sensor_msgs::PointCloud2ConstIterator iter_y(cloud, "y"); sensor_msgs::PointCloud2ConstIterator 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 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 &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 &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 iter_x(cloud, "x"); sensor_msgs::PointCloud2ConstIterator 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