omni_global.cpp

00001 
00002 /***************************************************************************
00003  *  omni_relative.cpp - Implementation of the relative ball model
00004  *                      for the omni cam
00005  *
00006  *  Created: Thu Mar 23 22:00:15 2006
00007  *  Copyright  2006-2008  Tim Niemueller [www.niemueller.de]
00008  *
00009  ****************************************************************************/
00010 
00011 /*  This program is free software; you can redistribute it and/or modify
00012  *  it under the terms of the GNU General Public License as published by
00013  *  the Free Software Foundation; either version 2 of the License, or
00014  *  (at your option) any later version. A runtime exception applies to
00015  *  this software (see LICENSE.GPL_WRE file mentioned below for details).
00016  *
00017  *  This program is distributed in the hope that it will be useful,
00018  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00019  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00020  *  GNU Library General Public License for more details.
00021  *
00022  *  Read the full text in the LICENSE.GPL_WRE file in the doc directory.
00023  */
00024 
00025 #include <models/global_position/omni_global.h>
00026 #include <models/mirror/mirrormodel.h>
00027 
00028 namespace firevision {
00029 #if 0 /* just to make Emacs auto-indent happy */
00030 }
00031 #endif
00032 
00033 /** @class OmniGlobal <models/global_position/omni_global.h>
00034  * Omni vision global position model.
00035  */
00036 
00037 /** Constructor.
00038  * @param mirror_model mirror model
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 } // end namespace firevision