00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <gtest/gtest.h>
00038 #include <ros/ros.h>
00039 #include <costmap_2d/costmap_2d_ros.h>
00040
00041 using namespace costmap_2d;
00042
00043 tf::TransformListener* tf_;
00044
00045 TEST( Costmap2DROS, unpadded_footprint_from_string_param )
00046 {
00047 Costmap2DROS cm( "unpadded/string", *tf_ );
00048 std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
00049 EXPECT_EQ( 3, footprint.size() );
00050
00051 EXPECT_EQ( 1.0f, footprint[ 0 ].x );
00052 EXPECT_EQ( 1.0f, footprint[ 0 ].y );
00053 EXPECT_EQ( 0.0f, footprint[ 0 ].z );
00054
00055 EXPECT_EQ( -1.0f, footprint[ 1 ].x );
00056 EXPECT_EQ( 1.0f, footprint[ 1 ].y );
00057 EXPECT_EQ( 0.0f, footprint[ 1 ].z );
00058
00059 EXPECT_EQ( -1.0f, footprint[ 2 ].x );
00060 EXPECT_EQ( -1.0f, footprint[ 2 ].y );
00061 EXPECT_EQ( 0.0f, footprint[ 2 ].z );
00062 }
00063
00064 TEST( Costmap2DROS, padded_footprint_from_string_param )
00065 {
00066 Costmap2DROS cm( "padded/string", *tf_ );
00067 std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
00068 EXPECT_EQ( 3, footprint.size() );
00069
00070 EXPECT_EQ( 1.5f, footprint[ 0 ].x );
00071 EXPECT_EQ( 1.5f, footprint[ 0 ].y );
00072 EXPECT_EQ( 0.0f, footprint[ 0 ].z );
00073
00074 EXPECT_EQ( -1.5f, footprint[ 1 ].x );
00075 EXPECT_EQ( 1.5f, footprint[ 1 ].y );
00076 EXPECT_EQ( 0.0f, footprint[ 1 ].z );
00077
00078 EXPECT_EQ( -1.5f, footprint[ 2 ].x );
00079 EXPECT_EQ( -1.5f, footprint[ 2 ].y );
00080 EXPECT_EQ( 0.0f, footprint[ 2 ].z );
00081 }
00082
00083 TEST( Costmap2DROS, radius_param )
00084 {
00085 Costmap2DROS cm( "radius/sub", *tf_ );
00086 std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
00087
00088 EXPECT_EQ( 16, footprint.size() );
00089
00090
00091 EXPECT_EQ( 10.0f, footprint[ 0 ].x );
00092 EXPECT_EQ( 0.0f, footprint[ 0 ].y );
00093 EXPECT_EQ( 0.0f, footprint[ 0 ].z );
00094
00095
00096 EXPECT_NEAR( 0.0f, footprint[ 4 ].x, 0.0001 );
00097 EXPECT_NEAR( 10.0f, footprint[ 4 ].y, 0.0001 );
00098 EXPECT_EQ( 0.0f, footprint[ 4 ].z );
00099 }
00100
00101 TEST( Costmap2DROS, footprint_from_xmlrpc_param )
00102 {
00103 Costmap2DROS cm( "xmlrpc", *tf_ );
00104 std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
00105 EXPECT_EQ( 4, footprint.size() );
00106
00107 EXPECT_EQ( 0.1f, footprint[ 0 ].x );
00108 EXPECT_EQ( 0.1f, footprint[ 0 ].y );
00109 EXPECT_EQ( 0.0f, footprint[ 0 ].z );
00110
00111 EXPECT_EQ( -0.1f, footprint[ 1 ].x );
00112 EXPECT_EQ( 0.1f, footprint[ 1 ].y );
00113 EXPECT_EQ( 0.0f, footprint[ 1 ].z );
00114
00115 EXPECT_EQ( -0.1f, footprint[ 2 ].x );
00116 EXPECT_EQ( -0.1f, footprint[ 2 ].y );
00117 EXPECT_EQ( 0.0f, footprint[ 2 ].z );
00118
00119 EXPECT_EQ( 0.1f, footprint[ 3 ].x );
00120 EXPECT_EQ( -0.1f, footprint[ 3 ].y );
00121 EXPECT_EQ( 0.0f, footprint[ 3 ].z );
00122 }
00123
00124 TEST( Costmap2DROS, footprint_from_same_level_param )
00125 {
00126 Costmap2DROS cm( "same_level", *tf_ );
00127 std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
00128 EXPECT_EQ( 3, footprint.size() );
00129
00130 EXPECT_EQ( 1.0f, footprint[ 0 ].x );
00131 EXPECT_EQ( 2.0f, footprint[ 0 ].y );
00132 EXPECT_EQ( 0.0f, footprint[ 0 ].z );
00133
00134 EXPECT_EQ( 3.0f, footprint[ 1 ].x );
00135 EXPECT_EQ( 4.0f, footprint[ 1 ].y );
00136 EXPECT_EQ( 0.0f, footprint[ 1 ].z );
00137
00138 EXPECT_EQ( 5.0f, footprint[ 2 ].x );
00139 EXPECT_EQ( 6.0f, footprint[ 2 ].y );
00140 EXPECT_EQ( 0.0f, footprint[ 2 ].z );
00141 }
00142
00143 TEST( Costmap2DROS, footprint_from_xmlrpc_param_failure )
00144 {
00145 ASSERT_ANY_THROW( Costmap2DROS cm( "xmlrpc_fail", *tf_ ));
00146 }
00147
00148 TEST( Costmap2DROS, footprint_empty )
00149 {
00150 Costmap2DROS cm( "empty", *tf_ );
00151 std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
00152
00153 EXPECT_EQ( 16, footprint.size() );
00154
00155 EXPECT_NEAR( 0.47f, footprint[ 0 ].x, 0.0001 );
00156 EXPECT_NEAR( 0.0f, footprint[ 0 ].y, 0.0001 );
00157 EXPECT_EQ( 0.0f, footprint[ 0 ].z );
00158 }
00159
00160 int main(int argc, char** argv)
00161 {
00162 ros::init(argc, argv, "footprint_tests_node");
00163
00164 tf_ = new tf::TransformListener( ros::Duration( 10 ));
00165
00166
00167
00168
00169 tf::StampedTransform base_rel_map;
00170 base_rel_map.child_frame_id_ = "/base_link";
00171 base_rel_map.frame_id_ = "/map";
00172 base_rel_map.stamp_ = ros::Time::now();
00173 tf_->setTransform( base_rel_map );
00174
00175 testing::InitGoogleTest(&argc, argv);
00176 return RUN_ALL_TESTS();
00177 }