erratic.h
1 // -*- mode:C++; tab-width:8; c-basic-offset:2; indent-tabs-mode:1; -*-
2 
26 #ifndef _ERRATICDEVICE_H
27 #define _ERRATICDEVICE_H
28 
29 #ifndef ERRATIC_VERSION
30 #define ERRATIC_VERSION "1.0b"
31 #endif
32 
33 #ifndef ERRATIC_DATE
34 #define ERRATIC_DATE "2006-05-07"
35 #endif
36 
37 #include <pthread.h>
38 #include <sys/time.h>
39 #include <queue>
40 
41 #include <libplayercore/playercore.h>
42 #include <replace/replace.h>
43 
44 #include "packet.h"
45 #include "robot_params.h"
46 
47 //#include <stdint.h>
48 
49 #define CPU_VOLTAGE 3.5
50 
51 // angular constants, angular units are 4096 / rev
52 #define ATOR(x) (M_PI * ((double)(x)) / 2048.0)
53 #define ATOD(x) (180.0 * ((double)(x)) / 2048.0)
54 #define RTOA(x) ((short)((x) * 2048.0) / M_PI)
55 
56 // Default max speeds
57 #define MOTOR_DEF_MAX_SPEED 0.5
58 #define MOTOR_DEF_MAX_TURNSPEED DTOR(100)
59 
60 // This merely sets a delay policy in the initial connection
61 #define ROBOT_CYCLETIME 20000
62 
63 /* Erratic constants */
64 
65 #define VIDERE_NOMINAL_VOLTAGE 12.0
66 
67 
68 // Commands for the robot
69 typedef enum command {
70  pulse = 0,
71  open_controller = 1,
72  close_controller = 2,
73  enable_motors = 4,
74  set_max_trans_acc = 5,
75  set_max_position_velocity = 6,
76  reset_origo = 7,
77  trans_vel = 11, // mm/s
78  rot_pos = 12, // deg
79  rot_dpos = 13, // deg
80  configuration = 18,
81  rot_vel = 21, // deg/s
82  set_max_rot_acc = 23,
83  set_sonar = 28,
84  stop = 29,
85  wheel_vel = 32, // mm/s
86  set_analog = 71,
87  save_config = 72,
88  set_pwm_freq = 73,
89  set_pwm_max_on = 74,
90  servo_pos = 75,
91  set_pid_trans_p = 80,
92  set_pid_trans_v = 81,
93  set_pid_trans_i = 82,
94  set_pid_rot_p = 83,
95  set_pid_rot_v = 84,
96  set_pid_rot_i = 85
97 
98 } command_e;
99 
100 // Argument types used in robot commands
101 typedef enum argtype {
102  argint = 0x3B,
103  argnint = 0x1B,
104  argstr = 0x2B
105 } argtype_e;
106 
107 // Types of replies from the robot
108 typedef enum reply {
109  debug = 0x15,
110  config = 0x20,
111  stopped = 0x32,
112  moving = 0x33,
113  motor = 0x80,
114  encoder = 0x90,
115  ain = 0x9a,
116  sonar = 0x9b
117 } reply_e;
118 
119 
120 #define DEFAULT_VIDERE_PORT "/dev/erratic"
121 
122 typedef struct player_erratic_data
123 {
124  player_position2d_data_t position;
125  player_power_data_t power;
126  player_aio_data_t aio;
127  player_ir_data ir;
128  player_sonar_data sonar;
129 } __attribute__ ((packed)) player_erratic_data_t;
130 
131 // this is here because we need the above typedef's before including it.
132 #include "motorpacket.h"
133 
134 extern bool debug_mode;
135 
136 class ErraticMotorPacket;
137 
138 class Erratic : public Driver
139 {
140 private:
141  int mcount;
142  player_erratic_data_t erratic_data;
143 
144  player_devaddr_t position_id;
145  player_devaddr_t power_id;
146  player_devaddr_t aio_id;
147  player_devaddr_t ir_id;
148  player_devaddr_t sonar_id;
149  player_devaddr_t ptz_id, ptz2_id;
150 
151  int position_subscriptions;
152  int aio_ir_subscriptions;
153  int sonar_subscriptions;
154  int ptz_subscriptions;
155  int ptz2_subscriptions;
156 
157  //ErraticMotorPacket* sippacket;
158  ErraticMotorPacket *motor_packet;
159  pthread_mutex_t motor_packet_mutex;
160 
161  int Connect();
162  int Disconnect();
163 
164  void ResetRawPositions();
165  void ToggleMotorPower(unsigned char val);
166 
167  void ToggleAIn(unsigned char val);
168  void ToggleSonar(unsigned char val);
169 
170  int HandleConfig(QueuePointer &resp_queue, player_msghdr * hdr, void* data);
171  int HandleCommand(player_msghdr * hdr, void * data);
172  void HandlePositionCommand(player_position2d_cmd_vel_t position_cmd);
173  void HandleCarCommand(player_position2d_cmd_car_t position_cmd);
174  void HandlePtzCommand(player_ptz_cmd_t ptz_cmd, player_devaddr_t id);
175 
176  void PublishAllData();
177  void PublishPosition2D();
178  void PublishPower();
179  void PublishAIn();
180  void PublishIR();
181  void PublishSonar();
182 
183  float IRRangeFromVoltage(float voltage);
184  float IRFloorRange(float value);
185 
186  void StartThreads();
187  void StopThreads();
188 
189  void Send(ErraticPacket *packet);
190  void SendThread();
191  static void *SendThreadDummy(void *driver);
192  void ReceiveThread();
193  static void *ReceiveThreadDummy(void *driver);
194 
195  int read_fd, write_fd;
196  const char* psos_serial_port;
197 
198  player_position2d_cmd_vel_t last_position_cmd;
199  player_position2d_cmd_car_t last_car_cmd;
200 
201  std::queue<ErraticPacket *> send_queue;
202  pthread_mutex_t send_queue_mutex;
203  pthread_cond_t send_queue_cond;
204 
205  pthread_t send_thread;
206  pthread_t receive_thread;
207 
208  // Parameters
209 
210  bool direct_wheel_vel_control;
211 
212  bool print_all_packets;
213  bool print_status_summary;
214 
215  bool save_settings_in_robot;
216 
217  int param_idx; // index in the RobotParams table for this robot
218 
219  // Max motor speeds (mm/sec,deg/sec)
220  int motor_max_speed;
221  int motor_max_turnspeed;
222 
223  // Customized control settings for the robot
224  int16_t pid_trans_p, pid_trans_v, pid_trans_i;
225  int16_t pid_rot_p, pid_rot_v, pid_rot_i;
226 
227  // This is a fairly low-level setting that is exposed
228  uint16_t motor_pwm_frequency, motor_pwm_max_on;
229 
230  // Bound the command velocities
231  bool use_vel_band;
232 
233  // Max motor accel/decel (mm/sec/sec, deg/sec/sec)
234  short motor_max_trans_accel, motor_max_trans_decel;
235  short motor_max_rot_accel, motor_max_rot_decel;
236 
237 public:
238 
239  Erratic(ConfigFile* cf, int section);
240 
241  virtual int Subscribe(player_devaddr_t id);
242  virtual int Unsubscribe(player_devaddr_t id);
243 
244  /* the main thread */
245  virtual void Main();
246 
247  virtual int Setup();
248  virtual int Shutdown();
249 
250  // MessageHandler
251  virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr * hdr, void * data);
252 };
253 
254 
255 #endif
uint8_t state
FALSE for off, TRUE for on.
Definition: player_interfaces.h:667
virtual int Unsubscribe(player_devaddr_t id)
Unsubscribe from this driver.
Definition: erratic.cc:802
position 2d velocity command
Definition: player_interfaces.h:617
virtual int Subscribe(player_devaddr_t addr)
Subscribe to this driver.
double velocity
forward velocity (m/s)
Definition: player_interfaces.h:640
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.
player_pose2d_t vel
translational velocities [m/s,m/s,rad/s] (x, y, yaw)
Definition: player_interfaces.h:620
#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.
Definition: erratic.h:122
int AddInterface(player_devaddr_t addr)
Add an interface.
double px
X [m].
Definition: player.h:231
Data: ranges (PLAYER_SONAR_DATA_RANGES)
Definition: player_interfaces.h:771
virtual int Subscribe(player_devaddr_t id)
Subscribe to this driver.
Definition: erratic.cc:773
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
position2d velocity mode config
Definition: player_interfaces.h:671
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition: erratic.cc:1238
Generic message header.
Definition: player.h:161
void ReceiveThread()
Talking to the robot.
Definition: erratic.cc:841
#define PLAYER_IR_REQ_POSE
Request/reply subtype: get pose.
Definition: player_interfaces.h:2102
uint8_t type
Message type; must be one of PLAYER_MSGTYPE_*.
Definition: player.h:166
float tilt
Desired tilt angle [rad].
Definition: player_interfaces.h:1253
Command: state (PLAYER_PTZ_CMD_STATE)
Definition: player_interfaces.h:1248
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
Request/reply: get pose.
Definition: player_interfaces.h:2130
#define PLAYER_POWER_DATA_STATE
Data subtype: voltage.
Definition: player_interfaces.h:274
#define PLAYER_POSITION2D_CMD_VEL
Command: velocity (PLAYER_POSITION2D_CMD_VEL)
Definition: player_interfaces.h:581
double pyaw
yaw [rad]
Definition: player.h:241
double ReadAngle(int section, const char *name, double value)
Read an angle (includes unit conversion).
int ReadInt(int section, const char *name, int value)
Read an integer value.
double angle
relative turning angle (rad)
Definition: player_interfaces.h:642
double ReadLength(int section, const char *name, double value)
Read a length (includes unit conversion, if any).
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
QueuePointer InQueue
Queue for all incoming messages for this driver.
Definition: driver.h:285
double py
Y [m].
Definition: player.h:222
#define PLAYER_MSGTYPE_RESP_ACK
A positive response message.
Definition: player.h:112
player_pose3d_t pose
Pose of the robot base, in the robot cs (m, rad).
Definition: player_interfaces.h:658
Definition: erratic.h:138
#define PLAYER_POSITION2D_REQ_GET_GEOM
Request/reply: geometry.
Definition: player_interfaces.h:483
static bool MatchDeviceAddress(player_devaddr_t addr1, player_devaddr_t addr2)
Compare two addresses.
Definition: device.h:201
#define PLAYER_POSITION2D_CMD_VEL_HEAD
Command: vel/head (PLAYER_POSITION2D_CMD_VEL_HEAD)
Definition: player_interfaces.h:599
uint32_t value
driver-specific
Definition: player_interfaces.h:674
virtual int Shutdown()
Finalize the driver.
Definition: erratic.cc:707
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:106
virtual void Main()
Talking to the Player architecture.
Definition: erratic.cc:1122
#define PLAYER_SONAR_DATA_RANGES
Data subtype: ranges.
Definition: player_interfaces.h:761
double sl
Length [m].
Definition: player.h:259
Data: voltage (PLAYER_POWER_DATA_STATE)
Definition: player_interfaces.h:291
position2d command setting velocity and steering angle
Definition: player_interfaces.h:637
Definition: erratic/packet.h:41
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
float pan
Desired pan angle [rad].
Definition: player_interfaces.h:1251
#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
#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
bool Empty()
Check whether a queue is empty.
Definition: message.h:345
Messages between wsn and a robot.
Definition: er.h:86
#define PLAYER_PTZ_CMD_STATE
Command subtype: state.
Definition: player_interfaces.h:1214
#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
#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
uint32_t size
Size in bytes of the payload to follow.
Definition: player.h:174
#define PLAYER_WARN(msg)
Warning message macros.
Definition: error.h:89
#define PLAYER_POSITION2D_REQ_VELOCITY_MODE
Request/reply: Change velocity control.
Definition: player_interfaces.h:522
#define PLAYER_MSGTYPE_CMD
A command message.
Definition: player.h:99
double pa
yaw [rad]
Definition: player.h:224
Base class for all drivers.
Definition: driver.h:108
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 int Setup()
Initialize the driver.
Definition: erratic.cc:349
#define PLAYER_MSGQUEUE_DEFAULT_MAXLEN
Default maximum length for a message queue.
Definition: player.h:76
#define PLAYER_POSITION2D_REQ_SET_ODOM
Request/reply: Set odometry.
Definition: player_interfaces.h:535
Definition: erratic/motorpacket.h:37
#define PLAYER_POSITION2D_CMD_CAR
Command: carlike (PLAYER_POSITION2D_CMD_CAR)
Definition: player_interfaces.h:594