32 #ifndef _RFLEXDEVICE_H
33 #define _RFLEXDEVICE_H
38 #include <libplayercore/playercore.h>
40 #include "rflex_commands.h"
43 #include "rflex_configs.h"
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
50 #define RFLEX_CONFIG_BUFFER_SIZE 256
52 #define DEFAULT_RFLEX_PORT "/dev/ttyS0"
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
65 #define DEFAULT_RFLEX_POWER_OFFSET 0
67 #define MAX_NUM_LOOPS 30
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;
80 player_dio_data_t dio;
81 player_aio_data_t aio;
107 player_position2d_cmd_vel_t command;
110 int position_subscriptions;
111 int sonar_subscriptions;
112 int ir_subscriptions;
113 int bumper_subscriptions;
118 char rflex_serial_port[MAX_FILENAME_SIZE];
121 double rad_odo_theta;
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);
129 void set_config_defaults();
144 static int joy_control;
int bumper_style
bumper bit style
Definition: rflex_configs.h:109
Definition: rflex_configs.h:53
#define PLAYER_MSG1(level, msg, a)
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.
static bool MatchMessage(player_msghdr_t *hdr, int type, int subtype, player_devaddr_t addr)
Helper for message processing.
Definition: message.h:159
double ReadFloat(int section, const char *name, double value)
Read a floating point (double) value.
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
Generic message header.
Definition: player.h:162
const char * ReadString(int section, const char *name, const char *value)
Read a string value.
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.
#define PLAYER_MSGTYPE_DATA
A data message.
Definition: player.h:95
#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.
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:106
virtual int Unsubscribe(player_devaddr_t addr)
Unsubscribe from this driver.
Definition: rflex.cc:840
int ReadDeviceAddr(player_devaddr_t *addr, int section, const char *name, int code, int index, const char *key)
Read a device id.
Class for loading configuration file information.
Definition: configfile.h:197
A device address.
Definition: player.h:146
virtual void Main()
Main method for driver thread.
Definition: rflex.cc:873
An autopointer for the message queue.
Definition: message.h:74
double py
Y [m].
Definition: player.h:233
void SetError(int code)
Set/reset error code.
Definition: driver.h:145
A pose in space.
Definition: player.h:229
#define PLAYER_ERROR(msg)
Definition: error.h:81
Base class for drivers which oeprate with a thread.
Definition: driver.h:553
Messages between wsn and a robot.
Definition: er.h:87
#define PLAYER_WARN(msg)
Warning message macros.
Definition: error.h:89
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
Base class for all drivers.
Definition: driver.h:109
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