void MarkerDisplay::update(float wall_dt, float ros_dt)
{
V_MarkerMessage local_queue;
{
boost::mutex::scoped_lock lock(queue_mutex_);
local_queue.swap( message_queue_ );
}
if ( !local_queue.empty() )
{
V_MarkerMessage::iterator message_it = local_queue.begin();
V_MarkerMessage::iterator message_end = local_queue.end();
for ( ; message_it != message_end; ++message_it )
{
visualization_msgs::Marker::ConstPtr& marker = *message_it;
processMessage( marker );
}
}
{
S_MarkerBase::iterator it = markers_with_expiration_.begin();
S_MarkerBase::iterator end = markers_with_expiration_.end();
for (; it != end;)
{
MarkerBasePtr marker = *it;
if (marker->expired())
{
++it;
deleteMarker(marker->getID());
}
else
{
++it;
}
}
}
{
S_MarkerBase::iterator it = frame_locked_markers_.begin();
S_MarkerBase::iterator end = frame_locked_markers_.end();
for (; it != end; ++it)
{
MarkerBasePtr marker = *it;
marker->updateFrameLocked();
}
}
}
void MarkerDisplay::processAdd( const visualization_msgs::Marker::ConstPtr& message )
{
QString namespace_name = QString::fromStdString( message->ns );
M_Namespace::iterator ns_it = namespaces_.find( namespace_name );
if( ns_it == namespaces_.end() )
{
ns_it = namespaces_.insert( namespace_name, new MarkerNamespace( namespace_name, namespaces_category_, this ));
}
if( !ns_it.value()->isEnabled() )
{
return;
}
deleteMarkerStatus( MarkerID( message->ns, message->id ));
bool create = true;
MarkerBasePtr marker;
M_IDToMarker::iterator it = markers_.find( MarkerID(message->ns, message->id) );
if ( it != markers_.end() )
{
marker = it->second;
markers_with_expiration_.erase(marker);
if ( message->type == marker->getMessage()->type )
{
create = false;
}
else
{
markers_.erase( it );
}
}
if ( create )
{
switch ( message->type )
{
case visualization_msgs::Marker::CUBE:
case visualization_msgs::Marker::CYLINDER:
case visualization_msgs::Marker::SPHERE:
{
marker.reset(new ShapeMarker(this, context_, scene_node_));
}
break;
case visualization_msgs::Marker::ARROW:
{
marker.reset(new ArrowMarker(this, context_, scene_node_));
}
break;
case visualization_msgs::Marker::LINE_STRIP:
{
marker.reset(new LineStripMarker(this, context_, scene_node_));
}
break;
case visualization_msgs::Marker::LINE_LIST:
{
marker.reset(new LineListMarker(this, context_, scene_node_));
}
break;
case visualization_msgs::Marker::SPHERE_LIST:
case visualization_msgs::Marker::CUBE_LIST:
case visualization_msgs::Marker::POINTS:
{
marker.reset(new PointsMarker(this, context_, scene_node_));
}
break;
case visualization_msgs::Marker::TEXT_VIEW_FACING:
{
marker.reset(new TextViewFacingMarker(this, context_, scene_node_));
}
break;
case visualization_msgs::Marker::MESH_RESOURCE:
{
marker.reset(new MeshResourceMarker(this, context_, scene_node_));
}
break;
case visualization_msgs::Marker::TRIANGLE_LIST:
{
marker.reset(new TriangleListMarker(this, context_, scene_node_));
}
break;
default:
ROS_ERROR( "Unknown marker type: %d", message->type );
}
markers_.insert(std::make_pair(MarkerID(message->ns, message->id), marker));
}
if (marker)
{
marker->setMessage(message);
if (message->lifetime.toSec() > 0.0001f)
{
markers_with_expiration_.insert(marker);
}
if (message->frame_locked)
{
frame_locked_markers_.insert(marker);
}
context_->queueRender();
}
}
void InteractiveMarkerControl::makeMarkers( const visualization_msgs::InteractiveMarkerControl& message )
{
for (unsigned i = 0; i < message.markers.size(); i++)
{
MarkerBasePtr marker;
// create a marker with the given type
switch (message.markers[i].type)
{
case visualization_msgs::Marker::CUBE:
case visualization_msgs::Marker::CYLINDER:
case visualization_msgs::Marker::SPHERE:
{
marker.reset(new ShapeMarker(0, context_, markers_node_));
}
break;
case visualization_msgs::Marker::ARROW:
{
marker.reset(new ArrowMarker(0, context_, markers_node_));
}
break;
case visualization_msgs::Marker::LINE_STRIP:
{
marker.reset(new LineStripMarker(0, context_, markers_node_));
}
break;
case visualization_msgs::Marker::LINE_LIST:
{
marker.reset(new LineListMarker(0, context_, markers_node_));
}
break;
case visualization_msgs::Marker::SPHERE_LIST:
case visualization_msgs::Marker::CUBE_LIST:
case visualization_msgs::Marker::POINTS:
{
PointsMarkerPtr points_marker;
points_marker.reset(new PointsMarker(0, context_, markers_node_));
points_markers_.push_back( points_marker );
marker = points_marker;
}
break;
case visualization_msgs::Marker::TEXT_VIEW_FACING:
{
marker.reset(new TextViewFacingMarker(0, context_, markers_node_));
}
break;
case visualization_msgs::Marker::MESH_RESOURCE:
{
marker.reset(new MeshResourceMarker(0, context_, markers_node_));
}
break;
case visualization_msgs::Marker::TRIANGLE_LIST:
{
marker.reset(new TriangleListMarker(0, context_, markers_node_));
}
break;
default:
ROS_ERROR( "Unknown marker type: %d", message.markers[i].type );
break;
}
marker->setMessage( message.markers[ i ]);
marker->setInteractiveObject( shared_from_this() );
addHighlightPass(marker->getMaterials());
// The marker will set its position relative to the fixed frame,
// but we have attached it our own scene node, so we will have to
// correct for that.
marker->setPosition( markers_node_->convertWorldToLocalPosition( marker->getPosition() ) );
marker->setOrientation( markers_node_->convertWorldToLocalOrientation( marker->getOrientation() ) );
markers_.push_back(marker);
}
}
