Player::~Player(void)
{
destroyObjects();
delete soundMgr;
delete mFireFlash;
}
void MapGraphDisplay::processMessage( const rtabmap_ros::MapData::ConstPtr& msg )
{
if(!(msg->graph.mapIds.size() == msg->graph.nodeIds.size() && msg->graph.poses.size() == msg->graph.nodeIds.size()))
{
ROS_ERROR("rtabmap_ros::MapGraph: Error map ids, pose ids and poses must have all the same size.");
return;
}
// Find all graphs
std::map<int, std::map<int, geometry_msgs::Point> > graphs;
for(unsigned int i=0; i<msg->graph.poses.size(); ++i)
{
std::map<int, std::map<int, geometry_msgs::Point> >::iterator iter = graphs.find(msg->graph.mapIds[i]);
if(iter == graphs.end())
{
iter = graphs.insert(std::make_pair(msg->graph.mapIds[i], std::map<int, geometry_msgs::Point>())).first;
}
iter->second.insert(std::make_pair(msg->graph.nodeIds[i], msg->graph.poses[i].position));
}
destroyObjects();
Ogre::Vector3 position;
Ogre::Quaternion orientation;
if( !context_->getFrameManager()->getTransform( msg->header, position, orientation ))
{
ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
}
Ogre::Matrix4 transform( orientation );
transform.setTrans( position );
Ogre::ColourValue color = color_property_->getOgreColor();
color.a = alpha_property_->getFloat();
for(std::map<int, std::map<int, geometry_msgs::Point> >::iterator iter=graphs.begin(); iter!=graphs.end(); ++iter)
{
uint32_t num_points = iter->second.size();
if(num_points > 0)
{
Ogre::ManualObject* manual_object = scene_manager_->createManualObject();
manual_object->setDynamic( true );
scene_node_->attachObject( manual_object );
manual_objects_.push_back(manual_object);
manual_object->estimateVertexCount( num_points );
manual_object->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP );
for( std::map<int, geometry_msgs::Point>::iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
{
const geometry_msgs::Point& pos = jter->second;
Ogre::Vector3 xpos = transform * Ogre::Vector3( pos.x, pos.y, pos.z );
manual_object->position( xpos.x, xpos.y, xpos.z );
manual_object->colour( color );
}
manual_object->end();
}
}
}
void MapView::unload()
{
if (mIsLoaded)
{
mIsLoaded = false;
destroyObjects();
destroyTerrain();
Ogre::Root::getSingleton().destroySceneManager(mManager);
}
}
int main(int argc, char **argv){
glutInit(&argc, argv); // Initialise GL environment
setup(); // Call additional initialisation commands
glutDisplayFunc(draw); // Register scene to render contents of draw() function
glutIdleFunc(draw);
glutReshapeFunc(reshape);
checkGLError(); // Check any OpenGL errors in initialisation
glutKeyboardFunc(keyPressed); // ASCII key handling
glutSpecialFunc(keyPressed); // Coded key handling
glutMainLoop(); // Begin rendering sequence
// Upon exit
destroyObjects();
return 0;
}
void PathDisplay::updateBufferLength()
{
destroyObjects();
int buffer_length = buffer_length_property_->getInt();
QColor color = color_property_->getColor();
manual_objects_.resize( buffer_length );
for( size_t i = 0; i < manual_objects_.size(); i++ )
{
Ogre::ManualObject* manual_object = scene_manager_->createManualObject();
manual_object->setDynamic( true );
scene_node_->attachObject( manual_object );
manual_objects_[ i ] = manual_object;
}
}
void PathDisplay::updateBufferLength()
{
// Delete old path objects
destroyObjects();
// Read options
int buffer_length = buffer_length_property_->getInt();
LineStyle style = (LineStyle) style_property_->getOptionInt();
// Create new path objects
switch(style)
{
case LINES: // simple lines with fixed width of 1px
manual_objects_.resize( buffer_length );
for( size_t i = 0; i < manual_objects_.size(); i++ )
{
Ogre::ManualObject* manual_object = scene_manager_->createManualObject();
manual_object->setDynamic( true );
scene_node_->attachObject( manual_object );
manual_objects_[ i ] = manual_object;
}
break;
case BILLBOARDS: // billboards with configurable width
billboard_lines_.resize( buffer_length );
for( size_t i = 0; i < billboard_lines_.size(); i++ )
{
rviz::BillboardLine* billboard_line = new rviz::BillboardLine(scene_manager_, scene_node_);
billboard_lines_[ i ] = billboard_line;
}
break;
}
}
void MapGraphDisplay::processMessage( const rtabmap_ros::MapGraph::ConstPtr& msg )
{
if(!(msg->poses.size() == msg->posesId.size()))
{
ROS_ERROR("rtabmap_ros::MapGraph: Error pose ids and poses must have all the same size.");
return;
}
// Get links
std::map<int, rtabmap::Transform> poses;
std::multimap<int, rtabmap::Link> links;
rtabmap::Transform mapToOdom;
rtabmap_ros::mapGraphFromROS(*msg, poses, links, mapToOdom);
destroyObjects();
Ogre::Vector3 position;
Ogre::Quaternion orientation;
if( !context_->getFrameManager()->getTransform( msg->header, position, orientation ))
{
ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
}
Ogre::Matrix4 transform( orientation );
transform.setTrans( position );
if(links.size())
{
Ogre::ColourValue color;
Ogre::ManualObject* manual_object = scene_manager_->createManualObject();
manual_object->setDynamic( true );
scene_node_->attachObject( manual_object );
manual_objects_.push_back(manual_object);
manual_object->estimateVertexCount(links.size() * 2);
manual_object->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST );
for(std::map<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
{
std::map<int, rtabmap::Transform>::iterator poseIterFrom = poses.find(iter->second.from());
std::map<int, rtabmap::Transform>::iterator poseIterTo = poses.find(iter->second.to());
if(poseIterFrom != poses.end() && poseIterTo != poses.end())
{
if(iter->second.type() == rtabmap::Link::kNeighbor)
{
color = color_neighbor_property_->getOgreColor();
}
else if(iter->second.type() == rtabmap::Link::kVirtualClosure)
{
color = color_virtual_property_->getOgreColor();
}
else if(iter->second.type() == rtabmap::Link::kUserClosure)
{
color = color_user_property_->getOgreColor();
}
else if(iter->second.type() == rtabmap::Link::kLocalSpaceClosure || iter->second.type() == rtabmap::Link::kLocalTimeClosure)
{
color = color_local_property_->getOgreColor();
}
else
{
color = color_global_property_->getOgreColor();
}
color.a = alpha_property_->getFloat();
Ogre::Vector3 pos;
pos = transform * Ogre::Vector3( poseIterFrom->second.x(), poseIterFrom->second.y(), poseIterFrom->second.z() );
manual_object->position( pos.x, pos.y, pos.z );
manual_object->colour( color );
pos = transform * Ogre::Vector3( poseIterTo->second.x(), poseIterTo->second.y(), poseIterTo->second.z() );
manual_object->position( pos.x, pos.y, pos.z );
manual_object->colour( color );
}
}
manual_object->end();
}
}
void MapGraphDisplay::reset()
{
MFDClass::reset();
destroyObjects();
}
void MapGraphDisplay::onInitialize()
{
MFDClass::onInitialize();
destroyObjects();
}
MapGraphDisplay::~MapGraphDisplay()
{
destroyObjects();
}
PathDisplay::~PathDisplay()
{
destroyObjects();
}
void pressNormalKeys(unsigned char key, int x, int y)
{
if(key == 27) // ESC
{
destroyObjects();
exit(0);
}
else if(key == '1')
{
glPolygonMode(GL_FRONT_AND_BACK, GL_POINT);
}
else if(key == '2')
{
glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
}
else if(key == '3')
{
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
}
else if(key == '4'){
turnLinearFogOn();
FOG_ON = 1;
}
else if(key == '5'){
turnExpFogOn();
FOG_ON = 1;
}
else if(key == '6'){
turnExp2FogOn();
FOG_ON = 1;
}
else if(key == '7'){
glDisable(GL_FOG);
FOG_ON = 0;
}
else if((key == 'A') || (key == 'a'))
{
_camera->deltaAnglev = 0.02f;
}
else if((key == 'Z') || (key == 'z'))
{
_camera->deltaAnglev = -0.02f;
}
else if((key == 'S') || (key == 's'))
{
_camera->deltaSpeed = 0.01f;
}
else if((key == 'X') || (key == 'x'))
{
_camera->deltaSpeed = -0.01f;
}
else if((key == 'N') || (key == 'n'))
{
glutLeaveGameMode();
OGLWindow_initWindowMode();
// Inicializações
init();
initCamera();
initLights();
initGameTime();
SSController_Initialize();
// Registar funções de callback
glutDisplayFunc(display);
glutReshapeFunc(reshape);
glutIdleFunc(display);
glutIgnoreKeyRepeat(1);
glutKeyboardFunc(pressNormalKeys);
glutKeyboardUpFunc(releaseNormalKeys);
glutSpecialFunc(pressSpecialKey);
glutSpecialUpFunc(releaseSpecialKey);
glutMotionFunc(mouseMove);
// - Directly redirect GLUT mouse button events to AntTweakBar
glutMouseFunc((GLUTmousebuttonfun)TwEventMouseButtonGLUT);
// - Directly redirect GLUT mouse "passive" motion events to AntTweakBar (same as MouseMotion)
glutPassiveMotionFunc((GLUTmousemotionfun)TwEventMouseMotionGLUT);
}
else if((key == 'F') || (key == 'f'))
{
OGLWindow_initGameMode();
// Inicializações
init();
initCamera();
initLights();
initGameTime();
SSController_Initialize();
// Registar funções de callback
glutDisplayFunc(display);
glutReshapeFunc(reshape);
glutIdleFunc(display);
glutIgnoreKeyRepeat(1);
glutKeyboardFunc(pressNormalKeys);
glutKeyboardUpFunc(releaseNormalKeys);
glutSpecialFunc(pressSpecialKey);
glutSpecialUpFunc(releaseSpecialKey);
glutMotionFunc(mouseMove);
// - Directly redirect GLUT mouse button events to AntTweakBar
glutMouseFunc((GLUTmousebuttonfun)TwEventMouseButtonGLUT);
// - Directly redirect GLUT mouse "passive" motion events to AntTweakBar (same as MouseMotion)
glutPassiveMotionFunc((GLUTmousemotionfun)TwEventMouseMotionGLUT);
} else if(key=='U' || key=='u') {
TimeScale -= 10.0f;
} else if(key=='I' || key=='i') {
TimeScale += 10.0f;
} else if(key=='O' || key=='o') {
Planet_setOrbitVisibility(!Planet_getOrbitVisibility());
orbits = !orbits;
} else if(key=='C' || key=='c') {
manualCam = !manualCam;
}
}
