playerc++.h

00001 /*
00002  *  Player - One Hell of a Robot Server
00003  *  Copyright (C) 2000-2003
00004  *     Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
00005  *
00006  *
00007  *  This program is free software; you can redistribute it and/or modify
00008  *  it under the terms of the GNU General Public License as published by
00009  *  the Free Software Foundation; either version 2 of the License, or
00010  *  (at your option) any later version.
00011  *
00012  *  This program is distributed in the hope that it will be useful,
00013  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015  *  GNU General Public License for more details.
00016  *
00017  *  You should have received a copy of the GNU General Public License
00018  *  along with this program; if not, write to the Free Software
00019  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00020  *
00021  */
00022 
00023 /***************************************************************************
00024  * Desc: Player v2.0 C++ client
00025  * Authors: Brad Kratochvil, Toby Collett
00026  *
00027  * Date: 23 Sep 2005
00028  # CVS: $Id: playerc++.h,v 1.54.2.15 2007/12/05 21:57:34 gerkey Exp $
00029  **************************************************************************/
00030 
00031 
00032 #ifndef PLAYERCC_H
00033 #define PLAYERCC_H
00034 
00035 #include <cmath>
00036 #include <string>
00037 #include <list>
00038 
00039 #include "libplayerc/playerc.h"
00040 #include "libplayerc++/utility.h"
00041 #include "libplayerc++/playerc++config.h"
00042 #include "libplayerc++/playerclient.h"
00043 #include "libplayerc++/playererror.h"
00044 #include "libplayerc++/clientproxy.h"
00045 
00046 #ifdef HAVE_BOOST_SIGNALS
00047   #include <boost/signal.hpp>
00048   #include <boost/bind.hpp>
00049 #endif
00050 
00051 #ifdef HAVE_BOOST_THREAD
00052   #include <boost/thread/mutex.hpp>
00053   #include <boost/thread/thread.hpp>
00054   #include <boost/thread/xtime.hpp>
00055 #endif
00056 
00057 namespace PlayerCc
00058 {
00059 
00060 // /**
00061 // * The @p SomethingProxy class is a template for adding new subclasses of
00062 // * ClientProxy.  You need to have at least all of the following:
00063 // */
00064 // class SomethingProxy : public ClientProxy
00065 // {
00066 //
00067 //   private:
00068 //
00069 //     // Subscribe
00070 //     void Subscribe(uint aIndex);
00071 //     // Unsubscribe
00072 //     void Unsubscribe();
00073 //
00074 //     // libplayerc data structure
00075 //     playerc_something_t *mDevice;
00076 //
00077 //   public:
00078 //     // Constructor
00079 //     SomethingProxy(PlayerClient *aPc, uint aIndex=0);
00080 //     // Destructor
00081 //     ~SomethingProxy();
00082 //
00083 // };
00084 
00096 // ==============================================================
00097 //
00098 // These are alphabetized, please keep them that way!!!
00099 //
00100 // ==============================================================
00101 
00106 class ActArrayProxy : public ClientProxy
00107 {
00108   private:
00109 
00110    void Subscribe(uint aIndex);
00111    void Unsubscribe();
00112 
00113    // libplayerc data structure
00114    playerc_actarray_t *mDevice;
00115 
00116   public:
00117 
00119     ActArrayProxy(PlayerClient *aPc, uint aIndex=0);
00121     ~ActArrayProxy();
00122 
00125     void RequestGeometry(void);
00126 
00128     void SetPowerConfig(bool aVal);
00130     void SetBrakesConfig(bool aVal);
00132     void SetSpeedConfig(uint aJoint, float aSpeed);
00133 
00135     void MoveTo(uint aJoint, float aPos);
00137     void MoveAtSpeed(uint aJoint, float aSpeed);
00139     void MoveHome(int aJoint);
00140 
00142     uint GetCount(void) const { return GetVar(mDevice->actuators_count); }
00144     player_actarray_actuator_t GetActuatorData(uint aJoint) const;
00146     player_actarray_actuatorgeom_t GetActuatorGeom(uint aJoint) const;
00147 
00152     player_actarray_actuator_t operator [](uint aJoint)
00153       { return(GetActuatorData(aJoint)); }
00154 };
00155 
00159 class AioProxy : public ClientProxy
00160 {
00161   private:
00162 
00163     void Subscribe(uint aIndex);
00164     void Unsubscribe();
00165 
00166     // libplayerc data structure
00167     playerc_aio_t *mDevice;
00168 
00169   public:
00170 
00171     AioProxy (PlayerClient *aPc, uint aIndex=0);
00172     ~AioProxy();
00173 
00175     uint GetCount() const { return(GetVar(mDevice->voltages_count)); };
00176 
00178     double GetVoltage(uint aIndex)  const
00179       { return(GetVar(mDevice->voltages[aIndex])); };
00180 
00182     void SetVoltage(uint aIndex, double aVoltage);
00183 
00188     double operator [](uint aIndex) const
00189       { return GetVoltage(aIndex); }
00190 
00191 };
00192 
00193 #if 0 // Not in libplayerc
00194 
00198 class AudioProxy : public ClientProxy
00199 {
00200 
00201   private:
00202 
00203     void Subscribe(uint aIndex);
00204     void Unsubscribe();
00205 
00206     // libplayerc data structure
00207     playerc_audio_t *mDevice;
00208 
00209   public:
00210 
00211     AudioProxy(PlayerClient *aPc, uint aIndex=0)
00212     ~AudioProxy();
00213 
00214     uint GetCount() const { return(GetVar(mDevice->count)); };
00215 
00216     double GetFrequency(uint aIndex) const
00217       { return(GetVar(mDevice->frequency[aIndex])); };
00218     double GetAmplitude(uint aIndex) const
00219       { return(GetVar(mDevice->amplitude[aIndex])); };
00220 
00221     // Play a fixed-frequency tone
00222     void PlayTone(uint aFreq, uint aAmp, uint aDur);
00223 };
00224 
00230 class AudioDspProxy : public ClientProxy
00231 {
00232   private:
00233 
00234     void Subscribe(uint aIndex);
00235     void Unsubscribe();
00236 
00237     // libplayerc data structure
00238     playerc_audiodsp_t *mDevice;
00239 
00240   public:
00241     AudioDspProxy(PlayerClient *aPc, uint aIndex=0);
00242     ~AudioDspProxy
00243 
00245     uint SetFormat(int aFormat);
00246 
00248     uint SetRate(uint aRate);
00249 
00250     uint GetCount() const { return(GetVar(mDevice->count)); };
00251 
00252     double GetFrequency(uint aIndex) const
00253       { return(GetVar(mDevice->frequency[aIndex])); };
00254     double GetAmplitude(uint aIndex) const
00255       { return(GetVar(mDevice->amplitude[aIndex])); };
00256 
00257     void Configure(uint aChan, uint aRate, int16_t aFormat=0xFFFFFFFF);
00258 
00259     void RequestConfigure();
00260 
00262     void PlayTone(uint aFreq, uint aAmp, uint aDur);
00263     void PlayChirp(uint aFreq, uint aAmp, uint aDur,
00264                    const uint8_t aBitString, uint aBitStringLen);
00265     void Replay();
00266 
00268     void Print ();
00269 };
00270 
00274 class AudioMixerProxy : public ClientProxy
00275 {
00276   private:
00277 
00278     void Subscribe(uint aIndex);
00279     void Unsubscribe();
00280 
00281     // libplayerc data structure
00282     playerc_audiodsp_t *mDevice;
00283 
00284   public:
00285 
00286     AudioMixerProxy (PlayerClient *aPc, uint aIndex=0);
00287 
00288     void GetConfigure();
00289 
00290     void SetMaster(uint aLeft, uint aRight);
00291     void SetPCM(uint aLeft, uint aRight);
00292     void SetLine(uint aLeft, uint aRight);
00293     void SetMic(uint aLeft, uint aRight);
00294     void SetIGain(uint aGain);
00295     void SetOGain(uint aGain);
00296 
00297 };
00298 
00303 class BlinkenLightProxy : public ClientProxy
00304 {
00305   private:
00306 
00307     void Subscribe(uint aIndex);
00308     void Unsubscribe();
00309 
00310     // libplayerc data structure
00311     playerc_blinkenlight_t *mDevice;
00312 
00313   public:
00317     BlinkenLightProxy(PlayerClient *aPc, uint aIndex=0);
00318     ~BlinkenLightProxy();
00319 
00320     // true: indicator light enabled, false: disabled.
00321     bool GetEnable();
00322 
00327     void SetPeriod(double aPeriod);
00328 
00333     void SetEnable(bool aSet);
00334 };
00335 
00336 #endif
00337 
00344 class BlobfinderProxy : public ClientProxy
00345 {
00346   private:
00347 
00348     void Subscribe(uint aIndex);
00349     void Unsubscribe();
00350 
00351     // libplayerc data structure
00352     playerc_blobfinder_t *mDevice;
00353 
00354   public:
00356     BlobfinderProxy(PlayerClient *aPc, uint aIndex=0);
00358     ~BlobfinderProxy();
00359 
00361     uint GetCount() const { return GetVar(mDevice->blobs_count); };
00363     playerc_blobfinder_blob_t GetBlob(uint aIndex) const
00364       { return GetVar(mDevice->blobs[aIndex]);};
00365 
00367     uint GetWidth() const { return GetVar(mDevice->width); };
00369     uint GetHeight() const { return GetVar(mDevice->height); };
00370 
00375     playerc_blobfinder_blob_t operator [](uint aIndex) const
00376       { return(GetBlob(aIndex)); }
00377 
00378 /*
00380     void SetTrackingColor(uint aReMin=0,   uint aReMax=255, uint aGrMin=0,
00381                           uint aGrMax=255, uint aBlMin=0,   uint aBlMax=255);
00382     void SetImagerParams(int aContrast, int aBrightness,
00383                          int aAutogain, int aColormode);
00384     void SetContrast(int aC);
00385     void SetColorMode(int aM);
00386     void SetBrightness(int aB);
00387     void SetAutoGain(int aG);*/
00388 
00389 };
00390 
00395 class BumperProxy : public ClientProxy
00396 {
00397 
00398   private:
00399 
00400     void Subscribe(uint aIndex);
00401     void Unsubscribe();
00402 
00403     // libplayerc data structure
00404     playerc_bumper_t *mDevice;
00405 
00406   public:
00407 
00408     BumperProxy(PlayerClient *aPc, uint aIndex=0);
00409     ~BumperProxy();
00410 
00411     uint GetCount() const { return GetVar(mDevice->bumper_count); };
00412 
00414     uint IsBumped(uint aIndex) const
00415       { return GetVar(mDevice->bumpers[aIndex]); };
00416 
00418     bool IsAnyBumped();
00419 
00421     void RequestBumperConfig();
00422 
00424     uint GetPoseCount() const { return GetVar(mDevice->pose_count); };
00425 
00427     player_bumper_define_t GetPose(uint aIndex) const
00428       { return GetVar(mDevice->poses[aIndex]); };
00429 
00434     bool operator [](uint aIndex) const
00435       { return IsBumped(aIndex); }
00436 
00437 };
00438 
00442 class CameraProxy : public ClientProxy
00443 {
00444 
00445   private:
00446 
00447     virtual void Subscribe(uint aIndex);
00448     virtual void Unsubscribe();
00449 
00450     // libplayerc data structure
00451     playerc_camera_t *mDevice;
00452 
00453     std::string mPrefix;
00454     int mFrameNo;
00455 
00456   public:
00457 
00459     CameraProxy (PlayerClient *aPc, uint aIndex=0);
00460 
00461     virtual ~CameraProxy();
00462 
00466     void SaveFrame(const std::string aPrefix, uint aWidth=4);
00467 
00469     void Decompress();
00470 
00472     uint GetDepth() const { return GetVar(mDevice->bpp); };
00473 
00475     uint GetWidth() const { return GetVar(mDevice->width); };
00476 
00478     uint GetHeight() const { return GetVar(mDevice->height); };
00479 
00486     uint GetFormat() const { return GetVar(mDevice->format); };
00487 
00489     uint GetImageSize() const { return GetVar(mDevice->image_count); };
00490 
00495     void GetImage(uint8_t* aImage) const
00496       {
00497         return GetVarByRef(mDevice->image,
00498                            mDevice->image+GetVar(mDevice->image_count),
00499                            aImage);
00500       };
00501 
00506     uint GetCompression() const { return GetVar(mDevice->compression); };
00507 
00508 };
00509 
00510 
00515 class DioProxy : public ClientProxy
00516 {
00517   private:
00518 
00519     void Subscribe(uint aIndex);
00520     void Unsubscribe();
00521 
00522     // libplayerc data structure
00523     playerc_dio_t *mDevice;
00524 
00525   public:
00527     DioProxy(PlayerClient *aPc, uint aIndex=0);
00529     ~DioProxy();
00530 
00532     uint GetCount() const { return GetVar(mDevice->count); };
00533 
00535     uint32_t GetDigin() const { return GetVar(mDevice->digin); };
00536 
00538     bool GetInput(uint aIndex) const;
00539 
00541     void SetOutput(uint aCount, uint32_t aDigout);
00542 
00547     uint operator [](uint aIndex) const
00548       { return GetInput(aIndex); }
00549 };
00550 
00551 // /**
00552 // The @p EnergyProxy class is used to read from an @ref
00553 // interface_energy device.
00554 // */
00555 // class EnergyProxy : public ClientProxy
00556 // {
00557 //   private:
00558 //
00559 //     void Subscribe(uint aIndex);
00560 //     void Unsubscribe();
00561 //
00562 //     // libplayerc data structure
00563 //     playerc_energy_t *mDevice;
00564 //
00565 // public:
00566 //
00567 //     EnergyProxy(PlayerClient *aPc, uint aIndex=0);
00568 //     ~EnergyProxy();
00569 //
00570 //     /** These members give the current amount of energy stored [Joules] */
00571 //     uint GetJoules() const { return GetVar(mDevice->joules); };
00572 //     /** the amount of energy current being consumed [Watts] */
00573 //     uint GetWatts() const { return GetVar(mDevice->watts); };
00574 //     /** The charging flag is true if we are currently charging, else false. */
00575 //     bool GetCharging() const { return GetVar(mDevice->charging); };
00576 // };
00577 
00583 class FiducialProxy : public ClientProxy
00584 {
00585   private:
00586     void Subscribe(uint aIndex);
00587     void Unsubscribe();
00588 
00589     // libplayerc data structure
00590     playerc_fiducial_t *mDevice;
00591 
00592   public:
00594     FiducialProxy(PlayerClient *aPc, uint aIndex=0);
00596     ~FiducialProxy();
00597 
00599     uint GetCount() const { return GetVar(mDevice->fiducials_count); };
00600 
00602     player_fiducial_item_t GetFiducialItem(uint aIndex) const
00603       { return GetVar(mDevice->fiducials[aIndex]);};
00604 
00606     player_pose_t GetSensorPose() const
00607       { return GetVar(mDevice->fiducial_geom.pose);};
00608 
00610     player_bbox_t GetSensorSize() const
00611       { return GetVar(mDevice->fiducial_geom.size);};
00612 
00614     player_bbox_t GetFiducialSize() const
00615       { return GetVar(mDevice->fiducial_geom.fiducial_size);};
00616 
00618     void RequestGeometry();
00619 
00624     player_fiducial_item_t operator [](uint aIndex) const
00625       { return GetFiducialItem(aIndex); }
00626 };
00627 
00631 class GpsProxy : public ClientProxy
00632 {
00633 
00634   private:
00635 
00636     void Subscribe(uint aIndex);
00637     void Unsubscribe();
00638 
00639     // libplayerc data structure
00640     playerc_gps_t *mDevice;
00641 
00642   public:
00643 
00644     // Constructor
00645     GpsProxy(PlayerClient *aPc, uint aIndex=0);
00646 
00647     ~GpsProxy();
00648 
00650     double GetLatitude() const { return GetVar(mDevice->lat); };
00651     double GetLongitude() const { return GetVar(mDevice->lon); };
00652 
00654     double GetAltitude() const { return GetVar(mDevice->alt); };
00655 
00657     uint GetSatellites() const { return GetVar(mDevice->sat_count); };
00658 
00660     uint GetQuality() const { return GetVar(mDevice->quality); };
00661 
00663     double GetHdop() const { return GetVar(mDevice->hdop); };
00664 
00666     double GetVdop() const { return GetVar(mDevice->vdop); };
00667 
00669     double GetUtmEasting() const { return GetVar(mDevice->utm_e); };
00670     double GetUtmNorthing() const { return GetVar(mDevice->utm_n); };
00671 
00673     double GetTime() const { return GetVar(mDevice->utc_time); };
00674 
00676     double GetErrHorizontal() const { return GetVar(mDevice->err_horz); };
00677     double GetErrVertical() const { return GetVar(mDevice->err_vert); };
00678 };
00679 
00687 class Graphics2dProxy : public ClientProxy
00688 {
00689 
00690   private:
00691 
00692     // Subscribe
00693     void Subscribe(uint aIndex);
00694     // Unsubscribe
00695     void Unsubscribe();
00696 
00697     // libplayerc data structure
00698     playerc_graphics2d_t *mDevice;
00699 
00700   public:
00701     // Constructor
00702     Graphics2dProxy(PlayerClient *aPc, uint aIndex=0);
00703     // Destructor
00704     ~Graphics2dProxy();
00705 
00707     void Color(player_color_t col);
00708 
00710     void Color(uint8_t red,  uint8_t green,  uint8_t blue,  uint8_t alpha);
00711 
00713     void Clear(void);
00714 
00716     void DrawPoints(player_point_2d_t pts[], int count);
00717 
00719     void DrawPolygon(player_point_2d_t pts[],
00720                      int count,
00721                      bool filled,
00722                      player_color_t fill_color);
00723 
00725     void DrawPolyline(player_point_2d_t pts[], int count);
00726 };
00727 
00733 class Graphics3dProxy : public ClientProxy
00734 {
00735 
00736   private:
00737 
00738     // Subscribe
00739     void Subscribe(uint aIndex);
00740     // Unsubscribe
00741     void Unsubscribe();
00742 
00743     // libplayerc data structure
00744     playerc_graphics3d_t *mDevice;
00745 
00746   public:
00747     // Constructor
00748     Graphics3dProxy(PlayerClient *aPc, uint aIndex=0);
00749     // Destructor
00750     ~Graphics3dProxy();
00751 
00753     void Color(player_color_t col);
00754 
00756     void Color(uint8_t red,  uint8_t green,  uint8_t blue,  uint8_t alpha);
00757 
00759     void Clear(void);
00760 
00762     void Draw(player_graphics3d_draw_mode_t mode, player_point_3d_t pts[], int count);
00763 
00764 };
00765 
00771 class GripperProxy : public ClientProxy
00772 {
00773 
00774   private:
00775 
00776     void Subscribe(uint aIndex);
00777     void Unsubscribe();
00778 
00779     // libplayerc data structure
00780     playerc_gripper_t *mDevice;
00781 
00782   public:
00783 
00785     GripperProxy(PlayerClient *aPc, uint aIndex=0);
00787     ~GripperProxy();
00788 
00790     uint GetState() const { return GetVar(mDevice->state); };
00792     uint GetBeams() const { return GetVar(mDevice->beams); };
00794     uint GetOuterBreakBeam() const
00795       { return GetVar(mDevice->outer_break_beam); };
00797     uint GetInnerBreakBeam() const
00798       { return GetVar(mDevice->inner_break_beam); };
00800     uint GetPaddlesOpen() const
00801       { return GetVar(mDevice->paddles_open); };
00803     uint GetPaddlesClosed() const
00804       { return GetVar(mDevice->paddles_closed); };
00806     uint GetPaddlesMoving() const
00807       { return GetVar(mDevice->paddles_moving); };
00809     uint GetGripperError() const
00810       { return GetVar(mDevice->gripper_error); };
00812     uint GetLiftUp() const { return GetVar(mDevice->lift_up); };
00814     uint GetLiftDown() const { return GetVar(mDevice->lift_down); };
00816     uint GetLiftMoving() const { return GetVar(mDevice->lift_moving); };
00818     uint GetLiftError() const { return GetVar(mDevice->lift_error); };
00819 
00822     void SetGrip(uint8_t aCmd, uint8_t aArg=0);
00823 };
00824 
00825 
00826 
00831 class IrProxy : public ClientProxy
00832 {
00833 
00834   private:
00835 
00836     void Subscribe(uint aIndex);
00837     void Unsubscribe();
00838 
00839     // libplayerc data structure
00840     playerc_ir_t *mDevice;
00841 
00842   public:
00843 
00845     IrProxy(PlayerClient *aPc, uint aIndex=0);
00847     ~IrProxy();
00848 
00850     uint GetCount() const { return GetVar(mDevice->ranges.ranges_count); };
00852     double GetRange(uint aIndex) const
00853       { return GetVar(mDevice->ranges.ranges[aIndex]); };
00855     double GetVoltage(uint aIndex) const
00856       { return GetVar(mDevice->ranges.voltages[aIndex]); };
00858     uint GetPoseCount() const { return GetVar(mDevice->poses.poses_count); };
00860     player_pose_t GetPose(uint aIndex) const
00861       {return GetVar(mDevice->poses.poses[aIndex]);};
00862 
00864     void RequestGeom();
00865 
00870     double operator [](uint aIndex) const
00871       { return GetRange(aIndex); }
00872 
00873 };
00874 
00880 class LaserProxy : public ClientProxy
00881 {
00882   private:
00883 
00884     void Subscribe(uint aIndex);
00885     void Unsubscribe();
00886 
00887     // libplayerc data structure
00888     playerc_laser_t *mDevice;
00889 
00890     // local storage of config
00891     double min_angle, max_angle, scan_res, range_res;
00892     bool intensity;
00893 
00894   public:
00895 
00897     LaserProxy(PlayerClient *aPc, uint aIndex=0);
00899     ~LaserProxy();
00900 
00902     uint GetCount() const { return GetVar(mDevice->scan_count); };
00903     
00905     double GetMaxRange() const { return GetVar(mDevice->max_range); };
00906 
00908     double GetScanRes() const { return GetVar(mDevice->scan_res); };
00909 
00911     double GetRangeRes() const { return GetVar(mDevice->range_res); };
00912 
00913 
00915     double GetMinAngle() const { return GetVar(mDevice->scan_start); };
00917     double GetMaxAngle() const
00918     {
00919       scoped_lock_t lock(mPc->mMutex);
00920       return mDevice->scan_start + mDevice->scan_count*mDevice->scan_res;
00921     };
00922 
00924     bool IntensityOn() const { return GetVar(mDevice->intensity_on); };
00925 
00926 //    /// Scan data (polar): range (m) and bearing (radians)
00927 //    double GetScan(uint aIndex) const
00928 //      { return GetVar(mDevice->scan[aIndex]); };
00929 
00931     player_point_2d_t GetPoint(uint aIndex) const
00932       { return GetVar(mDevice->point[aIndex]); };
00933 
00934 
00936     double GetRange(uint aIndex) const
00937       { return GetVar(mDevice->ranges[aIndex]); };
00938 
00940     double GetBearing(uint aIndex) const
00941       { return GetVar(mDevice->scan[aIndex][1]); };
00942 
00943 
00945     int GetIntensity(uint aIndex) const
00946       { return GetVar(mDevice->intensity[aIndex]); };
00947 
00955     void Configure(double aMinAngle,
00956                    double aMaxAngle,
00957                    uint aScanRes,
00958                    uint aRangeRes,
00959                    bool aIntensity);
00960 
00963     void RequestConfigure();
00964 
00967     void RequestGeom();
00968 
00971     player_pose_t GetPose()
00972     {
00973       player_pose_t p;
00974       scoped_lock_t lock(mPc->mMutex);
00975 
00976       p.px = mDevice->pose[0];
00977       p.py = mDevice->pose[1];
00978       p.pa = mDevice->pose[2];
00979       return(p);
00980     }
00981     
00984     player_pose_t GetRobotPose()
00985     {
00986       player_pose_t p;
00987       scoped_lock_t lock(mPc->mMutex);
00988 
00989       p.px = mDevice->robot_pose[0];
00990       p.py = mDevice->robot_pose[1];
00991       p.pa = mDevice->robot_pose[2];
00992       return(p);
00993     }
00994 
00996     player_bbox_t GetSize()
00997     {
00998       player_bbox_t b;
00999       scoped_lock_t lock(mPc->mMutex);
01000 
01001       b.sl = mDevice->size[0];
01002       b.sw = mDevice->size[1];
01003       return(b);
01004     }
01005     
01007     double GetMinLeft() const
01008       { return GetVar(mDevice->min_left); };
01009     
01011     double GetMinRight() const
01012       { return GetVar(mDevice->min_right); };
01013            
01015     double MinLeft () const 
01016       { return GetMinLeft(); }
01017 
01019     double MinRight () const
01020       { return GetMinRight(); }
01021 
01027     double operator [] (uint index) const
01028       { return GetRange(index);}
01029 
01030 };
01031 
01032 
01037 class LimbProxy : public ClientProxy
01038 {
01039   private:
01040 
01041     void Subscribe(uint aIndex);
01042     void Unsubscribe();
01043 
01044    // libplayerc data structure
01045     playerc_limb_t *mDevice;
01046 
01047   public:
01048 
01049     LimbProxy(PlayerClient *aPc, uint aIndex=0);
01050     ~LimbProxy();
01051 
01054     void RequestGeometry(void);
01055 
01057     void SetPowerConfig(bool aVal);
01059     void SetBrakesConfig(bool aVal);
01061     void SetSpeedConfig(float aSpeed);
01062 
01064     void MoveHome(void);
01066     void Stop(void);
01068     void SetPose(float aPX, float aPY, float aPZ,
01069                  float aAX, float aAY, float aAZ,
01070                  float aOX, float aOY, float aOZ);
01072     void SetPosition(float aX, float aY, float aZ);
01075     void VectorMove(float aX, float aY, float aZ, float aLength);
01076 
01078     player_limb_data_t GetData(void) const;
01080     player_limb_geom_req_t GetGeom(void) const;
01081 };
01082 
01083 
01084 
01090 class LocalizeProxy : public ClientProxy
01091 {
01092 
01093   private:
01094 
01095     void Subscribe(uint aIndex);
01096     void Unsubscribe();
01097 
01098     // libplayerc data structure
01099     playerc_localize_t *mDevice;
01100 
01101   public:
01102 
01104     LocalizeProxy(PlayerClient *aPc, uint aIndex=0);
01106     ~LocalizeProxy();
01107 
01109     // @todo should these be in a player_pose_t?
01110     uint GetMapSizeX() const { return GetVar(mDevice->map_size_x); };
01111     uint GetMapSizeY() const { return GetVar(mDevice->map_size_y); };
01112 
01113     // @todo should these be in a player_pose_t?
01114     uint GetMapTileX() const { return GetVar(mDevice->map_tile_x); };
01115     uint GetMapTileY() const { return GetVar(mDevice->map_tile_y); };
01116 
01118     double GetMapScale() const { return GetVar(mDevice->map_scale); };
01119 
01120     // Map data (empty = -1, unknown = 0, occupied = +1)
01121     // is this still needed?  if so,
01122     //void GetMapCells(uint8_t* aCells) const
01123     //{
01124     //  return GetVarByRef(mDevice->map_cells,
01125     //                     mDevice->image+GetVar(mDevice->??map_cell_cout??),
01126     //                     aCells);
01127     //};
01128 
01130     uint GetPendingCount() const { return GetVar(mDevice->pending_count); };
01131 
01133     uint GetHypothCount() const { return GetVar(mDevice->hypoth_count); };
01134 
01136     player_localize_hypoth_t GetHypoth(uint aIndex) const
01137       { return GetVar(mDevice->hypoths[aIndex]); };
01138 
01140     int GetParticles()
01141       { return playerc_localize_get_particles(mDevice); }
01142 
01144     player_pose_t GetParticlePose(int index) const;
01145 
01147     void SetPose(double pose[3], double cov[3]);
01148 
01150     uint GetNumHypoths() const { return GetVar(mDevice->hypoth_count); };
01151 
01154     uint GetNumParticles() const { return GetVar(mDevice->num_particles); };
01155 };
01156 
01157 
01161 class LogProxy : public ClientProxy
01162 {
01163   private:
01164 
01165     void Subscribe(uint aIndex);
01166     void Unsubscribe();
01167 
01168     // libplayerc data structure
01169     playerc_log_t *mDevice;
01170 
01171   public:
01173     LogProxy(PlayerClient *aPc, uint aIndex=0);
01174 
01176     ~LogProxy();
01177 
01180     int GetType() const { return GetVar(mDevice->type); };
01181 
01183     int GetState() const { return GetVar(mDevice->state); };
01184 
01186     void SetWriteState(int aState);
01187 
01189     void SetReadState(int aState);
01190 
01192     void Rewind();
01193 
01195     void SetFilename(const std::string aFilename);
01196 };
01197 
01201 class MapProxy : public ClientProxy
01202 {
01203   private:
01204 
01205     void Subscribe(uint aIndex);
01206     void Unsubscribe();
01207 
01208     // libplayerc data structure
01209     playerc_map_t *mDevice;
01210 
01211   public:
01213     MapProxy(PlayerClient *aPc, uint aIndex=0);
01214 
01216     ~MapProxy();
01217 
01219     void RequestMap();
01220 
01222     int GetCellIndex(int x, int y) const
01223     { return y*GetWidth() + x; };
01224 
01226     int8_t GetCell(int x, int y) const
01227     { return GetVar(mDevice->cells[GetCellIndex(x,y)]); };
01228 
01230     double GetResolution() const { return GetVar(mDevice->resolution); };
01231 
01234     uint GetWidth() const { return GetVar(mDevice->width); };
01237     uint GetHeight() const { return GetVar(mDevice->height); };
01238 
01239     double GetOriginX() const { return GetVar(mDevice->origin[0]); };
01240     double GetOriginY() const { return GetVar(mDevice->origin[1]); };
01241 
01243     void GetMap(int8_t* aMap) const
01244     {
01245       return GetVarByRef(reinterpret_cast<int8_t*>(mDevice->cells),
01246                          reinterpret_cast<int8_t*>(mDevice->cells+GetWidth()*GetHeight()),
01247                          aMap);
01248     };
01249 };
01250 
01256 class OpaqueProxy : public ClientProxy
01257 {
01258 
01259   private:
01260 
01261     void Subscribe(uint aIndex);
01262     void Unsubscribe();
01263 
01264     // libplayerc data structure
01265     playerc_opaque_t *mDevice;
01266 
01267   public:
01268 
01270     OpaqueProxy(PlayerClient *aPc, uint aIndex=0);
01272     ~OpaqueProxy();
01273 
01275     uint GetCount() const { return GetVar(mDevice->data_count); };
01276 
01278     void GetData(uint8_t* aDest) const
01279       {
01280         return GetVarByRef(mDevice->data,
01281                            mDevice->data+GetVar(mDevice->data_count),
01282                            aDest);
01283       };
01284 
01286     void SendCmd(player_opaque_data_t* aData);
01287 
01288 };
01289 
01293 class PlannerProxy : public ClientProxy
01294 {
01295 
01296   private:
01297 
01298     void Subscribe(uint aIndex);
01299     void Unsubscribe();
01300 
01301     // libplayerc data structure
01302     playerc_planner_t *mDevice;
01303 
01304   public:
01305 
01307     PlannerProxy(PlayerClient *aPc, uint aIndex=0);
01309     ~PlannerProxy();
01310 
01312     void SetGoalPose(double aGx, double aGy, double aGa);
01313 
01316     void RequestWaypoints();
01317 
01320     void SetEnable(bool aEnable);
01321 
01323     uint GetPathValid() const { return GetVar(mDevice->path_valid); };
01324 
01326     uint GetPathDone() const { return GetVar(mDevice->path_done); };
01327 
01330     double GetPx() const { return GetVar(mDevice->px); };
01333     double GetPy() const { return GetVar(mDevice->py); };
01336     double GetPa() const { return GetVar(mDevice->pa); };
01337 
01339     player_pose_t GetPose() const
01340     {
01341       player_pose_t p;
01342       scoped_lock_t lock(mPc->mMutex);
01343       p.px = mDevice->px;
01344       p.py = mDevice->py;
01345       p.pa = mDevice->pa;
01346       return(p);
01347     }
01348 
01351     double GetGx() const { return GetVar(mDevice->gx); };
01354     double GetGy() const { return GetVar(mDevice->gy); };
01357     double GetGa() const { return GetVar(mDevice->ga); };
01358 
01360     player_pose_t GetGoal() const
01361     {
01362       player_pose_t p;
01363       scoped_lock_t lock(mPc->mMutex);
01364       p.px = mDevice->gx;
01365       p.py = mDevice->gy;
01366       p.pa = mDevice->ga;
01367       return(p);
01368     }
01369 
01372     double GetWx() const { return GetVar(mDevice->wx); };
01375     double GetWy() const { return GetVar(mDevice->wy); };
01378     double GetWa() const { return GetVar(mDevice->wa); };
01379 
01381     player_pose_t GetCurrentWaypoint() const
01382     {
01383       player_pose_t p;
01384       scoped_lock_t lock(mPc->mMutex);
01385       p.px = mDevice->wx;
01386       p.py = mDevice->wy;
01387       p.pa = mDevice->wa;
01388       return(p);
01389     }
01390 
01393     double GetIx(int i) const;
01396     double GetIy(int i) const;
01399     double GetIa(int i) const;
01400 
01402     player_pose_t GetWaypoint(uint aIndex) const
01403     {
01404       assert(aIndex < GetWaypointCount());
01405       player_pose_t p;
01406       scoped_lock_t lock(mPc->mMutex);
01407       p.px = mDevice->waypoints[aIndex][0];
01408       p.py = mDevice->waypoints[aIndex][1];
01409       p.pa = mDevice->waypoints[aIndex][2];
01410       return(p);
01411     }
01412 
01416     int GetCurrentWaypointId() const
01417       { return GetVar(mDevice->curr_waypoint); };
01418 
01420     uint GetWaypointCount() const
01421       { return GetVar(mDevice->waypoint_count); };
01422 
01427     player_pose_t operator [](uint aIndex) const
01428       { return GetWaypoint(aIndex); }
01429 
01430 };
01431 
01436 class Position1dProxy : public ClientProxy
01437 {
01438 
01439   private:
01440 
01441     void Subscribe(uint aIndex);
01442     void Unsubscribe();
01443 
01444     // libplayerc data structure
01445     playerc_position1d_t *mDevice;
01446 
01447   public:
01448 
01450     Position1dProxy(PlayerClient *aPc, uint aIndex=0);
01452     ~Position1dProxy();
01453 
01457     void SetSpeed(double aVel);
01458 
01462     void GoTo(double aPos, double aVel);
01463 
01466     void RequestGeom();
01467 
01469     player_pose_t GetPose() const
01470     {
01471       player_pose_t p;
01472       scoped_lock_t lock(mPc->mMutex);
01473       p.px = mDevice->pose[0];
01474       p.py = mDevice->pose[1];
01475       p.pa = mDevice->pose[2];
01476       return(p);
01477     }
01478 
01480     player_bbox_t GetSize() const
01481     {
01482       player_bbox_t b;
01483       scoped_lock_t lock(mPc->mMutex);
01484       b.sl = mDevice->size[0];
01485       b.sw = mDevice->size[1];
01486       return(b);
01487     }
01488 
01493     void SetMotorEnable(bool enable);
01494 
01497     void SetOdometry(double aPos);
01498 
01500     void ResetOdometry() { SetOdometry(0); };
01501 
01503     //void SetSpeedPID(double kp, double ki, double kd);
01504 
01506     //void SetPositionPID(double kp, double ki, double kd);
01507 
01510     //void SetPositionSpeedProfile(double spd, double acc);
01511 
01513     double  GetPos() const { return GetVar(mDevice->pos); };
01514 
01516     double  GetVel() const { return GetVar(mDevice->vel); };
01517 
01519     bool GetStall() const { return GetVar(mDevice->stall); };
01520 
01522     uint8_t GetStatus() const { return GetVar(mDevice->status); };
01523 
01525     bool IsLimitMin() const
01526       { return (GetVar(mDevice->status) &
01527                (1 << PLAYER_POSITION1D_STATUS_LIMIT_MIN)) > 0; };
01528 
01530     bool IsLimitCen() const
01531       { return (GetVar(mDevice->status) &
01532                (1 << PLAYER_POSITION1D_STATUS_LIMIT_CEN)) > 0; };
01533 
01535     bool IsLimitMax() const
01536       { return (GetVar(mDevice->status) &
01537                (1 << PLAYER_POSITION1D_STATUS_LIMIT_MAX)) > 0; };
01538 
01540     bool IsOverCurrent() const
01541       { return (GetVar(mDevice->status) &
01542                (1 << PLAYER_POSITION1D_STATUS_OC)) > 0; };
01543 
01545     bool IsTrajComplete() const
01546       { return (GetVar(mDevice->status) &
01547                (1 << PLAYER_POSITION1D_STATUS_TRAJ_COMPLETE)) > 0; };
01548 
01550     bool IsEnabled() const
01551       { return (GetVar(mDevice->status) &
01552                (1 << PLAYER_POSITION1D_STATUS_ENABLED)) > 0; };
01553 
01554 };
01555 
01560 class Position2dProxy : public ClientProxy
01561 {
01562 
01563   private:
01564 
01565     void Subscribe(uint aIndex);
01566     void Unsubscribe();
01567 
01568     // libplayerc data structure
01569     playerc_position2d_t *mDevice;
01570 
01571   public:
01572 
01574     Position2dProxy(PlayerClient *aPc, uint aIndex=0);
01576     ~Position2dProxy();
01577 
01581     void SetSpeed(double aXSpeed, double aYSpeed, double aYawSpeed);
01582 
01585     void SetSpeed(double aXSpeed, double aYawSpeed)
01586         { return SetSpeed(aXSpeed, 0, aYawSpeed);}
01587 
01589     void SetSpeed(player_pose_t vel)
01590         { return SetSpeed(vel.px, vel.py, vel.pa);}
01591 
01595     void SetVelHead(double aXSpeed, double aYSpeed, double aYawHead);
01596 
01599     void SetVelHead(double aXSpeed, double aYawHead)
01600         { return SetVelHead(aXSpeed, 0, aYawHead);}
01601 
01602 
01606     void GoTo(player_pose_t pos, player_pose_t vel);
01607 
01609     void GoTo(player_pose_t pos)
01610       {GoTo(pos,(player_pose_t) {0,0,0}); }
01611 
01614     void GoTo(double aX, double aY, double aYaw)
01615       {GoTo((player_pose_t) {aX,aY,aYaw},(player_pose_t) {0,0,0}); }
01616 
01618     void SetCarlike(double aXSpeed, double aDriveAngle);
01619 
01622     void RequestGeom();
01623 
01625     player_pose_t GetPose()
01626     {
01627       player_pose_t p;
01628       scoped_lock_t lock(mPc->mMutex);
01629       p.px = mDevice->pose[0];
01630       p.py = mDevice->pose[1];
01631       p.pa = mDevice->pose[2];
01632       return(p);
01633     }
01634 
01636     player_bbox_t GetSize()
01637     {
01638       player_bbox_t b;
01639       scoped_lock_t lock(mPc->mMutex);
01640       b.sl = mDevice->size[0];
01641       b.sw = mDevice->size[1];
01642       return(b);
01643     }
01644 
01649     void SetMotorEnable(bool enable);
01650 
01651     // Select velocity control mode.
01652     //
01653     // For the the p2os_position driver, set @p mode to 0 for direct wheel
01654     // velocity control (default), or 1 for separate translational and
01655     // rotational control.
01656     //
01657     // For the reb_position driver: 0 is direct velocity control, 1 is for
01658     // velocity-based heading PD controller (uses DoDesiredHeading()).
01659     //void SelectVelocityControl(unsigned char mode);
01660 
01662     void ResetOdometry();
01663 
01666     //void SelectPositionMode(unsigned char mode);
01667 
01670     void SetOdometry(double aX, double aY, double aYaw);
01671 
01673     //void SetSpeedPID(double kp, double ki, double kd);
01674 
01676     //void SetPositionPID(double kp, double ki, double kd);
01677 
01680     //void SetPositionSpeedProfile(double spd, double acc);
01681 
01682     //
01683     // void DoStraightLine(double m);
01684 
01685     //
01686     //void DoRotation(double yawspeed);
01687 
01688     //
01689     //void DoDesiredHeading(double yaw, double xspeed, double yawspeed);
01690 
01691     //
01692     //void SetStatus(uint8_t cmd, uint16_t value);
01693 
01694     //
01695     //void PlatformShutdown();
01696 
01698     double  GetXPos() const { return GetVar(mDevice->px); };
01699 
01701     double  GetYPos() const { return GetVar(mDevice->py); };
01702 
01704     double GetYaw() const { return GetVar(mDevice->pa); };
01705 
01707     double  GetXSpeed() const { return GetVar(mDevice->vx); };
01708 
01710     double  GetYSpeed() const { return GetVar(mDevice->vy); };
01711 
01713     double  GetYawSpeed() const { return GetVar(mDevice->va); };
01714 
01716     bool GetStall() const { return GetVar(mDevice->stall); };
01717 
01718 };
01719 
01726 class Position3dProxy : public ClientProxy
01727 {
01728 
01729   private:
01730 
01731     void Subscribe(uint aIndex);
01732     void Unsubscribe();
01733 
01734     // libplayerc data structure
01735     playerc_position3d_t *mDevice;
01736 
01737   public:
01738 
01740     Position3dProxy(PlayerClient *aPc, uint aIndex=0);
01742     ~Position3dProxy();
01743 
01747     void SetSpeed(double aXSpeed, double aYSpeed, double aZSpeed,
01748                   double aRollSpeed, double aPitchSpeed, double aYawSpeed);
01749 
01753     void SetSpeed(double aXSpeed, double aYSpeed,
01754                   double aZSpeed, double aYawSpeed)
01755       { SetSpeed(aXSpeed,aYSpeed,aZSpeed,0,0,aYawSpeed); }
01756 
01758     void SetSpeed(double aXSpeed, double aYSpeed, double aYawSpeed)
01759       { SetSpeed(aXSpeed, aYSpeed, 0, 0, 0, aYawSpeed); }
01760 
01763     void SetSpeed(double aXSpeed, double aYawSpeed)
01764       { SetSpeed(aXSpeed,0,0,0,0,aYawSpeed);}
01765 
01767     void SetSpeed(player_pose3d_t vel)
01768       { SetSpeed(vel.px,vel.py,vel.pz,vel.proll,vel.ppitch,vel.pyaw);}
01769 
01770 
01774     void GoTo(player_pose3d_t aPos, player_pose3d_t aVel);
01775 
01777     void GoTo(player_pose3d_t aPos)
01778       { GoTo(aPos, (player_pose3d_t) {0,0,0,0,0,0}); }
01779 
01780 
01783     void GoTo(double aX, double aY, double aZ,
01784               double aRoll, double aPitch, double aYaw)
01785       { GoTo((player_pose3d_t) {aX,aY,aZ,aRoll,aPitch,aYaw},
01786               (player_pose3d_t) {0,0,0,0,0,0});
01787       }
01788 
01793     void SetMotorEnable(bool aEnable);
01794 
01797     void SelectVelocityControl(int aMode);
01798 
01800     void ResetOdometry();
01801 
01805     void SetOdometry(double aX, double aY, double aZ,
01806                      double aRoll, double aPitch, double aYaw);
01807 
01808     // Select position mode
01809     // Set @p mode for 0 for velocity mode, 1 for position mode.
01810     //void SelectPositionMode(unsigned char mode);
01811 
01812     //
01813     //void SetSpeedPID(double kp, double ki, double kd);
01814 
01815     //
01816     //void SetPositionPID(double kp, double ki, double kd);
01817 
01818     // Sets the ramp profile for position based control
01819     // spd rad/s, acc rad/s/s
01820     //void SetPositionSpeedProfile(double spd, double acc);
01821 
01823     double  GetXPos() const { return GetVar(mDevice->pos_x); };
01824 
01826     double  GetYPos() const { return GetVar(mDevice->pos_y); };
01827 
01829     double  GetZPos() const { return GetVar(mDevice->pos_z); };
01830 
01832     double  GetRoll() const { return GetVar(mDevice->pos_roll); };
01833 
01835     double  GetPitch() const { return GetVar(mDevice->pos_pitch); };
01836 
01838     double  GetYaw() const { return GetVar(mDevice->pos_yaw); };
01839 
01841     double  GetXSpeed() const { return GetVar(mDevice->vel_x); };
01842 
01844     double  GetYSpeed() const { return GetVar(mDevice->vel_y); };
01845 
01847     double  GetZSpeed() const { return GetVar(mDevice->vel_z); };
01848 
01850     double  GetRollSpeed() const { return GetVar(mDevice->vel_roll); };
01851 
01853     double  GetPitchSpeed() const { return GetVar(mDevice->vel_pitch); };
01854 
01856     double  GetYawSpeed() const { return GetVar(mDevice->vel_yaw); };
01857 
01859     bool GetStall () const { return GetVar(mDevice->stall); };
01860 };
01863 class PowerProxy : public ClientProxy
01864 {
01865   private:
01866 
01867     void Subscribe(uint aIndex);
01868     void Unsubscribe();
01869 
01870     // libplayerc data structure
01871     playerc_power_t *mDevice;
01872 
01873   public:
01875     PowerProxy(PlayerClient *aPc, uint aIndex=0);
01877     ~PowerProxy();
01878 
01880     double GetCharge() const { return GetVar(mDevice->charge); };
01881 
01882 };
01883 
01890 class PtzProxy : public ClientProxy
01891 {
01892 
01893   private:
01894 
01895     void Subscribe(uint aIndex);
01896     void Unsubscribe();
01897 
01898     // libplayerc data structure
01899     playerc_ptz_t *mDevice;
01900 
01901   public:
01903     PtzProxy(PlayerClient *aPc, uint aIndex=0);
01904     // destructor
01905     ~PtzProxy();
01906 
01907   public:
01908 
01912     void SetCam(double aPan, double aTilt, double aZoom);
01913 
01915     void SetSpeed(double aPanSpeed=0, double aTiltSpeed=0, double aZoomSpeed=0);
01916 
01919     void SelectControlMode(uint aMode);
01920 
01922     double GetPan() const { return GetVar(mDevice->pan); };
01924     double GetTilt() const { return GetVar(mDevice->tilt); };
01926     double GetZoom() const { return GetVar(mDevice->zoom); };
01927 
01928 };
01929 
01932 class RFIDProxy : public ClientProxy
01933 {
01934 
01935   private:
01936 
01937     void Subscribe(uint aIndex);
01938     void Unsubscribe();
01939 
01940     // libplayerc data structure
01941     playerc_rfid_t *mDevice;
01942 
01943   public:
01945     RFIDProxy(PlayerClient *aPc, uint aIndex=0);
01947     ~RFIDProxy();
01948 
01950     uint GetTagsCount() const { return GetVar(mDevice->tags_count); };
01952     playerc_rfidtag_t GetRFIDTag(uint aIndex) const
01953       { return GetVar(mDevice->tags[aIndex]);};
01954 
01959     playerc_rfidtag_t operator [](uint aIndex) const
01960       { return(GetRFIDTag(aIndex)); }
01961 };
01962 
01967 class SimulationProxy : public ClientProxy
01968 {
01969   private:
01970 
01971     void Subscribe(uint aIndex);
01972     void Unsubscribe();
01973 
01974     // libplayerc data structure
01975     playerc_simulation_t *mDevice;
01976 
01977   public:
01979     SimulationProxy(PlayerClient *aPc, uint aIndex=0);
01981     ~SimulationProxy();
01982 
01985     void SetPose2d(char* identifier, double x, double y, double a);
01986 
01989     void GetPose2d(char* identifier, double& x, double& y, double& a);
01990 
01992     void SetProperty(char * name, char * property, int value);
01993     
01995     void SetProperty(char * name, char * property, double value);
01996     
01998     void SetProperty(char * name, char * property, char * value);
01999 };
02000 
02001 
02007 class SonarProxy : public ClientProxy
02008 {
02009   private:
02010 
02011     void Subscribe(uint aIndex);
02012     void Unsubscribe();
02013 
02014     // libplayerc data structure
02015     playerc_sonar_t *mDevice;
02016 
02017   public:
02019     SonarProxy(PlayerClient *aPc, uint aIndex=0);
02021     ~SonarProxy();
02022 
02024     uint GetCount() const { return GetVar(mDevice->scan_count); };
02025 
02027     double GetScan(uint aIndex) const
02028       { return GetVar(mDevice->scan[aIndex]); };
02031     double operator [] (uint aIndex) const { return GetScan(aIndex); }
02032 
02034     uint GetPoseCount() const { return GetVar(mDevice->pose_count); };
02035 
02037     player_pose_t GetPose(uint aIndex) const
02038       { return GetVar(mDevice->poses[aIndex]); };
02039 
02040     // Enable/disable the sonars.
02041     // Set @p state to 1 to enable, 0 to disable.
02042     // Note that when sonars are disabled the client will still receive sonar
02043     // data, but the ranges will always be the last value read from the sonars
02044     // before they were disabled.
02045     //void SetEnable(bool aEnable);
02046 
02048     void RequestGeom();
02049 };
02050 
02051 // /**
02052 // The @p SoundProxy class is used to control a @ref interface_sound
02053 // device, which allows you to play pre-recorded sound files on a robot.
02054 // */
02055 // class SoundProxy : public ClientProxy
02056 // {
02057 //
02058 //   private:
02059 //
02060 //     void Subscribe(uint aIndex);
02061 //     void Unsubscribe();
02062 //
02063 //     // libplayerc data structure
02064 //     playerc_sound_t *mDevice;
02065 //
02066 //   public:
02067 //     // Constructor
02068 //     SoundProxy(PlayerClient *aPc, uint aIndex=0);
02069 //
02070 //     ~SoundProxy();
02071 //
02072 //     /** Play the sound indicated by the index. */
02073 //     void Play(int aIndex);
02074 // };
02075 
02080 class SpeechProxy : public ClientProxy
02081 {
02082 
02083   private:
02084 
02085     void Subscribe(uint aIndex);
02086     void Unsubscribe();
02087 
02088     // libplayerc data structure
02089     playerc_speech_t *mDevice;
02090 
02091   public:
02093     SpeechProxy(PlayerClient *aPc, uint aIndex=0);
02095     ~SpeechProxy();
02096 
02099     void Say(std::string aStr);
02100 };
02101 
02105 class SpeechRecognitionProxy : public ClientProxy
02106 {
02107    void Subscribe(uint aIndex);
02108    void Unsubscribe();
02109 
02111    playerc_speechrecognition_t *mDevice;
02112   public:
02114    SpeechRecognitionProxy(PlayerClient *aPc, uint aIndex=0);
02115    ~SpeechRecognitionProxy();
02117    std::string GetWord(uint aWord) const{ 
02118      scoped_lock_t lock(mPc->mMutex);
02119      return std::string(mDevice->words[aWord]); 
02120    }
02121 
02123    uint GetCount(void) const { return GetVar(mDevice->wordCount); }
02124 
02127    std::string operator [](uint aWord) { return(GetWord(aWord)); }
02128 };
02129 
02130 // /**
02131 // The @p WaveformProxy class is used to read raw digital waveforms from
02132 // a @ref interface_waveform device.  */
02133 // class WaveformProxy : public ClientProxy
02134 // {
02135 //
02136 //   private:
02137 //
02138 //     void Subscribe(uint aIndex);
02139 //     void Unsubscribe();
02140 //
02141 //     // libplayerc data structure
02142 //     playerc_waveform_t *mDevice;
02143 //
02144 //   public:
02145 //     /// Constructor
02146 //     WaveformProxy(PlayerClient *aPc, uint aIndex=0);
02147 //
02148 //     /// Destructor
02149 //     ~WaveformProxy();
02150 //
02151 //     /// How many samples?
02152 //     uint GetCount() const { return GetVar(mDevice->data_count); };
02153 //
02154 //     /// sample rate in bits per second
02155 //     uint GetBitRate() const { return GetVar(mDevice->rate); };
02156 //
02157 //     /// sample depth in bits
02158 //     uint GetDepth() const { return GetVar(mDevice->depth); };
02159 //
02160 //     /// the data is buffered here for playback
02161 //     void GetImage(uint8_t* aBuffer) const
02162 //     {
02163 //       return GetVarByRef(mDevice->data,
02164 //                          mDevice->data+GetCount(),
02165 //                          aBuffer);
02166 //     };
02167 //
02168 //     // dsp file descriptor
02169 //     //uint GetFd() const { return GetVar(mDevice->fd); };
02170 //
02171 //     /// set up the DSP to the current bitrate and depth
02172 //     void ConfigureDSP(uint aBitRate, uint aDepth);
02173 //
02174 //     /// open the sound device
02175 //     void OpenDSPforWrite();
02176 //
02177 //     /// Play the waveform through the DSP
02178 //     void Play();
02179 // };
02180 
02183 class WiFiProxy: public ClientProxy
02184 {
02185 
02186   private:
02187 
02188     void Subscribe(uint aIndex);
02189     void Unsubscribe();
02190 
02191     // libplayerc data structure
02192     playerc_wifi_t *mDevice;
02193 
02194   public:
02196     WiFiProxy(PlayerClient *aPc, uint aIndex=0);
02198     ~WiFiProxy();
02199 
02200                          int GetLinkCount(){ return mDevice->link_count; };
02201                          char* GetOwnIP(){ return mDevice->ip; };
02202 
02203                          char* GetLinkIP(int index) { return (char*) mDevice->links[index].ip; };
02204                          char* GetLinkMAC(int index) { return (char*) mDevice->links[index].mac; };
02205                          char* GetLinkESSID(int index) { return (char*)mDevice->links[index].essid; };
02206                          double GetLinkFreq(int index) {return mDevice->links[index].freq;};
02207                          int     GetLinkMode(int index) { return mDevice->links[index].mode; };
02208                          int     GetLinkEncrypt(int index) {return mDevice->links[index].encrypt; };
02209                          int   GetLinkQuality(int index) { return mDevice->links[index].qual; };
02210                          int     GetLinkLevel(int index) {return mDevice->links[index].level; };
02211                          int     GetLinkNoise(int index) {return mDevice->links[index].noise; } ;                
02212 
02213                         //player_wifi_link_t
02214 //     int GetLinkQuality(char/// ip = NULL);
02215 //     int GetLevel(char/// ip = NULL);
02216 //     int GetLeveldBm(char/// ip = NULL) { return GetLevel(ip) - 0x100; }
02217 //     int GetNoise(char/// ip = NULL);
02218 //     int GetNoisedBm(char/// ip = NULL) { return GetNoise(ip) - 0x100; }
02219 //
02220 //     uint16_t GetMaxLinkQuality() { return maxqual; }
02221 //     uint8_t GetMode() { return op_mode; }
02222 //
02223 //     int GetBitrate();
02224 //
02225 //     char/// GetMAC(char *buf, int len);
02226 //
02227 //     char/// GetIP(char *buf, int len);
02228 //     char/// GetAP(char *buf, int len);
02229 //
02230 //     int AddSpyHost(char *address);
02231 //     int RemoveSpyHost(char *address);
02232 //
02233 //   private:
02234 //     int GetLinkIndex(char *ip);
02235 //
02236 //     /// The current wifi data.
02237 //     int link_count;
02238 //     player_wifi_link_t links[PLAYER_WIFI_MAX_LINKS];
02239 //     uint32_t throughput;
02240 //     uint8_t op_mode;
02241 //     int32_t bitrate;
02242 //     uint16_t qual_type, maxqual, maxlevel, maxnoise;
02243 //
02244 //     char access_point[32];
02245 };
02246 
02249 class WSNProxy : public ClientProxy
02250 {
02251 
02252   private:
02253 
02254     void Subscribe(uint aIndex);
02255     void Unsubscribe();
02256 
02257     // libplayerc data structure
02258     playerc_wsn_t *mDevice;
02259 
02260   public:
02262     WSNProxy(PlayerClient *aPc, uint aIndex=0);
02264     ~WSNProxy();
02265 
02266     uint GetNodeType    () const { return GetVar(mDevice->node_type);      };
02267     uint GetNodeID      () const { return GetVar(mDevice->node_id);        };
02268     uint GetNodeParentID() const { return GetVar(mDevice->node_parent_id); };
02269 
02270     player_wsn_node_data_t
02271        GetNodeDataPacket() const { return GetVar(mDevice->data_packet);    };
02272 
02273     void SetDevState(int nodeID, int groupID, int devNr, int value);
02274     void Power(int nodeID, int groupID, int value);
02275     void DataType(int value);
02276     void DataFreq(int nodeID, int groupID, float frequency);
02277 };
02278 
02280 }
02281 
02282 namespace std
02283 {
02284   std::ostream& operator << (std::ostream& os, const player_point_2d_t& c);
02285   std::ostream& operator << (std::ostream& os, const player_pose_t& c);
02286   std::ostream& operator << (std::ostream& os, const player_pose3d_t& c);
02287   std::ostream& operator << (std::ostream& os, const player_bbox_t& c);
02288   std::ostream& operator << (std::ostream& os, const player_segment_t& c);
02289   std::ostream& operator << (std::ostream& os, const playerc_device_info_t& c);
02290 
02291   std::ostream& operator << (std::ostream& os, const PlayerCc::ClientProxy& c);
02292   std::ostream& operator << (std::ostream& os, const PlayerCc::ActArrayProxy& c);
02293   std::ostream& operator << (std::ostream& os, const PlayerCc::AioProxy& c);
02294   //std::ostream& operator << (std::ostream& os, const PlayerCc::AudioDspProxy& c);
02295   //std::ostream& operator << (std::ostream& os, const PlayerCc::AudioMixerProxy& c);
02296   //std::ostream& operator << (std::ostream& os, const PlayerCc::BlinkenLightProxy& c);
02297   std::ostream& operator << (std::ostream& os, const PlayerCc::BlobfinderProxy& c);
02298   std::ostream& operator << (std::ostream& os, const PlayerCc::BumperProxy& c);
02299   std::ostream& operator << (std::ostream& os, const PlayerCc::CameraProxy& c);
02300   std::ostream& operator << (std::ostream& os, const PlayerCc::DioProxy& c);
02301   //std::ostream& operator << (std::ostream& os, const PlayerCc::EnergyProxy& c);
02302   std::ostream& operator << (std::ostream& os, const PlayerCc::FiducialProxy& c);
02303   std::ostream& operator << (std::ostream& os, const PlayerCc::GpsProxy& c);
02304   std::ostream& operator << (std::ostream& os, const PlayerCc::GripperProxy& c);
02305   std::ostream& operator << (std::ostream& os, const PlayerCc::IrProxy& c);
02306   std::ostream& operator << (std::ostream& os, const PlayerCc::LaserProxy& c);
02307   std::ostream& operator << (std::ostream& os, const PlayerCc::LimbProxy& c);
02308   std::ostream& operator << (std::ostream& os, const PlayerCc::LocalizeProxy& c);
02309   std::ostream& operator << (std::ostream& os, const PlayerCc::LogProxy& c);
02310   std::ostream& operator << (std::ostream& os, const PlayerCc::MapProxy& c);
02311   std::ostream& operator << (std::ostream& os, const PlayerCc::OpaqueProxy& c);
02312   std::ostream& operator << (std::ostream& os, const PlayerCc::PlannerProxy& c);
02313   std::ostream& operator << (std::ostream& os, const PlayerCc::Position1dProxy& c);
02314   std::ostream& operator << (std::ostream& os, const PlayerCc::Position2dProxy& c);
02315   std::ostream& operator << (std::ostream& os, const PlayerCc::Position3dProxy& c);
02316   std::ostream& operator << (std::ostream& os, const PlayerCc::PowerProxy& c);
02317   std::ostream& operator << (std::ostream& os, const PlayerCc::PtzProxy& c);
02318   std::ostream& operator << (std::ostream& os, const PlayerCc::SimulationProxy& c);
02319   std::ostream& operator << (std::ostream& os, const PlayerCc::SonarProxy& c);
02320   std::ostream& operator << (std::ostream& os, const PlayerCc::SpeechProxy& c);
02321   std::ostream& operator << (std::ostream& os, const PlayerCc::SpeechRecognitionProxy& c);
02322   //std::ostream& operator << (std::ostream& os, const PlayerCc::WafeformProxy& c);
02323   std::ostream& operator << (std::ostream& os, const PlayerCc::WiFiProxy& c);
02324   std::ostream& operator << (std::ostream& os, const PlayerCc::RFIDProxy& c);
02325   std::ostream& operator << (std::ostream& os, const PlayerCc::WSNProxy& c);
02326   //std::ostream& operator << (std::ostream& os, const PlayerCc::TruthProxy& c);
02327 }
02328 
02329 #endif
02330 

Last updated 12 September 2005 21:38:45