DuongTD/costmap_2d
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first commit

Browse Source
This commit is contained in:
duongtd
2025-10-28 13:43:40 +07:00
parent 63dc18f3f0
commit 5d4d1943fe
56 changed files with 1660 additions and 144 deletions
Download Patch File Download Diff File Expand all files Collapse all files

173
src/costmap_layer.cpp Normal file
View File

@@ -0,0 +1,173 @@
#include<costmap_2d/costmap_layer.h>
namespace costmap_2d
{
void CostmapLayer::touch(double x, double y, double* min_x, double* min_y, double* max_x, double* max_y)
{
*min_x = std::min(x, *min_x);
*min_y = std::min(y, *min_y);
*max_x = std::max(x, *max_x);
*max_y = std::max(y, *max_y);
}
void CostmapLayer::matchSize()
{
Costmap2D* master = layered_costmap_->getCostmap();
resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
master->getOriginX(), master->getOriginY());
}
void CostmapLayer::clearArea(int start_x, int start_y, int end_x, int end_y, bool invert_area)
{
unsigned char* grid = getCharMap();
for(int x=0; x<(int)getSizeInCellsX(); x++){
bool xrange = x>start_x && x<end_x;
for(int y=0; y<(int)getSizeInCellsY(); y++){
if((xrange && y>start_y && y<end_y)!=invert_area)
continue;
int index = getIndex(x,y);
if(grid[index]!=NO_INFORMATION){
grid[index] = NO_INFORMATION;
}
}
}
}
void CostmapLayer::addExtraBounds(double mx0, double my0, double mx1, double my1)
{
extra_min_x_ = std::min(mx0, extra_min_x_);
extra_max_x_ = std::max(mx1, extra_max_x_);
extra_min_y_ = std::min(my0, extra_min_y_);
extra_max_y_ = std::max(my1, extra_max_y_);
has_extra_bounds_ = true;
}
void CostmapLayer::useExtraBounds(double* min_x, double* min_y, double* max_x, double* max_y)
{
if (!has_extra_bounds_)
return;
*min_x = std::min(extra_min_x_, *min_x);
*min_y = std::min(extra_min_y_, *min_y);
*max_x = std::max(extra_max_x_, *max_x);
*max_y = std::max(extra_max_y_, *max_y);
extra_min_x_ = 1e6;
extra_min_y_ = 1e6;
extra_max_x_ = -1e6;
extra_max_y_ = -1e6;
has_extra_bounds_ = false;
}
void CostmapLayer::updateWithMax(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!enabled_)
return;
unsigned char* master_array = master_grid.getCharMap();
unsigned int span = master_grid.getSizeInCellsX();
for (int j = min_j; j < max_j; j++)
{
unsigned int it = j * span + min_i;
for (int i = min_i; i < max_i; i++)
{
if (costmap_[it] == NO_INFORMATION){
it++;
continue;
}
if (costmap_[it] == CRITICAL_SPACE){
master_array[it] = costmap_[it];
it++;
continue;
}
if (costmap_[it] == PREFERRED_SPACE && master_array[it] < INSCRIBED_INFLATED_OBSTACLE ){
master_array[it] = costmap_[it];
it++;
continue;
}
if (costmap_[it] == UNPREFERRED_SPACE && master_array[it] < INSCRIBED_INFLATED_OBSTACLE ){
master_array[it] = costmap_[it];
it++;
continue;
}
unsigned char old_cost = master_array[it];
if (old_cost == NO_INFORMATION || old_cost < costmap_[it])
master_array[it] = costmap_[it];
it++;
}
}
}
void CostmapLayer::updateWithTrueOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j,
int max_i, int max_j)
{
if (!enabled_)
return;
unsigned char* master = master_grid.getCharMap();
unsigned int span = master_grid.getSizeInCellsX();
for (int j = min_j; j < max_j; j++)
{
unsigned int it = span*j+min_i;
for (int i = min_i; i < max_i; i++)
{
master[it] = costmap_[it];
it++;
}
}
}
void CostmapLayer::updateWithOverwrite(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!enabled_)
return;
unsigned char* master = master_grid.getCharMap();
unsigned int span = master_grid.getSizeInCellsX();
for (int j = min_j; j < max_j; j++)
{
unsigned int it = span*j+min_i;
for (int i = min_i; i < max_i; i++)
{
if (costmap_[it] != NO_INFORMATION)
master[it] = costmap_[it];
it++;
}
}
}
void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!enabled_)
return;
unsigned char* master_array = master_grid.getCharMap();
unsigned int span = master_grid.getSizeInCellsX();
for (int j = min_j; j < max_j; j++)
{
unsigned int it = j * span + min_i;
for (int i = min_i; i < max_i; i++)
{
if (costmap_[it] == NO_INFORMATION){
it++;
continue;
}
unsigned char old_cost = master_array[it];
if (old_cost == NO_INFORMATION)
master_array[it] = costmap_[it];
else
{
int sum = old_cost + costmap_[it];
if (sum >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
master_array[it] = costmap_2d::INSCRIBED_INFLATED_OBSTACLE - 1;
else
master_array[it] = sum;
}
it++;
}
}
}
} // namespace costmap_2d

90
src/costmap_math.cpp Normal file
View File

@@ -0,0 +1,90 @@
/*
* Copyright (c) 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 the 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.
*/
#include <costmap_2d/costmap_math.h>
#include <geometry_msgs/Point.h>
double distanceToLine(double pX, double pY, double x0, double y0, double x1, double y1)
{
double A = pX - x0;
double B = pY - y0;
double C = x1 - x0;
double D = y1 - y0;
double dot = A * C + B * D;
double len_sq = C * C + D * D;
double param = dot / len_sq;
double xx, yy;
if (param < 0)
{
xx = x0;
yy = y0;
}
else if (param > 1)
{
xx = x1;
yy = y1;
}
else
{
xx = x0 + param * C;
yy = y0 + param * D;
}
return distance(pX, pY, xx, yy);
}
bool intersects(std::vector<geometry_msgs::Point>& polygon, float testx, float testy)
{
bool c = false;
int i, j, nvert = polygon.size();
for (i = 0, j = nvert - 1; i < nvert; j = i++)
{
float yi = polygon[i].y, yj = polygon[j].y, xi = polygon[i].x, xj = polygon[j].x;
if (((yi > testy) != (yj > testy)) && (testx < (xj - xi) * (testy - yi) / (yj - yi) + xi))
c = !c;
}
return c;
}
bool intersects_helper(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometry_msgs::Point>& polygon2)
{
for (unsigned int i = 0; i < polygon1.size(); i++)
if (intersects(polygon2, polygon1[i].x, polygon1[i].y))
return true;
return false;
}
bool intersects(std::vector<geometry_msgs::Point>& polygon1, std::vector<geometry_msgs::Point>& polygon2)
{
return intersects_helper(polygon1, polygon2) || intersects_helper(polygon2, polygon1);
}

104
src/observation_buffer.cpp
View File

@@ -1,44 +1,13 @@
/*********************************************************************
*
* 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
*********************************************************************/
#include <costmap_2d/observation_buffer.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
// #include <tf2_sensor_msgs/tf2_sensor_msgs.h>
// #include <sensor_msgs/point_cloud2_iterator.h>
#include<tf2/convert.h>
#include <sensor_msgs/point_cloud2_iterator.h>
#include <cstdint>
using namespace std;
using namespace tf2;
@@ -47,10 +16,11 @@ namespace costmap_2d
{
ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time, double expected_update_rate,
double min_obstacle_height, double max_obstacle_height, double obstacle_range,
double raytrace_range, tf2_ros::Buffer& tf2_buffer, string global_frame,
double raytrace_range, tf2::BufferCore& tf2_buffer, string global_frame,
string sensor_frame, double tf_tolerance) :
tf2_buffer_(tf2_buffer), observation_keep_time_(observation_keep_time), expected_update_rate_(expected_update_rate),
last_updated_(ros::Time::now()), global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
last_updated_(std::chrono::system_clock::now()),
global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name),
min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height),
obstacle_range_(obstacle_range), raytrace_range_(raytrace_range), tf_tolerance_(tf_tolerance)
{
@@ -62,13 +32,17 @@ ObservationBuffer::~ObservationBuffer()
bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
{
ros::Time transform_time = ros::Time::now();
geometry_msgs::Point A;
// ros::Time transform_time = ros::Time::now();
double transform_time =
std::chrono::duration<double>(
std::chrono::system_clock::now().time_since_epoch()).count();
std::string tf_error;
geometry_msgs::TransformStamped transformStamped;
if (!tf2_buffer_.canTransform(new_global_frame, global_frame_, transform_time, ros::Duration(tf_tolerance_), &tf_error))
if (!tf2_buffer_.canTransform(new_global_frame, global_frame_, tf2::Time::now(), &tf_error))
{
ROS_ERROR("Transform between %s and %s with tolerance %.2f failed: %s.", new_global_frame.c_str(),
printf("Transform between %s and %s with tolerance %.2f failed: %s.", new_global_frame.c_str(),
global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
return false;
}
@@ -86,15 +60,21 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
origin.point = obs.origin_;
// we need to transform the origin of the observation to the new global frame
tf2_buffer_.transform(origin, origin, new_global_frame);
tf2::doTransform(origin, origin,
tf2_buffer_.lookupTransform(new_global_frame,
tf2::getFrameId(origin),
tf2::getTimestamp(origin)));
obs.origin_ = origin.point;
// we also need to transform the cloud of the observation to the new global frame
tf2_buffer_.transform(*(obs.cloud_), *(obs.cloud_), new_global_frame);
tf2::doTransform(*(obs.cloud_), *(obs.cloud_),
tf2_buffer_.lookupTransform(new_global_frame,
tf2::getFrameId(*(obs.cloud_)),
tf2::getTimestamp(*(obs.cloud_))));
}
catch (TransformException& ex)
{
ROS_ERROR("TF Error attempting to transform an observation from %s to %s: %s", global_frame_.c_str(),
printf("TF Error attempting to transform an observation from %s to %s: %s", global_frame_.c_str(),
new_global_frame.c_str(), ex.what());
return false;
}
@@ -124,7 +104,10 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
local_origin.point.x = 0;
local_origin.point.y = 0;
local_origin.point.z = 0;
tf2_buffer_.transform(local_origin, global_origin, global_frame_);
tf2::doTransform(local_origin, global_origin,
tf2_buffer_.lookupTransform(global_frame_,
tf2::getFrameId(local_origin),
tf2::getTimestamp(local_origin)));
tf2::convert(global_origin.point, observation_list_.front().origin_);
// make sure to pass on the raytrace/obstacle range of the observation buffer to the observations
@@ -134,7 +117,10 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
sensor_msgs::PointCloud2 global_frame_cloud;
// transform the point cloud
tf2_buffer_.transform(cloud, global_frame_cloud, global_frame_);
tf2::doTransform(cloud, global_frame_cloud,
tf2_buffer_.lookupTransform(global_frame_,
tf2::getFrameId(cloud),
tf2::getTimestamp(cloud)));
global_frame_cloud.header.stamp = cloud.header.stamp;
// now we need to remove observations from the cloud that are below or above our height thresholds
@@ -176,13 +162,13 @@ void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud)
{
// if an exception occurs, we need to remove the empty observation from the list
observation_list_.pop_front();
ROS_ERROR("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", sensor_frame_.c_str(),
printf("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", sensor_frame_.c_str(),
cloud.header.frame_id.c_str(), ex.what());
return;
}
// if the update was successful, we want to update the last updated time
last_updated_ = ros::Time::now();
last_updated_ = std::chrono::system_clock::now();
// we'll also remove any stale observations from the list
purgeStaleObservations();
@@ -208,7 +194,7 @@ void ObservationBuffer::purgeStaleObservations()
{
list<Observation>::iterator obs_it = observation_list_.begin();
// if we're keeping observations for no time... then we'll only keep one observation
if (observation_keep_time_ == ros::Duration(0.0))
if (observation_keep_time_ == std::chrono::duration<double>(0.0))
{
observation_list_.erase(++obs_it, observation_list_.end());
return;
@@ -219,7 +205,7 @@ void ObservationBuffer::purgeStaleObservations()
{
Observation& obs = *obs_it;
// check if the observation is out of date... and if it is, remove it and those that follow from the list
if ((last_updated_ - obs.cloud_->header.stamp) > observation_keep_time_)
if ((std::chrono::duration<double>(last_updated_.time_since_epoch()).count() - obs.cloud_->header.stamp) > observation_keep_time_.count())
{
observation_list_.erase(obs_it, observation_list_.end());
return;
@@ -230,22 +216,24 @@ void ObservationBuffer::purgeStaleObservations()
bool ObservationBuffer::isCurrent() const
{
if (expected_update_rate_ == ros::Duration(0.0))
if (expected_update_rate_ == std::chrono::duration<double>(0.0))
return true;
bool current = (ros::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();
bool current = (std::chrono::system_clock::now() - last_updated_) <= expected_update_rate_;
if (!current)
{
ROS_WARN(
printf(
"The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.",
topic_name_.c_str(), (ros::Time::now() - last_updated_).toSec(), expected_update_rate_.toSec());
topic_name_.c_str(), std::chrono::duration<double>(std::chrono::system_clock::now() - last_updated_).count(),
expected_update_rate_.count());
}
return current;
return true;
}
void ObservationBuffer::resetLastUpdated()
{
last_updated_ = ros::Time::now();
last_updated_ = std::chrono::system_clock::now();
}
} // namespace costmap_2d