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

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;
}
}
}