PolygonArrayDisplay::PolygonArrayDisplay()
{
auto_coloring_property_ = new rviz::BoolProperty("auto color", true,
"automatically change the color of the polygons",
this, SLOT(updateAutoColoring()));
color_property_ = new rviz::ColorProperty("Color", QColor(25, 255, 0),
"Color to draw the polygons.",
this, SLOT(queueRender()));
alpha_property_ = new rviz::FloatProperty("Alpha", 1.0,
"Amount of transparency to apply to the polygon.",
this, SLOT(queueRender()));
only_border_property_ = new rviz::BoolProperty("only border", true,
"only shows the borders of polygons",
this, SLOT(updateOnlyBorder()));
show_normal_property_ = new rviz::BoolProperty("show normal", true,
"show normal direction",
this, SLOT(updateShowNormal()));
normal_length_property_ = new rviz::FloatProperty("normal length", 0.1,
"normal length",
this, SLOT(updateNormalLength()));
normal_length_property_->setMin(0);
//only_border_ = true;
alpha_property_->setMin(0);
alpha_property_->setMax(1);
}
void VisualizationManager::handleMouseEvent( const ViewportMouseEvent& vme )
{
//process pending mouse events
Tool* current_tool = tool_manager_->getCurrentTool();
int flags = 0;
if( current_tool )
{
ViewportMouseEvent _vme = vme;
flags = current_tool->processMouseEvent( _vme );
vme.panel->setCursor( current_tool->getCursor() );
}
else
{
vme.panel->setCursor( QCursor( Qt::ArrowCursor ) );
}
if( flags & Tool::Render )
{
queueRender();
}
if( flags & Tool::Finished )
{
tool_manager_->setCurrentTool( tool_manager_->getDefaultTool() );
}
}
PolygonDisplay::PolygonDisplay()
{
color_property_ = new ColorProperty( "Color", QColor( 25, 255, 0 ),
"Color to draw the polygon.", this, SLOT( queueRender() ));
alpha_property_ = new FloatProperty( "Alpha", 1.0,
"Amount of transparency to apply to the polygon.", this, SLOT( queueRender() ));
alpha_property_->setMin( 0 );
alpha_property_->setMax( 1 );
}
void VisualizationManager::resetTime()
{
root_display_group_->reset();
frame_manager_->getTFClient()->clear();
ros_time_begin_ = ros::Time();
wall_clock_begin_ = ros::WallTime();
queueRender();
}
PolygonArrayDisplay::PolygonArrayDisplay()
{
color_property_ = new rviz::ColorProperty( "Color", QColor( 25, 255, 0 ),
"Color to draw the polygons.",
this, SLOT( queueRender() ));
alpha_property_ = new rviz::FloatProperty( "Alpha", 1.0,
"Amount of transparency to apply to the polygon.",
this, SLOT( queueRender() ));
alpha_property_->setMin( 0 );
alpha_property_->setMax( 1 );
static uint32_t count = 0;
std::stringstream ss;
ss << "PolygonArray" << count++;
ss << "Material";
material_name_ = ss.str();
material_ = Ogre::MaterialManager::getSingleton().create(material_name_, "rviz");
material_->setReceiveShadows(false);
material_->getTechnique(0)->setLightingEnabled(true);
material_->getTechnique(0)->setAmbient( 0.5, 0.5, 0.5 );
}
void VisualizationPanel::onPropertyChanged( wxPropertyGridEvent& event )
{
wxPGProperty* property = event.GetProperty();
if ( !property )
{
return;
}
manager_->getPropertyManager()->propertyChanged( event );
queueRender();
}
void NavViewPanel::processParticleCloud()
{
cloud_.lock();
new_cloud_ = false;
if ( !cloud_object_ )
{
static int count = 0;
std::stringstream ss;
ss << "NavViewCloud" << count++;
cloud_object_ = scene_manager_->createManualObject( ss.str() );
Ogre::SceneNode* node = root_node_->createChildSceneNode();
node->attachObject( cloud_object_ );
}
cloud_object_->clear();
Ogre::ColourValue color( 1.0f, 0.0f, 0.0f, 1.0f );
int num_particles = cloud_.get_particles_size();
cloud_object_->estimateVertexCount( num_particles * 8 );
cloud_object_->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST );
for( int i=0; i < num_particles; ++i)
{
Ogre::Vector3 pos( cloud_.particles[i].x, cloud_.particles[i].y, 0.0f );
Ogre::Quaternion orient( Ogre::Quaternion( Ogre::Radian( cloud_.particles[i].th ), Ogre::Vector3::UNIT_Z ) );
Ogre::Vector3 vertices[8];
vertices[0] = pos;
vertices[1] = pos + orient * Ogre::Vector3(ROBOT_RADIUS, 0.0f, 0.0f);
vertices[2] = vertices[1];
vertices[3] = pos + orient * Ogre::Vector3(0.75*ROBOT_RADIUS, -0.25*ROBOT_RADIUS, 0.0f);
vertices[4] = vertices[3];
vertices[5] = pos + orient * Ogre::Vector3(0.75*ROBOT_RADIUS, 0.25*ROBOT_RADIUS, 0.0f);
vertices[6] = vertices[5];
vertices[7] = pos + orient * Ogre::Vector3(ROBOT_RADIUS, 0.0f, 0.0f);
for ( int i = 0; i < 8; ++i )
{
cloud_object_->position( vertices[i] );
cloud_object_->colour( color );
}
}
cloud_object_->end();
cloud_object_->getParentSceneNode()->setPosition( Ogre::Vector3( 0.0f, 0.0f, CLOUD_DEPTH ) );
cloud_.unlock();
queueRender();
}
void NavViewPanel::incomingPoseArray(const geometry_msgs::PoseArray::ConstPtr& msg)
{
if ( !cloud_object_ )
{
static int count = 0;
std::stringstream ss;
ss << "NavViewCloud" << count++;
cloud_object_ = scene_manager_->createManualObject( ss.str() );
Ogre::SceneNode* node = root_node_->createChildSceneNode();
node->attachObject( cloud_object_ );
}
cloud_object_->clear();
Ogre::ColourValue color( 1.0f, 0.0f, 0.0f, 1.0f );
int num_particles = msg->poses.size();
cloud_object_->estimateVertexCount( num_particles * 8 );
cloud_object_->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST );
for( int i=0; i < num_particles; ++i)
{
Ogre::Vector3 pos( msg->poses[i].position.x, msg->poses[i].position.y, msg->poses[i].position.z );
tf::Quaternion orientation;
tf::quaternionMsgToTF(msg->poses[i].orientation, orientation);
double yaw, pitch, roll;
btMatrix3x3(orientation).getEulerYPR(yaw, pitch, roll);
Ogre::Quaternion orient( Ogre::Quaternion( Ogre::Radian( yaw ), Ogre::Vector3::UNIT_Z ) );
Ogre::Vector3 vertices[8];
vertices[0] = pos;
vertices[1] = pos + orient * Ogre::Vector3(ROBOT_RADIUS, 0.0f, 0.0f);
vertices[2] = vertices[1];
vertices[3] = pos + orient * Ogre::Vector3(0.75*ROBOT_RADIUS, -0.25*ROBOT_RADIUS, 0.0f);
vertices[4] = vertices[3];
vertices[5] = pos + orient * Ogre::Vector3(0.75*ROBOT_RADIUS, 0.25*ROBOT_RADIUS, 0.0f);
vertices[6] = vertices[5];
vertices[7] = pos + orient * Ogre::Vector3(ROBOT_RADIUS, 0.0f, 0.0f);
for ( int i = 0; i < 8; ++i )
{
cloud_object_->position( vertices[i] );
cloud_object_->colour( color );
}
}
cloud_object_->end();
cloud_object_->getParentSceneNode()->setPosition( Ogre::Vector3( 0.0f, 0.0f, CLOUD_DEPTH ) );
queueRender();
}
void NavViewPanel::updateRadiusPosition()
{
try
{
tf::Stamped<tf::Pose> robot_pose(btTransform(tf::createIdentityQuaternion(), btVector3(0,0,0)), ros::Time(), "base_link");
tf::Stamped<tf::Pose> map_pose;
tf_client_->transformPose(getGlobalFrame(), robot_pose, map_pose);
double yaw, pitch, roll;
map_pose.getBasis().getEulerYPR(yaw, pitch, roll);
Ogre::SceneNode* node = radius_object_->getParentSceneNode();
node->setPosition( Ogre::Vector3(map_pose.getOrigin().x() - map_origin_x_, map_pose.getOrigin().y() - map_origin_y_, RADIUS_DEPTH) );
node->setOrientation( Ogre::Quaternion( Ogre::Radian( yaw ), Ogre::Vector3::UNIT_Z ) );
queueRender();
}
catch ( tf::TransformException& e )
{
}
}
void NavViewPanel::createObjectFromGridCells(Ogre::ManualObject*& object, const nav_msgs::GridCells& cells, const Ogre::ColourValue& color, float depth)
{
if ( !object )
{
static int count = 0;
std::stringstream ss;
ss << "NavViewGridCells" << count++;
object = scene_manager_->createManualObject( ss.str() );
Ogre::SceneNode* node = root_node_->createChildSceneNode();
node->attachObject( object );
}
object->clear();
size_t num_points = cells.cells.size();
if ( num_points > 0 )
{
object->estimateVertexCount( num_points);
object->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_POINT_LIST );
for( size_t i=0; i < num_points; ++i)
{
tf::Stamped<tf::Point> point;
tf::pointMsgToTF(cells.cells[i], point);
point.frame_id_ = cells.header.frame_id;
point.stamp_ = cells.header.stamp;
tf_client_->transformPoint(global_frame_id_, point, point);
object->position(point.x() - map_origin_x_, point.y() - map_origin_y_, point.z());
object->colour( color );
}
object->end();
object->getParentSceneNode()->setPosition( Ogre::Vector3( 0.0f, 0.0f, depth ) );
}
queueRender();
}
void VisualizationPanel::onRenderWindowMouseEvents( wxMouseEvent& event )
{
int last_x = mouse_x_;
int last_y = mouse_y_;
mouse_x_ = event.GetX();
mouse_y_ = event.GetY();
render_panel_->SetFocus();
int flags = manager_->getCurrentTool()->processMouseEvent( event, last_x, last_y );
if ( flags & Tool::Render )
{
queueRender();
}
if ( flags & Tool::Finished )
{
manager_->setCurrentTool( manager_->getDefaultTool() );
}
}
void NavViewPanel::onRenderWindowMouseEvents( wxMouseEvent& event )
{
int last_x = mouse_x_;
int last_y = mouse_y_;
mouse_x_ = event.GetX();
mouse_y_ = event.GetY();
int flags = current_tool_->processMouseEvent( event, last_x, last_y, scale_ );
if ( flags & Tool::Render )
{
render_panel_->setOrthoScale( scale_ );
queueRender();
}
if ( flags & Tool::Finished )
{
delete current_tool_;
current_tool_ = new MoveTool( this );
toolbar_->ToggleTool( ID_MOVE_TOOL, true );
}
}
void VisualizationManager::updateBackgroundColor()
{
render_panel_->setBackgroundColor( qtToOgre( background_color_property_->getColor() ));
queueRender();
}
void NavViewPanel::displayMap(const nav_msgs::OccupancyGrid& map)
{
boost::mutex::scoped_lock lock(map_lock_);
map_resolution_ = map.info.resolution;
// Pad dimensions to power of 2
map_width_ = map.info.width;//(int)pow(2,ceil(log2(map.info.width)));
map_height_ = map.info.height;//(int)pow(2,ceil(log2(map.info.height)));
map_origin_x_ = map.info.origin.position.x;
map_origin_y_ = map.info.origin.position.y;
//ROS_INFO("Padded dimensions to %d X %d\n", map_width_, map_height_);
// Expand it to be RGB data
int pixels_size = map_width_ * map_height_ * 3;
unsigned char* pixels = new unsigned char[pixels_size];
memset(pixels, 255, pixels_size);
for(unsigned int j=0;j<map.info.height;j++)
{
for(unsigned int i=0;i<map.info.width;i++)
{
unsigned char val;
if(map.data[j*map.info.width+i] == 100)
val = 0;
else if(map.data[j*map.info.width+i] == 0)
val = 255;
else
val = 127;
int pidx = 3*(j*map_width_ + i);
pixels[pidx+0] = val;
pixels[pidx+1] = val;
pixels[pidx+2] = val;
}
}
Ogre::DataStreamPtr pixel_stream;
pixel_stream.bind(new Ogre::MemoryDataStream( pixels, pixels_size ));
static int tex_count = 0;
std::stringstream ss;
ss << "NavViewMapTexture" << tex_count++;
try
{
map_texture_ = Ogre::TextureManager::getSingleton().loadRawData( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
pixel_stream, map_width_, map_height_, Ogre::PF_BYTE_RGB, Ogre::TEX_TYPE_2D,
0);
}
catch(Ogre::RenderingAPIException&)
{
Ogre::Image image;
pixel_stream->seek(0);
float width = map_width_;
float height = map_height_;
if (map_width_ > map_height_)
{
float aspect = height / width;
width = 2048;
height = width * aspect;
}
else
{
float aspect = width / height;
height = 2048;
width = height * aspect;
}
ROS_WARN("Failed to create full-size map texture, likely because your graphics card does not support textures of size > 2048. Downsampling to [%d x %d]...", (int)width, (int)height);
image.loadRawData(pixel_stream, map_width_, map_height_, Ogre::PF_BYTE_RGB);
image.resize(width, height, Ogre::Image::FILTER_NEAREST);
ss << "Downsampled";
map_texture_ = Ogre::TextureManager::getSingleton().loadImage(ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, image);
}
delete [] pixels;
Ogre::SceneNode* map_node = NULL;
if ( !map_object_ )
{
static int map_count = 0;
std::stringstream ss;
ss << "NavViewMapObject" << map_count++;
map_object_ = scene_manager_->createManualObject( ss.str() );
map_node = root_node_->createChildSceneNode();
map_node->attachObject( map_object_ );
ss << "Material";
map_material_ = Ogre::MaterialManager::getSingleton().create( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME );
map_material_->setReceiveShadows(false);
map_material_->getTechnique(0)->setLightingEnabled(false);
}
else
{
map_node = map_object_->getParentSceneNode();
}
Ogre::Pass* pass = map_material_->getTechnique(0)->getPass(0);
Ogre::TextureUnitState* tex_unit = NULL;
if (pass->getNumTextureUnitStates() > 0)
{
tex_unit = pass->getTextureUnitState(0);
}
else
{
tex_unit = pass->createTextureUnitState();
}
tex_unit->setTextureName(map_texture_->getName());
tex_unit->setTextureFiltering( Ogre::TFO_NONE );
map_object_->clear();
map_object_->begin(map_material_->getName(), Ogre::RenderOperation::OT_TRIANGLE_LIST);
{
// First triangle
{
// Top left
map_object_->position( 0.0f, 0.0f, 0.0f );
map_object_->textureCoord(0.0f, 0.0f);
map_object_->normal( 0.0f, 0.0f, 1.0f );
// Bottom right
map_object_->position( map_resolution_*map_width_, map_resolution_*map_height_, 0.0f );
map_object_->textureCoord(1.0f, 1.0f);
map_object_->normal( 0.0f, 0.0f, 1.0f );
// Bottom left
map_object_->position( 0.0f, map_resolution_*map_height_, 0.0f );
map_object_->textureCoord(0.0f, 1.0f);
map_object_->normal( 0.0f, 0.0f, 1.0f );
}
// Second triangle
{
// Top left
map_object_->position( 0.0f, 0.0f, 0.0f );
map_object_->textureCoord(0.0f, 0.0f);
map_object_->normal( 0.0f, 0.0f, 1.0f );
// Top right
map_object_->position( map_resolution_*map_width_, 0.0f, 0.0f );
map_object_->textureCoord(1.0f, 0.0f);
map_object_->normal( 0.0f, 0.0f, 1.0f );
// Bottom right
map_object_->position( map_resolution_*map_width_, map_resolution_*map_height_, 0.0f );
map_object_->textureCoord(1.0f, 1.0f);
map_object_->normal( 0.0f, 0.0f, 1.0f );
}
}
map_object_->end();
//only center the position of the map if its the first received
if(first_map_){
root_node_->setPosition(Ogre::Vector3(-map_width_*map_resolution_/2, -map_height_*map_resolution_/2, 0.0f));
first_map_ = false;
}
map_node->setPosition(Ogre::Vector3(0.0f, 0.0f, MAP_DEPTH));
queueRender();
}