с ++ расчетные точки ROS расстояние

 

#include <iostream>       / * ставит, Printf * / 
#include <time.h>        / * time_t, структура тм, время, LocalTime * / 
#include <math.h> с использованием пространства имен STD;
структура установки {
     двойные х;
    двойной у;
    дважды г; 
}; структура ориентации {
     двойной ш;
    двойные х;
    двойной у;
    дважды г; 
}; структура точка { 
    положение Положения; 
    Ориентация ориентации; 
};

 



Двойной GetDistance (Cur точка разработки) {
     возвращение SQRT (пау ((cur.position.x-des.position.x) 2 ) + пау ((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;
    вернуться DES; 
} 

Указать 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 ;
    вернуться пса; 
} 

Точка (точка getclosest CUR) { 
    // 获取会车点
    точка дез; 
    дез= Getdes ();
     // получает расстояние от текущего местоположения ближайшей точки автомобиля будет 
    Double Расстояние = GetDistance (CUR, дез),
     символ * S = « huichedian » ; 
    Е ( " ближайшая точка автомобиля будет% s, расстояние: LF% \ n- " , S, расстояние);
     вернуть DES; 
} 
INT основной () 
{ 
    // текущая позиция 
    точки CUR, Ближайший, 
    CUR = getcur (); 
    Минимальное = getclosest (CUR); 
    
    вернуть  0 ; 
}