Revision history [back]

So I figured out how to do this. I used the Points and Lines tutorial as a reference and used the line strip function of the Rviz Markers Display types.

ros::Publisher marker_pub;
void VisPoseCallback(const geometry_msgs::PoseArrayConstPtr& msg)
{
const std::vector<geometry_msgs::Pose>& ps = msg->poses;
visualization_msgs::Marker line_strip;
line_strip.header.frame_id = "/whycon";

line_strip.header.stamp = ros::Time::now();
line_strip.ns = "lines";
line_strip.pose.orientation.w = 1.0;
line_strip.id = 0; //tag id
line_strip.type = visualization_msgs::Marker::LINE_STRIP;  //tag type
line_strip.scale.x = 0.05;
line_strip.color.b = 1.0;
line_strip.color.a = 1.0;

geometry_msgs::Point p;
p.x = ps[0].position.x;
p.y = ps[0].position.y;
p.z = ps[0].position.z;
line_strip.points.push_back(p);

I repeated the position bit for each of my poses then I subscribed to my poses message.

int main(int argc, char** argv)
{
  ros::init(argc, argv, "VisPose");
  ros::NodeHandle n;

   marker_pub = n.advertise<visualization_msgs::Marker>("line_visualization_marker", 10);
  ros::Rate r(100);
  ros::Subscriber pose_sub = n.subscribe<geometry_msgs::PoseArray>("/whycon/poses", 1, &VisPoseCallback);

  ros::spin();
}

This created a box between which moves as the poses change.