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
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
00081
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
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
00135 cvSetImageCOI(cam_alpha_image, 1);
00136 cvSetImageCOI(cam_image, 1);
00137 cvCopy(cam_image, cam_alpha_image);
00138
00139
00140 cvSetImageCOI(cam_alpha_image, 2);
00141 cvSetImageCOI(cam_image, 2);
00142 cvCopy(cam_image, cam_alpha_image);
00143
00144
00145 cvSetImageCOI(cam_alpha_image, 3);
00146 cvSetImageCOI(cam_image, 3);
00147 cvCopy(cam_image, cam_alpha_image);
00148
00149
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 robmod = new RobotMeshModel;
00179
00180
00181 glutInitWindowSize (robmod->cam_info_->width, robmod->cam_info_->height);
00182 glutInitDisplayMode ( GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
00183
00184
00185 glutCreateWindow ("dummy");
00186 glutHideWindow();
00187
00188 initializeGL();
00189
00190 glutDisplayFunc (display);
00191
00192
00193 ipl_maskBGRA = cvCreateImage(cvSize( robmod->cam_info_->width, robmod->cam_info_->height), IPL_DEPTH_8U, 4);
00194 ipl_maskBW = cvCreateImage(cvSize( robmod->cam_info_->width, robmod->cam_info_->height), IPL_DEPTH_8U, 1);
00195
00196
00197
00198 ros::NodeHandle nh;
00199 ros::NodeHandle nh_priv("~");
00200
00201
00202 bool publish_alpha_image;
00203
00204 std::string camera_topic;
00205
00206 nh_priv.param("publish_mask",publish_mask, true);
00207 nh_priv.param("publish_alpha",publish_alpha_image, true);
00208 nh_priv.param("inverted",inverted, false);
00209 nh_priv.param<std::string>("camera_topic", camera_topic, "/wide_stereo/right" );
00210
00211 image_transport::ImageTransport it(nh);
00212
00213 if (publish_mask)
00214 mask_publisher = it.advertise(camera_topic + "/self_mask", 10);
00215
00216 if (publish_alpha_image)
00217 image_publisher = it.advertise(camera_topic + "/image_rect_color_alpha_masked", 10);
00218
00219 image_transport::Subscriber image_sub;
00220 ros::Subscriber camerea_info_sub;
00221 if (publish_alpha_image){
00222 image_sub = it.subscribe(camera_topic + "/image_rect_color", 10, alpha_image_cb);
00223 }else{
00224 camerea_info_sub = nh.subscribe(camera_topic + "/camera_info", 10, mask_cb);
00225 }
00226
00227
00228
00229 ros::spin();
00230
00231
00232
00233 cvReleaseImage(&ipl_maskBGRA);
00234 cvReleaseImage(&ipl_maskBW);
00235 delete robmod;
00236 return 0;
00237 }
00238