Go to the documentation of this file.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 #include <depthimage_to_laserscan/DepthImageToLaserScan.h>
00036
00037 #include <gtest/gtest.h>
00038
00039 #include <limits.h>
00040 #include <math.h>
00041
00042 #include <stdio.h>
00043 #include <stdlib.h>
00044 #include <time.h>
00045
00046
00047 depthimage_to_laserscan::DepthImageToLaserScan dtl_;
00048
00049
00050 sensor_msgs::ImagePtr depth_msg_;
00051 sensor_msgs::CameraInfoPtr info_msg_;
00052
00053
00054 TEST(ConvertTest, setupLibrary)
00055 {
00056
00057 const float scan_time = 1.0/30.0;
00058 dtl_.set_scan_time(scan_time);
00059 const float range_min = 0.45;
00060 const float range_max = 10.0;
00061 dtl_.set_range_limits(range_min, range_max);
00062 const int scan_height = 1;
00063 dtl_.set_scan_height(scan_height);
00064 const std::string output_frame = "camera_depth_frame";
00065 dtl_.set_output_frame(output_frame);
00066
00067 depth_msg_.reset(new sensor_msgs::Image);
00068 depth_msg_->header.seq = 42;
00069 depth_msg_->header.stamp.fromNSec(1234567890);
00070 depth_msg_->header.frame_id = "frame";
00071 depth_msg_->height = 480;
00072 depth_msg_->width = 640;
00073 depth_msg_->encoding = sensor_msgs::image_encodings::TYPE_16UC1;
00074 depth_msg_->is_bigendian = false;
00075 depth_msg_->step = depth_msg_->width*2;
00076 uint16_t value = 0x0F;
00077 depth_msg_->data.assign(depth_msg_->height*depth_msg_->step, value);
00078
00079 info_msg_.reset(new sensor_msgs::CameraInfo);
00080 info_msg_->header = depth_msg_->header;
00081 info_msg_->height = depth_msg_->height;
00082 info_msg_->width = depth_msg_->width;
00083 info_msg_->distortion_model = "plumb_bob";
00084 info_msg_->D.resize(5);
00085 info_msg_->K[0] = 570.3422241210938;
00086 info_msg_->K[2] = 314.5;
00087 info_msg_->K[4] = 570.3422241210938;
00088 info_msg_->K[5] = 235.5;
00089 info_msg_->K[8] = 1.0;
00090 info_msg_->R[0] = 1.0;
00091 info_msg_->R[4] = 1.0;
00092 info_msg_->R[8] = 1.0;
00093 info_msg_->P[0] = 570.3422241210938;
00094 info_msg_->P[2] = 314.5;
00095 info_msg_->P[5] = 570.3422241210938;
00096 info_msg_->P[6] = 235.5;
00097 info_msg_->P[10] = 1.0;
00098
00099 sensor_msgs::LaserScanPtr scan_msg = dtl_.convert_msg(depth_msg_, info_msg_);
00100
00101
00102 EXPECT_EQ(scan_msg->scan_time, scan_time);
00103 EXPECT_EQ(scan_msg->range_min, range_min);
00104 EXPECT_EQ(scan_msg->range_max, range_max);
00105 EXPECT_EQ(scan_msg->header.frame_id, output_frame);
00106 EXPECT_EQ(scan_msg->ranges.size(), depth_msg_->width);
00107 }
00108
00109
00110 TEST(ConvertTest, testExceptions)
00111 {
00112
00113
00114 depth_msg_->encoding = sensor_msgs::image_encodings::RGB8;
00115 EXPECT_THROW(dtl_.convert_msg(depth_msg_, info_msg_), std::runtime_error);
00116 depth_msg_->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00117 EXPECT_NO_THROW(dtl_.convert_msg(depth_msg_, info_msg_));
00118 depth_msg_->encoding = sensor_msgs::image_encodings::TYPE_16UC1;
00119 EXPECT_NO_THROW(dtl_.convert_msg(depth_msg_, info_msg_));
00120 }
00121
00122
00123 TEST(ConvertTest, testScanHeight)
00124 {
00125 for(int scan_height = 1; scan_height <= 100; scan_height++){
00126 uint16_t low_value = 500;
00127 uint16_t high_value = 3000;
00128
00129 int data_len = depth_msg_->width;
00130 uint16_t* data = reinterpret_cast<uint16_t*>(&depth_msg_->data[0]);
00131 int row_step = depth_msg_->step / sizeof(uint16_t);
00132
00133 dtl_.set_scan_height(scan_height);
00134
00135 int offset = (int)(info_msg_->K[5]-scan_height/2);
00136 data += offset*row_step;
00137
00138 for(int v = 0; v < scan_height; v++, data += row_step){
00139 for (int u = 0; u < data_len; u++)
00140 {
00141 if(v % scan_height == u % scan_height){
00142 data[u] = low_value;
00143 } else {
00144 data[u] = high_value;
00145 }
00146 }
00147 }
00148
00149
00150 sensor_msgs::LaserScanPtr scan_msg = dtl_.convert_msg(depth_msg_, info_msg_);
00151
00152
00153 float high_float_thresh = (float)high_value * 1.0f/1000.0f * 0.9f;
00154 for(size_t i = 0; i < scan_msg->ranges.size(); i++){
00155
00156 if(scan_msg->range_min <= scan_msg->ranges[i] && scan_msg->ranges[i] <= scan_msg->range_max){
00157
00158 ASSERT_LT(scan_msg->ranges[i], high_float_thresh);
00159 }
00160 }
00161 }
00162
00163
00164 dtl_.set_scan_height(1);
00165
00166 }
00167
00168
00169
00170 TEST(ConvertTest, testRandom)
00171 {
00172 srand ( 8675309 );
00173
00174 uint16_t* data = reinterpret_cast<uint16_t*>(&depth_msg_->data[0]);
00175 for(size_t i = 0; i < depth_msg_->width*depth_msg_->height; i++){
00176 data[i] = rand() % 500;
00177 }
00178
00179
00180 sensor_msgs::LaserScanPtr scan_msg = dtl_.convert_msg(depth_msg_, info_msg_);
00181
00182
00183 for(size_t i = 0; i < scan_msg->ranges.size(); i++){
00184 if(std::isfinite(scan_msg->ranges[i])){
00185 ASSERT_GE(scan_msg->ranges[i], scan_msg->range_min);
00186 ASSERT_LE(scan_msg->ranges[i], scan_msg->range_max);
00187 }
00188 }
00189 }
00190
00191
00192 TEST(ConvertTest, testNaN)
00193 {
00194
00195 sensor_msgs::ImagePtr float_msg(new sensor_msgs::Image(*depth_msg_));
00196 float_msg->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00197 float_msg->step = float_msg->width*4;
00198 float_msg->data.resize(float_msg->step * float_msg->height);
00199
00200 float* data = reinterpret_cast<float*>(&float_msg->data[0]);
00201 for(size_t i = 0; i < float_msg->width*float_msg->height; i++){
00202 data[i] = std::numeric_limits<float>::quiet_NaN();
00203 }
00204
00205
00206 sensor_msgs::LaserScanPtr scan_msg = dtl_.convert_msg(float_msg, info_msg_);
00207
00208
00209 for(size_t i = 0; i < scan_msg->ranges.size(); i++){
00210 if(std::isfinite(scan_msg->ranges[i])){
00211 ADD_FAILURE() << "Non-NaN value produced from NaN test.";
00212 }
00213 }
00214 }
00215
00216
00217 TEST(ConvertTest, testPositiveInf)
00218 {
00219
00220 sensor_msgs::ImagePtr float_msg(new sensor_msgs::Image(*depth_msg_));
00221 float_msg->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00222 float_msg->step = float_msg->width*4;
00223 float_msg->data.resize(float_msg->step * float_msg->height);
00224
00225 float* data = reinterpret_cast<float*>(&float_msg->data[0]);
00226 for(size_t i = 0; i < float_msg->width*float_msg->height; i++){
00227 data[i] = std::numeric_limits<float>::infinity();
00228 }
00229
00230
00231 sensor_msgs::LaserScanPtr scan_msg = dtl_.convert_msg(float_msg, info_msg_);
00232
00233
00234 size_t nan_count = 0;
00235 for(size_t i = 0; i < scan_msg->ranges.size(); i++){
00236 if(std::isfinite(scan_msg->ranges[i])){
00237 ADD_FAILURE() << "Non-finite value produced from postive infniity test.";
00238 } else if(isnan(scan_msg->ranges[i])){
00239 nan_count++;
00240 } else if(scan_msg->ranges[i] < 0){
00241 ADD_FAILURE() << "Negative value produced from postive infinity test.";
00242 }
00243 }
00244
00245 ASSERT_LE(nan_count, scan_msg->ranges.size() * 0.80);
00246 }
00247
00248
00249 TEST(ConvertTest, testNegativeInf)
00250 {
00251
00252 sensor_msgs::ImagePtr float_msg(new sensor_msgs::Image(*depth_msg_));
00253 float_msg->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
00254 float_msg->step = float_msg->width*4;
00255 float_msg->data.resize(float_msg->step * float_msg->height);
00256
00257 float* data = reinterpret_cast<float*>(&float_msg->data[0]);
00258 for(size_t i = 0; i < float_msg->width*float_msg->height; i++){
00259 data[i] = -std::numeric_limits<float>::infinity();
00260 }
00261
00262
00263 sensor_msgs::LaserScanPtr scan_msg = dtl_.convert_msg(float_msg, info_msg_);
00264
00265
00266 size_t nan_count = 0;
00267 for(size_t i = 0; i < scan_msg->ranges.size(); i++){
00268 if(std::isfinite(scan_msg->ranges[i])){
00269 ADD_FAILURE() << "Non-finite value produced from postive infniity test.";
00270 } else if(isnan(scan_msg->ranges[i])){
00271 nan_count++;
00272 } else if(scan_msg->ranges[i] > 0){
00273 ADD_FAILURE() << "Postive value produced from negative infinity test.";
00274 }
00275 }
00276
00277 ASSERT_LE(nan_count, scan_msg->ranges.size() * 0.80);
00278 }
00279
00280
00281
00282 int main(int argc, char **argv){
00283 testing::InitGoogleTest(&argc, argv);
00284 return RUN_ALL_TESTS();
00285 }