31 #include <semaphore.h>
33 #include <libplayercore/playercore.h>
70 player_position2d_geom_t robot_geom;
71 int first_goal_has_been_set_to_init_position;
73 void SetSpeedCmd(player_position2d_cmd_vel_t cmd);
77 std::vector<double> laser__ranges;
78 double laser__resolution;
79 double laser__max_range;
80 uint32_t laser__ranges_count;
84 double obstacle_avoid_dist;
87 double goal_position_tol;
88 double goal_angle_tol;
89 double goalX,goalY,goalA;
90 pthread_t algorithm_thread;
91 pthread_mutex_t goal_mutex;
92 pthread_cond_t goal_changed_cond;
94 pthread_mutex_t data_mutex;
95 pthread_cond_t data_changed_cond;
97 int data_odometry_ready;
100 void WaitForNextGoal();
101 void SignalNextGoal(
double goalX,
double goalY,
double goalA);
103 void ReadIfWaiting();
113 double GetScanRes() ;
114 double GetMaxRange() ;
115 uint32_t GetCount() ;
116 double range(
const int index);
118 void SetMotorEnable(
int turnkey);
119 void SetOdometry(
double position_x0,
121 double position_alpha0);
126 void SetSpeed(
double velocity_modulus,
127 double velocity_angle);
137 extern "C" int player_driver_init(DriverTable* table);
#define PLAYER_WARN1(msg, a)
Error message macros.
Definition: error.h:90
#define PLAYER_MSG3(level, msg, a, b, c)
Error message macros.
Definition: error.h:108
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
A pose in the plane.
Definition: player.h:218
Generic message header.
Definition: player.h:162
int Subscribe(QueuePointer &sub_queue)
Subscribe the given queue to this device.
virtual int MainSetup(void)
Sets up the resources needed by the driver thread.
Definition: driver.h:658
virtual void MainQuit(void)
Cleanup method for driver thread (called when main exits)
Definition: driver.h:664
virtual int Shutdown()
Finalize the driver.
Definition: snd.cc:325
uint8_t type
Message type; must be one of PLAYER_MSGTYPE_*.
Definition: player.h:166
Encapsulates a device (i.e., a driver bound to an interface)
Definition: device.h:75
const char * ReadString(int section, const char *name, const char *value)
Read a string value.
double px
X [m].
Definition: player.h:220
uint8_t subtype
Message subtype; interface specific.
Definition: player.h:168
virtual void Main(void)=0
Main method for driver thread.
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 * GetPayload()
Get pointer to payload.
Definition: message.h:188
void ProcessMessages(void)
Process pending messages.
#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_ERROR2(msg, a, b)
Error message macros.
Definition: error.h:83
void PutMsg(QueuePointer &resp_queue, uint8_t type, uint8_t subtype, void *src, size_t deprecated, double *timestamp)
Send a message to this device.
bool Wait(double TimeOut=0.0)
Wait for new data to arrive on the driver's queue.
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
virtual int Setup()
Initialize the driver.
Definition: snd.cc:307
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:106
#define PLAYER_MSGTYPE_RESP_NACK
A negative response message.
Definition: player.h:125
virtual void Main()
Main method for driver thread.
Definition: snd.cc:343
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
An autopointer for the message queue.
Definition: message.h:74
#define PLAYER_ERROR(msg)
Error message macros.
Definition: error.h:81
player_devaddr_t device_addr
Default device address (single-interface drivers)
Definition: driver.h:269
Base class for drivers which oeprate with a thread.
Definition: driver.h:553
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
Definition: snd.cc:423
double timestamp
Time associated with message contents (seconds since epoch)
Definition: player.h:170
virtual void StopThread(void)
Cancel (and wait for termination) of the driver thread.
uint32_t size
Size in bytes of the payload to follow.
Definition: player.h:174
Reference-counted message objects.
Definition: message.h:133
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_WARN(msg)
Warning message macros.
Definition: error.h:89
#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:109
#define PLAYER_MSG0(level, msg)
General messages.
Definition: error.h:105
int Unsubscribe(QueuePointer &sub_queue)
Unsubscribe the given queue from this device.
player_msghdr_t * GetHeader()
Get pointer to header.
Definition: message.h:186
player_devaddr_t addr
Device to which this message pertains.
Definition: player.h:164
#define PLAYER_MSGQUEUE_DEFAULT_MAXLEN
Default maximum length for a message queue.
Definition: player.h:76