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 #include <rviz/uniform_string_stream.h>
00037 #include "camera_info_display.h"
00038 #include <OGRE/OgreMaterialManager.h>
00039 #include <OGRE/OgreMaterial.h>
00040 #include <OGRE/OgreBlendMode.h>
00041 #include <QImage>
00042 #include <OGRE/OgreHardwarePixelBuffer.h>
00043
00044 namespace jsk_rviz_plugin
00045 {
00046 TrianglePolygon::TrianglePolygon(
00047 Ogre::SceneManager* manager,
00048 Ogre::SceneNode* node,
00049 const cv::Point3d& O,
00050 const cv::Point3d& A,
00051 const cv::Point3d& B,
00052 const std::string& name,
00053 const Ogre::ColourValue& color)
00054 {
00055
00056
00057 manual_ = manager->createManualObject();
00058 manual_->clear();
00059 manual_->begin(name,
00060 Ogre::RenderOperation::OT_TRIANGLE_STRIP);
00061 manual_->position(O.x, O.y, O.z);
00062 manual_->colour(color);
00063 manual_->position(A.x, A.y, A.z);
00064 manual_->colour(color);
00065 manual_->position(B.x, B.y, B.z);
00066 manual_->colour(color);
00067 manual_->end();
00068 node->attachObject(manual_);
00069 }
00070
00071 TrianglePolygon::~TrianglePolygon()
00072 {
00073 manual_->detachFromParent();
00074
00075 }
00076
00077 CameraInfoDisplay::CameraInfoDisplay()
00078 {
00080
00082 far_clip_distance_property_ = new rviz::FloatProperty(
00083 "far clip",
00084 1.0,
00085 "far clip distance from the origin of camera info",
00086 this, SLOT(updateFarClipDistance()));
00087 show_polygons_property_ = new rviz::BoolProperty(
00088 "show polygons",
00089 true,
00090 "show polygons of the region of the camera info",
00091 this, SLOT(updateShowPolygons()));
00092 color_property_ = new rviz::ColorProperty(
00093 "color",
00094 QColor(85, 255, 255),
00095 "color of CameraInfo",
00096 this, SLOT(updateColor()));
00097 edge_color_property_ = new rviz::ColorProperty(
00098 "edge color",
00099 QColor(125, 125, 125),
00100 "edge color of CameraInfo",
00101 this, SLOT(updateEdgeColor()));
00102 alpha_property_ = new rviz::FloatProperty(
00103 "alpha",
00104 0.5,
00105 "alpha blending value",
00106 this, SLOT(updateAlpha()));
00107 }
00108
00109 CameraInfoDisplay::~CameraInfoDisplay()
00110 {
00111 if (edges_) {
00112 edges_->clear();
00113 }
00114 polygons_.clear();
00115 delete far_clip_distance_property_;
00116 delete color_property_;
00117 delete alpha_property_;
00118 delete show_polygons_property_;
00119 delete edge_color_property_;
00120 }
00121
00122 void CameraInfoDisplay::reset()
00123 {
00124 MFDClass::reset();
00125 if (edges_) {
00126 edges_->clear();
00127 }
00128 polygons_.clear();
00129 camera_info_ = sensor_msgs::CameraInfo::ConstPtr();
00130 }
00131
00132 void CameraInfoDisplay::onInitialize()
00133 {
00134 MFDClass::onInitialize();
00135 scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00136 updateColor();
00137 updateAlpha();
00138 updateFarClipDistance();
00139 updateShowPolygons();
00140 updateEdgeColor();
00141 }
00142
00143 void CameraInfoDisplay::processMessage(
00144 const sensor_msgs::CameraInfo::ConstPtr& msg)
00145 {
00146 if (!isSameCameraInfo(msg)) {
00147 createCameraInfoShapes(msg);
00148 }
00149
00150 Ogre::Vector3 position;
00151 Ogre::Quaternion quaternion;
00152 if(!context_->getFrameManager()->getTransform(msg->header.frame_id,
00153 msg->header.stamp,
00154 position,
00155 quaternion)) {
00156 ROS_ERROR( "Error transforming pose '%s' from frame '%s' to frame '%s'",
00157 qPrintable( getName() ), msg->header.frame_id.c_str(),
00158 qPrintable( fixed_frame_ ));
00159 }
00160 scene_node_->setPosition(position);
00161 scene_node_->setOrientation(quaternion);
00162 camera_info_ = msg;
00163 }
00164
00165 bool CameraInfoDisplay::isSameCameraInfo(
00166 const sensor_msgs::CameraInfo::ConstPtr& msg)
00167 {
00168 if (camera_info_) {
00169 bool meta_same_p =
00170 msg->header.frame_id == camera_info_->header.frame_id &&
00171 msg->height == camera_info_->height &&
00172 msg->width == camera_info_->width &&
00173 msg->distortion_model == camera_info_->distortion_model;
00174 if (meta_same_p) {
00175 for (size_t i = 0; i < msg->P.size(); i++) {
00176 if (msg->P[i] != camera_info_->P[i]) {
00177 return false;
00178 }
00179 }
00180 return true;
00181 }
00182 else {
00183 return false;
00184 }
00185 }
00186 else {
00187 return false;
00188 }
00189 }
00190
00191 void CameraInfoDisplay::addPointToEdge(
00192 const cv::Point3d& point)
00193 {
00194 Ogre::Vector3 p;
00195 p[0] = point.x;
00196 p[1] = point.y;
00197 p[2] = point.z;
00198 edges_->addPoint(p);
00199 }
00200
00201 void CameraInfoDisplay::addPolygon(
00202 const cv::Point3d& O, const cv::Point3d& A, const cv::Point3d& B)
00203 {
00204 Ogre::ColourValue color = rviz::qtToOgre(color_);
00205 color.a = alpha_;
00206 TrianglePolygon::Ptr triangle (new TrianglePolygon(
00207 scene_manager_,
00208 scene_node_,
00209 O, A, B, material_->getName(),
00210 color));
00211 polygons_.push_back(triangle);
00212 }
00213
00214 void CameraInfoDisplay::prepareMaterial()
00215 {
00216 if (texture_.isNull()) {
00217 static uint32_t count = 0;
00218 rviz::UniformStringStream ss;
00219 ss << "CameraInfoDisplayPolygon" << count++;
00220 material_
00221 = Ogre::MaterialManager::getSingleton().create(
00222 ss.str(),
00223 Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
00224
00225 texture_ = Ogre::TextureManager::getSingleton().createManual(
00226 material_->getName() + "Texture",
00227 Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
00228 Ogre::TEX_TYPE_2D, 1, 1, 0, Ogre::PF_A8R8G8B8, Ogre::TU_DEFAULT);
00229
00230 material_->getTechnique(0)->getPass(0)->setColourWriteEnabled(true);
00231 Ogre::ColourValue color = rviz::qtToOgre(color_);
00232 color.a = alpha_;
00233 material_->getTechnique(0)->getPass(0)->setAmbient(color);
00234 material_->setReceiveShadows(false);
00235 material_->getTechnique(0)->setLightingEnabled(true);
00236 material_->getTechnique(0)->getPass(0)->setCullingMode(Ogre::CULL_NONE);
00237 material_->getTechnique(0)->getPass(0)->setLightingEnabled(false);
00238 material_->getTechnique(0)->getPass(0)->setDepthWriteEnabled(false);
00239 material_->getTechnique(0)->getPass(0)->setDepthCheckEnabled(true);
00240
00241 material_->getTechnique(0)->getPass(0)->setVertexColourTracking(Ogre::TVC_DIFFUSE);
00242 material_->getTechnique(0)->getPass(0)->createTextureUnitState(texture_->getName());
00243 material_->getTechnique(0)->getPass(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
00244 }
00245 }
00246
00247
00248 void CameraInfoDisplay::createCameraInfoShapes(
00249 const sensor_msgs::CameraInfo::ConstPtr& msg)
00250 {
00251 polygons_.clear();
00252 image_geometry::PinholeCameraModel model;
00253 bool model_success_p = model.fromCameraInfo(msg);
00254 if (!model_success_p) {
00255 ROS_ERROR("failed to create camera model");
00256 return;
00257 }
00258
00260
00262 if (!edges_) {
00263 edges_.reset(new rviz::BillboardLine(context_->getSceneManager(),
00264 scene_node_));
00265 edges_->setLineWidth(0.01);
00266 }
00267
00268 cv::Point2d a(0, 0), b(msg->width, 0),
00269 c(msg->width, msg->height), d(0, msg->height);
00270
00271 cv::Point3d A = model.projectPixelTo3dRay(a);
00272 cv::Point3d B = model.projectPixelTo3dRay(b);
00273 cv::Point3d C = model.projectPixelTo3dRay(c);
00274 cv::Point3d D = model.projectPixelTo3dRay(d);
00275
00276 cv::Point3d scaled_A = A * far_clip_distance_;
00277 cv::Point3d scaled_B = B * far_clip_distance_;
00278 cv::Point3d scaled_C = C * far_clip_distance_;
00279 cv::Point3d scaled_D = D * far_clip_distance_;
00280
00281 cv::Point3d O(0, 0, 0);
00282
00284
00286 if (show_polygons_) {
00288
00290 Ogre::ColourValue color = rviz::qtToOgre(color_);
00291 color.a = alpha_;
00292 prepareMaterial();
00293 material_->getTechnique(0)->getPass(0)->setAmbient(color);
00294 {
00295 texture_->getBuffer()->lock( Ogre::HardwareBuffer::HBL_NORMAL );
00296 const Ogre::PixelBox& pixelBox = texture_->getBuffer()->getCurrentLock();
00297 Ogre::uint8* pDest = static_cast<Ogre::uint8*> (pixelBox.data);
00298 memset(pDest, 0, 1);
00299 QImage Hud(pDest, 1, 1, QImage::Format_ARGB32 );
00300 Hud.setPixel(0, 0, color_.rgba());
00301 texture_->getBuffer()->unlock();
00302 }
00303
00304 addPolygon(O, scaled_B, scaled_A);
00305 addPolygon(O, scaled_C, scaled_B);
00306 addPolygon(O, scaled_D, scaled_C);
00307 addPolygon(O, scaled_A, scaled_D);
00308 addPolygon(scaled_A, scaled_B, scaled_D);
00309 addPolygon(scaled_B, scaled_C, scaled_D);
00310 }
00312
00314
00315 edges_->clear();
00316 edges_->setMaxPointsPerLine(2);
00317 edges_->setNumLines(8);
00318 edges_->setColor(edge_color_.red() / 255.0,
00319 edge_color_.green() / 255.0,
00320 edge_color_.blue() / 255.0,
00321 alpha_);
00322 addPointToEdge(O); addPointToEdge(scaled_A); edges_->newLine();
00323 addPointToEdge(O); addPointToEdge(scaled_B); edges_->newLine();
00324 addPointToEdge(O); addPointToEdge(scaled_C); edges_->newLine();
00325 addPointToEdge(O); addPointToEdge(scaled_D); edges_->newLine();
00326 addPointToEdge(scaled_A); addPointToEdge(scaled_B); edges_->newLine();
00327 addPointToEdge(scaled_B); addPointToEdge(scaled_C); edges_->newLine();
00328 addPointToEdge(scaled_C); addPointToEdge(scaled_D); edges_->newLine();
00329 addPointToEdge(scaled_D); addPointToEdge(scaled_A);
00330 }
00331
00333
00335 void CameraInfoDisplay::updateColor()
00336 {
00337 color_ = color_property_->getColor();
00338 if (camera_info_) {
00339 createCameraInfoShapes(camera_info_);
00340 }
00341 }
00342
00343 void CameraInfoDisplay::updateEdgeColor()
00344 {
00345 edge_color_ = edge_color_property_->getColor();
00346 if (camera_info_) {
00347 createCameraInfoShapes(camera_info_);
00348 }
00349 }
00350
00351 void CameraInfoDisplay::updateAlpha()
00352 {
00353 alpha_ = alpha_property_->getFloat();
00354 if (camera_info_) {
00355 createCameraInfoShapes(camera_info_);
00356 }
00357 }
00358
00359 void CameraInfoDisplay::updateFarClipDistance()
00360 {
00361 far_clip_distance_ = far_clip_distance_property_->getFloat();
00362 if (camera_info_) {
00363 createCameraInfoShapes(camera_info_);
00364 }
00365 }
00366
00367 void CameraInfoDisplay::updateShowPolygons()
00368 {
00369 show_polygons_ = show_polygons_property_->getBool();
00370 if (camera_info_) {
00371 createCameraInfoShapes(camera_info_);
00372 }
00373 }
00374
00375 }
00376
00377
00378 #include <pluginlib/class_list_macros.h>
00379 PLUGINLIB_EXPORT_CLASS( jsk_rviz_plugin::CameraInfoDisplay, rviz::Display )