96 lines
3.2 KiB
C++
Executable File
96 lines
3.2 KiB
C++
Executable File
#include <ros/ros.h>
|
|
#include <gtest/gtest.h>
|
|
#include <clear_costmap_recovery/clear_costmap_recovery.h>
|
|
|
|
#include <costmap_2d/testing_helper.h>
|
|
#include <tf2_ros/transform_listener.h>
|
|
|
|
tf2_ros::Buffer* transformer;
|
|
tf2_ros::TransformListener* tfl;
|
|
|
|
using costmap_2d::LETHAL_OBSTACLE;
|
|
|
|
void testClearBehavior(std::string name,
|
|
double distance,
|
|
bool obstacles,
|
|
bool static_map,
|
|
costmap_2d::Costmap2DROS* global_costmap,
|
|
costmap_2d::Costmap2DROS* local_costmap){
|
|
clear_costmap_recovery::ClearCostmapRecovery clear = clear_costmap_recovery::ClearCostmapRecovery();
|
|
|
|
ros::NodeHandle clr("~/" + name);
|
|
clr.setParam("reset_distance", distance);
|
|
|
|
std::vector<std::string> clearable_layers;
|
|
if(obstacles)
|
|
clearable_layers.push_back( std::string("obstacles") );
|
|
if(static_map)
|
|
clearable_layers.push_back( std::string("static_map") );
|
|
|
|
clr.setParam("layer_names", clearable_layers);
|
|
|
|
clear.initialize(name, transformer, global_costmap, local_costmap);
|
|
|
|
clear.runBehavior();
|
|
}
|
|
|
|
void testCountLethal(std::string name, double distance, bool obstacles, bool static_map, int global_lethal, int local_lethal=0)
|
|
{
|
|
costmap_2d::Costmap2DROS global(name + "/global", *transformer);
|
|
costmap_2d::Costmap2DROS local(name + "/local" , *transformer);
|
|
boost::shared_ptr<costmap_2d::ObstacleLayer> olayer;
|
|
|
|
std::vector<boost::shared_ptr<costmap_2d::Layer> >* plugins = global.getLayeredCostmap()->getPlugins();
|
|
for (std::vector<boost::shared_ptr<costmap_2d::Layer> >::iterator pluginp = plugins->begin(); pluginp != plugins->end(); ++pluginp) {
|
|
boost::shared_ptr<costmap_2d::Layer> plugin = *pluginp;
|
|
if(plugin->getName().find("obstacles")!=std::string::npos){
|
|
olayer = boost::static_pointer_cast<costmap_2d::ObstacleLayer>(plugin);
|
|
addObservation(&(*olayer), 5.0, 0.0, MAX_Z/2, 0, 0, MAX_Z/2);
|
|
addObservation(&(*olayer), 0.0, 5.0, MAX_Z/2, 0, 0, MAX_Z/2);
|
|
}
|
|
}
|
|
|
|
global.updateMap();
|
|
local.updateMap();
|
|
olayer->clearStaticObservations(true, true);
|
|
|
|
testClearBehavior("clear", distance, obstacles, static_map, &global, &local);
|
|
|
|
global.updateMap();
|
|
local.updateMap();
|
|
|
|
printMap(*global.getCostmap());
|
|
ASSERT_EQ(countValues(*global.getCostmap(), LETHAL_OBSTACLE), global_lethal);
|
|
ASSERT_EQ(countValues( *local.getCostmap(), LETHAL_OBSTACLE), local_lethal);
|
|
|
|
}
|
|
|
|
TEST(ClearTester, basicTest){
|
|
testCountLethal("base", 3.0, true, false, 20);
|
|
}
|
|
|
|
TEST(ClearTester, bigRadiusTest){
|
|
testCountLethal("base", 20.0, true, false, 22);
|
|
}
|
|
|
|
TEST(ClearTester, clearNoLayersTest){
|
|
testCountLethal("base", 20.0, false, false, 22);
|
|
}
|
|
|
|
TEST(ClearTester, clearBothTest){
|
|
testCountLethal("base", 3.0, true, true, 0);
|
|
}
|
|
|
|
TEST(ClearTester, clearBothTest2){
|
|
testCountLethal("base", 12.0, true, true, 6);
|
|
}
|
|
|
|
|
|
int main(int argc, char** argv){
|
|
ros::init(argc, argv, "clear_tests");
|
|
testing::InitGoogleTest(&argc, argv);
|
|
transformer = new tf2_ros::Buffer(ros::Duration(10));
|
|
tfl = new tf2_ros::TransformListener(*transformer);
|
|
return RUN_ALL_TESTS();
|
|
}
|