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 6371 2008-04-23 03:44:00Z 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 
00057 #include "libplayerc/playerc.h"
00058 #include "libplayerc++/utility.h"
00059 #include "libplayerc++/playerc++config.h"
00060 #include "libplayerc++/playerclient.h"
00061 #include "libplayerc++/playererror.h"
00062 #include "libplayerc++/clientproxy.h"
00063 #include "libplayercore/interface_util.h"
00064 
00065 // Don't think we need to include these here
00066 /*
00067 #ifdef HAVE_BOOST_SIGNALS
00068   #include <boost/signal.hpp>
00069   #include <boost/bind.hpp>
00070 #endif
00071 
00072 #ifdef HAVE_BOOST_THREAD
00073   #include <boost/thread/mutex.hpp>
00074   #include <boost/thread/thread.hpp>
00075   #include <boost/thread/xtime.hpp>
00076 #endif
00077 */
00078 
00079 namespace PlayerCc
00080 {
00081 
00082 // /**
00083 // * The @p SomethingProxy class is a template for adding new subclasses of
00084 // * ClientProxy.  You need to have at least all of the following:
00085 // */
00086 // class SomethingProxy : public ClientProxy
00087 // {
00088 //
00089 //   private:
00090 //
00091 //     // Subscribe
00092 //     void Subscribe(uint32_t aIndex);
00093 //     // Unsubscribe
00094 //     void Unsubscribe();
00095 //
00096 //     // libplayerc data structure
00097 //     playerc_something_t *mDevice;
00098 //
00099 //   public:
00100 //     // Constructor
00101 //     SomethingProxy(PlayerClient *aPc, uint32_t aIndex=0);
00102 //     // Destructor
00103 //     ~SomethingProxy();
00104 //
00105 // };
00106 
00118 // ==============================================================
00119 //
00120 // These are alphabetized, please keep them that way!!!
00121 //
00122 // ==============================================================
00123 
00128 class ActArrayProxy : public ClientProxy
00129 {
00130   private:
00131 
00132    void Subscribe(uint32_t aIndex);
00133    void Unsubscribe();
00134 
00135    // libplayerc data structure
00136    playerc_actarray_t *mDevice;
00137 
00138   public:
00139 
00141     ActArrayProxy(PlayerClient *aPc, uint32_t aIndex=0);
00143     ~ActArrayProxy();
00144 
00147     void RequestGeometry(void);
00148 
00150     void SetPowerConfig(bool aVal);
00152     void SetBrakesConfig(bool aVal);
00154     void SetSpeedConfig(uint32_t aJoint, float aSpeed);
00155 
00157     void MoveTo(uint32_t aJoint, float aPos);
00159     void MoveToMulti(std::vector<float> aPos);
00161     void MoveAtSpeed(uint32_t aJoint, float aSpeed);
00163     void MoveAtSpeedMulti(std::vector<float> aSpeed);
00165     void MoveHome(int aJoint);
00167     void SetActuatorCurrent(uint32_t aJoint, float aCurrent);
00169     void SetActuatorCurrentMulti(std::vector<float> aCurrent);
00170 
00172     uint32_t GetCount(void) const { return GetVar(mDevice->actuators_count); }
00174     player_actarray_actuator_t GetActuatorData(uint32_t aJoint) const;
00176     player_actarray_actuatorgeom_t GetActuatorGeom(uint32_t aJoint) const;
00178     player_point_3d_t GetBasePos(void) const { return GetVar(mDevice->base_pos); }
00180     player_orientation_3d_t GetBaseOrientation(void) const { return GetVar(mDevice->base_orientation); }
00181 
00182 
00187     player_actarray_actuator_t operator [](uint32_t aJoint)
00188       { return(GetActuatorData(aJoint)); }
00189 };
00190 
00194 class AioProxy : public ClientProxy
00195 {
00196   private:
00197 
00198     void Subscribe(uint32_t aIndex);
00199     void Unsubscribe();
00200 
00201     // libplayerc data structure
00202     playerc_aio_t *mDevice;
00203 
00204   public:
00205 
00206     AioProxy (PlayerClient *aPc, uint32_t aIndex=0);
00207     ~AioProxy();
00208 
00210     uint32_t GetCount() const { return(GetVar(mDevice->voltages_count)); };
00211 
00213     double GetVoltage(uint32_t aIndex)  const
00214       { return(GetVar(mDevice->voltages[aIndex])); };
00215 
00217     void SetVoltage(uint32_t aIndex, double aVoltage);
00218 
00223     double operator [](uint32_t aIndex) const
00224       { return GetVoltage(aIndex); }
00225 
00226 };
00227 
00228 
00232 class AudioProxy : public ClientProxy
00233 {
00234 
00235   private:
00236 
00237     void Subscribe(uint32_t aIndex);
00238     void Unsubscribe();
00239 
00240     // libplayerc data structure
00241     playerc_audio_t *mDevice;
00242 
00243   public:
00244 
00245     AudioProxy(PlayerClient *aPc, uint32_t aIndex=0);
00246     ~AudioProxy();
00247 
00249     uint32_t GetMixerDetailsCount() const {return(GetVar(mDevice->channel_details_list.details_count));};
00251     player_audio_mixer_channel_detail_t GetMixerDetails(int aIndex) const  {return(GetVar(mDevice->channel_details_list.details[aIndex]));};
00253     uint32_t GetDefaultOutputChannel() const  {return(GetVar(mDevice->channel_details_list.default_output));};
00255     uint32_t GetDefaultInputChannel() const {return(GetVar(mDevice->channel_details_list.default_input));};
00256 
00258     uint32_t GetWavDataLength() const {return(GetVar(mDevice->wav_data.data_count));};
00263     void GetWavData(uint8_t* aData) const
00264       {
00265         return GetVarByRef(mDevice->wav_data.data,
00266                            mDevice->wav_data.data+GetWavDataLength(),
00267                            aData);
00268       };
00269 
00271     uint32_t GetSeqCount() const {return(GetVar(mDevice->seq_data.tones_count));};
00273     player_audio_seq_item_t GetSeqItem(int aIndex) const  {return(GetVar(mDevice->seq_data.tones[aIndex]));};
00274 
00276     uint32_t GetChannelCount() const {return(GetVar(mDevice->mixer_data.channels_count));};
00278     player_audio_mixer_channel_t GetChannel(int aIndex) const  {return(GetVar(mDevice->mixer_data.channels[aIndex]));};
00280     uint32_t GetState(void) const {return(GetVar(mDevice->state));};
00281 
00282 
00283 
00285     void PlayWav(uint32_t aDataCount, uint8_t *aData, uint32_t aFormat);
00286 
00288     void SetWavStremRec(bool aState);
00289 
00291     void PlaySample(int aIndex);
00292 
00294     void PlaySeq(player_audio_seq_t * aTones);
00295 
00297     void SetMultMixerLevels(player_audio_mixer_channel_list_t * aLevels);
00298 
00300     void SetMixerLevel(uint32_t index, float amplitude, uint8_t active);
00301 
00304     void RecordWav();
00305 
00307     void LoadSample(int aIndex, uint32_t aDataCount, uint8_t *aData, uint32_t aFormat);
00308 
00311     void GetSample(int aIndex);
00312 
00314     void RecordSample(int aIndex, uint32_t aLength);
00315 
00318     void GetMixerLevels();
00319 
00322     void GetMixerDetails();
00323 
00324 };
00325 
00333 class BlackBoardProxy : public ClientProxy
00334 {
00335   private:
00336     void Subscribe(uint aIndex);
00337     void Unsubscribe();
00338 
00339     // libplayerc data structure
00340     playerc_blackboard_t *mDevice;
00341 
00342   public:
00344         BlackBoardProxy(PlayerClient *aPc, uint aIndex=0);
00346         ~BlackBoardProxy();
00348         player_blackboard_entry_t *SubscribeToKey(const char *key, const char* group = "");
00350         void UnsubscribeFromKey(const char *key, const char* group = "");
00352         void SubscribeToGroup(const char* key);
00354         void UnsubscribeFromGroup(const char* group);
00356         void SetEntry(const player_blackboard_entry_t &entry);
00358         void SetEventHandler(void (*on_blackboard_event)(playerc_blackboard_t *, player_blackboard_entry_t));
00359 };
00360 
00361 // /**
00362 // The @p BlinkenlightProxy class is used to enable and disable
00363 // a flashing indicator light, and to set its period, via a @ref
00364 // interface_blinkenlight device */
00365 // class BlinkenLightProxy : public ClientProxy
00366 // {
00367 //   private:
00368 //
00369 //     void Subscribe(uint32_t aIndex);
00370 //     void Unsubscribe();
00371 //
00372 //     // libplayerc data structure
00373 //     playerc_blinkenlight_t *mDevice;
00374 //
00375 //   public:
00376 //     /** Constructor.
00377 //         Leave the access field empty to start unconnected.
00378 //     */
00379 //     BlinkenLightProxy(PlayerClient *aPc, uint32_t aIndex=0);
00380 //     ~BlinkenLightProxy();
00381 //
00382 //     // true: indicator light enabled, false: disabled.
00383 //     bool GetEnable();
00384 //
00385 //     /** The current period (one whole on/off cycle) of the blinking
00386 //         light. If the period is zero and the light is enabled, the light
00387 //         is on.
00388 //     */
00389 //     void SetPeriod(double aPeriod);
00390 //
00391 //     /** Set the state of the indicator light. A period of zero means
00392 //         the light will be unblinkingly on or off. Returns 0 on
00393 //         success, else -1.
00394 //       */
00395 //     void SetEnable(bool aSet);
00396 // };
00397 
00404 class BlobfinderProxy : public ClientProxy
00405 {
00406   private:
00407 
00408     void Subscribe(uint32_t aIndex);
00409     void Unsubscribe();
00410 
00411     // libplayerc data structure
00412     playerc_blobfinder_t *mDevice;
00413 
00414   public:
00416     BlobfinderProxy(PlayerClient *aPc, uint32_t aIndex=0);
00418     ~BlobfinderProxy();
00419 
00421     uint32_t GetCount() const { return GetVar(mDevice->blobs_count); };
00423     playerc_blobfinder_blob_t GetBlob(uint32_t aIndex) const
00424       { return GetVar(mDevice->blobs[aIndex]);};
00425 
00427     uint32_t GetWidth() const { return GetVar(mDevice->width); };
00429     uint32_t GetHeight() const { return GetVar(mDevice->height); };
00430 
00435     playerc_blobfinder_blob_t operator [](uint32_t aIndex) const
00436       { return(GetBlob(aIndex)); }
00437 
00438 /*
00440     void SetTrackingColor(uint32_t aReMin=0,   uint32_t aReMax=255, uint32_t aGrMin=0,
00441                           uint32_t aGrMax=255, uint32_t aBlMin=0,   uint32_t aBlMax=255);
00442     void SetImagerParams(int aContrast, int aBrightness,
00443                          int aAutogain, int aColormode);
00444     void SetContrast(int aC);
00445     void SetColorMode(int aM);
00446     void SetBrightness(int aB);
00447     void SetAutoGain(int aG);*/
00448 
00449 };
00450 
00455 class BumperProxy : public ClientProxy
00456 {
00457 
00458   private:
00459 
00460     void Subscribe(uint32_t aIndex);
00461     void Unsubscribe();
00462 
00463     // libplayerc data structure
00464     playerc_bumper_t *mDevice;
00465 
00466   public:
00467 
00468     BumperProxy(PlayerClient *aPc, uint32_t aIndex=0);
00469     ~BumperProxy();
00470 
00471     uint32_t GetCount() const { return GetVar(mDevice->bumper_count); };
00472 
00474     uint32_t IsBumped(uint32_t aIndex) const
00475       { return GetVar(mDevice->bumpers[aIndex]); };
00476 
00478     bool IsAnyBumped();
00479 
00481     void RequestBumperConfig();
00482 
00484     uint32_t GetPoseCount() const { return GetVar(mDevice->pose_count); };
00485 
00487     player_bumper_define_t GetPose(uint32_t aIndex) const
00488       { return GetVar(mDevice->poses[aIndex]); };
00489 
00494     bool operator [](uint32_t aIndex) const
00495       { return IsBumped(aIndex); }
00496 
00497 };
00498 
00502 class CameraProxy : public ClientProxy
00503 {
00504 
00505   private:
00506 
00507     virtual void Subscribe(uint32_t aIndex);
00508     virtual void Unsubscribe();
00509 
00510     // libplayerc data structure
00511     playerc_camera_t *mDevice;
00512 
00513     std::string mPrefix;
00514     int mFrameNo;
00515 
00516   public:
00517 
00519     CameraProxy (PlayerClient *aPc, uint32_t aIndex=0);
00520 
00521     virtual ~CameraProxy();
00522 
00526     void SaveFrame(const std::string aPrefix, uint32_t aWidth=4);
00527 
00529     void Decompress();
00530 
00532     uint32_t GetDepth() const { return GetVar(mDevice->bpp); };
00533 
00535     uint32_t GetWidth() const { return GetVar(mDevice->width); };
00536 
00538     uint32_t GetHeight() const { return GetVar(mDevice->height); };
00539 
00546     uint32_t GetFormat() const { return GetVar(mDevice->format); };
00547 
00549     uint32_t GetImageSize() const { return GetVar(mDevice->image_count); };
00550 
00555     void GetImage(uint8_t* aImage) const
00556       {
00557         return GetVarByRef(mDevice->image,
00558                            mDevice->image+GetVar(mDevice->image_count),
00559                            aImage);
00560       };
00561 
00566     uint32_t GetCompression() const { return GetVar(mDevice->compression); };
00567 
00568 };
00569 
00570 
00575 class DioProxy : public ClientProxy
00576 {
00577   private:
00578 
00579     void Subscribe(uint32_t aIndex);
00580     void Unsubscribe();
00581 
00582     // libplayerc data structure
00583     playerc_dio_t *mDevice;
00584 
00585   public:
00587     DioProxy(PlayerClient *aPc, uint32_t aIndex=0);
00589     ~DioProxy();
00590 
00592     uint32_t GetCount() const { return GetVar(mDevice->count); };
00593 
00595     uint32_t GetDigin() const { return GetVar(mDevice->digin); };
00596 
00598     bool GetInput(uint32_t aIndex) const;
00599 
00601     void SetOutput(uint32_t aCount, uint32_t aDigout);
00602 
00607     uint32_t operator [](uint32_t aIndex) const
00608       { return GetInput(aIndex); }
00609 };
00610 
00616 class FiducialProxy : public ClientProxy
00617 {
00618   private:
00619     void Subscribe(uint32_t aIndex);
00620     void Unsubscribe();
00621 
00622     // libplayerc data structure
00623     playerc_fiducial_t *mDevice;
00624 
00625   public:
00627     FiducialProxy(PlayerClient *aPc, uint32_t aIndex=0);
00629     ~FiducialProxy();
00630 
00632     uint32_t GetCount() const { return GetVar(mDevice->fiducials_count); };
00633 
00635     player_fiducial_item_t GetFiducialItem(uint32_t aIndex) const
00636       { return GetVar(mDevice->fiducials[aIndex]);};
00637 
00639     player_pose3d_t GetSensorPose() const
00640       { return GetVar(mDevice->fiducial_geom.pose);};
00641 
00643     player_bbox3d_t GetSensorSize() const
00644       { return GetVar(mDevice->fiducial_geom.size);};
00645 
00647     player_bbox2d_t GetFiducialSize() const
00648       { return GetVar(mDevice->fiducial_geom.fiducial_size);};
00649 
00651     void RequestGeometry();
00652 
00657     player_fiducial_item_t operator [](uint32_t aIndex) const
00658       { return GetFiducialItem(aIndex); }
00659 };
00660 
00664 class GpsProxy : public ClientProxy
00665 {
00666 
00667   private:
00668 
00669     void Subscribe(uint32_t aIndex);
00670     void Unsubscribe();
00671 
00672     // libplayerc data structure
00673     playerc_gps_t *mDevice;
00674 
00675   public:
00676 
00677     // Constructor
00678     GpsProxy(PlayerClient *aPc, uint32_t aIndex=0);
00679 
00680     ~GpsProxy();
00681 
00683     double GetLatitude() const { return GetVar(mDevice->lat); };
00684     double GetLongitude() const { return GetVar(mDevice->lon); };
00685 
00687     double GetAltitude() const { return GetVar(mDevice->alt); };
00688 
00690     uint32_t GetSatellites() const { return GetVar(mDevice->sat_count); };
00691 
00693     uint32_t GetQuality() const { return GetVar(mDevice->quality); };
00694 
00696     double GetHdop() const { return GetVar(mDevice->hdop); };
00697 
00699     double GetVdop() const { return GetVar(mDevice->vdop); };
00700 
00702     double GetUtmEasting() const { return GetVar(mDevice->utm_e); };
00703     double GetUtmNorthing() const { return GetVar(mDevice->utm_n); };
00704 
00706     double GetTime() const { return GetVar(mDevice->utc_time); };
00707 
00709     double GetErrHorizontal() const { return GetVar(mDevice->err_horz); };
00710     double GetErrVertical() const { return GetVar(mDevice->err_vert); };
00711 };
00712 
00720 class Graphics2dProxy : public ClientProxy
00721 {
00722 
00723   private:
00724 
00725     // Subscribe
00726     void Subscribe(uint32_t aIndex);
00727     // Unsubscribe
00728     void Unsubscribe();
00729 
00730     // libplayerc data structure
00731     playerc_graphics2d_t *mDevice;
00732 
00733   public:
00734     // Constructor
00735     Graphics2dProxy(PlayerClient *aPc, uint32_t aIndex=0);
00736     // Destructor
00737     ~Graphics2dProxy();
00738 
00740     void Color(player_color_t col);
00741 
00743     void Color(uint8_t red,  uint8_t green,  uint8_t blue,  uint8_t alpha);
00744 
00746     void Clear(void);
00747 
00749     void DrawPoints(player_point_2d_t pts[], int count);
00750 
00752     void DrawPolygon(player_point_2d_t pts[],
00753                      int count,
00754                      bool filled,
00755                      player_color_t fill_color);
00756 
00758     void DrawPolyline(player_point_2d_t pts[], int count);
00759 };
00760 
00766 class Graphics3dProxy : public ClientProxy
00767 {
00768 
00769   private:
00770 
00771     // Subscribe
00772     void Subscribe(uint32_t aIndex);
00773     // Unsubscribe
00774     void Unsubscribe();
00775 
00776     // libplayerc data structure
00777     playerc_graphics3d_t *mDevice;
00778 
00779   public:
00780     // Constructor
00781     Graphics3dProxy(PlayerClient *aPc, uint32_t aIndex=0);
00782     // Destructor
00783     ~Graphics3dProxy();
00784 
00786     void Color(player_color_t col);
00787 
00789     void Color(uint8_t red,  uint8_t green,  uint8_t blue,  uint8_t alpha);
00790 
00792     void Clear(void);
00793 
00795     void Draw(player_graphics3d_draw_mode_t mode, player_point_3d_t pts[], int count);
00796 
00797 };
00798 
00803 class GripperProxy : public ClientProxy
00804 {
00805 
00806   private:
00807 
00808     void Subscribe(uint32_t aIndex);
00809     void Unsubscribe();
00810 
00811     // libplayerc data structure
00812     playerc_gripper_t *mDevice;
00813 
00814   public:
00815 
00817     GripperProxy(PlayerClient *aPc, uint32_t aIndex=0);
00819     ~GripperProxy();
00820 
00823     void RequestGeometry(void);
00824 
00826     uint32_t GetState() const { return GetVar(mDevice->state); };
00828     uint32_t GetBeams() const { return GetVar(mDevice->beams); };
00830     player_pose3d_t GetPose() const { return GetVar(mDevice->pose); };
00832     player_bbox3d_t GetOuterSize() const { return GetVar(mDevice->outer_size); };
00834     player_bbox3d_t GetInnerSize() const { return GetVar(mDevice->inner_size); };
00836     uint32_t GetNumBeams() const { return GetVar(mDevice->num_beams); };
00838     uint32_t GetCapacity() const { return GetVar(mDevice->capacity); };
00840     uint32_t GetStored() const { return GetVar(mDevice->stored); };
00841 
00843     void Open();
00845     void Close();
00847     void Stop();
00849     void Store();
00851     void Retrieve();
00852 };
00853 
00856 class HealthProxy : public ClientProxy
00857 {
00858 
00859   private:
00860 
00861     void Subscribe(uint32_t aIndex);
00862     void Unsubscribe();
00863 
00864     // libplayerc data structure
00865     playerc_health_t *mDevice;
00866 
00867   public:
00869     HealthProxy(PlayerClient *aPc, uint32_t aIndex=0);
00871     ~HealthProxy();
00872 
00874     float GetIdleCPU();
00875 
00877     float GetSystemCPU();
00878 
00880     float GetUserCPU();
00881 
00883     int64_t GetMemTotal();
00884 
00886     int64_t GetMemUsed();
00887 
00889     int64_t GetMemFree();
00890 
00892     int64_t GetSwapTotal();
00893 
00895     int64_t GetSwapUsed();
00896 
00898     int64_t GetSwapFree();
00899 
00901     float GetPercMemUsed();
00902 
00904     float GetPercSwapUsed();
00905 
00907     float GetPercTotalUsed();
00908 };
00909 
00910 
00911 
00916 class ImuProxy : public ClientProxy
00917 {
00918   private:
00919     void Subscribe(uint32_t aIndex);
00920     void Unsubscribe();
00921 
00922     // libplayerc data structure
00923     playerc_imu_t *mDevice;
00924 
00925   public:
00926 
00928     ImuProxy(PlayerClient *aPc, uint32_t aIndex=0);
00930     ~ImuProxy();
00931 
00933     player_pose3d_t GetPose() const { return GetVar(mDevice->pose); };
00935     float GetXAccel();
00936     float GetYAccel();
00937     float GetZAccel();
00938     float GetXGyro();
00939     float GetYGyro();
00940     float GetZGyro();
00941     float GetXMagn();
00942     float GetYMagn();
00943     float GetZMagn();
00944 
00945     player_imu_data_calib_t GetRawValues() const
00946     { return GetVar(mDevice->calib_data); };
00947 
00949     void SetDatatype(int aDatatype);
00950 
00952     void ResetOrientation(int aValue);
00953     
00954 
00955 };
00956 
00957 
00962 class IrProxy : public ClientProxy
00963 {
00964 
00965   private:
00966 
00967     void Subscribe(uint32_t aIndex);
00968     void Unsubscribe();
00969 
00970     // libplayerc data structure
00971     playerc_ir_t *mDevice;
00972 
00973   public:
00974 
00976     IrProxy(PlayerClient *aPc, uint32_t aIndex=0);
00978     ~IrProxy();
00979 
00981     uint32_t GetCount() const { return GetVar(mDevice->data.ranges_count); };
00983     double GetRange(uint32_t aIndex) const
00984       { return GetVar(mDevice->data.ranges[aIndex]); };
00986     double GetVoltage(uint32_t aIndex) const
00987       { return GetVar(mDevice->data.voltages[aIndex]); };
00989     uint32_t GetPoseCount() const { return GetVar(mDevice->poses.poses_count); };
00991     player_pose3d_t GetPose(uint32_t aIndex) const
00992       {return GetVar(mDevice->poses.poses[aIndex]);};
00993 
00995     void RequestGeom();
00996 
01001     double operator [](uint32_t aIndex) const
01002       { return GetRange(aIndex); }
01003 
01004 };
01005 
01011 class LaserProxy : public ClientProxy
01012 {
01013   private:
01014 
01015     void Subscribe(uint32_t aIndex);
01016     void Unsubscribe();
01017 
01018     // libplayerc data structure
01019     playerc_laser_t *mDevice;
01020 
01021     // local storage of config
01022     double min_angle, max_angle, scan_res, range_res, scanning_frequency;
01023     bool intensity;
01024 
01025   public:
01026 
01028     LaserProxy(PlayerClient *aPc, uint32_t aIndex=0);
01030     ~LaserProxy();
01031 
01033     uint32_t GetCount() const { return GetVar(mDevice->scan_count); };
01034 
01036     double GetMaxRange() const { return GetVar(mDevice->max_range); };
01037 
01039     double GetScanRes() const { return GetVar(mDevice->scan_res); };
01040 
01042     double GetRangeRes() const { return GetVar(mDevice->range_res); };
01043 
01045     double GetScanningFrequency() const { return GetVar(mDevice->scanning_frequency); };
01046 
01048     double GetMinAngle() const { return GetVar(mDevice->scan_start); };
01050     double GetMaxAngle() const
01051     {
01052       scoped_lock_t lock(mPc->mMutex);
01053       return mDevice->scan_start + (mDevice->scan_count - 1)*mDevice->scan_res;
01054     };
01055 
01057     double GetConfMinAngle() const { return min_angle; };
01059     double GetConfMaxAngle() const { return max_angle; };
01060 
01062     bool IntensityOn() const { return GetVar(mDevice->intensity_on); };
01063 
01064 //    /// Scan data (polar): range (m) and bearing (radians)
01065 //    double GetScan(uint32_t aIndex) const
01066 //      { return GetVar(mDevice->scan[aIndex]); };
01067 
01069     player_point_2d_t GetPoint(uint32_t aIndex) const
01070       { return GetVar(mDevice->point[aIndex]); };
01071 
01072 
01074     double GetRange(uint32_t aIndex) const
01075       { return GetVar(mDevice->ranges[aIndex]); };
01076 
01078     double GetBearing(uint32_t aIndex) const
01079       { return GetVar(mDevice->scan[aIndex][1]); };
01080 
01081 
01083     int GetIntensity(uint32_t aIndex) const
01084       { return GetVar(mDevice->intensity[aIndex]); };
01085 
01087     int GetID() const
01088       { return GetVar(mDevice->laser_id); };
01089 
01090 
01099     void Configure(double aMinAngle,
01100                    double aMaxAngle,
01101                    uint32_t aScanRes,
01102                    uint32_t aRangeRes,
01103                    bool aIntensity,
01104                    double aScanningFrequency);
01105 
01108     void RequestConfigure();
01109     
01111     void RequestID();
01112 
01115     void RequestGeom();
01116 
01119     player_pose3d_t GetPose()
01120     {
01121       player_pose3d_t p;
01122       scoped_lock_t lock(mPc->mMutex);
01123 
01124       p.px = mDevice->pose[0];
01125       p.py = mDevice->pose[1];
01126       p.pyaw = mDevice->pose[2];
01127       return(p);
01128     }
01129 
01132     player_pose3d_t GetRobotPose()
01133     {
01134       player_pose3d_t p;
01135       scoped_lock_t lock(mPc->mMutex);
01136 
01137       p.px = mDevice->robot_pose[0];
01138       p.py = mDevice->robot_pose[1];
01139       p.pyaw = mDevice->robot_pose[2];
01140       return(p);
01141     }
01142 
01144     player_bbox3d_t GetSize()
01145     {
01146       player_bbox3d_t b;
01147       scoped_lock_t lock(mPc->mMutex);
01148 
01149       b.sl = mDevice->size[0];
01150       b.sw = mDevice->size[1];
01151       return(b);
01152     }
01153     
01155     double GetMinLeft() const
01156       { return GetVar(mDevice->min_left); };
01157     
01159     double GetMinRight() const
01160       { return GetVar(mDevice->min_right); };
01161            
01163     double MinLeft () const 
01164       { return GetMinLeft(); }
01165 
01167     double MinRight () const
01168       { return GetMinRight(); }
01169 
01174     double operator [] (uint32_t index) const
01175       { return GetRange(index);}
01176 
01177 };
01178 
01179 
01184 class LimbProxy : public ClientProxy
01185 {
01186   private:
01187 
01188     void Subscribe(uint32_t aIndex);
01189     void Unsubscribe();
01190 
01191    // libplayerc data structure
01192     playerc_limb_t *mDevice;
01193 
01194   public:
01195 
01196     LimbProxy(PlayerClient *aPc, uint32_t aIndex=0);
01197     ~LimbProxy();
01198 
01201     void RequestGeometry(void);
01202 
01204     void SetPowerConfig(bool aVal);
01206     void SetBrakesConfig(bool aVal);
01208     void SetSpeedConfig(float aSpeed);
01209 
01211     void MoveHome(void);
01213     void Stop(void);
01215     void SetPose(float aPX, float aPY, float aPZ,
01216                  float aAX, float aAY, float aAZ,
01217                  float aOX, float aOY, float aOZ);
01219     void SetPosition(float aX, float aY, float aZ);
01222     void VectorMove(float aX, float aY, float aZ, float aLength);
01223 
01225     player_limb_data_t GetData(void) const;
01227     player_limb_geom_req_t GetGeom(void) const;
01228 };
01229 
01230 
01231 
01237 class LocalizeProxy : public ClientProxy
01238 {
01239 
01240   private:
01241 
01242     void Subscribe(uint32_t aIndex);
01243     void Unsubscribe();
01244 
01245     // libplayerc data structure
01246     playerc_localize_t *mDevice;
01247 
01248   public:
01249 
01251     LocalizeProxy(PlayerClient *aPc, uint32_t aIndex=0);
01253     ~LocalizeProxy();
01254 
01256     // @todo should these be in a player_pose_t?
01257     uint32_t GetMapSizeX() const { return GetVar(mDevice->map_size_x); };
01258     uint32_t GetMapSizeY() const { return GetVar(mDevice->map_size_y); };
01259 
01260     // @todo should these be in a player_pose_t?
01261     uint32_t GetMapTileX() const { return GetVar(mDevice->map_tile_x); };
01262     uint32_t GetMapTileY() const { return GetVar(mDevice->map_tile_y); };
01263 
01265     double GetMapScale() const { return GetVar(mDevice->map_scale); };
01266 
01267     // Map data (empty = -1, unknown = 0, occupied = +1)
01268     // is this still needed?  if so,
01269     //void GetMapCells(uint8_t* aCells) const
01270     //{
01271     //  return GetVarByRef(mDevice->map_cells,
01272     //                     mDevice->image+GetVar(mDevice->??map_cell_cout??),
01273     //                     aCells);
01274     //};
01275 
01277     uint32_t GetPendingCount() const { return GetVar(mDevice->pending_count); };
01278 
01280     uint32_t GetHypothCount() const { return GetVar(mDevice->hypoth_count); };
01281 
01283     player_localize_hypoth_t GetHypoth(uint32_t aIndex) const
01284       { return GetVar(mDevice->hypoths[aIndex]); };
01285 
01287     int GetParticles()
01288       { return playerc_localize_get_particles(mDevice); }
01289 
01291     player_pose2d_t GetParticlePose(int index) const;
01292 
01294     void SetPose(double pose[3], double cov[3]);
01295 
01297     uint32_t GetNumHypoths() const { return GetVar(mDevice->hypoth_count); };
01298 
01301     uint32_t GetNumParticles() const { return GetVar(mDevice->num_particles); };
01302 };
01303 
01304 
01308 class LogProxy : public ClientProxy
01309 {
01310   private:
01311 
01312     void Subscribe(uint32_t aIndex);
01313     void Unsubscribe();
01314 
01315     // libplayerc data structure
01316     playerc_log_t *mDevice;
01317 
01318   public:
01320     LogProxy(PlayerClient *aPc, uint32_t aIndex=0);
01321 
01323     ~LogProxy();
01324 
01327     int GetType() const { return GetVar(mDevice->type); };
01328 
01330     int GetState() const { return GetVar(mDevice->state); };
01331 
01333     void QueryState();
01334 
01337     void SetState(int aState);
01338 
01340     void SetWriteState(int aState);
01341 
01343     void SetReadState(int aState);
01344 
01346     void Rewind();
01347 
01349     void SetFilename(const std::string aFilename);
01350 };
01351 
01355 class MapProxy : public ClientProxy
01356 {
01357   private:
01358 
01359     void Subscribe(uint32_t aIndex);
01360     void Unsubscribe();
01361 
01362     // libplayerc data structure
01363     playerc_map_t *mDevice;
01364 
01365   public:
01367     MapProxy(PlayerClient *aPc, uint32_t aIndex=0);
01368 
01370     ~MapProxy();
01371 
01373     void RequestMap();
01374 
01376     int GetCellIndex(int x, int y) const
01377     { return y*GetWidth() + x; };
01378 
01380     int8_t GetCell(int x, int y) const
01381     { return GetVar(mDevice->cells[GetCellIndex(x,y)]); };
01382 
01384     double GetResolution() const { return GetVar(mDevice->resolution); };
01385 
01388     uint32_t GetWidth() const { return GetVar(mDevice->width); };
01391     uint32_t GetHeight() const { return GetVar(mDevice->height); };
01392 
01393     double GetOriginX() const { return GetVar(mDevice->origin[0]); };
01394     double GetOriginY() const { return GetVar(mDevice->origin[1]); };
01395 
01397     void GetMap(int8_t* aMap) const
01398     {
01399       return GetVarByRef(reinterpret_cast<int8_t*>(mDevice->cells),
01400                          reinterpret_cast<int8_t*>(mDevice->cells+GetWidth()*GetHeight()),
01401                          aMap);
01402     };
01403 };
01404 
01410 class OpaqueProxy : public ClientProxy
01411 {
01412 
01413   private:
01414 
01415     void Subscribe(uint32_t aIndex);
01416     void Unsubscribe();
01417 
01418     // libplayerc data structure
01419     playerc_opaque_t *mDevice;
01420 
01421   public:
01422 
01424     OpaqueProxy(PlayerClient *aPc, uint32_t aIndex=0);
01426     ~OpaqueProxy();
01427 
01429     uint32_t GetCount() const { return GetVar(mDevice->data_count); };
01430 
01432     void GetData(uint8_t* aDest) const
01433       {
01434         return GetVarByRef(mDevice->data,
01435                            mDevice->data+GetVar(mDevice->data_count),
01436                            aDest);
01437       };
01438 
01440     void SendCmd(player_opaque_data_t* aData);
01441 
01443     int SendReq(player_opaque_data_t* aRequest);
01444 
01445 };
01446 
01450 class PlannerProxy : public ClientProxy
01451 {
01452 
01453   private:
01454 
01455     void Subscribe(uint32_t aIndex);
01456     void Unsubscribe();
01457 
01458     // libplayerc data structure
01459     playerc_planner_t *mDevice;
01460 
01461   public:
01462 
01464     PlannerProxy(PlayerClient *aPc, uint32_t aIndex=0);
01466     ~PlannerProxy();
01467 
01469     void SetGoalPose(double aGx, double aGy, double aGa);
01470 
01473     void RequestWaypoints();
01474 
01477     void SetEnable(bool aEnable);
01478 
01480     uint32_t GetPathValid() const { return GetVar(mDevice->path_valid); };
01481 
01483     uint32_t GetPathDone() const { return GetVar(mDevice->path_done); };
01484 
01487     double GetPx() const { return GetVar(mDevice->px); };
01490     double GetPy() const { return GetVar(mDevice->py); };
01493     double GetPa() const { return GetVar(mDevice->pa); };
01494 
01496     player_pose2d_t GetPose() const
01497     {
01498       player_pose2d_t p;
01499       scoped_lock_t lock(mPc->mMutex);
01500       p.px = mDevice->px;
01501       p.py = mDevice->py;
01502       p.pa = mDevice->pa;
01503       return(p);
01504     }
01505 
01508     double GetGx() const { return GetVar(mDevice->gx); };
01511     double GetGy() const { return GetVar(mDevice->gy); };
01514     double GetGa() const { return GetVar(mDevice->ga); };
01515 
01517     player_pose2d_t GetGoal() const
01518     {
01519       player_pose2d_t p;
01520       scoped_lock_t lock(mPc->mMutex);
01521       p.px = mDevice->gx;
01522       p.py = mDevice->gy;
01523       p.pa = mDevice->ga;
01524       return(p);
01525     }
01526 
01529     double GetWx() const { return GetVar(mDevice->wx); };
01532     double GetWy() const { return GetVar(mDevice->wy); };
01535     double GetWa() const { return GetVar(mDevice->wa); };
01536 
01538     player_pose2d_t GetCurrentWaypoint() const
01539     {
01540       player_pose2d_t p;
01541       scoped_lock_t lock(mPc->mMutex);
01542       p.px = mDevice->wx;
01543       p.py = mDevice->wy;
01544       p.pa = mDevice->wa;
01545       return(p);
01546     }
01547 
01550     double GetIx(int i) const;
01553     double GetIy(int i) const;
01556     double GetIa(int i) const;
01557 
01559     player_pose2d_t GetWaypoint(uint32_t aIndex) const
01560     {
01561       assert(aIndex < GetWaypointCount());
01562       player_pose2d_t p;
01563       scoped_lock_t lock(mPc->mMutex);
01564       p.px = mDevice->waypoints[aIndex][0];
01565       p.py = mDevice->waypoints[aIndex][1];
01566       p.pa = mDevice->waypoints[aIndex][2];
01567       return(p);
01568     }
01569 
01573     int GetCurrentWaypointId() const
01574       { return GetVar(mDevice->curr_waypoint); };
01575 
01577     uint32_t GetWaypointCount() const
01578       { return GetVar(mDevice->waypoint_count); };
01579 
01584     player_pose2d_t operator [](uint32_t aIndex) const
01585       { return GetWaypoint(aIndex); }
01586 
01587 };
01588 
01592 class Pointcloud3dProxy : public ClientProxy
01593 {
01594   private:
01595 
01596     void Subscribe(uint32_t aIndex);
01597     void Unsubscribe();
01598 
01599     // libplayerc data structure
01600     playerc_pointcloud3d_t *mDevice;
01601 
01602   public:
01604     Pointcloud3dProxy(PlayerClient *aPc, uint32_t aIndex=0);
01605 
01607     ~Pointcloud3dProxy();
01608 
01610     uint32_t GetCount() const { return GetVar(mDevice->points_count); };
01611 
01613     player_pointcloud3d_element_t GetPoint(uint32_t aIndex) const
01614       { return GetVar(mDevice->points[aIndex]); };
01615 
01618     player_pointcloud3d_element_t operator [] (uint32_t aIndex) const { return GetPoint(aIndex); }
01619 
01620 };
01621 
01622 
01627 class Position1dProxy : public ClientProxy
01628 {
01629 
01630   private:
01631 
01632     void Subscribe(uint32_t aIndex);
01633     void Unsubscribe();
01634 
01635     // libplayerc data structure
01636     playerc_position1d_t *mDevice;
01637 
01638   public:
01639 
01641     Position1dProxy(PlayerClient *aPc, uint32_t aIndex=0);
01643     ~Position1dProxy();
01644 
01648     void SetSpeed(double aVel);
01649 
01653     void GoTo(double aPos, double aVel);
01654 
01657     void RequestGeom();
01658 
01660     player_pose3d_t GetPose() const
01661     {
01662       player_pose3d_t p;
01663       scoped_lock_t lock(mPc->mMutex);
01664       p.px = mDevice->pose[0];
01665       p.py = mDevice->pose[1];
01666       p.pyaw = mDevice->pose[2];
01667       return(p);
01668     }
01669 
01671     player_bbox3d_t GetSize() const
01672     {
01673       player_bbox3d_t b;
01674       scoped_lock_t lock(mPc->mMutex);
01675       b.sl = mDevice->size[0];
01676       b.sw = mDevice->size[1];
01677       return(b);
01678     }
01679 
01684     void SetMotorEnable(bool enable);
01685 
01688     void SetOdometry(double aPos);
01689 
01691     void ResetOdometry() { SetOdometry(0); };
01692 
01694     //void SetSpeedPID(double kp, double ki, double kd);
01695 
01697     //void SetPositionPID(double kp, double ki, double kd);
01698 
01701     //void SetPositionSpeedProfile(double spd, double acc);
01702 
01704     double  GetPos() const { return GetVar(mDevice->pos); };
01705 
01707     double  GetVel() const { return GetVar(mDevice->vel); };
01708 
01710     bool GetStall() const { return GetVar(mDevice->stall); };
01711 
01713     uint8_t GetStatus() const { return GetVar(mDevice->status); };
01714 
01716     bool IsLimitMin() const
01717       { return (GetVar(mDevice->status) &
01718                (1 << PLAYER_POSITION1D_STATUS_LIMIT_MIN)) > 0; };
01719 
01721     bool IsLimitCen() const
01722       { return (GetVar(mDevice->status) &
01723                (1 << PLAYER_POSITION1D_STATUS_LIMIT_CEN)) > 0; };
01724 
01726     bool IsLimitMax() const
01727       { return (GetVar(mDevice->status) &
01728                (1 << PLAYER_POSITION1D_STATUS_LIMIT_MAX)) > 0; };
01729 
01731     bool IsOverCurrent() const
01732       { return (GetVar(mDevice->status) &
01733                (1 << PLAYER_POSITION1D_STATUS_OC)) > 0; };
01734 
01736     bool IsTrajComplete() const
01737       { return (GetVar(mDevice->status) &
01738                (1 << PLAYER_POSITION1D_STATUS_TRAJ_COMPLETE)) > 0; };
01739 
01741     bool IsEnabled() const
01742       { return (GetVar(mDevice->status) &
01743                (1 << PLAYER_POSITION1D_STATUS_ENABLED)) > 0; };
01744 
01745 };
01746 
01751 class Position2dProxy : public ClientProxy
01752 {
01753 
01754   private:
01755 
01756     void Subscribe(uint32_t aIndex);
01757     void Unsubscribe();
01758 
01759     // libplayerc data structure
01760     playerc_position2d_t *mDevice;
01761 
01762   public:
01763 
01765     Position2dProxy(PlayerClient *aPc, uint32_t aIndex=0);
01767     ~Position2dProxy();
01768 
01772     void SetSpeed(double aXSpeed, double aYSpeed, double aYawSpeed);
01773 
01776     void SetSpeed(double aXSpeed, double aYawSpeed)
01777         { return SetSpeed(aXSpeed, 0, aYawSpeed);}
01778 
01780     void SetSpeed(player_pose2d_t vel)
01781         { return SetSpeed(vel.px, vel.py, vel.pa);}
01782 
01786     void GoTo(player_pose2d_t pos, player_pose2d_t vel);
01787 
01789     void GoTo(player_pose2d_t pos)
01790       {GoTo(pos,(player_pose2d_t) {0,0,0}); }
01791 
01794     void GoTo(double aX, double aY, double aYaw)
01795       {GoTo((player_pose2d_t) {aX,aY,aYaw},(player_pose2d_t) {0,0,0}); }
01796 
01798     void SetCarlike(double aXSpeed, double aDriveAngle);
01799 
01802     void RequestGeom();
01803 
01805     //  body (fill it in by calling RequestGeom()).
01806     player_pose3d_t GetOffset()
01807     {
01808       player_pose3d_t p;
01809       scoped_lock_t lock(mPc->mMutex);
01810       p.px = mDevice->pose[0];
01811       p.py = mDevice->pose[1];
01812       p.pyaw = mDevice->pose[2];
01813       return(p);
01814     }
01815 
01817     player_bbox3d_t GetSize()
01818     {
01819       player_bbox3d_t b;
01820       scoped_lock_t lock(mPc->mMutex);
01821       b.sl = mDevice->size[0];
01822       b.sw = mDevice->size[1];
01823       return(b);
01824     }
01825 
01830     void SetMotorEnable(bool enable);
01831 
01832     // Select velocity control mode.
01833     //
01834     // For the the p2os_position driver, set @p mode to 0 for direct wheel
01835     // velocity control (default), or 1 for separate translational and
01836     // rotational control.
01837     //
01838     // For the reb_position driver: 0 is direct velocity control, 1 is for
01839     // velocity-based heading PD controller (uses DoDesiredHeading()).
01840     //void SelectVelocityControl(unsigned char mode);
01841 
01843     void ResetOdometry();
01844 
01847     //void SelectPositionMode(unsigned char mode);
01848 
01851     void SetOdometry(double aX, double aY, double aYaw);
01852 
01854     //void SetSpeedPID(double kp, double ki, double kd);
01855 
01857     //void SetPositionPID(double kp, double ki, double kd);
01858 
01861     //void SetPositionSpeedProfile(double spd, double acc);
01862 
01863     //
01864     // void DoStraightLine(double m);
01865 
01866     //
01867     //void DoRotation(double yawspeed);
01868 
01869     //
01870     //void DoDesiredHeading(double yaw, double xspeed, double yawspeed);
01871 
01872     //
01873     //void SetStatus(uint8_t cmd, uint16_t value);
01874 
01875     //
01876     //void PlatformShutdown();
01877 
01879     double  GetXPos() const { return GetVar(mDevice->px); };
01880 
01882     double  GetYPos() const { return GetVar(mDevice->py); };
01883 
01885     double GetYaw() const { return GetVar(mDevice->pa); };
01886 
01888     double  GetXSpeed() const { return GetVar(mDevice->vx); };
01889 
01891     double  GetYSpeed() const { return GetVar(mDevice->vy); };
01892 
01894     double  GetYawSpeed() const { return GetVar(mDevice->va); };
01895 
01897     bool GetStall() const { return GetVar(mDevice->stall); };
01898 
01899 };
01900 
01907 class Position3dProxy : public ClientProxy
01908 {
01909 
01910   private:
01911 
01912     void Subscribe(uint32_t aIndex);
01913     void Unsubscribe();
01914 
01915     // libplayerc data structure
01916     playerc_position3d_t *mDevice;
01917 
01918   public:
01919 
01921     Position3dProxy(PlayerClient *aPc, uint32_t aIndex=0);
01923     ~Position3dProxy();
01924 
01928     void SetSpeed(double aXSpeed, double aYSpeed, double aZSpeed,
01929                   double aRollSpeed, double aPitchSpeed, double aYawSpeed);
01930 
01934     void SetSpeed(double aXSpeed, double aYSpeed,
01935                   double aZSpeed, double aYawSpeed)
01936       { SetSpeed(aXSpeed,aYSpeed,aZSpeed,0,0,aYawSpeed); }
01937 
01939     void SetSpeed(double aXSpeed, double aYSpeed, double aYawSpeed)
01940       { SetSpeed(aXSpeed, aYSpeed, 0, 0, 0, aYawSpeed); }
01941 
01944     void SetSpeed(double aXSpeed, double aYawSpeed)
01945       { SetSpeed(aXSpeed,0,0,0,0,aYawSpeed);}
01946 
01948     void SetSpeed(player_pose3d_t vel)
01949       { SetSpeed(vel.px,vel.py,vel.pz,vel.proll,vel.ppitch,vel.pyaw);}
01950 
01954     void GoTo(player_pose3d_t aPos, player_pose3d_t aVel);
01955 
01957     void GoTo(player_pose3d_t aPos)
01958       { GoTo(aPos, (player_pose3d_t) {0,0,0,0,0,0}); }
01959 
01960 
01963     void GoTo(double aX, double aY, double aZ,
01964               double aRoll, double aPitch, double aYaw)
01965       { GoTo((player_pose3d_t) {aX,aY,aZ,aRoll,aPitch,aYaw},
01966               (player_pose3d_t) {0,0,0,0,0,0});
01967       }
01968 
01973     void SetMotorEnable(bool aEnable);
01974 
01977     void SelectVelocityControl(int aMode);
01978 
01980     void ResetOdometry();
01981 
01985     void SetOdometry(double aX, double aY, double aZ,
01986                      double aRoll, double aPitch, double aYaw);
01987 
01990     void RequestGeom();
01991 
01992     // Select position mode
01993     // Set @p mode for 0 for velocity mode, 1 for position mode.
01994     //void SelectPositionMode(unsigned char mode);
01995 
01996     //
01997     //void SetSpeedPID(double kp, double ki, double kd);
01998 
01999     //
02000     //void SetPositionPID(double kp, double ki, double kd);
02001 
02002     // Sets the ramp profile for position based control
02003     // spd rad/s, acc rad/s/s
02004     //void SetPositionSpeedProfile(double spd, double acc);
02005 
02007     double  GetXPos() const { return GetVar(mDevice->pos_x); };
02008 
02010     double  GetYPos() const { return GetVar(mDevice->pos_y); };
02011 
02013     double  GetZPos() const { return GetVar(mDevice->pos_z); };
02014 
02016     double  GetRoll() const { return GetVar(mDevice->pos_roll); };
02017 
02019     double  GetPitch() const { return GetVar(mDevice->pos_pitch); };
02020 
02022     double  GetYaw() const { return GetVar(mDevice->pos_yaw); };
02023 
02025     double  GetXSpeed() const { return GetVar(mDevice->vel_x); };
02026 
02028     double  GetYSpeed() const { return GetVar(mDevice->vel_y); };
02029 
02031     double  GetZSpeed() const { return GetVar(mDevice->vel_z); };
02032 
02034     double  GetRollSpeed() const { return GetVar(mDevice->vel_roll); };
02035 
02037     double  GetPitchSpeed() const { return GetVar(mDevice->vel_pitch); };
02038 
02040     double  GetYawSpeed() const { return GetVar(mDevice->vel_yaw); };
02041 
02043     bool GetStall () const { return GetVar(mDevice->stall); };
02044 };
02047 class PowerProxy : public ClientProxy
02048 {
02049   private:
02050 
02051     void Subscribe(uint32_t aIndex);
02052     void Unsubscribe();
02053 
02054     // libplayerc data structure
02055     playerc_power_t *mDevice;
02056 
02057   public:
02059     PowerProxy(PlayerClient *aPc, uint32_t aIndex=0);
02061     ~PowerProxy();
02062 
02064     double GetCharge() const { return GetVar(mDevice->charge); };
02065 
02067     double GetPercent() const {return GetVar(mDevice->percent); };
02068 
02070     double GetJoules() const {return GetVar(mDevice->joules); };
02071 
02073     double GetWatts() const {return GetVar(mDevice->watts); };
02074 
02076     bool GetCharging() const {return GetVar(mDevice->charging);};
02077 
02078     // Return whether the power data is valid
02079     bool IsValid() const {return GetVar(mDevice->valid);};
02080 };
02081 
02088 class PtzProxy : public ClientProxy
02089 {
02090 
02091   private:
02092 
02093     void Subscribe(uint32_t aIndex);
02094     void Unsubscribe();
02095 
02096     // libplayerc data structure
02097     playerc_ptz_t *mDevice;
02098 
02099   public:
02101     PtzProxy(PlayerClient *aPc, uint32_t aIndex=0);
02102     // destructor
02103     ~PtzProxy();
02104 
02105   public:
02106 
02110     void SetCam(double aPan, double aTilt, double aZoom);
02111 
02113     void SetSpeed(double aPanSpeed=0, double aTiltSpeed=0, double aZoomSpeed=0);
02114 
02117     void SelectControlMode(uint32_t aMode);
02118 
02120     double GetPan() const { return GetVar(mDevice->pan); };
02122     double GetTilt() const { return GetVar(mDevice->tilt); };
02124     double GetZoom() const { return GetVar(mDevice->zoom); };
02125 
02127     int GetStatus();
02128 
02129 
02130 };
02131 
02134 class RangerProxy : public ClientProxy
02135 {
02136   private:
02137 
02138     void Subscribe(uint32_t aIndex);
02139     void Unsubscribe();
02140 
02141     // libplayerc data structure
02142     playerc_ranger_t *mDevice;
02143 
02144   public:
02146     RangerProxy(PlayerClient *aPc, uint32_t aIndex=0);
02148     ~RangerProxy();
02149 
02151     uint32_t GetSensorCount() const { return GetVar(mDevice->sensor_count); };
02152 
02154     player_pose3d_t GetDevicePose() const { return GetVar(mDevice->device_pose); };
02156     player_bbox3d_t GetDeviceSize() const { return GetVar(mDevice->device_size); };
02157 
02159     player_pose3d_t GetSensorPose(uint32_t aIndex) const;
02161     player_bbox3d_t GetSensorSize(uint32_t aIndex) const;
02162 
02164     uint32_t GetRangeCount() const { return GetVar(mDevice->ranges_count); };
02166     double GetRange(uint32_t aIndex) const;
02168     double operator[] (uint32_t aIndex) const { return GetRange(aIndex); }
02169 
02171     uint32_t GetIntensityCount() const { return GetVar(mDevice->intensities_count); } ;
02173     double GetIntensity(uint32_t aIndex) const;
02174 
02177     void SetPower(bool aEnable);
02178 
02181     void SetIntensityData(bool aEnable);
02182 
02184     void RequestGeom();
02185 
02190     void Configure(double aMinAngle,
02191                    double aMaxAngle,
02192                    double aResolution,
02193                    double aMaxRange,
02194                    double aRangeRes,
02195                    double aFrequency);
02196 
02199     void RequestConfigure();
02200 
02202     double GetMinAngle() const { return GetVar(mDevice->min_angle); };
02203 
02205     double GetMaxAngle() const { return GetVar(mDevice->max_angle); };
02206 
02208     double GetResolution() const { return GetVar(mDevice->resolution); };
02209 
02211     double GetMaxRange() const { return GetVar(mDevice->max_range); };
02212 
02214     double GetRangeRes() const { return GetVar(mDevice->range_res); };
02215 
02217     double GetFrequency() const { return GetVar(mDevice->frequency); };
02218 };
02219 
02222 class RFIDProxy : public ClientProxy
02223 {
02224 
02225   private:
02226 
02227     void Subscribe(uint32_t aIndex);
02228     void Unsubscribe();
02229 
02230     // libplayerc data structure
02231     playerc_rfid_t *mDevice;
02232 
02233   public:
02235     RFIDProxy(PlayerClient *aPc, uint32_t aIndex=0);
02237     ~RFIDProxy();
02238 
02240     uint32_t GetTagsCount() const { return GetVar(mDevice->tags_count); };
02242     playerc_rfidtag_t GetRFIDTag(uint32_t aIndex) const
02243       { return GetVar(mDevice->tags[aIndex]);};
02244 
02249     playerc_rfidtag_t operator [](uint32_t aIndex) const
02250       { return(GetRFIDTag(aIndex)); }
02251 };
02252 
02257 class SimulationProxy : public ClientProxy
02258 {
02259   private:
02260 
02261     void Subscribe(uint32_t aIndex);
02262     void Unsubscribe();
02263 
02264     // libplayerc data structure
02265     playerc_simulation_t *mDevice;
02266 
02267   public:
02269     SimulationProxy(PlayerClient *aPc, uint32_t aIndex=0);
02271     ~SimulationProxy();
02272 
02275     void SetPose2d(char* identifier, double x, double y, double a);
02276 
02279     void GetPose2d(char* identifier, double& x, double& y, double& a);
02280 
02283     void SetPose3d(char* identifier, double x, double y, double z,
02284                    double roll, double pitch, double yaw);
02285 
02288     void GetPose3d(char* identifier, double& x, double& y, double& z,
02289                    double& roll, double& pitch, double& yaw, double& time);
02290 
02292     void GetProperty(char* identifier, char *name, void *value, size_t value_len );
02293 
02295     void SetProperty(char* identifier, char *name, void *value, size_t value_len );
02296 };
02297 
02298 
02304 class SonarProxy : public ClientProxy
02305 {
02306   private:
02307 
02308     void Subscribe(uint32_t aIndex);
02309     void Unsubscribe();
02310 
02311     // libplayerc data structure
02312     playerc_sonar_t *mDevice;
02313 
02314   public:
02316     SonarProxy(PlayerClient *aPc, uint32_t aIndex=0);
02318     ~SonarProxy();
02319 
02321     uint32_t GetCount() const { return GetVar(mDevice->scan_count); };
02322 
02324     double GetScan(uint32_t aIndex) const
02325       { if (GetVar(mDevice->scan_count) <= (int32_t)aIndex) return -1.0; return GetVar(mDevice->scan[aIndex]); };
02328     double operator [] (uint32_t aIndex) const { return GetScan(aIndex); }
02329 
02331     uint32_t GetPoseCount() const { return GetVar(mDevice->pose_count); };
02332 
02334     player_pose3d_t GetPose(uint32_t aIndex) const
02335       { return GetVar(mDevice->poses[aIndex]); };
02336 
02337     // Enable/disable the sonars.
02338     // Set @p state to 1 to enable, 0 to disable.
02339     // Note that when sonars are disabled the client will still receive sonar
02340     // data, but the ranges will always be the last value read from the sonars
02341     // before they were disabled.
02342     //void SetEnable(bool aEnable);
02343 
02345     void RequestGeom();
02346 };
02347 
02352 class SpeechProxy : public ClientProxy
02353 {
02354 
02355   private:
02356 
02357     void Subscribe(uint32_t aIndex);
02358     void Unsubscribe();
02359 
02360     // libplayerc data structure
02361     playerc_speech_t *mDevice;
02362 
02363   public:
02365     SpeechProxy(PlayerClient *aPc, uint32_t aIndex=0);
02367     ~SpeechProxy();
02368 
02371     void Say(std::string aStr);
02372 };
02373 
02377 class SpeechRecognitionProxy : public ClientProxy
02378 {
02379    void Subscribe(uint32_t aIndex);
02380    void Unsubscribe();
02381 
02383    playerc_speechrecognition_t *mDevice;
02384   public:
02386    SpeechRecognitionProxy(PlayerClient *aPc, uint32_t aIndex=0);
02387    ~SpeechRecognitionProxy();
02389    std::string GetWord(uint32_t aWord) const{
02390      scoped_lock_t lock(mPc->mMutex);
02391      return std::string(mDevice->words[aWord]);
02392    }
02393 
02395    uint32_t GetCount(void) const { return GetVar(mDevice->wordCount); }
02396 
02399    std::string operator [](uint32_t aWord) { return(GetWord(aWord)); }
02400 };
02401 
02405 class VectorMapProxy : public ClientProxy
02406 {
02407 
02408   private:
02409 
02410     // Subscribe
02411     void Subscribe(uint32_t aIndex);
02412     // Unsubscribe
02413     void Unsubscribe();
02414 
02415     // libplayerc data structure
02416     playerc_vectormap_t *mDevice;
02417 
02418     bool map_info_cached;
02419   public:
02420     // Constructor
02421     VectorMapProxy(PlayerClient *aPc, uint32_t aIndex=0);
02422     // Destructor
02423     ~VectorMapProxy();
02424 
02425     void GetMapInfo();
02426     void GetLayerData(unsigned layer_index);
02427 
02428     int GetLayerCount() const;
02429     std::vector<std::string> GetLayerNames() const;
02430     int GetFeatureCount(unsigned layer_index) const;
02431     GEOSGeom GetFeatureData(unsigned layer_index, unsigned feature_index) const;
02432 };
02433 
02436 class WiFiProxy: public ClientProxy
02437 {
02438 
02439   private:
02440 
02441     void Subscribe(uint32_t aIndex);
02442     void Unsubscribe();
02443 
02444     // libplayerc data structure
02445     playerc_wifi_t *mDevice;
02446 
02447   public:
02449     WiFiProxy(PlayerClient *aPc, uint32_t aIndex=0);
02451     ~WiFiProxy();
02452 
02453     const playerc_wifi_link_t *GetLink(int aLink);
02454 
02455                          int GetLinkCount() const { return mDevice->link_count; };
02456                          char* GetOwnIP() const { return mDevice->ip; };
02457 
02458                          char* GetLinkIP(int index)  const { return (char*) mDevice->links[index].ip; };
02459                          char* GetLinkMAC(int index)  const { return (char*) mDevice->links[index].mac; };
02460                          char* GetLinkESSID(int index)  const { return (char*)mDevice->links[index].essid; };
02461                          double GetLinkFreq(int index)  const {return mDevice->links[index].freq;};
02462                          int     GetLinkMode(int index)  const { return mDevice->links[index].mode; };
02463                          int     GetLinkEncrypt(int index)  const {return mDevice->links[index].encrypt; };
02464                          int   GetLinkQuality(int index)  const { return mDevice->links[index].qual; };
02465                          int     GetLinkLevel(int index)  const {return mDevice->links[index].level; };
02466                          int     GetLinkNoise(int index)  const {return mDevice->links[index].noise; }  ;                
02467 
02468                         //player_wifi_link_t
02469 //     int GetLinkQuality(char/// ip = NULL);
02470 //     int GetLevel(char/// ip = NULL);
02471 //     int GetLeveldBm(char/// ip = NULL) { return GetLevel(ip) - 0x100; }
02472 //     int GetNoise(char/// ip = NULL);
02473 //     int GetNoisedBm(char/// ip = NULL) { return GetNoise(ip) - 0x100; }
02474 //
02475 //     uint16_t GetMaxLinkQuality() { return maxqual; }
02476 //     uint8_t GetMode() { return op_mode; }
02477 //
02478 //     int GetBitrate();
02479 //
02480 //     char/// GetMAC(char *buf, int len);
02481 //
02482 //     char/// GetIP(char *buf, int len);
02483 //     char/// GetAP(char *buf, int len);
02484 //
02485 //     int AddSpyHost(char *address);
02486 //     int RemoveSpyHost(char *address);
02487 //
02488 //   private:
02489 //     int GetLinkIndex(char *ip);
02490 //
02491 //     /// The current wifi data.
02492 //     int link_count;
02493 //     player_wifi_link_t links[PLAYER_WIFI_MAX_LINKS];
02494 //     uint32_t throughput;
02495 //     uint8_t op_mode;
02496 //     int32_t bitrate;
02497 //     uint16_t qual_type, maxqual, maxlevel, maxnoise;
02498 //
02499 //     char access_point[32];
02500 };
02501 
02504 class WSNProxy : public ClientProxy
02505 {
02506 
02507   private:
02508 
02509     void Subscribe(uint32_t aIndex);
02510     void Unsubscribe();
02511 
02512     // libplayerc data structure
02513     playerc_wsn_t *mDevice;
02514 
02515   public:
02517     WSNProxy(PlayerClient *aPc, uint32_t aIndex=0);
02519     ~WSNProxy();
02520 
02521     uint32_t GetNodeType    () const { return GetVar(mDevice->node_type);      };
02522     uint32_t GetNodeID      () const { return GetVar(mDevice->node_id);        };
02523     uint32_t GetNodeParentID() const { return GetVar(mDevice->node_parent_id); };
02524 
02525     player_wsn_node_data_t
02526        GetNodeDataPacket() const { return GetVar(mDevice->data_packet);    };
02527 
02528     void SetDevState(int nodeID, int groupID, int devNr, int value);
02529     void Power(int nodeID, int groupID, int value);
02530     void DataType(int value);
02531     void DataFreq(int nodeID, int groupID, float frequency);
02532 };
02533 
02535 }
02536 
02537 namespace std
02538 {
02539   std::ostream& operator << (std::ostream& os, const player_point_2d_t& c);
02540   std::ostream& operator << (std::ostream& os, const player_pose2d_t& c);
02541   std::ostream& operator << (std::ostream& os, const player_pose3d_t& c);
02542   std::ostream& operator << (std::ostream& os, const player_bbox2d_t& c);
02543   std::ostream& operator << (std::ostream& os, const player_bbox3d_t& c);
02544   std::ostream& operator << (std::ostream& os, const player_segment_t& c);
02545   std::ostream& operator << (std::ostream& os, const player_extent2d_t& c);
02546   std::ostream& operator << (std::ostream& os, const playerc_device_info_t& c);
02547 
02548   std::ostream& operator << (std::ostream& os, const PlayerCc::ClientProxy& c);
02549   std::ostream& operator << (std::ostream& os, const PlayerCc::ActArrayProxy& c);
02550   std::ostream& operator << (std::ostream& os, const PlayerCc::AioProxy& c);
02551   std::ostream& operator << (std::ostream& os, const PlayerCc::AudioProxy& a);
02552   //std::ostream& operator << (std::ostream& os, const PlayerCc::BlinkenLightProxy& c);
02553   std::ostream& operator << (std::ostream& os, const PlayerCc::BlobfinderProxy& c);
02554   std::ostream& operator << (std::ostream& os, const PlayerCc::BumperProxy& c);
02555   std::ostream& operator << (std::ostream& os, const PlayerCc::CameraProxy& c);
02556   std::ostream& operator << (std::ostream& os, const PlayerCc::DioProxy& c);
02557   std::ostream& operator << (std::ostream& os, const PlayerCc::FiducialProxy& c);
02558   std::ostream& operator << (std::ostream& os, const PlayerCc::GpsProxy& c);
02559   std::ostream& operator << (std::ostream& os, const PlayerCc::GripperProxy& c);
02560   std::ostream& operator << (std::ostream& os, const PlayerCc::ImuProxy& c);
02561   std::ostream& operator << (std::ostream& os, const PlayerCc::IrProxy& c);
02562   std::ostream& operator << (std::ostream& os, const PlayerCc::LaserProxy& c);
02563   std::ostream& operator << (std::ostream& os, const PlayerCc::LimbProxy& c);
02564   std::ostream& operator << (std::ostream& os, const PlayerCc::LocalizeProxy& c);
02565   std::ostream& operator << (std::ostream& os, const PlayerCc::LogProxy& c);
02566   std::ostream& operator << (std::ostream& os, const PlayerCc::MapProxy& c);
02567   std::ostream& operator << (std::ostream& os, const PlayerCc::OpaqueProxy& c);
02568   std::ostream& operator << (std::ostream& os, const PlayerCc::PlannerProxy& c);
02569   std::ostream& operator << (std::ostream& os, const PlayerCc::Position1dProxy& c);
02570   std::ostream& operator << (std::ostream& os, const PlayerCc::Position2dProxy& c);
02571   std::ostream& operator << (std::ostream& os, const PlayerCc::Position3dProxy& c);
02572   std::ostream& operator << (std::ostream& os, const PlayerCc::PowerProxy& c);
02573   std::ostream& operator << (std::ostream& os, const PlayerCc::PtzProxy& c);
02574   std::ostream& operator << (std::ostream &os, const PlayerCc::RangerProxy &c);
02575   std::ostream& operator << (std::ostream& os, const PlayerCc::SimulationProxy& c);
02576   std::ostream& operator << (std::ostream& os, const PlayerCc::SonarProxy& c);
02577   std::ostream& operator << (std::ostream& os, const PlayerCc::SpeechProxy& c);
02578   std::ostream& operator << (std::ostream& os, const PlayerCc::SpeechRecognitionProxy& c);
02579   std::ostream& operator << (std::ostream& os, const PlayerCc::VectorMapProxy& c);
02580   //std::ostream& operator << (std::ostream& os, const PlayerCc::WafeformProxy& c);
02581   std::ostream& operator << (std::ostream& os, const PlayerCc::WiFiProxy& c);
02582   std::ostream& operator << (std::ostream& os, const PlayerCc::RFIDProxy& c);
02583   std::ostream& operator << (std::ostream& os, const PlayerCc::WSNProxy& c);
02584 }
02585 
02586 #endif
02587 

Last updated 12 September 2005 21:38:45