$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 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 #define GL_GLEXT_PROTOTYPES 00056 #include <GL/gl.h> 00057 #include <GL/glext.h> 00058 #include <GL/freeglut.h> 00059 00060 #include "camera_self_filter/robotMeshModel.h" 00061 #include "camera_self_filter/mask.h" 00062 00063 00064 RobotMeshModel* robmod=NULL; 00065 IplImage* ipl_maskBGRA = NULL; 00066 IplImage* ipl_maskBW = NULL; 00067 00068 bool inverted; 00069 sensor_msgs::CvBridge bridge; 00070 00071 00072 void initializeGL () 00073 { 00074 glClearColor(0, 0, 0, 0); 00075 // glEnable(GL_LIGHTING); 00076 // glEnable(GL_LIGHT0); 00077 glEnable(GL_NORMALIZE); 00078 glEnable(GL_COLOR_MATERIAL); 00079 glEnable(GL_CULL_FACE); 00080 glEnable(GL_DEPTH_TEST); 00081 glShadeModel(GL_SMOOTH); 00082 glTexParameterf (GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); 00083 glTexParameterf (GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, 00084 GL_LINEAR_MIPMAP_LINEAR); 00085 } 00086 00087 00088 00089 //function that draws the robot at current state 00090 void display() { 00091 00092 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); 00093 00094 robmod->setCamera(); 00095 robmod->setCameraPose(); 00096 robmod->paintRobot(); 00097 glReadPixels(0, 0, robmod->cam_info_->width, robmod->cam_info_->height , GL_BGRA, GL_UNSIGNED_BYTE, ipl_maskBGRA->imageData); 00098 00099 glutSwapBuffers(); 00100 } 00101 00102 00103 00104 sensor_msgs::ImagePtr calc_and_publish_BWMask(const ros::Time time_stamp, const std::string frame_id ){ 00105 robmod->updateRobotLinks(time_stamp); 00106 glutPostRedisplay(); 00107 glutMainLoopEvent(); 00108 cvCvtColor(ipl_maskBGRA, ipl_maskBW, CV_BGRA2GRAY); 00109 cvFlip(ipl_maskBW); 00110 if (inverted) 00111 cvNot(ipl_maskBW,ipl_maskBW); 00112 00113 sensor_msgs::ImagePtr img_msg = sensor_msgs::CvBridge::cvToImgMsg(ipl_maskBW); 00114 img_msg->header.frame_id = frame_id; 00115 img_msg->header.stamp = time_stamp; 00116 return img_msg; 00117 00118 00119 00120 } 00121 00122 bool calcMaskCB(camera_self_filter::maskRequest& req, camera_self_filter::maskResponse& res){ 00123 sensor_msgs::ImagePtr img = calc_and_publish_BWMask(req.header.stamp, req.header.frame_id); 00124 res.mask_image = *img; 00125 return true; 00126 00127 } 00128 00129 00130 int main(int argc, char **argv) { 00131 glutInit(&argc, argv); 00132 ros::init(argc, argv, "robot_self_filter"); 00133 00134 00135 //create selfilter service 00136 ros::NodeHandle nh; 00137 ros::NodeHandle nh_priv("~"); 00138 00139 //set parameters 00140 00141 00142 std::string camera_topic; 00143 std::string camera_info_topic; 00144 00145 nh_priv.param("inverted",inverted, false); 00146 nh_priv.param<std::string>("camera_topic", camera_topic, "/wide_stereo/right/image_rect_color" ); 00147 nh_priv.param<std::string>("camera_info_topic", camera_info_topic, "/wide_stereo/right/camera_info" ); 00148 00149 ROS_INFO("camera topic %s", camera_topic.c_str()); 00150 ROS_INFO("camera info %s", camera_info_topic.c_str()); 00151 00152 //create robot model 00153 robmod = new RobotMeshModel; 00154 00155 //initialize glut 00156 glutInitWindowSize (robmod->cam_info_->width, robmod->cam_info_->height); 00157 glutInitDisplayMode ( GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH); 00158 00159 00160 glutCreateWindow ("dummy"); 00161 glutHideWindow(); 00162 00163 initializeGL(); 00164 // Register glut callbacks: 00165 glutDisplayFunc (display); 00166 00167 ipl_maskBGRA = cvCreateImage(cvSize( robmod->cam_info_->width, robmod->cam_info_->height), IPL_DEPTH_8U, 4); 00168 ipl_maskBW = cvCreateImage(cvSize( robmod->cam_info_->width, robmod->cam_info_->height), IPL_DEPTH_8U, 1); 00169 00170 00171 00172 00173 00174 ros::ServiceServer mask_service = nh.advertiseService("self_mask", calcMaskCB ); 00175 00176 00177 ros::spin(); 00178 00179 00180 //clean up 00181 cvReleaseImage(&ipl_maskBGRA); 00182 cvReleaseImage(&ipl_maskBW); 00183 delete robmod; 00184 return 0; 00185 } 00186