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
00036
00037
00038
00039
00040
00041
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
00076
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
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 robmod = new RobotMeshModel;
00136
00137
00138 glutInitWindowSize (robmod->cam_info_->width, robmod->cam_info_->height);
00139 glutInitDisplayMode ( GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
00140
00141
00142 glutCreateWindow ("dummy");
00143 glutHideWindow();
00144
00145 initializeGL();
00146
00147 glutDisplayFunc (display);
00148
00149 ipl_maskBGRA = cvCreateImage(cvSize( robmod->cam_info_->width, robmod->cam_info_->height), IPL_DEPTH_8U, 4);
00150 ipl_maskBW = cvCreateImage(cvSize( robmod->cam_info_->width, robmod->cam_info_->height), IPL_DEPTH_8U, 1);
00151
00152
00153
00154
00155 ros::NodeHandle nh;
00156 ros::NodeHandle nh_priv("~");
00157
00158
00159
00160
00161 std::string camera_topic;
00162
00163 nh_priv.param("inverted",inverted, false);
00164 nh_priv.param<std::string>("camera_topic", camera_topic, "/wide_stereo/right" );
00165
00166
00167 ros::ServiceServer mask_service = nh.advertiseService("self_mask", calcMaskCB );
00168
00169
00170 ros::spin();
00171
00172
00173
00174 cvReleaseImage(&ipl_maskBGRA);
00175 cvReleaseImage(&ipl_maskBW);
00176 delete robmod;
00177 return 0;
00178 }
00179