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>
40 
41 using namespace costmap_2d;
42 
44 
45 TEST( Costmap2DROS, unpadded_footprint_from_string_param )
46 {
47  Costmap2DROS cm( "unpadded/string", *tf_ );
48  std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
49  EXPECT_EQ( 3, footprint.size() );
50 
51  EXPECT_EQ( 1.0f, footprint[ 0 ].x );
52  EXPECT_EQ( 1.0f, footprint[ 0 ].y );
53  EXPECT_EQ( 0.0f, footprint[ 0 ].z );
54 
55  EXPECT_EQ( -1.0f, footprint[ 1 ].x );
56  EXPECT_EQ( 1.0f, footprint[ 1 ].y );
57  EXPECT_EQ( 0.0f, footprint[ 1 ].z );
58 
59  EXPECT_EQ( -1.0f, footprint[ 2 ].x );
60  EXPECT_EQ( -1.0f, footprint[ 2 ].y );
61  EXPECT_EQ( 0.0f, footprint[ 2 ].z );
62 }
63 
64 TEST( Costmap2DROS, padded_footprint_from_string_param )
65 {
66  Costmap2DROS cm( "padded/string", *tf_ );
67  std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
68  EXPECT_EQ( 3, footprint.size() );
69 
70  EXPECT_EQ( 1.5f, footprint[ 0 ].x );
71  EXPECT_EQ( 1.5f, footprint[ 0 ].y );
72  EXPECT_EQ( 0.0f, footprint[ 0 ].z );
73 
74  EXPECT_EQ( -1.5f, footprint[ 1 ].x );
75  EXPECT_EQ( 1.5f, footprint[ 1 ].y );
76  EXPECT_EQ( 0.0f, footprint[ 1 ].z );
77 
78  EXPECT_EQ( -1.5f, footprint[ 2 ].x );
79  EXPECT_EQ( -1.5f, footprint[ 2 ].y );
80  EXPECT_EQ( 0.0f, footprint[ 2 ].z );
81 }
82 
83 TEST( Costmap2DROS, radius_param )
84 {
85  Costmap2DROS cm( "radius/sub", *tf_ );
86  std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
87  // Circular robot has 16-point footprint auto-generated.
88  EXPECT_EQ( 16, footprint.size() );
89 
90  // Check the first point
91  EXPECT_EQ( 10.0f, footprint[ 0 ].x );
92  EXPECT_EQ( 0.0f, footprint[ 0 ].y );
93  EXPECT_EQ( 0.0f, footprint[ 0 ].z );
94 
95  // Check the 4th point, which should be 90 degrees around the circle from the first.
96  EXPECT_NEAR( 0.0f, footprint[ 4 ].x, 0.0001 );
97  EXPECT_NEAR( 10.0f, footprint[ 4 ].y, 0.0001 );
98  EXPECT_EQ( 0.0f, footprint[ 4 ].z );
99 }
100 
101 TEST( Costmap2DROS, footprint_from_xmlrpc_param )
102 {
103  Costmap2DROS cm( "xmlrpc", *tf_ );
104  std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
105  EXPECT_EQ( 4, footprint.size() );
106 
107  EXPECT_EQ( 0.1f, footprint[ 0 ].x );
108  EXPECT_EQ( 0.1f, footprint[ 0 ].y );
109  EXPECT_EQ( 0.0f, footprint[ 0 ].z );
110 
111  EXPECT_EQ( -0.1f, footprint[ 1 ].x );
112  EXPECT_EQ( 0.1f, footprint[ 1 ].y );
113  EXPECT_EQ( 0.0f, footprint[ 1 ].z );
114 
115  EXPECT_EQ( -0.1f, footprint[ 2 ].x );
116  EXPECT_EQ( -0.1f, footprint[ 2 ].y );
117  EXPECT_EQ( 0.0f, footprint[ 2 ].z );
118 
119  EXPECT_EQ( 0.1f, footprint[ 3 ].x );
120  EXPECT_EQ( -0.1f, footprint[ 3 ].y );
121  EXPECT_EQ( 0.0f, footprint[ 3 ].z );
122 }
123 
124 TEST( Costmap2DROS, footprint_from_same_level_param )
125 {
126  Costmap2DROS cm( "same_level", *tf_ );
127  std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
128  EXPECT_EQ( 3, footprint.size() );
129 
130  EXPECT_EQ( 1.0f, footprint[ 0 ].x );
131  EXPECT_EQ( 2.0f, footprint[ 0 ].y );
132  EXPECT_EQ( 0.0f, footprint[ 0 ].z );
133 
134  EXPECT_EQ( 3.0f, footprint[ 1 ].x );
135  EXPECT_EQ( 4.0f, footprint[ 1 ].y );
136  EXPECT_EQ( 0.0f, footprint[ 1 ].z );
137 
138  EXPECT_EQ( 5.0f, footprint[ 2 ].x );
139  EXPECT_EQ( 6.0f, footprint[ 2 ].y );
140  EXPECT_EQ( 0.0f, footprint[ 2 ].z );
141 }
142 
143 TEST( Costmap2DROS, footprint_from_xmlrpc_param_failure )
144 {
145  ASSERT_ANY_THROW( Costmap2DROS cm( "xmlrpc_fail", *tf_ ));
146 }
147 
148 TEST( Costmap2DROS, footprint_empty )
149 {
150  Costmap2DROS cm( "empty", *tf_ );
151  std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
152  // With no specification of footprint or radius, defaults to 0.46 meter radius plus 0.01 meter padding.
153  EXPECT_EQ( 16, footprint.size() );
154 
155  EXPECT_NEAR( 0.47f, footprint[ 0 ].x, 0.0001 );
156  EXPECT_NEAR( 0.0f, footprint[ 0 ].y, 0.0001 );
157  EXPECT_EQ( 0.0f, footprint[ 0 ].z );
158 }
159 
160 int main(int argc, char** argv)
161 {
162  ros::init(argc, argv, "footprint_tests_node");
163 
164  tf_ = new tf::TransformListener( ros::Duration( 10 ));
165 
166  // This empty transform is added to satisfy the constructor of
167  // Costmap2DROS, which waits for the transform from map to base_link
168  // to become available.
169  tf::StampedTransform base_rel_map;
170  base_rel_map.child_frame_id_ = "/base_link";
171  base_rel_map.frame_id_ = "/map";
172  base_rel_map.stamp_ = ros::Time::now();
173  tf_->setTransform( base_rel_map );
174 
175  testing::InitGoogleTest(&argc, argv);
176  return RUN_ALL_TESTS();
177 }
f
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool setTransform(const StampedTransform &transform, const std::string &authority="default_authority")
tf::TransformListener * tf_
TFSIMD_FORCE_INLINE const tfScalar & y() const
TEST(Costmap2DROS, unpadded_footprint_from_string_param)
std::string child_frame_id_
int main(int argc, char **argv)
TFSIMD_FORCE_INLINE const tfScalar & x() const
TFSIMD_FORCE_INLINE const tfScalar & z() const
static Time now()
A ROS wrapper for a 2D Costmap. Handles subscribing to topics that provide observations about obstacl...
std::vector< geometry_msgs::Point > getRobotFootprint()
Return the current footprint of the robot as a vector of points.
std::string frame_id_


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:42