Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have Weboreceived a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : simulator connector for webots simulator
17 : */
18 :
19 : #include "SIM_config.h"
20 :
21 : #if AP_SIM_WEBOTS_ENABLED
22 :
23 : #include "SIM_Webots.h"
24 :
25 : #include <arpa/inet.h>
26 : #include <errno.h>
27 : #include <fcntl.h>
28 : #include <stdio.h>
29 : #include <stdarg.h>
30 : #include <sys/stat.h>
31 : #include <sys/types.h>
32 :
33 : #include <AP_HAL/AP_HAL.h>
34 : #include "pthread.h"
35 : #include <AP_HAL/utility/replace.h>
36 :
37 : extern const AP_HAL::HAL& hal;
38 :
39 : using namespace SITL;
40 :
41 : static const struct {
42 : const char *name;
43 : float value;
44 : bool save;
45 : } sim_defaults[] = {
46 : { "AHRS_EKF_TYPE", 10 },
47 : { "INS_GYR_CAL", 0 },
48 : { "RC1_MIN", 1000, true },
49 : { "RC1_MAX", 2000, true },
50 : { "RC2_MIN", 1000, true },
51 : { "RC2_MAX", 2000, true },
52 : { "RC3_MIN", 1000, true },
53 : { "RC3_MAX", 2000, true },
54 : { "RC4_MIN", 1000, true },
55 : { "RC4_MAX", 2000, true },
56 : { "RC2_REVERSED", 1 }, // interlink has reversed rc2
57 : { "SERVO1_MIN", 1000 },
58 : { "SERVO1_MAX", 2000 },
59 : { "SERVO2_MIN", 1000 },
60 : { "SERVO2_MAX", 2000 },
61 : { "SERVO3_MIN", 1000 },
62 : { "SERVO3_MAX", 2000 },
63 : { "SERVO4_MIN", 1000 },
64 : { "SERVO4_MAX", 2000 },
65 : { "SERVO5_MIN", 1000 },
66 : { "SERVO5_MAX", 2000 },
67 : { "SERVO6_MIN", 1000 },
68 : { "SERVO6_MAX", 2000 },
69 : { "SERVO6_MIN", 1000 },
70 : { "SERVO6_MAX", 2000 },
71 : { "INS_ACC2OFFS_X", 0.001 },
72 : { "INS_ACC2OFFS_Y", 0.001 },
73 : { "INS_ACC2OFFS_Z", 0.001 },
74 : { "INS_ACC2SCAL_X", 1.001 },
75 : { "INS_ACC2SCAL_Y", 1.001 },
76 : { "INS_ACC2SCAL_Z", 1.001 },
77 : { "INS_ACCOFFS_X", 0.001 },
78 : { "INS_ACCOFFS_Y", 0.001 },
79 : { "INS_ACCOFFS_Z", 0.001 },
80 : { "INS_ACCSCAL_X", 1.001 },
81 : { "INS_ACCSCAL_Y", 1.001 },
82 : { "INS_ACCSCAL_Z", 1.001 },
83 : };
84 :
85 :
86 0 : Webots::Webots(const char *frame_str) :
87 0 : Aircraft(frame_str)
88 : {
89 0 : use_time_sync = false;
90 0 : use_smoothing = false;
91 0 : last_state.timestamp = 0.0l;
92 0 : char *saveptr = nullptr;
93 0 : char *s = strdup(frame_str);
94 0 : char *frame_option = strtok_r(s, ":", &saveptr);
95 0 : char *args1 = strtok_r(nullptr, ":", &saveptr);
96 0 : char *args2 = strtok_r(nullptr, ":", &saveptr);
97 : /*
98 : allow setting of IP, sensors port and control port
99 : format morse:IPADDRESS:SENSORS_PORT:CONTROL_PORT
100 : */
101 0 : if (args1) {
102 0 : webots_ip = args1;
103 0 : printf("Simulation Port %s\n",args1);
104 : }
105 0 : if (args2) {
106 0 : webots_sensors_port = atoi(args2);
107 : }
108 :
109 :
110 0 : if (strstr(frame_option, "-rover")) {
111 0 : output_type = OUTPUT_ROVER;
112 0 : } else if (strstr(frame_option, "-quad")) {
113 0 : output_type = OUTPUT_QUAD;
114 0 : } else if (strstr(frame_option, "-tri")) {
115 0 : output_type = OUTPUT_TRICOPTER;
116 0 : } else if (strstr(frame_option, "-pwm")) {
117 0 : output_type = OUTPUT_PWM;
118 : } else {
119 : // default to rover
120 0 : output_type = OUTPUT_ROVER;
121 : }
122 :
123 0 : for (uint8_t i=0; i<ARRAY_SIZE(sim_defaults); i++) {
124 0 : AP_Param::set_default_by_name(sim_defaults[i].name, sim_defaults[i].value);
125 0 : if (sim_defaults[i].save) {
126 : enum ap_var_type ptype;
127 0 : AP_Param *p = AP_Param::find(sim_defaults[i].name, &ptype);
128 0 : if (!p->configured()) {
129 0 : p->save();
130 : }
131 : }
132 : }
133 0 : printf("Started Webots with %s:%u type %u\n",
134 0 : webots_ip, webots_sensors_port,
135 0 : (unsigned)output_type);
136 0 : }
137 :
138 : /*
139 : very simple JSON parser for sensor data
140 : called with pointer to one row of sensor data, nul terminated
141 :
142 : This parser does not do any syntax checking, and is not at all
143 : general purpose
144 :
145 : {"timestamp": 1563474924.817575,
146 : "vehicle.imu": {"timestamp": 1563474924.8009083,
147 : "angular_velocity": [2.319516170246061e-06, -3.5830129263558774e-07, 7.009341995711793e-09],
148 : "linear_acceleration": [0.005075275432318449, 0.22471635043621063, 9.80748176574707],
149 : "magnetic_field": [23088.65625, 3875.89453125, -53204.51171875]},
150 : "vehicle.gps": {"timestamp": 1563474924.8009083, "x": 5.386466364143416e-05, "y": -0.0010969983413815498, "z": 0.03717954829335213},
151 : "vehicle.velocity": {"timestamp": 1563474924.8009083,
152 : "linear_velocity": [4.818238585890811e-10, 2.1333558919423012e-08, 9.310780910709582e-07],
153 : "angular_velocity": [2.319516170246061e-06, -3.5830129263558774e-07, 7.009341995711793e-09],
154 : "world_linear_velocity": [5.551115123125783e-17, 0.0, 9.313225746154785e-07]},
155 : "vehicle.pose": {"timestamp": 1563474924.8009083,
156 : "x": 5.386466364143416e-05, "y": -0.0010969983413815498, "z": 0.03717954829335213,
157 : "yaw": 7.137723878258839e-05, "pitch": -0.0005173543468117714, "roll": 0.022908739745616913}}
158 :
159 : */
160 :
161 0 : bool Webots::parse_sensors(const char *json)
162 : {
163 : //printf("%s\n", json);
164 0 : for (uint16_t i=0; i<ARRAY_SIZE(keytable); i++) {
165 0 : struct keytable &key = keytable[i];
166 : //printf("search %s/%s\n", key.section, key.key);
167 : // look for section header
168 0 : const char *p = strstr(json, key.section);
169 0 : if (!p) {
170 : // we don't have this sensor
171 0 : continue;
172 : }
173 0 : p += strlen(key.section)+1;
174 :
175 : // find key inside section
176 0 : p = strstr(p, key.key);
177 0 : if (!p) {
178 0 : printf("Failed to find key %s/%s\n", key.section, key.key);
179 0 : return false;
180 : }
181 :
182 0 : p += strlen(key.key)+3;
183 0 : switch (key.type) {
184 0 : case DATA_FLOAT:
185 0 : *((float *)key.ptr) = strtof(p, nullptr);
186 : //printf("GOT %s/%s value: %f\n", key.section, key.key, *((float *)key.ptr));
187 0 : break;
188 :
189 0 : case DATA_DOUBLE:
190 0 : *((double *)key.ptr) = atof(p);
191 : //printf("GOT %s/%s value: %f\n", key.section, key.key, *((double *)key.ptr));
192 0 : break;
193 :
194 0 : case DATA_VECTOR3F: {
195 0 : Vector3f *v = (Vector3f *)key.ptr;
196 0 : if (sscanf(p, "[%f, %f, %f]", &v->x, &v->y, &v->z) != 3) {
197 0 : printf("Failed to parse Vector3f for %s %s/%s\n",p, key.section, key.key);
198 : //printf("Failed to parse Vector3f for %s/%s\n", key.section, key.key);
199 0 : return false;
200 : }
201 : else
202 : {
203 : //printf("GOT %s/%s [%f, %f, %f]\n", key.section, key.key, v->x, v->y, v->z);
204 : }
205 :
206 0 : break;
207 : }
208 :
209 0 : case DATA_VECTOR3F_ARRAY: {
210 : // example: [[0.0, 0.0, 0.0], [-8.97607135772705, -8.976069450378418, -8.642673492431641e-07]]
211 0 : if (*p++ != '[') {
212 0 : return false;
213 : }
214 0 : uint16_t n = 0;
215 0 : struct vector3f_array *v = (struct vector3f_array *)key.ptr;
216 : while (true) {
217 0 : if (n >= v->length) {
218 0 : Vector3f *d = (Vector3f *)realloc(v->data, sizeof(Vector3f)*(n+1));
219 0 : if (d == nullptr) {
220 0 : return false;
221 : }
222 0 : v->data = d;
223 0 : v->length = n+1;
224 : }
225 0 : if (sscanf(p, "[%f, %f, %f]", &v->data[n].x, &v->data[n].y, &v->data[n].z) != 3) {
226 : //printf("Failed to parse Vector3f for %s/%s[%u]\n", key.section, key.key, n);
227 0 : return false;
228 : }
229 : else
230 : {
231 : //printf("GOT %s/%s [%f, %f, %f]\n", key.section, key.key, v->data[n].x, v->data[n].y, v->data[n].z);
232 : }
233 0 : n++;
234 0 : p = strchr(p,']');
235 0 : if (!p) {
236 0 : return false;
237 : }
238 0 : p++;
239 0 : if (p[0] != ',') {
240 0 : break;
241 : }
242 0 : if (p[1] != ' ') {
243 0 : return false;
244 : }
245 0 : p += 2;
246 0 : }
247 0 : if (p[0] != ']') {
248 0 : return false;
249 : }
250 0 : v->length = n;
251 0 : break;
252 : }
253 :
254 0 : case DATA_FLOAT_ARRAY: {
255 : // example: [18.0, 12.694079399108887]
256 0 : if (*p++ != '[') {
257 0 : return false;
258 : }
259 0 : uint16_t n = 0;
260 0 : struct float_array *v = (struct float_array *)key.ptr;
261 : while (true) {
262 0 : if (n >= v->length) {
263 0 : float *d = (float *)realloc(v->data, sizeof(float)*(n+1));
264 0 : if (d == nullptr) {
265 0 : return false;
266 : }
267 0 : v->data = d;
268 0 : v->length = n+1;
269 : }
270 0 : v->data[n] = strtof(p, nullptr);
271 0 : n++;
272 0 : p = strchr(p,',');
273 0 : if (!p) {
274 0 : break;
275 : }
276 0 : p++;
277 0 : }
278 0 : v->length = n;
279 0 : break;
280 : }
281 : }
282 : }
283 :
284 0 : socket_frame_counter++;
285 0 : return true;
286 :
287 : }
288 :
289 : /*
290 : connect to the required sockets
291 : */
292 0 : bool Webots::connect_sockets(void)
293 : {
294 0 : if (!sim_sock) {
295 0 : sim_sock = NEW_NOTHROW SocketAPM_native(false);
296 0 : if (!sim_sock) {
297 0 : AP_HAL::panic("Out of memory for sensors socket");
298 : }
299 0 : if (!sim_sock->connect(webots_ip, webots_sensors_port)) {
300 0 : usleep(100000);
301 0 : if (connect_counter++ == 20) {
302 0 : printf("Waiting to connect to sensors control on %s:%u\n",
303 0 : webots_ip, webots_sensors_port);
304 0 : connect_counter = 0;
305 : }
306 0 : delete sim_sock;
307 0 : sim_sock = nullptr;
308 0 : return false;
309 : }
310 : // wait for Webots packets
311 0 : sim_sock->set_blocking(true);
312 0 : sim_sock->reuseaddress();
313 0 : printf("Sensors connected\n");
314 : }
315 0 : return true;
316 : }
317 :
318 : /*
319 : get any new data from the sensors socket
320 : */
321 0 : bool Webots::sensors_receive(void)
322 : {
323 0 : ssize_t ret = sim_sock->recv(&sensor_buffer[sensor_buffer_len], sizeof(sensor_buffer)-sensor_buffer_len, 0);
324 0 : if (ret <= 0) {
325 0 : return false;
326 : }
327 :
328 : // convert '\n' into nul
329 0 : while (uint8_t *p = (uint8_t *)memchr(&sensor_buffer[sensor_buffer_len], '\n', ret)) {
330 0 : *p = 0;
331 0 : }
332 0 : sensor_buffer_len += ret;
333 :
334 0 : const uint8_t *p2 = (const uint8_t *)memrchr(sensor_buffer, 0, sensor_buffer_len);
335 0 : if (p2 == nullptr || p2 == sensor_buffer) {
336 0 : return false;
337 : }
338 0 : const uint8_t *p1 = (const uint8_t *)memrchr(sensor_buffer, 0, p2 - sensor_buffer);
339 0 : if (p1 == nullptr) {
340 0 : return false;
341 : }
342 :
343 0 : bool parse_ok = parse_sensors((const char *)(p1+1));
344 :
345 0 : memmove(sensor_buffer, p2, sensor_buffer_len - (p2 - sensor_buffer));
346 0 : sensor_buffer_len = sensor_buffer_len - (p2 - sensor_buffer);
347 :
348 0 : return parse_ok;
349 : }
350 :
351 : /*
352 : output control command assuming skid-steering rover
353 : */
354 0 : void Webots::output_rover(const struct sitl_input &input)
355 : {
356 :
357 0 : const float motor1 = 2*((input.servos[0]-1000)/1000.0f - 0.5f);
358 0 : const float motor2 = 2*((input.servos[2]-1000)/1000.0f - 0.5f);
359 :
360 : // construct a JSON packet for v and w
361 : char buf[200];
362 :
363 0 : const int len = snprintf(buf, sizeof(buf)-1, "{\"rover\": [%f, %f], \"wnd\": [%f, %f, %f, %f]}\n",
364 : motor1, motor2,
365 0 : input.wind.speed, wind_ef.x, wind_ef.y, wind_ef.z);
366 :
367 0 : buf[len] = 0;
368 :
369 0 : sim_sock->send(buf, len);
370 0 : }
371 :
372 : /*
373 : output control command assuming a 3 channels motors and 1 channel servo
374 : */
375 0 : void Webots::output_tricopter(const struct sitl_input &input)
376 : {
377 0 : const float max_thrust = 1.0;
378 : float motors[3];
379 0 : const float servo = ((input.servos[6]-1000)/1000.0f - 0.5f);
380 0 : motors[0] = constrain_float(((input.servos[0]-1000)/1000.0f) * max_thrust, 0, max_thrust);
381 0 : motors[1] = constrain_float(((input.servos[1]-1000)/1000.0f) * max_thrust, 0, max_thrust);
382 0 : motors[2] = constrain_float(((input.servos[3]-1000)/1000.0f) * max_thrust, 0, max_thrust);
383 :
384 0 : const float &m_right = motors[0];
385 0 : const float &m_left = motors[1];
386 0 : const float &m_servo = servo ;
387 0 : const float &m_back = motors[2];
388 :
389 : // construct a JSON packet for motors
390 : char buf[200];
391 0 : const int len = snprintf(buf, sizeof(buf)-1, "{\"eng\": [%.3f, %.3f, %.3f, %.3f], \"wnd\": [%f, %3.1f, %1.1f, %2.1f]}\n",
392 0 : m_right, m_left, m_servo, m_back,
393 0 : input.wind.speed, wind_ef.x, wind_ef.y, wind_ef.z);
394 : //printf("\"eng\": [%.3f, %.3f, %.3f, %.3f]\n",m_right, m_left, m_servo, m_back);
395 0 : buf[len] = 0;
396 :
397 0 : sim_sock->send(buf, len);
398 0 : }
399 :
400 :
401 :
402 : /*
403 : output all 16 channels as PWM values. This allows for general
404 : control of a robot
405 : */
406 0 : void Webots::output_pwm(const struct sitl_input &input)
407 : {
408 : char buf[2000];
409 0 : const int len = snprintf(buf, sizeof(buf)-1, "{\"pwm\": [%d.0, %d.0, %d.0, %d.0, %d.0, %d.0, %d.0, %d.0, %d.0, %d.0, %d.0, %d.0, %d.0, %d.0, %d.0, %d.0], \"wnd\": [%3.3f, %f, %3.3f, %3.3f]}\n",
410 0 : input.servos[0], input.servos[1], input.servos[2], input.servos[3],
411 0 : input.servos[4], input.servos[5], input.servos[6], input.servos[7],
412 0 : input.servos[8], input.servos[9], input.servos[10], input.servos[11],
413 0 : input.servos[12], input.servos[13], input.servos[14], input.servos[15],
414 0 : input.wind.speed, wind_ef.x, wind_ef.y, wind_ef.z);
415 0 : buf[len] = 0;
416 0 : sim_sock->send(buf, len);
417 0 : }
418 :
419 :
420 0 : void Webots::output (const struct sitl_input &input)
421 : {
422 :
423 0 : switch (output_type) {
424 0 : case OUTPUT_ROVER:
425 0 : output_rover(input);
426 0 : break;
427 0 : case OUTPUT_QUAD:
428 0 : output_pwm(input);
429 0 : break;
430 0 : case OUTPUT_TRICOPTER:
431 0 : output_tricopter(input);
432 0 : break;
433 0 : case OUTPUT_PWM:
434 0 : output_pwm(input);
435 0 : break;
436 : }
437 0 : }
438 :
439 : /*
440 : update the Webots simulation by one time step
441 : */
442 0 : void Webots::update(const struct sitl_input &input)
443 : {
444 : static bool first = true;
445 0 : update_battery();
446 :
447 0 : if (!connect_sockets()) {
448 0 : return;
449 : }
450 :
451 0 : const bool valid = sensors_receive();
452 0 : if (!valid)
453 : {
454 0 : return ;
455 : }
456 :
457 :
458 : //time frame from simulator
459 0 : frame_time_us = ((state.timestamp - last_state.timestamp) * 1.0e6f); //HERE
460 0 : if ((!first) && (frame_time_us ==0))
461 : {
462 0 : first = false;
463 0 : printf("frame_time_us Zero \n");
464 0 : output(input);
465 0 : return ;
466 : }
467 :
468 0 : time_now_us += frame_time_us;
469 :
470 :
471 0 : if (valid)
472 : {
473 :
474 : // convert from state variables to ardupilot conventions
475 0 : dcm.from_euler(state.pose.roll, state.pose.pitch, -state.pose.yaw);
476 :
477 0 : gyro = Vector3f(state.imu.angular_velocity[0] ,
478 0 : state.imu.angular_velocity[1] ,
479 0 : -state.imu.angular_velocity[2] );
480 :
481 0 : accel_body = Vector3f(+state.imu.linear_acceleration[0],
482 0 : +state.imu.linear_acceleration[1],
483 0 : -state.imu.linear_acceleration[2]);
484 :
485 0 : velocity_ef = Vector3f(+state.velocity.world_linear_velocity[0],
486 0 : +state.velocity.world_linear_velocity[1],
487 0 : -state.velocity.world_linear_velocity[2]);
488 :
489 0 : position = Vector3d(state.gps.x, state.gps.y, -state.gps.z);
490 0 : position.xy() += origin.get_distance_NE_double(home);
491 :
492 : // limit to 16G to match pixhawk1
493 0 : float a_limit = GRAVITY_MSS*16;
494 0 : accel_body.x = constrain_float(accel_body.x, -a_limit, a_limit);
495 0 : accel_body.y = constrain_float(accel_body.y, -a_limit, a_limit);
496 0 : accel_body.z = constrain_float(accel_body.z, -a_limit, a_limit);
497 :
498 : // fill in laser scanner results, if available
499 0 : scanner.points = state.scanner.points;
500 0 : scanner.ranges = state.scanner.ranges;
501 :
502 0 : update_position();
503 :
504 : // update magnetic field
505 0 : update_mag_field_bf();
506 :
507 0 : time_advance();
508 :
509 0 : update_wind (input);
510 :
511 :
512 :
513 : //report_FPS();
514 : }
515 :
516 0 : output(input);
517 :
518 0 : last_state = state;
519 :
520 : }
521 :
522 :
523 : /*
524 : report frame rates
525 : */
526 0 : void Webots::report_FPS(void)
527 : {
528 : // if (frame_counter++ % 1000 == 0) {
529 : // if (!is_zero(last_frame_count_s)) {
530 : // uint64_t frames = socket_frame_counter - last_socket_frame_counter;
531 : // last_socket_frame_counter = socket_frame_counter;
532 : // double dt = state.timestamp - last_frame_count_s;
533 : // printf("%.2f/%.2f FPS avg=%.2f\n",
534 : // frames / dt, 1000 / dt, 1.0/average_frame_time_s);
535 : // } else {
536 : // printf("Initial position %f %f %f\n", position.x, position.y, position.z);
537 : // }
538 : // last_frame_count_s = state.timestamp;
539 : // }
540 0 : }
541 :
542 : #endif // AP_SIM_WEBOTS_ENABLED
|
