rostest.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <gtest/gtest.h>
00003 #include <camera_calibration_parsers/parse.h>
00004 #include <cv_bridge/cv_bridge.h>
00005 #include <opencv2/highgui/highgui.hpp>
00006 #include <image_transport/image_transport.h>
00007 
00008 #include <boost/foreach.hpp>
00009 
00010 class ImageProcTest : public testing::Test
00011 {
00012 protected:
00013   virtual void SetUp()
00014   {
00015     ros::NodeHandle local_nh("~");
00016 
00017     // Determine topic names
00018     std::string camera_ns = nh.resolveName("camera") + "/";
00019     if (camera_ns == "/camera")
00020       throw "Must remap 'camera' to the camera namespace.";
00021     topic_raw        = camera_ns + "image_raw";
00022     topic_mono       = camera_ns + "image_mono";
00023     topic_rect       = camera_ns + "image_rect";
00024     topic_color      = camera_ns + "image_color";
00025     topic_rect_color = camera_ns + "image_rect_color";
00026 
00027     // Load raw image and cam info
00029     std::string raw_image_file, cam_info_file;
00030     if (!local_nh.getParam("raw_image_file", raw_image_file))
00031       throw "Must set parameter ~raw_image_file.";
00032     if (!local_nh.getParam("camera_info_file", cam_info_file))
00033       throw "Must set parameter ~camera_info_file.";
00034 
00036     cv::Mat img = cv::imread(raw_image_file, 0);
00037     raw_image = cv_bridge::CvImage(std_msgs::Header(), "mono8", img).toImageMsg();
00038     std::string cam_name;
00039     if (!camera_calibration_parsers::readCalibration(cam_info_file, cam_name, cam_info))
00040       throw "Failed to read camera info file.";
00041 
00042     // Create raw camera publisher
00043     image_transport::ImageTransport it(nh);
00044     cam_pub = it.advertiseCamera(topic_raw, 1);
00045 
00046     // Wait for image_proc to be operational
00047     ros::master::V_TopicInfo topics;
00048     while (true) {
00049       if (ros::master::getTopics(topics)) {
00050         BOOST_FOREACH(ros::master::TopicInfo& topic, topics) {
00051           if (topic.name == topic_rect_color)
00052             return;
00053         }
00054       }
00055       ros::Duration(0.5).sleep();
00056     }
00057   }
00058 
00059   ros::NodeHandle nh;
00060   std::string topic_raw;
00061   std::string topic_mono;
00062   std::string topic_rect;
00063   std::string topic_color;
00064   std::string topic_rect_color;
00065 
00066   sensor_msgs::ImagePtr raw_image;
00067   sensor_msgs::CameraInfo cam_info;
00068   image_transport::CameraPublisher cam_pub;
00069 
00070   void publishRaw()
00071   {
00072     cam_pub.publish(*raw_image, cam_info);
00073   }
00074 };
00075 
00076 void callback(const sensor_msgs::ImageConstPtr& msg)
00077 {
00078   ROS_FATAL("Got an image");
00079   ros::shutdown();
00080 }
00081 
00082 TEST_F(ImageProcTest, monoSubscription)
00083 {
00084   ROS_INFO("In test. Subscribing.");
00085   ros::Subscriber mono_sub = nh.subscribe(topic_mono, 1, callback);
00086   ROS_INFO("Publishing.");
00087   publishRaw();
00088 
00089   ROS_INFO("Spinning.");
00090   ros::spin();
00091   ROS_INFO("Done.");
00092 }
00093 
00094 int main(int argc, char** argv)
00095 {
00096   ros::init(argc, argv, "imageproc_rostest");
00097   testing::InitGoogleTest(&argc, argv);
00098   return RUN_ALL_TESTS();
00099 }


image_proc
Author(s): Patrick Mihelich, Kurt Konolige, Jeremy Leibs
autogenerated on Tue Sep 19 2017 02:56:13