calcul.h
1 /***************************************************/
2 /* Last Revised:
3 $Id$
4 */
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 #ifndef Calcul
23 #define Calcul
24 
25 #include <stdio.h>
26 #include <math.h>
27 #include "TData.h"
28 
29 #ifdef __cplusplus
30 extern "C" {
31 #endif
32 
33 /*
34  Este fichero tiene operaciones de transformacion de sistemas de referencia,
35  transformaciones de puntos entre sistemas, de paso de coordenadadas polares,
36  a cartesianas y de corte de segmentos
37 
38 */
39 
40 /* --------------------------------------------------------------------------------------- */
41 /* TRANSFORMACIONES DE PUNTO DE UN SISTEMA DE REFERENCIA A OTRO */
42 /* --------------------------------------------------------------------------------------- */
43 
44 /* --------------------------------------------------------------------------------------- */
45 /* transfor_directa_p */
46 /* .... Hace la transformacion directa de un punto a un sistema a otro */
47 /* .... In: (x,y) las coordenadas del punto, sistema es el sistema de referencia */
48 /* .... Out: en sol se devuelve las coordenadas del punto en el nuevo sistema */
49 
50 void transfor_directa_p ( float x, float y, Tsc *sistema, Tpf *sol );
51 
52 /* --------------------------------------------------------------------------------------- */
53 /* transfor_directa_p */
54 /* .... Hace la transformacion directa de un punto a un sistema a otro */
55 /* .... La diferencia es que aqui el punto de entrada es el (0,0) (optimiza la anterior) */
56 /* .... In: (x,y) las coordenadas del punto, sistema es el sistema de referencia */
57 /* .... Out: en sol se devuelve las coordenadas del punto en el nuevo sistema */
58 
59 void transfor_directa_pt0(float x, float y,
60  Tsc *sistema, Tpf *sol);
61 
62 /* --------------------------------------------------------------------------------------- */
63 /* transfor_inversa_p */
64 /* .... Hace la transformacion inversa de un punto a un sistema a otro */
65 /* .... In: (x,y) las coordenadas del punto, sistema es el sistema de referencia */
66 /* .... Out: en sol se devuelve las coordenadas del punto en el nuevo sistema */
67 
68 void transfor_inversa_p ( float x, float y, Tsc *sistema, Tpf *sol );
69 
70 /* --------------------------------------------------------------------------------------- */
71 /* TRANSFORMACIONES DE COMPOSICION E INVERSION DE SISTEMAS DE REFERENCIA */
72 /* --------------------------------------------------------------------------------------- */
73 
74 /* --------------------------------------------------------------------------------------- */
75 /* composicion_sis */
76 /* .... Realiza la composicion de sistemas de referencia en otro sistema */
77 /* .... In: compone sis1 y sis2 */
78 /* .... Out: la salida sisOut es el resultado de la composicion de los sistemas */
79 /* .... Nota: resulta muy importante el orden de las entradas en la composicion */
80 
81 void composicion_sis(Tsc *sis1,Tsc *sis2,Tsc *sisOut);
82 
83 /* --------------------------------------------------------------------------------------- */
84 /* inversion_sis */
85 /* .... Realiza la inversion de un sistema de referencia */
86 /* .... In: sisIn es el sistema a invertir */
87 /* .... Out: sisOut es el sistema invertido */
88 
89 void inversion_sis(Tsc *sisIn, Tsc *sisOut);
90 
91 /* --------------------------------------------------------------------------------------- */
92 /* TRANSFORMACIONES DE PUNTO DE UN SISTEMA DE REFERENCIA A OTRO */
93 /* --------------------------------------------------------------------------------------- */
94 
95 /* --------------------------------------------------------------------------------------- */
96 /* car2pol */
97 /* .... Transforma un punto de coordenadas cartesianas a polares */
98 /* .... In: el punto en coordenadas cartesianas a transformar */
99 /* .... Out: el punto salida en coordenadas polares */
100 
101 void car2pol(Tpf *in, Tpfp *out);
102 
103 /* --------------------------------------------------------------------------------------- */
104 /* pol2car */
105 /* .... Transforma un punto de coordenadas polares a cartesianas */
106 /* .... In: el punto entrada en coordenadas polares a transformar */
107 /* .... Out: el punto en coordenadas cartesianas transformado */
108 
109 void pol2car(Tpfp *in, Tpf *out);
110 
111 /* --------------------------------------------------------------------------------------- */
112 /* TRANSFORMACIONES DE PUNTO DE UN SISTEMA DE REFERENCIA A OTRO */
113 /* --------------------------------------------------------------------------------------- */
114 
115 /* --------------------------------------------------------------------------------------- */
116 /* corte_segmentos */
117 /* .... Calcula el punto de corte entre dos segmentos */
118 /* .... In: las coordenadas de los puntos extremos (x1,y1)-(x2,y2) y (x3,y3)-(x4,y4) */
119 /* .... Out: sol son las coordenadas del punto de corte. return --> 1 si hay corte. -->0 no */
120 
121 int corte_segmentos ( float x1, float y1, float x2, float y2,
122  float x3, float y3, float x4, float y4,
123  Tpf *sol );
124 
125 
126 /* Normaliza el angulo entre [-PI, PI] */
127 float NormalizarPI(float ang);
128 
129 #ifdef __cplusplus
130 }
131 #endif
132 
133 #endif
#define PLAYER_DIO_CMD_VALUES
Data: input values (PLAYER_DIO_DATA_VALUES)
Definition: player_interfaces.h:1987
#define PLAYER_WARN1(msg, a)
Error message macros.
Definition: error.h:89
position 2d velocity command
Definition: player_interfaces.h:617
uint32_t bumpers_count
the number of valid bumper readings
Definition: player_interfaces.h:1925
uint32_t blobs_count
The number of blobs.
Definition: player_interfaces.h:1103
Request/reply: Get/set scan properties.
Definition: player_interfaces.h:961
Data: detected blobs (PLAYER_BLOBFINDER_DATA_BLOBS)
Definition: player_interfaces.h:1096
uint32_t color
A descriptive color for the blob (useful for gui's).
Definition: player_interfaces.h:1074
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_LASER_DATA_SCAN
Data subtype: scan.
Definition: player_interfaces.h:845
static bool MatchMessage(player_msghdr_t *hdr, int type, int subtype, player_devaddr_t addr)
Helper for message processing.
Definition: message.h:158
double ReadFloat(int section, const char *name, double value)
Read a floating point (double) value.
#define PLAYER_PTZ_DATA_STATE
Data subtype: state.
Definition: player_interfaces.h:1208
int AddInterface(player_devaddr_t addr)
Add an interface.
double px
X [m].
Definition: player.h:230
#define PLAYER_LASER_REQ_SET_CONFIG
Request/reply subtype: set configuration.
Definition: player_interfaces.h:857
float tiltspeed
Desired tilt velocity [rad/s].
Definition: player_interfaces.h:1259
player_pose3d_t pose
Laser pose, in robot cs (m, m, m, rad, rad, rad).
Definition: player_interfaces.h:948
uint32_t height
The image dimensions.
Definition: player_interfaces.h:1101
double ReadTupleLength(int section, const char *name, int index, double value)
Read a length from a tuple (includes units conversion)
Data AND Request/reply: bumper geometry.
Definition: player_interfaces.h:1948
Generic message header.
Definition: player.h:160
virtual int Subscribe(QueuePointer &, player_devaddr_t)
Subscribe to this driver.
Definition: driver.h:342
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:657
virtual void MainQuit(void)
Cleanup method for driver thread (called when main exits)
Definition: driver.h:663
Data: state (PLAYER_PTZ_DATA_STATE)
Definition: player_interfaces.h:1228
uint8_t type
Message type; must be one of PLAYER_MSGTYPE_*.
Definition: player.h:165
float tilt
Desired tilt angle [rad].
Definition: player_interfaces.h:1253
Command: state (PLAYER_PTZ_CMD_STATE)
Definition: player_interfaces.h:1248
Encapsulates a device (i.e., a driver bound to an interface)
Definition: device.h:73
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:219
float zoom
Desired field of view [rad].
Definition: player_interfaces.h:1255
#define PLAYER_BLOBFINDER_DATA_BLOBS
Structure describing a single blob.
Definition: player_interfaces.h:1055
#define PLAYER_POSITION2D_CMD_VEL
Command: velocity (PLAYER_POSITION2D_CMD_VEL)
Definition: player_interfaces.h:581
double pyaw
yaw [rad]
Definition: player.h:240
uint8_t subtype
Message subtype; interface specific.
Definition: player.h:167
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).
const char * ReadTupleString(int section, const char *name, int index, const char *value)
Read a string from a tuple field.
int ReadInt(int section, const char *name, int value)
Read an integer value.
uint32_t y
The blob centroid [pixels].
Definition: player_interfaces.h:1080
#define PLAYER_MSGTYPE_DATA
A data message.
Definition: player.h:94
double py
Y [m].
Definition: player.h:221
#define PLAYER_ERROR2(msg, a, b)
Error message macros.
Definition: error.h:82
uint32_t digout
output bitfield
Definition: player_interfaces.h:2011
uint32_t area
The blob area [pixels].
Definition: player_interfaces.h:1076
#define PLAYER_MSGTYPE_RESP_ACK
A positive response message.
Definition: player.h:111
void PutMsg(QueuePointer &resp_queue, uint8_t type, uint8_t subtype, void *src, size_t deprecated, double *timestamp)
Send a message to this device.
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
static bool MatchDeviceAddress(player_devaddr_t addr1, player_devaddr_t addr2)
Compare two addresses.
Definition: device.h:200
Data: state (PLAYER_BUMPER_DATA_GEOM)
Definition: player_interfaces.h:1922
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:105
#define PLAYER_LASER_REQ_GET_GEOM
Request/reply subtype: get geometry.
Definition: player_interfaces.h:854
#define PLAYER_MSGTYPE_RESP_NACK
A negative response message.
Definition: player.h:124
double sl
Length [m].
Definition: player.h:258
Request/reply: Get geometry.
Definition: player_interfaces.h:945
#define PLAYER_LASER_REQ_GET_CONFIG
Request/reply subtype: get configuration.
Definition: player_interfaces.h:860
Command: output values (PLAYER_DIO_CMD_VALUES)
Definition: player_interfaces.h:2006
int ReadDeviceAddr(player_devaddr_t *addr, int section, const char *name, int code, int index, const char *key)
Read a device id.
Data: input values (PLAYER_DIO_DATA_VALUES)
Definition: player_interfaces.h:1994
float pan
Desired pan angle [rad].
Definition: player_interfaces.h:1251
int GetTupleCount(int section, const char *name)
Get the number of values in a tuple.
#define PLAYER_BUMPER_DATA_STATE
Data: state (PLAYER_BUMPER_DATA_GEOM)
Definition: player_interfaces.h:1910
Class for loading configuration file information.
Definition: configfile.h:195
int ReadTupleInt(int section, const char *name, int index, int value)
Read an integer from a tuple field.
double sw
Width [m].
Definition: player.h:256
virtual int Setup()
Initialize the driver.
Definition: driver.h:385
A device address.
Definition: player.h:144
float panspeed
Desired pan velocity [rad/s].
Definition: player_interfaces.h:1257
An autopointer for the message queue.
Definition: message.h:72
double py
Y [m].
Definition: player.h:232
uint8_t * bumpers
array of bumper values
Definition: player_interfaces.h:1927
uint32_t bits
bitfield of samples
Definition: player_interfaces.h:1999
void SetError(int code)
Set/reset error code.
Definition: driver.h:144
void SetFilter(int host, int robot, int interf, int index, int type, int subtype)
Set filter values.
position2d data
Definition: player_interfaces.h:606
Definition: TData.h:41
#define PLAYER_ERROR1(msg, a)
Error message macros.
Definition: error.h:81
uint32_t count
the command
Definition: player_interfaces.h:2009
uint32_t width
The image dimensions.
Definition: player_interfaces.h:1099
#define PLAYER_ERROR(msg)
Error message macros.
Definition: error.h:80
Base class for drivers which oeprate with a thread.
Definition: driver.h:551
#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_blobfinder_blob_t * blobs
The list of blobs.
Definition: player_interfaces.h:1105
double timestamp
Time associated with message contents (seconds since epoch)
Definition: player.h:169
player_pose2d_t pos
position [m,m,rad] (x, y, yaw)
Definition: player_interfaces.h:609
#define PLAYER_POSITION2D_CMD_POS
Command: position (PLAYER_POSITION2D_CMD_POS)
Definition: player_interfaces.h:588
uint32_t size
Size in bytes of the payload to follow.
Definition: player.h:173
player_bbox3d_t size
Laser dimensions (m, m, m).
Definition: player_interfaces.h:950
#define PLAYER_WARN(msg)
Warning message macros.
Definition: error.h:88
#define PLAYER_MSGTYPE_CMD
A command message.
Definition: player.h:98
uint32_t count
number of samples
Definition: player_interfaces.h:1997
virtual int Shutdown()
Finalize the driver.
Definition: driver.h:392
#define PLAYER_DIO_DATA_VALUES
Data: input values (PLAYER_DIO_DATA_VALUES)
Definition: player_interfaces.h:1984
double pa
yaw [rad]
Definition: player.h:223
Base class for all drivers.
Definition: driver.h:107
Definition: TData.h:57
uint32_t x
The blob centroid [pixels].
Definition: player_interfaces.h:1078
uint8_t stall
Are the motors stalled?
Definition: player_interfaces.h:613
int Unsubscribe(QueuePointer &sub_queue)
Unsubscribe the given queue from this device.
#define PLAYER_MSGTYPE_SYNCH
A synch message.
Definition: player.h:116
Definition: TData.h:47
Data: scan (PLAYER_LASER_DATA_SCAN)
Definition: player_interfaces.h:883
player_devaddr_t addr
Device to which this message pertains.
Definition: player.h:163
#define PLAYER_MSGQUEUE_DEFAULT_MAXLEN
Default maximum length for a message queue.
Definition: player.h:75
position2d position command
Definition: player_interfaces.h:626