HiepLM/mir_amr
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add costmap_2d package sources

Browse Source 
Convert navigations/costmap_2d from gitlink to normal tracked files.
This commit is contained in:
HiepLM
2026-05-28 10:44:00 +07:00
parent 167c52aeb6
commit c478cbee78
72 changed files with 11576 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
navigations/costmap_2d/test/TenByTen.pgm Executable file
View File

Binary file not shown.

7
navigations/costmap_2d/test/TenByTen.yaml Executable file
View File

@@ -0,0 +1,7 @@
image: TenByTen.pgm
resolution: 1.0
origin: [0,0,0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.196

80
navigations/costmap_2d/test/array_parser_test.cpp Executable file
View File

@@ -0,0 +1,80 @@
/*
* Copyright (c) 2012, 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 <gtest/gtest.h>
#include "costmap_2d/array_parser.h"
using namespace costmap_2d;
TEST(array_parser, basic_operation)
{
std::string error;
std::vector<std::vector<float> > vvf;
vvf = parseVVF( "[[1, 2.2], [.3, -4e4]]", error );
EXPECT_EQ( 2, vvf.size() );
EXPECT_EQ( 2, vvf[0].size() );
EXPECT_EQ( 2, vvf[1].size() );
EXPECT_EQ( 1.0f, vvf[0][0] );
EXPECT_EQ( 2.2f, vvf[0][1] );
EXPECT_EQ( 0.3f, vvf[1][0] );
EXPECT_EQ( -40000.0f, vvf[1][1] );
EXPECT_EQ( "", error );
}
TEST(array_parser, missing_open)
{
std::string error;
std::vector<std::vector<float> > vvf;
vvf = parseVVF( "[1, 2.2], [.3, -4e4]]", error );
EXPECT_FALSE( error == "" );
}
TEST(array_parser, missing_close)
{
std::string error;
std::vector<std::vector<float> > vvf;
vvf = parseVVF( "[[1, 2.2], [.3, -4e4]", error );
EXPECT_FALSE( error == "" );
}
TEST(array_parser, wrong_depth)
{
std::string error;
std::vector<std::vector<float> > vvf;
vvf = parseVVF( "[1, 2.2], [.3, -4e4]", error );
EXPECT_FALSE( error == "" );
}
int main(int argc, char** argv)
{
testing::InitGoogleTest( &argc, argv );
return RUN_ALL_TESTS();
}

132
navigations/costmap_2d/test/coordinates_test.cpp Executable file
View File

@@ -0,0 +1,132 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2019, Locus Robotics
* 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 copyright holder 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 HOLDER 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.
*/
// Tests ripped from https://github.com/locusrobotics/robot_navigation/blob/master/nav_grid/test/utest.cpp
#include <gtest/gtest.h>
#include <costmap_2d/costmap_2d.h>
using namespace costmap_2d;
TEST(CostmapCoordinates, easy_coordinates_test)
{
Costmap2D costmap(2, 3, 1.0, 0, 0);
double wx, wy;
costmap.mapToWorld(0u, 0u, wx, wy);
EXPECT_DOUBLE_EQ(wx, 0.5);
EXPECT_DOUBLE_EQ(wy, 0.5);
costmap.mapToWorld(1u, 2u, wx, wy);
EXPECT_DOUBLE_EQ(wx, 1.5);
EXPECT_DOUBLE_EQ(wy, 2.5);
unsigned int umx, umy;
int mx, my;
ASSERT_TRUE(costmap.worldToMap(wx, wy, umx, umy));
EXPECT_EQ(umx, 1);
EXPECT_EQ(umy, 2);
costmap.worldToMapNoBounds(wx, wy, mx, my);
EXPECT_EQ(mx, 1);
EXPECT_EQ(my, 2);
// Invalid Coordinate
wx = 2.5;
EXPECT_FALSE(costmap.worldToMap(wx, wy, umx, umy));
costmap.worldToMapEnforceBounds(wx, wy, mx, my);
EXPECT_EQ(mx, 1);
EXPECT_EQ(my, 2);
costmap.worldToMapNoBounds(wx, wy, mx, my);
EXPECT_EQ(mx, 2);
EXPECT_EQ(my, 2);
// Border Cases
EXPECT_TRUE(costmap.worldToMap(0.0, wy, umx, umy));
EXPECT_EQ(umx, 0);
EXPECT_TRUE(costmap.worldToMap(0.25, wy, umx, umy));
EXPECT_EQ(umx, 0);
EXPECT_TRUE(costmap.worldToMap(0.75, wy, umx, umy));
EXPECT_EQ(umx, 0);
EXPECT_TRUE(costmap.worldToMap(0.9999, wy, umx, umy));
EXPECT_EQ(umx, 0);
EXPECT_TRUE(costmap.worldToMap(1.0, wy, umx, umy));
EXPECT_EQ(umx, 1);
EXPECT_TRUE(costmap.worldToMap(1.25, wy, umx, umy));
EXPECT_EQ(umx, 1);
EXPECT_TRUE(costmap.worldToMap(1.75, wy, umx, umy));
EXPECT_EQ(umx, 1);
EXPECT_TRUE(costmap.worldToMap(1.9999, wy, umx, umy));
EXPECT_EQ(umx, 1);
EXPECT_FALSE(costmap.worldToMap(2.0, wy, umx, umy));
costmap.worldToMapEnforceBounds(2.0, wy, mx, my);
EXPECT_EQ(mx, 1);
}
TEST(CostmapCoordinates, hard_coordinates_test)
{
Costmap2D costmap(2, 3, 0.1, -0.2, 0.2);
double wx, wy;
costmap.mapToWorld(0, 0, wx, wy);
EXPECT_DOUBLE_EQ(wx, -0.15);
EXPECT_DOUBLE_EQ(wy, 0.25);
costmap.mapToWorld(1, 2, wx, wy);
EXPECT_DOUBLE_EQ(wx, -0.05);
EXPECT_DOUBLE_EQ(wy, 0.45);
unsigned int umx, umy;
int mx, my;
EXPECT_TRUE(costmap.worldToMap(wx, wy, umx, umy));
EXPECT_EQ(umx, 1);
EXPECT_EQ(umy, 2);
costmap.worldToMapNoBounds(wx, wy, mx, my);
EXPECT_EQ(mx, 1);
EXPECT_EQ(my, 2);
// Invalid Coordinate
wx = 2.5;
EXPECT_FALSE(costmap.worldToMap(wx, wy, umx, umy));
costmap.worldToMapEnforceBounds(wx, wy, mx, my);
EXPECT_EQ(mx, 1);
EXPECT_EQ(my, 2);
costmap.worldToMapNoBounds(wx, wy, mx, my);
EXPECT_EQ(mx, 27);
EXPECT_EQ(my, 2);
}
int main(int argc, char** argv)
{
testing::InitGoogleTest( &argc, argv );
return RUN_ALL_TESTS();
}

29
navigations/costmap_2d/test/costmap_params.yaml Executable file
View File

@@ -0,0 +1,29 @@
global_frame: map
robot_base_frame: base_link
update_frequency: 5.0
publish_frequency: 0.0
static_map: true
rolling_window: false
#START VOXEL STUFF
map_type: voxel
origin_z: 0.0
z_resolution: 0.2
z_voxels: 10
unknown_threshold: 10
mark_threshold: 0
#END VOXEL STUFF
transform_tolerance: 0.3
obstacle_range: 2.5
max_obstacle_height: 2.0
raytrace_range: 3.0
footprint: [[-0.325, -0.325], [-0.325, 0.325], [0.325, 0.325], [0.46, 0.0], [0.325, -0.325]]
#robot_radius: 0.46
footprint_padding: 0.01
inflation_radius: 0.55
cost_scaling_factor: 10.0
lethal_cost_threshold: 100
observation_sources: base_scan
base_scan: {data_type: LaserScan, expected_update_rate: 0.4,
observation_persistence: 0.0, marking: true, clearing: true, max_obstacle_height: 0.4, min_obstacle_height: 0.08}

146
navigations/costmap_2d/test/costmap_tester.cpp Executable file
View File

@@ -0,0 +1,146 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, 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 <gtest/gtest.h>
#include <ros/ros.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/cost_values.h>
#include <tf2_ros/transform_listener.h>
namespace costmap_2d {
class CostmapTester : public testing::Test {
public:
CostmapTester(tf2_ros::Buffer& tf);
void checkConsistentCosts();
void compareCellToNeighbors(costmap_2d::Costmap2D& costmap, unsigned int x, unsigned int y);
void compareCells(costmap_2d::Costmap2D& costmap,
unsigned int x, unsigned int y, unsigned int nx, unsigned int ny);
virtual void TestBody(){}
private:
costmap_2d::Costmap2DROS costmap_ros_;
};
CostmapTester::CostmapTester(tf2_ros::Buffer& tf): costmap_ros_("test_costmap", tf){}
void CostmapTester::checkConsistentCosts(){
costmap_2d::Costmap2D* costmap = costmap_ros_.getCostmap();
//get a copy of the costmap contained by our ros wrapper
costmap->saveMap("costmap_test.pgm");
//loop through the costmap and check for any unexpected drop-offs in costs
for(unsigned int i = 0; i < costmap->getSizeInCellsX(); ++i){
for(unsigned int j = 0; j < costmap->getSizeInCellsY(); ++j){
compareCellToNeighbors(*costmap, i, j);
}
}
}
void CostmapTester::compareCellToNeighbors(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){
int nx = x + offset_x;
int ny = y + offset_y;
//check to make sure that the neighbor cell is a legal one
if(nx >= 0 && nx < (int)costmap.getSizeInCellsX() && ny >=0 && ny < (int)costmap.getSizeInCellsY()){
compareCells(costmap, x, y, nx, ny);
}
}
}
}
//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){
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 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));
}
else if(cell_cost == 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);
if(neighbor_cost < expected_lowest_cost){
ROS_ERROR("Cell cost (%d, %d): %d, neighbor cost (%d, %d): %d, expected lowest cost: %d, cell distance: %.2f, furthest valid distance: %.2f",
x, y, cell_cost, nx, ny, neighbor_cost, expected_lowest_cost, cell_distance, furthest_valid_distance);
ROS_ERROR("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));
}
}
};
costmap_2d::CostmapTester* map_tester = NULL;
TEST(CostmapTester, checkConsistentCosts){
map_tester->checkConsistentCosts();
}
void testCallback(const ros::TimerEvent& e){
int test_result = RUN_ALL_TESTS();
ROS_INFO("gtest return value: %d", test_result);
ros::shutdown();
}
int main(int argc, char** argv){
ros::init(argc, argv, "costmap_tester_node");
testing::InitGoogleTest(&argc, argv);
ros::NodeHandle n;
ros::NodeHandle private_nh("~");
tf2_ros::Buffer tf(ros::Duration(10));
tf2_ros::TransformListener tfl(tf);
map_tester = new costmap_2d::CostmapTester(tf);
double wait_time;
private_nh.param("wait_time", wait_time, 30.0);
ros::Timer timer = n.createTimer(ros::Duration(wait_time), testCallback);
ros::spin();
return(0);
}

182
navigations/costmap_2d/test/footprint_tests.cpp Executable file
View File

@@ -0,0 +1,182 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, 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: Dave Hershberger
*********************************************************************/
#include <gtest/gtest.h>
#include <ros/ros.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
using namespace costmap_2d;
tf2_ros::TransformListener* tfl_;
tf2_ros::Buffer* tf_;
TEST( Costmap2DROS, unpadded_footprint_from_string_param )
{
Costmap2DROS cm( "unpadded/string", *tf_ );
std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
EXPECT_EQ( 3, footprint.size() );
EXPECT_EQ( 1.0f, footprint[ 0 ].x );
EXPECT_EQ( 1.0f, footprint[ 0 ].y );
EXPECT_EQ( 0.0f, footprint[ 0 ].z );
EXPECT_EQ( -1.0f, footprint[ 1 ].x );
EXPECT_EQ( 1.0f, footprint[ 1 ].y );
EXPECT_EQ( 0.0f, footprint[ 1 ].z );
EXPECT_EQ( -1.0f, footprint[ 2 ].x );
EXPECT_EQ( -1.0f, footprint[ 2 ].y );
EXPECT_EQ( 0.0f, footprint[ 2 ].z );
}
TEST( Costmap2DROS, padded_footprint_from_string_param )
{
Costmap2DROS cm( "padded/string", *tf_ );
std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
EXPECT_EQ( 3, footprint.size() );
EXPECT_EQ( 1.5f, footprint[ 0 ].x );
EXPECT_EQ( 1.5f, footprint[ 0 ].y );
EXPECT_EQ( 0.0f, footprint[ 0 ].z );
EXPECT_EQ( -1.5f, footprint[ 1 ].x );
EXPECT_EQ( 1.5f, footprint[ 1 ].y );
EXPECT_EQ( 0.0f, footprint[ 1 ].z );
EXPECT_EQ( -1.5f, footprint[ 2 ].x );
EXPECT_EQ( -1.5f, footprint[ 2 ].y );
EXPECT_EQ( 0.0f, footprint[ 2 ].z );
}
TEST( Costmap2DROS, radius_param )
{
Costmap2DROS cm( "radius/sub", *tf_ );
std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
// Circular robot has 16-point footprint auto-generated.
EXPECT_EQ( 16, footprint.size() );
// Check the first point
EXPECT_EQ( 10.0f, footprint[ 0 ].x );
EXPECT_EQ( 0.0f, footprint[ 0 ].y );
EXPECT_EQ( 0.0f, footprint[ 0 ].z );
// Check the 4th point, which should be 90 degrees around the circle from the first.
EXPECT_NEAR( 0.0f, footprint[ 4 ].x, 0.0001 );
EXPECT_NEAR( 10.0f, footprint[ 4 ].y, 0.0001 );
EXPECT_EQ( 0.0f, footprint[ 4 ].z );
}
TEST( Costmap2DROS, footprint_from_xmlrpc_param )
{
Costmap2DROS cm( "xmlrpc", *tf_ );
std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
EXPECT_EQ( 4, footprint.size() );
EXPECT_EQ( 0.1f, footprint[ 0 ].x );
EXPECT_EQ( 0.1f, footprint[ 0 ].y );
EXPECT_EQ( 0.0f, footprint[ 0 ].z );
EXPECT_EQ( -0.1f, footprint[ 1 ].x );
EXPECT_EQ( 0.1f, footprint[ 1 ].y );
EXPECT_EQ( 0.0f, footprint[ 1 ].z );
EXPECT_EQ( -0.1f, footprint[ 2 ].x );
EXPECT_EQ( -0.1f, footprint[ 2 ].y );
EXPECT_EQ( 0.0f, footprint[ 2 ].z );
EXPECT_EQ( 0.1f, footprint[ 3 ].x );
EXPECT_EQ( -0.1f, footprint[ 3 ].y );
EXPECT_EQ( 0.0f, footprint[ 3 ].z );
}
TEST( Costmap2DROS, footprint_from_same_level_param )
{
Costmap2DROS cm( "same_level", *tf_ );
std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
EXPECT_EQ( 3, footprint.size() );
EXPECT_EQ( 1.0f, footprint[ 0 ].x );
EXPECT_EQ( 2.0f, footprint[ 0 ].y );
EXPECT_EQ( 0.0f, footprint[ 0 ].z );
EXPECT_EQ( 3.0f, footprint[ 1 ].x );
EXPECT_EQ( 4.0f, footprint[ 1 ].y );
EXPECT_EQ( 0.0f, footprint[ 1 ].z );
EXPECT_EQ( 5.0f, footprint[ 2 ].x );
EXPECT_EQ( 6.0f, footprint[ 2 ].y );
EXPECT_EQ( 0.0f, footprint[ 2 ].z );
}
TEST( Costmap2DROS, footprint_from_xmlrpc_param_failure )
{
ASSERT_ANY_THROW( Costmap2DROS cm( "xmlrpc_fail", *tf_ ));
}
TEST( Costmap2DROS, footprint_empty )
{
Costmap2DROS cm( "empty", *tf_ );
std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
// With no specification of footprint or radius, defaults to 0.46 meter radius plus 0.01 meter padding.
EXPECT_EQ( 16, footprint.size() );
EXPECT_NEAR( 0.47f, footprint[ 0 ].x, 0.0001 );
EXPECT_NEAR( 0.0f, footprint[ 0 ].y, 0.0001 );
EXPECT_EQ( 0.0f, footprint[ 0 ].z );
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "footprint_tests_node");
tf_ = new tf2_ros::Buffer( ros::Duration( 10 ));
tfl_ = new tf2_ros::TransformListener(*tf_);
// This empty transform is added to satisfy the constructor of
// Costmap2DROS, which waits for the transform from map to base_link
// to become available.
geometry_msgs::TransformStamped base_rel_map;
base_rel_map.transform = tf2::toMsg(tf2::Transform::getIdentity());
base_rel_map.child_frame_id = "base_link";
base_rel_map.header.frame_id = "map";
base_rel_map.header.stamp = ros::Time::now();
tf_->setTransform( base_rel_map, "footprint_tests" );
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

29
navigations/costmap_2d/test/footprint_tests.launch Executable file
View File

@@ -0,0 +1,29 @@
<launch>
<test time-limit="10" test-name="footprint_test" pkg="costmap_2d" type="footprint_tests">
<param name="unpadded/string/footprint_padding" value="0" />
<param name="unpadded/footprint" value="[[1, 1], [-1, 1], [-1, -1]]" />
<param name="padded/string/footprint_padding" value=".5" />
<param name="padded/footprint" value="[[1, 1], [-1, 1], [-1, -1]]" />
<param name="radius/sub/footprint_padding" value="0" />
<param name="radius/robot_radius" value="10" />
<rosparam ns="xmlrpc">
footprint_padding: 0
footprint: [[0.1, 0.1], [-0.1, 0.1], [-0.1, -0.1], [0.1, -0.1]]
</rosparam>
<rosparam ns="xmlrpc_fail"> <!-- Footprint includes a 3-value point, which should make it fail. -->
footprint_padding: 0
footprint: [[0.1, 0.1], [-0.1, 0.1, 77.0], [-0.1, -0.1], [0.1, -0.1]]
</rosparam>
<param name="same_level/footprint_padding" value="0" />
<param name="same_level/footprint" value="[[1, 2], [3, 4], [5, 6]]" />
<param name="empty/empty" value="0" /> <!-- just so you can see there are no real params under "empty". -->
</test>
</launch>

417
navigations/costmap_2d/test/inflation_tests.cpp Executable file
View File

@@ -0,0 +1,417 @@
/*
* 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.
*/
/**
* @author David Lu!!
* Test harness for InflationLayer for Costmap2D
*/
#include <map>
#include <cmath>
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/obstacle_layer.h>
#include <costmap_2d/inflation_layer.h>
#include <costmap_2d/observation_buffer.h>
#include <costmap_2d/testing_helper.h>
#include <gtest/gtest.h>
using namespace costmap_2d;
using geometry_msgs::Point;
std::vector<Point> setRadii(LayeredCostmap& layers, double length, double width, double inflation_radius)
{
std::vector<Point> polygon;
Point p;
p.x = width;
p.y = length;
polygon.push_back(p);
p.x = width;
p.y = -length;
polygon.push_back(p);
p.x = -width;
p.y = -length;
polygon.push_back(p);
p.x = -width;
p.y = length;
polygon.push_back(p);
layers.setFootprint(polygon);
ros::NodeHandle nh;
nh.setParam("/inflation_tests/inflation/inflation_radius", inflation_radius);
return polygon;
}
// Test that a single point gets inflated properly
void validatePointInflation(unsigned int mx, unsigned int my, Costmap2D* costmap, InflationLayer* ilayer, double inflation_radius)
{
bool* seen = new bool[costmap->getSizeInCellsX() * costmap->getSizeInCellsY()];
memset(seen, false, costmap->getSizeInCellsX() * costmap->getSizeInCellsY() * sizeof(bool));
std::map<double, std::vector<CellData> > m;
CellData initial(costmap->getIndex(mx, my), mx, my, mx, my);
m[0].push_back(initial);
for (std::map<double, std::vector<CellData> >::iterator bin = m.begin(); bin != m.end(); ++bin)
{
for (int i = 0; i < bin->second.size(); ++i)
{
const CellData& cell = bin->second[i];
if (!seen[cell.index_])
{
seen[cell.index_] = true;
unsigned int dx = (cell.x_ > cell.src_x_) ? cell.x_ - cell.src_x_ : cell.src_x_ - cell.x_;
unsigned int dy = (cell.y_ > cell.src_y_) ? cell.y_ - cell.src_y_ : cell.src_y_ - cell.y_;
double dist = hypot(dx, dy);
unsigned char expected_cost = ilayer->computeCost(dist);
ASSERT_TRUE(costmap->getCost(cell.x_, cell.y_) >= expected_cost);
if (dist > inflation_radius)
{
continue;
}
if (dist == bin->first)
{
// Adding to our current bin could cause a reallocation
// Which appears to cause the iterator to get messed up
dist += 0.001;
}
if (cell.x_ > 0)
{
CellData data(costmap->getIndex(cell.x_-1, cell.y_),
cell.x_-1, cell.y_, cell.src_x_, cell.src_y_);
m[dist].push_back(data);
}
if (cell.y_ > 0)
{
CellData data(costmap->getIndex(cell.x_, cell.y_-1),
cell.x_, cell.y_-1, cell.src_x_, cell.src_y_);
m[dist].push_back(data);
}
if (cell.x_ < costmap->getSizeInCellsX() - 1)
{
CellData data(costmap->getIndex(cell.x_+1, cell.y_),
cell.x_+1, cell.y_, cell.src_x_, cell.src_y_);
m[dist].push_back(data);
}
if (cell.y_ < costmap->getSizeInCellsY() - 1)
{
CellData data(costmap->getIndex(cell.x_, cell.y_+1),
cell.x_, cell.y_+1, cell.src_x_, cell.src_y_);
m[dist].push_back(data);
}
}
}
}
delete[] seen;
}
TEST(costmap, testAdjacentToObstacleCanStillMove){
tf2_ros::Buffer tf;
LayeredCostmap layers("frame", false, false);
layers.resizeMap(10, 10, 1, 0, 0);
// Footprint with inscribed radius = 2.1
// circumscribed radius = 3.1
std::vector<Point> polygon = setRadii(layers, 2.1, 2.3, 4.1);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
InflationLayer* ilayer = addInflationLayer(layers, tf);
layers.setFootprint(polygon);
addObservation(olayer, 0, 0, MAX_Z);
layers.updateMap(0,0,0);
Costmap2D* costmap = layers.getCostmap();
//printMap(*costmap);
EXPECT_EQ( LETHAL_OBSTACLE, costmap->getCost( 0, 0 ));
EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, costmap->getCost( 1, 0 ));
EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, costmap->getCost( 2, 0 ));
EXPECT_TRUE( INSCRIBED_INFLATED_OBSTACLE > costmap->getCost( 3, 0 ));
EXPECT_TRUE( INSCRIBED_INFLATED_OBSTACLE > costmap->getCost( 2, 1 ));
EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, costmap->getCost( 1, 1 ));
}
TEST(costmap, testInflationShouldNotCreateUnknowns){
tf2_ros::Buffer tf;
LayeredCostmap layers("frame", false, false);
layers.resizeMap(10, 10, 1, 0, 0);
// Footprint with inscribed radius = 2.1
// circumscribed radius = 3.1
std::vector<Point> polygon = setRadii(layers, 2.1, 2.3, 4.1);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
InflationLayer* ilayer = addInflationLayer(layers, tf);
layers.setFootprint(polygon);
addObservation(olayer, 0, 0, MAX_Z);
layers.updateMap(0,0,0);
Costmap2D* costmap = layers.getCostmap();
EXPECT_EQ( countValues(*costmap, NO_INFORMATION), 0 );
}
/**
* Test for the cost function correctness with a larger range and different values
*/
TEST(costmap, testCostFunctionCorrectness){
tf2_ros::Buffer tf;
LayeredCostmap layers("frame", false, false);
layers.resizeMap(100, 100, 1, 0, 0);
// Footprint with inscribed radius = 5.0
// circumscribed radius = 8.0
std::vector<Point> polygon = setRadii(layers, 5.0, 6.25, 10.5);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
InflationLayer* ilayer = addInflationLayer(layers, tf);
layers.setFootprint(polygon);
addObservation(olayer, 50, 50, MAX_Z);
layers.updateMap(0,0,0);
Costmap2D* map = layers.getCostmap();
// Verify that the circumscribed cost lower bound is as expected: based on the cost function.
//unsigned char c = ilayer->computeCost(8.0);
//ASSERT_EQ(ilayer->getCircumscribedCost(), c);
for(unsigned int i = 0; i <= (unsigned int)ceil(5.0); i++){
// To the right
ASSERT_EQ(map->getCost(50 + i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
ASSERT_EQ(map->getCost(50 + i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
// To the left
ASSERT_EQ(map->getCost(50 - i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
ASSERT_EQ(map->getCost(50 - i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
// Down
ASSERT_EQ(map->getCost(50, 50 + i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
ASSERT_EQ(map->getCost(50, 50 + i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
// Up
ASSERT_EQ(map->getCost(50, 50 - i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
ASSERT_EQ(map->getCost(50, 50 - i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
}
// Verify the normalized cost attenuates as expected
for(unsigned int i = (unsigned int)(ceil(5.0) + 1); i <= (unsigned int)ceil(10.5); i++){
unsigned char expectedValue = ilayer->computeCost(i/1.0);
ASSERT_EQ(map->getCost(50 + i, 50), expectedValue);
}
// Update with no hits. Should clear (revert to the static map
/*map->resetMapOutsideWindow(0, 0, 0.0, 0.0);
cloud.points.resize(0);
p.x = 0.0;
p.y = 0.0;
p.z = MAX_Z;
Observation obs2(p, cloud, 100.0, 100.0);
std::vector<Observation> obsBuf2;
obsBuf2.push_back(obs2);
map->updateWorld(0, 0, obsBuf2, obsBuf2);
for(unsigned int i = 0; i < 100; i++)
for(unsigned int j = 0; j < 100; j++)
ASSERT_EQ(map->getCost(i, j), costmap_2d::FREE_SPACE);*/
}
/**
* Test that there is no regression and that costs do not get
* underestimated with the distance-as-key map used to replace
* the previously used priority queue. This is a more thorough
* test of the cost function being correctly applied.
*/
TEST(costmap, testInflationOrderCorrectness){
tf2_ros::Buffer tf;
LayeredCostmap layers("frame", false, false);
layers.resizeMap(10, 10, 1, 0, 0);
// Footprint with inscribed radius = 2.1
// circumscribed radius = 3.1
const double inflation_radius = 4.1;
std::vector<Point> polygon = setRadii(layers, 2.1, 2.3, inflation_radius);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
InflationLayer* ilayer = addInflationLayer(layers, tf);
layers.setFootprint(polygon);
// Add two diagonal cells, they would induce problems under the
// previous implementations
addObservation(olayer, 4, 4, MAX_Z);
addObservation(olayer, 5, 5, MAX_Z);
layers.updateMap(0, 0, 0);
validatePointInflation(4, 4, layers.getCostmap(), ilayer, inflation_radius);
validatePointInflation(5, 5, layers.getCostmap(), ilayer, inflation_radius);
}
/**
* Test inflation for both static and dynamic obstacles
*/
TEST(costmap, testInflation){
tf2_ros::Buffer tf;
LayeredCostmap layers("frame", false, false);
// Footprint with inscribed radius = 2.1
// circumscribed radius = 3.1
std::vector<Point> polygon = setRadii(layers, 1, 1, 1);
addStaticLayer(layers, tf);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
InflationLayer* ilayer = addInflationLayer(layers, tf);
layers.setFootprint(polygon);
Costmap2D* costmap = layers.getCostmap();
layers.updateMap(0,0,0);
//printMap(*costmap);
ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE), (unsigned int)20);
ASSERT_EQ(countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)28);
/*/ Iterate over all id's and verify they are obstacles
for(std::vector<unsigned int>::const_iterator it = occupiedCells.begin(); it != occupiedCells.end(); ++it){
unsigned int ind = *it;
unsigned int x, y;
map.indexToCells(ind, x, y);
ASSERT_EQ(find(occupiedCells, map.getIndex(x, y)), true);
ASSERT_EQ(map.getCost(x, y) == costmap_2d::LETHAL_OBSTACLE || map.getCost(x, y) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
}*/
addObservation(olayer, 0, 0, 0.4);
layers.updateMap(0,0,0);
// It and its 2 neighbors makes 3 obstacles
ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE) + countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)51);
// @todo Rewrite
// Add an obstacle at <2,0> which will inflate and refresh to of the other inflated cells
addObservation(olayer, 2, 0);
layers.updateMap(0,0,0);
// Now we expect insertions for it, and 2 more neighbors, but not all 5. Free space will propagate from
// the origin to the target, clearing the point at <0, 0>, but not over-writing the inflation of the obstacle
// at <0, 1>
ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE) + countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)54);
// Add an obstacle at <1, 9>. This will inflate obstacles around it
addObservation(olayer, 1, 9);
layers.updateMap(0,0,0);
ASSERT_EQ(costmap->getCost(1, 9), LETHAL_OBSTACLE);
ASSERT_EQ(costmap->getCost(0, 9), INSCRIBED_INFLATED_OBSTACLE);
ASSERT_EQ(costmap->getCost(2, 9), INSCRIBED_INFLATED_OBSTACLE);
// Add an obstacle and verify that it over-writes its inflated status
addObservation(olayer, 0, 9);
layers.updateMap(0,0,0);
ASSERT_EQ(costmap->getCost(0, 9), LETHAL_OBSTACLE);
}
/**
* Test specific inflation scenario to ensure we do not set inflated obstacles to be raw obstacles.
*/
TEST(costmap, testInflation2){
tf2_ros::Buffer tf;
LayeredCostmap layers("frame", false, false);
// Footprint with inscribed radius = 2.1
// circumscribed radius = 3.1
std::vector<Point> polygon = setRadii(layers, 1, 1, 1);
addStaticLayer(layers, tf);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
InflationLayer* ilayer = addInflationLayer(layers, tf);
layers.setFootprint(polygon);
// Creat a small L-Shape all at once
addObservation(olayer, 1, 1, MAX_Z);
addObservation(olayer, 2, 1, MAX_Z);
addObservation(olayer, 2, 2, MAX_Z);
layers.updateMap(0,0,0);
Costmap2D* costmap = layers.getCostmap();
//printMap(*costmap);
ASSERT_EQ(costmap->getCost(2, 3), costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
ASSERT_EQ(costmap->getCost(3, 3), costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
}
/**
* Test inflation behavior, starting with an empty map
*/
TEST(costmap, testInflation3){
tf2_ros::Buffer tf;
LayeredCostmap layers("frame", false, false);
layers.resizeMap(10, 10, 1, 0, 0);
// 1 2 3
std::vector<Point> polygon = setRadii(layers, 1, 1.75, 3);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
InflationLayer* ilayer = addInflationLayer(layers, tf);
layers.setFootprint(polygon);
// There should be no occupied cells
Costmap2D* costmap = layers.getCostmap();
ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE), (unsigned int)0);
ASSERT_EQ(countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)0);
printMap(*costmap);
// Add an obstacle at 5,5
addObservation(olayer, 5, 5, MAX_Z);
layers.updateMap(0,0,0);
printMap(*costmap);
// Test fails because updated cell value is 0
ASSERT_EQ(countValues(*costmap, FREE_SPACE, false), (unsigned int)29);
ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE), (unsigned int)1);
ASSERT_EQ(countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)4);
// Update again - should see no change
layers.updateMap(0,0,0);
ASSERT_EQ(countValues(*costmap, FREE_SPACE, false), (unsigned int)29);
ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE), (unsigned int)1);
ASSERT_EQ(countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)4);
}
int main(int argc, char** argv){
ros::init(argc, argv, "inflation_tests");
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

6
navigations/costmap_2d/test/inflation_tests.launch Executable file
View File

@@ -0,0 +1,6 @@
<launch>
<node name="ms" pkg="map_server" type="map_server" args="$(find costmap_2d)/test/TenByTen.yaml"/>
<test time-limit="300" test-name="inflation_tests" pkg="costmap_2d" type="inflation_tests">
<param name="inflation/cost_scaling_factor" value="1" />
</test>
</launch>

1176
navigations/costmap_2d/test/module_tests.cpp Executable file
View File

File diff suppressed because it is too large Load Diff

251
navigations/costmap_2d/test/obstacle_tests.cpp Executable file
View File

@@ -0,0 +1,251 @@
/*
* 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.
*/
/**
* @author David Lu!!
* Test harness for ObstacleLayer for Costmap2D
*/
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/observation_buffer.h>
#include <costmap_2d/testing_helper.h>
#include <set>
#include <gtest/gtest.h>
using namespace costmap_2d;
/*
* For reference, the static map looks like this:
*
* 0 0 0 0 0 0 0 254 254 254
*
* 0 0 0 0 0 0 0 254 254 254
*
* 0 0 0 254 254 254 0 0 0 0
*
* 0 0 0 0 0 0 0 0 0 0
*
* 0 0 0 0 0 0 0 0 0 0
*
* 0 0 0 0 254 0 0 254 254 254
*
* 0 0 0 0 254 0 0 254 254 254
*
* 0 0 0 0 0 0 0 254 254 254
*
* 0 0 0 0 0 0 0 0 0 0
*
* 0 0 0 0 0 0 0 0 0 0
*
* upper left is 0,0, lower right is 9,9
*/
/**
* Test for ray tracing free space
*/
TEST(costmap, testRaytracing){
tf2_ros::Buffer tf;
LayeredCostmap layers("frame", false, false); // Not rolling window, not tracking unknown
addStaticLayer(layers, tf); // This adds the static map
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
// Add a point at 0, 0, 0
addObservation(olayer, 0.0, 0.0, MAX_Z/2, 0, 0, MAX_Z/2);
// This actually puts the LETHAL (254) point in the costmap at (0,0)
layers.updateMap(0,0,0); // 0, 0, 0 is robot pose
//printMap(*(layers.getCostmap()));
int lethal_count = countValues(*(layers.getCostmap()), LETHAL_OBSTACLE);
// We expect just one obstacle to be added (20 in static map)
ASSERT_EQ(lethal_count, 21);
}
/**
* Test for ray tracing free space
*/
TEST(costmap, testRaytracing2){
tf2_ros::Buffer tf;
LayeredCostmap layers("frame", false, false);
addStaticLayer(layers, tf);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
//If we print map now, it is 10x10 all value 0
//printMap(*(layers.getCostmap()));
// Update will fill in the costmap with the static map
layers.updateMap(0,0,0);
//If we print the map now, we get the static map
//printMap(*(layers.getCostmap()));
// Static map has 20 LETHAL cells (see diagram above)
int obs_before = countValues(*(layers.getCostmap()), LETHAL_OBSTACLE);
ASSERT_EQ(obs_before, 20);
// The sensor origin will be <0,0>. So if we add an obstacle at 9,9,
// we would expect cells <0, 0> thru <8, 8> to be traced through
// however the static map is not cleared by obstacle layer
addObservation(olayer, 9.5, 9.5, MAX_Z/2, 0.5, 0.5, MAX_Z/2);
layers.updateMap(0, 0, 0);
// If we print map now, we have static map + <9,9> is LETHAL
//printMap(*(layers.getCostmap()));
int obs_after = countValues(*(layers.getCostmap()), LETHAL_OBSTACLE);
// Change from previous test:
// No obstacles from the static map will be cleared, so the
// net change is +1.
ASSERT_EQ(obs_after, obs_before + 1);
// Fill in the diagonal, <7,7> and <9,9> already filled in, <0,0> is robot
for(int i = 0; i < olayer->getSizeInCellsY(); ++i)
{
olayer->setCost(i, i, LETHAL_OBSTACLE);
}
// This will updateBounds, which will raytrace the static observation added
// above, thus clearing out the diagonal again!
layers.updateMap(0, 0, 0);
// Map now has diagonal except <0,0> filled with LETHAL (254)
//printMap(*(layers.getCostmap()));
int with_static = countValues(*(layers.getCostmap()), LETHAL_OBSTACLE);
// Should thus be the same
ASSERT_EQ(with_static, obs_after);
// If 21 are filled, 79 should be free
ASSERT_EQ(79, countValues(*(layers.getCostmap()), costmap_2d::FREE_SPACE));
}
/**
* Test for wave interference
*/
TEST(costmap, testWaveInterference){
tf2_ros::Buffer tf;
// Start with an empty map, no rolling window, tracking unknown
LayeredCostmap layers("frame", false, true);
layers.resizeMap(10, 10, 1, 0, 0);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
// If we print map now, it is 10x10, all cells are 255 (NO_INFORMATION)
//printMap(*(layers.getCostmap()));
// Lay out 3 obstacles in a line - along the diagonal, separated by a cell.
addObservation(olayer, 3.0, 3.0, MAX_Z);
addObservation(olayer, 5.0, 5.0, MAX_Z);
addObservation(olayer, 7.0, 7.0, MAX_Z);
layers.updateMap(0,0,0);
Costmap2D* costmap = layers.getCostmap();
// 3 obstacle cells are filled, <1,1>,<2,2>,<4,4> and <6,6> are now free
// <0,0> is footprint and is free
//printMap(*costmap);
ASSERT_EQ(3, countValues(*costmap, costmap_2d::LETHAL_OBSTACLE));
ASSERT_EQ(92, countValues(*costmap, costmap_2d::NO_INFORMATION));
ASSERT_EQ(5, countValues(*costmap, costmap_2d::FREE_SPACE));
}
/**
* Make sure we ignore points outside of our z threshold
*/
TEST(costmap, testZThreshold){
tf2_ros::Buffer tf;
// Start with an empty map
LayeredCostmap layers("frame", false, true);
layers.resizeMap(10, 10, 1, 0, 0);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
// A point cloud with 2 points falling in a cell with a non-lethal cost
addObservation(olayer, 0.0, 5.0, 0.4);
addObservation(olayer, 1.0, 5.0, 2.2);
layers.updateMap(0,0,0);
Costmap2D* costmap = layers.getCostmap();
ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 1);
}
/**
* Verify that dynamic obstacles are added
*/
TEST(costmap, testDynamicObstacles){
tf2_ros::Buffer tf;
LayeredCostmap layers("frame", false, false);
addStaticLayer(layers, tf);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
// Add a point cloud and verify its insertion. There should be only one new one
addObservation(olayer, 0.0, 0.0);
addObservation(olayer, 0.0, 0.0);
addObservation(olayer, 0.0, 0.0);
layers.updateMap(0,0,0);
Costmap2D* costmap = layers.getCostmap();
// Should now have 1 insertion and no deletions
ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 21);
// Repeating the call - we should see no insertions or deletions
ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 21);
}
/**
* Verify that if we add a point that is already a static obstacle we do not end up with a new ostacle
*/
TEST(costmap, testMultipleAdditions){
tf2_ros::Buffer tf;
LayeredCostmap layers("frame", false, false);
addStaticLayer(layers, tf);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
// A point cloud with one point that falls within an existing obstacle
addObservation(olayer, 9.5, 0.0);
layers.updateMap(0,0,0);
Costmap2D* costmap = layers.getCostmap();
//printMap(*costmap);
ASSERT_EQ(countValues(*costmap, costmap_2d::LETHAL_OBSTACLE), 20);
}
int main(int argc, char** argv){
ros::init(argc, argv, "obstacle_tests");
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

5
navigations/costmap_2d/test/obstacle_tests.launch Executable file
View File

@@ -0,0 +1,5 @@
<launch>
<node name="ms" pkg="map_server" type="map_server" args="$(find costmap_2d)/test/TenByTen.yaml"/>
<test time-limit="300" test-name="obstacle_tests" pkg="costmap_2d" type="obstacle_tests" />
</launch>

14
navigations/costmap_2d/test/simple_driving_test.xml Executable file
View File

@@ -0,0 +1,14 @@
<launch>
<param name="/use_sim_time" value="true"/>
<node name="rosplay" pkg="rosbag" type="play"
args="-s 5 -r 1 --clock --hz=10 $(find costmap_2d)/test/simple_driving_test_indexed.bag" />
<node name="map_server" pkg="map_server" type="map_server" args="$(find costmap_2d)/test/willow-full-0.025.pgm 0.025" />
<rosparam file="$(find costmap_2d)/test/costmap_params.yaml" command="load" ns="simple_driving_test/test_costmap" />
<test time-limit="600" test-name="simple_driving_test" pkg="costmap_2d" type="costmap_tester">
<param name="wait_time" value="40.0" />
</test>
</launch>

330
navigations/costmap_2d/test/static_tests.cpp Executable file
View File

@@ -0,0 +1,330 @@
/*
* 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.
*/
/**
* @author David Lu!!
* Test harness for StaticMap Layer for Costmap2D
*/
#include <costmap_2d/costmap_2d.h>
#include <costmap_2d/layered_costmap.h>
#include <costmap_2d/obstacle_layer.h>
#include <costmap_2d/static_layer.h>
#include <costmap_2d/observation_buffer.h>
#include <costmap_2d/testing_helper.h>
#include <set>
#include <gtest/gtest.h>
using namespace costmap_2d;
/**
* Tests the reset method
*
TEST(costmap, testResetForStaticMap){
// Define a static map with a large object in the center
std::vector<unsigned char> staticMap;
for(unsigned int i=0; i<10; i++){
for(unsigned int j=0; j<10; j++){
staticMap.push_back(costmap_2d::LETHAL_OBSTACLE);
}
}
// Allocate the cost map, with a inflation to 3 cells all around
Costmap2D map(10, 10, RESOLUTION, 0.0, 0.0, 3, 3, 3, OBSTACLE_RANGE, MAX_Z, RAYTRACE_RANGE, 25, staticMap, THRESHOLD);
// Populate the cost map with a wall around the perimeter. Free space should clear out the room.
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.points.resize(40);
// Left wall
unsigned int ind = 0;
for (unsigned int i = 0; i < 10; i++){
// Left
cloud.points[ind].x = 0;
cloud.points[ind].y = i;
cloud.points[ind].z = MAX_Z;
ind++;
// Top
cloud.points[ind].x = i;
cloud.points[ind].y = 0;
cloud.points[ind].z = MAX_Z;
ind++;
// Right
cloud.points[ind].x = 9;
cloud.points[ind].y = i;
cloud.points[ind].z = MAX_Z;
ind++;
// Bottom
cloud.points[ind].x = i;
cloud.points[ind].y = 9;
cloud.points[ind].z = MAX_Z;
ind++;
}
double wx = 5.0, wy = 5.0;
geometry_msgs::Point p;
p.x = wx;
p.y = wy;
p.z = MAX_Z;
Observation obs(p, cloud, OBSTACLE_RANGE, RAYTRACE_RANGE);
std::vector<Observation> obsBuf;
obsBuf.push_back(obs);
// Update the cost map for this observation
map.updateWorld(wx, wy, obsBuf, obsBuf);
// Verify that we now have only 36 cells with lethal cost, thus provong that we have correctly cleared
// free space
int hitCount = 0;
for(unsigned int i=0; i < 10; ++i){
for(unsigned int j=0; j < 10; ++j){
if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){
hitCount++;
}
}
}
ASSERT_EQ(hitCount, 36);
// Veriy that we have 64 non-leathal
hitCount = 0;
for(unsigned int i=0; i < 10; ++i){
for(unsigned int j=0; j < 10; ++j){
if(map.getCost(i, j) != costmap_2d::LETHAL_OBSTACLE)
hitCount++;
}
}
ASSERT_EQ(hitCount, 64);
// Now if we reset the cost map, we should have our map go back to being completely occupied
map.resetMapOutsideWindow(wx, wy, 0.0, 0.0);
//We should now go back to everything being occupied
hitCount = 0;
for(unsigned int i=0; i < 10; ++i){
for(unsigned int j=0; j < 10; ++j){
if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE)
hitCount++;
}
}
ASSERT_EQ(hitCount, 100);
}
/** Test for copying a window of a costmap *
TEST(costmap, testWindowCopy){
Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
/*
for(unsigned int i = 0; i < 10; ++i){
for(unsigned int j = 0; j < 10; ++j){
printf("%3d ", map.getCost(i, j));
}
printf("\n");
}
printf("\n");
*
Costmap2D windowCopy;
//first test that if we try to make a window that is too big, things fail
windowCopy.copyCostmapWindow(map, 2.0, 2.0, 6.0, 12.0);
ASSERT_EQ(windowCopy.getSizeInCellsX(), (unsigned int)0);
ASSERT_EQ(windowCopy.getSizeInCellsY(), (unsigned int)0);
//Next, test that trying to make a map window itself fails
map.copyCostmapWindow(map, 2.0, 2.0, 6.0, 6.0);
ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)10);
ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)10);
//Next, test that legal input generates the result that we expect
windowCopy.copyCostmapWindow(map, 2.0, 2.0, 6.0, 6.0);
ASSERT_EQ(windowCopy.getSizeInCellsX(), (unsigned int)6);
ASSERT_EQ(windowCopy.getSizeInCellsY(), (unsigned int)6);
//check that we actually get the windo that we expect
for(unsigned int i = 0; i < windowCopy.getSizeInCellsX(); ++i){
for(unsigned int j = 0; j < windowCopy.getSizeInCellsY(); ++j){
ASSERT_EQ(windowCopy.getCost(i, j), map.getCost(i + 2, j + 2));
//printf("%3d ", windowCopy.getCost(i, j));
}
//printf("\n");
}
}
//test for updating costmaps with static data
TEST(costmap, testFullyContainedStaticMapUpdate){
Costmap2D map(5, 5, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
10.0, MAX_Z, 10.0, 25, MAP_5_BY_5, THRESHOLD);
Costmap2D static_map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
map.updateStaticMapWindow(0, 0, 10, 10, MAP_10_BY_10);
for(unsigned int i = 0; i < map.getSizeInCellsX(); ++i){
for(unsigned int j = 0; j < map.getSizeInCellsY(); ++j){
ASSERT_EQ(map.getCost(i, j), static_map.getCost(i, j));
}
}
}
TEST(costmap, testOverlapStaticMapUpdate){
Costmap2D map(5, 5, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
10.0, MAX_Z, 10.0, 25, MAP_5_BY_5, THRESHOLD);
Costmap2D static_map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
map.updateStaticMapWindow(-10, -10, 10, 10, MAP_10_BY_10);
ASSERT_FLOAT_EQ(map.getOriginX(), -10);
ASSERT_FLOAT_EQ(map.getOriginX(), -10);
ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)15);
ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)15);
for(unsigned int i = 0; i < 10; ++i){
for(unsigned int j = 0; j < 10; ++j){
ASSERT_EQ(map.getCost(i, j), static_map.getCost(i, j));
}
}
std::vector<unsigned char> blank(100);
//check to make sure that inflation on updates are being done correctly
map.updateStaticMapWindow(-10, -10, 10, 10, blank);
for(unsigned int i = 0; i < map.getSizeInCellsX(); ++i){
for(unsigned int j = 0; j < map.getSizeInCellsY(); ++j){
ASSERT_EQ(map.getCost(i, j), 0);
}
}
std::vector<unsigned char> fully_contained(25);
fully_contained[0] = 254;
fully_contained[4] = 254;
fully_contained[5] = 254;
fully_contained[9] = 254;
Costmap2D small_static_map(5, 5, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
10.0, MAX_Z, 10.0, 25, fully_contained, THRESHOLD);
map.updateStaticMapWindow(0, 0, 5, 5, fully_contained);
ASSERT_FLOAT_EQ(map.getOriginX(), -10);
ASSERT_FLOAT_EQ(map.getOriginX(), -10);
ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)15);
ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)15);
for(unsigned int j = 0; j < 5; ++j){
for(unsigned int i = 0; i < 5; ++i){
ASSERT_EQ(map.getCost(i + 10, j + 10), small_static_map.getCost(i, j));
}
}
}
TEST(costmap, testStaticMap){
Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)10);
ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)10);
// Verify that obstacles correctly identified from the static map.
std::vector<unsigned int> occupiedCells;
for(unsigned int i = 0; i < 10; ++i){
for(unsigned int j = 0; j < 10; ++j){
if(map.getCost(i, j) == costmap_2d::LETHAL_OBSTACLE){
occupiedCells.push_back(map.getIndex(i, j));
}
}
}
ASSERT_EQ(occupiedCells.size(), (unsigned int)20);
// Iterate over all id's and verify that they are present according to their
for(std::vector<unsigned int>::const_iterator it = occupiedCells.begin(); it != occupiedCells.end(); ++it){
unsigned int ind = *it;
unsigned int x, y;
map.indexToCells(ind, x, y);
ASSERT_EQ(find(occupiedCells, map.getIndex(x, y)), true);
ASSERT_EQ(MAP_10_BY_10[ind] >= 100, true);
ASSERT_EQ(map.getCost(x, y) >= 100, true);
}
// Block of 200
ASSERT_EQ(find(occupiedCells, map.getIndex(7, 2)), true);
ASSERT_EQ(find(occupiedCells, map.getIndex(8, 2)), true);
ASSERT_EQ(find(occupiedCells, map.getIndex(9, 2)), true);
ASSERT_EQ(find(occupiedCells, map.getIndex(7, 3)), true);
ASSERT_EQ(find(occupiedCells, map.getIndex(8, 3)), true);
ASSERT_EQ(find(occupiedCells, map.getIndex(9, 3)), true);
ASSERT_EQ(find(occupiedCells, map.getIndex(7, 4)), true);
ASSERT_EQ(find(occupiedCells, map.getIndex(8, 4)), true);
ASSERT_EQ(find(occupiedCells, map.getIndex(9, 4)), true);
// Block of 100
ASSERT_EQ(find(occupiedCells, map.getIndex(4, 3)), true);
ASSERT_EQ(find(occupiedCells, map.getIndex(4, 4)), true);
// Block of 200
ASSERT_EQ(find(occupiedCells, map.getIndex(3, 7)), true);
ASSERT_EQ(find(occupiedCells, map.getIndex(4, 7)), true);
ASSERT_EQ(find(occupiedCells, map.getIndex(5, 7)), true);
// Verify Coordinate Transformations, ROW MAJOR ORDER
ASSERT_EQ(worldToIndex(map, 0.0, 0.0), (unsigned int)0);
ASSERT_EQ(worldToIndex(map, 0.0, 0.99), (unsigned int)0);
ASSERT_EQ(worldToIndex(map, 0.0, 1.0), (unsigned int)10);
ASSERT_EQ(worldToIndex(map, 1.0, 0.99), (unsigned int)1);
ASSERT_EQ(worldToIndex(map, 9.99, 9.99), (unsigned int)99);
ASSERT_EQ(worldToIndex(map, 8.2, 3.4), (unsigned int)38);
// Ensure we hit the middle of the cell for world co-ordinates
double wx, wy;
indexToWorld(map, 99, wx, wy);
ASSERT_EQ(wx, 9.5);
ASSERT_EQ(wy, 9.5);
}
//*/
int main(int argc, char** argv){
ros::init(argc, argv, "obstacle_tests");
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

5
navigations/costmap_2d/test/static_tests.launch Executable file
View File

@@ -0,0 +1,5 @@
<launch>
<node name="ms" pkg="map_server" type="map_server" args="$(find costmap_2d)/test/TenByTen.yaml"/>
<test time-limit="300" test-name="static_tests" pkg="costmap_2d" type="static_tests" />
</launch>