73 QString::fromStdString(ros::message_traits::datatype<mesh_msgs::TriangleMeshStamped>()),
74 "Mesh topic to subscribe to.",
83 for (std::map<
size_t, std::vector<size_t> >::iterator it =
m_goalFaces.begin();
119 Ogre::SceneNode* rootNode =
scene_manager->getRootSceneNode();
120 scene_node = rootNode->createChildSceneNode();
129 .create(
"ReferenceMeshMaterial2", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
true);
131 ref_tech->removeAllPasses();
133 Ogre::Pass* ref_pass = ref_tech->createPass();
136 ref_pass->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
137 ref_pass->setDepthWriteEnabled(
false);
138 ref_pass->setPolygonMode(Ogre::PM_SOLID);
139 ref_pass->setCullingMode(Ogre::CULL_NONE);
147 .create(
"SegmentMatrial2", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
true);
149 seg_tech->removeAllPasses();
150 Ogre::Pass* seg_pass = seg_tech->createPass();
154 seg_pass->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
155 seg_pass->setDepthWriteEnabled(
false);
156 seg_pass->setPolygonMode(Ogre::PM_SOLID);
157 seg_pass->setCullingMode(Ogre::CULL_NONE);
189 Ogre::Quaternion orientation;
190 Ogre::Vector3 position;
194 position, orientation))
196 ROS_ERROR(
"Error transforming from frame '%s' to the fixed_frame",
197 mesh.header.frame_id.c_str());
208 reference_mesh->begin(
"ReferenceMeshMaterial2", Ogre::RenderOperation::OT_TRIANGLE_LIST);
210 for (
size_t i = 0; i < mesh.vertices.size(); i++ )
212 reference_mesh->position(mesh.vertices[i].x, mesh.vertices[i].y, mesh.vertices[i].z);
215 for (
size_t i = 0; i < mesh.triangles.size(); i++ )
217 reference_mesh->triangle( mesh.triangles[i].vertex_indices[0], mesh.triangles[i].vertex_indices[1], mesh.triangles[i].vertex_indices[2] );
229 Ogre::Vector3* vertices;
231 unsigned long* indices;
238 geometry_msgs::Point vertex;
239 mesh_msgs::TriangleIndices index;
240 for (
size_t i = 0; i < vertexCount; i++ )
242 vertex.x = vertices[i].x;
243 vertex.y = vertices[i].y;
244 vertex.z = vertices[i].z;
245 mesh.vertices.push_back(vertex);
248 for (
size_t i = 0; i < indexCount; i+=3 )
250 index.vertex_indices[0] = indices[i];
251 index.vertex_indices[1] = indices[i+1];
252 index.vertex_indices[2] = indices[i+2];
253 mesh.triangles.push_back(index);
261 for (std::map<
size_t, std::vector<size_t> >::iterator it =
m_goalFaces.begin(); it !=
m_goalFaces.end(); it++)
275 size_t facesSize = 0;
276 size_t vertexCount = 0;
277 Ogre::Vector3* vertices;
278 size_t indexCount = 0;
279 unsigned long* indices;
282 segment_mesh->begin(
"SegmentMatrial2", Ogre::RenderOperation::OT_TRIANGLE_LIST);
283 for (std::map<
size_t, std::vector<size_t> >::iterator it =
m_goalFaces.begin(); it !=
m_goalFaces.end(); it++)
286 facesSize += it->second.size();
288 for (
size_t j = 0; j < it->second.size(); j++)
290 segment_mesh->position(vertices[indices[it->second[j]]].x,
291 vertices[indices[it->second[j]]].y,
292 vertices[indices[it->second[j]]].z);
293 segment_mesh->position(vertices[indices[it->second[j] + 1]].x,
294 vertices[indices[it->second[j] + 1]].y,
295 vertices[indices[it->second[j] + 1]].z);
296 segment_mesh->position(vertices[indices[it->second[j] + 2]].x,
297 vertices[indices[it->second[j] + 2]].y,
298 vertices[indices[it->second[j] + 2]].z);
304 for (
size_t j = 0; j < facesSize; j++)
314 if (event->key() == Qt::Key_K)
317 for (std::map<
size_t, std::vector<size_t> >::iterator it =
m_goalFaces.begin(); it !=
m_goalFaces.end(); it++)
319 for (
size_t j = 0; j < it->second.size(); j++)
334 if (event->key() == Qt::Key_R)
339 if (event->key() == Qt::Key_T)
394 ray =
event.viewport->getCamera()->getCameraToViewportRay((
float) event.
x / event.
viewport->getActualWidth(),
395 (
float) event.
y / event.
viewport->getActualHeight());
396 Ogre::RaySceneQuery* query =
context_->
getSceneManager()->createRayQuery(ray, Ogre::SceneManager::WORLD_GEOMETRY_TYPE_MASK);
399 Ogre::RaySceneQueryResult &results = query->getLastResults();
401 for (
size_t i = 0; i < results.size(); i++)
404 Ogre::ManualObject* mesh =
static_cast<Ogre::ManualObject*
>(results[i].movable);
406 if (mesh->getName().find(
"ReferenceMesh") != std::string::npos)
417 size_t goalSection = -1 ;
418 size_t goalIndex = -1;
419 Ogre::Real dist = -1;
428 std_msgs::Int32 index;
429 index.data = goalIndex / 3;
433 Ogre::Vector3 goal = ray.getPoint(dist);
434 geometry_msgs::PoseStamped goal_msg;
437 goal_msg.pose.position.x = goal.x;
438 goal_msg.pose.position.y = goal.y;
439 goal_msg.pose.position.z = goal.z;
445 std::vector<size_t> faces;
446 m_goalFaces.insert( std::pair<
size_t, std::vector<size_t> >( goalSection, faces ) );
461 size_t goalSection = -1 ;
462 size_t goalIndex = -1;
463 Ogre::Real dist = -1;
492 Ogre::Real& closestDistance)
494 closestDistance = -1.0f;
495 size_t vertexCount = 0;
496 Ogre::Vector3* vertices;
497 size_t indexCount = 0;
498 unsigned long* indices;
499 size_t numSections = mesh->getNumSections();
501 for (
size_t i = 0; i < numSections; i++)
506 for (
size_t j = 0; j < indexCount; j += 3)
508 std::pair<bool, Ogre::Real> goal =
509 Ogre::Math::intersects(
511 vertices[indices[j]],
512 vertices[indices[j + 1]],
513 vertices[indices[j + 2]],
519 if ((closestDistance < 0.0
f) || (goal.second < closestDistance))
521 closestDistance = goal.second;
535 size_t sectionNumber,
537 Ogre::Vector3* &vertices,
539 unsigned long* &indices)
541 Ogre::VertexData* vertexData;
542 const Ogre::VertexElement* vertexElement;
543 Ogre::HardwareVertexBufferSharedPtr vertexBuffer;
544 unsigned char* vertexChar;
547 vertexData = mesh->getSection(sectionNumber)->getRenderOperation()->vertexData;
548 vertexElement = vertexData->vertexDeclaration->findElementBySemantic(Ogre::VES_POSITION);
549 vertexBuffer = vertexData->vertexBufferBinding->getBuffer(vertexElement->getSource());
550 vertexChar =
static_cast<unsigned char*
>(vertexBuffer->lock(Ogre::HardwareBuffer::HBL_READ_ONLY));
552 vertexCount = vertexData->vertexCount;
553 vertices =
new Ogre::Vector3[vertexCount];
555 for (
size_t i = 0; i < vertexCount; i++, vertexChar += vertexBuffer->getVertexSize())
557 vertexElement->baseVertexPointerToElement(vertexChar, &vertexFloat);
558 vertices[i] = (mesh->getParentNode()->_getDerivedOrientation() *
559 (Ogre::Vector3(vertexFloat[0], vertexFloat[1], vertexFloat[2]) * mesh->getParentNode()->_getDerivedScale())) +
560 mesh->getParentNode()->_getDerivedPosition();
563 vertexBuffer->unlock();
565 Ogre::IndexData* indexData;
566 Ogre::HardwareIndexBufferSharedPtr indexBuffer;
567 indexData = mesh->getSection(sectionNumber)->getRenderOperation()->indexData;
568 indexCount = indexData->indexCount;
569 indices =
new unsigned long[indexCount];
570 indexBuffer = indexData->indexBuffer;
571 unsigned int* pLong =
static_cast<unsigned int*
>(indexBuffer->lock(Ogre::HardwareBuffer::HBL_READ_ONLY));
572 unsigned short* pShort =
reinterpret_cast<unsigned short*
>(pLong);
574 for (
size_t i = 0; i < indexCount; i++)
577 if (indexBuffer->getType() == Ogre::HardwareIndexBuffer::IT_32BIT)
579 index =
static_cast<unsigned long>(pLong[i]);
584 index =
static_cast<unsigned long>(pShort[i]);
589 indexBuffer->unlock();