void PointCloudBase::onPluginUnloading(const PluginStatus& status)
{
boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
typedef std::set<std::string> S_string;
S_string to_erase;
bool xyz_unloaded = false;
bool color_unloaded = false;
M_TransformerInfo::iterator it = transformers_.begin();
M_TransformerInfo::iterator end = transformers_.end();
for (; it != end; ++it)
{
const std::string& name = it->first;
TransformerInfo& info = it->second;
if (info.plugin != status.plugin)
{
continue;
}
if (name == xyz_transformer_)
{
xyz_unloaded = true;
}
if (name == color_transformer_)
{
color_unloaded = true;
}
to_erase.insert(it->first);
if (property_manager_)
{
deleteProperties(property_manager_, info.xyz_props);
deleteProperties(property_manager_, info.color_props);
}
info.transformer.reset();
}
{
S_string::iterator it = to_erase.begin();
S_string::iterator end = to_erase.end();
for (; it != end; ++it)
{
transformers_.erase(*it);
}
}
if (xyz_unloaded || color_unloaded)
{
boost::mutex::scoped_lock lock(clouds_mutex_);
if (!clouds_.empty())
{
updateTransformers((*clouds_.rbegin())->message_, true);
}
}
}
bool MapCloudDisplay::transformCloud(const CloudInfoPtr& cloud_info, bool update_transformers)
{
rviz::V_PointCloudPoint& cloud_points = cloud_info->transformed_points_;
cloud_points.clear();
size_t size = cloud_info->message_->width * cloud_info->message_->height;
rviz::PointCloud::Point default_pt;
default_pt.color = Ogre::ColourValue(1, 1, 1);
default_pt.position = Ogre::Vector3::ZERO;
cloud_points.resize(size, default_pt);
{
boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
if( update_transformers )
{
updateTransformers( cloud_info->message_ );
}
rviz::PointCloudTransformerPtr xyz_trans = getXYZTransformer(cloud_info->message_);
rviz::PointCloudTransformerPtr color_trans = getColorTransformer(cloud_info->message_);
if (!xyz_trans)
{
std::stringstream ss;
ss << "No position transformer available for cloud";
this->setStatusStd(rviz::StatusProperty::Error, "Message", ss.str());
return false;
}
if (!color_trans)
{
std::stringstream ss;
ss << "No color transformer available for cloud";
this->setStatusStd(rviz::StatusProperty::Error, "Message", ss.str());
return false;
}
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);
}
for (rviz::V_PointCloudPoint::iterator cloud_point = cloud_points.begin(); cloud_point != cloud_points.end(); ++cloud_point)
{
if (!rviz::validateFloats(cloud_point->position))
{
cloud_point->position.x = 999999.0f;
cloud_point->position.y = 999999.0f;
cloud_point->position.z = 999999.0f;
}
}
return true;
}
