costmap_2d
footprint_tests.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 *
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2009, Willow Garage, Inc.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Willow Garage, Inc. nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 * Author: Dave Hershberger
36 *********************************************************************/
37 #include <gtest/gtest.h>
38 #include <ros/ros.h>
39 #include <costmap_2d/costmap_2d_ros.h>
40 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
41 #include <tf2_ros/transform_listener.h>
42 
43 using namespace costmap_2d;
44 
45 tf2_ros::TransformListener* tfl_;
46 tf2_ros::Buffer* tf_;
47 
48 TEST( Costmap2DROS, unpadded_footprint_from_string_param )
49 {
50  Costmap2DROS cm( "unpadded/string", *tf_ );
51  std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
52  EXPECT_EQ( 3, footprint.size() );
53 
54  EXPECT_EQ( 1.0f, footprint[ 0 ].x );
55  EXPECT_EQ( 1.0f, footprint[ 0 ].y );
56  EXPECT_EQ( 0.0f, footprint[ 0 ].z );
57 
58  EXPECT_EQ( -1.0f, footprint[ 1 ].x );
59  EXPECT_EQ( 1.0f, footprint[ 1 ].y );
60  EXPECT_EQ( 0.0f, footprint[ 1 ].z );
61 
62  EXPECT_EQ( -1.0f, footprint[ 2 ].x );
63  EXPECT_EQ( -1.0f, footprint[ 2 ].y );
64  EXPECT_EQ( 0.0f, footprint[ 2 ].z );
65 }
66 
67 TEST( Costmap2DROS, padded_footprint_from_string_param )
68 {
69  Costmap2DROS cm( "padded/string", *tf_ );
70  std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
71  EXPECT_EQ( 3, footprint.size() );
72 
73  EXPECT_EQ( 1.5f, footprint[ 0 ].x );
74  EXPECT_EQ( 1.5f, footprint[ 0 ].y );
75  EXPECT_EQ( 0.0f, footprint[ 0 ].z );
76 
77  EXPECT_EQ( -1.5f, footprint[ 1 ].x );
78  EXPECT_EQ( 1.5f, footprint[ 1 ].y );
79  EXPECT_EQ( 0.0f, footprint[ 1 ].z );
80 
81  EXPECT_EQ( -1.5f, footprint[ 2 ].x );
82  EXPECT_EQ( -1.5f, footprint[ 2 ].y );
83  EXPECT_EQ( 0.0f, footprint[ 2 ].z );
84 }
85 
86 TEST( Costmap2DROS, radius_param )
87 {
88  Costmap2DROS cm( "radius/sub", *tf_ );
89  std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
90  // Circular robot has 16-point footprint auto-generated.
91  EXPECT_EQ( 16, footprint.size() );
92 
93  // Check the first point
94  EXPECT_EQ( 10.0f, footprint[ 0 ].x );
95  EXPECT_EQ( 0.0f, footprint[ 0 ].y );
96  EXPECT_EQ( 0.0f, footprint[ 0 ].z );
97 
98  // Check the 4th point, which should be 90 degrees around the circle from the first.
99  EXPECT_NEAR( 0.0f, footprint[ 4 ].x, 0.0001 );
100  EXPECT_NEAR( 10.0f, footprint[ 4 ].y, 0.0001 );
101  EXPECT_EQ( 0.0f, footprint[ 4 ].z );
102 }
103 
104 TEST( Costmap2DROS, footprint_from_xmlrpc_param )
105 {
106  Costmap2DROS cm( "xmlrpc", *tf_ );
107  std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
108  EXPECT_EQ( 4, footprint.size() );
109 
110  EXPECT_EQ( 0.1f, footprint[ 0 ].x );
111  EXPECT_EQ( 0.1f, footprint[ 0 ].y );
112  EXPECT_EQ( 0.0f, footprint[ 0 ].z );
113 
114  EXPECT_EQ( -0.1f, footprint[ 1 ].x );
115  EXPECT_EQ( 0.1f, footprint[ 1 ].y );
116  EXPECT_EQ( 0.0f, footprint[ 1 ].z );
117 
118  EXPECT_EQ( -0.1f, footprint[ 2 ].x );
119  EXPECT_EQ( -0.1f, footprint[ 2 ].y );
120  EXPECT_EQ( 0.0f, footprint[ 2 ].z );
121 
122  EXPECT_EQ( 0.1f, footprint[ 3 ].x );
123  EXPECT_EQ( -0.1f, footprint[ 3 ].y );
124  EXPECT_EQ( 0.0f, footprint[ 3 ].z );
125 }
126 
127 TEST( Costmap2DROS, footprint_from_same_level_param )
128 {
129  Costmap2DROS cm( "same_level", *tf_ );
130  std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
131  EXPECT_EQ( 3, footprint.size() );
132 
133  EXPECT_EQ( 1.0f, footprint[ 0 ].x );
134  EXPECT_EQ( 2.0f, footprint[ 0 ].y );
135  EXPECT_EQ( 0.0f, footprint[ 0 ].z );
136 
137  EXPECT_EQ( 3.0f, footprint[ 1 ].x );
138  EXPECT_EQ( 4.0f, footprint[ 1 ].y );
139  EXPECT_EQ( 0.0f, footprint[ 1 ].z );
140 
141  EXPECT_EQ( 5.0f, footprint[ 2 ].x );
142  EXPECT_EQ( 6.0f, footprint[ 2 ].y );
143  EXPECT_EQ( 0.0f, footprint[ 2 ].z );
144 }
145 
146 TEST( Costmap2DROS, footprint_from_xmlrpc_param_failure )
147 {
148  ASSERT_ANY_THROW( Costmap2DROS cm( "xmlrpc_fail", *tf_ ));
149 }
150 
151 TEST( Costmap2DROS, footprint_empty )
152 {
153  Costmap2DROS cm( "empty", *tf_ );
154  std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
155  // With no specification of footprint or radius, defaults to 0.46 meter radius plus 0.01 meter padding.
156  EXPECT_EQ( 16, footprint.size() );
157 
158  EXPECT_NEAR( 0.47f, footprint[ 0 ].x, 0.0001 );
159  EXPECT_NEAR( 0.0f, footprint[ 0 ].y, 0.0001 );
160  EXPECT_EQ( 0.0f, footprint[ 0 ].z );
161 }
162 
163 int main(int argc, char** argv)
164 {
165  ros::init(argc, argv, "footprint_tests_node");
166 
167  tf_ = new tf2_ros::Buffer( ros::Duration( 10 ));
168  tfl_ = new tf2_ros::TransformListener(*tf_);
169 
170  // This empty transform is added to satisfy the constructor of
171  // Costmap2DROS, which waits for the transform from map to base_link
172  // to become available.
173  geometry_msgs::TransformStamped base_rel_map;
174  base_rel_map.transform = tf2::toMsg(tf2::Transform::getIdentity());
175  base_rel_map.child_frame_id = "base_link";
176  base_rel_map.header.frame_id = "map";
177  base_rel_map.header.stamp = ros::Time::now();
178  tf_->setTransform( base_rel_map, "footprint_tests" );
179 
180  testing::InitGoogleTest(&argc, argv);
181  return RUN_ALL_TESTS();
182 }
std::vector< geometry_msgs::Point >
std::vector::size
T size(T... args)
main
int main(int argc, char **argv)
Definition: footprint_tests.cpp:163
costmap_2d::Costmap2DROS::getRobotFootprint
const std::vector< geometry_msgs::Point > & getRobotFootprint() const noexcept
Return the current footprint of the robot as a vector of points.
Definition: costmap_2d_ros.h:191
tfl_
tf2_ros::TransformListener * tfl_
Definition: footprint_tests.cpp:45
TEST
TEST(Costmap2DROS, unpadded_footprint_from_string_param)
Definition: footprint_tests.cpp:48
tf_
tf2_ros::Buffer * tf_
Definition: footprint_tests.cpp:46
costmap_2d
Definition: array_parser.h:37
costmap_2d::Costmap2DROS
A ROS wrapper for a 2D Costmap. Handles subscribing to topics that provide observations about obstacl...
Definition: costmap_2d_ros.h:74
costmap_2d_ros.h
Generated on Sun Feb 23 2025 04:33:26 for costmap_2d by   doxygen 1.8.17