$search
00001 /* 00002 * Copyright (c) 2009, Willow Garage, Inc. 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 00030 #include "cob_3d_mapping_rviz_plugins/shape_marker.h" 00031 #include "cob_3d_mapping_rviz_plugins/polypartition.h" 00032 #include "rviz/default_plugin/markers/marker_selection_handler.h" 00033 #include "cob_3d_mapping_rviz_plugins/shape_display.h" 00034 00035 #include "rviz/visualization_manager.h" 00036 #include "rviz/selection/selection_manager.h" 00037 00038 #include <OGRE/OgreManualObject.h> 00039 #include <OGRE/OgreSceneManager.h> 00040 #include <OGRE/OgreSceneNode.h> 00041 #include <OGRE/OgreMatrix3.h> 00042 00043 #include <pcl/point_types.h> 00044 #include <pcl/point_cloud.h> 00045 #include <pcl/ros/conversions.h> 00046 #include <pcl/common/transform.h> 00047 00048 #include <sensor_msgs/PointCloud2.h> 00049 00050 namespace rviz 00051 { 00052 00053 ShapeMarker::ShapeMarker( ShapeDisplay* owner, VisualizationManager* manager, 00054 Ogre::SceneNode* parent_node ) : 00055 ShapeBase(owner, manager, parent_node), polygon_(0) 00056 { 00057 00058 scene_node_ = manager->getSceneManager()->getRootSceneNode()->createChildSceneNode(); 00059 00060 static int count = 0; 00061 std::stringstream ss; 00062 ss << "Polygon" << count++; 00063 polygon_ = manager->getSceneManager()->createManualObject( ss.str() ); 00064 polygon_->setDynamic( true ); 00065 scene_node_->attachObject( polygon_ ); 00066 } 00067 00068 ShapeMarker::~ShapeMarker() 00069 { 00070 delete polygon_; 00071 } 00072 00073 std::string createMaterialIfNotExists(const float r, const float g, const float b, const float a) 00074 { 00075 char buf[128]; 00076 sprintf(buf, "ShapeColor%f;%f;%f;%f",r,g,b,a); 00077 if(!Ogre::MaterialManager::getSingleton().getByName(buf, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME).isNull()) 00078 return buf; 00079 00080 Ogre::ColourValue color( r,g,b,a ); 00081 Ogre::MaterialPtr mat = Ogre::MaterialManager::getSingleton().create( buf, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME ); 00082 mat->getTechnique(0)->setAmbient(color*0.5/*color * 0.01f*/); 00083 //mat->setDiffuse(color); 00084 mat->getTechnique(0)->setLightingEnabled(true); 00085 mat->setReceiveShadows(false); 00086 //mat->setCullingMode(Ogre::CULL_NONE); 00087 mat->getTechnique(0)->setDiffuse( color ); 00088 00089 if ( color.a < 0.9998 ) 00090 { 00091 mat->getTechnique(0)->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA ); 00092 mat->getTechnique(0)->setDepthWriteEnabled( false ); 00093 } 00094 else 00095 { 00096 mat->getTechnique(0)->setSceneBlending( Ogre::SBT_REPLACE ); 00097 mat->getTechnique(0)->setDepthWriteEnabled( true ); 00098 } 00099 00100 return buf; 00101 } 00102 00103 TPPLPoint MsgToPoint2D(const pcl::PointXYZ &point, const cob_3d_mapping_msgs::Shape::ConstPtr& new_message) { 00104 TPPLPoint pt; 00105 00106 if(new_message->params.size()==4) { 00107 Eigen::Vector3f u,v,normal,origin; 00108 Eigen::Affine3f transformation; 00109 normal(0)=new_message->params[0]; 00110 normal(1)=new_message->params[1]; 00111 normal(2)=new_message->params[2]; 00112 origin(0)=new_message->centroid.x; 00113 origin(1)=new_message->centroid.y; 00114 origin(2)=new_message->centroid.z; 00115 //std::cout << "normal: " << normal << std::endl; 00116 //std::cout << "centroid: " << origin << std::endl; 00117 v = normal.unitOrthogonal (); 00118 u = normal.cross (v); 00119 pcl::getTransformationFromTwoUnitVectorsAndOrigin(v, normal, origin, transformation); 00120 00121 Eigen::Vector3f p3 = transformation*point.getVector3fMap(); 00122 pt.x = p3(0); 00123 pt.y = p3(1); 00124 } 00125 else if(new_message->params.size()==5) { 00126 pt.x=point.x; 00127 pt.y=point.y; 00128 } 00129 00130 return pt; 00131 } 00132 00133 void MsgToPoint3D(const TPPLPoint &pt, const cob_3d_mapping_msgs::Shape::ConstPtr& new_message, Eigen::Vector3f &pos, Eigen::Vector3f &normal) { 00134 if(new_message->params.size()==4) { 00135 Eigen::Vector3f u,v,origin; 00136 Eigen::Affine3f transformation; 00137 normal(0)=new_message->params[0]; 00138 normal(1)=new_message->params[1]; 00139 normal(2)=new_message->params[2]; 00140 origin(0)=new_message->centroid.x; 00141 origin(1)=new_message->centroid.y; 00142 origin(2)=new_message->centroid.z; 00143 //std::cout << "normal: " << normal << std::endl; 00144 //std::cout << "centroid: " << origin << std::endl; 00145 v = normal.unitOrthogonal (); 00146 u = normal.cross (v); 00147 pcl::getTransformationFromTwoUnitVectorsAndOrigin(v, normal, origin, transformation); 00148 00149 transformation=transformation.inverse(); 00150 00151 Eigen::Vector3f p3; 00152 p3(0)=pt.x; 00153 p3(1)=pt.y; 00154 p3(2)=0; 00155 pos = transformation*p3; 00156 } 00157 else if(new_message->params.size()==5) { 00158 Eigen::Vector2f v,v2,n2; 00159 v(0)=pt.x; 00160 v(1)=pt.y; 00161 v2=v; 00162 v2(0)*=v2(0); 00163 v2(1)*=v2(1); 00164 n2(0)=new_message->params[3]; 00165 n2(1)=new_message->params[4]; 00166 00167 //dummy normal 00168 normal(0)=new_message->params[0]; 00169 normal(1)=new_message->params[1]; 00170 normal(2)=new_message->params[2]; 00171 00172 Eigen::Vector3f x,y, origin; 00173 x(0)=1.f; 00174 y(1)=1.f; 00175 x(1)=x(2)=y(0)=y(2)=0.f; 00176 00177 Eigen::Matrix<float,3,2> proj2plane_; 00178 proj2plane_.col(0)=normal.cross(y); 00179 proj2plane_.col(1)=normal.cross(x); 00180 00181 origin(0)=new_message->centroid.x; 00182 origin(1)=new_message->centroid.y; 00183 origin(2)=new_message->centroid.z; 00184 00185 pos = origin+proj2plane_*v + normal*(v2.dot(n2)); 00186 normal += normal*(v).dot(n2); 00187 } 00188 } 00189 00190 Eigen::Vector3f MsgToOrigin(const cob_3d_mapping_msgs::Shape::ConstPtr& new_message) { 00191 Eigen::Vector3f origin; 00192 00193 origin(0)=origin(1)=origin(2)=0.f; 00194 00195 if(new_message->params.size()==4) { 00196 origin(0)=new_message->centroid.x; 00197 origin(1)=new_message->centroid.y; 00198 origin(2)=new_message->centroid.z; 00199 } 00200 00201 return origin; 00202 } 00203 00204 void ShapeMarker::onNewMessage( const MarkerConstPtr& old_message, 00205 const MarkerConstPtr& new_message ) 00206 { 00207 00208 TPPLPartition pp; 00209 list<TPPLPoly> polys,result; 00210 00211 //std::cout << "id: " << new_message->id << std::endl; 00212 //std::cout << new_message->centroid << std::endl; 00213 //fill polys 00214 for(size_t i=0; i<new_message->points.size(); i++) { 00215 pcl::PointCloud<pcl::PointXYZ> pc; 00216 TPPLPoly poly; 00217 00218 pcl::fromROSMsg(new_message->points[i],pc); 00219 00220 poly.Init(pc.size()); 00221 poly.SetHole(new_message->holes[i]); 00222 00223 for(size_t j=0; j<pc.size(); j++) { 00224 poly[j] = MsgToPoint2D(pc[j], new_message); 00225 } 00226 if(new_message->holes[i]) 00227 poly.SetOrientation(TPPL_CW); 00228 else 00229 poly.SetOrientation(TPPL_CCW); 00230 00231 polys.push_back(poly); 00232 } 00233 00234 pp.Triangulate_EC(&polys,&result); 00235 00236 00237 polygon_->clear(); 00238 polygon_->begin(createMaterialIfNotExists(new_message->color.r,new_message->color.g,new_message->color.b,new_message->color.a), Ogre::RenderOperation::OT_TRIANGLE_LIST); 00239 00240 TPPLPoint p1; 00241 00242 for(std::list<TPPLPoly>::iterator it=result.begin(); it!=result.end(); it++) { 00243 //draw each triangle 00244 for(long i=0;i<it->GetNumPoints();i++) { 00245 p1 = it->GetPoint(i); 00246 00247 Eigen::Vector3f p3, normal; 00248 MsgToPoint3D(p1,new_message,p3,normal); 00249 00250 polygon_->position(p3(0),p3(1),p3(2)); // start position 00251 polygon_->normal(normal(0),normal(1),normal(2)); 00252 } 00253 } 00254 00255 for(std::list<TPPLPoly>::iterator it=result.begin(); it!=result.end(); it++) { 00256 //draw each triangle 00257 for(int i=it->GetNumPoints()-1;i>=0;i--) { 00258 p1 = it->GetPoint(i); 00259 00260 Eigen::Vector3f p3, normal; 00261 MsgToPoint3D(p1,new_message,p3,normal); 00262 00263 polygon_->position(p3(0),p3(1),p3(2)); // start position 00264 polygon_->normal(-normal(0),-normal(1),-normal(2)); 00265 } 00266 } 00267 00268 polygon_->end(); 00269 00270 vis_manager_->getSelectionManager()->removeObject(coll_); 00271 /*coll_ = vis_manager_->getSelectionManager()->createCollisionForObject( 00272 shape_, SelectionHandlerPtr(new MarkerSelectionHandler(this, MarkerID( 00273 "fake_ns", new_message->id))), coll_);*/ 00274 00275 Ogre::Vector3 pos, scale, scale_correct; 00276 Ogre::Quaternion orient; 00277 //transform(new_message, pos, orient, scale); 00278 00279 /*if (owner_ && (new_message->scale.x * new_message->scale.y 00280 * new_message->scale.z == 0.0f)) 00281 { 00282 owner_->setMarkerStatus(getID(), status_levels::Warn, 00283 "Scale of 0 in one of x/y/z"); 00284 }*/ 00285 Eigen::Vector3f origin=MsgToOrigin(new_message); 00286 pos.x = origin(0); 00287 pos.y = origin(1); 00288 pos.z = origin(2); 00289 00290 //setPosition(pos); 00291 return; 00292 setOrientation( orient * Ogre::Quaternion( Ogre::Degree(90), Ogre::Vector3(1,0,0) ) ); 00293 00294 //scale_correct = Ogre::Quaternion( Ogre::Degree(90), Ogre::Vector3(1,0,0) ) * scale; 00295 00296 //shape_->setScale(scale_correct); 00297 } 00298 00299 S_MaterialPtr ShapeMarker::getMaterials() 00300 { 00301 S_MaterialPtr materials; 00302 return materials; 00303 } 00304 00305 }