// This is our callback to handle an incoming message.
void MoveItCollisionMapDisplay::processMessage(const moveit_msgs::CollisionMap::ConstPtr& msg) {
// Here we call the rviz::FrameManager to get the transform from the
// fixed frame to the frame in the header of this Imu message. If
// it fails, we can't do anything else so we return.
Ogre::Quaternion orientation;
Ogre::Vector3 position;
if(!context_->getFrameManager()->getTransform(msg->header.frame_id,
msg->header.stamp,
position, orientation)) {
ROS_DEBUG("Error transforming from frame '%s' to frame '%s'",
msg->header.frame_id.c_str(), qPrintable(fixed_frame_));
return;
}
// We are keeping a circular buffer of visual pointers. This gets
// the next one, or creates and stores it if the buffer is not full
visuals_.clear();
boost::shared_ptr<MoveItCollisionMapVisual> visual;
vector<moveit_msgs::OrientedBoundingBox> vecBoxes = msg->boxes;
for(vector<moveit_msgs::OrientedBoundingBox>::iterator itBox = vecBoxes.begin();
itBox != vecBoxes.end();
itBox++) {
moveit_msgs::OrientedBoundingBox bxBox = *itBox;
const geometry_msgs::Point& ptPosition = bxBox.pose.position;
const geometry_msgs::Point32& ptExtents = bxBox.extents;
Ogre::Vector3 vecPosition(ptPosition.x, ptPosition.y, ptPosition.z);
Ogre::Vector3 vecExtents(ptExtents.x - 0.01, ptExtents.y - 0.01, ptExtents.z - 0.01);
//visual->setFramePosition(vecPosition);
//cube_->setPosition(vecPosition);
//cube_->setScale(vecExtents);
if(visuals_.full()) {
visual = visuals_.front();
} else {
visual.reset(new MoveItCollisionMapVisual(context_->getSceneManager(), scene_node_));
}
// Now set or update the contents of the chosen visual.
visual->setMessage(msg);
visual->setFramePosition(vecPosition);
visual->setExtents(vecExtents);
float alpha = alpha_property_->getFloat();
Ogre::ColourValue color = color_property_->getOgreColor();
visual->setColor(color.r, color.g, color.b, alpha);
// And send it to the end of the circular buffer
// NOTE(winkler): Check here if this cube is already in the buffer.
visuals_.push_back(visual);
}
}
//-----------------------------------------------------------------------------
// Name: Render()
// Desc: Called once per frame, the call is the entry point for 3d
// rendering. This function sets up render states, clears the
// viewport, and renders the scene.
//-----------------------------------------------------------------------------
HRESULT CMyD3DApplication::Render()
{
// Begin the scene
if( SUCCEEDED( m_pd3dDevice->BeginScene() ) )
{
// Render the Skybox
{
D3DXMATRIX matWorld;
D3DXMatrixScaling( &matWorld, 10.0f, 10.0f, 10.0f );
D3DXMATRIX matView(m_matView);
matView._41 = matView._42 = matView._43 = 0.0f;
m_pd3dDevice->SetTransform( D3DTS_WORLD, &matWorld );
m_pd3dDevice->SetTransform( D3DTS_VIEW, &matView );
m_pd3dDevice->SetTransform( D3DTS_PROJECTION, &m_matProject );
m_pd3dDevice->SetTextureStageState( 0, D3DTSS_COLORARG1, D3DTA_TEXTURE );
m_pd3dDevice->SetTextureStageState( 0, D3DTSS_COLOROP, D3DTOP_SELECTARG1 );
m_pd3dDevice->SetTextureStageState( 0, D3DTSS_MINFILTER, D3DTEXF_LINEAR );
m_pd3dDevice->SetTextureStageState( 0, D3DTSS_MAGFILTER, D3DTEXF_LINEAR );
m_pd3dDevice->SetTextureStageState( 0, D3DTSS_ADDRESSU, D3DTADDRESS_MIRROR );
m_pd3dDevice->SetTextureStageState( 0, D3DTSS_ADDRESSV, D3DTADDRESS_MIRROR );
// Always pass Z-test, so we can avoid clearing color and depth buffers
m_pd3dDevice->SetRenderState( D3DRS_ZFUNC, D3DCMP_ALWAYS );
m_pSkyBox->Render( m_pd3dDevice );
m_pd3dDevice->SetRenderState( D3DRS_ZFUNC, D3DCMP_LESSEQUAL );
}
// Render the environment-mapped ShinyTeapot
{
// Set transform state
D3DXMATRIX matViewProject;
D3DXMatrixMultiply( &matViewProject, &m_matView, &m_matProject );
D3DXMATRIX matViewInv;
D3DXMatrixInverse( &matViewInv, NULL, &m_matView );
D3DXVECTOR4 vecPosition( matViewInv._41, matViewInv._42, matViewInv._43, 1.0f );
m_pEffect->SetMatrix( "matWorld", &m_matWorld );
m_pEffect->SetMatrix( "matViewProject", &matViewProject );
m_pEffect->SetVector( "vecPosition", &vecPosition );
// Draw teapot
LPDIRECT3DVERTEXBUFFER8 pVB;
LPDIRECT3DINDEXBUFFER8 pIB;
m_pShinyTeapot->m_pLocalMesh->GetVertexBuffer( &pVB );
m_pShinyTeapot->m_pLocalMesh->GetIndexBuffer( &pIB );
m_pd3dDevice->SetStreamSource( 0, pVB, sizeof(ENVMAPPEDVERTEX) );
m_pd3dDevice->SetIndices( pIB, 0 );
UINT uPasses;
m_pEffect->Begin( &uPasses, 0 );
for( UINT iPass = 0; iPass < uPasses; iPass++ )
{
m_pEffect->Pass( iPass );
m_pd3dDevice->DrawIndexedPrimitive( D3DPT_TRIANGLELIST,
0, m_pShinyTeapot->m_pLocalMesh->GetNumVertices(),
0, m_pShinyTeapot->m_pLocalMesh->GetNumFaces() );
}
m_pEffect->End();
SAFE_RELEASE( pVB );
SAFE_RELEASE( pIB );
}
// Output statistics
m_pFont->DrawText( 2, 0, D3DCOLOR_ARGB(255,255,255,0), m_strFrameStats );
m_pFont->DrawText( 2, 20, D3DCOLOR_ARGB(255,255,255,0), m_strDeviceStats );
// End the scene.
m_pd3dDevice->EndScene();
}
return S_OK;
}
