Exemplo n.º 1
0
bool validateFloats(const nav_msgs::OccupancyGrid& msg)
{
  bool valid = true;
  valid = valid && validateFloats( msg.info.resolution );
  valid = valid && validateFloats( msg.info.origin );
  return valid;
}
Exemplo n.º 2
0
bool validateFloats(const nav_msgs::Odometry& msg)
{
  bool valid = true;
  valid = valid && validateFloats( msg.pose.pose );
  valid = valid && validateFloats( msg.twist.twist );
  return valid;
}
Exemplo n.º 3
0
bool validateFloats(const sensor_msgs::CameraInfo& msg)
{
  bool valid = true;
  valid = valid && validateFloats(msg.D);
  valid = valid && validateFloats(msg.K);
  valid = valid && validateFloats(msg.R);
  valid = valid && validateFloats(msg.P);
  return valid;
}
Exemplo n.º 4
0
bool validateFloats(const visualization_msgs::Marker& msg)
{
  bool valid = true;
  valid = valid && validateFloats(msg.pose);
  valid = valid && validateFloats(msg.scale);
  valid = valid && validateFloats(msg.color);
  valid = valid && validateFloats(msg.points);
  return valid;
}
void InteractiveMarkerDisplay::updatePoses(
    const std::string& server_id,
    const std::vector<visualization_msgs::InteractiveMarkerPose>& marker_poses )
{
  M_StringToIMPtr& im_map = getImMap( server_id );

  for ( size_t i=0; i<marker_poses.size(); i++ )
  {
    const visualization_msgs::InteractiveMarkerPose& marker_pose = marker_poses[i];

    if ( !validateFloats( marker_pose.pose ) )
    {
      setStatusStd( StatusProperty::Error, marker_pose.name, "Pose message contains invalid floats!" );
      return;
    }

    std::map< std::string, IMPtr >::iterator int_marker_entry = im_map.find( marker_pose.name );

    if ( int_marker_entry != im_map.end() )
    {
      int_marker_entry->second->processMessage( marker_pose );
    }
    else
    {
      setStatusStd( StatusProperty::Error, marker_pose.name, "Pose received for non-existing marker '" + marker_pose.name );
      unsubscribe();
      return;
    }
  }
}
Exemplo n.º 6
0
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() );
  }
}
Exemplo n.º 7
0
void PoseArrayDisplay::processMessage( const geometry_msgs::PoseArray::ConstPtr& msg )
{
  if( !validateFloats( *msg ))
  {
    setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
    return;
  }

  manual_object_->clear();

  Ogre::Vector3 position;
  Ogre::Quaternion orientation;
  if( !context_->getFrameManager()->getTransform( msg->header, position, orientation ))
  {
    ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
  }

  scene_node_->setPosition( position );
  scene_node_->setOrientation( orientation );

  manual_object_->clear();

  Ogre::ColourValue color = color_property_->getOgreColor();
  float length = length_property_->getFloat();
  size_t num_poses = msg->poses.size();
  manual_object_->estimateVertexCount( num_poses * 6 );
  manual_object_->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_LIST );
  for( size_t i=0; i < num_poses; ++i )
  {
    Ogre::Vector3 pos( msg->poses[i].position.x,
                       msg->poses[i].position.y,
                       msg->poses[i].position.z );
    Ogre::Quaternion orient( msg->poses[i].orientation.w,
                             msg->poses[i].orientation.x,
                             msg->poses[i].orientation.y,
                             msg->poses[i].orientation.z );
    // orient here is not normalized, so the scale of the quaternion
    // will affect the scale of the arrow.

    Ogre::Vector3 vertices[6];
    vertices[0] = pos; // back of arrow
    vertices[1] = pos + orient * Ogre::Vector3( length, 0, 0 ); // tip of arrow
    vertices[2] = vertices[ 1 ];
    vertices[3] = pos + orient * Ogre::Vector3( 0.75*length, 0.2*length, 0 );
    vertices[4] = vertices[ 1 ];
    vertices[5] = pos + orient * Ogre::Vector3( 0.75*length, -0.2*length, 0 );

    for( int i = 0; i < 6; ++i )
    {
      manual_object_->position( vertices[i] );
      manual_object_->colour( color );
    }
  }
  manual_object_->end();

  context_->queueRender();
}
Exemplo n.º 8
0
void PointCloud2Display::processMessage( const sensor_msgs::PointCloud2ConstPtr& cloud )
{
  // Filter any nan values out of the cloud.  Any nan values that make it through to PointCloudBase
  // will get their points put off in lala land, but it means they still do get processed/rendered
  // which can be a big performance hit
  sensor_msgs::PointCloud2Ptr filtered(new sensor_msgs::PointCloud2);
  int32_t xi = findChannelIndex(cloud, "x");
  int32_t yi = findChannelIndex(cloud, "y");
  int32_t zi = findChannelIndex(cloud, "z");

  if (xi == -1 || yi == -1 || zi == -1)
  {
    return;
  }

  const uint32_t xoff = cloud->fields[xi].offset;
  const uint32_t yoff = cloud->fields[yi].offset;
  const uint32_t zoff = cloud->fields[zi].offset;
  const uint32_t point_step = cloud->point_step;
  const size_t point_count = cloud->width * cloud->height;
  filtered->data.resize(cloud->data.size());
  if (point_count == 0)
  {
    return;
  }

  uint32_t output_count = 0;
  const uint8_t* ptr = &cloud->data.front();
  for (size_t i = 0; i < point_count; ++i)
  {
    float x = *reinterpret_cast<const float*>(ptr + xoff);
    float y = *reinterpret_cast<const float*>(ptr + yoff);
    float z = *reinterpret_cast<const float*>(ptr + zoff);
    if (validateFloats(Ogre::Vector3(x, y, z)))
    {
      memcpy(&filtered->data.front() + (output_count * point_step), ptr, point_step);
      ++output_count;
    }

    ptr += point_step;
  }

  filtered->header = cloud->header;
  filtered->fields = cloud->fields;
  filtered->data.resize(output_count * point_step);
  filtered->height = 1;
  filtered->width = output_count;
  filtered->is_bigendian = cloud->is_bigendian;
  filtered->point_step = point_step;
  filtered->row_step = output_count;

  point_cloud_common_->addMessage( filtered );
}
Exemplo n.º 9
0
void PolygonDisplay::processMessage(const geometry_msgs::PolygonStamped::ConstPtr& msg)
{
  if( !validateFloats( *msg ))
  {
    setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
    return;
  }

  Ogre::Vector3 position;
  Ogre::Quaternion orientation;
  if( !context_->getFrameManager()->getTransform( msg->header, position, orientation ))
  {
    ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
               msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
  }

  scene_node_->setPosition( position );
  scene_node_->setOrientation( orientation );

  manual_object_->clear();

  Ogre::ColourValue color = qtToOgre( color_property_->getColor() );
  color.a = alpha_property_->getFloat();
  // TODO: this does not actually support alpha as-is.  The
  // "BaseWhiteNoLighting" material ends up ignoring the alpha
  // component of the color values we set at each point.  Need to make
  // a material and do the whole setSceneBlending() rigamarole.

  uint32_t num_points = msg->polygon.points.size();
  if( num_points > 0 )
  {
    manual_object_->estimateVertexCount( num_points );
    manual_object_->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP );
    for( uint32_t i=0; i < num_points + 1; ++i )
    {
      const geometry_msgs::Point32& msg_point = msg->polygon.points[ i % num_points ];
      manual_object_->position( msg_point.x, msg_point.y, msg_point.z );
      manual_object_->colour( color );
    }

    manual_object_->end();
  }
}
bool validateFloats(const visualization_msgs::InteractiveMarker& msg)
{
  bool valid = true;
  valid = valid && validateFloats(msg.pose);
  valid = valid && validateFloats(msg.scale);
  for ( unsigned c=0; c<msg.controls.size(); c++)
  {
    valid = valid && validateFloats( msg.controls[c].orientation );
    for ( unsigned m=0; m<msg.controls[c].markers.size(); m++ )
    {
      valid = valid && validateFloats(msg.controls[c].markers[m].pose);
      valid = valid && validateFloats(msg.controls[c].markers[m].scale);
      valid = valid && validateFloats(msg.controls[c].markers[m].color);
      valid = valid && validateFloats(msg.controls[c].markers[m].points);
    }
  }
  return valid;
}
Exemplo n.º 11
0
void OdometryDisplay::incomingMessage( const nav_msgs::Odometry::ConstPtr& message )
{
  ++messages_received_;

  if( !validateFloats( *message ))
  {
    setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
    return;
  }

  setStatus( StatusProperty::Ok, "Topic", QString::number( messages_received_ ) + " messages received" );

  if( last_used_message_ )
  {
    Ogre::Vector3 last_position(last_used_message_->pose.pose.position.x, last_used_message_->pose.pose.position.y, last_used_message_->pose.pose.position.z);
    Ogre::Vector3 current_position(message->pose.pose.position.x, message->pose.pose.position.y, message->pose.pose.position.z);
    Ogre::Quaternion last_orientation(last_used_message_->pose.pose.orientation.w, last_used_message_->pose.pose.orientation.x, last_used_message_->pose.pose.orientation.y, last_used_message_->pose.pose.orientation.z);
    Ogre::Quaternion current_orientation(message->pose.pose.orientation.w, message->pose.pose.orientation.x, message->pose.pose.orientation.y, message->pose.pose.orientation.z);

    if( (last_position - current_position).length() < position_tolerance_property_->getFloat() &&
        (last_orientation - current_orientation).normalise() < angle_tolerance_property_->getFloat() )
    {
      return;
    }
  }

  Arrow* arrow = new Arrow( scene_manager_, scene_node_, 0.8f, 0.05f, 0.2f, 0.2f );

  transformArrow( message, arrow );

  QColor color = color_property_->getColor();
  arrow->setColor( color.redF(), color.greenF(), color.blueF(), 1.0f );

  float length = length_property_->getFloat();
  Ogre::Vector3 scale( length, length, length );
  arrow->setScale( scale );

  arrows_.push_back( arrow );

  last_used_message_ = message;
  context_->queueRender();
}
  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);
      }
    }
  }
Exemplo n.º 13
0
void PathDisplay::processMessage( const nav_msgs::Path::ConstPtr& msg )
{
  Ogre::ManualObject* manual_object = manual_objects_[ messages_received_ % buffer_length_property_->getInt() ];
  manual_object->clear();

  if( !validateFloats( *msg ))
  {
    setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
    return;
  }

  Ogre::Vector3 position;
  Ogre::Quaternion orientation;
  if( !context_->getFrameManager()->getTransform( msg->header, position, orientation ))
  {
    ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
  }

  Ogre::Matrix4 transform( orientation );
  transform.setTrans( position );

//  scene_node_->setPosition( position );
//  scene_node_->setOrientation( orientation );

  Ogre::ColourValue color = color_property_->getOgreColor();
  color.a = alpha_property_->getFloat();

  uint32_t num_points = msg->poses.size();
  manual_object->estimateVertexCount( num_points );
  manual_object->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP );
  for( uint32_t i=0; i < num_points; ++i)
  {
    const geometry_msgs::Point& pos = msg->poses[ i ].pose.position;
    Ogre::Vector3 xpos = transform * Ogre::Vector3( pos.x, pos.y, pos.z );
    manual_object->position( xpos.x, xpos.y, xpos.z );
    manual_object->colour( color );
  }

  manual_object->end();
}
Exemplo n.º 14
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 InteractiveMarkerDisplay::updateMarkers(
    const std::string& server_id,
    const std::vector<visualization_msgs::InteractiveMarker>& markers
    )
{
  M_StringToIMPtr& im_map = getImMap( server_id );

  for ( size_t i=0; i<markers.size(); i++ )
  {
    const visualization_msgs::InteractiveMarker& marker = markers[i];

    if ( !validateFloats( marker ) )
    {
      setStatusStd( StatusProperty::Error, marker.name, "Marker contains invalid floats!" );
      //setStatusStd( StatusProperty::Error, "General", "Marker " + marker.name + " contains invalid floats!" );
      continue;
    }
    ROS_DEBUG("Processing interactive marker '%s'. %d", marker.name.c_str(), (int)marker.controls.size() );

    std::map< std::string, IMPtr >::iterator int_marker_entry = im_map.find( marker.name );

    if ( int_marker_entry == im_map.end() )
    {
      int_marker_entry = im_map.insert( std::make_pair(marker.name, IMPtr ( new InteractiveMarker(this, context_, topic_ns_, client_id_) ) ) ).first;
    }

    if ( int_marker_entry->second->processMessage( marker ) )
    {
      int_marker_entry->second->setShowAxes( show_axes_property_->getBool() );
      int_marker_entry->second->setShowDescription( show_descriptions_property_->getBool() );
    }
    else
    {
      unsubscribe();
      return;
    }
  }
}
Exemplo n.º 16
0
bool CameraPub::updateCamera()
{
  sensor_msgs::CameraInfo::ConstPtr info;
  {
    boost::mutex::scoped_lock lock(caminfo_mutex_);
    info = current_caminfo_;
  }

  if (!info)
  {
    return false;
  }

  if (!validateFloats(*info))
  {
    setStatus(StatusProperty::Error, "Camera Info", "Contains invalid floating point values (nans or infs)");
    return false;
  }

  // if we're in 'exact' time mode, only show image if the time is exactly right
  ros::Time rviz_time = context_->getFrameManager()->getTime();
  if (context_->getFrameManager()->getSyncMode() == FrameManager::SyncExact &&
      rviz_time != info->header.stamp)
  {
    std::ostringstream s;
    s << "Time-syncing active and no info at timestamp " << rviz_time.toSec() << ".";
    setStatus(StatusProperty::Warn, "Time", s.str().c_str());
    return false;
  }

  // TODO(lucasw) this will make the img vs. texture size code below unnecessary
  if ((info->width != render_texture_->getWidth()) ||
      (info->height != render_texture_->getHeight()))
  {
    rtt_texture_ = Ogre::TextureManager::getSingleton().createManual(
        "RttTex",
        Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
        Ogre::TEX_TYPE_2D,
        info->width, info->height,
        0,
        Ogre::PF_R8G8B8,
        Ogre::TU_RENDERTARGET);
    render_texture_ = rtt_texture_->getBuffer()->getRenderTarget();
    render_texture_->addViewport(camera_);
    render_texture_->getViewport(0)->setClearEveryFrame(true);
    render_texture_->getViewport(0)->setBackgroundColour(Ogre::ColourValue::Black);
    render_texture_->getViewport(0)->setVisibilityMask(vis_bit_);

    render_texture_->getViewport(0)->setOverlaysEnabled(false);
    render_texture_->setAutoUpdated(false);
    render_texture_->setActive(false);
    render_texture_->addListener(this);
  }

  Ogre::Vector3 position;
  Ogre::Quaternion orientation;
  context_->getFrameManager()->getTransform(info->header.frame_id, info->header.stamp, position, orientation);

  // printf( "CameraPub:updateCamera(): pos = %.2f, %.2f, %.2f.\n", position.x, position.y, position.z );

  // convert vision (Z-forward) frame to ogre frame (Z-out)
  orientation = orientation * Ogre::Quaternion(Ogre::Degree(180), Ogre::Vector3::UNIT_X);

  float img_width = info->width;
  float img_height = info->height;

  // If the image width is 0 due to a malformed caminfo, try to grab the width from the image.
  if (img_width == 0)
  {
    ROS_DEBUG("Malformed CameraInfo on camera [%s], width = 0", qPrintable(getName()));
    img_width = 640;
  }

  if (img_height == 0)
  {
    ROS_DEBUG("Malformed CameraInfo on camera [%s], height = 0", qPrintable(getName()));
    img_height = 480;
  }

  if (img_height == 0.0 || img_width == 0.0)
  {
    setStatus(StatusProperty::Error, "Camera Info",
              "Could not determine width/height of image due to malformed CameraInfo (either width or height is 0)");
    return false;
  }

  double fx = info->P[0];
  double fy = info->P[5];

  float win_width = render_texture_->getWidth();
  float win_height = render_texture_->getHeight();
  float zoom_x = 1.0;
  float zoom_y = zoom_x;

  // Preserve aspect ratio
  if (win_width != 0 && win_height != 0)
  {
    float img_aspect = (img_width / fx) / (img_height / fy);
    float win_aspect = win_width / win_height;

    if (img_aspect > win_aspect)
    {
      zoom_y = zoom_y / img_aspect * win_aspect;
    }
    else
    {
      zoom_x = zoom_x / win_aspect * img_aspect;
    }
  }

  // Add the camera's translation relative to the left camera (from P[3]);
  double tx = -1 * (info->P[3] / fx);
  Ogre::Vector3 right = orientation * Ogre::Vector3::UNIT_X;
  position = position + (right * tx);

  double ty = -1 * (info->P[7] / fy);
  Ogre::Vector3 down = orientation * Ogre::Vector3::UNIT_Y;
  position = position + (down * ty);

  if (!validateFloats(position))
  {
    setStatus(StatusProperty::Error, "Camera Info",
        "CameraInfo/P resulted in an invalid position calculation (nans or infs)");
    return false;
  }

  camera_->setPosition(position);
  camera_->setOrientation(orientation);

  // calculate the projection matrix
  double cx = info->P[2];
  double cy = info->P[6];

  double far_plane = 100;
  double near_plane = 0.01;

  Ogre::Matrix4 proj_matrix;
  proj_matrix = Ogre::Matrix4::ZERO;

  proj_matrix[0][0] = 2.0 * fx / img_width * zoom_x;
  proj_matrix[1][1] = 2.0 * fy / img_height * zoom_y;

  proj_matrix[0][2] = 2.0 * (0.5 - cx / img_width) * zoom_x;
  proj_matrix[1][2] = 2.0 * (cy / img_height - 0.5) * zoom_y;

  proj_matrix[2][2] = -(far_plane + near_plane) / (far_plane - near_plane);
  proj_matrix[2][3] = -2.0 * far_plane * near_plane / (far_plane - near_plane);

  proj_matrix[3][2] = -1;

  camera_->setCustomProjectionMatrix(true, proj_matrix);

  setStatus(StatusProperty::Ok, "Camera Info", "OK");

#if 0
  static Axes* debug_axes = new Axes(scene_manager_, 0, 0.2, 0.01);
  debug_axes->setPosition(position);
  debug_axes->setOrientation(orientation);
#endif

  setStatus(StatusProperty::Ok, "Time", "ok");

  return true;
}
void CameraDisplay::updateCamera()
{
  sensor_msgs::CameraInfo::ConstPtr info;
  sensor_msgs::Image::ConstPtr image;
  {
    boost::mutex::scoped_lock lock( caminfo_mutex_ );

    info = current_caminfo_;
    image = texture_.getImage();
  }

  if( !info || !image )
  {
    return;
  }

  if( !validateFloats( *info ))
  {
    setStatus( StatusProperty::Error, "Camera Info", "Contains invalid floating point values (nans or infs)" );
    return;
  }

  Ogre::Vector3 position;
  Ogre::Quaternion orientation;
  context_->getFrameManager()->getTransform( image->header.frame_id, ros::Time(0), position, orientation );

  //printf( "CameraDisplay:updateCamera(): pos = %.2f, %.2f, %.2f.\n", position.x, position.y, position.z );

  // convert vision (Z-forward) frame to ogre frame (Z-out)
  orientation = orientation * Ogre::Quaternion( Ogre::Degree( 180 ), Ogre::Vector3::UNIT_X );

  float img_width = info->width;
  float img_height = info->height;

  // If the image width is 0 due to a malformed caminfo, try to grab the width from the image.
  if( img_width == 0 )
  {
    ROS_DEBUG( "Malformed CameraInfo on camera [%s], width = 0", qPrintable( getName() ));

    img_width = texture_.getWidth();
  }

  if (img_height == 0)
  {
    ROS_DEBUG( "Malformed CameraInfo on camera [%s], height = 0", qPrintable( getName() ));

    img_height = texture_.getHeight();
  }

  if( img_height == 0.0 || img_width == 0.0 )
  {
    setStatus( StatusProperty::Error, "Camera Info",
               "Could not determine width/height of image due to malformed CameraInfo (either width or height is 0)" );
    return;
  }

  double fx = info->P[0];
  double fy = info->P[5];

  float win_width = render_panel_->width();
  float win_height = render_panel_->height();
  float zoom_x = zoom_property_->getFloat();
  float zoom_y = zoom_x;

  // Preserve aspect ratio
  if( win_width != 0 && win_height != 0 )
  {
    float img_aspect = (img_width/fx) / (img_height/fy);
    float win_aspect = win_width / win_height;

    if ( img_aspect > win_aspect )
    {
      zoom_y = zoom_y / img_aspect * win_aspect;
    }
    else
    {
      zoom_x = zoom_x / win_aspect * img_aspect;
    }
  }

  // Add the camera's translation relative to the left camera (from P[3]);
  double tx = -1 * (info->P[3] / fx);
  Ogre::Vector3 right = orientation * Ogre::Vector3::UNIT_X;
  position = position + (right * tx);

  double ty = -1 * (info->P[7] / fy);
  Ogre::Vector3 down = orientation * Ogre::Vector3::UNIT_Y;
  position = position + (down * ty);

  if( !validateFloats( position ))
  {
    setStatus( StatusProperty::Error, "Camera Info", "CameraInfo/P resulted in an invalid position calculation (nans or infs)" );
    return;
  }

  render_panel_->getCamera()->setPosition( position );
  render_panel_->getCamera()->setOrientation( orientation );

  // calculate the projection matrix
  double cx = info->P[2];
  double cy = info->P[6];

  double far_plane = 100;
  double near_plane = 0.01;

  Ogre::Matrix4 proj_matrix;
  proj_matrix = Ogre::Matrix4::ZERO;
 
  proj_matrix[0][0]= 2.0 * fx/img_width * zoom_x;
  proj_matrix[1][1]= 2.0 * fy/img_height * zoom_y;

  proj_matrix[0][2]= 2.0 * (0.5 - cx/img_width) * zoom_x;
  proj_matrix[1][2]= 2.0 * (cy/img_height - 0.5) * zoom_y;

  proj_matrix[2][2]= -(far_plane+near_plane) / (far_plane-near_plane);
  proj_matrix[2][3]= -2.0*far_plane*near_plane / (far_plane-near_plane);

  proj_matrix[3][2]= -1;

  render_panel_->getCamera()->setCustomProjectionMatrix( true, proj_matrix );

  setStatus( StatusProperty::Ok, "Camera Info", "OK" );

#if 0
  static Axes* debug_axes = new Axes(scene_manager_, 0, 0.2, 0.01);
  debug_axes->setPosition(position);
  debug_axes->setOrientation(orientation);
#endif

  //adjust the image rectangles to fit the zoom & aspect ratio
  bg_screen_rect_->setCorners( -1.0f*zoom_x, 1.0f*zoom_y, 1.0f*zoom_x, -1.0f*zoom_y );
  fg_screen_rect_->setCorners( -1.0f*zoom_x, 1.0f*zoom_y, 1.0f*zoom_x, -1.0f*zoom_y );

  Ogre::AxisAlignedBox aabInf;
  aabInf.setInfinite();
  bg_screen_rect_->setBoundingBox( aabInf );
  fg_screen_rect_->setBoundingBox( aabInf );
}
Exemplo n.º 18
0
bool validateFloats( const geometry_msgs::PoseArray& msg )
{
  return validateFloats( msg.poses );
}
Exemplo n.º 19
0
void MapDisplay::showMap()
{
  if (current_map_.data.empty())
  {
    return;
  }

  if( !validateFloats( current_map_ ))
  {
    setStatus( StatusProperty::Error, "Map", "Message contained invalid floating point values (nans or infs)" );
    return;
  }

  if( current_map_.info.width * current_map_.info.height == 0 )
  {
    std::stringstream ss;
    ss << "Map is zero-sized (" << current_map_.info.width << "x" << current_map_.info.height << ")";
    setStatus( StatusProperty::Error, "Map", QString::fromStdString( ss.str() ));
    return;
  }

  setStatus( StatusProperty::Ok, "Message", "Map received" );

  ROS_DEBUG( "Received a %d X %d map @ %.3f m/pix\n",
             current_map_.info.width,
             current_map_.info.height,
             current_map_.info.resolution );

  float resolution = current_map_.info.resolution;

  int width = current_map_.info.width;
  int height = current_map_.info.height;


  Ogre::Vector3 position( current_map_.info.origin.position.x,
                          current_map_.info.origin.position.y,
                          current_map_.info.origin.position.z );
  Ogre::Quaternion orientation( current_map_.info.origin.orientation.w,
                                current_map_.info.origin.orientation.x,
                                current_map_.info.origin.orientation.y,
                                current_map_.info.origin.orientation.z );
  frame_ = current_map_.header.frame_id;
  if (frame_.empty())
  {
    frame_ = "/map";
  }

  unsigned int pixels_size = width * height;
  unsigned char* pixels = new unsigned char[pixels_size];
  memset(pixels, 255, pixels_size);

  bool map_status_set = false;
  unsigned int num_pixels_to_copy = pixels_size;
  if( pixels_size != current_map_.data.size() )
  {
    std::stringstream ss;
    ss << "Data size doesn't match width*height: width = " << width
       << ", height = " << height << ", data size = " << current_map_.data.size();
    setStatus( StatusProperty::Error, "Map", QString::fromStdString( ss.str() ));
    map_status_set = true;

    // Keep going, but don't read past the end of the data.
    if( current_map_.data.size() < pixels_size )
    {
      num_pixels_to_copy = current_map_.data.size();
    }
  }

  memcpy( pixels, &current_map_.data[0], num_pixels_to_copy );

  Ogre::DataStreamPtr pixel_stream;
  pixel_stream.bind( new Ogre::MemoryDataStream( pixels, pixels_size ));

  if( !texture_.isNull() )
  {
    Ogre::TextureManager::getSingleton().remove( texture_->getName() );
    texture_.setNull();
  }

  static int tex_count = 0;
  std::stringstream ss;
  ss << "MapTexture" << tex_count++;
  try
  {
    texture_ = Ogre::TextureManager::getSingleton().loadRawData( ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
                                                                 pixel_stream, width, height, Ogre::PF_L8, Ogre::TEX_TYPE_2D,
                                                                 0);

    if( !map_status_set )
    {
      setStatus( StatusProperty::Ok, "Map", "Map OK" );
    }
  }
  catch(Ogre::RenderingAPIException&)
  {
    Ogre::Image image;
    pixel_stream->seek(0);
    float fwidth = width;
    float fheight = height;
    if( width > height )
    {
      float aspect = fheight / fwidth;
      fwidth = 2048;
      fheight = fwidth * aspect;
    }
    else
    {
      float aspect = fwidth / fheight;
      fheight = 2048;
      fwidth = fheight * aspect;
    }

    {
      std::stringstream ss;
      ss << "Map is larger than your graphics card supports.  Downsampled from [" << width << "x" << height << "] to [" << fwidth << "x" << fheight << "]";
      setStatus(StatusProperty::Ok, "Map", QString::fromStdString( ss.str() ));
    }

    ROS_WARN("Failed to create full-size map texture, likely because your graphics card does not support textures of size > 2048.  Downsampling to [%d x %d]...", (int)fwidth, (int)fheight);
    //ROS_INFO("Stream size [%d], width [%f], height [%f], w * h [%f]", pixel_stream->size(), width, height, width * height);
    image.loadRawData(pixel_stream, width, height, Ogre::PF_L8);
    image.resize(fwidth, fheight, Ogre::Image::FILTER_NEAREST);
    ss << "Downsampled";
    texture_ = Ogre::TextureManager::getSingleton().loadImage(ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, image);
  }

  delete [] pixels;

  Ogre::Pass* pass = material_->getTechnique(0)->getPass(0);
  Ogre::TextureUnitState* tex_unit = NULL;
  if (pass->getNumTextureUnitStates() > 0)
  {
    tex_unit = pass->getTextureUnitState(0);
  }
  else
  {
    tex_unit = pass->createTextureUnitState();
  }

  tex_unit->setTextureName(texture_->getName());
  tex_unit->setTextureFiltering( Ogre::TFO_NONE );

  updatePalette();

  resolution_property_->setValue( resolution );
  width_property_->setValue( width );
  height_property_->setValue( height );
  position_property_->setVector( position );
  orientation_property_->setQuaternion( orientation );

  transformMap();
  manual_object_->setVisible( true );
  scene_node_->setScale( resolution * width, resolution * height, 1.0 );

  context_->queueRender();
}
void DepthCloudDisplay::convert(const sensor_msgs::ImageConstPtr& depth_msg,
                                const sensor_msgs::ImageConstPtr& color_msg,
                                const sensor_msgs::CameraInfo::ConstPtr camInfo_msg,
                                sensor_msgs::PointCloud2Ptr& cloud_msg)
  {
    int width = depth_msg->width;
    int height = depth_msg->height;

    //////////////////////////////
    // initialize cloud message
    //////////////////////////////

    cloud_msg->header = depth_msg->header;

    if (color_msg)
    {
      cloud_msg->fields.resize(4);
      cloud_msg->fields[0].name = "x";
      cloud_msg->fields[1].name = "y";
      cloud_msg->fields[2].name = "z";
      cloud_msg->fields[3].name = "rgb";
    }
    else
    {
      cloud_msg->fields.resize(3);
      cloud_msg->fields[0].name = "x";
      cloud_msg->fields[1].name = "y";
      cloud_msg->fields[2].name = "z";
    }

    int offset = 0;
    // All offsets are *4, as all field data types are float32
    for (size_t d = 0; d < cloud_msg->fields.size(); ++d, offset += 4)
    {
      cloud_msg->fields[d].offset = offset;
      cloud_msg->fields[d].datatype = sensor_msgs::PointField::FLOAT32;
      cloud_msg->fields[d].count = 1;
    }

    cloud_msg->point_step = offset;

    cloud_msg->data.resize(height * width * offset);
    cloud_msg->is_bigendian = false;
    cloud_msg->is_dense = false;

    //////////////////////////////
    // Update camera model
    //////////////////////////////

    image_geometry::PinholeCameraModel cameraModel;

    if (camInfo_msg)
    {
      cameraModel.fromCameraInfo(camInfo_msg);
    }
    else
    {
      setStatus(StatusProperty::Error, "Message", QString("Waiting for CameraInfo message.."));
      return;
    }

    // Use correct principal point from calibration
    float center_x = cameraModel.cx();
    float center_y = cameraModel.cy();

    // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
    float constant_x = 1.0f / cameraModel.fx();
    float constant_y = 1.0f / cameraModel.fy();

    //////////////////////////////
    // Color conversion
    //////////////////////////////

    unsigned char* colorImgPtr = 0;

    if (color_msg)
    {

      if (depth_msg->header.frame_id != color_msg->header.frame_id)
      {
        std::stringstream errorMsg;
        errorMsg << "Depth image frame id [" << depth_msg->header.frame_id.c_str()
            << "] doesn't match RGB image frame id [" << color_msg->header.frame_id.c_str() << "]";
        setStatus(StatusProperty::Error, "Message", QString(errorMsg.str().c_str()) );
        return;
      }

      if (depth_msg->width != color_msg->width || depth_msg->height != color_msg->height)
      {
        std::stringstream errorMsg;
        errorMsg << "Depth resolution (" << (int)depth_msg->width << "x" << (int)depth_msg->height << ") "
            "does not match RGB resolution (" << (int)color_msg->width << "x" << (int)color_msg->height << ")";
        setStatus(StatusProperty::Error, "Message", QString(errorMsg.str().c_str()) );
        return;
      }

      // OpenCV-ros bridge
      cv_bridge::CvImagePtr cv_ptr;
      try
      {
        cv_ptr = cv_bridge::toCvCopy(color_msg, "rgba8");

      }
      catch (cv_bridge::Exception& e)
      {
        setStatus(StatusProperty::Error, "Message", QString("OpenCV-ROS bridge: ") + e.what());
        return;
      }
      catch (cv::Exception& e)
      {
        setStatus(StatusProperty::Error, "Message", QString("OpenCV: ") + e.what());
        return;
      }

      colorImgPtr = (unsigned char*)&cv_ptr->image.data[0];
    }

    ////////////////////////////////////////////////
    // depth map to point cloud conversion
    ////////////////////////////////////////////////

    float* cloudDataPtr = reinterpret_cast<float*>(&cloud_msg->data[0]);

    size_t pointCount_ = 0;

    Ogre::Vector3 newPoint;
    Ogre::Vector3 transPoint;

    const T* img_ptr = (T*)&depth_msg->data[0];
    for (int v = 0; v < height; ++v)
    {
      for (int u = 0; u < width; ++u)
      {
        uint32_t color_rgb;

        if (colorImgPtr)
        {
          uint8_t color_r, color_g, color_b;
          color_r = *colorImgPtr; ++colorImgPtr;
          color_g = *colorImgPtr; ++colorImgPtr;
          color_b = *colorImgPtr; ++colorImgPtr;
          colorImgPtr++; // alpha padding

          color_rgb = ((uint32_t)color_r << 16 | (uint32_t)color_g << 8 | (uint32_t)color_b);
        }

        float depth = DepthTraits<T>::toMeters( *img_ptr++ );

        // Missing points denoted by NaNs
        if (!DepthTraits<T>::valid(depth))
        {
          continue;
        }

        // Fill in XYZ
        newPoint.x = (u - center_x) * depth * constant_x;
        newPoint.y = (v - center_y) * depth * constant_y;
        newPoint.z = depth;

        if (validateFloats(newPoint))
        {
          *cloudDataPtr = newPoint.x; ++cloudDataPtr;
          *cloudDataPtr = newPoint.y; ++cloudDataPtr;
          *cloudDataPtr = newPoint.z; ++cloudDataPtr;
        }

        ++pointCount_;

        if (colorImgPtr)
        {
          *cloudDataPtr = *reinterpret_cast<float*>(&color_rgb);
          ++cloudDataPtr;
        }
      }
    }

    ////////////////////////////////////////////////
    // finalize pointcloud2 message
    ////////////////////////////////////////////////
    cloud_msg->width = pointCount_;
    cloud_msg->height = 1;
    cloud_msg->data.resize(cloud_msg->height * cloud_msg->width * cloud_msg->point_step);
    cloud_msg->row_step = cloud_msg->point_step * cloud_msg->width;
  }
Exemplo n.º 21
0
bool validateFloats( const geometry_msgs::PolygonStamped& msg )
{
  return validateFloats(msg.polygon.points);
}
void Selection3DDisplayCustom::preRenderTargetUpdate( const Ogre::RenderTargetEvent& evt )
{
    if(initialized_)
    {
        if(constant_size_property_->getBool())
        {
            int view_id = 0;//((rviz::VisualizationManager*)context_)->getActiveViewID();

            for(int i = 0; i < render_panel_list_.size(); i++)
                if(render_panel_list_[i]->getRenderWindow() == (Ogre::RenderWindow*)evt.source)
                    view_id = i;

            //std::cout << "Update " << view_id << std::endl;

            // get transform from world to fixed frame
            Ogre::Quaternion selection_orientation(1,0,0,0);
            Ogre::Vector3 selection_position = selection_position_;
            transform(selection_position,selection_orientation,"/world",fixed_frame_.toUtf8().constData());
            //std::cout << "selection: " << selection_position.x << "," << selection_position.y << "," << selection_position.z << std::endl;

    //        Ogre::Quaternion selection_orientation_roi(1,0,0,0);
    //        Ogre::Vector3 selection_position_roi = selection_position_roi_;
    //        transform(selection_position_roi,selection_orientation_roi,"/world",fixed_frame_.toUtf8().constData());
            //std::cout << selection_position_roi.x << "," << selection_position_roi.y << "," << selection_position_roi.z << std::endl;

            // get camera position to calculate selection marker
            //std::cout << "selection " << render_panel_->getCamera() << std::endl;
            Ogre::Vector3 camera_position = this->render_panel_list_[view_id]->getCamera()->getPosition();
            //std::cout << this->context_->getSceneManager()->getCameras().size() << std::endl;
            //Ogre::SceneManager::CameraList camera_list = this->context_->getSceneManager()->getCameras();
            //Ogre::SceneManager::CameraIterator camera_list_iterator = this->context_->getSceneManager()->getCameraIterator();
            //while(camera_list_iterator.hasMoreElements())
            //{
            //    Ogre::Camera* temp_cam = camera_list_iterator.getNext();
            //    Ogre::Vector3 temp_pos = temp_cam->getDerivedPosition();
            //    std::cout << "it " << temp_pos.x << "," << temp_pos.y << "," << temp_pos.z << std::endl;
            //}

            //Ogre::Vector3 camera_position2 = this->context_->getSceneManager()->getCamera();
           // std::cout << "Camera: " << camera_position.x << "," << camera_position.y << "," << camera_position.z << std::endl;

            if(this->render_panel_list_[view_id]->getCamera()->getProjectionType() == Ogre::PT_ORTHOGRAPHIC) // if it's ortho, we need to calculate z again
            {
                Ogre::Matrix4 m = this->render_panel_list_[view_id]->getCamera()->getProjectionMatrix();
                float near   =  (1+m[2][3])/m[2][2];
                float far    = -(1-m[2][3])/m[2][2];
                float bottom =  (1-m[1][3])/m[1][1];
                float top    = -(1+m[1][3])/m[1][1];
                float left   = -(1+m[0][3])/m[0][0];
                float right  =  (1-m[0][3])/m[0][0];
                //std::cout << "ortho:\n\t" << left << "\n\t" << right << "\n\t" << bottom << "\n\t" << top << "\n\t" << near << "\n\t" << far << std::endl;
                if(this->render_panel_list_[view_id]->getCamera()->getPosition().z == 500)
                    camera_position.z = fabs(bottom)+fabs(top);
                else if(this->render_panel_list_[view_id]->getCamera()->getPosition().y == -500)
                    camera_position.y = fabs(bottom)+fabs(top);
                else if(this->render_panel_list_[view_id]->getCamera()->getPosition().x == 500)
                    camera_position.x = fabs(bottom)+fabs(top);
            }

            //std::cout << "camera: " << camera_position.x << "," << camera_position.y << "," << camera_position.z << std::endl;

            // get the current fov
            float size = marker_scale_property_->getFloat();

            // calculate distance from markers
            if(validateFloats(selection_position))
            {
                float distance_selection = camera_position.distance(selection_position);
                //std::cout << distance_selection << std::endl;
                if(distance_selection != std::numeric_limits<float>::infinity() && !(distance_selection != distance_selection)) // check for inf and nan
                {
                    if(!selection_position.isNaN()) selection_marker_->setPosition(selection_position);
                    float scale_selection = 2.0f * distance_selection * tan(this->render_panel_list_[view_id]->getCamera()->getFOVy().valueRadians() / 2.0f);//distance_selection*tan(radians)*0.002f;
                    //std::cout << scale_selection << std::endl;
                    if(scale_selection > 0.0f) selection_marker_->setScale(scale_selection*size,scale_selection*size,scale_selection*size);

    //                if(validateFloats(selection_position_roi))
    //                {
    //                    float distance_selection_roi = camera_position.distance(selection_position_roi);
    //                    //std::cout << distance_selection_roi << std::endl; // <<< inf
    //                    if(distance_selection_roi != std::numeric_limits<float>::infinity() && !(distance_selection_roi != distance_selection_roi))
    //                    {
    //                        if(!selection_position_roi.isNaN()) roi_marker_final_->setPosition(selection_position_roi);
    //                        float scale_selection_roi = 2.0f * distance_selection_roi * tan(this->render_panel_list_[view_id]->getCamera()->getFOVy().valueRadians() / 2.0f);// = distance_selection_roi*tan(radians)*0.002f;
    //                        //std::cout << scale_selection_roi << std::endl;
    //                        if(scale_selection_roi > 0.0f) roi_marker_final_->setScale(scale_selection_roi*size,scale_selection_roi*size,scale_selection_roi*size);

    //                        Ogre::Vector3 box_position = (selection_position+selection_position_roi)/2.0f;

    //                        if(!box_position.isNaN()) roi_marker_box_->setPosition(box_position);
    //                        if(!selection_orientation.isNaN()) roi_marker_box_->setOrientation(selection_orientation);
    //                    }
    //                }
                }
            }
        }
        else
        {
            Ogre::Vector3 selection_position = selection_position_;
            Ogre::Quaternion selection_orientation(1,0,0,0);
            transform(selection_position,selection_orientation,"/world",fixed_frame_.toUtf8().constData());

            // only set position if values are valid
            if(validateFloats(selection_position) && marker_scale_property_->getFloat() > 0.0f)
            {
                selection_marker_->setPosition(selection_position);
                selection_marker_->setScale(marker_scale_property_->getFloat(),marker_scale_property_->getFloat(),marker_scale_property_->getFloat());
            }
        }
    }

    context_->queueRender();
}
Exemplo n.º 23
0
bool validateFloats( const nav_msgs::Path& msg )
{
  bool valid = true;
  valid = valid && validateFloats( msg.poses );
  return valid;
}
  void FootstepDisplay::processMessage(const jsk_footstep_msgs::FootstepArray::ConstPtr& msg)
  {
    if (!validateFloats(*msg)) {
      setStatus(rviz::StatusProperty::Error, "Topic", "message contained invalid floating point values (nans or infs)");
      return;
    }
    latest_footstep_ = msg;
    Ogre::Quaternion orientation;
    Ogre::Vector3 position;
    if(!context_->getFrameManager()->getTransform( msg->header.frame_id,
                                                    msg->header.stamp,
                                                   position, orientation)) {
      ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
                 msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
      return;
    }

    // check thhe length of the shapes_
    allocateCubes(msg->footsteps.size());
    allocateTexts(msg->footsteps.size());
    line_->clear();
    line_->setLineWidth(0.01);
    line_->setNumLines(1);
    line_->setMaxPointsPerLine(1024);

    for (size_t i = 0; i < msg->footsteps.size(); i++)
    {
      ShapePtr shape = shapes_[i];
      rviz::MovableText* text = texts_[i];
      Ogre::SceneNode* node = text_nodes_[i];
      jsk_footstep_msgs::Footstep footstep = msg->footsteps[i];
      Ogre::Vector3 step_position;
      Ogre::Quaternion step_quaternion;
      if( !context_->getFrameManager()->transform( msg->header, footstep.pose,
                                                   step_position,
                                                   step_quaternion ))
      {
        ROS_ERROR( "Error transforming pose '%s' from frame '%s' to frame '%s'",
                   qPrintable( getName() ), msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
        return;
      }
      shape->setPosition(step_position);
      shape->setOrientation(step_quaternion);
      // size of shape
      Ogre::Vector3 scale;
      if (footstep.dimensions.x == 0 &&
          footstep.dimensions.y == 0 &&
          footstep.dimensions.z == 0) {
        scale[0] = depth_;
        scale[1] = width_;
        scale[2] = height_;
      }
      else {
        scale[0] = footstep.dimensions.x;
        scale[1] = footstep.dimensions.y;
        scale[2] = footstep.dimensions.z;
      }
      shape->setScale(scale);      
      // update the size of text
      if (footstep.leg == jsk_footstep_msgs::Footstep::LEFT) {
        text->setCaption("L");
      }
      else if (footstep.leg == jsk_footstep_msgs::Footstep::RIGHT) {
        text->setCaption("R");
      }
      else {
        text->setCaption("unknown");
      }
      text->setCharacterHeight(estimateTextSize(footstep));
      node->setPosition(step_position);
      node->setOrientation(step_quaternion);
      text->setVisible(show_name_); // TODO
      line_->addPoint(step_position);
      
    }
    //updateFootstepSize();
    updateAlpha();
    context_->queueRender();
  }
  void FootstepDisplay::processMessage(const jsk_footstep_msgs::FootstepArray::ConstPtr& msg)
  {
    if (!validateFloats(*msg)) {
      setStatus(rviz::StatusProperty::Error, "Topic", "message contained invalid floating point values (nans or infs)");
      return;
    }
    latest_footstep_ = msg;
    Ogre::Quaternion orientation;
    Ogre::Vector3 position;
    if( !context_->getFrameManager()->getTransform( msg->header.frame_id,
                                                    msg->header.stamp,
                                                    position, orientation ))
    {
      ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
                 msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
      return;
    }

    // check thhe length of the shapes_
    if (msg->footsteps.size() > shapes_.size())
    {
      // need to allocate
      for (size_t i = shapes_.size(); i < msg->footsteps.size(); i++)
      {
        ShapePtr shape;
        shape.reset(new rviz::Shape(rviz::Shape::Cube, context_->getSceneManager(),
                                    scene_node_));
        shapes_.push_back(shape);
      }
    }
    else if (msg->footsteps.size() < shapes_.size())
    {
      // need to remove
      shapes_.resize(msg->footsteps.size());
    }

    line_->clear();
    line_->setLineWidth(0.01);
    line_->setNumLines(1);
    line_->setMaxPointsPerLine(1024);

    for (size_t i = 0; i < msg->footsteps.size(); i++)
    {
      ShapePtr shape = shapes_[i];
      jsk_footstep_msgs::Footstep footstep = msg->footsteps[i];
      Ogre::Vector3 step_position;
      Ogre::Quaternion step_quaternion;
      if( !context_->getFrameManager()->transform( msg->header, footstep.pose,
                                                   step_position,
                                                   step_quaternion ))
      {
        ROS_ERROR( "Error transforming pose '%s' from frame '%s' to frame '%s'",
                   qPrintable( getName() ), msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
        return;
      }
      shape->setPosition(step_position);
      shape->setOrientation(step_quaternion);

      line_->addPoint(step_position);
      
    }
    updateFootstepSize();
    updateAlpha();
    context_->queueRender();
  }
  void PolygonArrayDisplay::processMessage(const jsk_pcl_ros::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
    if (msg->polygons.size() * 2 > manual_objects_.size()) {
      for (size_t i = manual_objects_.size(); i < msg->polygons.size() * 2; i++) {
        Ogre::SceneNode* scene_node = scene_node_->createChildSceneNode();
        Ogre::ManualObject* manual_object = scene_manager_->createManualObject();
        manual_object->setDynamic( true );
        scene_node->attachObject( manual_object );
        
        manual_objects_.push_back(manual_object);
        scene_nodes_.push_back(scene_node);
      }
    }
    else if (msg->polygons.size() * 2 < manual_objects_.size()) {
      for (size_t i = msg->polygons.size() * 2; i < manual_objects_.size(); i++) {
        scene_manager_->destroyManualObject( manual_objects_[i] );
        scene_manager_->destroySceneNode( scene_nodes_[i] );
      }
      // resize the array
      manual_objects_.resize(msg->polygons.size() * 2);
      scene_nodes_.resize(msg->polygons.size() * 2);
    }

    Ogre::ColourValue color = rviz::qtToOgre( color_property_->getColor() );
    color.a = alpha_property_->getFloat();
    material_->getTechnique(0)->setAmbient( color * 0.5 );
    material_->getTechnique(0)->setDiffuse( color );
    if ( color.a < 0.9998 )
    {
      material_->getTechnique(0)->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA );
      material_->getTechnique(0)->setDepthWriteEnabled( false );
    }
    else
    {
      material_->getTechnique(0)->setSceneBlending( Ogre::SBT_REPLACE );
      material_->getTechnique(0)->setDepthWriteEnabled( true );
    }
    
    for (size_t i = 0; i < msg->polygons.size(); i++) {
      geometry_msgs::PolygonStamped polygon = msg->polygons[i];
      Ogre::SceneNode* scene_node = scene_nodes_[i * 2];
      Ogre::ManualObject* manual_object = manual_objects_[i * 2];
      Ogre::Vector3 position;
      Ogre::Quaternion orientation;
      if( !context_->getFrameManager()->getTransform( polygon.header, position, orientation )) {
        ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
                   polygon.header.frame_id.c_str(), qPrintable( fixed_frame_ ));
      }
      scene_node->setPosition( position );
      scene_node->setOrientation( orientation );
      manual_object->clear();
        
      uint32_t num_points = polygon.polygon.points.size();
      if( num_points > 0 )
      {
        manual_object->estimateVertexCount( num_points );
        manual_object->begin(material_name_, Ogre::RenderOperation::OT_TRIANGLE_FAN );
        for( uint32_t i = 0; i < num_points + 1; ++i )
        {
          const geometry_msgs::Point32& msg_point = polygon.polygon.points[ i % num_points ];
          manual_object->position( msg_point.x, msg_point.y, msg_point.z );
        }
          
        manual_object->end();
      }
    }
    // reverse order
    for (size_t i = 0; i < msg->polygons.size(); i++) {
      geometry_msgs::PolygonStamped polygon = msg->polygons[i];
      Ogre::SceneNode* scene_node = scene_nodes_[i * 2 + 1];
      Ogre::ManualObject* manual_object = manual_objects_[i * 2 + 1];
      Ogre::Vector3 position;
      Ogre::Quaternion orientation;
      if( !context_->getFrameManager()->getTransform( polygon.header, position, orientation )) {
        ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'",
                   polygon.header.frame_id.c_str(), qPrintable( fixed_frame_ ));
      }
      scene_node->setPosition( position );
      scene_node->setOrientation( orientation );
      manual_object->clear();
        
      uint32_t num_points = polygon.polygon.points.size();
      if( num_points > 0 )
      {
        manual_object->estimateVertexCount( num_points );
        manual_object->begin(material_name_, Ogre::RenderOperation::OT_TRIANGLE_FAN );
        for( uint32_t i = num_points; i > 0; --i )
        {
          const geometry_msgs::Point32& msg_point = polygon.polygon.points[ i % num_points ];
          manual_object->position( msg_point.x, msg_point.y, msg_point.z );
        }
          
        manual_object->end();
      }
    }
  }
Exemplo n.º 27
0
void PathDisplay::processMessage( const nav_msgs::Path::ConstPtr& msg )
{
  // Calculate index of oldest element in cyclic buffer
  size_t bufferIndex = messages_received_ % buffer_length_property_->getInt();

  LineStyle style = (LineStyle) style_property_->getOptionInt();
  Ogre::ManualObject* manual_object = NULL;
  rviz::BillboardLine* billboard_line = NULL;

  // Delete oldest element
  switch(style)
  {
  case LINES:
    manual_object = manual_objects_[ bufferIndex ];
    manual_object->clear();
    break;

  case BILLBOARDS:
    billboard_line = billboard_lines_[ bufferIndex ];
    billboard_line->clear();
    break;
  }

  // Check if path contains invalid coordinate values
  if( !validateFloats( *msg ))
  {
    setStatus( StatusProperty::Error, "Topic", "Message contained invalid floating point values (nans or infs)" );
    return;
  }

  // Lookup transform into fixed frame
  Ogre::Vector3 position;
  Ogre::Quaternion orientation;
  if( !context_->getFrameManager()->getTransform( msg->header, position, orientation ))
  {
    ROS_DEBUG( "Error transforming from frame '%s' to frame '%s'", msg->header.frame_id.c_str(), qPrintable( fixed_frame_ ));
  }

  Ogre::Matrix4 transform( orientation );
  transform.setTrans( position );

//  scene_node_->setPosition( position );
//  scene_node_->setOrientation( orientation );

  Ogre::ColourValue color = color_property_->getOgreColor();
  color.a = alpha_property_->getFloat();

  uint32_t num_points = msg->poses.size();
  float line_width = line_width_property_->getFloat();

  switch(style)
  {
  case LINES:
    manual_object->estimateVertexCount( num_points );
    manual_object->begin( "BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP );
    for( uint32_t i=0; i < num_points; ++i)
    {
      const geometry_msgs::Point& pos = msg->poses[ i ].pose.position;
      Ogre::Vector3 xpos = transform * Ogre::Vector3( pos.x, pos.y, pos.z );
      manual_object->position( xpos.x, xpos.y, xpos.z );
      manual_object->colour( color );
    }

    manual_object->end();
    break;

  case BILLBOARDS:
    billboard_line->setNumLines( 1 );
    billboard_line->setMaxPointsPerLine( num_points );
    billboard_line->setLineWidth( line_width );

    for( uint32_t i=0; i < num_points; ++i)
    {
      const geometry_msgs::Point& pos = msg->poses[ i ].pose.position;
      Ogre::Vector3 xpos = transform * Ogre::Vector3( pos.x, pos.y, pos.z );
      billboard_line->addPoint( xpos, color );
    }

    break;
  }

}