void MarkerVisualizer::processAdd( const std_msgs::VisualizationMarker& message )
{
ogre_tools::Object* object = NULL;
bool create = true;
M_IDToMarker::iterator it = markers_.find( message.id );
if ( it != markers_.end() )
{
MarkerInfo& info = it->second;
if ( message.type == info.message_.type )
{
object = info.object_;
info.message_ = message;
create = false;
}
else
{
delete it->second.object_;
markers_.erase( it );
}
}
if ( create )
{
switch ( message.type )
{
case MarkerTypes::Cube:
{
ogre_tools::SuperEllipsoid* cube = new ogre_tools::SuperEllipsoid( scene_manager_, scene_node_ );
cube->create( ogre_tools::SuperEllipsoid::Cube, 10, Ogre::Vector3( 1.0f, 1.0f, 1.0f ) );
object = cube;
}
break;
case MarkerTypes::Sphere:
{
ogre_tools::SuperEllipsoid* sphere = new ogre_tools::SuperEllipsoid( scene_manager_, scene_node_ );
sphere->create( ogre_tools::SuperEllipsoid::Sphere, 20, Ogre::Vector3( 1.0f, 1.0f, 1.0f ) );
object = sphere;
}
break;
case MarkerTypes::Arrow:
{
object = new ogre_tools::Arrow( scene_manager_, scene_node_, 0.8, 0.5, 0.2, 1.0 );
}
break;
case MarkerTypes::Robot:
{
Robot* robot = new Robot( scene_manager_ );
robot->load( urdf_, false, true );
robot->update( kinematic_model_, target_frame_ );
object = robot;
}
break;
default:
printf( "Unknown marker type: %d\n", message.type );
}
if ( object )
{
markers_.insert( std::make_pair( message.id, MarkerInfo(object, message) ) );
}
}
if ( object )
{
setCommonValues( message, object );
causeRender();
}
}
void DebugDraw::addMyAvatarMarker(const std::string& key, const glm::quat& rotation, const glm::vec3& position, const glm::vec4& color) {
_myAvatarMarkers[key] = MarkerInfo(rotation, position, color);
}
