/********************************************************************* * * 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 using costmap_2d::NO_INFORMATION; using costmap_2d::LETHAL_OBSTACLE; using costmap_2d::FREE_SPACE; using costmap_2d::ObservationBuffer; using costmap_2d::Observation; namespace costmap_2d { void ObstacleLayer::onInitialize() { rolling_window_ = layered_costmap_->isRolling(); ObstacleLayer::matchSize(); current_ = true; stop_receiving_data_ = false; global_frame_ = layered_costmap_->getGlobalFrameID(); getParams(); } ObstacleLayer::~ObstacleLayer() {} bool ObstacleLayer::getParams() { try { std::string config_file_name = "config.yaml"; std::string folder = COSTMAP_2D_DIR; std::string path_source = getSourceFile(folder,config_file_name); if(path_source != " ") { std::cout << "Path source: " << path_source << std::endl; } else { std::cout << "/cfg folder not found!" << std::endl; } YAML::Node config = YAML::LoadFile(path_source); YAML::Node layer = config["obstacle_layer"]; bool track_unknown_space = loadParam(layer, "track_unknown_space", layered_costmap_->isTrackingUnknown()); if (track_unknown_space) default_value_ = NO_INFORMATION; else default_value_ = FREE_SPACE; double transform_tolerance = loadParam(layer,"transform_tolerance", 0.2); // get the topics that we'll subscribe to from the parameter server std::string topics_string = loadParam(layer,"observation_sources", std::string("")); printf(" Subscribed to Topics: %s\n", topics_string.c_str()); // get the parameters for the specific topic double observation_keep_time = 0, expected_update_rate = 0, min_obstacle_height = 0, max_obstacle_height = 2; std::string topic = "map", sensor_frame = "laser_frame", data_type = "PointCloud"; bool inf_is_valid = false, clearing=false, marking=true; topic = loadParam(layer,"topic", topic); sensor_frame = loadParam(layer,"sensor_frame", std::string("")); observation_keep_time = loadParam(layer,"observation_persistence", 0.0); expected_update_rate = loadParam(layer,"expected_update_rate", 0.0); data_type = loadParam(layer,"data_type", std::string("PointCloud")); min_obstacle_height = loadParam(layer,"min_obstacle_height", 0.0); max_obstacle_height = loadParam(layer,"max_obstacle_height", 2.0); inf_is_valid = loadParam(layer,"inf_is_valid", false); clearing = loadParam(layer,"clearing", false); marking = loadParam(layer,"marking", true); if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan")) { printf("Only topics that use point clouds or laser scans are currently supported\n"); 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; double obstacle_range = 2.5; obstacle_range = loadParam(layer,"obstacle_range", obstacle_range); double raytrace_range = 3.0; raytrace_range = loadParam(layer,"raytrace_range", raytrace_range); bool footprint_clearing_enabled = loadParam(layer, "footprint_clearing_enabled", true); int combination_method = loadParam(layer, "combination_method", 1); // enabled_ = enabled; footprint_clearing_enabled_ = footprint_clearing_enabled; max_obstacle_height_ = max_obstacle_height; combination_method_ = combination_method; printf("Creating an observation buffer for topic %s, frame %s\n", 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))); if (marking) marking_buffers_.push_back(observation_buffers_.back()); // check if we'll also add this buffer to our clearing observation buffers if (clearing) clearing_buffers_.push_back(observation_buffers_.back()); printf( "Created an observation buffer for topic %s, global frame: %s, " "expected update rate: %.2f, observation persistence: %.2f\n", topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time); } catch (const YAML::BadFile& e) { std::cerr << "Cannot open YAML file: " << e.what() << std::endl; return false; } return true; } void ObstacleLayer::handleImpl(const void* data, const std::type_info& type, const std::string& topic) { if(!stop_receiving_data_) { if(observation_buffers_.empty()) return; boost::shared_ptr buffer = observation_buffers_.back(); if (type == typeid(sensor_msgs::LaserScan) && topic == "laser") { laserScanCallback(*static_cast(data), buffer); } else if (type == typeid(sensor_msgs::LaserScan) && topic == "laser_valid") { laserScanValidInfCallback(*static_cast(data), buffer); } else if (type == typeid(sensor_msgs::PointCloud) && topic == "pcl_cb") { pointCloudCallback(*static_cast(data), buffer); } else if (type == typeid(sensor_msgs::PointCloud2) && topic == "pcl2_cb") { pointCloud2Callback(*static_cast(data), buffer); } else { std::cout << "[Plugin] Unknown type: " << type.name() << std::endl; } } else { std::cout << "Stop receiving data!" << std::endl; return; } } void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScan& 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 (tf3::TransformException &ex) { printf("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n", global_frame_.c_str(), ex.what()); projector_.projectLaser(message, cloud); } catch (std::runtime_error &ex) { printf("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s\n", ex.what()); return; //ignore this message } // buffer the point cloud buffer->lock(); buffer->bufferCloud(cloud); buffer->unlock(); } void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScan& 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 (tf3::TransformException &ex) { printf("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n", global_frame_.c_str(), ex.what()); projector_.projectLaser(message, cloud); } catch (std::runtime_error &ex) { printf("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s\n", ex.what()); return; //ignore this message } // buffer the point cloud buffer->lock(); buffer->bufferCloud(cloud); buffer->unlock(); } void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloud& message, const boost::shared_ptr& buffer) { sensor_msgs::PointCloud2 cloud2; if (!sensor_msgs::convertPointCloudToPointCloud2(message, cloud2)) { printf("Failed to convert a PointCloud to a PointCloud2, dropping message\n"); return; } // buffer the point cloud buffer->lock(); buffer->bufferCloud(cloud2); buffer->unlock(); } void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2& 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) { 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_) { printf("The point is too high\n"); 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) { printf("The point is too far away\n"); continue; } // now we need to compute the map coordinates for the observation unsigned int mx, my; if (!worldToMap(px, py, mx, my)) { printf("Computing map coords failed\n"); 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); } 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) { if (footprint_clearing_enabled_) { setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE); } switch (combination_method_) { case 0: // Overwrite updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); break; case 1: // Maximum updateWithMax(master_grid, min_i, min_j, max_i, max_j); 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 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) { 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)) { printf( "The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.\n", 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() { stop_receiving_data_ = false; for (unsigned int i = 0; i < observation_buffers_.size(); ++i) { if (observation_buffers_[i]) observation_buffers_[i]->resetLastUpdated(); } } void ObstacleLayer::deactivate() { stop_receiving_data_ = true; } 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(); } // Export factory function static std::shared_ptr create_obstacle_plugin() { return std::make_shared(); } // Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias) BOOST_DLL_ALIAS(create_obstacle_plugin, create_obstacle_layer) } // namespace costmap_2d