$search
00001 /********************************************************************* 00002 * 00003 * Software License Agreement (BSD License) 00004 * 00005 * Copyright (c) 2008, Robert Bosch LLC. 00006 * All rights reserved. 00007 * 00008 * Redistribution and use in source and binary forms, with or without 00009 * modification, are permitted provided that the following conditions 00010 * are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above 00015 * copyright notice, this list of conditions and the following 00016 * disclaimer in the documentation and/or other materials provided 00017 * with the distribution. 00018 * * Neither the name of the Robert Bosch nor the names of its 00019 * contributors may be used to endorse or promote products derived 00020 * from this software without specific prior written permission. 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 * POSSIBILITY OF SUCH DAMAGE. 00034 * 00035 *********************************************************************/ 00036 00037 /* 00038 * camera_self_filter.cpp 00039 * 00040 * Created on: Oct 6, 2010 00041 * Author: Christian Bersch 00042 */ 00043 00044 #include <cstdio> 00045 00046 #include <ros/ros.h> 00047 #include <opencv/cv.h> 00048 #include <opencv/highgui.h> 00049 #include "cv_bridge/CvBridge.h" 00050 #include "sensor_msgs/Image.h" 00051 #include "sensor_msgs/CameraInfo.h" 00052 #include "image_transport/image_transport.h" 00053 #include <image_geometry/pinhole_camera_model.h> 00054 00055 00056 #define GL_GLEXT_PROTOTYPES 00057 #include <GL/gl.h> 00058 #include <GL/glext.h> 00059 #include <GL/freeglut.h> 00060 00061 00062 #include "camera_self_filter/robotMeshModel.h" 00063 00064 00065 00066 RobotMeshModel* robmod=NULL; 00067 IplImage* ipl_maskBGRA = NULL; 00068 IplImage* ipl_maskBW = NULL; 00069 00070 bool publish_mask; 00071 bool inverted; 00072 image_transport::Publisher mask_publisher; 00073 image_transport::Publisher image_publisher; 00074 sensor_msgs::CvBridge bridge; 00075 00076 00077 void initializeGL () 00078 { 00079 glClearColor(0, 0, 0, 0); 00080 // glEnable(GL_LIGHTING); 00081 // glEnable(GL_LIGHT0); 00082 glEnable(GL_NORMALIZE); 00083 glEnable(GL_COLOR_MATERIAL); 00084 glEnable(GL_CULL_FACE); 00085 glEnable(GL_DEPTH_TEST); 00086 glShadeModel(GL_SMOOTH); 00087 glTexParameterf (GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); 00088 glTexParameterf (GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, 00089 GL_LINEAR_MIPMAP_LINEAR); 00090 } 00091 00092 00093 00094 //function that draws the robot at current state 00095 void display() { 00096 00097 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 00098 00099 robmod->setCamera(); 00100 robmod->setCameraPose(); 00101 robmod->paintRobot(); 00102 glReadPixels(0, 0, robmod->cam_info_->width, robmod->cam_info_->height , GL_BGRA, GL_UNSIGNED_BYTE, ipl_maskBGRA->imageData); 00103 00104 glutSwapBuffers(); 00105 } 00106 00107 00108 00109 void calc_and_publish_BWMask(const ros::Time time_stamp, const std::string frame_id ){ 00110 robmod->updateRobotLinks(time_stamp); 00111 glutPostRedisplay(); 00112 glutMainLoopEvent(); 00113 cvCvtColor(ipl_maskBGRA, ipl_maskBW, CV_BGRA2GRAY); 00114 cvFlip(ipl_maskBW); 00115 if (inverted) 00116 cvNot(ipl_maskBW,ipl_maskBW); 00117 00118 00119 if(publish_mask){ 00120 sensor_msgs::ImagePtr img_msg = sensor_msgs::CvBridge::cvToImgMsg(ipl_maskBW); 00121 img_msg->header.frame_id = frame_id; 00122 img_msg->header.stamp = time_stamp; 00123 mask_publisher.publish(img_msg); 00124 } 00125 00126 } 00127 void alpha_image_cb(const sensor_msgs::ImageConstPtr& msg_ptr){ 00128 00129 00130 calc_and_publish_BWMask(msg_ptr->header.stamp, msg_ptr->header.frame_id); 00131 IplImage* cam_image = bridge.imgMsgToCv(msg_ptr); 00132 IplImage* cam_alpha_image = cvCreateImage(cvGetSize(cam_image), IPL_DEPTH_8U, 4); 00133 00134 //b 00135 cvSetImageCOI(cam_alpha_image, 1); 00136 cvSetImageCOI(cam_image, 1); 00137 cvCopy(cam_image, cam_alpha_image); 00138 00139 //g 00140 cvSetImageCOI(cam_alpha_image, 2); 00141 cvSetImageCOI(cam_image, 2); 00142 cvCopy(cam_image, cam_alpha_image); 00143 00144 //r 00145 cvSetImageCOI(cam_alpha_image, 3); 00146 cvSetImageCOI(cam_image, 3); 00147 cvCopy(cam_image, cam_alpha_image); 00148 00149 //alpha 00150 cvSetImageCOI(cam_alpha_image, 4); 00151 cvCopy(ipl_maskBW, cam_alpha_image); 00152 cvSetImageCOI(cam_alpha_image, 0); 00153 00154 00155 sensor_msgs::ImagePtr img_msg = sensor_msgs::CvBridge::cvToImgMsg(cam_alpha_image); 00156 img_msg->header = msg_ptr->header; 00157 image_publisher.publish(img_msg); 00158 cvReleaseImage(&cam_alpha_image); 00159 00160 00161 } 00162 00163 00164 void mask_cb(const sensor_msgs::CameraInfoConstPtr& msg_ptr){ 00165 calc_and_publish_BWMask(msg_ptr->header.stamp, msg_ptr->header.frame_id); 00166 } 00167 00168 00169 00170 00171 00172 00173 int main(int argc, char **argv) { 00174 glutInit(&argc, argv); 00175 ros::init(argc, argv, "robot_self_filter"); 00176 00177 00178 00179 00180 //create selfilter service 00181 ros::NodeHandle nh; 00182 ros::NodeHandle nh_priv("~"); 00183 00184 //set parameters 00185 bool publish_alpha_image; 00186 00187 std::string camera_topic; 00188 std::string camera_info_topic; 00189 00190 nh_priv.param("publish_mask",publish_mask, true); 00191 nh_priv.param("publish_alpha",publish_alpha_image, true); 00192 nh_priv.param("inverted",inverted, false); 00193 00194 00195 nh_priv.param<std::string>("camera_topic", camera_topic, "/wide_stereo/right/image_rect_color" ); 00196 nh_priv.param<std::string>("camera_info_topic", camera_info_topic, "/wide_stereo/right/camera_info" ); 00197 00198 ROS_INFO("camera topic %s", camera_topic.c_str()); 00199 ROS_INFO("camera info %s", camera_info_topic.c_str()); 00200 00201 00202 //create robot model 00203 robmod = new RobotMeshModel; 00204 00205 //initialize glut 00206 glutInitWindowSize (robmod->cam_info_->width, robmod->cam_info_->height); 00207 glutInitDisplayMode ( GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH); 00208 00209 00210 glutCreateWindow ("dummy"); 00211 glutHideWindow(); 00212 00213 initializeGL(); 00214 // Register glut callbacks: 00215 glutDisplayFunc (display); 00216 00217 00218 ipl_maskBGRA = cvCreateImage(cvSize( robmod->cam_info_->width, robmod->cam_info_->height), IPL_DEPTH_8U, 4); 00219 ipl_maskBW = cvCreateImage(cvSize( robmod->cam_info_->width, robmod->cam_info_->height), IPL_DEPTH_8U, 1); 00220 00221 00222 00223 00224 image_transport::ImageTransport it(nh); 00225 00226 if (publish_mask) 00227 mask_publisher = it.advertise(camera_topic + "/self_mask", 10); 00228 00229 if (publish_alpha_image) 00230 image_publisher = it.advertise(camera_topic + "/image_rect_color_alpha_masked", 10); 00231 00232 image_transport::Subscriber image_sub; 00233 ros::Subscriber camerea_info_sub; 00234 if (publish_alpha_image){ 00235 image_sub = it.subscribe(camera_topic, 10, alpha_image_cb); 00236 }else{ 00237 camerea_info_sub = nh.subscribe(camera_info_topic, 10, mask_cb); 00238 } 00239 00240 00241 00242 ros::spin(); 00243 00244 00245 //clean up 00246 cvReleaseImage(&ipl_maskBGRA); 00247 cvReleaseImage(&ipl_maskBW); 00248 delete robmod; 00249 return 0; 00250 } 00251