rflex.h
1 /*
2  * Player - One Hell of a Robot Server
3  * Copyright (C) 2000
4  * Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
5  *
6  *
7  * This program is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2 of the License, or
10  * (at your option) any later version.
11  *
12  * This program is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with this program; if not, write to the Free Software
19  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
20  *
21  */
22 
23 /*
24  * this file was adapted from the P2OS device for the RWI RFLEX robots
25  *
26  * the RFLEX device. it's the parent device for all the RFLEX 'sub-devices',
27  * like, position, sonar, etc. there's a thread here that
28  * actually interacts with RFLEX via the serial line. the other
29  * "devices" communicate with this thread by putting into and getting
30  * data out of shared buffers.
31  */
32 #ifndef _RFLEXDEVICE_H
33 #define _RFLEXDEVICE_H
34 
35 #include <pthread.h>
36 #include <sys/time.h>
37 
38 #include <libplayercore/playercore.h>
39 
40 #include "rflex_commands.h"
41 #include "rflex-io.h"
42 
43 #include "rflex_configs.h"
44 
45 #define RFLEX_MOTORS_REQUEST_ON 0
46 #define RFLEX_MOTORS_ON 1
47 #define RFLEX_MOTORS_REQUEST_OFF 2
48 #define RFLEX_MOTORS_OFF 3
49 
50 #define RFLEX_CONFIG_BUFFER_SIZE 256
51 
52 #define DEFAULT_RFLEX_PORT "/dev/ttyS0"
53 
54 #define DEFAULT_RFLEX_BUMPER_ADDRESS 0x40
55 #define RFLEX_BUMPER_STYLE_BIT "bit"
56 #define RFLEX_BUMPER_STYLE_ADDR "addr"
57 #define DEFAULT_RFLEX_BUMPER_STYLE RFLEX_BUMPER_STYLE_ADDR
58 
59 enum
60 {
61  BUMPER_BIT,
62  BUMPER_ADDR
63 };
64 
65 #define DEFAULT_RFLEX_POWER_OFFSET 0
66 
67 #define MAX_NUM_LOOPS 30
68 #define B_STX 0x02
69 #define B_ETX 0x03
70 #define B_ESC 0x1b
71 
72 typedef struct player_rflex_data
73 {
74  player_position2d_data_t position;
75  player_sonar_data_t sonar;
76  player_sonar_data_t sonar2;
77  player_gripper_data_t gripper;
78  player_power_data_t power;
79  player_bumper_data_t bumper;
82  player_ir_data ir;
83 } __attribute__ ((packed)) player_rflex_data_t;
84 
85 /*
86 typedef struct
87 {
88  player_position_cmd_t position;
89  player_gripper_cmd_t gripper;
90  player_sound_cmd_t sound;
91 } __attribute__ ((packed)) player_rflex_cmd_t;
92 */
93 
94 // this is here because we need the above typedef's before including it.
95 
96 class RFLEX : public ThreadedDriver
97 {
98  private:
99  player_devaddr_t position_id;
100  player_devaddr_t sonar_id;
101  player_devaddr_t sonar_id_2;
102  player_devaddr_t ir_id;
103  player_devaddr_t bumper_id;
104  player_devaddr_t power_id;
105  player_devaddr_t aio_id;
106  player_devaddr_t dio_id;
108  int command_type;
109 
110  int position_subscriptions;
111  int sonar_subscriptions;
112  int ir_subscriptions;
113  int bumper_subscriptions;
114 
115  int rflex_fd; // rflex device file descriptor
116 
117  // device used to communicate with rflex
118  char rflex_serial_port[MAX_FILENAME_SIZE];
119  double m_odo_x;
120  double m_odo_y;
121  double rad_odo_theta;
122 
123  void ResetRawPositions();
124  int initialize_robot();
125  void reset_odometry();
126  void set_odometry(float,float,float);
127  void update_everything(player_rflex_data_t* d);
128 
129  void set_config_defaults();
130 
131  public:
132  RFLEX(ConfigFile* cf, int section);
133  ~RFLEX();
134 
135  /* the main thread */
136  virtual void Main();
137 
138  // we override these, because we will maintain our own subscription count
139  virtual int Subscribe(player_devaddr_t addr);
140  virtual int Unsubscribe(player_devaddr_t addr);
141 
142  virtual void MainQuit();
143 
144  static int joy_control;
145 
146  // MessageHandler
147  int ProcessMessage(QueuePointer & resp_queue, player_msghdr * hdr,
148  void * data);
149 };
150 
151 
152 #endif
#define PLAYER_BUMPER_REQ_GET_GEOM
Data: state (PLAYER_BUMPER_DATA_GEOM)
Definition: player_interfaces.h:1916
position 2d velocity command
Definition: player_interfaces.h:617
int bumper_style
bumper bit style
Definition: rflex_configs.h:109
Definition: rflex_configs.h:53
#define PLAYER_MSG1(level, msg, a)
Error message macros.
Definition: error.h:106
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.
#define PLAYER_IR_DATA_RANGES
Data subtype: ranges.
Definition: player_interfaces.h:2108
static bool MatchMessage(player_msghdr_t *hdr, int type, int subtype, player_devaddr_t addr)
Helper for message processing.
Definition: message.h:159
uint32_t poses_count
The number of valid poses.
Definition: player_interfaces.h:788
double ReadFloat(int section, const char *name, double value)
Read a floating point (double) value.
player_bumper_define_t * bumper_def
geometry of each bumper
Definition: player_interfaces.h:1953
int AddInterface(player_devaddr_t addr)
Add an interface.
virtual int Subscribe(player_devaddr_t addr)
Subscribe to this driver.
Definition: rflex.cc:811
double px
X [m].
Definition: player.h:231
Data: ranges (PLAYER_SONAR_DATA_RANGES)
Definition: player_interfaces.h:771
Data AND Request/reply: geometry.
Definition: player_interfaces.h:785
uint32_t poses_count
the number of ir samples returned by this robot
Definition: player_interfaces.h:2133
Data AND Request/reply: bumper geometry.
Definition: player_interfaces.h:1948
Generic message header.
Definition: player.h:161
#define PLAYER_IR_REQ_POSE
Request/reply subtype: get pose.
Definition: player_interfaces.h:2102
const char * ReadString(int section, const char *name, const char *value)
Read a string value.
#define PLAYER_POSITION2D_REQ_MOTOR_POWER
Request/reply: Motor power.
Definition: player_interfaces.h:496
position2d power config
Definition: player_interfaces.h:664
double px
X [m].
Definition: player.h:220
#define PLAYER_POWER_DATA_STATE
Data subtype: voltage.
Definition: player_interfaces.h:274
#define PLAYER_POWER_MASK_VOLTS
bit masks for the player_power_data_t mask field
Definition: player_interfaces.h:282
#define PLAYER_POSITION2D_CMD_VEL
Command: velocity (PLAYER_POSITION2D_CMD_VEL)
Definition: player_interfaces.h:581
double pyaw
yaw [rad]
Definition: player.h:241
uint8_t subtype
Message subtype; interface specific.
Definition: player.h:168
double ReadTupleFloat(int section, const char *name, int index, double value)
Read a float (double) from a tuple field.
int ReadInt(int section, const char *name, int value)
Read an integer value.
void ProcessMessages(void)
Process pending messages.
player_pose3d_t * poses
the pose of each IR detector on this robot
Definition: player_interfaces.h:2135
#define PLAYER_MSGTYPE_DATA
A data message.
Definition: player.h:95
double py
Y [m].
Definition: player.h:222
float radius
radius of curvature [m] - zero for straight lines
Definition: player_interfaces.h:1938
uint32_t bumper_def_count
The number of valid bumper definitions.
Definition: player_interfaces.h:1951
#define PLAYER_MSGTYPE_RESP_ACK
A positive response message.
Definition: player.h:112
bool Wait(double TimeOut=0.0)
Wait for new data to arrive on the driver's queue.
Data: state (PLAYER_GRIPPER_DATA_STATE)
Definition: player_interfaces.h:418
Definition: rflex.h:96
#define PLAYER_POSITION2D_REQ_GET_GEOM
Request/reply: geometry.
Definition: player_interfaces.h:483
Data: state (PLAYER_BUMPER_DATA_GEOM)
Definition: player_interfaces.h:1922
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:106
#define PLAYER_SONAR_REQ_POWER
Request/reply subtype: power.
Definition: player_interfaces.h:758
#define PLAYER_SONAR_DATA_RANGES
Data subtype: ranges.
Definition: player_interfaces.h:761
virtual int Unsubscribe(player_devaddr_t addr)
Unsubscribe from this driver.
Definition: rflex.cc:840
double sl
Length [m].
Definition: player.h:259
Data: voltage (PLAYER_POWER_DATA_STATE)
Definition: player_interfaces.h:291
int ReadDeviceAddr(player_devaddr_t *addr, int section, const char *name, int code, int index, const char *key)
Read a device id.
player_pose2d_t pose
(x, y, yaw) [m, m, rad]
Definition: player_interfaces.h:689
Data: input values (PLAYER_DIO_DATA_VALUES)
Definition: player_interfaces.h:1994
#define PLAYER_BUMPER_DATA_STATE
Data: state (PLAYER_BUMPER_DATA_GEOM)
Definition: player_interfaces.h:1910
#define PLAYER_SONAR_REQ_GET_GEOM
Request/reply subtype: get geometry.
Definition: player_interfaces.h:755
#define PLAYER_SONAR_DATA_GEOM
Data subtype: geometry.
Definition: player_interfaces.h:764
Class for loading configuration file information.
Definition: configfile.h:196
Data: state (PLAYER_AIO_DATA_STATE)
Definition: player_interfaces.h:2053
double sw
Width [m].
Definition: player.h:257
A device address.
Definition: player.h:145
virtual void Main()
Main method for driver thread.
Definition: rflex.cc:873
#define PLAYER_POSITION2D_REQ_RESET_ODOM
Request/reply: Reset odometry.
Definition: player_interfaces.h:541
An autopointer for the message queue.
Definition: message.h:73
double py
Y [m].
Definition: player.h:233
void SetError(int code)
Set/reset error code.
Definition: driver.h:145
position2d data
Definition: player_interfaces.h:606
position2d geom
Definition: player_interfaces.h:655
A pose in space.
Definition: player.h:228
#define PLAYER_ERROR(msg)
Error message macros.
Definition: error.h:81
Base class for drivers which oeprate with a thread.
Definition: driver.h:552
Messages between wsn and a robot.
Definition: er.h:86
uint8_t state
FALSE for power off, TRUE for power on.
Definition: player_interfaces.h:2145
#define PLAYER_POSITION2D_DATA_STATE
Data: state (PLAYER_POSITION2D_DATA_STATE)
Definition: player_interfaces.h:568
player_pose3d_t * poses
Pose of each sonar, in robot cs.
Definition: player_interfaces.h:790
Request/reply: Sonar power.
Definition: player_interfaces.h:797
#define PLAYER_AIO_DATA_STATE
Data: state (PLAYER_AIO_DATA_STATE)
Definition: player_interfaces.h:2046
Data: ranges (PLAYER_IR_DATA_RANGES)
Definition: player_interfaces.h:2115
#define PLAYER_WARN(msg)
Warning message macros.
Definition: error.h:89
float length
length of the sensor [m]
Definition: player_interfaces.h:1936
Definition: rflex.h:72
#define PLAYER_POSITION2D_REQ_VELOCITY_MODE
Request/reply: Change velocity control.
Definition: player_interfaces.h:522
int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition: rflex.cc:357
#define PLAYER_MSGTYPE_CMD
A command message.
Definition: player.h:99
#define PLAYER_DIO_DATA_VALUES
Data: input values (PLAYER_DIO_DATA_VALUES)
Definition: player_interfaces.h:1984
double pa
yaw [rad]
Definition: player.h:224
Base class for all drivers.
Definition: driver.h:108
#define PLAYER_POWER_MASK_PERCENT
Data subtype: voltage.
Definition: player_interfaces.h:285
#define PLAYER_IR_REQ_POWER
Request/reply subtype: set power.
Definition: player_interfaces.h:2105
player_bbox3d_t size
Dimensions of the base (m).
Definition: player_interfaces.h:660
set odometry
Definition: player_interfaces.h:686
virtual int Unsubscribe(player_devaddr_t addr)
Unsubscribe from this driver.
virtual void MainQuit()
Cleanup method for driver thread (called when main exits)
Definition: rflex.cc:795
uint16_t interf
The interface provided by the device; must be one of PLAYER_*_CODE.
Definition: player.h:153
The geometry of a single bumper.
Definition: player_interfaces.h:1931
Request/reply: set power.
Definition: player_interfaces.h:2142
#define PLAYER_POSITION2D_REQ_SET_ODOM
Request/reply: Set odometry.
Definition: player_interfaces.h:535
player_pose3d_t pose
the local pose of a single bumper
Definition: player_interfaces.h:1934