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