C++ wp=wp+1; C++ if(((x-route[curent_wp].x)*(x-route[curent_wp].x) + (y-route[curent_wp].y)*(y-route[curent_wp].y) + (z-route[curent_wp].z)*(z-route[curent_wp].z)) < 0.5) then return true else return false end C++ if(wp == route.size()) then return true else return false end C++ outport.write(route[wp]) C++ geometry_msgs::Point* pt = new geometry_msgs::Point() pt->x = argminx; pt->y = argminy; pt->z = 10.0 route.push_back(*pt) pt->x = argminx pt->y = argminy; pt->z = 0.0 route.push_back(*pt) delete pt outport.write(route[wp]) C++ return true C++ if(wp < route.size()) then return true else return false end