segwayrmp400.h
1 /*
2  * Player - One Hell of a Robot Server
3  * Copyright (C) 2009 Goutham Mallapragda, Anthony Cascone, Rich Mattes & Brian Gerkey
4  *
5  * This program is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program; if not, write to the Free Software
17  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
18  *
19  */
20 /*
21  Desc: Segway RMP 400 Driver
22  Authors: Goutham Mallapragada, Anthony Cascone, Rich Mattes
23  Updated: July 21, 2009
24 */
25 
26 #ifndef __SEGWAYRMP400_H_
27 #define __SEGWAYRMP400_H_
28 
29 #include <stdio.h>
30 #include <pthread.h>
31 #include <string.h>
32 #include <stdlib.h>
33 #include <math.h>
34 #if !defined (WIN32) || defined (__MINGW32__)
35  #include <unistd.h>
36 #endif
37 #if !defined (WIN32)
38  #include <netinet/in.h>
39 #endif
40 
41 #include <libplayercore/playercore.h>
42 
43 
44 // SegwayRMP400 Position Driver
46 {
47 
48 public:
49 
50  // Standard Constructor
51  SegwayRMP400(ConfigFile* cf, int section);
52 
53  // Initialize (Player Standard)
54  virtual int MainSetup();
55 
56  // Shutdown (Player Standard)
57  virtual void MainQuit();
58 
59 private:
60 
61  // Devices
62  Device* segwayrmp2d[2]; // child segwayrmp200 devices for 2d subsrciption
63  Device* segwayrmp3d[2]; // child segwayrmp200 devices for 3d subsrciption
64 
65  // Device Addresses
66  player_devaddr_t segwayrmp2d_id[2]; // 2d Position Interface Address (Output)
67  player_devaddr_t segwayrmp3d_id[2]; // 3d Position Interface Address (Output)
68 
69  player_devaddr_t position3d_id; // 3d Position Interface Address (Input)
70  player_devaddr_t position2d_id; // 2d Position Interface Address (Input)
71 
72  // Device Data Structures
73  player_position2d_data_t rmp2d_data[2]; // Incoming data from child segwayrmp200 devices 2d
74  player_position3d_data_t rmp3d_data[2]; // Incoming data from child segwayrmp200 devices 3d
75 
76  player_position2d_data_t position2d_data; // Output data for parent segwayrmp400 device 2d
77  player_position2d_cmd_vel_t position2d_cmd; // Output cmd for parent segwayrmp400 device 2d
78 
79  player_position3d_data_t position3d_data; // Output data for parent segwayrmp400 device 3d
80  player_position3d_cmd_vel_t position3d_cmd; // Output cmd for parent segwayrmp400 device 3d
81 
82  // Flags
83  bool provide_2d; // Provide 2d interface Flag
84  bool provide_3d; // Provide 3d interface Flag
85 
86  int counter;
87  // Main
88  void Main();
89 
90  // Process message function (Player Standard)
91  int ProcessMessage(QueuePointer &resp_queue, player_msghdr_t* hdr, void* data);
92 
93  // Packages position data and publishes
94  void ProcessData();
95 
96  // Internal method to handle position 3D commands
98 
99  // Internal method to handle position 2D commands
101 
102  // Flags for new data
103  bool newfront3d, newback3d, newfront2d, newback2d;
104 
105  // Flag for full speed data reporting.
106  bool fullspeed;
107 };
108 
109 #endif
position 2d velocity command
Definition: player_interfaces.h:617
virtual void MainQuit()
Cleanup method for driver thread (called when main exits)
Definition: segwayrmp400.cc:247
double proll
roll [rad]
Definition: player.h:236
virtual int Subscribe(player_devaddr_t addr)
Subscribe to this driver.
virtual void Publish(player_devaddr_t addr, QueuePointer &queue, uint8_t type, uint8_t subtype, void *src=NULL, size_t deprecated=0, double *timestamp=NULL, bool copy=true)
Publish a message via one of this driver's interfaces.
SegwayRMP400(ConfigFile *cf, int section)
Definition: segwayrmp400.cc:116
static bool MatchMessage(player_msghdr_t *hdr, int type, int subtype, player_devaddr_t addr)
Helper for message processing.
Definition: message.h:158
int AddInterface(player_devaddr_t addr)
Add an interface.
int ProcessMessage(QueuePointer &resp_queue, player_msghdr_t *hdr, void *data)
Message handler.
Definition: segwayrmp400.cc:292
double px
X [m].
Definition: player.h:230
Generic message header.
Definition: player.h:160
double pz
Z [m].
Definition: player.h:234
Data: state (PLAYER_POSITION3D_DATA_STATE)
Definition: player_interfaces.h:2476
void Main()
Main method for driver thread.
Definition: segwayrmp400.cc:268
player_pose3d_t pos
(x, y, z, roll, pitch, yaw) position [m, m, m, rad, rad, rad]
Definition: player_interfaces.h:2479
player_pose3d_t vel
(x, y, z, roll, pitch, yaw) velocity [m, m, m, rad, rad, rad]
Definition: player_interfaces.h:2481
uint8_t type
Message type; must be one of PLAYER_MSGTYPE_*.
Definition: player.h:165
Encapsulates a device (i.e., a driver bound to an interface)
Definition: device.h:73
double px
X [m].
Definition: player.h:219
#define PLAYER_POSITION3D_DATA_STATE
Data subtype: state.
Definition: player_interfaces.h:2432
#define PLAYER_POSITION2D_CMD_VEL
Command: velocity (PLAYER_POSITION2D_CMD_VEL)
Definition: player_interfaces.h:581
double pyaw
yaw [rad]
Definition: player.h:240
uint8_t subtype
Message subtype; interface specific.
Definition: player.h:167
int ReadInt(int section, const char *name, int value)
Read an integer value.
void * GetPayload()
Get pointer to payload.
Definition: message.h:187
void ProcessMessages(void)
Process pending messages.
#define PLAYER_MSGTYPE_DATA
A data message.
Definition: player.h:94
QueuePointer InQueue
Queue for all incoming messages for this driver.
Definition: driver.h:284
double py
Y [m].
Definition: player.h:221
virtual int MainSetup()
Sets up the resources needed by the driver thread.
Definition: segwayrmp400.cc:197
#define PLAYER_MSGTYPE_RESP_ACK
A positive response message.
Definition: player.h:111
void PutMsg(QueuePointer &resp_queue, uint8_t type, uint8_t subtype, void *src, size_t deprecated, double *timestamp)
Send a message to this device.
player_pose3d_t pose
Pose of the robot base, in the robot cs (m, rad).
Definition: player_interfaces.h:658
#define PLAYER_POSITION2D_REQ_GET_GEOM
Request/reply: geometry.
Definition: player_interfaces.h:483
#define PLAYER_POSITION3D_CMD_SET_VEL
Command subtype: velocity control.
Definition: player_interfaces.h:2438
double ppitch
pitch [rad]
Definition: player.h:238
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:105
double sl
Length [m].
Definition: player.h:258
int ReadDeviceAddr(player_devaddr_t *addr, int section, const char *name, int code, int index, const char *key)
Read a device id.
int HandlePosition2DCmd(player_position2d_cmd_vel_t *cmd)
Definition: segwayrmp400.cc:405
Class for loading configuration file information.
Definition: configfile.h:195
double sw
Width [m].
Definition: player.h:256
A device address.
Definition: player.h:144
An autopointer for the message queue.
Definition: message.h:72
double py
Y [m].
Definition: player.h:232
player_pose2d_t vel
translational velocities [m/s,m/s,rad/s] (x, y, yaw)
Definition: player_interfaces.h:611
void SetError(int code)
Set/reset error code.
Definition: driver.h:144
position2d data
Definition: player_interfaces.h:606
#define PLAYER_ERROR1(msg, a)
Error message macros.
Definition: error.h:81
void ProcessData()
Definition: segwayrmp400.cc:421
position2d geom
Definition: player_interfaces.h:655
#define PLAYER_ERROR(msg)
Error message macros.
Definition: error.h:80
Base class for drivers which oeprate with a thread.
Definition: driver.h:551
Command: velocity (PLAYER_POSITION3D_CMD_SET_VEL)
Definition: player_interfaces.h:2504
#define PLAYER_POSITION2D_DATA_STATE
Data: state (PLAYER_POSITION2D_DATA_STATE)
Definition: player_interfaces.h:568
double timestamp
Time associated with message contents (seconds since epoch)
Definition: player.h:169
player_pose2d_t pos
position [m,m,rad] (x, y, yaw)
Definition: player_interfaces.h:609
uint32_t size
Size in bytes of the payload to follow.
Definition: player.h:173
Reference-counted message objects.
Definition: message.h:131
Message * Request(QueuePointer &resp_queue, uint8_t type, uint8_t subtype, void *src, size_t deprecated, double *timestamp, bool threaded=true)
Make a request of another device.
#define PLAYER_MSGTYPE_CMD
A command message.
Definition: player.h:98
double pa
yaw [rad]
Definition: player.h:223
Base class for all drivers.
Definition: driver.h:107
#define PLAYER_MSG0(level, msg)
General messages.
Definition: error.h:104
Definition: segwayrmp400.h:45
player_bbox3d_t size
Dimensions of the base (m).
Definition: player_interfaces.h:660
virtual int Unsubscribe(player_devaddr_t addr)
Unsubscribe from this driver.
player_msghdr_t * GetHeader()
Get pointer to header.
Definition: message.h:185
player_devaddr_t addr
Device to which this message pertains.
Definition: player.h:163
#define PLAYER_MSGQUEUE_DEFAULT_MAXLEN
Default maximum length for a message queue.
Definition: player.h:75
int HandlePosition3DCmd(player_position3d_cmd_vel_t *cmd)
Definition: segwayrmp400.cc:390