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 <gtest/gtest.h>
00036
00037 #include <ros/ros.h>
00038
00039 #include <sensor_msgs/Image.h>
00040 #include <sensor_msgs/CameraInfo.h>
00041 #include <sensor_msgs/LaserScan.h>
00042 #include <sensor_msgs/image_encodings.h>
00043
00044
00045 sensor_msgs::ImagePtr depth_msg_;
00046 sensor_msgs::CameraInfoPtr info_msg_;
00047
00048 size_t message_count_;
00049
00050 void callback(const sensor_msgs::LaserScanConstPtr& msg){
00051 message_count_++;
00052 }
00053
00054 TEST(transform_listener, commsTest)
00055 {
00056 ros::NodeHandle n;
00057
00058
00059 depth_msg_.reset(new sensor_msgs::Image);
00060 depth_msg_->header.seq = 42;
00061 depth_msg_->header.stamp.fromNSec(1234567890);
00062 depth_msg_->header.frame_id = "frame";
00063 depth_msg_->height = 480;
00064 depth_msg_->width = 640;
00065 depth_msg_->encoding = sensor_msgs::image_encodings::TYPE_16UC1;
00066 depth_msg_->is_bigendian = false;
00067 depth_msg_->step = depth_msg_->width*2;
00068 uint16_t value = 0x0F;
00069 depth_msg_->data.assign(depth_msg_->height*depth_msg_->step, value);
00070
00071 info_msg_.reset(new sensor_msgs::CameraInfo);
00072 info_msg_->header = depth_msg_->header;
00073 info_msg_->height = depth_msg_->height;
00074 info_msg_->width = depth_msg_->width;
00075 info_msg_->distortion_model = "plumb_bob";
00076 info_msg_->D.resize(5);
00077 info_msg_->K[0] = 570.3422241210938;
00078 info_msg_->K[2] = 314.5;
00079 info_msg_->K[4] = 570.3422241210938;
00080 info_msg_->K[5] = 235.5;
00081 info_msg_->K[8] = 1.0;
00082 info_msg_->R[0] = 1.0;
00083 info_msg_->R[4] = 1.0;
00084 info_msg_->R[8] = 1.0;
00085 info_msg_->P[0] = 570.3422241210938;
00086 info_msg_->P[2] = 314.5;
00087 info_msg_->P[5] = 570.3422241210938;
00088 info_msg_->P[6] = 235.5;
00089 info_msg_->P[10] = 1.0;
00090
00091
00092 ros::Publisher pub = n.advertise<sensor_msgs::Image>("image", 5);
00093 ros::Publisher pub_info = n.advertise<sensor_msgs::CameraInfo>("camera_info", 5);
00094
00095
00096 ros::Subscriber sub = n.subscribe("scan", 20, callback);
00097
00098
00099 ros::Duration(5.0).sleep();
00100
00101 size_t number_published = 10;
00102 double sleep_duration = 1.0/30.0;
00103
00104
00105 for(size_t i = 0; i < number_published; i++){
00106 pub.publish(depth_msg_);
00107 pub_info.publish(info_msg_);
00108 ros::Duration(sleep_duration).sleep();
00109 ros::spinOnce();
00110 }
00111
00112
00113 ros::Duration(2.0).sleep();
00114 ros::spinOnce();
00115
00116 EXPECT_EQ(number_published, message_count_);
00117 }
00118
00119
00120 int main(int argc, char **argv){
00121 testing::InitGoogleTest(&argc, argv);
00122 ros::init(argc, argv, "depthimage_to_laserscan_unittest");
00123
00124 message_count_ = 0;
00125
00126 return RUN_ALL_TESTS();
00127 }