ROS C++ : 比较nav_msgs/Path的一致性

在C++中，要比较nav_msgs/Path的一致性，通常是指比较两个nav_msgs::Path对象是否包含相同的poses。下面提供一个简单的函数来实现这个需求。

#include <navigation_msgs/Path.h>
#include <geometry_msgs/PoseArray.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <algorithm>


bool comparePoses(const geometry_msgs::Pose& pose1, 
                  const geometry_msgs::Pose& pose2) 
{
    // 实现比较两个pose的逻辑，例如只比较位置或位置和方向
    return pose1.position.x == pose2.position.x &&
           pose1.position.y == pose2.position.y &&
           pose1.position.z == pose2.position.z;
} 


bool arePathsConsistent(const nav_msgs::Path& path1, 
                        const nav_msgs::Path& path2) 
{

    if (path1.poses.size() != path2.poses.size()) 
    {
        return false;
    }
 
    // 将两个路径的pose数组转换为数组
    //geometry_msgs::PoseArray poseArray1, poseArray2;
    //poseArray1.poses = path1.poses;  //编译不过
    //poseArray2.poses = path2.poses;  //编译不过
    
    // 将两个路径的pose数组转换为数组
    geometry_msgs::PoseArray poseArray1;
    {
        poseArray1.poses.reserve(path1.poses.size());
        for (const auto& pathPose : path1.poses)
        {
        geometry_msgs::Pose pose;
        pose.position.x = pathPose.pose.position.x;
        pose.position.y = pathPose.pose.position.y;
        pose.position.z = pathPose.pose.position.z;
        pose.orientation.x = pathPose.pose.orientation.x;
        pose.orientation.y = pathPose.pose.orientation.y;
        pose.orientation.z = pathPose.pose.orientation.z;
        pose.orientation.w = pathPose.pose.orientation.w;
        poseArray1.poses.push_back(pose);
        }
    }

    geometry_msgs::PoseArray poseArray2;
    {
        poseArray2.poses.reserve(path2.poses.size());
        for (const auto& pathPose : path2.poses)
        {
        geometry_msgs::Pose pose;
        pose.position.x = pathPose.pose.position.x;
        pose.position.y = pathPose.pose.position.y;
        pose.position.z = pathPose.pose.position.z;
        pose.orientation.x = pathPose.pose.orientation.x;
        pose.orientation.y = pathPose.pose.orientation.y;
        pose.orientation.z = pathPose.pose.orientation.z;
        pose.orientation.w = pathPose.pose.orientation.w;
        poseArray2.poses.push_back(pose);
        }
    }

 
    // 使用std::sort对pose数组进行排序，然后比较数组
    std::sort(poseArray1.poses.begin(), poseArray1.poses.end(), comparePoses);
    std::sort(poseArray2.poses.begin(), poseArray2.poses.end(), comparePoses);
 
    return std::equal(poseArray1.poses.begin(), poseArray1.poses.end(), poseArray2.poses.begin(), comparePoses);

}
 

函数arePathsConsistent接收两个nav_msgs::Path对象作为参数，并返回它们是否完全一致的布尔值。函数comparePoses是一个比较两个geometry_msgs::Pose对象的函数，你可以根据需要修改它的实现来比较更多的pose属性。

注意：这个例子假设了路径中的每个pose都是不同的。如果有可能有重复的pose，那么在排序或比较时需要进行相应的处理。