294 lines
11 KiB
C++
Executable File
294 lines
11 KiB
C++
Executable File
/*********************************************************************
|
|
*
|
|
* 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 <robot_costmap_2d/layered_costmap.h>
|
|
#include <robot_costmap_2d/footprint.h>
|
|
#include <cstdio>
|
|
#include <string>
|
|
#include <algorithm>
|
|
#include <vector>
|
|
|
|
using std::vector;
|
|
|
|
namespace robot_costmap_2d
|
|
{
|
|
|
|
LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bool track_unknown)
|
|
: costmap_(),
|
|
global_frame_(global_frame),
|
|
rolling_window_(rolling_window),
|
|
current_(false),
|
|
minx_(0.0),
|
|
miny_(0.0),
|
|
maxx_(0.0),
|
|
maxy_(0.0),
|
|
bx0_(0),
|
|
bxn_(0),
|
|
by0_(0),
|
|
byn_(0),
|
|
initialized_(false),
|
|
size_locked_(false),
|
|
circumscribed_radius_(1.0),
|
|
inscribed_radius_(0.1)
|
|
{
|
|
if (track_unknown)
|
|
costmap_.setDefaultValue(NO_INFORMATION);
|
|
else
|
|
costmap_.setDefaultValue(FREE_SPACE);
|
|
performance_window_start_ = std::chrono::steady_clock::now();
|
|
}
|
|
|
|
void LayeredCostmap::setPerformanceMetrics(bool enabled, double reporting_period_seconds)
|
|
{
|
|
performance_metrics_enabled_ = enabled;
|
|
performance_metrics_period_seconds_ = reporting_period_seconds > 0.0 ? reporting_period_seconds : 5.0;
|
|
resetPerformanceMetrics();
|
|
}
|
|
|
|
void LayeredCostmap::resetPerformanceMetrics()
|
|
{
|
|
performance_window_start_ = std::chrono::steady_clock::now();
|
|
performance_cycle_nanoseconds_ = 0;
|
|
performance_reset_nanoseconds_ = 0;
|
|
performance_cycles_ = 0;
|
|
performance_cycle_samples_.clear();
|
|
performance_cycle_samples_.reserve(128);
|
|
layer_performance_.assign(plugins_.size(), LayerPerformance());
|
|
}
|
|
|
|
void LayeredCostmap::maybeReportPerformance()
|
|
{
|
|
if (!performance_metrics_enabled_ || performance_cycles_ == 0)
|
|
return;
|
|
|
|
const auto now = std::chrono::steady_clock::now();
|
|
const double elapsed = std::chrono::duration<double>(now - performance_window_start_).count();
|
|
if (elapsed < performance_metrics_period_seconds_)
|
|
return;
|
|
|
|
const double average_cycle_ms =
|
|
static_cast<double>(performance_cycle_nanoseconds_) / performance_cycles_ / 1.0e6;
|
|
const double average_reset_ms =
|
|
static_cast<double>(performance_reset_nanoseconds_) / performance_cycles_ / 1.0e6;
|
|
std::sort(performance_cycle_samples_.begin(), performance_cycle_samples_.end());
|
|
const auto percentile_ms = [this](double percentile) {
|
|
if (performance_cycle_samples_.empty())
|
|
return 0.0;
|
|
const std::size_t index = static_cast<std::size_t>(
|
|
percentile * static_cast<double>(performance_cycle_samples_.size() - 1));
|
|
return static_cast<double>(performance_cycle_samples_[index]) / 1.0e6;
|
|
};
|
|
robot::log_info(
|
|
"Costmap performance: cycles=%llu avg_cycle_ms=%.3f p95_cycle_ms=%.3f "
|
|
"p99_cycle_ms=%.3f avg_reset_ms=%.3f\n",
|
|
static_cast<unsigned long long>(performance_cycles_), average_cycle_ms,
|
|
percentile_ms(0.95), percentile_ms(0.99), average_reset_ms);
|
|
|
|
for (std::size_t i = 0; i < plugins_.size() && i < layer_performance_.size(); ++i)
|
|
{
|
|
const LayerPerformance& stats = layer_performance_[i];
|
|
const double average_bounds_ms = stats.bounds_calls == 0 ? 0.0 :
|
|
static_cast<double>(stats.bounds_nanoseconds) / stats.bounds_calls / 1.0e6;
|
|
const double average_costs_ms = stats.costs_calls == 0 ? 0.0 :
|
|
static_cast<double>(stats.costs_nanoseconds) / stats.costs_calls / 1.0e6;
|
|
robot::log_info(
|
|
"Costmap layer [%s]: avg_bounds_ms=%.3f avg_costs_ms=%.3f\n",
|
|
plugins_[i]->getName().c_str(), average_bounds_ms, average_costs_ms);
|
|
}
|
|
|
|
resetPerformanceMetrics();
|
|
}
|
|
|
|
LayeredCostmap::~LayeredCostmap()
|
|
{
|
|
while (plugins_.size() > 0)
|
|
{
|
|
plugins_.pop_back();
|
|
}
|
|
}
|
|
|
|
void LayeredCostmap::resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x,
|
|
double origin_y, bool size_locked)
|
|
{
|
|
boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
|
|
size_locked_ = size_locked;
|
|
costmap_.resizeMap(size_x, size_y, resolution, origin_x, origin_y);
|
|
for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
|
|
++plugin)
|
|
{
|
|
(*plugin)->matchSize();
|
|
}
|
|
}
|
|
|
|
void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
|
|
{
|
|
const auto cycle_start = performance_metrics_enabled_ ? std::chrono::steady_clock::now() :
|
|
std::chrono::steady_clock::time_point();
|
|
// Lock for the remainder of this function, some plugins (e.g. VoxelLayer)
|
|
// implement thread unsafe updateBounds() functions.
|
|
boost::unique_lock<Costmap2D::mutex_t> lock(*(costmap_.getMutex()));
|
|
|
|
// if we're using a rolling buffer costmap... we need to update the origin using the robot's position
|
|
if (rolling_window_)
|
|
{
|
|
double new_origin_x = robot_x - costmap_.getSizeInMetersX() / 2;
|
|
double new_origin_y = robot_y - costmap_.getSizeInMetersY() / 2;
|
|
costmap_.updateOrigin(new_origin_x, new_origin_y);
|
|
}
|
|
|
|
if (plugins_.size() == 0)
|
|
return;
|
|
|
|
minx_ = miny_ = 1e30;
|
|
maxx_ = maxy_ = -1e30;
|
|
if (performance_metrics_enabled_ && layer_performance_.size() != plugins_.size())
|
|
layer_performance_.assign(plugins_.size(), LayerPerformance());
|
|
|
|
for (std::size_t plugin_index = 0; plugin_index < plugins_.size(); ++plugin_index)
|
|
{
|
|
const boost::shared_ptr<Layer>& plugin = plugins_[plugin_index];
|
|
if (!plugin->isEnabled())
|
|
continue;
|
|
double prev_minx = minx_;
|
|
double prev_miny = miny_;
|
|
double prev_maxx = maxx_;
|
|
double prev_maxy = maxy_;
|
|
const auto bounds_start = performance_metrics_enabled_ ? std::chrono::steady_clock::now() :
|
|
std::chrono::steady_clock::time_point();
|
|
plugin->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
|
|
if (performance_metrics_enabled_)
|
|
{
|
|
layer_performance_[plugin_index].bounds_nanoseconds +=
|
|
std::chrono::duration_cast<std::chrono::nanoseconds>(
|
|
std::chrono::steady_clock::now() - bounds_start).count();
|
|
++layer_performance_[plugin_index].bounds_calls;
|
|
}
|
|
if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy)
|
|
{
|
|
robot::log_error("Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
|
|
"is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s\n",
|
|
prev_minx, prev_miny, prev_maxx, prev_maxy,
|
|
minx_, miny_, maxx_, maxy_,
|
|
plugin->getName().c_str());
|
|
}
|
|
}
|
|
|
|
int x0, xn, y0, yn;
|
|
costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0);
|
|
costmap_.worldToMapEnforceBounds(maxx_, maxy_, xn, yn);
|
|
|
|
x0 = std::max(0, x0);
|
|
xn = std::min(int(costmap_.getSizeInCellsX()), xn + 1);
|
|
y0 = std::max(0, y0);
|
|
yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1);
|
|
|
|
if (xn < x0 || yn < y0)
|
|
return;
|
|
|
|
const auto reset_start = performance_metrics_enabled_ ? std::chrono::steady_clock::now() :
|
|
std::chrono::steady_clock::time_point();
|
|
costmap_.resetMap(x0, y0, xn, yn);
|
|
if (performance_metrics_enabled_)
|
|
{
|
|
performance_reset_nanoseconds_ +=
|
|
std::chrono::duration_cast<std::chrono::nanoseconds>(
|
|
std::chrono::steady_clock::now() - reset_start).count();
|
|
}
|
|
|
|
for (std::size_t plugin_index = 0; plugin_index < plugins_.size(); ++plugin_index)
|
|
{
|
|
const boost::shared_ptr<Layer>& plugin = plugins_[plugin_index];
|
|
if (!plugin->isEnabled())
|
|
continue;
|
|
|
|
const auto costs_start = performance_metrics_enabled_ ? std::chrono::steady_clock::now() :
|
|
std::chrono::steady_clock::time_point();
|
|
plugin->updateCosts(costmap_, x0, y0, xn, yn);
|
|
if (performance_metrics_enabled_)
|
|
{
|
|
layer_performance_[plugin_index].costs_nanoseconds +=
|
|
std::chrono::duration_cast<std::chrono::nanoseconds>(
|
|
std::chrono::steady_clock::now() - costs_start).count();
|
|
++layer_performance_[plugin_index].costs_calls;
|
|
}
|
|
}
|
|
|
|
bx0_ = x0;
|
|
bxn_ = xn;
|
|
by0_ = y0;
|
|
byn_ = yn;
|
|
|
|
initialized_ = true;
|
|
|
|
if (performance_metrics_enabled_)
|
|
{
|
|
const std::uint64_t cycle_nanoseconds = static_cast<std::uint64_t>(
|
|
std::chrono::duration_cast<std::chrono::nanoseconds>(
|
|
std::chrono::steady_clock::now() - cycle_start).count());
|
|
performance_cycle_nanoseconds_ += cycle_nanoseconds;
|
|
performance_cycle_samples_.push_back(cycle_nanoseconds);
|
|
++performance_cycles_;
|
|
maybeReportPerformance();
|
|
}
|
|
}
|
|
|
|
bool LayeredCostmap::isCurrent()
|
|
{
|
|
current_ = true;
|
|
for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
|
|
++plugin)
|
|
{
|
|
if ((*plugin)->isEnabled())
|
|
current_ = current_ && (*plugin)->isCurrent();
|
|
}
|
|
return current_;
|
|
}
|
|
|
|
void LayeredCostmap::setFootprint(const std::vector<robot_geometry_msgs::Point> &footprint_spec)
|
|
{
|
|
footprint_ = footprint_spec;
|
|
robot_costmap_2d::calculateMinAndMaxDistances(footprint_spec, inscribed_radius_, circumscribed_radius_);
|
|
|
|
for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
|
|
++plugin)
|
|
{
|
|
(*plugin)->onFootprintChanged();
|
|
}
|
|
}
|
|
|
|
} // namespace robot_costmap_2d
|