void PolygonArrayDisplay::processMessage(const jsk_recognition_msgs::PolygonArray::ConstPtr& msg)
{
if (!validateFloats(*msg)) {
setStatus(rviz::StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)");
return;
}
// create nodes and manual objects
updateSceneNodes(msg);
allocateMaterials(msg->polygons.size());
updateLines(msg->polygons.size());
if (only_border_) {
// use line_
for (size_t i = 0; i < manual_objects_.size(); i++) {
manual_objects_[i]->setVisible(false);
}
for (size_t i = 0; i < msg->polygons.size(); i++) {
geometry_msgs::PolygonStamped polygon = msg->polygons[i];
if (polygon.polygon.points.size() >= 3) {
processLine(i, polygon);
}
}
}
else {
for (size_t i = 0; i < msg->polygons.size(); i++) {
processPolygonMaterial(i);
}
for (size_t i = 0; i < msg->polygons.size(); i++) {
geometry_msgs::PolygonStamped polygon = msg->polygons[i];
processPolygon(i, polygon);
}
}
if (show_normal_) {
for (size_t i = 0; i < msg->polygons.size(); i++) {
geometry_msgs::PolygonStamped polygon = msg->polygons[i];
processNormal(i, polygon);
}
}
}
void processLine( const char *line )
{
char temp[256];
strcpy( temp, line );
char *token = strtok( temp, " " );
if( strcmp( token, "v" ) == 0 )
{
processVertex( line );
}
else if( strcmp( token, "vn" ) == 0 )
{
processNormal( line );
}
else if( strcmp( token, "vt" ) == 0 )
{
processTexture( line );
}
else if( strcmp( token, "f" ) == 0 )
{
processFace( line );
}
}