c++ ros 计算两点距离

 

#include <iostream>      /* puts, printf */
#include <time.h>       /* time_t, struct tm, time, localtime */
#include <math.h>

using namespace std;
struct Position{
    double x;
    double y;
    double z;
};
struct Orientation{
    double w;
    double x;
    double y;
    double z;
};
struct point{
    Position position;
    Orientation orientation;
};

double getdistance(point cur,point des){
    return sqrt(pow((cur.position.x-des.position.x),2)+pow((cur.position.y-des.position.y),2));
}

point getdes(){
    point des;
    des.position.x=-8.75;
    des.position.y=-10.93;
    des.position.z=0.0;
    des.orientation.w=0.0;
    des.orientation.x=0.0;
    des.orientation.y=0.0;
    des.orientation.z=0.0;
    return des;
}

point getcur(){
    point cur;
    cur.position.x=-4.95;
    cur.position.y=-2.07;
    cur.position.z=0.0;
    cur.orientation.w=0.25;
    cur.orientation.x=0.0;
    cur.orientation.y=0.0;
    cur.orientation.z=0.97;
    return cur;
}

point getclosest(point cur){
    //获取会车点
    point des;
    des=getdes();
    //获取距离当前位置最近的会车点
    double distance=getdistance(cur,des);
    char* s="huichedian";
    printf("距离最近的会车点是%s,距离为：%lf\n",s,distance);
    return des;
}
int main ()
{
    //当前位置
    point cur,closest;
    cur=getcur();
    closest=getclosest(cur);
    
    return 0;
}