00001
00002
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00031
00032 #ifndef RCSC_PLAYER_LOCALIZER_H
00033 #define RCSC_PLAYER_LOCALIZER_H
00034
00035 #include <rcsc/player/visual_sensor.h>
00036 #include <rcsc/geom/vector_2d.h>
00037 #include <rcsc/types.h>
00038
00039 #include <boost/scoped_ptr.hpp>
00040
00041 namespace rcsc {
00042
00043 class LocalizeImpl;
00044
00049 class Localization {
00050 public:
00051
00055 struct PlayerT {
00056 int unum_;
00057 bool goalie_;
00058 Vector2D pos_;
00059 Vector2D rpos_;
00060 Vector2D vel_;
00061 double body_;
00062 double face_;
00063 bool has_face_;
00064 double arm_;
00065 bool pointto_;
00066 bool tackle_;
00067
00071 PlayerT()
00072 : unum_( Unum_Unknown )
00073 , goalie_( false )
00074 , pos_( Vector2D::INVALIDATED )
00075 , rpos_( Vector2D::INVALIDATED )
00076 , vel_( Vector2D::INVALIDATED )
00077 , body_( 0.0 )
00078 , face_( 0.0 )
00079 , has_face_( false )
00080 , arm_( 0.0 )
00081 , pointto_( false )
00082 , tackle_( false )
00083 { }
00084
00088 void reset()
00089 {
00090 pos_.invalidate();
00091 rpos_.invalidate();
00092 unum_ = Unum_Unknown;
00093 has_face_ = pointto_ = tackle_ = false;
00094 }
00095
00100 bool hasVel() const
00101 {
00102 return vel_.valid();
00103 }
00104
00109 bool hasAngle() const
00110 {
00111 return has_face_;
00112 }
00113
00118 bool isPointing() const
00119 {
00120 return pointto_;
00121 }
00122
00127 bool isTackling() const
00128 {
00129 return tackle_;
00130 }
00131 };
00132
00133 private:
00134
00136 boost::scoped_ptr< LocalizeImpl > M_impl;
00137
00138 public:
00143 explicit
00144 Localization( const SideID ourside );
00145
00149 ~Localization();
00150
00151 public:
00160 void localizeSelf( const VisualSensor & see,
00161 double * self_face,
00162 double * self_face_err,
00163 Vector2D * self_pos,
00164 Vector2D * self_pos_err ) const;
00165
00176 void localizeBallRelative( const VisualSensor & see,
00177 const double & self_face,
00178 const double & self_face_err,
00179 Vector2D * rpos,
00180 Vector2D * rpos_err,
00181 Vector2D * rvel,
00182 Vector2D * rvel_err ) const;
00183
00193 void localizePlayer( const VisualSensor::PlayerT & from,
00194 const double & self_face,
00195 const double & self_face_err,
00196 const Vector2D & self_pos,
00197 const Vector2D & self_vel,
00198 PlayerT * to ) const;
00199 };
00200
00201 }
00202
00203 #endif