Esempio n. 1
0
void MarkerDisplay::failedMarker(const ros::MessageEvent<visualization_msgs::Marker>& marker_evt, tf::FilterFailureReason reason)
{
  visualization_msgs::Marker::ConstPtr marker = marker_evt.getConstMessage();
  std::string authority = marker_evt.getPublisherName();
  std::string error = context_->getFrameManager()->discoverFailureReason(marker->header.frame_id, marker->header.stamp, authority, reason);
  setMarkerStatus(MarkerID(marker->ns, marker->id), StatusProperty::Error, error);
}
void MarkerDisplayCustom::failedMarker(const ros::MessageEvent<visualization_msgs::Marker>& marker_evt, tf::FilterFailureReason reason)
{
/// Changed in indigo
//  std::string error = context_->getFrameManager()->discoverFailureReason(marker->header.frame_id, marker->header.stamp, marker->__connection_header ? (*marker->__connection_header)["callerid"] : "unknown", reason);
//  setMarkerStatus(MarkerID(marker->ns, marker->id), StatusProperty::Error, error);
  visualization_msgs::Marker::ConstPtr marker = marker_evt.getConstMessage();
  std::string authority = marker_evt.getPublisherName();
  std::string error = context_->getFrameManager()->discoverFailureReason(marker->header.frame_id, marker->header.stamp, authority, reason);
  setMarkerStatus(MarkerID(marker->ns, marker->id), StatusProperty::Error, error);
}
Esempio n. 3
0
void ShapeMarker::onNewMessage( const MarkerConstPtr& old_message,
    const MarkerConstPtr& new_message )
{
  if (!shape_ || old_message->type != new_message->type)
  {
    delete shape_;
    shape_ = 0;

    Shape::Type shape_type = Shape::Cube;
    switch( new_message->type )
    {
    case visualization_msgs::Marker::CUBE:     shape_type = Shape::Cube;     break;
    case visualization_msgs::Marker::CYLINDER: shape_type = Shape::Cylinder; break;
    case visualization_msgs::Marker::SPHERE:   shape_type = Shape::Sphere;   break;
    default:
      ROS_BREAK();
      break;
    }
    shape_ = new Shape( shape_type, context_->getSceneManager(), scene_node_ );

    handler_.reset( new MarkerSelectionHandler( this, MarkerID( new_message->ns, new_message->id ), context_ ));
    handler_->addTrackedObjects( shape_->getRootNode() );
  }

  Ogre::Vector3 pos, scale, scale_correct;
  Ogre::Quaternion orient;
  transform(new_message, pos, orient, scale);

  if (owner_ && (new_message->scale.x * new_message->scale.y
      * new_message->scale.z == 0.0f))
  {
    owner_->setMarkerStatus(getID(), StatusProperty::Warn,
        "Scale of 0 in one of x/y/z");
  }

  setPosition(pos);
  setOrientation( orient * Ogre::Quaternion( Ogre::Degree(90), Ogre::Vector3(1,0,0) ) );

  scale_correct = Ogre::Quaternion( Ogre::Degree(90), Ogre::Vector3(1,0,0) ) * scale;

  shape_->setScale(scale_correct);

  shape_->setColor(new_message->color.r, new_message->color.g,
      new_message->color.b, new_message->color.a);
}
Esempio n. 4
0
void MarkerDisplay::processMessage( const visualization_msgs::Marker::ConstPtr& message )
{
  if (!validateFloats(*message))
  {
    setMarkerStatus(MarkerID(message->ns, message->id), StatusProperty::Error, "Contains invalid floating point values (nans or infs)");
    return;
  }

  switch ( message->action )
  {
  case visualization_msgs::Marker::ADD:
    processAdd( message );
    break;

  case visualization_msgs::Marker::DELETE:
    processDelete( message );
    break;

  default:
    ROS_ERROR( "Unknown marker action: %d\n", message->action );
  }
}
void LineListMarker::onNewMessage(const MarkerConstPtr& old_message, const MarkerConstPtr& new_message)
{
  ROS_ASSERT(new_message->type == visualization_msgs::Marker::LINE_LIST);

  if (!lines_)
  {
    lines_ = new BillboardLine(context_->getSceneManager(), scene_node_);
  }

  Ogre::Vector3 pos, scale;
  Ogre::Quaternion orient;
  transform(new_message, pos, orient, scale);

  setPosition(pos);
  setOrientation(orient);
  lines_->setScale(scale);
  lines_->setColor(new_message->color.r, new_message->color.g, new_message->color.b, new_message->color.a);

  lines_->clear();

  if (new_message->points.empty())
  {
    return;
  }

  bool has_per_point_color = new_message->colors.size() == new_message->points.size();

  if (new_message->points.size() % 2 == 0)
  {
    lines_->setLineWidth( new_message->scale.x );
    lines_->setMaxPointsPerLine(2);
    lines_->setNumLines(new_message->points.size() / 2);

    size_t i = 0;
    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 ( ; it != end; )
    {
      if (it != new_message->points.begin())
      {
        lines_->newLine();
      }

      for (uint32_t j = 0; j < 2; ++j, ++it, ++i)
      {
        const geometry_msgs::Point& p = *it;

        Ogre::ColourValue c;
        if (has_per_point_color)
        {
          const std_msgs::ColorRGBA& color = new_message->colors[i];
          c.r = color.r;
          c.g = color.g;
          c.b = color.b;
          c.a = color.a;
        }
        else
        {
          c.r = new_message->color.r;
          c.g = new_message->color.g;
          c.b = new_message->color.b;
          c.a = new_message->color.a;
        }

        Ogre::Vector3 v( p.x, p.y, p.z );
        lines_->addPoint( v, c );
      }
    }

    handler_.reset( new MarkerSelectionHandler( this, MarkerID( new_message->ns, new_message->id ), context_ ));
    handler_->addTrackedObjects( lines_->getSceneNode() );
  }
  else
  {
    std::stringstream ss;
    ss << "Line list marker [" << getStringID() << "] has an odd number of points.";
    if ( owner_ )
    {
      owner_->setMarkerStatus(getID(), StatusProperty::Error, ss.str());
    }
    ROS_DEBUG("%s", ss.str().c_str());
  }
}
Esempio n. 6
0
void MarkerDisplay::processDelete( const visualization_msgs::Marker::ConstPtr& message )
{
  deleteMarker(MarkerID(message->ns, message->id));
  context_->queueRender();
}
Esempio n. 7
0
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 MeshResourceMarker::onNewMessage(const MarkerConstPtr& old_message, const MarkerConstPtr& new_message)
{
  ROS_ASSERT(new_message->type == visualization_msgs::Marker::MESH_RESOURCE);

  bool need_color = false;

  scene_node_->setVisible(false);

  if( !entity_ ||
      old_message->mesh_resource != new_message->mesh_resource ||
      old_message->mesh_use_embedded_materials != new_message->mesh_use_embedded_materials )
  {
    reset();

    if (new_message->mesh_resource.empty())
    {
      return;
    }

    if (loadMeshFromResource(new_message->mesh_resource).isNull())
    {
      std::stringstream ss;
      ss << "Mesh resource marker [" << getStringID() << "] could not load [" << new_message->mesh_resource << "]";
      if ( owner_ )
      {
        owner_->setMarkerStatus(getID(), StatusProperty::Error, ss.str());
      }
      ROS_DEBUG("%s", ss.str().c_str());
      return;
    }

    static uint32_t count = 0;
    std::stringstream ss;
    ss << "mesh_resource_marker_" << count++;
    std::string id = ss.str();
    entity_ = context_->getSceneManager()->createEntity(id, new_message->mesh_resource);
    scene_node_->attachObject(entity_);
    need_color = true;

    // create a default material for any sub-entities which don't have their own.
    ss << "Material";
    Ogre::MaterialPtr default_material = Ogre::MaterialManager::getSingleton().create( ss.str(), ROS_PACKAGE_NAME );
    default_material->setReceiveShadows(false);
    default_material->getTechnique(0)->setLightingEnabled(true);
    default_material->getTechnique(0)->setAmbient( 0.5, 0.5, 0.5 );
    materials_.insert( default_material );

    if ( new_message->mesh_use_embedded_materials )
    {
      // make clones of all embedded materials so selection works correctly
      S_MaterialPtr materials = getMaterials();

      S_MaterialPtr::iterator it;
      for ( it = materials.begin(); it!=materials.end(); it++ )
      {
        if( (*it)->getName() != "BaseWhiteNoLighting" )
        {
          Ogre::MaterialPtr new_material = (*it)->clone( id + (*it)->getName() );
          materials_.insert( new_material );
        }
      }

      // make sub-entities use cloned materials
      for (uint32_t i = 0; i < entity_->getNumSubEntities(); ++i)
      {
        std::string mat_name = entity_->getSubEntity(i)->getMaterialName();
        if( mat_name != "BaseWhiteNoLighting" )
        {
          entity_->getSubEntity(i)->setMaterialName( id + mat_name );
        }
        else
        {
          // BaseWhiteNoLighting is the default material Ogre uses
          // when it sees a mesh with no material.  Here we replace
          // that with our default_material which gets colored with
          // new_message->color.
          entity_->getSubEntity(i)->setMaterial( default_material );
        }
      }
    }
    else
    {
      entity_->setMaterial( default_material );
    }

    context_->getSelectionManager()->removeObject(coll_);
    coll_ = context_->getSelectionManager()->createCollisionForEntity(entity_, SelectionHandlerPtr(new MarkerSelectionHandler(this, MarkerID(new_message->ns, new_message->id))), coll_);
  }

  if( need_color ||
      old_message->color.r != new_message->color.r ||
      old_message->color.g != new_message->color.g ||
      old_message->color.b != new_message->color.b ||
      old_message->color.a != new_message->color.a )
  {
    float r = new_message->color.r;
    float g = new_message->color.g;
    float b = new_message->color.b;
    float a = new_message->color.a;

    // Old way was to ignore the color and alpha when using embedded
    // materials, which meant you could leave them unset, which means
    // 0.  Since we now USE the color and alpha values, leaving them
    // all 0 will mean the object will be invisible.  Therefore detect
    // the situation where RGBA are all 0 and treat that the same as
    // all 1 (full white).
    if( new_message->mesh_use_embedded_materials && r == 0 && g == 0 && b == 0 && a == 0 )
    {
      r = 1; g = 1; b = 1; a = 1;
    }

    Ogre::SceneBlendType blending;
    bool depth_write;

    if ( a < 0.9998 )
    {
      blending = Ogre::SBT_TRANSPARENT_ALPHA;
      depth_write = false;
    }
    else
    {
      blending = Ogre::SBT_REPLACE;
      depth_write = true;
    }

    S_MaterialPtr::iterator it;
    for( it = materials_.begin(); it != materials_.end(); it++ )
    {    
      Ogre::Technique* technique = (*it)->getTechnique( 0 );

      technique->setAmbient( r*0.5, g*0.5, b*0.5 );
      technique->setDiffuse( r, g, b, a );
      technique->setSceneBlending( blending );
      technique->setDepthWriteEnabled( depth_write );
    }
  }

  Ogre::Vector3 pos, scale;
  Ogre::Quaternion orient;
  transform(new_message, pos, orient, scale);

  scene_node_->setVisible(true);
  setPosition(pos);
  setOrientation(orient);

  // In Ogre, mesh surface normals are not normalized if object is not
  // scaled.  This forces the surface normals to be renormalized by
  // invisibly tweaking the scale.
  if( scale.x == 1.0 && scale.y == 1.0 && scale.z == 1.0 )
  {
    scale.z = 1.0001;
  }
  scene_node_->setScale(scale);
}
Esempio n. 9
0
void MeshResourceMarker::onNewMessage(const MarkerConstPtr& old_message, const MarkerConstPtr& new_message)
{
  ROS_ASSERT(new_message->type == visualization_msgs::Marker::MESH_RESOURCE);

  // flag indicating if the mesh material color needs to be updated
  bool update_color = false;

  scene_node_->setVisible(false);

  if (!entity_ ||
      old_message->mesh_resource != new_message->mesh_resource ||
      old_message->mesh_use_embedded_materials != new_message->mesh_use_embedded_materials)
  {
    reset();

    if (new_message->mesh_resource.empty())
    {
      return;
    }

    if (loadMeshFromResource(new_message->mesh_resource).isNull())
    {
      std::stringstream ss;
      ss << "Mesh resource marker [" << getStringID() << "] could not load [" << new_message->mesh_resource << "]";
      if (owner_)
      {
        owner_->setMarkerStatus(getID(), StatusProperty::Error, ss.str());
      }
      ROS_DEBUG("%s", ss.str().c_str());
      return;
    }

    static uint32_t count = 0;
    std::stringstream ss;
    ss << "mesh_resource_marker_" << count++;
    std::string id = ss.str();
    entity_ = context_->getSceneManager()->createEntity(id, new_message->mesh_resource);
    scene_node_->attachObject(entity_);

    // create a default material for any sub-entities which don't have their own.
    ss << "Material";
    Ogre::MaterialPtr default_material = Ogre::MaterialManager::getSingleton().create(ss.str(), ROS_PACKAGE_NAME);
    default_material->setReceiveShadows(false);
    default_material->getTechnique(0)->setLightingEnabled(true);
    default_material->getTechnique(0)->setAmbient(0.5, 0.5, 0.5);
    materials_.insert(default_material);

    if (new_message->mesh_use_embedded_materials)
    {
      // make clones of all embedded materials so selection works correctly
      S_MaterialPtr materials = getMaterials();

      S_MaterialPtr::iterator it;
      for (it = materials.begin(); it != materials.end(); it++)
      {
        if ((*it)->getName() != "BaseWhiteNoLighting")
        {
          Ogre::MaterialPtr new_material = (*it)->clone(id + (*it)->getName());
          materials_.insert(new_material);
        }
      }

      // make sub-entities use cloned materials
      for (uint32_t i = 0; i < entity_->getNumSubEntities(); ++i)
      {
        std::string mat_name = entity_->getSubEntity(i)->getMaterialName();
        if (mat_name != "BaseWhiteNoLighting")
        {
          entity_->getSubEntity(i)->setMaterialName(id + mat_name);
        }
        else
        {
          // BaseWhiteNoLighting is the default material Ogre uses
          // when it sees a mesh with no material.  Here we replace
          // that with our default_material which gets colored with
          // new_message->color.
          entity_->getSubEntity(i)->setMaterial(default_material);
        }
      }
    }
    else
    {
      entity_->setMaterial(default_material);
    }

    // add a pass to every material to perform the color tinting
    S_MaterialPtr::iterator material_it;
    for (material_it = materials_.begin(); material_it != materials_.end(); material_it++)
    {
      Ogre::Technique* technique = (*material_it)->getTechnique(0);
      color_tint_passes_.push_back(technique->createPass());
    }

    // always update color on resource change
    update_color = true;

    handler_.reset(new MarkerSelectionHandler(this, MarkerID(new_message->ns, new_message->id), context_));
    handler_->addTrackedObject(entity_);
  }
  else
  {
    // underlying mesh resource has not changed but if the color has
    //  then we need to update the materials color
    if (!old_message
        || old_message->color.r != new_message->color.r
        || old_message->color.g != new_message->color.g
        || old_message->color.b != new_message->color.b
        || old_message->color.a != new_message->color.a)
    {
      update_color = true;
    }
  }

  // update material color
  //  if the mesh_use_embedded_materials is true and color is non-zero
  //  then the color will be used to tint the embedded materials
  if (update_color)
  {
    float r = new_message->color.r;
    float g = new_message->color.g;
    float b = new_message->color.b;
    float a = new_message->color.a;

    Ogre::SceneBlendType blending;
    bool depth_write;

    if (a < 0.9998)
    {
      blending = Ogre::SBT_TRANSPARENT_ALPHA;
      depth_write = false;
    }
    else
    {
      blending = Ogre::SBT_REPLACE;
      depth_write = true;
    }

    for (std::vector<Ogre::Pass*>::iterator it = color_tint_passes_.begin();
         it != color_tint_passes_.end();
         ++it)
    {
      (*it)->setAmbient(0.5 * r, 0.5 * g, 0.5 * b);
      (*it)->setDiffuse(r, g, b, a);
      (*it)->setSceneBlending(blending);
      (*it)->setDepthWriteEnabled(depth_write);
      (*it)->setLightingEnabled(true);
    }
  }

  Ogre::Vector3 pos, scale;
  Ogre::Quaternion orient;
  transform(new_message, pos, orient, scale);

  scene_node_->setVisible(true);
  setPosition(pos);
  setOrientation(orient);

  scene_node_->setScale(scale);
}
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() ));
}
void MarkerDisplay::failedMarker(const visualization_msgs::Marker::ConstPtr& marker, tf::FilterFailureReason reason)
{
  std::string error = context_->getFrameManager()->discoverFailureReason(marker->header.frame_id, marker->header.stamp, marker->__connection_header ? (*marker->__connection_header)["callerid"] : "unknown", reason);
  setMarkerStatus(MarkerID(marker->ns, marker->id), StatusProperty::Error, error);
}