24 #include <core/exception.h>
25 #include <fvmodels/relative_position/relativepositionmodel.h>
26 #include <fvutils/camera/tracker.h>
27 #include <utils/math/angle.h>
28 #include <utils/system/console_colors.h>
32 namespace firevision {
67 rpm = relative_position_model;
69 this->camera_height = camera_height;
89 float w_r_x = world_x - robot_x;
90 float w_r_y = world_y - robot_y;
92 float distance = sqrt(w_r_x * w_r_x + w_r_y * w_r_y);
117 new_pan = atan2f(w_r_y, w_r_x);
119 new_tilt = atan2f(camera_height, distance);