ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
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);
2 | No.2 Revision |
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);
you can do the same with the odometry publications, but you may have an accumulated error there.