00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #include <models/global_position/omni_global.h>
00026 #include <models/mirror/mirrormodel.h>
00027
00028 namespace firevision {
00029 #if 0
00030 }
00031 #endif
00032
00033
00034
00035
00036
00037
00038
00039
00040 OmniGlobal::OmniGlobal(MirrorModel *mirror_model)
00041 {
00042 this->mirror_model = mirror_model;
00043
00044 ball_x = ball_y = 0.f;
00045 }
00046
00047
00048 void
00049 OmniGlobal::set_position_in_image(unsigned int x, unsigned int y)
00050 {
00051 image_x = x;
00052 image_y = y;
00053 }
00054
00055
00056 void
00057 OmniGlobal::set_robot_position(float x, float y, float ori)
00058 {
00059 pose_x = x;
00060 pose_y = y;
00061 pose_ori = ori;
00062 }
00063
00064
00065 float
00066 OmniGlobal::get_y(void) const
00067 {
00068 return ball_y;
00069 }
00070
00071
00072 float
00073 OmniGlobal::get_x(void) const
00074 {
00075 return ball_x;
00076 }
00077
00078
00079 void
00080 OmniGlobal::calc()
00081 {
00082 if ( mirror_model->isValidPoint( image_x, image_y ) ) {
00083
00084 fawkes::cart_coord_2d_t glob_pos = mirror_model->getWorldPointGlobal( image_x,
00085 image_y,
00086 pose_x,
00087 pose_y,
00088 pose_ori);
00089
00090 ball_x = glob_pos.x;
00091 ball_y = glob_pos.y;
00092 }
00093 }
00094
00095
00096 bool
00097 OmniGlobal::is_pos_valid() const
00098 {
00099 return mirror_model->isValidPoint( image_x, image_y );
00100 }
00101
00102 }