00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #ifndef PLAYERCC_H
00033 #define PLAYERCC_H
00034
00035 #include <cmath>
00036 #include <string>
00037 #include <list>
00038
00039 #include "libplayerc/playerc.h"
00040 #include "libplayerc++/utility.h"
00041 #include "libplayerc++/playerc++config.h"
00042 #include "libplayerc++/playerclient.h"
00043 #include "libplayerc++/playererror.h"
00044 #include "libplayerc++/clientproxy.h"
00045
00046 #ifdef HAVE_BOOST_SIGNALS
00047 #include <boost/signal.hpp>
00048 #include <boost/bind.hpp>
00049 #endif
00050
00051 #ifdef HAVE_BOOST_THREAD
00052 #include <boost/thread/mutex.hpp>
00053 #include <boost/thread/thread.hpp>
00054 #include <boost/thread/xtime.hpp>
00055 #endif
00056
00057 namespace PlayerCc
00058 {
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00096
00097
00098
00099
00100
00101
00106 class ActArrayProxy : public ClientProxy
00107 {
00108 private:
00109
00110 void Subscribe(uint aIndex);
00111 void Unsubscribe();
00112
00113
00114 playerc_actarray_t *mDevice;
00115
00116 public:
00117
00119 ActArrayProxy(PlayerClient *aPc, uint aIndex=0);
00121 ~ActArrayProxy();
00122
00125 void RequestGeometry(void);
00126
00128 void SetPowerConfig(bool aVal);
00130 void SetBrakesConfig(bool aVal);
00132 void SetSpeedConfig(uint aJoint, float aSpeed);
00133
00135 void MoveTo(uint aJoint, float aPos);
00137 void MoveAtSpeed(uint aJoint, float aSpeed);
00139 void MoveHome(int aJoint);
00140
00142 uint GetCount(void) const { return GetVar(mDevice->actuators_count); }
00144 player_actarray_actuator_t GetActuatorData(uint aJoint) const;
00146 player_actarray_actuatorgeom_t GetActuatorGeom(uint aJoint) const;
00147
00152 player_actarray_actuator_t operator [](uint aJoint)
00153 { return(GetActuatorData(aJoint)); }
00154 };
00155
00159 class AioProxy : public ClientProxy
00160 {
00161 private:
00162
00163 void Subscribe(uint aIndex);
00164 void Unsubscribe();
00165
00166
00167 playerc_aio_t *mDevice;
00168
00169 public:
00170
00171 AioProxy (PlayerClient *aPc, uint aIndex=0);
00172 ~AioProxy();
00173
00175 uint GetCount() const { return(GetVar(mDevice->voltages_count)); };
00176
00178 double GetVoltage(uint aIndex) const
00179 { return(GetVar(mDevice->voltages[aIndex])); };
00180
00182 void SetVoltage(uint aIndex, double aVoltage);
00183
00188 double operator [](uint aIndex) const
00189 { return GetVoltage(aIndex); }
00190
00191 };
00192
00193 #if 0 // Not in libplayerc
00194
00198 class AudioProxy : public ClientProxy
00199 {
00200
00201 private:
00202
00203 void Subscribe(uint aIndex);
00204 void Unsubscribe();
00205
00206
00207 playerc_audio_t *mDevice;
00208
00209 public:
00210
00211 AudioProxy(PlayerClient *aPc, uint aIndex=0)
00212 ~AudioProxy();
00213
00214 uint GetCount() const { return(GetVar(mDevice->count)); };
00215
00216 double GetFrequency(uint aIndex) const
00217 { return(GetVar(mDevice->frequency[aIndex])); };
00218 double GetAmplitude(uint aIndex) const
00219 { return(GetVar(mDevice->amplitude[aIndex])); };
00220
00221
00222 void PlayTone(uint aFreq, uint aAmp, uint aDur);
00223 };
00224
00230 class AudioDspProxy : public ClientProxy
00231 {
00232 private:
00233
00234 void Subscribe(uint aIndex);
00235 void Unsubscribe();
00236
00237
00238 playerc_audiodsp_t *mDevice;
00239
00240 public:
00241 AudioDspProxy(PlayerClient *aPc, uint aIndex=0);
00242 ~AudioDspProxy
00243
00245 uint SetFormat(int aFormat);
00246
00248 uint SetRate(uint aRate);
00249
00250 uint GetCount() const { return(GetVar(mDevice->count)); };
00251
00252 double GetFrequency(uint aIndex) const
00253 { return(GetVar(mDevice->frequency[aIndex])); };
00254 double GetAmplitude(uint aIndex) const
00255 { return(GetVar(mDevice->amplitude[aIndex])); };
00256
00257 void Configure(uint aChan, uint aRate, int16_t aFormat=0xFFFFFFFF);
00258
00259 void RequestConfigure();
00260
00262 void PlayTone(uint aFreq, uint aAmp, uint aDur);
00263 void PlayChirp(uint aFreq, uint aAmp, uint aDur,
00264 const uint8_t aBitString, uint aBitStringLen);
00265 void Replay();
00266
00268 void Print ();
00269 };
00270
00274 class AudioMixerProxy : public ClientProxy
00275 {
00276 private:
00277
00278 void Subscribe(uint aIndex);
00279 void Unsubscribe();
00280
00281
00282 playerc_audiodsp_t *mDevice;
00283
00284 public:
00285
00286 AudioMixerProxy (PlayerClient *aPc, uint aIndex=0);
00287
00288 void GetConfigure();
00289
00290 void SetMaster(uint aLeft, uint aRight);
00291 void SetPCM(uint aLeft, uint aRight);
00292 void SetLine(uint aLeft, uint aRight);
00293 void SetMic(uint aLeft, uint aRight);
00294 void SetIGain(uint aGain);
00295 void SetOGain(uint aGain);
00296
00297 };
00298
00303 class BlinkenLightProxy : public ClientProxy
00304 {
00305 private:
00306
00307 void Subscribe(uint aIndex);
00308 void Unsubscribe();
00309
00310
00311 playerc_blinkenlight_t *mDevice;
00312
00313 public:
00317 BlinkenLightProxy(PlayerClient *aPc, uint aIndex=0);
00318 ~BlinkenLightProxy();
00319
00320
00321 bool GetEnable();
00322
00327 void SetPeriod(double aPeriod);
00328
00333 void SetEnable(bool aSet);
00334 };
00335
00336 #endif
00337
00344 class BlobfinderProxy : public ClientProxy
00345 {
00346 private:
00347
00348 void Subscribe(uint aIndex);
00349 void Unsubscribe();
00350
00351
00352 playerc_blobfinder_t *mDevice;
00353
00354 public:
00356 BlobfinderProxy(PlayerClient *aPc, uint aIndex=0);
00358 ~BlobfinderProxy();
00359
00361 uint GetCount() const { return GetVar(mDevice->blobs_count); };
00363 playerc_blobfinder_blob_t GetBlob(uint aIndex) const
00364 { return GetVar(mDevice->blobs[aIndex]);};
00365
00367 uint GetWidth() const { return GetVar(mDevice->width); };
00369 uint GetHeight() const { return GetVar(mDevice->height); };
00370
00375 playerc_blobfinder_blob_t operator [](uint aIndex) const
00376 { return(GetBlob(aIndex)); }
00377
00378
00380
00381
00382
00383
00384
00385
00386
00387
00388
00389 };
00390
00395 class BumperProxy : public ClientProxy
00396 {
00397
00398 private:
00399
00400 void Subscribe(uint aIndex);
00401 void Unsubscribe();
00402
00403
00404 playerc_bumper_t *mDevice;
00405
00406 public:
00407
00408 BumperProxy(PlayerClient *aPc, uint aIndex=0);
00409 ~BumperProxy();
00410
00411 uint GetCount() const { return GetVar(mDevice->bumper_count); };
00412
00414 uint IsBumped(uint aIndex) const
00415 { return GetVar(mDevice->bumpers[aIndex]); };
00416
00418 bool IsAnyBumped();
00419
00421 void RequestBumperConfig();
00422
00424 uint GetPoseCount() const { return GetVar(mDevice->pose_count); };
00425
00427 player_bumper_define_t GetPose(uint aIndex) const
00428 { return GetVar(mDevice->poses[aIndex]); };
00429
00434 bool operator [](uint aIndex) const
00435 { return IsBumped(aIndex); }
00436
00437 };
00438
00442 class CameraProxy : public ClientProxy
00443 {
00444
00445 private:
00446
00447 virtual void Subscribe(uint aIndex);
00448 virtual void Unsubscribe();
00449
00450
00451 playerc_camera_t *mDevice;
00452
00453 std::string mPrefix;
00454 int mFrameNo;
00455
00456 public:
00457
00459 CameraProxy (PlayerClient *aPc, uint aIndex=0);
00460
00461 virtual ~CameraProxy();
00462
00466 void SaveFrame(const std::string aPrefix, uint aWidth=4);
00467
00469 void Decompress();
00470
00472 uint GetDepth() const { return GetVar(mDevice->bpp); };
00473
00475 uint GetWidth() const { return GetVar(mDevice->width); };
00476
00478 uint GetHeight() const { return GetVar(mDevice->height); };
00479
00486 uint GetFormat() const { return GetVar(mDevice->format); };
00487
00489 uint GetImageSize() const { return GetVar(mDevice->image_count); };
00490
00495 void GetImage(uint8_t* aImage) const
00496 {
00497 return GetVarByRef(mDevice->image,
00498 mDevice->image+GetVar(mDevice->image_count),
00499 aImage);
00500 };
00501
00506 uint GetCompression() const { return GetVar(mDevice->compression); };
00507
00508 };
00509
00510
00515 class DioProxy : public ClientProxy
00516 {
00517 private:
00518
00519 void Subscribe(uint aIndex);
00520 void Unsubscribe();
00521
00522
00523 playerc_dio_t *mDevice;
00524
00525 public:
00527 DioProxy(PlayerClient *aPc, uint aIndex=0);
00529 ~DioProxy();
00530
00532 uint GetCount() const { return GetVar(mDevice->count); };
00533
00535 uint32_t GetDigin() const { return GetVar(mDevice->digin); };
00536
00538 bool GetInput(uint aIndex) const;
00539
00541 void SetOutput(uint aCount, uint32_t aDigout);
00542
00547 uint operator [](uint aIndex) const
00548 { return GetInput(aIndex); }
00549 };
00550
00551
00552
00553
00554
00555
00556
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570
00571
00572
00573
00574
00575
00576
00577
00583 class FiducialProxy : public ClientProxy
00584 {
00585 private:
00586 void Subscribe(uint aIndex);
00587 void Unsubscribe();
00588
00589
00590 playerc_fiducial_t *mDevice;
00591
00592 public:
00594 FiducialProxy(PlayerClient *aPc, uint aIndex=0);
00596 ~FiducialProxy();
00597
00599 uint GetCount() const { return GetVar(mDevice->fiducials_count); };
00600
00602 player_fiducial_item_t GetFiducialItem(uint aIndex) const
00603 { return GetVar(mDevice->fiducials[aIndex]);};
00604
00606 player_pose_t GetSensorPose() const
00607 { return GetVar(mDevice->fiducial_geom.pose);};
00608
00610 player_bbox_t GetSensorSize() const
00611 { return GetVar(mDevice->fiducial_geom.size);};
00612
00614 player_bbox_t GetFiducialSize() const
00615 { return GetVar(mDevice->fiducial_geom.fiducial_size);};
00616
00618 void RequestGeometry();
00619
00624 player_fiducial_item_t operator [](uint aIndex) const
00625 { return GetFiducialItem(aIndex); }
00626 };
00627
00631 class GpsProxy : public ClientProxy
00632 {
00633
00634 private:
00635
00636 void Subscribe(uint aIndex);
00637 void Unsubscribe();
00638
00639
00640 playerc_gps_t *mDevice;
00641
00642 public:
00643
00644
00645 GpsProxy(PlayerClient *aPc, uint aIndex=0);
00646
00647 ~GpsProxy();
00648
00650 double GetLatitude() const { return GetVar(mDevice->lat); };
00651 double GetLongitude() const { return GetVar(mDevice->lon); };
00652
00654 double GetAltitude() const { return GetVar(mDevice->alt); };
00655
00657 uint GetSatellites() const { return GetVar(mDevice->sat_count); };
00658
00660 uint GetQuality() const { return GetVar(mDevice->quality); };
00661
00663 double GetHdop() const { return GetVar(mDevice->hdop); };
00664
00666 double GetVdop() const { return GetVar(mDevice->vdop); };
00667
00669 double GetUtmEasting() const { return GetVar(mDevice->utm_e); };
00670 double GetUtmNorthing() const { return GetVar(mDevice->utm_n); };
00671
00673 double GetTime() const { return GetVar(mDevice->utc_time); };
00674
00676 double GetErrHorizontal() const { return GetVar(mDevice->err_horz); };
00677 double GetErrVertical() const { return GetVar(mDevice->err_vert); };
00678 };
00679
00687 class Graphics2dProxy : public ClientProxy
00688 {
00689
00690 private:
00691
00692
00693 void Subscribe(uint aIndex);
00694
00695 void Unsubscribe();
00696
00697
00698 playerc_graphics2d_t *mDevice;
00699
00700 public:
00701
00702 Graphics2dProxy(PlayerClient *aPc, uint aIndex=0);
00703
00704 ~Graphics2dProxy();
00705
00707 void Color(player_color_t col);
00708
00710 void Color(uint8_t red, uint8_t green, uint8_t blue, uint8_t alpha);
00711
00713 void Clear(void);
00714
00716 void DrawPoints(player_point_2d_t pts[], int count);
00717
00719 void DrawPolygon(player_point_2d_t pts[],
00720 int count,
00721 bool filled,
00722 player_color_t fill_color);
00723
00725 void DrawPolyline(player_point_2d_t pts[], int count);
00726 };
00727
00733 class Graphics3dProxy : public ClientProxy
00734 {
00735
00736 private:
00737
00738
00739 void Subscribe(uint aIndex);
00740
00741 void Unsubscribe();
00742
00743
00744 playerc_graphics3d_t *mDevice;
00745
00746 public:
00747
00748 Graphics3dProxy(PlayerClient *aPc, uint aIndex=0);
00749
00750 ~Graphics3dProxy();
00751
00753 void Color(player_color_t col);
00754
00756 void Color(uint8_t red, uint8_t green, uint8_t blue, uint8_t alpha);
00757
00759 void Clear(void);
00760
00762 void Draw(player_graphics3d_draw_mode_t mode, player_point_3d_t pts[], int count);
00763
00764 };
00765
00771 class GripperProxy : public ClientProxy
00772 {
00773
00774 private:
00775
00776 void Subscribe(uint aIndex);
00777 void Unsubscribe();
00778
00779
00780 playerc_gripper_t *mDevice;
00781
00782 public:
00783
00785 GripperProxy(PlayerClient *aPc, uint aIndex=0);
00787 ~GripperProxy();
00788
00790 uint GetState() const { return GetVar(mDevice->state); };
00792 uint GetBeams() const { return GetVar(mDevice->beams); };
00794 uint GetOuterBreakBeam() const
00795 { return GetVar(mDevice->outer_break_beam); };
00797 uint GetInnerBreakBeam() const
00798 { return GetVar(mDevice->inner_break_beam); };
00800 uint GetPaddlesOpen() const
00801 { return GetVar(mDevice->paddles_open); };
00803 uint GetPaddlesClosed() const
00804 { return GetVar(mDevice->paddles_closed); };
00806 uint GetPaddlesMoving() const
00807 { return GetVar(mDevice->paddles_moving); };
00809 uint GetGripperError() const
00810 { return GetVar(mDevice->gripper_error); };
00812 uint GetLiftUp() const { return GetVar(mDevice->lift_up); };
00814 uint GetLiftDown() const { return GetVar(mDevice->lift_down); };
00816 uint GetLiftMoving() const { return GetVar(mDevice->lift_moving); };
00818 uint GetLiftError() const { return GetVar(mDevice->lift_error); };
00819
00822 void SetGrip(uint8_t aCmd, uint8_t aArg=0);
00823 };
00824
00825
00826
00831 class IrProxy : public ClientProxy
00832 {
00833
00834 private:
00835
00836 void Subscribe(uint aIndex);
00837 void Unsubscribe();
00838
00839
00840 playerc_ir_t *mDevice;
00841
00842 public:
00843
00845 IrProxy(PlayerClient *aPc, uint aIndex=0);
00847 ~IrProxy();
00848
00850 uint GetCount() const { return GetVar(mDevice->ranges.ranges_count); };
00852 double GetRange(uint aIndex) const
00853 { return GetVar(mDevice->ranges.ranges[aIndex]); };
00855 double GetVoltage(uint aIndex) const
00856 { return GetVar(mDevice->ranges.voltages[aIndex]); };
00858 uint GetPoseCount() const { return GetVar(mDevice->poses.poses_count); };
00860 player_pose_t GetPose(uint aIndex) const
00861 {return GetVar(mDevice->poses.poses[aIndex]);};
00862
00864 void RequestGeom();
00865
00870 double operator [](uint aIndex) const
00871 { return GetRange(aIndex); }
00872
00873 };
00874
00880 class LaserProxy : public ClientProxy
00881 {
00882 private:
00883
00884 void Subscribe(uint aIndex);
00885 void Unsubscribe();
00886
00887
00888 playerc_laser_t *mDevice;
00889
00890 double aMinLeft;
00891 double aMinRight;
00892
00893
00894 double min_angle, max_angle, scan_res, range_res;
00895 bool intensity;
00896
00897 public:
00898
00900 LaserProxy(PlayerClient *aPc, uint aIndex=0);
00902 ~LaserProxy();
00903
00905 uint GetCount() const { return GetVar(mDevice->scan_count); };
00906
00908 double GetMaxRange() const { return GetVar(mDevice->max_range); };
00909
00911 double GetScanRes() const { return GetVar(mDevice->scan_res); };
00912
00914 double GetRangeRes() const { return GetVar(mDevice->range_res); };
00915
00916
00918 double GetMinAngle() const { return GetVar(mDevice->scan_start); };
00920 double GetMaxAngle() const
00921 {
00922 scoped_lock_t lock(mPc->mMutex);
00923 return mDevice->scan_start + mDevice->scan_count*mDevice->scan_res;
00924 };
00925
00927 bool IntensityOn() const { return GetVar(mDevice->intensity_on); };
00928
00929
00930
00931
00932
00934 player_point_2d_t GetPoint(uint aIndex) const
00935 { return GetVar(mDevice->point[aIndex]); };
00936
00937
00939 double GetRange(uint aIndex) const
00940 { return GetVar(mDevice->ranges[aIndex]); };
00941
00943 double GetBearing(uint aIndex) const
00944 { return GetVar(mDevice->scan[aIndex][1]); };
00945
00946
00948 int GetIntensity(uint aIndex) const
00949 { return GetVar(mDevice->intensity[aIndex]); };
00950
00958 void Configure(double aMinAngle,
00959 double aMaxAngle,
00960 uint aScanRes,
00961 uint aRangeRes,
00962 bool aIntensity);
00963
00966 void RequestConfigure();
00967
00970 void RequestGeom();
00971
00974 player_pose_t GetPose()
00975 {
00976 player_pose_t p;
00977 scoped_lock_t lock(mPc->mMutex);
00978
00979 p.px = mDevice->pose[0];
00980 p.py = mDevice->pose[1];
00981 p.pa = mDevice->pose[2];
00982 return(p);
00983 }
00984
00987 player_pose_t GetRobotPose()
00988 {
00989 player_pose_t p;
00990 scoped_lock_t lock(mPc->mMutex);
00991
00992 p.px = mDevice->robot_pose[0];
00993 p.py = mDevice->robot_pose[1];
00994 p.pa = mDevice->robot_pose[2];
00995 return(p);
00996 }
00997
00999 player_bbox_t GetSize()
01000 {
01001 player_bbox_t b;
01002 scoped_lock_t lock(mPc->mMutex);
01003
01004 b.sl = mDevice->size[0];
01005 b.sw = mDevice->size[1];
01006 return(b);
01007 }
01008
01010 double MinLeft () { return aMinLeft; }
01012 double MinRight () { return aMinRight; }
01013
01019 double operator [] (uint index) const
01020 { return GetRange(index);}
01021
01022 };
01023
01024
01029 class LimbProxy : public ClientProxy
01030 {
01031 private:
01032
01033 void Subscribe(uint aIndex);
01034 void Unsubscribe();
01035
01036
01037 playerc_limb_t *mDevice;
01038
01039 public:
01040
01041 LimbProxy(PlayerClient *aPc, uint aIndex=0);
01042 ~LimbProxy();
01043
01046 void RequestGeometry(void);
01047
01049 void SetPowerConfig(bool aVal);
01051 void SetBrakesConfig(bool aVal);
01053 void SetSpeedConfig(float aSpeed);
01054
01056 void MoveHome(void);
01058 void Stop(void);
01060 void SetPose(float aPX, float aPY, float aPZ,
01061 float aAX, float aAY, float aAZ,
01062 float aOX, float aOY, float aOZ);
01064 void SetPosition(float aX, float aY, float aZ);
01067 void VectorMove(float aX, float aY, float aZ, float aLength);
01068
01070 player_limb_data_t GetData(void) const;
01072 player_limb_geom_req_t GetGeom(void) const;
01073 };
01074
01075
01076
01082 class LocalizeProxy : public ClientProxy
01083 {
01084
01085 private:
01086
01087 void Subscribe(uint aIndex);
01088 void Unsubscribe();
01089
01090
01091 playerc_localize_t *mDevice;
01092
01093 public:
01094
01096 LocalizeProxy(PlayerClient *aPc, uint aIndex=0);
01098 ~LocalizeProxy();
01099
01101
01102 uint GetMapSizeX() const { return GetVar(mDevice->map_size_x); };
01103 uint GetMapSizeY() const { return GetVar(mDevice->map_size_y); };
01104
01105
01106 uint GetMapTileX() const { return GetVar(mDevice->map_tile_x); };
01107 uint GetMapTileY() const { return GetVar(mDevice->map_tile_y); };
01108
01110 double GetMapScale() const { return GetVar(mDevice->map_scale); };
01111
01112
01113
01114
01115
01116
01117
01118
01119
01120
01122 uint GetPendingCount() const { return GetVar(mDevice->pending_count); };
01123
01125 uint GetHypothCount() const { return GetVar(mDevice->hypoth_count); };
01126
01128 player_localize_hypoth_t GetHypoth(uint aIndex) const
01129 { return GetVar(mDevice->hypoths[aIndex]); };
01130
01132 int GetParticles()
01133 { return playerc_localize_get_particles(mDevice); }
01134
01136 player_pose_t GetParticlePose(int index) const;
01137
01139 void SetPose(double pose[3], double cov[3]);
01140
01142 uint GetNumHypoths() const { return GetVar(mDevice->hypoth_count); };
01143
01146 uint GetNumParticles() const { return GetVar(mDevice->num_particles); };
01147 };
01148
01149
01153 class LogProxy : public ClientProxy
01154 {
01155 private:
01156
01157 void Subscribe(uint aIndex);
01158 void Unsubscribe();
01159
01160
01161 playerc_log_t *mDevice;
01162
01163 public:
01165 LogProxy(PlayerClient *aPc, uint aIndex=0);
01166
01168 ~LogProxy();
01169
01172 int GetType() const { return GetVar(mDevice->type); };
01173
01175 int GetState() const { return GetVar(mDevice->state); };
01176
01178 void SetWriteState(int aState);
01179
01181 void SetReadState(int aState);
01182
01184 void Rewind();
01185
01187 void SetFilename(const std::string aFilename);
01188 };
01189
01193 class MapProxy : public ClientProxy
01194 {
01195 private:
01196
01197 void Subscribe(uint aIndex);
01198 void Unsubscribe();
01199
01200
01201 playerc_map_t *mDevice;
01202
01203 public:
01205 MapProxy(PlayerClient *aPc, uint aIndex=0);
01206
01208 ~MapProxy();
01209
01211 void RequestMap();
01212
01214 int GetCellIndex(int x, int y) const
01215 { return y*GetWidth() + x; };
01216
01218 unsigned char GetCell(int x, int y) const
01219 { return GetVar(mDevice->cells[GetCellIndex(x,y)]); };
01220
01222 double GetResolution() const { return GetVar(mDevice->resolution); };
01223
01226 uint GetWidth() const { return GetVar(mDevice->width); };
01229 uint GetHeight() const { return GetVar(mDevice->height); };
01230
01231 double GetOriginX() const { return GetVar(mDevice->origin[0]); };
01232 double GetOriginY() const { return GetVar(mDevice->origin[1]); };
01233
01235 void GetMap(int8_t* aMap) const
01236 {
01237 return GetVarByRef(reinterpret_cast<int8_t*>(mDevice->cells),
01238 reinterpret_cast<int8_t*>(mDevice->cells+GetWidth()*GetHeight()),
01239 aMap);
01240 };
01241 };
01242
01248 class OpaqueProxy : public ClientProxy
01249 {
01250
01251 private:
01252
01253 void Subscribe(uint aIndex);
01254 void Unsubscribe();
01255
01256
01257 playerc_opaque_t *mDevice;
01258
01259 public:
01260
01262 OpaqueProxy(PlayerClient *aPc, uint aIndex=0);
01264 ~OpaqueProxy();
01265
01267 uint GetCount() const { return GetVar(mDevice->data_count); };
01268
01270 void GetData(uint8_t* aDest) const
01271 {
01272 return GetVarByRef(mDevice->data,
01273 mDevice->data+GetVar(mDevice->data_count),
01274 aDest);
01275 };
01276
01278 void SendCmd(player_opaque_data_t* aData);
01279
01280 };
01281
01285 class PlannerProxy : public ClientProxy
01286 {
01287
01288 private:
01289
01290 void Subscribe(uint aIndex);
01291 void Unsubscribe();
01292
01293
01294 playerc_planner_t *mDevice;
01295
01296 public:
01297
01299 PlannerProxy(PlayerClient *aPc, uint aIndex=0);
01301 ~PlannerProxy();
01302
01304 void SetGoalPose(double aGx, double aGy, double aGa);
01305
01308 void RequestWaypoints();
01309
01312 void SetEnable(bool aEnable);
01313
01315 uint GetPathValid() const { return GetVar(mDevice->path_valid); };
01316
01318 uint GetPathDone() const { return GetVar(mDevice->path_done); };
01319
01322 double GetPx() const { return GetVar(mDevice->px); };
01325 double GetPy() const { return GetVar(mDevice->py); };
01328 double GetPa() const { return GetVar(mDevice->pa); };
01329
01331 player_pose_t GetPose() const
01332 {
01333 player_pose_t p;
01334 scoped_lock_t lock(mPc->mMutex);
01335 p.px = mDevice->px;
01336 p.py = mDevice->py;
01337 p.pa = mDevice->pa;
01338 return(p);
01339 }
01340
01343 double GetGx() const { return GetVar(mDevice->gx); };
01346 double GetGy() const { return GetVar(mDevice->gy); };
01349 double GetGa() const { return GetVar(mDevice->ga); };
01350
01352 player_pose_t GetGoal() const
01353 {
01354 player_pose_t p;
01355 scoped_lock_t lock(mPc->mMutex);
01356 p.px = mDevice->gx;
01357 p.py = mDevice->gy;
01358 p.pa = mDevice->ga;
01359 return(p);
01360 }
01361
01364 double GetWx() const { return GetVar(mDevice->wx); };
01367 double GetWy() const { return GetVar(mDevice->wy); };
01370 double GetWa() const { return GetVar(mDevice->wa); };
01371
01373 player_pose_t GetCurrentWaypoint() const
01374 {
01375 player_pose_t p;
01376 scoped_lock_t lock(mPc->mMutex);
01377 p.px = mDevice->wx;
01378 p.py = mDevice->wy;
01379 p.pa = mDevice->wa;
01380 return(p);
01381 }
01382
01385 double GetIx(int i) const;
01388 double GetIy(int i) const;
01391 double GetIa(int i) const;
01392
01394 player_pose_t GetWaypoint(uint aIndex) const
01395 {
01396 assert(aIndex < GetWaypointCount());
01397 player_pose_t p;
01398 scoped_lock_t lock(mPc->mMutex);
01399 p.px = mDevice->waypoints[aIndex][0];
01400 p.py = mDevice->waypoints[aIndex][1];
01401 p.pa = mDevice->waypoints[aIndex][2];
01402 return(p);
01403 }
01404
01408 int GetCurrentWaypointId() const
01409 { return GetVar(mDevice->curr_waypoint); };
01410
01412 uint GetWaypointCount() const
01413 { return GetVar(mDevice->waypoint_count); };
01414
01419 player_pose_t operator [](uint aIndex) const
01420 { return GetWaypoint(aIndex); }
01421
01422 };
01423
01428 class Position1dProxy : public ClientProxy
01429 {
01430
01431 private:
01432
01433 void Subscribe(uint aIndex);
01434 void Unsubscribe();
01435
01436
01437 playerc_position1d_t *mDevice;
01438
01439 public:
01440
01442 Position1dProxy(PlayerClient *aPc, uint aIndex=0);
01444 ~Position1dProxy();
01445
01449 void SetSpeed(double aVel);
01450
01454 void GoTo(double aPos, double aVel);
01455
01458 void RequestGeom();
01459
01461 player_pose_t GetPose() const
01462 {
01463 player_pose_t p;
01464 scoped_lock_t lock(mPc->mMutex);
01465 p.px = mDevice->pose[0];
01466 p.py = mDevice->pose[1];
01467 p.pa = mDevice->pose[2];
01468 return(p);
01469 }
01470
01472 player_bbox_t GetSize() const
01473 {
01474 player_bbox_t b;
01475 scoped_lock_t lock(mPc->mMutex);
01476 b.sl = mDevice->size[0];
01477 b.sw = mDevice->size[1];
01478 return(b);
01479 }
01480
01485 void SetMotorEnable(bool enable);
01486
01489 void SetOdometry(double aPos);
01490
01492 void ResetOdometry() { SetOdometry(0); };
01493
01495
01496
01498
01499
01502
01503
01505 double GetPos() const { return GetVar(mDevice->pos); };
01506
01508 double GetVel() const { return GetVar(mDevice->vel); };
01509
01511 bool GetStall() const { return GetVar(mDevice->stall); };
01512
01514 uint8_t GetStatus() const { return GetVar(mDevice->status); };
01515
01517 bool IsLimitMin() const
01518 { return (GetVar(mDevice->status) &
01519 (1 << PLAYER_POSITION1D_STATUS_LIMIT_MIN)) > 0; };
01520
01522 bool IsLimitCen() const
01523 { return (GetVar(mDevice->status) &
01524 (1 << PLAYER_POSITION1D_STATUS_LIMIT_CEN)) > 0; };
01525
01527 bool IsLimitMax() const
01528 { return (GetVar(mDevice->status) &
01529 (1 << PLAYER_POSITION1D_STATUS_LIMIT_MAX)) > 0; };
01530
01532 bool IsOverCurrent() const
01533 { return (GetVar(mDevice->status) &
01534 (1 << PLAYER_POSITION1D_STATUS_OC)) > 0; };
01535
01537 bool IsTrajComplete() const
01538 { return (GetVar(mDevice->status) &
01539 (1 << PLAYER_POSITION1D_STATUS_TRAJ_COMPLETE)) > 0; };
01540
01542 bool IsEnabled() const
01543 { return (GetVar(mDevice->status) &
01544 (1 << PLAYER_POSITION1D_STATUS_ENABLED)) > 0; };
01545
01546 };
01547
01552 class Position2dProxy : public ClientProxy
01553 {
01554
01555 private:
01556
01557 void Subscribe(uint aIndex);
01558 void Unsubscribe();
01559
01560
01561 playerc_position2d_t *mDevice;
01562
01563 public:
01564
01566 Position2dProxy(PlayerClient *aPc, uint aIndex=0);
01568 ~Position2dProxy();
01569
01573 void SetSpeed(double aXSpeed, double aYSpeed, double aYawSpeed);
01574
01577 void SetSpeed(double aXSpeed, double aYawSpeed)
01578 { return SetSpeed(aXSpeed, 0, aYawSpeed);}
01579
01581 void SetSpeed(player_pose_t vel)
01582 { return SetSpeed(vel.px, vel.py, vel.pa);}
01583
01587 void GoTo(player_pose_t pos, player_pose_t vel);
01588
01590 void GoTo(player_pose_t pos)
01591 {GoTo(pos,(player_pose_t) {0,0,0}); }
01592
01595 void GoTo(double aX, double aY, double aYaw)
01596 {GoTo((player_pose_t) {aX,aY,aYaw},(player_pose_t) {0,0,0}); }
01597
01599 void SetCarlike(double aXSpeed, double aDriveAngle);
01600
01603 void RequestGeom();
01604
01606 player_pose_t GetPose()
01607 {
01608 player_pose_t p;
01609 scoped_lock_t lock(mPc->mMutex);
01610 p.px = mDevice->pose[0];
01611 p.py = mDevice->pose[1];
01612 p.pa = mDevice->pose[2];
01613 return(p);
01614 }
01615
01617 player_bbox_t GetSize()
01618 {
01619 player_bbox_t b;
01620 scoped_lock_t lock(mPc->mMutex);
01621 b.sl = mDevice->size[0];
01622 b.sw = mDevice->size[1];
01623 return(b);
01624 }
01625
01630 void SetMotorEnable(bool enable);
01631
01632
01633
01634
01635
01636
01637
01638
01639
01640
01641
01643 void ResetOdometry();
01644
01647
01648
01651 void SetOdometry(double aX, double aY, double aYaw);
01652
01654
01655
01657
01658
01661
01662
01663
01664
01665
01666
01667
01668
01669
01670
01671
01672
01673
01674
01675
01676
01677
01679 double GetXPos() const { return GetVar(mDevice->px); };
01680
01682 double GetYPos() const { return GetVar(mDevice->py); };
01683
01685 double GetYaw() const { return GetVar(mDevice->pa); };
01686
01688 double GetXSpeed() const { return GetVar(mDevice->vx); };
01689
01691 double GetYSpeed() const { return GetVar(mDevice->vy); };
01692
01694 double GetYawSpeed() const { return GetVar(mDevice->va); };
01695
01697 bool GetStall() const { return GetVar(mDevice->stall); };
01698
01699 };
01700
01707 class Position3dProxy : public ClientProxy
01708 {
01709
01710 private:
01711
01712 void Subscribe(uint aIndex);
01713 void Unsubscribe();
01714
01715
01716 playerc_position3d_t *mDevice;
01717
01718 public:
01719
01721 Position3dProxy(PlayerClient *aPc, uint aIndex=0);
01723 ~Position3dProxy();
01724
01728 void SetSpeed(double aXSpeed, double aYSpeed, double aZSpeed,
01729 double aRollSpeed, double aPitchSpeed, double aYawSpeed);
01730
01734 void SetSpeed(double aXSpeed, double aYSpeed,
01735 double aZSpeed, double aYawSpeed)
01736 { SetSpeed(aXSpeed,aYSpeed,aZSpeed,0,0,aYawSpeed); }
01737
01739 void SetSpeed(double aXSpeed, double aYSpeed, double aYawSpeed)
01740 { SetSpeed(aXSpeed, aYSpeed, 0, 0, 0, aYawSpeed); }
01741
01744 void SetSpeed(double aXSpeed, double aYawSpeed)
01745 { SetSpeed(aXSpeed,0,0,0,0,aYawSpeed);}
01746
01748 void SetSpeed(player_pose3d_t vel)
01749 { SetSpeed(vel.px,vel.py,vel.pz,vel.proll,vel.ppitch,vel.pyaw);}
01750
01751
01755 void GoTo(player_pose3d_t aPos, player_pose3d_t aVel);
01756
01758 void GoTo(player_pose3d_t aPos)
01759 { GoTo(aPos, (player_pose3d_t) {0,0,0,0,0,0}); }
01760
01761
01764 void GoTo(double aX, double aY, double aZ,
01765 double aRoll, double aPitch, double aYaw)
01766 { GoTo((player_pose3d_t) {aX,aY,aZ,aRoll,aPitch,aYaw},
01767 (player_pose3d_t) {0,0,0,0,0,0});
01768 }
01769
01774 void SetMotorEnable(bool aEnable);
01775
01778 void SelectVelocityControl(int aMode);
01779
01781 void ResetOdometry();
01782
01786 void SetOdometry(double aX, double aY, double aZ,
01787 double aRoll, double aPitch, double aYaw);
01788
01789
01790
01791
01792
01793
01794
01795
01796
01797
01798
01799
01800
01801
01802
01804 double GetXPos() const { return GetVar(mDevice->pos_x); };
01805
01807 double GetYPos() const { return GetVar(mDevice->pos_y); };
01808
01810 double GetZPos() const { return GetVar(mDevice->pos_z); };
01811
01813 double GetRoll() const { return GetVar(mDevice->pos_roll); };
01814
01816 double GetPitch() const { return GetVar(mDevice->pos_pitch); };
01817
01819 double GetYaw() const { return GetVar(mDevice->pos_yaw); };
01820
01822 double GetXSpeed() const { return GetVar(mDevice->vel_x); };
01823
01825 double GetYSpeed() const { return GetVar(mDevice->vel_y); };
01826
01828 double GetZSpeed() const { return GetVar(mDevice->vel_z); };
01829
01831 double GetRollSpeed() const { return GetVar(mDevice->vel_roll); };
01832
01834 double GetPitchSpeed() const { return GetVar(mDevice->vel_pitch); };
01835
01837 double GetYawSpeed() const { return GetVar(mDevice->vel_yaw); };
01838
01840 bool GetStall () const { return GetVar(mDevice->stall); };
01841 };
01844 class PowerProxy : public ClientProxy
01845 {
01846 private:
01847
01848 void Subscribe(uint aIndex);
01849 void Unsubscribe();
01850
01851
01852 playerc_power_t *mDevice;
01853
01854 public:
01856 PowerProxy(PlayerClient *aPc, uint aIndex=0);
01858 ~PowerProxy();
01859
01861 double GetCharge() const { return GetVar(mDevice->charge); };
01862
01863 };
01864
01871 class PtzProxy : public ClientProxy
01872 {
01873
01874 private:
01875
01876 void Subscribe(uint aIndex);
01877 void Unsubscribe();
01878
01879
01880 playerc_ptz_t *mDevice;
01881
01882 public:
01884 PtzProxy(PlayerClient *aPc, uint aIndex=0);
01885
01886 ~PtzProxy();
01887
01888 public:
01889
01893 void SetCam(double aPan, double aTilt, double aZoom);
01894
01896 void SetSpeed(double aPanSpeed=0, double aTiltSpeed=0, double aZoomSpeed=0);
01897
01900 void SelectControlMode(uint aMode);
01901
01903 double GetPan() const { return GetVar(mDevice->pan); };
01905 double GetTilt() const { return GetVar(mDevice->tilt); };
01907 double GetZoom() const { return GetVar(mDevice->zoom); };
01908
01909 };
01910
01913 class RFIDProxy : public ClientProxy
01914 {
01915
01916 private:
01917
01918 void Subscribe(uint aIndex);
01919 void Unsubscribe();
01920
01921
01922 playerc_rfid_t *mDevice;
01923
01924 public:
01926 RFIDProxy(PlayerClient *aPc, uint aIndex=0);
01928 ~RFIDProxy();
01929
01931 uint GetTagsCount() const { return GetVar(mDevice->tags_count); };
01933 playerc_rfidtag_t GetRFIDTag(uint aIndex) const
01934 { return GetVar(mDevice->tags[aIndex]);};
01935
01940 playerc_rfidtag_t operator [](uint aIndex) const
01941 { return(GetRFIDTag(aIndex)); }
01942 };
01943
01948 class SimulationProxy : public ClientProxy
01949 {
01950 private:
01951
01952 void Subscribe(uint aIndex);
01953 void Unsubscribe();
01954
01955
01956 playerc_simulation_t *mDevice;
01957
01958 public:
01960 SimulationProxy(PlayerClient *aPc, uint aIndex=0);
01962 ~SimulationProxy();
01963
01966 void SetPose2d(char* identifier, double x, double y, double a);
01967
01970 void GetPose2d(char* identifier, double& x, double& y, double& a);
01971 };
01972
01973
01979 class SonarProxy : public ClientProxy
01980 {
01981 private:
01982
01983 void Subscribe(uint aIndex);
01984 void Unsubscribe();
01985
01986
01987 playerc_sonar_t *mDevice;
01988
01989 public:
01991 SonarProxy(PlayerClient *aPc, uint aIndex=0);
01993 ~SonarProxy();
01994
01996 uint GetCount() const { return GetVar(mDevice->scan_count); };
01997
01999 double GetScan(uint aIndex) const
02000 { return GetVar(mDevice->scan[aIndex]); };
02003 double operator [] (uint aIndex) const { return GetScan(aIndex); }
02004
02006 uint GetPoseCount() const { return GetVar(mDevice->pose_count); };
02007
02009 player_pose_t GetPose(uint aIndex) const
02010 { return GetVar(mDevice->poses[aIndex]); };
02011
02012
02013
02014
02015
02016
02017
02018
02020 void RequestGeom();
02021 };
02022
02023
02024
02025
02026
02027
02028
02029
02030
02031
02032
02033
02034
02035
02036
02037
02038
02039
02040
02041
02042
02043
02044
02045
02046
02047
02052 class SpeechProxy : public ClientProxy
02053 {
02054
02055 private:
02056
02057 void Subscribe(uint aIndex);
02058 void Unsubscribe();
02059
02060
02061 playerc_speech_t *mDevice;
02062
02063 public:
02065 SpeechProxy(PlayerClient *aPc, uint aIndex=0);
02067 ~SpeechProxy();
02068
02071 void Say(std::string aStr);
02072 };
02073
02077 class SpeechRecognitionProxy : public ClientProxy
02078 {
02079 void Subscribe(uint aIndex);
02080 void Unsubscribe();
02081
02083 playerc_speechrecognition_t *mDevice;
02084 public:
02086 SpeechRecognitionProxy(PlayerClient *aPc, uint aIndex=0);
02087 ~SpeechRecognitionProxy();
02089 std::string GetWord(uint aWord) const{
02090 scoped_lock_t lock(mPc->mMutex);
02091 return std::string(mDevice->words[aWord]);
02092 }
02093
02095 uint GetCount(void) const { return GetVar(mDevice->wordCount); }
02096
02099 std::string operator [](uint aWord) { return(GetWord(aWord)); }
02100 };
02101
02102
02103
02104
02105
02106
02107
02108
02109
02110
02111
02112
02113
02114
02115
02116
02117
02118
02119
02120
02121
02122
02123
02124
02125
02126
02127
02128
02129
02130
02131
02132
02133
02134
02135
02136
02137
02138
02139
02140
02141
02142
02143
02144
02145
02146
02147
02148
02149
02150
02151
02152
02155 class WiFiProxy: public ClientProxy
02156 {
02157
02158 private:
02159
02160 void Subscribe(uint aIndex);
02161 void Unsubscribe();
02162
02163
02164 playerc_wifi_t *mDevice;
02165
02166 public:
02168 WiFiProxy(PlayerClient *aPc, uint aIndex=0);
02170 ~WiFiProxy();
02171
02172
02173
02174
02175
02176
02177
02178
02179
02180
02181
02182
02183
02184
02185
02186
02187
02188
02189
02190
02191
02192
02193
02194
02195
02196
02197
02198
02199
02200
02201
02202
02203 };
02204
02207 class WSNProxy : public ClientProxy
02208 {
02209
02210 private:
02211
02212 void Subscribe(uint aIndex);
02213 void Unsubscribe();
02214
02215
02216 playerc_wsn_t *mDevice;
02217
02218 public:
02220 WSNProxy(PlayerClient *aPc, uint aIndex=0);
02222 ~WSNProxy();
02223
02224 uint GetNodeType () const { return GetVar(mDevice->node_type); };
02225 uint GetNodeID () const { return GetVar(mDevice->node_id); };
02226 uint GetNodeParentID() const { return GetVar(mDevice->node_parent_id); };
02227
02228 player_wsn_node_data_t
02229 GetNodeDataPacket() const { return GetVar(mDevice->data_packet); };
02230
02231 void SetDevState(int nodeID, int groupID, int devNr, int value);
02232 void Power(int nodeID, int groupID, int value);
02233 void DataType(int value);
02234 void DataFreq(int nodeID, int groupID, float frequency);
02235 };
02236
02238 }
02239
02240 namespace std
02241 {
02242 std::ostream& operator << (std::ostream& os, const player_point_2d_t& c);
02243 std::ostream& operator << (std::ostream& os, const player_pose_t& c);
02244 std::ostream& operator << (std::ostream& os, const player_pose3d_t& c);
02245 std::ostream& operator << (std::ostream& os, const player_bbox_t& c);
02246 std::ostream& operator << (std::ostream& os, const player_segment_t& c);
02247 std::ostream& operator << (std::ostream& os, const playerc_device_info_t& c);
02248
02249 std::ostream& operator << (std::ostream& os, const PlayerCc::ClientProxy& c);
02250 std::ostream& operator << (std::ostream& os, const PlayerCc::ActArrayProxy& c);
02251 std::ostream& operator << (std::ostream& os, const PlayerCc::AioProxy& c);
02252
02253
02254
02255 std::ostream& operator << (std::ostream& os, const PlayerCc::BlobfinderProxy& c);
02256 std::ostream& operator << (std::ostream& os, const PlayerCc::BumperProxy& c);
02257 std::ostream& operator << (std::ostream& os, const PlayerCc::CameraProxy& c);
02258 std::ostream& operator << (std::ostream& os, const PlayerCc::DioProxy& c);
02259
02260 std::ostream& operator << (std::ostream& os, const PlayerCc::FiducialProxy& c);
02261 std::ostream& operator << (std::ostream& os, const PlayerCc::GpsProxy& c);
02262 std::ostream& operator << (std::ostream& os, const PlayerCc::GripperProxy& c);
02263 std::ostream& operator << (std::ostream& os, const PlayerCc::IrProxy& c);
02264 std::ostream& operator << (std::ostream& os, const PlayerCc::LaserProxy& c);
02265 std::ostream& operator << (std::ostream& os, const PlayerCc::LimbProxy& c);
02266 std::ostream& operator << (std::ostream& os, const PlayerCc::LocalizeProxy& c);
02267 std::ostream& operator << (std::ostream& os, const PlayerCc::LogProxy& c);
02268 std::ostream& operator << (std::ostream& os, const PlayerCc::MapProxy& c);
02269 std::ostream& operator << (std::ostream& os, const PlayerCc::OpaqueProxy& c);
02270 std::ostream& operator << (std::ostream& os, const PlayerCc::PlannerProxy& c);
02271 std::ostream& operator << (std::ostream& os, const PlayerCc::Position1dProxy& c);
02272 std::ostream& operator << (std::ostream& os, const PlayerCc::Position2dProxy& c);
02273 std::ostream& operator << (std::ostream& os, const PlayerCc::Position3dProxy& c);
02274 std::ostream& operator << (std::ostream& os, const PlayerCc::PowerProxy& c);
02275 std::ostream& operator << (std::ostream& os, const PlayerCc::PtzProxy& c);
02276 std::ostream& operator << (std::ostream& os, const PlayerCc::SimulationProxy& c);
02277 std::ostream& operator << (std::ostream& os, const PlayerCc::SonarProxy& c);
02278 std::ostream& operator << (std::ostream& os, const PlayerCc::SpeechProxy& c);
02279 std::ostream& operator << (std::ostream& os, const PlayerCc::SpeechRecognitionProxy& c);
02280
02281 std::ostream& operator << (std::ostream& os, const PlayerCc::WiFiProxy& c);
02282 std::ostream& operator << (std::ostream& os, const PlayerCc::RFIDProxy& c);
02283 std::ostream& operator << (std::ostream& os, const PlayerCc::WSNProxy& c);
02284
02285 }
02286
02287 #endif
02288