Revision history [back]

If you're fine not using PoseStamped, I'd recommand you using Marker. You can choose an arrow as marke type and add two points, which then will be used to create this arrow, therefore no orientation coordinates are needed. (LINK: http://wiki.ros.org/rviz/DisplayTypes/Marker)

Supposed you have Point A and Point B with x, y, z position properties in C++:

#include <geometry_msgs/Point.h>
#include <visualization_msgs/Marker.h>    


const int MARKERTYPE = 0; // 0 = Arrow
//Example values
const double ARROWSHAFTDIAMETER = 10;
const double ARROWHEADDIAMETER = 20;
const double ARROWHEADLENGTH = 15;
const double ARROWTRANSPARENCY = 1;
const double ARROWCOLORRED = 1;

visualization_msgs::Marker vector_msg;

vector_msg.header.frame_id = "YouFrameId";

vector_msg.type = MARKERTYPE;
vector_msg.id = 0; //any number except existing id
vector_msg.scale.x = ARROWSHAFTDIAMETER;
vector_msg.scale.y = ARROWHEADDIAMETER;
vector_msg.scale.z = ARROWHEADLENGTH;
vector_msg.scale.a = ARROWTRANSPARENCY;
vector_msg.scale.r = ARROWREDCOLORRED;

geometry_msgs::Point originPoint;
geometry_msgs::Point destinyPoint;

//check if your data is in metric units before
originPoint.x = A.x;
originPoint.y = A.y;
originPoint.z = A.z;

destinyPoint.x = B.x;
destinyPoint.y = B.y;
destinyPoint.z = B.z;

vector_msg.points.push_back(originPoint);
vector_msg.points.push_back(destinyPoint);

//don't forget to change data type of the publisher to "visualization_msgs::Marker"
yourPublisher.publish(vector_msg);

if you want to change the size of the vector you can do it by using this formular

k: Wanted length of Vector
A: Origin Point
B: Original Destiniy Point
AB: Vector from A to B
|AB|: Length of Vector AB (determinted by sqrt((AB.x)² + (AB.y)² + (AB.z)²)
C: New Destiniy Point

A = k⋅A
C = k⋅(A + |AB|)