Project

General

Profile

Revision 959

Changed receive to work when there's no data,
haven't tested for when there is,
test.c works as expected,
Makefile modified to work on receive.c on a linux machine with matlab.
testing robots pending - not hopeful - see code.

View differences:

trunk/code/projects/mapping/matlab/receive.c
1 1
#include <stdio.h>
2 2
#include <stdlib.h>
3
//#include <mex.h>
3
#include <mex.h>
4 4
#include <pthread.h>
5 5
#include <unistd.h>
6 6
#include "../../libwireless/lib/wireless.h"
......
12 12
#define MAP_RECEIVE_GROUP 1
13 13
#define MAP_REQUEST_GROUP 2
14 14

  
15
#define POINT_RAW 1 // packet type for map data points w/ raw encoder data
16
#define POINT_ODO 2 // packet type for map data points w/ odometry data
15
#define POINT_RAW 1 /* packet type for map data points w/ raw encoder data*/
16
#define POINT_ODO 2 /* packet type for map data points w/ odometry data*/
17 17

  
18 18
#define BUFFER_SIZE 1000
19 19
#define BUFFER_EMPTY -1
......
21 21
pthread_rwlock_t buffer_lock;
22 22
pthread_t receive_thread;
23 23

  
24
int packets_received; //Used to block overlapping packet requests.
24
int packets_received; /*Used to block overlapping packet requests.*/
25 25

  
26 26
void packet_receive(char type, int source, unsigned char* packet, int length);
27 27

  
28 28
PacketGroupHandler receiveHandler = {MAP_REQUEST_GROUP, NULL, NULL, &packet_receive, NULL};
29 29
PacketGroupHandler requestHandler = {MAP_RECEIVE_GROUP, NULL, NULL, NULL, NULL};
30 30

  
31
//Buffers
32
short robot_buffer[BUFFER_SIZE];			//by #
33
short range_buffer[BUFFER_SIZE][5]; 		//IR1, IR2 ... 
34
short coordinate_buffer[BUFFER_SIZE][2];	//X Y
31
/*Buffers*/
32
short robot_buffer[BUFFER_SIZE];			/*by #*/
33
short range_buffer[BUFFER_SIZE][5]; 		/*IR1, IR2 ... */
34
short coordinate_buffer[BUFFER_SIZE][2];	/*X Y*/
35 35
float orientation_buffer[BUFFER_SIZE];
36 36

  
37
int tail_index; //Points to the newest data in the buffers.
38
int head_index; //Points to the oldest data in the buffers.
37
int tail_index; /*Points to the newest data in the buffers.*/
38
int head_index; /*Points to the oldest data in the buffers.*/
39 39

  
40
int receive_initialized = 0;
41

  
42 40
void* buffer_data(void* arg);
43 41

  
44
//void mexFunction(int nlhs, mxArray* ret[], int nrhs, const mxArray* args[]){
45
int main(){	
46
	if(!receive_initialized){	
42
void mexFunction(int nlhs, mxArray* ret[], int nrhs, const mxArray* args[]){
43
	
44
	if(nrhs == 1){	
47 45
		
48
		//Initialization
46
		/*Initialization*/
49 47
		head_index = BUFFER_EMPTY;
50 48
		tail_index = BUFFER_EMPTY;	
51 49
 		
52 50
		pthread_rwlock_init(&buffer_lock, NULL);
53 51

  
54 52
		pthread_create(&receive_thread, NULL, buffer_data, NULL);
55
		receive_initialized = 1;
53
		return;
56 54
	}
57 55
	
58
	while(1){}
59
	
60
	/*Return stuff to matlab
61
	//deliberately ignore nrhs.	
56
	/*Return stuff to matlab*/
57
	/*deliberately ignore nrhs.	*/
58

  
62 59
	if(nlhs != 4){
63
		mexErrMsgTxt("Error: Expect output arguments [P IR T R]\n 
64
					  where P = [X Y], IR = [IR1, IR2 ...],\n
60
		mexErrMsgTxt("Error: Expect output arguments [P IR T R]\n  \
61
					  where P = [X Y], IR = [IR1, IR2 ...],\n \
65 62
					  T = angle, and R = Robot number."); 
66 63
	}
67 64
	
68
	double* point, ranges, theta;
69 65
	int index,i,j;
70 66
	
71 67
	pthread_rwlock_rdlock(&buffer_lock);
72 68
	{
73 69

  
74 70
		if(tail_index == BUFFER_EMPTY || head_index == BUFFER_EMPTY){
75
			//Appropriately return with nothing.
71
			double* point, *ranges, *theta, *robot;
72
			/*Appropriately return with nothing.*/
73
			ret[0] = mxCreateDoubleMatrix(1, 2, mxREAL);
74
			ret[1] = mxCreateDoubleMatrix(1, 5, mxREAL);
75
			ret[2] = mxCreateDoubleMatrix(1, 1, mxREAL);
76
			ret[3] = mxCreateDoubleMatrix(1, 1, mxREAL);
77
			
78
			point  = mxGetPr(ret[0]);
79
			ranges = mxGetPr(ret[1]);
80
			theta = mxGetPr(ret[2]);
81
			robot = mxGetPr(ret[3]);
82

  
83
			point[0] = point[1] = 0.0;
84
			
85
			ranges[0] = ranges[1] = ranges[2] = 0.0;
86
			ranges[3] = ranges[4] = 0.0;
87

  
88
			theta[0] = 0.0;
89
			robot[0] = 0.0;
90

  
91
			return;
76 92
		}
77 93
		else{
78
			//TODO Check for the 'already empty' case.
94
			double** point, **ranges, *theta, *robot;
95
			/*TODO Check for the 'already empty' case.*/
79 96

  
80 97
			int num_buffer_elements = tail_index - head_index;
81 98
			if(num_buffer_elements < 0) num_buffer_elements += BUFFER_SIZE;	
......
85 102
			ret[2] = mxCreateDoubleMatrix(num_buffer_elements, 1, mxREAL);
86 103
			ret[3] = mxCreateDoubleMatrix(num_buffer_elements, 1, mxREAL);
87 104
		
88
			point  = mxGetPr(ret[0]);
89
			ranges = mxGetPr(ret[1]);
105
			/*This may not work on some computers.*/
106
			point  = (double**)mxGetPr(ret[0]);
107
			ranges = (double**)mxGetPr(ret[1]);
90 108
			theta = mxGetPr(ret[2]);
91 109
			robot = mxGetPr(ret[3]);
92
			
110

  
93 111
			for(index = head_index; index <= tail_index; index = (index + 1)%1024){
94 112
				point[i][0] = (double)coordinate_buffer[index][0];	
95 113
				point[i][1] = (double)coordinate_buffer[index][1];	
......
102 120
			}
103 121
			head_index = BUFFER_EMPTY;
104 122
			tail_index = BUFFER_EMPTY;
123
		}
105 124
	}	
106 125
	pthread_rwlock_unlock(&buffer_lock);
107
	*/
108
	return -1;
109 126
}
110 127

  
111 128
/*
......
117 134

  
118 135
	pthread_detach(pthread_self());
119 136
	
120
	//General communication initialization.	
137
	/*General communication initialization.	*/
121 138
	wl_set_com_port(WL_COM_PORT);	
122 139
	wl_init();
123 140
	wl_set_channel(WL_CHANNEL);
124 141
	wl_register_packet_group(&receiveHandler);
125 142

  
126
	//Token ring / sensor network initialization.
143
	/*Token ring / sensor network initialization.*/
127 144
	wl_token_ring_register();	
128 145
	
129
	//Continuously iterate over all the robots - buffering data.
146
	/*Continuously iterate over all the robots - buffering data.*/
130 147
	while(1){
131 148
		
132 149
		wl_token_iterator_begin();
133 150
		
134 151
		num_robots = wl_token_get_num_robots();
135 152
		
136
		//Initialize the 'packets received' number. 
153
		/*Initialize the 'packets received' number. */
137 154
		packets_received = 0;
138 155
	
139
		//Send a data request to all robots.
156
		/*Send a data request to all robots.*/
140 157
		while(wl_token_iterator_has_next()){
141 158
			robot_num = wl_token_iterator_next();
142 159
    		
......
146 163

  
147 164
		}
148 165
		
149
		//Wait for all packets to be received before sending more requests.
166
		/*Wait for all packets to be received before sending more requests.*/
150 167
		while(packets_received < num_robots 
151 168
		   && packets_received < wl_token_get_num_robots()){
152 169
			
153
			usleep(10000); //10ms
170
			usleep(10000); /*10ms*/
154 171
		}
155 172
		
156 173
		usleep(10000); 
......
163 180

  
164 181
		if(length!=18) printf("Packet came without the standard length!\n");
165 182

  
166
		// expected input: x y theta(double) IR1 IR2 IR3 IR4 IR5
167
		// Pull in data:
183
		/* expected input: x y theta(double) IR1 IR2 IR3 IR4 IR5*/
184
		/* Pull in data:*/
168 185
		
169 186
		short x,y,i;
170 187
		short ranges[5];	
......
179 196

  
180 197
		float theta = *((float*)(packet + 4));
181 198
	
182
		//Write new data to buffers. (Lock down!)
199
		/*Write new data to buffers. (Lock down!)*/
183 200
		pthread_rwlock_wrlock(&buffer_lock);
184 201
		{	
185
			//Note that you write no matter what.	
202
			/*Note that you write no matter what.	*/
186 203
			tail_index = (tail_index + 1) % BUFFER_SIZE; 
187 204
			
188
			//Write the robots number.
205
			/*Write the robots number.*/
189 206
			robot_buffer[tail_index] = source;
190 207
			
191
			//Write in range data.
208
			/*Write in range data.*/
192 209
			for(i=0; i<5; i++) range_buffer[tail_index][i] = ranges[i];
193 210
			
194 211
			coordinate_buffer[tail_index][0] = x;
......
196 213
			
197 214
			orientation_buffer[tail_index] = theta;
198 215
			
199
			//If the oldest member didn't exist, buffer was empty.
216
			/*If the oldest member didn't exist, buffer was empty.*/
200 217
			if(head_index == BUFFER_EMPTY) head_index = 0;
201 218
		
202
			//Otherwise check for overwritten data.	
219
			/*Otherwise check for overwritten data.	*/
203 220
			else if(tail_index == head_index) 
204 221
				head_index = (head_index +1) % BUFFER_SIZE;  
205 222
			
206 223
		}
207 224
		pthread_rwlock_unlock(&buffer_lock);
208 225
		
209
		//Increment for the transmit thread.
226
		/*Increment for the transmit thread.*/
210 227
		packets_received++;
211 228
	}
212 229
}
trunk/code/projects/mapping/matlab/testMex/test.c
4 4
pthread_t counter;
5 5

  
6 6
void* count(void* arg);
7
int first_time = 0, global = 0;
7
int first_time = 1, global = 0;
8 8

  
9 9
void mexFunction(int nlhs, 
10 10
	mxArray* ret[], int nrhs, const mxArray* args[])
11 11
{
12
	//deliberately ignore nrhs.	
12
	/*deliberately ignore nrhs.	*/
13 13
	if(nlhs != 1){
14 14
		mexErrMsgTxt("Error - only returns one number.\n");
15 15
	}
16 16
	else{
17 17
		
18 18
		if(first_time){
19
			//Deploy a thread to count.
19
			/*Deploy a thread to count.*/
20 20
			pthread_create(&counter, NULL, count, NULL);
21
			first_time = 0;
21 22
		}
22 23

  
23 24
		ret[0] = mxCreateDoubleMatrix(1, 1, mxREAL);
......
27 28
}
28 29

  
29 30
void* count(void* arg){
30
	global++;
31
	usleep(10000);
31
	while(1){
32
		global++;
33
		usleep(10000);
34
	}
32 35
}
trunk/code/projects/mapping/matlab/Makefile
1
CC= gcc
2
CFLAGS= -Wall -g
1
CC= mex
2
CFLAGS=
3 3
LIBS= -lpthread -lwireless
4 4

  
5
all: test
5
all: receive
6 6

  
7 7
test: *.c ../../libwireless/lib/*.c 
8
	$(CC) $(CFLAGS) *.c -L../../../lib/bin -o test -DWL_DEBUG $(LIBS)
8
	$(CC) $(CFLAGS) *.c -L../../../lib/bin -DWL_DEBUG $(LIBS)
9 9

  
10
receive:
11
	 mex -g receive.c -L../../libwireless/lib -lwireless
12

  
10 13
clean:
11
	rm *.o test ../lib/*.o
14
	rm *.o 
12 15

  

Also available in: Unified diff