How to publish "real-time" trajectory using markers and Rviz

edit

I have a trajectory generator and I want to visualize it using Rviz. The marker publishing function obtains the coordinates from the main.cpp through geometry_msgs::Vector3 these are later converted to points and added to the marker data. I have tried with both Marker and MarkerArray and I am not able to visualize the markers. I am new to publishing markers in Rviz and I have based the code on the Points and Lines tutorial. I have included a cout command to make sure the information was being added to the Points data and I can see the information in the terminal window.

I am using Unbuntu 12.04 and Ros Fuerte. I have copied the algorithm below. I apologize for the Spanish text in it but it is for my thesis project. Also I appreciate any suggestions on how to improve it and make this code look better.

---marcadores_tf.h---

#ifndef _MARCADORES_TF
#define _MARCADORES_TF

#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Point.h>
#include <tf/transform_broadcaster.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <Eigen/Core>

struct marcadores_trayectoria{
  void publicar_marcadores(const geometry_msgs::Vector3 coordenadas);

  marcadores_trayectoria(){
    pub_trayectoria = nh.advertise<visualization_msgs::MarkerArray>("trayectoria_array", 10 );
    pub_marcador = nh.advertise<visualization_msgs::Marker>("marcador_trayectoria", 10 );
  }

  ros::NodeHandle nh;
  ros::Publisher pub_trayectoria, pub_marcador;

  tf::TransformBroadcaster br;
  geometry_msgs::Point puntos;
};

#endif

---marcadores_tf.cpp---

#include <planif_tray_ardrone/marcadores_tf.h>

using namespace std;

void marcadores_trayectoria::publicar_marcadores(const geometry_msgs::Vector3 coordenadas){

  visualization_msgs::MarkerArray lista_marcadores;
  visualization_msgs::Marker marcador;

  marcador.header.frame_id = "/world";
  marcador.ns = "trayectoria_ns";
  marcador.header.stamp = ros::Time::now();

  marcador.type = visualization_msgs::Marker::LINE_STRIP;
  marcador.action = visualization_msgs::Marker::ADD;
  marcador.id = 404;

  marcador.scale.x = 1.0;

  marcador.color.a = 1.0;
  marcador.color.b = 1.0;

  marcador.pose.orientation.w = 1.0;

  //coordenadas
  puntos.x = coordenadas.x;
  puntos.y = coordenadas.y;
  puntos.z = coordenadas.z;

  cout << "puntos x: " << puntos.x << endl;
  marcador.points.push_back(puntos);
  lista_marcadores.markers.push_back(marcador);

  if (pub_marcador.getNumSubscribers() > 0)
      pub_marcador.publish(marcador);

  if (pub_trayectoria.getNumSubscribers() > 0)
      pub_trayectoria.publish(lista_marcadores);

  ros::Rate r(30);
  r.sleep();

}