Revision history [back]

You can subscribe to the publication of the pose topic and store each publication in a pose array, for example. Then you can process that array to sum the distances between each reading.

float path_length = 0.0;
std::vector<geometry_msgs::PoseStamped>::iterator it = plan.poses.begin();
geometry_msgs::PoseStamped last_pose;
last_pose = *it;
it++;
for (; it!=plan.poses.end(); ++it) {
   path_length += hypot(  (*it).pose.position.x - last_pose.pose.position.x, 
                         (*it).pose.position.y - last_pose.pose.position.y );
   last_pose = *it;
}
ROS_INFO("... path length: %6.3f", path_length);

source of this code

You can subscribe to the publication of the pose topic and store each publication in a pose array, for example. Then you can process that array to sum the distances between each reading.

float path_length = 0.0;
std::vector<geometry_msgs::PoseStamped>::iterator it = plan.poses.begin();
geometry_msgs::PoseStamped last_pose;
last_pose = *it;
it++;
for (; it!=plan.poses.end(); ++it) {
   path_length += hypot(  (*it).pose.position.x - last_pose.pose.position.x, 
                         (*it).pose.position.y - last_pose.pose.position.y );
   last_pose = *it;
}
ROS_INFO("... path length: %6.3f", path_length);

source of this code