C ++ ROS距離算出点

 

書式#include <iostreamの>       / * プット、printf関数* / 
書式#include <TIME.H>        / * time_t型、構造体tm、時間、localtimeの* / 
の#include <math.h>の使用して名前空間はstdを、
構造体の位置{
     ダブルX。
    ダブルY;
    ダブルZ; 
}。構造体の方位{
     ダブルW。
    ダブルX;
    ダブルY;
    ダブルZ; 
}。構造体の点{ 
    ポジション位置。
    オリエンテーションオリエンテーション; 
}。

 



二重GetDistance（CUR点が開発）{
     戻り SQRT（POW（（cur.position.x-des.position.x）2）+ POW（（cur.position.y-des.position.y）、2 ） ）; 
} 

アイテムgetdes（）{ 
    開発。
    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;
    返すデス。
} 

getcur（）{ポイント
    ポイント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 ;
    リターンCUR。
} 

（点CUR）{getclosest指す
    // 获取会车点
    DES点と、
    デ= Getdes（）;
     // 車の最も近い点の現在位置までの距離を取得します
    ダブルの距離= getDistance（CUR、DES）;
     のchar * S = 「huichedian 」; 
    のprintf（" 車の最も近い点は％sとなり、距離： LF％\ N- " 、S、距離）を
     返すDESを; 
} 
int型のmain（）
{ 
    // 現在位置
    ポイントCUR最も近い; 
    CUR = getcur（）; 
    最寄り = getclosest（CUR）; 
    
    戻り 0 ; 
}