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

Hiep update

Browse Source
This commit is contained in:
HiepLM
2025-12-30 10:24:18 +07:00
parent 4246453ae6
commit 72b2f3c639
49 changed files with 476 additions and 476 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
plugins/critical_layer.cpp
View File

@@ -1,11 +1,11 @@
#include <costmap_2d/critical_layer.h>
#include <robot_costmap_2d/critical_layer.h>
#include <boost/dll/alias.hpp>
using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::CRITICAL_SPACE;
using robot_costmap_2d::NO_INFORMATION;
using robot_costmap_2d::LETHAL_OBSTACLE;
using robot_costmap_2d::CRITICAL_SPACE;
namespace costmap_2d
namespace robot_costmap_2d
{
CriticalLayer::CriticalLayer(){}
@@ -21,7 +21,7 @@ unsigned char CriticalLayer::interpretValue(unsigned char value)
return NO_INFORMATION;
}
void CriticalLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
void CriticalLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
// printf("TEST PLUGIN CRITICAL\n");
if (!map_received_)

62
plugins/directional_layer.cpp
View File

@@ -1,18 +1,18 @@
#include <costmap_2d/directional_layer.h>
#include <robot_costmap_2d/directional_layer.h>
#include <data_convert/data_convert.h>
#include <tf3/convert.h>
#include <tf3/utils.h>
#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <boost/dll/alias.hpp>
#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
using costmap_2d::CRITICAL_SPACE;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::NO_INFORMATION;
using robot_costmap_2d::CRITICAL_SPACE;
using robot_costmap_2d::LETHAL_OBSTACLE;
using robot_costmap_2d::NO_INFORMATION;
namespace costmap_2d
namespace robot_costmap_2d
{
DirectionalLayer::DirectionalLayer()
{}
@@ -79,7 +79,7 @@ namespace costmap_2d
int index = getIndex(ix / 2, iy / 2);
directional_areas_[index][0] = (uint16_t)(color_avg & 0xfff);
directional_areas_[index][1] = (uint16_t)(color_avg >> 12u);
costmap_[index] = costmap_2d::NO_INFORMATION;
costmap_[index] = robot_costmap_2d::NO_INFORMATION;
}
}
map_frame_ = new_map.header.frame_id;
@@ -144,7 +144,7 @@ namespace costmap_2d
|| p == path.poses.back()
)
{
if (value == costmap_2d::INSCRIBED_INFLATED_OBSTACLE || value == costmap_2d::LETHAL_OBSTACLE)
if (value == robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE || value == robot_costmap_2d::LETHAL_OBSTACLE)
{
std::pair<unsigned int, double> x_val(mx, p.pose.position.x);
std::pair<unsigned int, double> y_val(my, p.pose.position.y);
@@ -187,7 +187,7 @@ namespace costmap_2d
for (int i = 0; i < size_costmap; i++)
{
int8_t value;
if (costmap[i] == costmap_2d::NO_INFORMATION)
if (costmap[i] == robot_costmap_2d::NO_INFORMATION)
{
value = -1;
}
@@ -211,24 +211,24 @@ namespace costmap_2d
unsigned char DirectionalLayer::laneFilter(unsigned int x, unsigned int y, double yaw_robot)
{
double yaw_lane;
unsigned char cost = costmap_2d::NO_INFORMATION;
unsigned char cost = robot_costmap_2d::NO_INFORMATION;
int index = getIndex(x, y);
for (auto &lane : directional_areas_[index])
{
if (lane > 359)
{
cost = std::min(cost, costmap_2d::NO_INFORMATION);
cost = std::min(cost, robot_costmap_2d::NO_INFORMATION);
}
else
{
double yaw_lane = (double)lane / 180 * M_PI;
if (0.4 <= cos(yaw_robot - yaw_lane) && cos(yaw_robot - yaw_lane) <= 1.0)
cost = std::min(cost, costmap_2d::FREE_SPACE);
cost = std::min(cost, robot_costmap_2d::FREE_SPACE);
else if (-1.0 <= cos(yaw_robot - yaw_lane) && cos(yaw_robot - yaw_lane) <= -0.4)
cost = std::min(cost, costmap_2d::LETHAL_OBSTACLE);
cost = std::min(cost, robot_costmap_2d::LETHAL_OBSTACLE);
else
cost = std::min(cost, costmap_2d::NO_INFORMATION);
cost = std::min(cost, robot_costmap_2d::NO_INFORMATION);
}
}
return cost;
@@ -278,10 +278,10 @@ namespace costmap_2d
for (int j = y[i].first; j <= y_max; j++)
{
int y_ = j;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
continue;
unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
costmap_[getIndex(x_, y_)] = value;
else
break;
@@ -290,10 +290,10 @@ namespace costmap_2d
for (int k = y[i].first; k >= y_min; k--)
{
int y_ = k;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
continue;
unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
costmap_[getIndex(x_, y_)] = value;
else
break;
@@ -304,10 +304,10 @@ namespace costmap_2d
for (int j = x[i].first; j < layered_costmap_->getCostmap()->getSizeInCellsX(); j++)
{
int x_ = j;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
continue;
unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
costmap_[getIndex(x_, y_)] = value;
else
break;
@@ -316,10 +316,10 @@ namespace costmap_2d
for (int k = x[i].first; k >= 0; k--)
{
int x_ = k;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
continue;
unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
costmap_[getIndex(x_, y_)] = value;
else
break;
@@ -332,10 +332,10 @@ namespace costmap_2d
for (int j = x[i].first; j <= x_max; j++)
{
int x_ = j;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
continue;
unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
costmap_[getIndex(x_, y_)] = value;
else
break;
@@ -344,10 +344,10 @@ namespace costmap_2d
for (int k = x[i].first; k >= x_min; k--)
{
int x_ = k;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
continue;
unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
costmap_[getIndex(x_, y_)] = value;
else
break;
@@ -358,10 +358,10 @@ namespace costmap_2d
for (int j = y[i].first; j < layered_costmap_->getCostmap()->getSizeInCellsY(); j++)
{
int y_ = j;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
continue;
unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
costmap_[getIndex(x_, y_)] = value;
else
break;
@@ -370,10 +370,10 @@ namespace costmap_2d
for (int k = y[i].first; k >= 0; k--)
{
int y_ = k;
if (costmap_[getIndex(x_, y_)] == costmap_2d::LETHAL_OBSTACLE)
if (costmap_[getIndex(x_, y_)] == robot_costmap_2d::LETHAL_OBSTACLE)
continue;
unsigned char value = laneFilter(x_, y_, yaw_robot[i]);
if (value >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != costmap_2d::NO_INFORMATION)
if (value >= robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE && value != robot_costmap_2d::NO_INFORMATION)
costmap_[getIndex(x_, y_)] = value;
else
break;
@@ -391,7 +391,7 @@ namespace costmap_2d
for (unsigned int ix = 0; ix < size_x; ix++)
{
int index = getIndex(ix, iy);
costmap_[index] = costmap_2d::NO_INFORMATION;
costmap_[index] = robot_costmap_2d::NO_INFORMATION;
}
}
}

22
plugins/inflation_layer.cpp
View File

@@ -36,18 +36,18 @@
* David V. Lu!!
*********************************************************************/
#include <algorithm>
#include <costmap_2d/inflation_layer.h>
#include <costmap_2d/costmap_math.h>
#include <costmap_2d/footprint.h>
#include <robot_costmap_2d/inflation_layer.h>
#include <robot_costmap_2d/costmap_math.h>
#include <robot_costmap_2d/footprint.h>
#include <boost/thread.hpp>
#include <boost/dll/alias.hpp>
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
using costmap_2d::NO_INFORMATION;
using robot_costmap_2d::LETHAL_OBSTACLE;
using robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
using robot_costmap_2d::NO_INFORMATION;
namespace costmap_2d
namespace robot_costmap_2d
{
InflationLayer::InflationLayer()
@@ -91,7 +91,7 @@ void InflationLayer::onInitialize()
bool InflationLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
{
try {
std::string folder = COSTMAP_2D_DIR;
std::string folder = ROBOT_COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name);
YAML::Node config = YAML::LoadFile(path_source);
@@ -132,7 +132,7 @@ bool InflationLayer::getParams(const std::string& config_file_name, robot::NodeH
void InflationLayer::matchSize()
{
boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap();
robot_costmap_2d::Costmap2D* costmap = layered_costmap_->getCostmap();
resolution_ = costmap->getResolution();
cell_inflation_radius_ = cellDistance(inflation_radius_);
computeCaches();
@@ -191,7 +191,7 @@ void InflationLayer::onFootprintChanged()
layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_);
}
void InflationLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_);
if (cell_inflation_radius_ == 0)
@@ -416,4 +416,4 @@ static boost::shared_ptr<Layer> create_inflation_plugin() {
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_inflation_plugin, InflationLayer)
} // namespace costmap_2d
} // namespace robot_costmap_2d

26
plugins/obstacle_layer.cpp
View File

@@ -35,8 +35,8 @@
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include <costmap_2d/obstacle_layer.h>
#include <costmap_2d/costmap_math.h>
#include <robot_costmap_2d/obstacle_layer.h>
#include <robot_costmap_2d/costmap_math.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h>
@@ -44,14 +44,14 @@
#include <boost/dll/alias.hpp>
using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::FREE_SPACE;
using robot_costmap_2d::NO_INFORMATION;
using robot_costmap_2d::LETHAL_OBSTACLE;
using robot_costmap_2d::FREE_SPACE;
using costmap_2d::ObservationBuffer;
using costmap_2d::Observation;
using robot_costmap_2d::ObservationBuffer;
using robot_costmap_2d::Observation;
namespace costmap_2d
namespace robot_costmap_2d
{
void ObstacleLayer::onInitialize()
@@ -75,7 +75,7 @@ ObstacleLayer::~ObstacleLayer()
bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
{
try {
std::string folder = COSTMAP_2D_DIR;
std::string folder = ROBOT_COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name);
@@ -405,11 +405,11 @@ void ObstacleLayer::updateFootprint(double robot_x, double robot_y, double robot
}
}
void ObstacleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
void ObstacleLayer::updateCosts(robot_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);
setConvexPolygonCost(transformed_footprint_, robot_costmap_2d::FREE_SPACE);
}
switch (combination_method_)
@@ -426,7 +426,7 @@ void ObstacleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, i
}
void ObstacleLayer::addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing)
void ObstacleLayer::addStaticObservation(robot_costmap_2d::Observation& obs, bool marking, bool clearing)
{
if (marking)
static_marking_observations_.push_back(obs);
@@ -597,4 +597,4 @@ static boost::shared_ptr<Layer> create_obstacle_plugin() {
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_obstacle_plugin, ObstacleLayer)
} // namespace costmap_2d
} // namespace robot_costmap_2d

12
plugins/preferred_layer.cpp
View File

@@ -1,12 +1,12 @@
#include <costmap_2d/preferred_layer.h>
#include <robot_costmap_2d/preferred_layer.h>
#include <boost/dll/alias.hpp>
using costmap_2d::NO_INFORMATION;
using costmap_2d::FREE_SPACE;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::PREFERRED_SPACE;
using robot_costmap_2d::NO_INFORMATION;
using robot_costmap_2d::FREE_SPACE;
using robot_costmap_2d::LETHAL_OBSTACLE;
using robot_costmap_2d::PREFERRED_SPACE;
namespace costmap_2d
namespace robot_costmap_2d
{
PreferredLayer::PreferredLayer(){}

20
plugins/static_layer.cpp
View File

@@ -36,20 +36,20 @@
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include <costmap_2d/static_layer.h>
#include <costmap_2d/costmap_math.h>
#include <robot_costmap_2d/static_layer.h>
#include <robot_costmap_2d/costmap_math.h>
#include <tf3/convert.h>
#include <tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
#include <boost/dll/alias.hpp>
using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::FREE_SPACE;
using robot_costmap_2d::NO_INFORMATION;
using robot_costmap_2d::LETHAL_OBSTACLE;
using robot_costmap_2d::FREE_SPACE;
namespace costmap_2d
namespace robot_costmap_2d
{
StaticLayer::StaticLayer()
@@ -75,7 +75,7 @@ void StaticLayer::onInitialize()
bool StaticLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh)
{
try {
std::string folder = COSTMAP_2D_DIR;
std::string folder = ROBOT_COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name);
YAML::Node config = YAML::LoadFile(path_source);
@@ -324,7 +324,7 @@ void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
has_updated_data_ = false;
}
void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
void StaticLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
if (!map_received_)
return;
@@ -389,4 +389,4 @@ static boost::shared_ptr<Layer> create_static_plugin() {
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_static_plugin, StaticLayer)
} // namespace costmap_2d
} // namespace robot_costmap_2d

10
plugins/unpreferred_layer.cpp
View File

@@ -1,11 +1,11 @@
#include <costmap_2d/unpreferred_layer.h>
#include <robot_costmap_2d/unpreferred_layer.h>
#include <boost/dll/alias.hpp>
using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::PREFERRED_SPACE;
using robot_costmap_2d::NO_INFORMATION;
using robot_costmap_2d::LETHAL_OBSTACLE;
using robot_costmap_2d::PREFERRED_SPACE;
namespace costmap_2d
namespace robot_costmap_2d
{
UnPreferredLayer::UnPreferredLayer(){}

20
plugins/voxel_layer.cpp
View File

@@ -35,20 +35,20 @@
* Author: Eitan Marder-Eppstein
* David V. Lu!!
*********************************************************************/
#include <costmap_2d/voxel_layer.h>
#include <robot_costmap_2d/voxel_layer.h>
#include <robot_sensor_msgs/point_cloud2_iterator.h>
#include <boost/dll/alias.hpp>
#define VOXEL_BITS 16
using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::FREE_SPACE;
using robot_costmap_2d::NO_INFORMATION;
using robot_costmap_2d::LETHAL_OBSTACLE;
using robot_costmap_2d::FREE_SPACE;
using costmap_2d::ObservationBuffer;
using costmap_2d::Observation;
using robot_costmap_2d::ObservationBuffer;
using robot_costmap_2d::Observation;
namespace costmap_2d
namespace robot_costmap_2d
{
void VoxelLayer::onInitialize()
@@ -68,7 +68,7 @@ bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandl
{
try
{
std::string folder = COSTMAP_2D_DIR;
std::string folder = ROBOT_COSTMAP_2D_DIR;
std::string path_source = getSourceFile(folder,config_file_name);
YAML::Node config = YAML::LoadFile(path_source);
@@ -228,7 +228,7 @@ void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw,
// if (publish_voxel_)
// {
// costmap_2d::VoxelGrid grid_msg;
// robot_costmap_2d::VoxelGrid grid_msg;
// unsigned int size = voxel_grid_.sizeX() * voxel_grid_.sizeY();
// grid_msg.size_x = voxel_grid_.sizeX();
// grid_msg.size_y = voxel_grid_.sizeY();
@@ -490,4 +490,4 @@ static boost::shared_ptr<Layer> create_voxel_plugin() {
// Alias cho Boost.DLL (nếu muốn dùng boost::dll::import_alias)
BOOST_DLL_ALIAS(create_voxel_plugin, VoxelLayer)
} // namespace costmap_2d
} // namespace robot_costmap_2d