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/cv_bridge.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
00075
00076 void initializeGL ()
00077 {
00078 glClearColor(0, 0, 0, 0);
00079
00080
00081 glEnable(GL_NORMALIZE);
00082 glEnable(GL_COLOR_MATERIAL);
00083 glEnable(GL_CULL_FACE);
00084 glEnable(GL_DEPTH_TEST);
00085 glShadeModel(GL_SMOOTH);
00086 glTexParameterf (GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
00087 glTexParameterf (GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER,
00088 GL_LINEAR_MIPMAP_LINEAR);
00089 }
00090
00091
00092
00093
00094 void display() {
00095
00096 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
00097
00098 robmod->setCamera();
00099 robmod->setCameraPose();
00100 robmod->paintRobot();
00101 glReadPixels(0, 0, robmod->cam_info_->width, robmod->cam_info_->height , GL_BGRA, GL_UNSIGNED_BYTE, ipl_maskBGRA->imageData);
00102
00103 glutSwapBuffers();
00104 }
00105
00106
00107
00108 void calc_and_publish_BWMask(const ros::Time time_stamp, const std::string frame_id ){
00109 robmod->updateRobotLinks(time_stamp);
00110 glutPostRedisplay();
00111 glutMainLoopEvent();
00112 cvCvtColor(ipl_maskBGRA, ipl_maskBW, CV_BGRA2GRAY);
00113 cvFlip(ipl_maskBW);
00114 if (inverted)
00115 cvNot(ipl_maskBW,ipl_maskBW);
00116
00117
00118 if(publish_mask){
00119 std_msgs::Header header;
00120 header.frame_id = frame_id;
00121 header.stamp = time_stamp;
00122 cv_bridge::CvImage cv_image(header,"mono8",ipl_maskBW);
00123 mask_publisher.publish(cv_image.toImageMsg());
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
00132 cv_bridge::CvImagePtr cv_ptr;
00133 try
00134 {
00135 cv_ptr = cv_bridge::toCvCopy(msg_ptr, "passthrough");
00136 }
00137 catch (cv_bridge::Exception& e)
00138 {
00139 ROS_ERROR("cv_bridge exception: %s", e.what());
00140 return;
00141 }
00142
00143 IplImage cam_image = cv_ptr->image;
00144 IplImage* cam_alpha_image = cvCreateImage(cvGetSize(&cam_image), IPL_DEPTH_8U, 4);
00145
00146
00147 cvSetImageCOI(cam_alpha_image, 1);
00148 cvSetImageCOI(&cam_image, 1);
00149 cvCopy(&cam_image, cam_alpha_image);
00150
00151
00152 cvSetImageCOI(cam_alpha_image, 2);
00153 cvSetImageCOI(&cam_image, 2);
00154 cvCopy(&cam_image, cam_alpha_image);
00155
00156
00157 cvSetImageCOI(cam_alpha_image, 3);
00158 cvSetImageCOI(&cam_image, 3);
00159 cvCopy(&cam_image, cam_alpha_image);
00160
00161
00162 cvSetImageCOI(cam_alpha_image, 4);
00163 cvCopy(ipl_maskBW, cam_alpha_image);
00164 cvSetImageCOI(cam_alpha_image, 0);
00165
00166
00167 cv_bridge::CvImage cv_image(msg_ptr->header,"rgba8",cam_alpha_image);
00168 image_publisher.publish(cv_image.toImageMsg());
00169 cvReleaseImage(&cam_alpha_image);
00170
00171
00172 }
00173
00174
00175 void mask_cb(const sensor_msgs::CameraInfoConstPtr& msg_ptr){
00176 calc_and_publish_BWMask(msg_ptr->header.stamp, msg_ptr->header.frame_id);
00177 }
00178
00179
00180
00181
00182
00183
00184 int main(int argc, char **argv) {
00185 glutInit(&argc, argv);
00186 ros::init(argc, argv, "robot_self_filter");
00187
00188
00189
00190
00191
00192 ros::NodeHandle nh;
00193 ros::NodeHandle nh_priv("~");
00194
00195
00196 bool publish_alpha_image;
00197
00198 std::string camera_topic;
00199 std::string camera_info_topic;
00200
00201 nh_priv.param("publish_mask",publish_mask, true);
00202 nh_priv.param("publish_alpha",publish_alpha_image, true);
00203 nh_priv.param("inverted",inverted, false);
00204
00205
00206 nh_priv.param<std::string>("camera_topic", camera_topic, "/wide_stereo/right/image_rect_color" );
00207 nh_priv.param<std::string>("camera_info_topic", camera_info_topic, "/wide_stereo/right/camera_info" );
00208
00209 ROS_INFO("camera topic %s", camera_topic.c_str());
00210 ROS_INFO("camera info %s", camera_info_topic.c_str());
00211
00212
00213
00214 robmod = new RobotMeshModel;
00215
00216
00217 glutInitWindowSize (robmod->cam_info_->width, robmod->cam_info_->height);
00218 glutInitDisplayMode ( GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
00219
00220
00221 glutCreateWindow ("dummy");
00222 glutHideWindow();
00223
00224 initializeGL();
00225
00226 glutDisplayFunc (display);
00227
00228
00229 ipl_maskBGRA = cvCreateImage(cvSize( robmod->cam_info_->width, robmod->cam_info_->height), IPL_DEPTH_8U, 4);
00230 ipl_maskBW = cvCreateImage(cvSize( robmod->cam_info_->width, robmod->cam_info_->height), IPL_DEPTH_8U, 1);
00231
00232
00233
00234
00235 image_transport::ImageTransport it(nh);
00236
00237 if (publish_mask)
00238 mask_publisher = it.advertise(camera_topic + "/self_mask", 10);
00239
00240 if (publish_alpha_image)
00241 image_publisher = it.advertise(camera_topic + "/image_rect_color_alpha_masked", 10);
00242
00243 image_transport::Subscriber image_sub;
00244 ros::Subscriber camerea_info_sub;
00245 if (publish_alpha_image){
00246 image_sub = it.subscribe(camera_topic, 10, alpha_image_cb);
00247 }else{
00248 camerea_info_sub = nh.subscribe(camera_info_topic, 10, mask_cb);
00249 }
00250
00251
00252
00253 ros::spin();
00254
00255
00256
00257 cvReleaseImage(&ipl_maskBGRA);
00258 cvReleaseImage(&ipl_maskBW);
00259 delete robmod;
00260 return 0;
00261 }
00262