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

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