DuongTD/costmap_2d
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
747c9697a2
costmap_2d/plugins/obstacle_layer.cpp

574 lines
20 KiB
C++
Executable File
Raw Blame History

/*********************************************************************
*
* 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 <sensor_msgs/point_cloud2_iterator.h>
#include <tf3/exceptions.h>
#include <boost/dll/alias.hpp>
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<ObservationBuffer> buffer = observation_buffers_.back();
if (type == typeid(sensor_msgs::LaserScan) && topic == "laser") {
laserScanCallback(*static_cast<const sensor_msgs::LaserScan*>(data), buffer);
} else if (type == typeid(sensor_msgs::LaserScan) && topic == "laser_valid") {
laserScanValidInfCallback(*static_cast<const sensor_msgs::LaserScan*>(data), buffer);
} else if (type == typeid(sensor_msgs::PointCloud) && topic == "pcl_cb") {
pointCloudCallback(*static_cast<const sensor_msgs::PointCloud*>(data), buffer);
} else if (type == typeid(sensor_msgs::PointCloud2) && topic == "pcl2_cb") {
pointCloud2Callback(*static_cast<const sensor_msgs::PointCloud2*>(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<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 (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<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 (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<ObservationBuffer>& 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<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)
{
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_)
{
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<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
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<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()
{
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<Layer> create_obstacle_plugin() {
return std::make_shared<ObstacleLayer>();
}
// 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
Reference in New Issue View Git Blame Copy Permalink