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

30
test/static_layer_test.cpp
View File

@@ -1,28 +1,28 @@
#include <costmap_2d/costmap_2d_robot.h>
#include <costmap_2d/cost_values.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <robot_costmap_2d/cost_values.h>
#include <gtest/gtest.h>
#include <tf3/buffer_core.h>
#include <robot_geometry_msgs/PoseWithCovariance.h>
namespace costmap_2d {
namespace robot_costmap_2d {
class CostmapTester : public testing::Test {
public:
CostmapTester(tf3::BufferCore& tf);
void checkConsistentCosts();
void compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y);
void compareCells(costmap_2d::Costmap2D& costmap,
void compareCellToNeighbors(robot_costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y);
void compareCells(robot_costmap_2d::Costmap2D& costmap,
unsigned int x, unsigned int y, unsigned int nx, unsigned int ny);
virtual void TestBody(){}
private:
costmap_2d::Costmap2DROBOT costmap_ros_;
robot_costmap_2d::Costmap2DROBOT costmap_ros_;
};
CostmapTester::CostmapTester(tf3::BufferCore& tf): costmap_ros_("costmap_params", tf){}
void CostmapTester::checkConsistentCosts(){
costmap_2d::Costmap2D* costmap = costmap_ros_.getCostmap();
robot_costmap_2d::Costmap2D* costmap = costmap_ros_.getCostmap();
//get a copy of the costmap contained by our ros wrapper
costmap->saveMap("costmap_test.pgm");
@@ -35,7 +35,7 @@ void CostmapTester::checkConsistentCosts(){
}
}
void CostmapTester::compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y){
void CostmapTester::compareCellToNeighbors(robot_costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y){
//we'll compare the cost of this cell with that of its eight neighbors to see if they're reasonable
for(int offset_x = -1; offset_x <= 1; ++offset_x){
for(int offset_y = -1; offset_y <= 1; ++offset_y){
@@ -51,18 +51,18 @@ void CostmapTester::compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsig
}
//for all lethal and inscribed costs, we'll make sure that their neighbors have the cost values we'd expect
void CostmapTester::compareCells(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y, unsigned int nx, unsigned int ny){
void CostmapTester::compareCells(robot_costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y, unsigned int nx, unsigned int ny){
double cell_distance = hypot(static_cast<int>(x-nx), static_cast<int>(y-ny));
unsigned char cell_cost = costmap.getCost(x, y);
unsigned char neighbor_cost = costmap.getCost(nx, ny);
if(cell_cost == costmap_2d::LETHAL_OBSTACLE){
if(cell_cost == robot_costmap_2d::LETHAL_OBSTACLE){
//if the cell is a lethal obstacle, then we know that all its neighbors should have equal or slighlty less cost
unsigned char expected_lowest_cost = 0; // ################costmap.computeCost(cell_distance);
EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (cell_distance > 0 /*costmap.cell_inflation_radius_*/ && neighbor_cost == costmap_2d::FREE_SPACE));
EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (cell_distance > 0 /*costmap.cell_inflation_radius_*/ && neighbor_cost == robot_costmap_2d::FREE_SPACE));
}
else if(cell_cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
else if(cell_cost == robot_costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
//the furthest valid distance from an obstacle is the inscribed radius plus the cell distance away
double furthest_valid_distance = 0; // ################costmap.cell_inscribed_radius_ + cell_distance + 1;
unsigned char expected_lowest_cost = 0; // ################costmap.computeCost(furthest_valid_distance);
@@ -72,19 +72,19 @@ void CostmapTester::compareCells(costmap_2d::Costmap2D& costmap, unsigned int x,
printf("Cell: (%d, %d), Neighbor: (%d, %d)", x, y, nx, ny);
costmap.saveMap("failing_costmap.pgm");
}
EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (furthest_valid_distance > 0/* costmap.cell_inflation_radius_ */&& neighbor_cost == costmap_2d::FREE_SPACE));
EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (furthest_valid_distance > 0/* costmap.cell_inflation_radius_ */&& neighbor_cost == robot_costmap_2d::FREE_SPACE));
}
}
};
costmap_2d::CostmapTester* map_tester = NULL;
robot_costmap_2d::CostmapTester* map_tester = NULL;
int main(int argc, char** argv){
tf3::BufferCore tf(tf3::Duration(10));
map_tester = new costmap_2d::CostmapTester(tf);
map_tester = new robot_costmap_2d::CostmapTester(tf);
return(0);