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  *  This library is free software; you can redistribute it and/or
00025  *  modify it under the terms of the GNU Lesser General Public
00026  *  License as published by the Free Software Foundation; either
00027  *  version 2.1 of the License, or (at your option) any later version.
00028  *
00029  *  This library is distributed in the hope that it will be useful,
00030  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00031  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00032  *  Lesser General Public License for more details.
00033  *
00034  *  You should have received a copy of the GNU Lesser General Public
00035  *  License along with this library; if not, write to the Free Software
00036  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00037  *
00038  ********************************************************************/
00039 
00040 /***************************************************************************
00041  * Desc: Player v2.0 C++ client
00042  * Authors: Brad Kratochvil, Toby Collett
00043  *
00044  * Date: 23 Sep 2005
00045  # CVS: $Id: playerc++.h 6996 2008-08-30 02:45:26Z thjc $
00046  **************************************************************************/
00047 
00048 
00049 #ifndef PLAYERCC_H
00050 #define PLAYERCC_H
00051 
00052 #include <cmath>
00053 #include <string>
00054 #include <list>
00055 #include <vector>
00056 #include <cstring>
00057 
00058 #include "libplayerc/playerc.h"
00059 #include "libplayerc++/utility.h"
00060 #include "libplayerc++/playerc++config.h"
00061 #include "libplayerc++/playerclient.h"
00062 #include "libplayerc++/playererror.h"
00063 #include "libplayerc++/clientproxy.h"
00064 #include "libplayercore/interface_util.h"
00065 
00066 // Don't think we need to include these here
00067 /*
00068 #ifdef HAVE_BOOST_SIGNALS
00069   #include <boost/signal.hpp>
00070   #include <boost/bind.hpp>
00071 #endif
00072 
00073 #ifdef HAVE_BOOST_THREAD
00074   #include <boost/thread/mutex.hpp>
00075   #include <boost/thread/thread.hpp>
00076   #include <boost/thread/xtime.hpp>
00077 #endif
00078 */
00079 
00080 namespace PlayerCc
00081 {
00082 
00083 // /**
00084 // * The @p SomethingProxy class is a template for adding new subclasses of
00085 // * ClientProxy.  You need to have at least all of the following:
00086 // */
00087 // class SomethingProxy : public ClientProxy
00088 // {
00089 //
00090 //   private:
00091 //
00092 //     // Subscribe
00093 //     void Subscribe(uint32_t aIndex);
00094 //     // Unsubscribe
00095 //     void Unsubscribe();
00096 //
00097 //     // libplayerc data structure
00098 //     playerc_something_t *mDevice;
00099 //
00100 //   public:
00101 //     // Constructor
00102 //     SomethingProxy(PlayerClient *aPc, uint32_t aIndex=0);
00103 //     // Destructor
00104 //     ~SomethingProxy();
00105 //
00106 // };
00107 
00119 // ==============================================================
00120 //
00121 // These are alphabetized, please keep them that way!!!
00122 //
00123 // ==============================================================
00124 
00129 class ActArrayProxy : public ClientProxy
00130 {
00131   private:
00132 
00133    void Subscribe(uint32_t aIndex);
00134    void Unsubscribe();
00135 
00136    // libplayerc data structure
00137    playerc_actarray_t *mDevice;
00138 
00139   public:
00140 
00142     ActArrayProxy(PlayerClient *aPc, uint32_t aIndex=0);
00144     ~ActArrayProxy();
00145 
00148     void RequestGeometry(void);
00149 
00151     void SetPowerConfig(bool aVal);
00153     void SetBrakesConfig(bool aVal);
00155     void SetSpeedConfig(uint32_t aJoint, float aSpeed);
00156 
00158     void MoveTo(uint32_t aJoint, float aPos);
00160     void MoveToMulti(std::vector<float> aPos);
00162     void MoveAtSpeed(uint32_t aJoint, float aSpeed);
00164     void MoveAtSpeedMulti(std::vector<float> aSpeed);
00166     void MoveHome(int aJoint);
00168     void SetActuatorCurrent(uint32_t aJoint, float aCurrent);
00170     void SetActuatorCurrentMulti(std::vector<float> aCurrent);
00171 
00173     uint32_t GetCount(void) const { return GetVar(mDevice->actuators_count); }
00175     player_actarray_actuator_t GetActuatorData(uint32_t aJoint) const;
00177     player_actarray_actuatorgeom_t GetActuatorGeom(uint32_t aJoint) const;
00179     player_point_3d_t GetBasePos(void) const { return GetVar(mDevice->base_pos); }
00181     player_orientation_3d_t GetBaseOrientation(void) const { return GetVar(mDevice->base_orientation); }
00182 
00183 
00188     player_actarray_actuator_t operator [](uint32_t aJoint)
00189       { return(GetActuatorData(aJoint)); }
00190 };
00191 
00195 class AioProxy : public ClientProxy
00196 {
00197   private:
00198 
00199     void Subscribe(uint32_t aIndex);
00200     void Unsubscribe();
00201 
00202     // libplayerc data structure
00203     playerc_aio_t *mDevice;
00204 
00205   public:
00206 
00207     AioProxy (PlayerClient *aPc, uint32_t aIndex=0);
00208     ~AioProxy();
00209 
00211     uint32_t GetCount() const { return(GetVar(mDevice->voltages_count)); };
00212 
00214     double GetVoltage(uint32_t aIndex)  const
00215       { return(GetVar(mDevice->voltages[aIndex])); };
00216 
00218     void SetVoltage(uint32_t aIndex, double aVoltage);
00219 
00224     double operator [](uint32_t aIndex) const
00225       { return GetVoltage(aIndex); }
00226 
00227 };
00228 
00229 
00233 class AudioProxy : public ClientProxy
00234 {
00235 
00236   private:
00237 
00238     void Subscribe(uint32_t aIndex);
00239     void Unsubscribe();
00240 
00241     // libplayerc data structure
00242     playerc_audio_t *mDevice;
00243 
00244   public:
00245 
00246     AudioProxy(PlayerClient *aPc, uint32_t aIndex=0);
00247     ~AudioProxy();
00248 
00250     uint32_t GetMixerDetailsCount() const {return(GetVar(mDevice->channel_details_list.details_count));};
00252     player_audio_mixer_channel_detail_t GetMixerDetails(int aIndex) const  {return(GetVar(mDevice->channel_details_list.details[aIndex]));};
00254     uint32_t GetDefaultOutputChannel() const  {return(GetVar(mDevice->channel_details_list.default_output));};
00256     uint32_t GetDefaultInputChannel() const {return(GetVar(mDevice->channel_details_list.default_input));};
00257 
00259     uint32_t GetWavDataLength() const {return(GetVar(mDevice->wav_data.data_count));};
00264     void GetWavData(uint8_t* aData) const
00265       {
00266         return GetVarByRef(mDevice->wav_data.data,
00267                            mDevice->wav_data.data+GetWavDataLength(),
00268                            aData);
00269       };
00270 
00272     uint32_t GetSeqCount() const {return(GetVar(mDevice->seq_data.tones_count));};
00274     player_audio_seq_item_t GetSeqItem(int aIndex) const  {return(GetVar(mDevice->seq_data.tones[aIndex]));};
00275 
00277     uint32_t GetChannelCount() const {return(GetVar(mDevice->mixer_data.channels_count));};
00279     player_audio_mixer_channel_t GetChannel(int aIndex) const  {return(GetVar(mDevice->mixer_data.channels[aIndex]));};
00281     uint32_t GetState(void) const {return(GetVar(mDevice->state));};
00282 
00283 
00284 
00286     void PlayWav(uint32_t aDataCount, uint8_t *aData, uint32_t aFormat);
00287 
00289     void SetWavStremRec(bool aState);
00290 
00292     void PlaySample(int aIndex);
00293 
00295     void PlaySeq(player_audio_seq_t * aTones);
00296 
00298     void SetMultMixerLevels(player_audio_mixer_channel_list_t * aLevels);
00299 
00301     void SetMixerLevel(uint32_t index, float amplitude, uint8_t active);
00302 
00305     void RecordWav();
00306 
00308     void LoadSample(int aIndex, uint32_t aDataCount, uint8_t *aData, uint32_t aFormat);
00309 
00312     void GetSample(int aIndex);
00313 
00315     void RecordSample(int aIndex, uint32_t aLength);
00316 
00319     void GetMixerLevels();
00320 
00323     void GetMixerDetails();
00324 
00325 };
00326 
00334 class BlackBoardProxy : public ClientProxy
00335 {
00336   private:
00337     void Subscribe(uint aIndex);
00338     void Unsubscribe();
00339 
00340     // libplayerc data structure
00341     playerc_blackboard_t *mDevice;
00342 
00343   public:
00345         BlackBoardProxy(PlayerClient *aPc, uint aIndex=0);
00347         ~BlackBoardProxy();
00349         player_blackboard_entry_t *SubscribeToKey(const char *key, const char* group = "");
00351         void UnsubscribeFromKey(const char *key, const char* group = "");
00353         void SubscribeToGroup(const char* key);
00355         void UnsubscribeFromGroup(const char* group);
00357         void SetEntry(const player_blackboard_entry_t &entry);
00359         player_blackboard_entry_t *GetEntry(const char* key, const char* group);
00361         void SetEventHandler(void (*on_blackboard_event)(playerc_blackboard_t *, player_blackboard_entry_t));
00362 };
00363 
00364 // /**
00365 // The @p BlinkenlightProxy class is used to enable and disable
00366 // a flashing indicator light, and to set its period, via a @ref
00367 // interface_blinkenlight device */
00368 // class BlinkenLightProxy : public ClientProxy
00369 // {
00370 //   private:
00371 //
00372 //     void Subscribe(uint32_t aIndex);
00373 //     void Unsubscribe();
00374 //
00375 //     // libplayerc data structure
00376 //     playerc_blinkenlight_t *mDevice;
00377 //
00378 //   public:
00379 //     /** Constructor.
00380 //         Leave the access field empty to start unconnected.
00381 //     */
00382 //     BlinkenLightProxy(PlayerClient *aPc, uint32_t aIndex=0);
00383 //     ~BlinkenLightProxy();
00384 //
00385 //     // true: indicator light enabled, false: disabled.
00386 //     bool GetEnable();
00387 //
00388 //     /** The current period (one whole on/off cycle) of the blinking
00389 //         light. If the period is zero and the light is enabled, the light
00390 //         is on.
00391 //     */
00392 //     void SetPeriod(double aPeriod);
00393 //
00394 //     /** Set the state of the indicator light. A period of zero means
00395 //         the light will be unblinkingly on or off. Returns 0 on
00396 //         success, else -1.
00397 //       */
00398 //     void SetEnable(bool aSet);
00399 // };
00400 
00407 class BlobfinderProxy : public ClientProxy
00408 {
00409   private:
00410 
00411     void Subscribe(uint32_t aIndex);
00412     void Unsubscribe();
00413 
00414     // libplayerc data structure
00415     playerc_blobfinder_t *mDevice;
00416 
00417   public:
00419     BlobfinderProxy(PlayerClient *aPc, uint32_t aIndex=0);
00421     ~BlobfinderProxy();
00422 
00424     uint32_t GetCount() const { return GetVar(mDevice->blobs_count); };
00426     playerc_blobfinder_blob_t GetBlob(uint32_t aIndex) const
00427       { return GetVar(mDevice->blobs[aIndex]);};
00428 
00430     uint32_t GetWidth() const { return GetVar(mDevice->width); };
00432     uint32_t GetHeight() const { return GetVar(mDevice->height); };
00433 
00438     playerc_blobfinder_blob_t operator [](uint32_t aIndex) const
00439       { return(GetBlob(aIndex)); }
00440 
00441 /*
00443     void SetTrackingColor(uint32_t aReMin=0,   uint32_t aReMax=255, uint32_t aGrMin=0,
00444                           uint32_t aGrMax=255, uint32_t aBlMin=0,   uint32_t aBlMax=255);
00445     void SetImagerParams(int aContrast, int aBrightness,
00446                          int aAutogain, int aColormode);
00447     void SetContrast(int aC);
00448     void SetColorMode(int aM);
00449     void SetBrightness(int aB);
00450     void SetAutoGain(int aG);*/
00451 
00452 };
00453 
00458 class BumperProxy : public ClientProxy
00459 {
00460 
00461   private:
00462 
00463     void Subscribe(uint32_t aIndex);
00464     void Unsubscribe();
00465 
00466     // libplayerc data structure
00467     playerc_bumper_t *mDevice;
00468 
00469   public:
00470 
00471     BumperProxy(PlayerClient *aPc, uint32_t aIndex=0);
00472     ~BumperProxy();
00473 
00474     uint32_t GetCount() const { return GetVar(mDevice->bumper_count); };
00475 
00477     uint32_t IsBumped(uint32_t aIndex) const
00478       { return GetVar(mDevice->bumpers[aIndex]); };
00479 
00481     bool IsAnyBumped();
00482 
00484     void RequestBumperConfig();
00485 
00487     uint32_t GetPoseCount() const { return GetVar(mDevice->pose_count); };
00488 
00490     player_bumper_define_t GetPose(uint32_t aIndex) const
00491       { return GetVar(mDevice->poses[aIndex]); };
00492 
00497     bool operator [](uint32_t aIndex) const
00498       { return IsBumped(aIndex); }
00499 
00500 };
00501 
00505 class CameraProxy : public ClientProxy
00506 {
00507 
00508   private:
00509 
00510     virtual void Subscribe(uint32_t aIndex);
00511     virtual void Unsubscribe();
00512 
00513     // libplayerc data structure
00514     playerc_camera_t *mDevice;
00515 
00516     std::string mPrefix;
00517     int mFrameNo;
00518 
00519   public:
00520 
00522     CameraProxy (PlayerClient *aPc, uint32_t aIndex=0);
00523 
00524     virtual ~CameraProxy();
00525 
00529     void SaveFrame(const std::string aPrefix, uint32_t aWidth=4);
00530 
00532     void Decompress();
00533 
00535     uint32_t GetDepth() const { return GetVar(mDevice->bpp); };
00536 
00538     uint32_t GetWidth() const { return GetVar(mDevice->width); };
00539 
00541     uint32_t GetHeight() const { return GetVar(mDevice->height); };
00542 
00549     uint32_t GetFormat() const { return GetVar(mDevice->format); };
00550 
00552     uint32_t GetImageSize() const { return GetVar(mDevice->image_count); };
00553 
00558     void GetImage(uint8_t* aImage) const
00559       {
00560         return GetVarByRef(mDevice->image,
00561                            mDevice->image+GetVar(mDevice->image_count),
00562                            aImage);
00563       };
00564 
00569     uint32_t GetCompression() const { return GetVar(mDevice->compression); };
00570 
00571 };
00572 
00573 
00578 class DioProxy : public ClientProxy
00579 {
00580   private:
00581 
00582     void Subscribe(uint32_t aIndex);
00583     void Unsubscribe();
00584 
00585     // libplayerc data structure
00586     playerc_dio_t *mDevice;
00587 
00588   public:
00590     DioProxy(PlayerClient *aPc, uint32_t aIndex=0);
00592     ~DioProxy();
00593 
00595     uint32_t GetCount() const { return GetVar(mDevice->count); };
00596 
00598     uint32_t GetDigin() const { return GetVar(mDevice->digin); };
00599 
00601     bool GetInput(uint32_t aIndex) const;
00602 
00604     void SetOutput(uint32_t aCount, uint32_t aDigout);
00605 
00610     uint32_t operator [](uint32_t aIndex) const
00611       { return GetInput(aIndex); }
00612 };
00613 
00619 class FiducialProxy : public ClientProxy
00620 {
00621   private:
00622     void Subscribe(uint32_t aIndex);
00623     void Unsubscribe();
00624 
00625     // libplayerc data structure
00626     playerc_fiducial_t *mDevice;
00627 
00628   public:
00630     FiducialProxy(PlayerClient *aPc, uint32_t aIndex=0);
00632     ~FiducialProxy();
00633 
00635     uint32_t GetCount() const { return GetVar(mDevice->fiducials_count); };
00636 
00638     player_fiducial_item_t GetFiducialItem(uint32_t aIndex) const
00639       { return GetVar(mDevice->fiducials[aIndex]);};
00640 
00642     player_pose3d_t GetSensorPose() const
00643       { return GetVar(mDevice->fiducial_geom.pose);};
00644 
00646     player_bbox3d_t GetSensorSize() const
00647       { return GetVar(mDevice->fiducial_geom.size);};
00648 
00650     player_bbox2d_t GetFiducialSize() const
00651       { return GetVar(mDevice->fiducial_geom.fiducial_size);};
00652 
00654     void RequestGeometry();
00655 
00660     player_fiducial_item_t operator [](uint32_t aIndex) const
00661       { return GetFiducialItem(aIndex); }
00662 };
00663 
00667 class GpsProxy : public ClientProxy
00668 {
00669 
00670   private:
00671 
00672     void Subscribe(uint32_t aIndex);
00673     void Unsubscribe();
00674 
00675     // libplayerc data structure
00676     playerc_gps_t *mDevice;
00677 
00678   public:
00679 
00680     // Constructor
00681     GpsProxy(PlayerClient *aPc, uint32_t aIndex=0);
00682 
00683     ~GpsProxy();
00684 
00686     double GetLatitude() const { return GetVar(mDevice->lat); };
00687     double GetLongitude() const { return GetVar(mDevice->lon); };
00688 
00690     double GetAltitude() const { return GetVar(mDevice->alt); };
00691 
00693     uint32_t GetSatellites() const { return GetVar(mDevice->sat_count); };
00694 
00696     uint32_t GetQuality() const { return GetVar(mDevice->quality); };
00697 
00699     double GetHdop() const { return GetVar(mDevice->hdop); };
00700 
00702     double GetVdop() const { return GetVar(mDevice->vdop); };
00703 
00705     double GetUtmEasting() const { return GetVar(mDevice->utm_e); };
00706     double GetUtmNorthing() const { return GetVar(mDevice->utm_n); };
00707 
00709     double GetTime() const { return GetVar(mDevice->utc_time); };
00710 
00712     double GetErrHorizontal() const { return GetVar(mDevice->err_horz); };
00713     double GetErrVertical() const { return GetVar(mDevice->err_vert); };
00714 };
00715 
00723 class Graphics2dProxy : public ClientProxy
00724 {
00725 
00726   private:
00727 
00728     // Subscribe
00729     void Subscribe(uint32_t aIndex);
00730     // Unsubscribe
00731     void Unsubscribe();
00732 
00733     // libplayerc data structure
00734     playerc_graphics2d_t *mDevice;
00735 
00736   public:
00737     // Constructor
00738     Graphics2dProxy(PlayerClient *aPc, uint32_t aIndex=0);
00739     // Destructor
00740     ~Graphics2dProxy();
00741 
00743     void Color(player_color_t col);
00744 
00746     void Color(uint8_t red,  uint8_t green,  uint8_t blue,  uint8_t alpha);
00747 
00749     void Clear(void);
00750 
00752     void DrawPoints(player_point_2d_t pts[], int count);
00753 
00755     void DrawPolygon(player_point_2d_t pts[],
00756                      int count,
00757                      bool filled,
00758                      player_color_t fill_color);
00759 
00761     void DrawPolyline(player_point_2d_t pts[], int count);
00762 };
00763 
00769 class Graphics3dProxy : public ClientProxy
00770 {
00771 
00772   private:
00773 
00774     // Subscribe
00775     void Subscribe(uint32_t aIndex);
00776     // Unsubscribe
00777     void Unsubscribe();
00778 
00779     // libplayerc data structure
00780     playerc_graphics3d_t *mDevice;
00781 
00782   public:
00783     // Constructor
00784     Graphics3dProxy(PlayerClient *aPc, uint32_t aIndex=0);
00785     // Destructor
00786     ~Graphics3dProxy();
00787 
00789     void Color(player_color_t col);
00790 
00792     void Color(uint8_t red,  uint8_t green,  uint8_t blue,  uint8_t alpha);
00793 
00795     void Clear(void);
00796 
00798     void Draw(player_graphics3d_draw_mode_t mode, player_point_3d_t pts[], int count);
00799 
00800 };
00801 
00806 class GripperProxy : public ClientProxy
00807 {
00808 
00809   private:
00810 
00811     void Subscribe(uint32_t aIndex);
00812     void Unsubscribe();
00813 
00814     // libplayerc data structure
00815     playerc_gripper_t *mDevice;
00816 
00817   public:
00818 
00820     GripperProxy(PlayerClient *aPc, uint32_t aIndex=0);
00822     ~GripperProxy();
00823 
00826     void RequestGeometry(void);
00827 
00829     uint32_t GetState() const { return GetVar(mDevice->state); };
00831     uint32_t GetBeams() const { return GetVar(mDevice->beams); };
00833     player_pose3d_t GetPose() const { return GetVar(mDevice->pose); };
00835     player_bbox3d_t GetOuterSize() const { return GetVar(mDevice->outer_size); };
00837     player_bbox3d_t GetInnerSize() const { return GetVar(mDevice->inner_size); };
00839     uint32_t GetNumBeams() const { return GetVar(mDevice->num_beams); };
00841     uint32_t GetCapacity() const { return GetVar(mDevice->capacity); };
00843     uint32_t GetStored() const { return GetVar(mDevice->stored); };
00844 
00846     void Open();
00848     void Close();
00850     void Stop();
00852     void Store();
00854     void Retrieve();
00855 };
00856 
00859 class HealthProxy : public ClientProxy
00860 {
00861 
00862   private:
00863 
00864     void Subscribe(uint32_t aIndex);
00865     void Unsubscribe();
00866 
00867     // libplayerc data structure
00868     playerc_health_t *mDevice;
00869 
00870   public:
00872     HealthProxy(PlayerClient *aPc, uint32_t aIndex=0);
00874     ~HealthProxy();
00875 
00877     float GetIdleCPU();
00878 
00880     float GetSystemCPU();
00881 
00883     float GetUserCPU();
00884 
00886     int64_t GetMemTotal();
00887 
00889     int64_t GetMemUsed();
00890 
00892     int64_t GetMemFree();
00893 
00895     int64_t GetSwapTotal();
00896 
00898     int64_t GetSwapUsed();
00899 
00901     int64_t GetSwapFree();
00902 
00904     float GetPercMemUsed();
00905 
00907     float GetPercSwapUsed();
00908 
00910     float GetPercTotalUsed();
00911 };
00912 
00913 
00914 
00919 class ImuProxy : public ClientProxy
00920 {
00921   private:
00922     void Subscribe(uint32_t aIndex);
00923     void Unsubscribe();
00924 
00925     // libplayerc data structure
00926     playerc_imu_t *mDevice;
00927 
00928   public:
00929 
00931     ImuProxy(PlayerClient *aPc, uint32_t aIndex=0);
00933     ~ImuProxy();
00934 
00936     player_pose3d_t GetPose() const { return GetVar(mDevice->pose); };
00938     float GetXAccel();
00939     float GetYAccel();
00940     float GetZAccel();
00941     float GetXGyro();
00942     float GetYGyro();
00943     float GetZGyro();
00944     float GetXMagn();
00945     float GetYMagn();
00946     float GetZMagn();
00947 
00948     player_imu_data_calib_t GetRawValues() const
00949     { return GetVar(mDevice->calib_data); };
00950 
00952     void SetDatatype(int aDatatype);
00953 
00955     void ResetOrientation(int aValue);
00956     
00957 
00958 };
00959 
00960 
00965 class IrProxy : public ClientProxy
00966 {
00967 
00968   private:
00969 
00970     void Subscribe(uint32_t aIndex);
00971     void Unsubscribe();
00972 
00973     // libplayerc data structure
00974     playerc_ir_t *mDevice;
00975 
00976   public:
00977 
00979     IrProxy(PlayerClient *aPc, uint32_t aIndex=0);
00981     ~IrProxy();
00982 
00984     uint32_t GetCount() const { return GetVar(mDevice->data.ranges_count); };
00986     double GetRange(uint32_t aIndex) const
00987       { return GetVar(mDevice->data.ranges[aIndex]); };
00989     double GetVoltage(uint32_t aIndex) const
00990       { return GetVar(mDevice->data.voltages[aIndex]); };
00992     uint32_t GetPoseCount() const { return GetVar(mDevice->poses.poses_count); };
00994     player_pose3d_t GetPose(uint32_t aIndex) const
00995       {return GetVar(mDevice->poses.poses[aIndex]);};
00996 
00998     void RequestGeom();
00999 
01004     double operator [](uint32_t aIndex) const
01005       { return GetRange(aIndex); }
01006 
01007 };
01008 
01014 class LaserProxy : public ClientProxy
01015 {
01016   private:
01017 
01018     void Subscribe(uint32_t aIndex);
01019     void Unsubscribe();
01020 
01021     // libplayerc data structure
01022     playerc_laser_t *mDevice;
01023 
01024     // local storage of config
01025     double min_angle, max_angle, scan_res, range_res, scanning_frequency;
01026     bool intensity;
01027 
01028   public:
01029 
01031     LaserProxy(PlayerClient *aPc, uint32_t aIndex=0);
01033     ~LaserProxy();
01034 
01036     uint32_t GetCount() const { return GetVar(mDevice->scan_count); };
01037 
01039     double GetMaxRange() const { return GetVar(mDevice->max_range); };
01040 
01042     double GetScanRes() const { return GetVar(mDevice->scan_res); };
01043 
01045     double GetRangeRes() const { return GetVar(mDevice->range_res); };
01046 
01048     double GetScanningFrequency() const { return GetVar(mDevice->scanning_frequency); };
01049 
01051     double GetMinAngle() const { return GetVar(mDevice->scan_start); };
01053     double GetMaxAngle() const
01054     {
01055       scoped_lock_t lock(mPc->mMutex);
01056       return mDevice->scan_start + (mDevice->scan_count - 1)*mDevice->scan_res;
01057     };
01058 
01060     double GetConfMinAngle() const { return min_angle; };
01062     double GetConfMaxAngle() const { return max_angle; };
01063 
01065     bool IntensityOn() const { return GetVar(mDevice->intensity_on); };
01066 
01067 //    /// Scan data (polar): range (m) and bearing (radians)
01068 //    double GetScan(uint32_t aIndex) const
01069 //      { return GetVar(mDevice->scan[aIndex]); };
01070 
01072     player_point_2d_t GetPoint(uint32_t aIndex) const
01073       { return GetVar(mDevice->point[aIndex]); };
01074 
01075 
01077     double GetRange(uint32_t aIndex) const
01078       { return GetVar(mDevice->ranges[aIndex]); };
01079 
01081     double GetBearing(uint32_t aIndex) const
01082       { return GetVar(mDevice->scan[aIndex][1]); };
01083 
01084 
01086     int GetIntensity(uint32_t aIndex) const
01087       { return GetVar(mDevice->intensity[aIndex]); };
01088 
01090     int GetID() const
01091       { return GetVar(mDevice->laser_id); };
01092 
01093 
01102     void Configure(double aMinAngle,
01103                    double aMaxAngle,
01104                    uint32_t aScanRes,
01105                    uint32_t aRangeRes,
01106                    bool aIntensity,
01107                    double aScanningFrequency);
01108 
01111     void RequestConfigure();
01112     
01114     void RequestID();
01115 
01118     void RequestGeom();
01119 
01122     player_pose3d_t GetPose()
01123     {
01124       player_pose3d_t p;
01125       scoped_lock_t lock(mPc->mMutex);
01126 
01127       p.px = mDevice->pose[0];
01128       p.py = mDevice->pose[1];
01129       p.pyaw = mDevice->pose[2];
01130       return(p);
01131     }
01132 
01135     player_pose3d_t GetRobotPose()
01136     {
01137       player_pose3d_t p;
01138       scoped_lock_t lock(mPc->mMutex);
01139 
01140       p.px = mDevice->robot_pose[0];
01141       p.py = mDevice->robot_pose[1];
01142       p.pyaw = mDevice->robot_pose[2];
01143       return(p);
01144     }
01145 
01147     player_bbox3d_t GetSize()
01148     {
01149       player_bbox3d_t b;
01150       scoped_lock_t lock(mPc->mMutex);
01151 
01152       b.sl = mDevice->size[0];
01153       b.sw = mDevice->size[1];
01154       return(b);
01155     }
01156     
01158     double GetMinLeft() const
01159       { return GetVar(mDevice->min_left); };
01160     
01162     double GetMinRight() const
01163       { return GetVar(mDevice->min_right); };
01164            
01166     double MinLeft () const 
01167       { return GetMinLeft(); }
01168 
01170     double MinRight () const
01171       { return GetMinRight(); }
01172 
01177     double operator [] (uint32_t index) const
01178       { return GetRange(index);}
01179 
01180 };
01181 
01182 
01187 class LimbProxy : public ClientProxy
01188 {
01189   private:
01190 
01191     void Subscribe(uint32_t aIndex);
01192     void Unsubscribe();
01193 
01194    // libplayerc data structure
01195     playerc_limb_t *mDevice;
01196 
01197   public:
01198 
01199     LimbProxy(PlayerClient *aPc, uint32_t aIndex=0);
01200     ~LimbProxy();
01201 
01204     void RequestGeometry(void);
01205 
01207     void SetPowerConfig(bool aVal);
01209     void SetBrakesConfig(bool aVal);
01211     void SetSpeedConfig(float aSpeed);
01212 
01214     void MoveHome(void);
01216     void Stop(void);
01218     void SetPose(float aPX, float aPY, float aPZ,
01219                  float aAX, float aAY, float aAZ,
01220                  float aOX, float aOY, float aOZ);
01222     void SetPosition(float aX, float aY, float aZ);
01225     void VectorMove(float aX, float aY, float aZ, float aLength);
01226 
01228     player_limb_data_t GetData(void) const;
01230     player_limb_geom_req_t GetGeom(void) const;
01231 };
01232 
01233 
01234 
01240 class LocalizeProxy : public ClientProxy
01241 {
01242 
01243   private:
01244 
01245     void Subscribe(uint32_t aIndex);
01246     void Unsubscribe();
01247 
01248     // libplayerc data structure
01249     playerc_localize_t *mDevice;
01250 
01251   public:
01252 
01254     LocalizeProxy(PlayerClient *aPc, uint32_t aIndex=0);
01256     ~LocalizeProxy();
01257 
01259     // @todo should these be in a player_pose_t?
01260     uint32_t GetMapSizeX() const { return GetVar(mDevice->map_size_x); };
01261     uint32_t GetMapSizeY() const { return GetVar(mDevice->map_size_y); };
01262 
01263     // @todo should these be in a player_pose_t?
01264     uint32_t GetMapTileX() const { return GetVar(mDevice->map_tile_x); };
01265     uint32_t GetMapTileY() const { return GetVar(mDevice->map_tile_y); };
01266 
01268     double GetMapScale() const { return GetVar(mDevice->map_scale); };
01269 
01270     // Map data (empty = -1, unknown = 0, occupied = +1)
01271     // is this still needed?  if so,
01272     //void GetMapCells(uint8_t* aCells) const
01273     //{
01274     //  return GetVarByRef(mDevice->map_cells,
01275     //                     mDevice->image+GetVar(mDevice->??map_cell_cout??),
01276     //                     aCells);
01277     //};
01278 
01280     uint32_t GetPendingCount() const { return GetVar(mDevice->pending_count); };
01281 
01283     uint32_t GetHypothCount() const { return GetVar(mDevice->hypoth_count); };
01284 
01286     player_localize_hypoth_t GetHypoth(uint32_t aIndex) const
01287       { return GetVar(mDevice->hypoths[aIndex]); };
01288 
01290     int GetParticles()
01291       { return playerc_localize_get_particles(mDevice); }
01292 
01294     player_pose2d_t GetParticlePose(int index) const;
01295 
01297     void SetPose(double pose[3], double cov[3]);
01298 
01300     uint32_t GetNumHypoths() const { return GetVar(mDevice->hypoth_count); };
01301 
01304     uint32_t GetNumParticles() const { return GetVar(mDevice->num_particles); };
01305 };
01306 
01307 
01311 class LogProxy : public ClientProxy
01312 {
01313   private:
01314 
01315     void Subscribe(uint32_t aIndex);
01316     void Unsubscribe();
01317 
01318     // libplayerc data structure
01319     playerc_log_t *mDevice;
01320 
01321   public:
01323     LogProxy(PlayerClient *aPc, uint32_t aIndex=0);
01324 
01326     ~LogProxy();
01327 
01330     int GetType() const { return GetVar(mDevice->type); };
01331 
01333     int GetState() const { return GetVar(mDevice->state); };
01334 
01336     void QueryState();
01337 
01340     void SetState(int aState);
01341 
01343     void SetWriteState(int aState);
01344 
01346     void SetReadState(int aState);
01347 
01349     void Rewind();
01350 
01352     void SetFilename(const std::string aFilename);
01353 };
01354 
01358 class MapProxy : public ClientProxy
01359 {
01360   private:
01361 
01362     void Subscribe(uint32_t aIndex);
01363     void Unsubscribe();
01364 
01365     // libplayerc data structure
01366     playerc_map_t *mDevice;
01367 
01368   public:
01370     MapProxy(PlayerClient *aPc, uint32_t aIndex=0);
01371 
01373     ~MapProxy();
01374 
01376     void RequestMap();
01377 
01379     int GetCellIndex(int x, int y) const
01380     { return y*GetWidth() + x; };
01381 
01383     int8_t GetCell(int x, int y) const
01384     { return GetVar(mDevice->cells[GetCellIndex(x,y)]); };
01385 
01387     double GetResolution() const { return GetVar(mDevice->resolution); };
01388 
01391     uint32_t GetWidth() const { return GetVar(mDevice->width); };
01394     uint32_t GetHeight() const { return GetVar(mDevice->height); };
01395 
01396     double GetOriginX() const { return GetVar(mDevice->origin[0]); };
01397     double GetOriginY() const { return GetVar(mDevice->origin[1]); };
01398 
01400     void GetMap(int8_t* aMap) const
01401     {
01402       return GetVarByRef(reinterpret_cast<int8_t*>(mDevice->cells),
01403                          reinterpret_cast<int8_t*>(mDevice->cells+GetWidth()*GetHeight()),
01404                          aMap);
01405     };
01406 };
01407 
01413 class OpaqueProxy : public ClientProxy
01414 {
01415 
01416   private:
01417 
01418     void Subscribe(uint32_t aIndex);
01419     void Unsubscribe();
01420 
01421     // libplayerc data structure
01422     playerc_opaque_t *mDevice;
01423 
01424   public:
01425 
01427     OpaqueProxy(PlayerClient *aPc, uint32_t aIndex=0);
01429     ~OpaqueProxy();
01430 
01432     uint32_t GetCount() const { return GetVar(mDevice->data_count); };
01433 
01435     void GetData(uint8_t* aDest) const
01436       {
01437         return GetVarByRef(mDevice->data,
01438                            mDevice->data+GetVar(mDevice->data_count),
01439                            aDest);
01440       };
01441 
01443     void SendCmd(player_opaque_data_t* aData);
01444 
01446     int SendReq(player_opaque_data_t* aRequest);
01447 
01448 };
01449 
01453 class PlannerProxy : public ClientProxy
01454 {
01455 
01456   private:
01457 
01458     void Subscribe(uint32_t aIndex);
01459     void Unsubscribe();
01460 
01461     // libplayerc data structure
01462     playerc_planner_t *mDevice;
01463 
01464   public:
01465 
01467     PlannerProxy(PlayerClient *aPc, uint32_t aIndex=0);
01469     ~PlannerProxy();
01470 
01472     void SetGoalPose(double aGx, double aGy, double aGa);
01473 
01476     void RequestWaypoints();
01477 
01480     void SetEnable(bool aEnable);
01481 
01483     uint32_t GetPathValid() const { return GetVar(mDevice->path_valid); };
01484 
01486     uint32_t GetPathDone() const { return GetVar(mDevice->path_done); };
01487 
01490     double GetPx() const { return GetVar(mDevice->px); };
01493     double GetPy() const { return GetVar(mDevice->py); };
01496     double GetPa() const { return GetVar(mDevice->pa); };
01497 
01499     player_pose2d_t GetPose() const
01500     {
01501       player_pose2d_t p;
01502       scoped_lock_t lock(mPc->mMutex);
01503       p.px = mDevice->px;
01504       p.py = mDevice->py;
01505       p.pa = mDevice->pa;
01506       return(p);
01507     }
01508 
01511     double GetGx() const { return GetVar(mDevice->gx); };
01514     double GetGy() const { return GetVar(mDevice->gy); };
01517     double GetGa() const { return GetVar(mDevice->ga); };
01518 
01520     player_pose2d_t GetGoal() const
01521     {
01522       player_pose2d_t p;
01523       scoped_lock_t lock(mPc->mMutex);
01524       p.px = mDevice->gx;
01525       p.py = mDevice->gy;
01526       p.pa = mDevice->ga;
01527       return(p);
01528     }
01529 
01532     double GetWx() const { return GetVar(mDevice->wx); };
01535     double GetWy() const { return GetVar(mDevice->wy); };
01538     double GetWa() const { return GetVar(mDevice->wa); };
01539 
01541     player_pose2d_t GetCurrentWaypoint() const
01542     {
01543       player_pose2d_t p;
01544       scoped_lock_t lock(mPc->mMutex);
01545       p.px = mDevice->wx;
01546       p.py = mDevice->wy;
01547       p.pa = mDevice->wa;
01548       return(p);
01549     }
01550 
01553     double GetIx(int i) const;
01556     double GetIy(int i) const;
01559     double GetIa(int i) const;
01560 
01562     player_pose2d_t GetWaypoint(uint32_t aIndex) const
01563     {
01564       assert(aIndex < GetWaypointCount());
01565       player_pose2d_t p;
01566       scoped_lock_t lock(mPc->mMutex);
01567       p.px = mDevice->waypoints[aIndex][0];
01568       p.py = mDevice->waypoints[aIndex][1];
01569       p.pa = mDevice->waypoints[aIndex][2];
01570       return(p);
01571     }
01572 
01576     int GetCurrentWaypointId() const
01577       { return GetVar(mDevice->curr_waypoint); };
01578 
01580     uint32_t GetWaypointCount() const
01581       { return GetVar(mDevice->waypoint_count); };
01582 
01587     player_pose2d_t operator [](uint32_t aIndex) const
01588       { return GetWaypoint(aIndex); }
01589 
01590 };
01591 
01595 class Pointcloud3dProxy : public ClientProxy
01596 {
01597   private:
01598 
01599     void Subscribe(uint32_t aIndex);
01600     void Unsubscribe();
01601 
01602     // libplayerc data structure
01603     playerc_pointcloud3d_t *mDevice;
01604 
01605   public:
01607     Pointcloud3dProxy(PlayerClient *aPc, uint32_t aIndex=0);
01608 
01610     ~Pointcloud3dProxy();
01611 
01613     uint32_t GetCount() const { return GetVar(mDevice->points_count); };
01614 
01616     player_pointcloud3d_element_t GetPoint(uint32_t aIndex) const
01617       { return GetVar(mDevice->points[aIndex]); };
01618 
01621     player_pointcloud3d_element_t operator [] (uint32_t aIndex) const { return GetPoint(aIndex); }
01622 
01623 };
01624 
01625 
01630 class Position1dProxy : public ClientProxy
01631 {
01632 
01633   private:
01634 
01635     void Subscribe(uint32_t aIndex);
01636     void Unsubscribe();
01637 
01638     // libplayerc data structure
01639     playerc_position1d_t *mDevice;
01640 
01641   public:
01642 
01644     Position1dProxy(PlayerClient *aPc, uint32_t aIndex=0);
01646     ~Position1dProxy();
01647 
01651     void SetSpeed(double aVel);
01652 
01656     void GoTo(double aPos, double aVel);
01657 
01660     void RequestGeom();
01661 
01663     player_pose3d_t GetPose() const
01664     {
01665       player_pose3d_t p;
01666       scoped_lock_t lock(mPc->mMutex);
01667       p.px = mDevice->pose[0];
01668       p.py = mDevice->pose[1];
01669       p.pyaw = mDevice->pose[2];
01670       return(p);
01671     }
01672 
01674     player_bbox3d_t GetSize() const
01675     {
01676       player_bbox3d_t b;
01677       scoped_lock_t lock(mPc->mMutex);
01678       b.sl = mDevice->size[0];
01679       b.sw = mDevice->size[1];
01680       return(b);
01681     }
01682 
01687     void SetMotorEnable(bool enable);
01688 
01691     void SetOdometry(double aPos);
01692 
01694     void ResetOdometry() { SetOdometry(0); };
01695 
01697     //void SetSpeedPID(double kp, double ki, double kd);
01698 
01700     //void SetPositionPID(double kp, double ki, double kd);
01701 
01704     //void SetPositionSpeedProfile(double spd, double acc);
01705 
01707     double  GetPos() const { return GetVar(mDevice->pos); };
01708 
01710     double  GetVel() const { return GetVar(mDevice->vel); };
01711 
01713     bool GetStall() const { return GetVar(mDevice->stall); };
01714 
01716     uint8_t GetStatus() const { return GetVar(mDevice->status); };
01717 
01719     bool IsLimitMin() const
01720       { return (GetVar(mDevice->status) &
01721                (1 << PLAYER_POSITION1D_STATUS_LIMIT_MIN)) > 0; };
01722 
01724     bool IsLimitCen() const
01725       { return (GetVar(mDevice->status) &
01726                (1 << PLAYER_POSITION1D_STATUS_LIMIT_CEN)) > 0; };
01727 
01729     bool IsLimitMax() const
01730       { return (GetVar(mDevice->status) &
01731                (1 << PLAYER_POSITION1D_STATUS_LIMIT_MAX)) > 0; };
01732 
01734     bool IsOverCurrent() const
01735       { return (GetVar(mDevice->status) &
01736                (1 << PLAYER_POSITION1D_STATUS_OC)) > 0; };
01737 
01739     bool IsTrajComplete() const
01740       { return (GetVar(mDevice->status) &
01741                (1 << PLAYER_POSITION1D_STATUS_TRAJ_COMPLETE)) > 0; };
01742 
01744     bool IsEnabled() const
01745       { return (GetVar(mDevice->status) &
01746                (1 << PLAYER_POSITION1D_STATUS_ENABLED)) > 0; };
01747 
01748 };
01749 
01754 class Position2dProxy : public ClientProxy
01755 {
01756 
01757   private:
01758 
01759     void Subscribe(uint32_t aIndex);
01760     void Unsubscribe();
01761 
01762     // libplayerc data structure
01763     playerc_position2d_t *mDevice;
01764 
01765   public:
01766 
01768     Position2dProxy(PlayerClient *aPc, uint32_t aIndex=0);
01770     ~Position2dProxy();
01771 
01775     void SetSpeed(double aXSpeed, double aYSpeed, double aYawSpeed);
01776 
01779     void SetSpeed(double aXSpeed, double aYawSpeed)
01780         { return SetSpeed(aXSpeed, 0, aYawSpeed);}
01781 
01783     void SetSpeed(player_pose2d_t vel)
01784         { return SetSpeed(vel.px, vel.py, vel.pa);}
01785 
01789     void GoTo(player_pose2d_t pos, player_pose2d_t vel);
01790 
01792     void GoTo(player_pose2d_t pos)
01793       {GoTo(pos,(player_pose2d_t) {0,0,0}); }
01794 
01797     void GoTo(double aX, double aY, double aYaw)
01798       {GoTo((player_pose2d_t) {aX,aY,aYaw},(player_pose2d_t) {0,0,0}); }
01799 
01801     void SetCarlike(double aXSpeed, double aDriveAngle);
01802 
01805     void RequestGeom();
01806 
01808     //  body (fill it in by calling RequestGeom()).
01809     player_pose3d_t GetOffset()
01810     {
01811       player_pose3d_t p;
01812       scoped_lock_t lock(mPc->mMutex);
01813       p.px = mDevice->pose[0];
01814       p.py = mDevice->pose[1];
01815       p.pyaw = mDevice->pose[2];
01816       return(p);
01817     }
01818 
01820     player_bbox3d_t GetSize()
01821     {
01822       player_bbox3d_t b;
01823       scoped_lock_t lock(mPc->mMutex);
01824       b.sl = mDevice->size[0];
01825       b.sw = mDevice->size[1];
01826       return(b);
01827     }
01828 
01833     void SetMotorEnable(bool enable);
01834 
01835     // Select velocity control mode.
01836     //
01837     // For the the p2os_position driver, set @p mode to 0 for direct wheel
01838     // velocity control (default), or 1 for separate translational and
01839     // rotational control.
01840     //
01841     // For the reb_position driver: 0 is direct velocity control, 1 is for
01842     // velocity-based heading PD controller (uses DoDesiredHeading()).
01843     //void SelectVelocityControl(unsigned char mode);
01844 
01846     void ResetOdometry();
01847 
01850     //void SelectPositionMode(unsigned char mode);
01851 
01854     void SetOdometry(double aX, double aY, double aYaw);
01855 
01857     //void SetSpeedPID(double kp, double ki, double kd);
01858 
01860     //void SetPositionPID(double kp, double ki, double kd);
01861 
01864     //void SetPositionSpeedProfile(double spd, double acc);
01865 
01866     //
01867     // void DoStraightLine(double m);
01868 
01869     //
01870     //void DoRotation(double yawspeed);
01871 
01872     //
01873     //void DoDesiredHeading(double yaw, double xspeed, double yawspeed);
01874 
01875     //
01876     //void SetStatus(uint8_t cmd, uint16_t value);
01877 
01878     //
01879     //void PlatformShutdown();
01880 
01882     double  GetXPos() const { return GetVar(mDevice->px); };
01883 
01885     double  GetYPos() const { return GetVar(mDevice->py); };
01886 
01888     double GetYaw() const { return GetVar(mDevice->pa); };
01889 
01891     double  GetXSpeed() const { return GetVar(mDevice->vx); };
01892 
01894     double  GetYSpeed() const { return GetVar(mDevice->vy); };
01895 
01897     double  GetYawSpeed() const { return GetVar(mDevice->va); };
01898 
01900     bool GetStall() const { return GetVar(mDevice->stall); };
01901 
01902 };
01903 
01910 class Position3dProxy : public ClientProxy
01911 {
01912 
01913   private:
01914 
01915     void Subscribe(uint32_t aIndex);
01916     void Unsubscribe();
01917 
01918     // libplayerc data structure
01919     playerc_position3d_t *mDevice;
01920 
01921   public:
01922 
01924     Position3dProxy(PlayerClient *aPc, uint32_t aIndex=0);
01926     ~Position3dProxy();
01927 
01931     void SetSpeed(double aXSpeed, double aYSpeed, double aZSpeed,
01932                   double aRollSpeed, double aPitchSpeed, double aYawSpeed);
01933 
01937     void SetSpeed(double aXSpeed, double aYSpeed,
01938                   double aZSpeed, double aYawSpeed)
01939       { SetSpeed(aXSpeed,aYSpeed,aZSpeed,0,0,aYawSpeed); }
01940 
01942     void SetSpeed(double aXSpeed, double aYSpeed, double aYawSpeed)
01943       { SetSpeed(aXSpeed, aYSpeed, 0, 0, 0, aYawSpeed); }
01944 
01947     void SetSpeed(double aXSpeed, double aYawSpeed)
01948       { SetSpeed(aXSpeed,0,0,0,0,aYawSpeed);}
01949 
01951     void SetSpeed(player_pose3d_t vel)
01952       { SetSpeed(vel.px,vel.py,vel.pz,vel.proll,vel.ppitch,vel.pyaw);}
01953 
01957     void GoTo(player_pose3d_t aPos, player_pose3d_t aVel);
01958 
01960     void GoTo(player_pose3d_t aPos)
01961       { GoTo(aPos, (player_pose3d_t) {0,0,0,0,0,0}); }
01962 
01963 
01966     void GoTo(double aX, double aY, double aZ,
01967               double aRoll, double aPitch, double aYaw)
01968       { GoTo((player_pose3d_t) {aX,aY,aZ,aRoll,aPitch,aYaw},
01969               (player_pose3d_t) {0,0,0,0,0,0});
01970       }
01971 
01976     void SetMotorEnable(bool aEnable);
01977 
01980     void SelectVelocityControl(int aMode);
01981 
01983     void ResetOdometry();
01984 
01988     void SetOdometry(double aX, double aY, double aZ,
01989                      double aRoll, double aPitch, double aYaw);
01990 
01993     void RequestGeom();
01994 
01995     // Select position mode
01996     // Set @p mode for 0 for velocity mode, 1 for position mode.
01997     //void SelectPositionMode(unsigned char mode);
01998 
01999     //
02000     //void SetSpeedPID(double kp, double ki, double kd);
02001 
02002     //
02003     //void SetPositionPID(double kp, double ki, double kd);
02004 
02005     // Sets the ramp profile for position based control
02006     // spd rad/s, acc rad/s/s
02007     //void SetPositionSpeedProfile(double spd, double acc);
02008 
02010     double  GetXPos() const { return GetVar(mDevice->pos_x); };
02011 
02013     double  GetYPos() const { return GetVar(mDevice->pos_y); };
02014 
02016     double  GetZPos() const { return GetVar(mDevice->pos_z); };
02017 
02019     double  GetRoll() const { return GetVar(mDevice->pos_roll); };
02020 
02022     double  GetPitch() const { return GetVar(mDevice->pos_pitch); };
02023 
02025     double  GetYaw() const { return GetVar(mDevice->pos_yaw); };
02026 
02028     double  GetXSpeed() const { return GetVar(mDevice->vel_x); };
02029 
02031     double  GetYSpeed() const { return GetVar(mDevice->vel_y); };
02032 
02034     double  GetZSpeed() const { return GetVar(mDevice->vel_z); };
02035 
02037     double  GetRollSpeed() const { return GetVar(mDevice->vel_roll); };
02038 
02040     double  GetPitchSpeed() const { return GetVar(mDevice->vel_pitch); };
02041 
02043     double  GetYawSpeed() const { return GetVar(mDevice->vel_yaw); };
02044 
02046     bool GetStall () const { return GetVar(mDevice->stall); };
02047 };
02050 class PowerProxy : public ClientProxy
02051 {
02052   private:
02053 
02054     void Subscribe(uint32_t aIndex);
02055     void Unsubscribe();
02056 
02057     // libplayerc data structure
02058     playerc_power_t *mDevice;
02059 
02060   public:
02062     PowerProxy(PlayerClient *aPc, uint32_t aIndex=0);
02064     ~PowerProxy();
02065 
02067     double GetCharge() const { return GetVar(mDevice->charge); };
02068 
02070     double GetPercent() const {return GetVar(mDevice->percent); };
02071 
02073     double GetJoules() const {return GetVar(mDevice->joules); };
02074 
02076     double GetWatts() const {return GetVar(mDevice->watts); };
02077 
02079     bool GetCharging() const {return GetVar(mDevice->charging);};
02080 
02081     // Return whether the power data is valid
02082     bool IsValid() const {return GetVar(mDevice->valid);};
02083 };
02084 
02091 class PtzProxy : public ClientProxy
02092 {
02093 
02094   private:
02095 
02096     void Subscribe(uint32_t aIndex);
02097     void Unsubscribe();
02098 
02099     // libplayerc data structure
02100     playerc_ptz_t *mDevice;
02101 
02102   public:
02104     PtzProxy(PlayerClient *aPc, uint32_t aIndex=0);
02105     // destructor
02106     ~PtzProxy();
02107 
02108   public:
02109 
02113     void SetCam(double aPan, double aTilt, double aZoom);
02114 
02116     void SetSpeed(double aPanSpeed=0, double aTiltSpeed=0, double aZoomSpeed=0);
02117 
02120     void SelectControlMode(uint32_t aMode);
02121 
02123     double GetPan() const { return GetVar(mDevice->pan); };
02125     double GetTilt() const { return GetVar(mDevice->tilt); };
02127     double GetZoom() const { return GetVar(mDevice->zoom); };
02128 
02130     int GetStatus();
02131 
02132 
02133 };
02134 
02137 class RangerProxy : public ClientProxy
02138 {
02139   private:
02140 
02141     void Subscribe(uint32_t aIndex);
02142     void Unsubscribe();
02143 
02144     // libplayerc data structure
02145     playerc_ranger_t *mDevice;
02146 
02147   public:
02149     RangerProxy(PlayerClient *aPc, uint32_t aIndex=0);
02151     ~RangerProxy();
02152 
02154     uint32_t GetSensorCount() const { return GetVar(mDevice->sensor_count); };
02155 
02157     player_pose3d_t GetDevicePose() const { return GetVar(mDevice->device_pose); };
02159     player_bbox3d_t GetDeviceSize() const { return GetVar(mDevice->device_size); };
02160 
02162     player_pose3d_t GetSensorPose(uint32_t aIndex) const;
02164     player_bbox3d_t GetSensorSize(uint32_t aIndex) const;
02165 
02167     uint32_t GetRangeCount() const { return GetVar(mDevice->ranges_count); };
02169     double GetRange(uint32_t aIndex) const;
02171     double operator[] (uint32_t aIndex) const { return GetRange(aIndex); }
02172 
02174     uint32_t GetIntensityCount() const { return GetVar(mDevice->intensities_count); } ;
02176     double GetIntensity(uint32_t aIndex) const;
02177 
02180     void SetPower(bool aEnable);
02181 
02184     void SetIntensityData(bool aEnable);
02185 
02187     void RequestGeom();
02188 
02193     void Configure(double aMinAngle,
02194                    double aMaxAngle,
02195                    double aResolution,
02196                    double aMaxRange,
02197                    double aRangeRes,
02198                    double aFrequency);
02199 
02202     void RequestConfigure();
02203 
02205     double GetMinAngle() const { return GetVar(mDevice->min_angle); };
02206 
02208     double GetMaxAngle() const { return GetVar(mDevice->max_angle); };
02209 
02211     double GetResolution() const { return GetVar(mDevice->resolution); };
02212 
02214     double GetMaxRange() const { return GetVar(mDevice->max_range); };
02215 
02217     double GetRangeRes() const { return GetVar(mDevice->range_res); };
02218 
02220     double GetFrequency() const { return GetVar(mDevice->frequency); };
02221 };
02222 
02225 class RFIDProxy : public ClientProxy
02226 {
02227 
02228   private:
02229 
02230     void Subscribe(uint32_t aIndex);
02231     void Unsubscribe();
02232 
02233     // libplayerc data structure
02234     playerc_rfid_t *mDevice;
02235 
02236   public:
02238     RFIDProxy(PlayerClient *aPc, uint32_t aIndex=0);
02240     ~RFIDProxy();
02241 
02243     uint32_t GetTagsCount() const { return GetVar(mDevice->tags_count); };
02245     playerc_rfidtag_t GetRFIDTag(uint32_t aIndex) const
02246       { return GetVar(mDevice->tags[aIndex]);};
02247 
02252     playerc_rfidtag_t operator [](uint32_t aIndex) const
02253       { return(GetRFIDTag(aIndex)); }
02254 };
02255 
02260 class SimulationProxy : public ClientProxy
02261 {
02262   private:
02263 
02264     void Subscribe(uint32_t aIndex);
02265     void Unsubscribe();
02266 
02267     // libplayerc data structure
02268     playerc_simulation_t *mDevice;
02269 
02270   public:
02272     SimulationProxy(PlayerClient *aPc, uint32_t aIndex=0);
02274     ~SimulationProxy();
02275 
02278     void SetPose2d(char* identifier, double x, double y, double a);
02279 
02282     void GetPose2d(char* identifier, double& x, double& y, double& a);
02283 
02286     void SetPose3d(char* identifier, double x, double y, double z,
02287                    double roll, double pitch, double yaw);
02288 
02291     void GetPose3d(char* identifier, double& x, double& y, double& z,
02292                    double& roll, double& pitch, double& yaw, double& time);
02293 
02295     void GetProperty(char* identifier, char *name, void *value, size_t value_len );
02296 
02298     void SetProperty(char* identifier, char *name, void *value, size_t value_len );
02299 };
02300 
02301 
02307 class SonarProxy : public ClientProxy
02308 {
02309   private:
02310 
02311     void Subscribe(uint32_t aIndex);
02312     void Unsubscribe();
02313 
02314     // libplayerc data structure
02315     playerc_sonar_t *mDevice;
02316 
02317   public:
02319     SonarProxy(PlayerClient *aPc, uint32_t aIndex=0);
02321     ~SonarProxy();
02322 
02324     uint32_t GetCount() const { return GetVar(mDevice->scan_count); };
02325 
02327     double GetScan(uint32_t aIndex) const
02328       { if (GetVar(mDevice->scan_count) <= (int32_t)aIndex) return -1.0; return GetVar(mDevice->scan[aIndex]); };
02331     double operator [] (uint32_t aIndex) const { return GetScan(aIndex); }
02332 
02334     uint32_t GetPoseCount() const { return GetVar(mDevice->pose_count); };
02335 
02337     player_pose3d_t GetPose(uint32_t aIndex) const
02338       { return GetVar(mDevice->poses[aIndex]); };
02339 
02340     // Enable/disable the sonars.
02341     // Set @p state to 1 to enable, 0 to disable.
02342     // Note that when sonars are disabled the client will still receive sonar
02343     // data, but the ranges will always be the last value read from the sonars
02344     // before they were disabled.
02345     //void SetEnable(bool aEnable);
02346 
02348     void RequestGeom();
02349 };
02350 
02355 class SpeechProxy : public ClientProxy
02356 {
02357 
02358   private:
02359 
02360     void Subscribe(uint32_t aIndex);
02361     void Unsubscribe();
02362 
02363     // libplayerc data structure
02364     playerc_speech_t *mDevice;
02365 
02366   public:
02368     SpeechProxy(PlayerClient *aPc, uint32_t aIndex=0);
02370     ~SpeechProxy();
02371 
02374     void Say(std::string aStr);
02375 };
02376 
02380 class SpeechRecognitionProxy : public ClientProxy
02381 {
02382    void Subscribe(uint32_t aIndex);
02383    void Unsubscribe();
02384 
02386    playerc_speechrecognition_t *mDevice;
02387   public:
02389    SpeechRecognitionProxy(PlayerClient *aPc, uint32_t aIndex=0);
02390    ~SpeechRecognitionProxy();
02392    std::string GetWord(uint32_t aWord) const{
02393      scoped_lock_t lock(mPc->mMutex);
02394      return std::string(mDevice->words[aWord]);
02395    }
02396 
02398    uint32_t GetCount(void) const { return GetVar(mDevice->wordCount); }
02399 
02402    std::string operator [](uint32_t aWord) { return(GetWord(aWord)); }
02403 };
02404 
02408 class VectorMapProxy : public ClientProxy
02409 {
02410 
02411   private:
02412 
02413     // Subscribe
02414     void Subscribe(uint32_t aIndex);
02415     // Unsubscribe
02416     void Unsubscribe();
02417 
02418     // libplayerc data structure
02419     playerc_vectormap_t *mDevice;
02420 
02421     bool map_info_cached;
02422   public:
02423     // Constructor
02424     VectorMapProxy(PlayerClient *aPc, uint32_t aIndex=0);
02425     // Destructor
02426     ~VectorMapProxy();
02427 
02428     void GetMapInfo();
02429     void GetLayerData(unsigned layer_index);
02430 
02431     int GetLayerCount() const;
02432     std::vector<std::string> GetLayerNames() const;
02433     int GetFeatureCount(unsigned layer_index) const;
02434     GEOSGeom GetFeatureData(unsigned layer_index, unsigned feature_index) const;
02435 };
02436 
02439 class WiFiProxy: public ClientProxy
02440 {
02441 
02442   private:
02443 
02444     void Subscribe(uint32_t aIndex);
02445     void Unsubscribe();
02446 
02447     // libplayerc data structure
02448     playerc_wifi_t *mDevice;
02449 
02450   public:
02452     WiFiProxy(PlayerClient *aPc, uint32_t aIndex=0);
02454     ~WiFiProxy();
02455 
02456     const playerc_wifi_link_t *GetLink(int aLink);
02457 
02458                          int GetLinkCount() const { return mDevice->link_count; };
02459                          char* GetOwnIP() const { return mDevice->ip; };
02460 
02461                          char* GetLinkIP(int index)  const { return (char*) mDevice->links[index].ip; };
02462                          char* GetLinkMAC(int index)  const { return (char*) mDevice->links[index].mac; };
02463                          char* GetLinkESSID(int index)  const { return (char*)mDevice->links[index].essid; };
02464                          double GetLinkFreq(int index)  const {return mDevice->links[index].freq;};
02465                          int     GetLinkMode(int index)  const { return mDevice->links[index].mode; };
02466                          int     GetLinkEncrypt(int index)  const {return mDevice->links[index].encrypt; };
02467                          int   GetLinkQuality(int index)  const { return mDevice->links[index].qual; };
02468                          int     GetLinkLevel(int index)  const {return mDevice->links[index].level; };
02469                          int     GetLinkNoise(int index)  const {return mDevice->links[index].noise; }  ;                
02470 
02471                         //player_wifi_link_t
02472 //     int GetLinkQuality(char/// ip = NULL);
02473 //     int GetLevel(char/// ip = NULL);
02474 //     int GetLeveldBm(char/// ip = NULL) { return GetLevel(ip) - 0x100; }
02475 //     int GetNoise(char/// ip = NULL);
02476 //     int GetNoisedBm(char/// ip = NULL) { return GetNoise(ip) - 0x100; }
02477 //
02478 //     uint16_t GetMaxLinkQuality() { return maxqual; }
02479 //     uint8_t GetMode() { return op_mode; }
02480 //
02481 //     int GetBitrate();
02482 //
02483 //     char/// GetMAC(char *buf, int len);
02484 //
02485 //     char/// GetIP(char *buf, int len);
02486 //     char/// GetAP(char *buf, int len);
02487 //
02488 //     int AddSpyHost(char *address);
02489 //     int RemoveSpyHost(char *address);
02490 //
02491 //   private:
02492 //     int GetLinkIndex(char *ip);
02493 //
02494 //     /// The current wifi data.
02495 //     int link_count;
02496 //     player_wifi_link_t links[PLAYER_WIFI_MAX_LINKS];
02497 //     uint32_t throughput;
02498 //     uint8_t op_mode;
02499 //     int32_t bitrate;
02500 //     uint16_t qual_type, maxqual, maxlevel, maxnoise;
02501 //
02502 //     char access_point[32];
02503 };
02504 
02507 class WSNProxy : public ClientProxy
02508 {
02509 
02510   private:
02511 
02512     void Subscribe(uint32_t aIndex);
02513     void Unsubscribe();
02514 
02515     // libplayerc data structure
02516     playerc_wsn_t *mDevice;
02517 
02518   public:
02520     WSNProxy(PlayerClient *aPc, uint32_t aIndex=0);
02522     ~WSNProxy();
02523 
02524     uint32_t GetNodeType    () const { return GetVar(mDevice->node_type);      };
02525     uint32_t GetNodeID      () const { return GetVar(mDevice->node_id);        };
02526     uint32_t GetNodeParentID() const { return GetVar(mDevice->node_parent_id); };
02527 
02528     player_wsn_node_data_t
02529        GetNodeDataPacket() const { return GetVar(mDevice->data_packet);    };
02530 
02531     void SetDevState(int nodeID, int groupID, int devNr, int value);
02532     void Power(int nodeID, int groupID, int value);
02533     void DataType(int value);
02534     void DataFreq(int nodeID, int groupID, float frequency);
02535 };
02536 
02538 }
02539 
02540 namespace std
02541 {
02542   std::ostream& operator << (std::ostream& os, const player_point_2d_t& c);
02543   std::ostream& operator << (std::ostream& os, const player_pose2d_t& c);
02544   std::ostream& operator << (std::ostream& os, const player_pose3d_t& c);
02545   std::ostream& operator << (std::ostream& os, const player_bbox2d_t& c);
02546   std::ostream& operator << (std::ostream& os, const player_bbox3d_t& c);
02547   std::ostream& operator << (std::ostream& os, const player_segment_t& c);
02548   std::ostream& operator << (std::ostream& os, const player_extent2d_t& c);
02549   std::ostream& operator << (std::ostream& os, const playerc_device_info_t& c);
02550 
02551   std::ostream& operator << (std::ostream& os, const PlayerCc::ClientProxy& c);
02552   std::ostream& operator << (std::ostream& os, const PlayerCc::ActArrayProxy& c);
02553   std::ostream& operator << (std::ostream& os, const PlayerCc::AioProxy& c);
02554   std::ostream& operator << (std::ostream& os, const PlayerCc::AudioProxy& a);
02555   //std::ostream& operator << (std::ostream& os, const PlayerCc::BlinkenLightProxy& c);
02556   std::ostream& operator << (std::ostream& os, const PlayerCc::BlobfinderProxy& c);
02557   std::ostream& operator << (std::ostream& os, const PlayerCc::BumperProxy& c);
02558   std::ostream& operator << (std::ostream& os, const PlayerCc::CameraProxy& c);
02559   std::ostream& operator << (std::ostream& os, const PlayerCc::DioProxy& c);
02560   std::ostream& operator << (std::ostream& os, const PlayerCc::FiducialProxy& c);
02561   std::ostream& operator << (std::ostream& os, const PlayerCc::GpsProxy& c);
02562   std::ostream& operator << (std::ostream& os, const PlayerCc::GripperProxy& c);
02563   std::ostream& operator << (std::ostream& os, const PlayerCc::ImuProxy& c);
02564   std::ostream& operator << (std::ostream& os, const PlayerCc::IrProxy& c);
02565   std::ostream& operator << (std::ostream& os, const PlayerCc::LaserProxy& c);
02566   std::ostream& operator << (std::ostream& os, const PlayerCc::LimbProxy& c);
02567   std::ostream& operator << (std::ostream& os, const PlayerCc::LocalizeProxy& c);
02568   std::ostream& operator << (std::ostream& os, const PlayerCc::LogProxy& c);
02569   std::ostream& operator << (std::ostream& os, const PlayerCc::MapProxy& c);
02570   std::ostream& operator << (std::ostream& os, const PlayerCc::OpaqueProxy& c);
02571   std::ostream& operator << (std::ostream& os, const PlayerCc::PlannerProxy& c);
02572   std::ostream& operator << (std::ostream& os, const PlayerCc::Position1dProxy& c);
02573   std::ostream& operator << (std::ostream& os, const PlayerCc::Position2dProxy& c);
02574   std::ostream& operator << (std::ostream& os, const PlayerCc::Position3dProxy& c);
02575   std::ostream& operator << (std::ostream& os, const PlayerCc::PowerProxy& c);
02576   std::ostream& operator << (std::ostream& os, const PlayerCc::PtzProxy& c);
02577   std::ostream& operator << (std::ostream &os, const PlayerCc::RangerProxy &c);
02578   std::ostream& operator << (std::ostream& os, const PlayerCc::SimulationProxy& c);
02579   std::ostream& operator << (std::ostream& os, const PlayerCc::SonarProxy& c);
02580   std::ostream& operator << (std::ostream& os, const PlayerCc::SpeechProxy& c);
02581   std::ostream& operator << (std::ostream& os, const PlayerCc::SpeechRecognitionProxy& c);
02582   std::ostream& operator << (std::ostream& os, const PlayerCc::VectorMapProxy& c);
02583   //std::ostream& operator << (std::ostream& os, const PlayerCc::WafeformProxy& c);
02584   std::ostream& operator << (std::ostream& os, const PlayerCc::WiFiProxy& c);
02585   std::ostream& operator << (std::ostream& os, const PlayerCc::RFIDProxy& c);
02586   std::ostream& operator << (std::ostream& os, const PlayerCc::WSNProxy& c);
02587 }
02588 
02589 #endif
02590 

Last updated 12 September 2005 21:38:45