37 #ifndef _mbasedriverDEVICE_H
38 #define _mbasedriverDEVICE_H
40 #ifndef mbasedriver_VERSION
41 #define mbasedriver_VERSION "2.0"
44 #ifndef mbasedriver_DATE
45 #define mbasedriver_DATE "2010-08-31"
49 #define mbase_author "Hernandez Malagon, Ana Teresa"
57 #include <libplayercore/playercore.h>
58 #include <replace/replace.h>
61 #include "robot_params.h"
65 #define GradToRad(x) (M_PI * ((double)(x)) / 180.0)
66 #define RadToGrad(x) ((short)((x) * 180.0) / M_PI)
69 #define ATOR(x) (M_PI * ((double)(x)) / 2048.0)
70 #define ATOD(x) (180.0 * ((double)(x)) / 2048.0)
71 #define RTOA(x) ((short)((x) * 2048.0) / M_PI)
74 #define MOTOR_DEF_MAX_SPEED 0.5
75 #define MOTOR_DEF_MAX_TURNSPEED DTOR(100)
78 #define ROBOT_CYCLETIME 20000
81 #define VIDERE_NOMINAL_VOLTAGE 13.0
84 #define MOTOR_MAX_TRANS_ACEL 1500
85 #define MOTOR_DEF_TRANS_ACEL 500
87 #define MOTOR_DEF_MAX_ROT_ACEL 135
92 #define ROBOT_WHEEL_MR5 190
93 #define ROBOT_WHEEL_MR7 310
94 #define ROBOT_WIDTH_MR5 410
95 #define ROBOT_WIDTH_MR7 495
104 #define FREC_OSC_MICRO 3.6864
105 #define FACTOR_ESCALA 256
115 open_controller = 0x01,
116 close_controller = 0x02,
117 enable_motors = 0x04,
127 set_trans_acel = 0x5A,
129 set_driffactor = 0x5C,
130 set_robot_width = 0x5D,
131 set_robot_wheel = 0x5E,
132 set_odometria = 0x5F,
137 typedef enum argtype {
157 #define DEFAULT_VIDERE_PORT "/dev/ttyS0"
169 #include "motorpacket.h"
171 extern bool debug_mbasedriver;
172 extern bool debug_send;
173 extern bool debug_receive_aio;
174 extern bool debug_receive_sonar;
175 extern bool debug_receive_motor;
176 extern bool debug_susbcribe;
177 extern bool debug_mbase_send_msj;
185 player_mbasedriver_data_t mbasedriver_data;
193 int position_subscriptions;
194 int aio_ir_subscriptions;
195 int sonar_subscriptions;
199 pthread_mutex_t motor_packet_mutex;
230 int read_fd, write_fd;
231 const char* psos_serial_port;
236 std::queue<mbasedriverPacket *> send_queue;
237 pthread_mutex_t send_queue_mutex;
238 pthread_cond_t send_queue_cond;
240 pthread_t send_thread;
241 pthread_t receive_thread;
244 bool print_all_packets;
249 int motor_max_turnspeed;
254 int motor_trans_acel;
258 int16_t pid_p, pid_v, pid_i;
uint8_t state
FALSE for off, TRUE for on.
Definition: player_interfaces.h:667
position 2d velocity command
Definition: player_interfaces.h:617
void PublishIR()
PublishIR.
Definition: mbasedriver.cc:1303
void PublishSonar()
PublishSonar.
Definition: mbasedriver.cc:1317
virtual int Shutdown()
Shutdown.
Definition: mbasedriver.cc:692
void StopThreads()
StopThreads.
Definition: mbasedriver.cc:755
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.
player_pose2d_t vel
translational velocities [m/s,m/s,rad/s] (x, y, yaw)
Definition: player_interfaces.h:620
virtual int Unsubscribe(player_devaddr_t id)
Unsubscribe.
Definition: mbasedriver.cc:812
#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:158
int Disconnect()
Disconnect.
Definition: mbasedriver.cc:703
Definition: mbasedriver.h:159
void ReceiveThread()
ReceiveThread.
Definition: mbasedriver.cc:849
uint32_t poses_count
The number of valid poses.
Definition: player_interfaces.h:788
Definition: mbasedriver.h:181
int AddInterface(player_devaddr_t addr)
Add an interface.
double px
X [m].
Definition: player.h:230
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
Generic message header.
Definition: player.h:160
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
ProcessMessage.
Definition: mbasedriver.cc:1345
void PublishAllData()
PublishAllData.
Definition: mbasedriver.cc:1331
#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:165
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
void HandlePositionCommand(player_position2d_cmd_vel_t position_cmd)
HandlePositionCommand.
Definition: mbasedriver.cc:1532
position2d power config
Definition: player_interfaces.h:664
double px
X [m].
Definition: player.h:219
Request/reply: get pose.
Definition: player_interfaces.h:2130
#define PLAYER_POWER_DATA_STATE
Data subtype: voltage.
Definition: player_interfaces.h:274
void StartThreads()
StrarThreads.
Definition: mbasedriver.cc:746
#define PLAYER_POSITION2D_CMD_VEL
Command: velocity (PLAYER_POSITION2D_CMD_VEL)
Definition: player_interfaces.h:581
double pyaw
yaw [rad]
Definition: player.h:240
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 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:94
QueuePointer InQueue
Queue for all incoming messages for this driver.
Definition: driver.h:284
double py
Y [m].
Definition: player.h:221
void ToggleAIn(unsigned char val)
ToggleAIn.
Definition: mbasedriver.cc:1150
virtual int Subscribe(player_devaddr_t id)
Subscribe.
Definition: mbasedriver.cc:772
#define PLAYER_MSGTYPE_RESP_ACK
A positive response message.
Definition: player.h:111
player_pose3d_t pose
Pose of the robot base, in the robot cs (m, rad).
Definition: player_interfaces.h:658
int HandleCommand(player_msghdr *hdr, void *data)
HandleCommand.
Definition: mbasedriver.cc:1622
#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:200
static void * SendThreadDummy(void *driver)
SedThreadDummy.
Definition: mbasedriver.cc:1079
void PublishPower()
PublishPower.
Definition: mbasedriver.cc:1275
int Connect()
Connect.
Definition: mbasedriver.cc:381
void ToggleMotorPower(unsigned char val)
ToggleMotorPower.
Definition: mbasedriver.cc:1126
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:105
#define PLAYER_SONAR_DATA_RANGES
Data subtype: ranges.
Definition: player_interfaces.h:761
int Build(unsigned char *data, unsigned char datasize)
Build.
Definition: mbase/packet.cc:319
double sl
Length [m].
Definition: player.h:258
Data: voltage (PLAYER_POWER_DATA_STATE)
Definition: player_interfaces.h:291
position2d command setting velocity and steering angle
Definition: player_interfaces.h:637
int ReadDeviceAddr(player_devaddr_t *addr, int section, const char *name, int code, int index, const char *key)
Read a device id.
void PublishAIn()
PublishAIn.
Definition: mbasedriver.cc:1289
Copyright (C) 2010 Ana Teresa Hernández Malagón anat.hernandezm@gmail.com Movirobotics athernandez@...
Definition: mbase/motorpacket.h:45
player_pose2d_t pose
(x, y, yaw) [m, m, rad]
Definition: player_interfaces.h:689
#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:195
Data: state (PLAYER_AIO_DATA_STATE)
Definition: player_interfaces.h:2053
double sw
Width [m].
Definition: player.h:256
A device address.
Definition: player.h:144
void PublishPosition2D()
PublishPosition2D.
Definition: mbasedriver.cc:1261
#define PLAYER_POSITION2D_REQ_RESET_ODOM
Request/reply: Reset odometry.
Definition: player_interfaces.h:541
An autopointer for the message queue.
Definition: message.h:72
double py
Y [m].
Definition: player.h:232
int HandleConfig(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
HandleConfig.
Definition: mbasedriver.cc:1361
mbasedriver(ConfigFile *cf, int section)
Definition: mbasedriver.cc:209
void SetError(int code)
Set/reset error code.
Definition: driver.h:144
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:344
A pose in space.
Definition: player.h:227
int getms()
getms
Definition: mbasedriver.cc:1520
bool Parse(unsigned char *buffer, int length)
Parse -Parses and absorbs a standard packet from the robot.
Definition: mbase/motorpacket.cc:58
Messages between wsn and a robot.
Definition: er.h:86
#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
void SendThread()
SendThread.
Definition: mbasedriver.cc:1039
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:173
#define PLAYER_WARN(msg)
Warning message macros.
Definition: error.h:88
virtual void Main()
Main.
Definition: mbasedriver.cc:1195
#define PLAYER_MSGTYPE_CMD
A command message.
Definition: player.h:98
static void * ReceiveThreadDummy(void *driver)
ReceiveThreadDummy.
Definition: mbasedriver.cc:1030
double pa
yaw [rad]
Definition: player.h:223
virtual int Setup()
Setup.
Definition: mbasedriver.cc:371
void Send(mbasedriverPacket *packet)
Send.
Definition: mbasedriver.cc:1090
Base class for all drivers.
Definition: driver.h:107
player_bbox3d_t size
Dimensions of the base (m).
Definition: player_interfaces.h:660
int Receive(int fd, uint16_t wait=30)
Receive.
Definition: mbase/packet.cc:130
Definition: mbase/packet.h:48
set odometry
Definition: player_interfaces.h:686
virtual int Unsubscribe(player_devaddr_t addr)
Unsubscribe from this driver.
void ResetRawPositions()
ResetRawPositions.
Definition: mbasedriver.cc:1102
void Fill(player_mbasedriver_data_t *data)
Fill.
Definition: mbase/motorpacket.cc:111
#define PLAYER_MSGQUEUE_DEFAULT_MAXLEN
Default maximum length for a message queue.
Definition: player.h:75
#define PLAYER_POSITION2D_REQ_SET_ODOM
Request/reply: Set odometry.
Definition: player_interfaces.h:535
void ToggleSonar(unsigned char val)
ToggleSonar.
Definition: mbasedriver.cc:1172