mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 08:47:45 +08:00
fixed RVIZ crash when changing map cloud style
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1457 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
+272
-270
@@ -27,23 +27,23 @@ namespace rtabmap
|
|||||||
{
|
{
|
||||||
|
|
||||||
|
|
||||||
MapCloudDisplay::CloudInfo::CloudInfo()
|
MapCloudDisplay::CloudInfo::CloudInfo() :
|
||||||
: manager_(0)
|
manager_(0),
|
||||||
, scene_node_(0)
|
scene_node_(0)
|
||||||
{}
|
{}
|
||||||
|
|
||||||
MapCloudDisplay::CloudInfo::~CloudInfo()
|
MapCloudDisplay::CloudInfo::~CloudInfo()
|
||||||
{
|
{
|
||||||
clear();
|
clear();
|
||||||
}
|
}
|
||||||
|
|
||||||
void MapCloudDisplay::CloudInfo::clear()
|
void MapCloudDisplay::CloudInfo::clear()
|
||||||
{
|
{
|
||||||
if ( scene_node_ )
|
if ( scene_node_ )
|
||||||
{
|
{
|
||||||
manager_->destroySceneNode( scene_node_ );
|
manager_->destroySceneNode( scene_node_ );
|
||||||
scene_node_=0;
|
scene_node_=0;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
MapCloudDisplay::MapCloudDisplay()
|
MapCloudDisplay::MapCloudDisplay()
|
||||||
@@ -53,46 +53,46 @@ MapCloudDisplay::MapCloudDisplay()
|
|||||||
//QIcon icon;
|
//QIcon icon;
|
||||||
//this->setIcon(icon);
|
//this->setIcon(icon);
|
||||||
|
|
||||||
style_property_ = new rviz::EnumProperty( "Style", "Flat Squares",
|
style_property_ = new rviz::EnumProperty( "Style", "Flat Squares",
|
||||||
"Rendering mode to use, in order of computational complexity.",
|
"Rendering mode to use, in order of computational complexity.",
|
||||||
this, SLOT( updateStyle() ), this );
|
this, SLOT( updateStyle() ), this );
|
||||||
style_property_->addOption( "Points", rviz::PointCloud::RM_POINTS );
|
style_property_->addOption( "Points", rviz::PointCloud::RM_POINTS );
|
||||||
style_property_->addOption( "Squares", rviz::PointCloud::RM_SQUARES );
|
style_property_->addOption( "Squares", rviz::PointCloud::RM_SQUARES );
|
||||||
style_property_->addOption( "Flat Squares", rviz::PointCloud::RM_FLAT_SQUARES );
|
style_property_->addOption( "Flat Squares", rviz::PointCloud::RM_FLAT_SQUARES );
|
||||||
style_property_->addOption( "Spheres", rviz::PointCloud::RM_SPHERES );
|
style_property_->addOption( "Spheres", rviz::PointCloud::RM_SPHERES );
|
||||||
style_property_->addOption( "Boxes", rviz::PointCloud::RM_BOXES );
|
style_property_->addOption( "Boxes", rviz::PointCloud::RM_BOXES );
|
||||||
|
|
||||||
point_world_size_property_ = new rviz::FloatProperty( "Size (m)", 0.01,
|
point_world_size_property_ = new rviz::FloatProperty( "Size (m)", 0.01,
|
||||||
"Point size in meters.",
|
"Point size in meters.",
|
||||||
this, SLOT( updateBillboardSize() ), this );
|
this, SLOT( updateBillboardSize() ), this );
|
||||||
point_world_size_property_->setMin( 0.0001 );
|
point_world_size_property_->setMin( 0.0001 );
|
||||||
|
|
||||||
point_pixel_size_property_ = new rviz::FloatProperty( "Size (Pixels)", 3,
|
point_pixel_size_property_ = new rviz::FloatProperty( "Size (Pixels)", 3,
|
||||||
"Point size in pixels.",
|
"Point size in pixels.",
|
||||||
this, SLOT( updateBillboardSize() ), this );
|
this, SLOT( updateBillboardSize() ), this );
|
||||||
point_pixel_size_property_->setMin( 1 );
|
point_pixel_size_property_->setMin( 1 );
|
||||||
|
|
||||||
alpha_property_ = new rviz::FloatProperty( "Alpha", 1.0,
|
alpha_property_ = new rviz::FloatProperty( "Alpha", 1.0,
|
||||||
"Amount of transparency to apply to the points. Note that this is experimental and does not always look correct.",
|
"Amount of transparency to apply to the points. Note that this is experimental and does not always look correct.",
|
||||||
this, SLOT( updateAlpha() ), this );
|
this, SLOT( updateAlpha() ), this );
|
||||||
alpha_property_->setMin( 0 );
|
alpha_property_->setMin( 0 );
|
||||||
alpha_property_->setMax( 1 );
|
alpha_property_->setMax( 1 );
|
||||||
|
|
||||||
xyz_transformer_property_ = new rviz::EnumProperty( "Position Transformer", "",
|
xyz_transformer_property_ = new rviz::EnumProperty( "Position Transformer", "",
|
||||||
"Set the transformer to use to set the position of the points.",
|
"Set the transformer to use to set the position of the points.",
|
||||||
this, SLOT( updateXyzTransformer() ), this );
|
this, SLOT( updateXyzTransformer() ), this );
|
||||||
connect( xyz_transformer_property_, SIGNAL( requestOptions( EnumProperty* )),
|
connect( xyz_transformer_property_, SIGNAL( requestOptions( EnumProperty* )),
|
||||||
this, SLOT( setXyzTransformerOptions( EnumProperty* )));
|
this, SLOT( setXyzTransformerOptions( EnumProperty* )));
|
||||||
|
|
||||||
color_transformer_property_ = new rviz::EnumProperty( "Color Transformer", "",
|
color_transformer_property_ = new rviz::EnumProperty( "Color Transformer", "",
|
||||||
"Set the transformer to use to set the color of the points.",
|
"Set the transformer to use to set the color of the points.",
|
||||||
this, SLOT( updateColorTransformer() ), this );
|
this, SLOT( updateColorTransformer() ), this );
|
||||||
connect( color_transformer_property_, SIGNAL( requestOptions( EnumProperty* )),
|
connect( color_transformer_property_, SIGNAL( requestOptions( EnumProperty* )),
|
||||||
this, SLOT( setColorTransformerOptions( EnumProperty* )));
|
this, SLOT( setColorTransformerOptions( EnumProperty* )));
|
||||||
|
|
||||||
// PointCloudCommon sets up a callback queue with a thread for each
|
// PointCloudCommon sets up a callback queue with a thread for each
|
||||||
// instance. Use that for processing incoming messages.
|
// instance. Use that for processing incoming messages.
|
||||||
update_nh_.setCallbackQueue( &cbqueue_ );
|
update_nh_.setCallbackQueue( &cbqueue_ );
|
||||||
}
|
}
|
||||||
|
|
||||||
MapCloudDisplay::~MapCloudDisplay()
|
MapCloudDisplay::~MapCloudDisplay()
|
||||||
@@ -107,37 +107,37 @@ MapCloudDisplay::~MapCloudDisplay()
|
|||||||
|
|
||||||
void MapCloudDisplay::loadTransformers()
|
void MapCloudDisplay::loadTransformers()
|
||||||
{
|
{
|
||||||
std::vector<std::string> classes = transformer_class_loader_->getDeclaredClasses();
|
std::vector<std::string> classes = transformer_class_loader_->getDeclaredClasses();
|
||||||
std::vector<std::string>::iterator ci;
|
std::vector<std::string>::iterator ci;
|
||||||
|
|
||||||
for( ci = classes.begin(); ci != classes.end(); ci++ )
|
for( ci = classes.begin(); ci != classes.end(); ci++ )
|
||||||
{
|
{
|
||||||
const std::string& lookup_name = *ci;
|
const std::string& lookup_name = *ci;
|
||||||
std::string name = transformer_class_loader_->getName( lookup_name );
|
std::string name = transformer_class_loader_->getName( lookup_name );
|
||||||
|
|
||||||
if( transformers_.count( name ) > 0 )
|
if( transformers_.count( name ) > 0 )
|
||||||
{
|
{
|
||||||
ROS_ERROR( "Transformer type [%s] is already loaded.", name.c_str() );
|
ROS_ERROR( "Transformer type [%s] is already loaded.", name.c_str() );
|
||||||
continue;
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
rviz::PointCloudTransformerPtr trans( transformer_class_loader_->createUnmanagedInstance( lookup_name ));
|
rviz::PointCloudTransformerPtr trans( transformer_class_loader_->createUnmanagedInstance( lookup_name ));
|
||||||
trans->init();
|
trans->init();
|
||||||
connect( trans.get(), SIGNAL( needRetransform() ), this, SLOT( causeRetransform() ));
|
connect( trans.get(), SIGNAL( needRetransform() ), this, SLOT( causeRetransform() ));
|
||||||
|
|
||||||
TransformerInfo info;
|
TransformerInfo info;
|
||||||
info.transformer = trans;
|
info.transformer = trans;
|
||||||
info.readable_name = name;
|
info.readable_name = name;
|
||||||
info.lookup_name = lookup_name;
|
info.lookup_name = lookup_name;
|
||||||
|
|
||||||
info.transformer->createProperties( this, rviz::PointCloudTransformer::Support_XYZ, info.xyz_props );
|
info.transformer->createProperties( this, rviz::PointCloudTransformer::Support_XYZ, info.xyz_props );
|
||||||
setPropertiesHidden( info.xyz_props, true );
|
setPropertiesHidden( info.xyz_props, true );
|
||||||
|
|
||||||
info.transformer->createProperties( this, rviz::PointCloudTransformer::Support_Color, info.color_props );
|
info.transformer->createProperties( this, rviz::PointCloudTransformer::Support_Color, info.color_props );
|
||||||
setPropertiesHidden( info.color_props, true );
|
setPropertiesHidden( info.color_props, true );
|
||||||
|
|
||||||
transformers_[ name ] = info;
|
transformers_[ name ] = info;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void MapCloudDisplay::onInitialize()
|
void MapCloudDisplay::onInitialize()
|
||||||
@@ -257,123 +257,127 @@ void MapCloudDisplay::processMessage( const rtabmap::InfoExConstPtr& msg )
|
|||||||
|
|
||||||
void MapCloudDisplay::setPropertiesHidden( const QList<Property*>& props, bool hide )
|
void MapCloudDisplay::setPropertiesHidden( const QList<Property*>& props, bool hide )
|
||||||
{
|
{
|
||||||
for( int i = 0; i < props.size(); i++ )
|
for( int i = 0; i < props.size(); i++ )
|
||||||
{
|
{
|
||||||
props[ i ]->setHidden( hide );
|
props[ i ]->setHidden( hide );
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void MapCloudDisplay::updateTransformers( const sensor_msgs::PointCloud2ConstPtr& cloud )
|
void MapCloudDisplay::updateTransformers( const sensor_msgs::PointCloud2ConstPtr& cloud )
|
||||||
{
|
{
|
||||||
std::string xyz_name = xyz_transformer_property_->getStdString();
|
std::string xyz_name = xyz_transformer_property_->getStdString();
|
||||||
std::string color_name = color_transformer_property_->getStdString();
|
std::string color_name = color_transformer_property_->getStdString();
|
||||||
|
|
||||||
xyz_transformer_property_->clearOptions();
|
xyz_transformer_property_->clearOptions();
|
||||||
color_transformer_property_->clearOptions();
|
color_transformer_property_->clearOptions();
|
||||||
|
|
||||||
// Get the channels that we could potentially render
|
// Get the channels that we could potentially render
|
||||||
typedef std::set<std::pair<uint8_t, std::string> > S_string;
|
typedef std::set<std::pair<uint8_t, std::string> > S_string;
|
||||||
S_string valid_xyz, valid_color;
|
S_string valid_xyz, valid_color;
|
||||||
bool cur_xyz_valid = false;
|
bool cur_xyz_valid = false;
|
||||||
bool cur_color_valid = false;
|
bool cur_color_valid = false;
|
||||||
bool has_rgb_transformer = false;
|
bool has_rgb_transformer = false;
|
||||||
M_TransformerInfo::iterator trans_it = transformers_.begin();
|
M_TransformerInfo::iterator trans_it = transformers_.begin();
|
||||||
M_TransformerInfo::iterator trans_end = transformers_.end();
|
M_TransformerInfo::iterator trans_end = transformers_.end();
|
||||||
for(;trans_it != trans_end; ++trans_it)
|
for(;trans_it != trans_end; ++trans_it)
|
||||||
{
|
{
|
||||||
const std::string& name = trans_it->first;
|
const std::string& name = trans_it->first;
|
||||||
const rviz::PointCloudTransformerPtr& trans = trans_it->second.transformer;
|
const rviz::PointCloudTransformerPtr& trans = trans_it->second.transformer;
|
||||||
uint32_t mask = trans->supports(cloud);
|
uint32_t mask = trans->supports(cloud);
|
||||||
if (mask & rviz::PointCloudTransformer::Support_XYZ)
|
if (mask & rviz::PointCloudTransformer::Support_XYZ)
|
||||||
{
|
{
|
||||||
valid_xyz.insert(std::make_pair(trans->score(cloud), name));
|
valid_xyz.insert(std::make_pair(trans->score(cloud), name));
|
||||||
if (name == xyz_name)
|
if (name == xyz_name)
|
||||||
{
|
{
|
||||||
cur_xyz_valid = true;
|
cur_xyz_valid = true;
|
||||||
}
|
}
|
||||||
xyz_transformer_property_->addOptionStd( name );
|
xyz_transformer_property_->addOptionStd( name );
|
||||||
}
|
}
|
||||||
|
|
||||||
if (mask & rviz::PointCloudTransformer::Support_Color)
|
if (mask & rviz::PointCloudTransformer::Support_Color)
|
||||||
{
|
{
|
||||||
valid_color.insert(std::make_pair(trans->score(cloud), name));
|
valid_color.insert(std::make_pair(trans->score(cloud), name));
|
||||||
if (name == color_name)
|
if (name == color_name)
|
||||||
{
|
{
|
||||||
cur_color_valid = true;
|
cur_color_valid = true;
|
||||||
}
|
}
|
||||||
if (name == "RGB8")
|
if (name == "RGB8")
|
||||||
{
|
{
|
||||||
has_rgb_transformer = true;
|
has_rgb_transformer = true;
|
||||||
}
|
}
|
||||||
color_transformer_property_->addOptionStd( name );
|
color_transformer_property_->addOptionStd( name );
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if( !cur_xyz_valid )
|
if( !cur_xyz_valid )
|
||||||
{
|
{
|
||||||
if( !valid_xyz.empty() )
|
if( !valid_xyz.empty() )
|
||||||
{
|
{
|
||||||
xyz_transformer_property_->setStringStd( valid_xyz.rbegin()->second );
|
xyz_transformer_property_->setStringStd( valid_xyz.rbegin()->second );
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
if( !cur_color_valid )
|
if( !cur_color_valid )
|
||||||
{
|
{
|
||||||
if( !valid_color.empty() )
|
if( !valid_color.empty() )
|
||||||
{
|
{
|
||||||
if (has_rgb_transformer)
|
if (has_rgb_transformer)
|
||||||
{
|
{
|
||||||
color_transformer_property_->setStringStd( "RGB8" );
|
color_transformer_property_->setStringStd( "RGB8" );
|
||||||
} else
|
}
|
||||||
{
|
else
|
||||||
color_transformer_property_->setStringStd( valid_color.rbegin()->second );
|
{
|
||||||
}
|
color_transformer_property_->setStringStd( valid_color.rbegin()->second );
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void MapCloudDisplay::updateAlpha()
|
void MapCloudDisplay::updateAlpha()
|
||||||
{
|
{
|
||||||
for ( unsigned i=0; i<cloud_infos_.size(); i++ )
|
for( std::map<int, CloudInfoPtr>::iterator it = cloud_infos_.begin(); it != cloud_infos_.end(); ++it )
|
||||||
{
|
{
|
||||||
cloud_infos_[i]->cloud_->setAlpha( alpha_property_->getFloat() );
|
it->second->cloud_->setAlpha( alpha_property_->getFloat() );
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void MapCloudDisplay::updateStyle()
|
void MapCloudDisplay::updateStyle()
|
||||||
{
|
{
|
||||||
rviz::PointCloud::RenderMode mode = (rviz::PointCloud::RenderMode) style_property_->getOptionInt();
|
rviz::PointCloud::RenderMode mode = (rviz::PointCloud::RenderMode) style_property_->getOptionInt();
|
||||||
if( mode == rviz::PointCloud::RM_POINTS )
|
if( mode == rviz::PointCloud::RM_POINTS )
|
||||||
{
|
{
|
||||||
point_world_size_property_->hide();
|
point_world_size_property_->hide();
|
||||||
point_pixel_size_property_->show();
|
point_pixel_size_property_->show();
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
point_world_size_property_->show();
|
point_world_size_property_->show();
|
||||||
point_pixel_size_property_->hide();
|
point_pixel_size_property_->hide();
|
||||||
}
|
}
|
||||||
for( unsigned int i = 0; i < cloud_infos_.size(); i++ )
|
for( std::map<int, CloudInfoPtr>::iterator it = cloud_infos_.begin(); it != cloud_infos_.end(); ++it )
|
||||||
{
|
{
|
||||||
cloud_infos_[i]->cloud_->setRenderMode( mode );
|
it->second->cloud_->setRenderMode( mode );
|
||||||
}
|
}
|
||||||
updateBillboardSize();
|
updateBillboardSize();
|
||||||
}
|
}
|
||||||
|
|
||||||
void MapCloudDisplay::updateBillboardSize()
|
void MapCloudDisplay::updateBillboardSize()
|
||||||
{
|
{
|
||||||
rviz::PointCloud::RenderMode mode = (rviz::PointCloud::RenderMode) style_property_->getOptionInt();
|
rviz::PointCloud::RenderMode mode = (rviz::PointCloud::RenderMode) style_property_->getOptionInt();
|
||||||
float size;
|
float size;
|
||||||
if( mode == rviz::PointCloud::RM_POINTS ) {
|
if( mode == rviz::PointCloud::RM_POINTS )
|
||||||
size = point_pixel_size_property_->getFloat();
|
{
|
||||||
} else {
|
size = point_pixel_size_property_->getFloat();
|
||||||
size = point_world_size_property_->getFloat();
|
}
|
||||||
}
|
else
|
||||||
for ( unsigned i=0; i<cloud_infos_.size(); i++ )
|
{
|
||||||
{
|
size = point_world_size_property_->getFloat();
|
||||||
cloud_infos_[i]->cloud_->setDimensions( size, size, size );
|
}
|
||||||
}
|
for( std::map<int, CloudInfoPtr>::iterator it = cloud_infos_.begin(); it != cloud_infos_.end(); ++it )
|
||||||
context_->queueRender();
|
{
|
||||||
|
it->second->cloud_->setDimensions( size, size, size );
|
||||||
|
}
|
||||||
|
context_->queueRender();
|
||||||
}
|
}
|
||||||
|
|
||||||
void MapCloudDisplay::causeRetransform()
|
void MapCloudDisplay::causeRetransform()
|
||||||
@@ -520,160 +524,158 @@ void MapCloudDisplay::reset()
|
|||||||
|
|
||||||
void MapCloudDisplay::updateXyzTransformer()
|
void MapCloudDisplay::updateXyzTransformer()
|
||||||
{
|
{
|
||||||
boost::recursive_mutex::scoped_lock lock( transformers_mutex_ );
|
boost::recursive_mutex::scoped_lock lock( transformers_mutex_ );
|
||||||
if( transformers_.count( xyz_transformer_property_->getStdString() ) == 0 )
|
if( transformers_.count( xyz_transformer_property_->getStdString() ) == 0 )
|
||||||
{
|
{
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
new_xyz_transformer_ = true;
|
new_xyz_transformer_ = true;
|
||||||
causeRetransform();
|
causeRetransform();
|
||||||
}
|
}
|
||||||
|
|
||||||
void MapCloudDisplay::updateColorTransformer()
|
void MapCloudDisplay::updateColorTransformer()
|
||||||
{
|
{
|
||||||
boost::recursive_mutex::scoped_lock lock( transformers_mutex_ );
|
boost::recursive_mutex::scoped_lock lock( transformers_mutex_ );
|
||||||
if( transformers_.count( color_transformer_property_->getStdString() ) == 0 )
|
if( transformers_.count( color_transformer_property_->getStdString() ) == 0 )
|
||||||
{
|
{
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
new_color_transformer_ = true;
|
new_color_transformer_ = true;
|
||||||
causeRetransform();
|
causeRetransform();
|
||||||
}
|
}
|
||||||
|
|
||||||
void MapCloudDisplay::setXyzTransformerOptions( EnumProperty* prop )
|
void MapCloudDisplay::setXyzTransformerOptions( EnumProperty* prop )
|
||||||
{
|
{
|
||||||
fillTransformerOptions( prop, rviz::PointCloudTransformer::Support_XYZ );
|
fillTransformerOptions( prop, rviz::PointCloudTransformer::Support_XYZ );
|
||||||
}
|
}
|
||||||
|
|
||||||
void MapCloudDisplay::setColorTransformerOptions( EnumProperty* prop )
|
void MapCloudDisplay::setColorTransformerOptions( EnumProperty* prop )
|
||||||
{
|
{
|
||||||
fillTransformerOptions( prop, rviz::PointCloudTransformer::Support_Color );
|
fillTransformerOptions( prop, rviz::PointCloudTransformer::Support_Color );
|
||||||
}
|
}
|
||||||
|
|
||||||
void MapCloudDisplay::fillTransformerOptions( rviz::EnumProperty* prop, uint32_t mask )
|
void MapCloudDisplay::fillTransformerOptions( rviz::EnumProperty* prop, uint32_t mask )
|
||||||
{
|
{
|
||||||
prop->clearOptions();
|
prop->clearOptions();
|
||||||
|
|
||||||
if (cloud_infos_.empty())
|
if (cloud_infos_.empty())
|
||||||
{
|
{
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
boost::recursive_mutex::scoped_lock tlock(transformers_mutex_);
|
boost::recursive_mutex::scoped_lock tlock(transformers_mutex_);
|
||||||
|
|
||||||
const sensor_msgs::PointCloud2ConstPtr& msg = cloud_infos_.begin()->second->message_;
|
const sensor_msgs::PointCloud2ConstPtr& msg = cloud_infos_.begin()->second->message_;
|
||||||
|
|
||||||
M_TransformerInfo::iterator it = transformers_.begin();
|
M_TransformerInfo::iterator it = transformers_.begin();
|
||||||
M_TransformerInfo::iterator end = transformers_.end();
|
M_TransformerInfo::iterator end = transformers_.end();
|
||||||
for (; it != end; ++it)
|
for (; it != end; ++it)
|
||||||
{
|
{
|
||||||
const rviz::PointCloudTransformerPtr& trans = it->second.transformer;
|
const rviz::PointCloudTransformerPtr& trans = it->second.transformer;
|
||||||
if ((trans->supports(msg) & mask) == mask)
|
if ((trans->supports(msg) & mask) == mask)
|
||||||
{
|
{
|
||||||
prop->addOption( QString::fromStdString( it->first ));
|
prop->addOption( QString::fromStdString( it->first ));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
rviz::PointCloudTransformerPtr MapCloudDisplay::getXYZTransformer( const sensor_msgs::PointCloud2ConstPtr& cloud )
|
rviz::PointCloudTransformerPtr MapCloudDisplay::getXYZTransformer( const sensor_msgs::PointCloud2ConstPtr& cloud )
|
||||||
{
|
{
|
||||||
boost::recursive_mutex::scoped_lock lock( transformers_mutex_);
|
boost::recursive_mutex::scoped_lock lock( transformers_mutex_);
|
||||||
M_TransformerInfo::iterator it = transformers_.find( xyz_transformer_property_->getStdString() );
|
M_TransformerInfo::iterator it = transformers_.find( xyz_transformer_property_->getStdString() );
|
||||||
if( it != transformers_.end() )
|
if( it != transformers_.end() )
|
||||||
{
|
{
|
||||||
const rviz::PointCloudTransformerPtr& trans = it->second.transformer;
|
const rviz::PointCloudTransformerPtr& trans = it->second.transformer;
|
||||||
if( trans->supports( cloud ) & rviz::PointCloudTransformer::Support_XYZ )
|
if( trans->supports( cloud ) & rviz::PointCloudTransformer::Support_XYZ )
|
||||||
{
|
{
|
||||||
return trans;
|
return trans;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return rviz::PointCloudTransformerPtr();
|
return rviz::PointCloudTransformerPtr();
|
||||||
}
|
}
|
||||||
|
|
||||||
rviz::PointCloudTransformerPtr MapCloudDisplay::getColorTransformer( const sensor_msgs::PointCloud2ConstPtr& cloud )
|
rviz::PointCloudTransformerPtr MapCloudDisplay::getColorTransformer( const sensor_msgs::PointCloud2ConstPtr& cloud )
|
||||||
{
|
{
|
||||||
boost::recursive_mutex::scoped_lock lock( transformers_mutex_ );
|
boost::recursive_mutex::scoped_lock lock( transformers_mutex_ );
|
||||||
M_TransformerInfo::iterator it = transformers_.find( color_transformer_property_->getStdString() );
|
M_TransformerInfo::iterator it = transformers_.find( color_transformer_property_->getStdString() );
|
||||||
if( it != transformers_.end() )
|
if( it != transformers_.end() )
|
||||||
{
|
{
|
||||||
const rviz::PointCloudTransformerPtr& trans = it->second.transformer;
|
const rviz::PointCloudTransformerPtr& trans = it->second.transformer;
|
||||||
if( trans->supports( cloud ) & rviz::PointCloudTransformer::Support_Color )
|
if( trans->supports( cloud ) & rviz::PointCloudTransformer::Support_Color )
|
||||||
{
|
{
|
||||||
return trans;
|
return trans;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return rviz::PointCloudTransformerPtr();
|
return rviz::PointCloudTransformerPtr();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
void MapCloudDisplay::retransform()
|
void MapCloudDisplay::retransform()
|
||||||
{
|
{
|
||||||
boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
|
boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
|
||||||
|
|
||||||
std::map<int, CloudInfoPtr>::iterator it = cloud_infos_.begin();
|
for( std::map<int, CloudInfoPtr>::iterator it = cloud_infos_.begin(); it != cloud_infos_.end(); ++it )
|
||||||
std::map<int, CloudInfoPtr>::iterator end = cloud_infos_.end();
|
{
|
||||||
for (; it != end; ++it)
|
const CloudInfoPtr& cloud_info = it->second;
|
||||||
{
|
transformCloud(cloud_info, false);
|
||||||
const CloudInfoPtr& cloud_info = it->second;
|
cloud_info->cloud_->clear();
|
||||||
transformCloud(cloud_info, false);
|
cloud_info->cloud_->addPoints(&cloud_info->transformed_points_.front(), cloud_info->transformed_points_.size());
|
||||||
cloud_info->cloud_->clear();
|
}
|
||||||
cloud_info->cloud_->addPoints(&cloud_info->transformed_points_.front(), cloud_info->transformed_points_.size());
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
bool MapCloudDisplay::transformCloud(const CloudInfoPtr& cloud_info, bool update_transformers)
|
bool MapCloudDisplay::transformCloud(const CloudInfoPtr& cloud_info, bool update_transformers)
|
||||||
{
|
{
|
||||||
rviz::V_PointCloudPoint& cloud_points = cloud_info->transformed_points_;
|
rviz::V_PointCloudPoint& cloud_points = cloud_info->transformed_points_;
|
||||||
cloud_points.clear();
|
cloud_points.clear();
|
||||||
|
|
||||||
size_t size = cloud_info->message_->width * cloud_info->message_->height;
|
size_t size = cloud_info->message_->width * cloud_info->message_->height;
|
||||||
rviz::PointCloud::Point default_pt;
|
rviz::PointCloud::Point default_pt;
|
||||||
default_pt.color = Ogre::ColourValue(1, 1, 1);
|
default_pt.color = Ogre::ColourValue(1, 1, 1);
|
||||||
default_pt.position = Ogre::Vector3::ZERO;
|
default_pt.position = Ogre::Vector3::ZERO;
|
||||||
cloud_points.resize(size, default_pt);
|
cloud_points.resize(size, default_pt);
|
||||||
|
|
||||||
{
|
{
|
||||||
boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
|
boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
|
||||||
if( update_transformers )
|
if( update_transformers )
|
||||||
{
|
{
|
||||||
updateTransformers( cloud_info->message_ );
|
updateTransformers( cloud_info->message_ );
|
||||||
}
|
}
|
||||||
rviz::PointCloudTransformerPtr xyz_trans = getXYZTransformer(cloud_info->message_);
|
rviz::PointCloudTransformerPtr xyz_trans = getXYZTransformer(cloud_info->message_);
|
||||||
rviz::PointCloudTransformerPtr color_trans = getColorTransformer(cloud_info->message_);
|
rviz::PointCloudTransformerPtr color_trans = getColorTransformer(cloud_info->message_);
|
||||||
|
|
||||||
if (!xyz_trans)
|
if (!xyz_trans)
|
||||||
{
|
{
|
||||||
std::stringstream ss;
|
std::stringstream ss;
|
||||||
ss << "No position transformer available for cloud";
|
ss << "No position transformer available for cloud";
|
||||||
this->setStatusStd(rviz::StatusProperty::Error, "Message", ss.str());
|
this->setStatusStd(rviz::StatusProperty::Error, "Message", ss.str());
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (!color_trans)
|
if (!color_trans)
|
||||||
{
|
{
|
||||||
std::stringstream ss;
|
std::stringstream ss;
|
||||||
ss << "No color transformer available for cloud";
|
ss << "No color transformer available for cloud";
|
||||||
this->setStatusStd(rviz::StatusProperty::Error, "Message", ss.str());
|
this->setStatusStd(rviz::StatusProperty::Error, "Message", ss.str());
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
xyz_trans->transform(cloud_info->message_, rviz::PointCloudTransformer::Support_XYZ, Ogre::Matrix4::IDENTITY, cloud_points);
|
xyz_trans->transform(cloud_info->message_, rviz::PointCloudTransformer::Support_XYZ, Ogre::Matrix4::IDENTITY, cloud_points);
|
||||||
color_trans->transform(cloud_info->message_, rviz::PointCloudTransformer::Support_Color, Ogre::Matrix4::IDENTITY, cloud_points);
|
color_trans->transform(cloud_info->message_, rviz::PointCloudTransformer::Support_Color, Ogre::Matrix4::IDENTITY, cloud_points);
|
||||||
}
|
}
|
||||||
|
|
||||||
for (rviz::V_PointCloudPoint::iterator cloud_point = cloud_points.begin(); cloud_point != cloud_points.end(); ++cloud_point)
|
for (rviz::V_PointCloudPoint::iterator cloud_point = cloud_points.begin(); cloud_point != cloud_points.end(); ++cloud_point)
|
||||||
{
|
{
|
||||||
if (!rviz::validateFloats(cloud_point->position))
|
if (!rviz::validateFloats(cloud_point->position))
|
||||||
{
|
{
|
||||||
cloud_point->position.x = 999999.0f;
|
cloud_point->position.x = 999999.0f;
|
||||||
cloud_point->position.y = 999999.0f;
|
cloud_point->position.y = 999999.0f;
|
||||||
cloud_point->position.z = 999999.0f;
|
cloud_point->position.z = 999999.0f;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace rviz
|
} // namespace rviz
|
||||||
|
|||||||
Reference in New Issue
Block a user