void GridCellsDisplay::processMessage(const nav_msgs::GridCells::ConstPtr& msg)
{
if (!msg)
{
return;
}
cloud_->clear();
++messages_received_;
if (!validateFloats(*msg))
{
setStatus("Topic", eu::nifti::ocu::STATUS_LEVEL_ERROR, "Message contained invalid floating point values (nans or infs)");
return;
}
std::stringstream ss;
ss << messages_received_ << " messages received";
setStatus("Topic", eu::nifti::ocu::STATUS_LEVEL_OK, ss.str());
Ogre::Vector3 position;
Ogre::Quaternion orientation;
if (!frameTransformer->getTransform(msg->header, position, orientation))
{
ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), fixed_frame_.c_str() );
}
scene_node_->setPosition( position );
scene_node_->setOrientation( orientation );
cloud_->setDimensions(msg->cell_width, msg->cell_height, 0.0);
uint32_t num_points = msg->cells.size();
typedef std::vector< ogre_tools::PointCloud::Point > V_Point;
V_Point points;
points.resize( num_points );
for(uint32_t i = 0; i < num_points; i++)
{
ogre_tools::PointCloud::Point& current_point = points[ i ];
Ogre::Vector3 pos(msg->cells[i].x, msg->cells[i].y, msg->cells[i].z);
current_point.x = pos.x;
current_point.y = pos.y;
current_point.z = pos.z;
current_point.setColor(color.r, color.g, color.b);
}
cloud_->clear();
if ( !points.empty() )
{
cloud_->addPoints( &points.front(), points.size() );
}
}
void PointCloudDisplay::transformCloud()
{
if ( message_.header.frame_id.empty() )
{
message_.header.frame_id = fixed_frame_;
}
tf::Stamped<tf::Pose> pose( btTransform( btQuaternion( 0, 0, 0 ), btVector3( 0, 0, 0 ) ), message_.header.stamp, message_.header.frame_id );
try
{
tf_->transformPose( fixed_frame_, pose, pose );
}
catch(tf::TransformException& e)
{
ROS_ERROR( "Error transforming point cloud '%s' from frame '%s' to frame '%s'\n", name_.c_str(), message_.header.frame_id.c_str(), fixed_frame_.c_str() );
}
Ogre::Vector3 position( pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z() );
robotToOgre( position );
btScalar yaw, pitch, roll;
pose.getBasis().getEulerZYX( yaw, pitch, roll );
Ogre::Matrix3 orientation( ogreMatrixFromRobotEulers( yaw, pitch, roll ) );
// First find the min/max intensity values
float min_intensity = 999999.0f;
float max_intensity = -999999.0f;
typedef std::vector<std_msgs::ChannelFloat32> V_Chan;
typedef std::vector<bool> V_bool;
V_bool valid_channels(message_.chan.size());
uint32_t point_count = message_.get_pts_size();
V_Chan::iterator chan_it = message_.chan.begin();
V_Chan::iterator chan_end = message_.chan.end();
uint32_t index = 0;
for ( ; chan_it != chan_end; ++chan_it, ++index )
{
std_msgs::ChannelFloat32& chan = *chan_it;
uint32_t val_count = chan.vals.size();
bool channel_size_correct = val_count == point_count;
ROS_ERROR_COND(!channel_size_correct, "Point cloud '%s' on topic '%s' has channel 0 with fewer values than points (%d values, %d points)", name_.c_str(), topic_.c_str(), val_count, point_count);
valid_channels[index] = channel_size_correct;
if ( channel_size_correct && ( chan.name.empty() || chan.name == "intensity" || chan.name == "intensities" ) )
{
for(uint32_t i = 0; i < point_count; i++)
{
float& intensity = chan.vals[i];
// arbitrarily cap to 4096 for now
intensity = std::min( intensity, 4096.0f );
min_intensity = std::min( min_intensity, intensity );
max_intensity = std::max( max_intensity, intensity );
}
}
}
float diff_intensity = max_intensity - min_intensity;
typedef std::vector< ogre_tools::PointCloud::Point > V_Point;
V_Point points;
points.resize( point_count );
for(uint32_t i = 0; i < point_count; i++)
{
Ogre::Vector3 color( color_.r_, color_.g_, color_.b_ );
ogre_tools::PointCloud::Point& current_point = points[ i ];
current_point.x_ = message_.pts[i].x;
current_point.y_ = message_.pts[i].y;
current_point.z_ = message_.pts[i].z;
current_point.r_ = color.x;
current_point.g_ = color.y;
current_point.b_ = color.z;
}
chan_it = message_.chan.begin();
index = 0;
for ( ; chan_it != chan_end; ++chan_it, ++index )
{
if ( !valid_channels[index] )
{
continue;
}
std_msgs::ChannelFloat32& chan = *chan_it;
enum ChannelType
{
CT_INTENSITY,
CT_RGB,
CT_R,
CT_G,
CT_B,
CT_COUNT
};
ChannelType type = CT_INTENSITY;
if ( chan.name == "rgb" )
{
type = CT_RGB;
}
else if ( chan.name == "r" )
{
type = CT_R;
}
else if ( chan.name == "g" )
{
type = CT_G;
}
else if ( chan.name == "b" )
{
type = CT_B;
}
typedef void (*TransformFunc)(float, ogre_tools::PointCloud::Point&, float, float, float);
TransformFunc funcs[CT_COUNT] =
{
transformIntensity,
transformRGB,
transformR,
transformG,
transformB
};
for(uint32_t i = 0; i < point_count; i++)
{
ogre_tools::PointCloud::Point& current_point = points[ i ];
funcs[type]( chan.vals[i], current_point, min_intensity, max_intensity, diff_intensity );
}
}
{
RenderAutoLock renderLock( this );
scene_node_->setPosition( position );
scene_node_->setOrientation( orientation );
cloud_->clear();
if ( !points.empty() )
{
cloud_->addPoints( &points.front(), points.size() );
}
}
causeRender();
}
void PointsMarkerCustom::onNewMessage(const MarkerConstPtr& old_message, const MarkerConstPtr& new_message)
{
ROS_ASSERT(new_message->type == visualization_msgs::Marker::POINTS ||
new_message->type == visualization_msgs::Marker::CUBE_LIST ||
new_message->type == visualization_msgs::Marker::SPHERE_LIST);
if (!points_)
{
points_ = new PointCloudCustom();
scene_node_->attachObject(points_);
}
Ogre::Vector3 pos, scale;
Ogre::Quaternion orient;
transform(new_message, pos, orient, scale);
switch (new_message->type)
{
case visualization_msgs::Marker::POINTS:
points_->setRenderMode(PointCloudCustom::RM_SQUARES);
points_->setDimensions(new_message->scale.x, new_message->scale.y, 0.0f);
break;
case visualization_msgs::Marker::CUBE_LIST:
points_->setRenderMode(PointCloudCustom::RM_BOXES);
points_->setDimensions(scale.x, scale.y, scale.z);
break;
case visualization_msgs::Marker::SPHERE_LIST:
points_->setRenderMode(PointCloudCustom::RM_SPHERES);
points_->setDimensions(scale.x, scale.y, scale.z);
break;
}
setPosition(pos);
setOrientation(orient);
points_->clear();
if (new_message->points.empty())
{
return;
}
float r = new_message->color.r;
float g = new_message->color.g;
float b = new_message->color.b;
float a = new_message->color.a;
bool has_per_point_color = new_message->colors.size() == new_message->points.size();
bool has_nonzero_alpha = false;
bool has_per_point_alpha = false;
typedef std::vector< PointCloudCustom::Point > V_Point;
V_Point points;
points.resize(new_message->points.size());
std::vector<geometry_msgs::Point>::const_iterator it = new_message->points.begin();
std::vector<geometry_msgs::Point>::const_iterator end = new_message->points.end();
for (int i = 0; it != end; ++it, ++i)
{
const geometry_msgs::Point& p = *it;
PointCloudCustom::Point& point = points[i];
Ogre::Vector3 v(p.x, p.y, p.z);
point.position.x = v.x;
point.position.y = v.y;
point.position.z = v.z;
if (has_per_point_color)
{
const std_msgs::ColorRGBA& color = new_message->colors[i];
r = color.r;
g = color.g;
b = color.b;
a = color.a;
has_nonzero_alpha = has_nonzero_alpha || a != 0.0;
has_per_point_alpha = has_per_point_alpha || a != 1.0;
}
point.setColor(r, g, b, a);
}
if (has_per_point_color)
{
if (!has_nonzero_alpha )
{
owner_->setMarkerStatus(getID(), StatusProperty::Warn, "All points have a zero alpha value.");
}
points_->setAlpha(1.0, has_per_point_alpha);
}
else
{
points_->setAlpha(a);
}
points_->addPoints(&points.front(), points.size());
handler_.reset( new MarkerSelectionHandlerCustom( this, MarkerID( new_message->ns, new_message->id ), context_ ));
points_->setPickColor( SelectionManager::handleToColor( handler_->getHandle() ));
}
