$search
00001 /* 00002 * Copyright (C) 2010, Dejan Pangercic <dejan.pangercic@cs.tum.edu>, Alexis Maldonado <maldonado@tum.de> 00003 * 00004 * This program is free software; you can redistribute it and/or modify 00005 * it under the terms of the GNU General Public License as published by 00006 * the Free Software Foundation; either version 3 of the License, or 00007 * (at your option) any later version. 00008 * 00009 * This program is distributed in the hope that it will be useful, 00010 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00011 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00012 * GNU General Public License for more details. 00013 * 00014 * You should have received a copy of the GNU General Public License 00015 * along with this program. If not, see <http://www.gnu.org/licenses/>. 00016 */ 00017 00018 #include <ace/config.h> 00019 #include <stdio.h> 00020 #include <iostream> 00021 #include <string.h> 00022 using namespace std; 00023 //openCV 00024 #include <cv.h> 00025 #include <cvaux.h> 00026 #include <highgui.h> 00027 00028 //yarp 00029 #include <yarp/os/all.h> 00030 #include <yarp/sig/all.h> 00031 00032 //ROS 00033 #include <ros/ros.h> 00034 #include <ros/node_handle.h> 00035 #include "sensor_msgs/Image.h" 00036 #include "sensor_msgs/CameraInfo.h" 00037 #include "sensor_msgs/SetCameraInfo.h" 00038 #include "image_transport/image_transport.h" 00039 #include "cv_bridge/CvBridge.h" 00040 #include <camera_calibration_parsers/parse_ini.h> 00041 00042 00043 00044 //namespaces 00045 using namespace yarp::os; 00046 using namespace yarp::sig; 00047 using namespace yarp::sig::draw; 00048 using namespace yarp::sig::file; 00049 00050 class YARPToROSImage { 00051 00052 public: 00053 string port_name_, output_image_topic_; 00054 // Initialize network 00055 Network yarp_; 00056 // Make a port for reading and writing images 00057 BufferedPort<ImageOf<PixelRgb> > port_; 00058 IplImage *cvImage_; 00059 ImageOf<PixelRgb> *yarpImage; 00060 sensor_msgs::CameraInfo camera_info; 00061 std::string camera_parameters_file; 00062 std::string camera_frame_id; 00063 00064 00065 bool setCameraInfo(sensor_msgs::SetCameraInfo::Request &req, sensor_msgs::SetCameraInfo::Response &rsp) { 00066 00067 camera_info = req.camera_info; 00068 ROS_INFO("New camera info received and accepted"); 00069 00070 if (camera_calibration_parsers::writeCalibrationIni(camera_parameters_file.c_str(), "yarp_to_ros_image", camera_info)) { 00071 ROS_INFO( "Camera information written to the camera parameters file: %s", camera_parameters_file.c_str() ); 00072 return true; 00073 } 00074 else { 00075 ROS_ERROR( "Could not write the camera parameters file: %s", camera_parameters_file.c_str() ); 00076 return false; 00077 } 00078 } 00079 00080 // Constructor 00081 YARPToROSImage(ros::NodeHandle &n) : 00082 n_(n), it_(n_) 00083 { 00084 n_.param("port_name", port_name_, std::string("/yarp_to_ros_image")); 00085 n_.param("output_image_topic", output_image_topic_, std::string("image")); 00086 n_.param("camera_parameters_file", camera_parameters_file, std::string("camera_parameters.txt")); 00087 n_.param("camera_frame_id", camera_frame_id, std::string("/r_eye3")); 00088 00089 std::string camera_name; 00090 if (camera_calibration_parsers::readCalibrationIni(camera_parameters_file.c_str(), camera_name, camera_info)) { 00091 ROS_INFO("Successfully read camera calibration: \"%s\" Rerun camera calibrator if it is incorrect.", camera_parameters_file.c_str()); 00092 } else { 00093 ROS_ERROR("No camera parameters file found. Use default file if no other is available. (This process will publish invalid camera calibration data)."); 00094 } 00095 00096 ConstString yarp_port_name_(port_name_.c_str()); 00097 yarp_.init(); 00098 port_.open(yarp_port_name_); 00099 pub_ = it_.advertiseCamera(output_image_topic_, 1); 00100 set_camera_info = n_.advertiseService("set_camera_info", &YARPToROSImage::setCameraInfo, this); 00101 } 00102 00103 00104 ~YARPToROSImage() 00105 { 00106 yarp_.fini(); 00107 } 00108 00109 00110 // Spin (!) 00111 bool spin () 00112 { 00113 yarpImage = port_.read(); 00114 cvImage_ = cvCreateImage(cvSize(yarpImage->width(),yarpImage->height()), 00115 IPL_DEPTH_8U, 3 ); 00116 while (n_.ok ()) 00117 { 00118 // read an image from the port 00119 yarpImage = port_.read(); 00120 ros::Time now = ros::Time::now(); 00121 00122 if (yarpImage==NULL) continue; 00123 00124 cvCvtColor((IplImage*)yarpImage->getIplImage(), cvImage_, CV_RGB2BGR); 00125 camera_info.header.stamp.sec = now.sec; 00126 camera_info.header.stamp.nsec = now.nsec; 00127 //The other fields of camera_info are already there: loaded from file 00128 // or set by the service call 00129 00130 try 00131 { 00132 sensor_msgs::Image::Ptr pimg = bridge_.cvToImgMsg(cvImage_, "bgr8"); 00133 pimg->header.stamp.sec = now.sec; 00134 pimg->header.stamp.nsec = now.nsec; 00135 pimg->header.frame_id = camera_frame_id.c_str(); 00136 pub_.publish(*pimg, camera_info); 00137 } 00138 catch (sensor_msgs::CvBridgeException error) 00139 { 00140 ROS_ERROR("CvBridgeException error"); 00141 } 00142 00143 ros::spinOnce (); 00144 } 00145 cvReleaseImage(&cvImage_); 00146 return (true); 00147 } 00148 00149 protected: 00150 ros::NodeHandle n_; 00151 image_transport::ImageTransport it_; 00152 sensor_msgs::CvBridge bridge_; 00153 image_transport::CameraPublisher pub_; 00154 ros::ServiceServer set_camera_info; 00155 00156 }; 00157 00158 00159 00160 int main(int argc, char** argv) 00161 { 00162 ros::init(argc, argv, "yarp_to_ros_image"); 00163 ros::NodeHandle n("~"); 00164 YARPToROSImage ytri(n); 00165 ytri.spin(); 00166 return 0; 00167 }