Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

SITL: add Webots support and examples

This commit is contained in:
mhefny 2019-08-14 14:49:08 +02:00 committed by Andrew Tridgell
parent 42cbb370f9
commit 1e41f7cc61
36 changed files with 10450 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.gitignore vendored
View File

@ -113,4 +113,5 @@ cmake-build-*/
/reports/
/GCOV_*.log
way.txt
*.wbproj
*.wbproj

581
libraries/SITL/SIM_Webots.cpp Normal file
View File

@ -0,0 +1,581 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have Weboreceived a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
simulator connector for webots simulator
*/
#include "SIM_Webots.h"
#include <arpa/inet.h>
#include <errno.h>
#include <fcntl.h>
#include <stdio.h>
#include <stdarg.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_Logger/AP_Logger.h>
#include "pthread.h"
#include <AP_HAL/utility/replace.h>
extern const AP_HAL::HAL& hal;
using namespace SITL;
static const struct {
const char *name;
float value;
bool save;
} sim_defaults[] = {
{ "AHRS_EKF_TYPE", 10 },
{ "INS_GYR_CAL", 0 },
{ "RC1_MIN", 1000, true },
{ "RC1_MAX", 2000, true },
{ "RC2_MIN", 1000, true },
{ "RC2_MAX", 2000, true },
{ "RC3_MIN", 1000, true },
{ "RC3_MAX", 2000, true },
{ "RC4_MIN", 1000, true },
{ "RC4_MAX", 2000, true },
{ "RC2_REVERSED", 1 }, // interlink has reversed rc2
{ "SERVO1_MIN", 1000 },
{ "SERVO1_MAX", 2000 },
{ "SERVO2_MIN", 1000 },
{ "SERVO2_MAX", 2000 },
{ "SERVO3_MIN", 1000 },
{ "SERVO3_MAX", 2000 },
{ "SERVO4_MIN", 1000 },
{ "SERVO4_MAX", 2000 },
{ "SERVO5_MIN", 1000 },
{ "SERVO5_MAX", 2000 },
{ "SERVO6_MIN", 1000 },
{ "SERVO6_MAX", 2000 },
{ "SERVO6_MIN", 1000 },
{ "SERVO6_MAX", 2000 },
{ "INS_ACC2OFFS_X", 0.001 },
{ "INS_ACC2OFFS_Y", 0.001 },
{ "INS_ACC2OFFS_Z", 0.001 },
{ "INS_ACC2SCAL_X", 1.001 },
{ "INS_ACC2SCAL_Y", 1.001 },
{ "INS_ACC2SCAL_Z", 1.001 },
{ "INS_ACCOFFS_X", 0.001 },
{ "INS_ACCOFFS_Y", 0.001 },
{ "INS_ACCOFFS_Z", 0.001 },
{ "INS_ACCSCAL_X", 1.001 },
{ "INS_ACCSCAL_Y", 1.001 },
{ "INS_ACCSCAL_Z", 1.001 },
};
Webots::Webots(const char *frame_str) :
Aircraft(frame_str)
{
use_time_sync = true;
rate_hz = 4000;
char *saveptr = nullptr;
char *s = strdup(frame_str);
char *frame_option = strtok_r(s, ":", &saveptr);
char *args1 = strtok_r(nullptr, ":", &saveptr);
char *args2 = strtok_r(nullptr, ":", &saveptr);
/*
allow setting of IP, sensors port and control port
format morse:IPADDRESS:SENSORS_PORT:CONTROL_PORT
*/
if (args1) {
webots_ip = args1;
}
if (args2) {
webots_sensors_port = atoi(args2);
}
if (strstr(frame_option, "-rover")) {
output_type = OUTPUT_ROVER;
} else if (strstr(frame_option, "-quad")) {
output_type = OUTPUT_QUAD;
} else if (strstr(frame_option, "-pwm")) {
output_type = OUTPUT_PWM;
} else {
// default to rover
output_type = OUTPUT_ROVER;
}
for (uint8_t i=0; i<ARRAY_SIZE(sim_defaults); i++) {
AP_Param::set_default_by_name(sim_defaults[i].name, sim_defaults[i].value);
if (sim_defaults[i].save) {
enum ap_var_type ptype;
AP_Param *p = AP_Param::find(sim_defaults[i].name, &ptype);
if (!p->configured()) {
p->save();
}
}
}
printf("Started Webots with %s:%u type %u\n",
webots_ip, webots_sensors_port,
(unsigned)output_type);
}
/*
very simple JSON parser for sensor data
called with pointer to one row of sensor data, nul terminated
This parser does not do any syntax checking, and is not at all
general purpose
{"timestamp": 1563474924.817575,
"vehicle.imu": {"timestamp": 1563474924.8009083,
"angular_velocity": [2.319516170246061e-06, -3.5830129263558774e-07, 7.009341995711793e-09],
"linear_acceleration": [0.005075275432318449, 0.22471635043621063, 9.80748176574707],
"magnetic_field": [23088.65625, 3875.89453125, -53204.51171875]},
"vehicle.gps": {"timestamp": 1563474924.8009083, "x": 5.386466364143416e-05, "y": -0.0010969983413815498, "z": 0.03717954829335213},
"vehicle.velocity": {"timestamp": 1563474924.8009083,
"linear_velocity": [4.818238585890811e-10, 2.1333558919423012e-08, 9.310780910709582e-07],
"angular_velocity": [2.319516170246061e-06, -3.5830129263558774e-07, 7.009341995711793e-09],
"world_linear_velocity": [5.551115123125783e-17, 0.0, 9.313225746154785e-07]},
"vehicle.pose": {"timestamp": 1563474924.8009083,
"x": 5.386466364143416e-05, "y": -0.0010969983413815498, "z": 0.03717954829335213,
"yaw": 7.137723878258839e-05, "pitch": -0.0005173543468117714, "roll": 0.022908739745616913}}
*/
bool Webots::parse_sensors(const char *json)
{
//printf("%s\n", json);
///*
for (uint16_t i=0; i<ARRAY_SIZE(keytable); i++) {
struct keytable &key = keytable[i];
//printf("search %s/%s\n", key.section, key.key);
// look for section header
const char *p = strstr(json, key.section);
if (!p) {
// we don't have this sensor
continue;
}
p += strlen(key.section)+1;
// find key inside section
p = strstr(p, key.key);
if (!p) {
printf("Failed to find key %s/%s\n", key.section, key.key);
return false;
}
p += strlen(key.key)+3;
switch (key.type) {
case DATA_FLOAT:
*((float *)key.ptr) = atof(p);
//printf("GOT %s/%s value: %f\n", key.section, key.key, *((float *)key.ptr));
break;
case DATA_DOUBLE:
*((double *)key.ptr) = atof(p);
//printf("GOT %s/%s value: %f\n", key.section, key.key, *((double *)key.ptr));
break;
case DATA_VECTOR3F: {
Vector3f *v = (Vector3f *)key.ptr;
if (sscanf(p, "[%f, %f, %f]", &v->x, &v->y, &v->z) != 3) {
printf("Failed to parse Vector3f for %s %s/%s\n",p, key.section, key.key);
//printf("Failed to parse Vector3f for %s/%s\n", key.section, key.key);
return false;
}
else
{
//printf("GOT %s/%s [%f, %f, %f]\n", key.section, key.key, v->x, v->y, v->z);
}
break;
}
case DATA_VECTOR3F_ARRAY: {
// example: [[0.0, 0.0, 0.0], [-8.97607135772705, -8.976069450378418, -8.642673492431641e-07]]
if (*p++ != '[') {
return false;
}
uint16_t n = 0;
struct vector3f_array *v = (struct vector3f_array *)key.ptr;
while (true) {
if (n >= v->length) {
Vector3f *d = (Vector3f *)realloc(v->data, sizeof(Vector3f)*(n+1));
if (d == nullptr) {
return false;
}
v->data = d;
v->length = n+1;
}
if (sscanf(p, "[%f, %f, %f]", &v->data[n].x, &v->data[n].y, &v->data[n].z) != 3) {
//printf("Failed to parse Vector3f for %s %s/%s[%u]\n",p, key.section, key.key, n);
//printf("Failed to parse Vector3f for %s/%s[%u]\n", key.section, key.key, n);
return false;
}
else
{
//printf("GOT %s/%s [%f, %f, %f]\n", key.section, key.key, v->data[n].x, v->data[n].y, v->data[n].z);
}
n++;
p = strchr(p,']');
if (!p) {
return false;
}
p++;
if (p[0] != ',') {
break;
}
if (p[1] != ' ') {
return false;
}
p += 2;
}
if (p[0] != ']') {
return false;
}
v->length = n;
break;
}
case DATA_FLOAT_ARRAY: {
// example: [18.0, 12.694079399108887]
if (*p++ != '[') {
return false;
}
uint16_t n = 0;
struct float_array *v = (struct float_array *)key.ptr;
while (true) {
if (n >= v->length) {
float *d = (float *)realloc(v->data, sizeof(float)*(n+1));
if (d == nullptr) {
return false;
}
v->data = d;
v->length = n+1;
}
v->data[n] = atof(p);
n++;
p = strchr(p,',');
if (!p) {
break;
}
p++;
}
v->length = n;
break;
}
}
}
// */
socket_frame_counter++;
return true;
}
/*
connect to the required sockets
*/
bool Webots::connect_sockets(void)
{
if (!sim_sock) {
sim_sock = new SocketAPM(false);
if (!sim_sock) {
AP_HAL::panic("Out of memory for sensors socket");
}
if (!sim_sock->connect(webots_ip, webots_sensors_port)) {
usleep(100000);
if (connect_counter++ == 20) {
printf("Waiting to connect to sensors control on %s:%u\n",
webots_ip, webots_sensors_port);
connect_counter = 0;
}
delete sim_sock;
sim_sock = nullptr;
return false;
}
sim_sock->reuseaddress();
printf("Sensors connected\n");
}
return true;
}
/*
get any new data from the sensors socket
*/
bool Webots::sensors_receive(void)
{
ssize_t ret = sim_sock->recv(&sensor_buffer[sensor_buffer_len], sizeof(sensor_buffer)-sensor_buffer_len, 0);
if (ret <= 0) {
no_data_counter++;
if (no_data_counter == 1000) {
no_data_counter = 0;
delete sim_sock;
sim_sock = nullptr;
}
return false;
}
no_data_counter = 0;
// convert '\n' into nul
while (uint8_t *p = (uint8_t *)memchr(&sensor_buffer[sensor_buffer_len], '\n', ret)) {
*p = 0;
}
sensor_buffer_len += ret;
const uint8_t *p2 = (const uint8_t *)memrchr(sensor_buffer, 0, sensor_buffer_len);
if (p2 == nullptr || p2 == sensor_buffer) {
return false;
}
const uint8_t *p1 = (const uint8_t *)memrchr(sensor_buffer, 0, p2 - sensor_buffer);
if (p1 == nullptr) {
return false;
}
bool parse_ok = parse_sensors((const char *)(p1+1));
memmove(sensor_buffer, p2, sensor_buffer_len - (p2 - sensor_buffer));
sensor_buffer_len = sensor_buffer_len - (p2 - sensor_buffer);
return parse_ok;
}
/*
output control command assuming skid-steering rover
*/
void Webots::output_rover(const struct sitl_input &input)
{
const float motor1 = 2*((input.servos[0]-1000)/1000.0f - 0.5f);
const float motor2 = 2*((input.servos[2]-1000)/1000.0f - 0.5f);
// construct a JSON packet for v and w
char buf[60];
snprintf(buf, sizeof(buf)-1, "{\"rover\": [%f, %f]}\n",
motor1, motor2);
//printf("rover motors m1: %f m2: %f\n", steer_angle, speed_ms);
buf[sizeof(buf)-1] = 0;
sim_sock->send(buf, strlen(buf));
}
/*
output control command assuming a 4 channel quad
*/
void Webots::output_quad(const struct sitl_input &input)
{
const float max_thrust = 1.0;
float motors[4];
for (uint8_t i=0; i<4; i++) {
//return a filtered servo input as a value from 0 to 1
//servo is assumed to be 1000 to 2000
motors[i] = constrain_float(((input.servos[i]-1000)/1000.0f) * max_thrust, 0, max_thrust);
}
const float &m_right = motors[0];
const float &m_left = motors[1];
const float &m_front = motors[2];
const float &m_back = motors[3];
// quad format in Webots is:
// m1: front
// m2: right
// m3: back
// m4: left
// construct a JSON packet for motors
char buf[60];
snprintf(buf, sizeof(buf)-1, "{\"engines\": [%f, %f, %f, %f]}\n",
m_front, m_right, m_back, m_left);
buf[sizeof(buf)-1] = 0;
sim_sock->send(buf, strlen(buf));
}
/*
output all 16 channels as PWM values. This allows for general
control of a robot
*/
void Webots::output_pwm(const struct sitl_input &input)
{
char buf[200];
snprintf(buf, sizeof(buf)-1, "{\"pwm\": [%u, %uf, %u, %u, %u, %uf, %u, %u, %u, %uf, %u, %u, %u, %uf, %u, %u]}\n",
input.servos[0], input.servos[1], input.servos[2], input.servos[3],
input.servos[4], input.servos[5], input.servos[6], input.servos[7],
input.servos[8], input.servos[9], input.servos[10], input.servos[11],
input.servos[12], input.servos[13], input.servos[14], input.servos[15]);
buf[sizeof(buf)-1] = 0;
sim_sock->send(buf, strlen(buf));
}
void Webots::output (const struct sitl_input &input)
{
switch (output_type) {
case OUTPUT_ROVER:
output_rover(input);
break;
case OUTPUT_QUAD:
output_quad(input);
break;
case OUTPUT_PWM:
output_pwm(input);
break;
}
}
/*
update the Webots simulation by one time step
*/
void Webots::update(const struct sitl_input &input)
{
if (!connect_sockets()) {
return;
}
last_state = state;
const bool valid = sensors_receive();
if (valid) {
// update average frame time used for extrapolation
double dt = constrain_float(state.timestamp - last_state.timestamp, 0.001, 1.0/50);
if (average_frame_time_s < 1.0e-6) {
//if average is too small take the current dt as a good start.
average_frame_time_s = dt;
}
// this is complementry filter for updating average.
average_frame_time_s = average_frame_time_s * 0.98 + dt * 0.02;
}
// again measure dt as dt_s but with no constraints....
double dt_s = state.timestamp - last_state.timestamp;
if (dt_s < 0 || dt_s > 1) {
// cope with restarting while connected
initial_time_s = time_now_us * 1.0e-6f;
return;
}
if (dt_s < 0.00001f) {
float delta_time = 0.001;
// don't go past the next expected frame
if (delta_time + extrapolated_s > average_frame_time_s) {
delta_time = average_frame_time_s - extrapolated_s;
}
// check if extrapolation_s duration is longer than average_frame_time_s
if (delta_time <= 0) {
// dont extrapolate anymore untill there is valid data with long-enough dt_s
usleep(1000);
return;
}
// extrapolated_s duration is safe. keep on extrapolation.
time_now_us += delta_time * 1.0e6;
extrapolate_sensors(delta_time);
update_position();
//update body magnetic field from position and rotation
update_mag_field_bf();
usleep(delta_time * 1.0e6);
extrapolated_s += delta_time;
//output(input);
report_FPS();
return;
}
if (valid)
{
// reset extrapolation duration.
extrapolated_s = 0;
if (initial_time_s <= 0) {
dt_s = 0.001f;
initial_time_s = state.timestamp - dt_s;
}
// convert from state variables to ardupilot conventions
dcm.from_euler(state.pose.roll, state.pose.pitch, -state.pose.yaw);
gyro = Vector3f(state.imu.angular_velocity[0] ,
state.imu.angular_velocity[1] ,
-state.imu.angular_velocity[2] );
accel_body = Vector3f(+state.imu.linear_acceleration[0],
+state.imu.linear_acceleration[1],
-state.imu.linear_acceleration[2]);
velocity_ef = Vector3f(+state.velocity.world_linear_velocity[0],
+state.velocity.world_linear_velocity[1],
-state.velocity.world_linear_velocity[2]);
position = Vector3f(state.gps.x, state.gps.y, -state.gps.z);
// limit to 16G to match pixhawk1
float a_limit = GRAVITY_MSS*16;
accel_body.x = constrain_float(accel_body.x, -a_limit, a_limit);
accel_body.y = constrain_float(accel_body.y, -a_limit, a_limit);
accel_body.z = constrain_float(accel_body.z, -a_limit, a_limit);
// fill in laser scanner results, if available
scanner.points = state.scanner.points;
scanner.ranges = state.scanner.ranges;
update_position();
uint64_t new_time_us = (state.timestamp - initial_time_s)*1.0e6;
if (new_time_us < time_now_us) {
uint64_t dt_us = time_now_us - new_time_us;
if (dt_us > 500000) {
// time going backwards
time_now_us = new_time_us;
}
} else {
// update SITL time with webots time.
time_now_us = new_time_us;
}
time_advance();
// update magnetic field
update_mag_field_bf();
output(input);
report_FPS();
}
}
/*
report frame rates
*/
void Webots::report_FPS(void)
{
if (frame_counter++ % 1000 == 0) {
if (!is_zero(last_frame_count_s)) {
uint64_t frames = socket_frame_counter - last_socket_frame_counter;
last_socket_frame_counter = socket_frame_counter;
double dt = state.timestamp - last_frame_count_s;
printf("%.2f/%.2f FPS avg=%.2f\n",
frames / dt, 1000 / dt, 1.0/average_frame_time_s);
} else {
printf("Initial position %f %f %f\n", position.x, position.y, position.z);
}
last_frame_count_s = state.timestamp;
}
}

140
libraries/SITL/SIM_Webots.h Normal file
View File

@ -0,0 +1,140 @@
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
simulator connection for webots simulator https://github.com/omichel/webots
*/
#pragma once
#include <cmath>
#include <AP_HAL/utility/Socket.h>
#include "SIM_Aircraft.h"
namespace SITL {
/*
simulation interface
*/
class Webots : public Aircraft {
public:
Webots(const char *frame_str);
/* update model by one time step */
void update(const struct sitl_input &input) override;
/* output servo to simulator */
void output(const struct sitl_input &input);
/* static object creator */
static Aircraft *create(const char *frame_str) {
return new Webots(frame_str);
}
private:
const char *webots_ip = "127.0.0.1";
// assume sensors are streamed on port 5599
uint16_t webots_sensors_port = 5599;
enum {
OUTPUT_ROVER=1,
OUTPUT_QUAD=2,
OUTPUT_PWM=3
} output_type;
bool connect_sockets(void);
bool parse_sensors(const char *json);
bool sensors_receive(void);
void output_rover(const struct sitl_input &input);
void output_quad(const struct sitl_input &input);
void output_pwm(const struct sitl_input &input);
void report_FPS();
// buffer for parsing pose data in JSON format
uint8_t sensor_buffer[50000];
uint32_t sensor_buffer_len;
SocketAPM *sim_sock;
uint32_t no_data_counter;
uint32_t connect_counter;
double initial_time_s;
double time_diff;
double extrapolated_s;
double average_frame_time_s;
uint64_t socket_frame_counter;
uint64_t last_socket_frame_counter;
uint64_t frame_counter;
double last_frame_count_s;
enum data_type {
DATA_FLOAT,
DATA_DOUBLE,
DATA_VECTOR3F,
DATA_VECTOR3F_ARRAY,
DATA_FLOAT_ARRAY,
};
struct {
double timestamp;
struct {
Vector3f angular_velocity;
Vector3f linear_acceleration;
Vector3f magnetic_field;
} imu;
struct {
float x, y, z;
} gps;
struct {
float roll, pitch, yaw;
} pose;
struct {
Vector3f world_linear_velocity;
} velocity;
struct {
struct vector3f_array points;
struct float_array ranges;
} scanner;
} state, last_state;
// table to aid parsing of JSON sensor data
struct keytable {
const char *section;
const char *key;
void *ptr;
enum data_type type;
} keytable[13] = {
{ "", "ts", &state.timestamp, DATA_DOUBLE },
{ ".imu", "av", &state.imu.angular_velocity, DATA_VECTOR3F },
{ ".imu", "la", &state.imu.linear_acceleration, DATA_VECTOR3F },
{ ".imu", "mf", &state.imu.magnetic_field, DATA_VECTOR3F },
{ ".gps", "x", &state.gps.x, DATA_FLOAT },
{ ".gps", "y", &state.gps.y, DATA_FLOAT },
{ ".gps", "z", &state.gps.z, DATA_FLOAT },
{ ".pose", "roll", &state.pose.roll, DATA_FLOAT },
{ ".pose", "pitch", &state.pose.pitch, DATA_FLOAT },
{ ".pose", "yaw", &state.pose.yaw, DATA_FLOAT },
{ ".velocity", "wlv", &state.velocity.world_linear_velocity, DATA_VECTOR3F },
{ ".scan", "pl", &state.scanner.points, DATA_VECTOR3F_ARRAY },
{ ".scan", "rl", &state.scanner.ranges, DATA_FLOAT_ARRAY },
};
};
} // namespace SITL

22
libraries/SITL/examples/Webots/README.md Normal file
View File

@ -0,0 +1,22 @@
# Using SITL with Webots
[Webots](https://www.cyberbotics.com/#webots "Webots") is an open source robot simulator that provides a complete development environment to model, program and simulate robots. Thousands of institutions worldwide use it for R&D and teaching. Webots has been codeveloped by the Swiss Federal Institute of Technology in Lausanne, thoroughly tested, well documented and continuously maintained since 1996.
#### Installing Webots
Please check this [page](https://www.cyberbotics.com/download "page"). The steps is very easy and straight forward.
#### Running Simulator
1- open webots and open file libraries/SITL/examples/Webots/worlds/webots_quadPlus.wbt
2- press "run" button.
3- run ./libraries/SITL/examples/Webots/drone.sh
please note that to re-run the simulator you need to stop ardupilot SITL then stop webots simulator "stop button". then press "run" button on webots and then rerun ardupilot SITL.
#### Simulation using Map Street
You can use [OpenStreetMaps](https://www.openstreetmap.org/ "OpenStreetMaps") with [Webots](https://cyberbotics.com/doc/automobile/openstreetmap-importer "Webots"), it is fairly straight forward. CAUTION: when creating worlds using **osm_importer** world "northDirection" point to [0 0 1] instead of [1 0 0] and this leads to changes in axis that corrupt the readings. Webots controller insternally takes care of this issue as you can see in **./libraries/SITL/examples/Webots/worlds/pyramidMapReduced2.wbt**
[![Watch the video] Flying at Giza Pyramids](https://www.youtube.com/embed/c5CJaRH9Pig)

75
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/Makefile Normal file
View File

@ -0,0 +1,75 @@
# Copyright 1996-2018 Cyberbotics Ltd.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
### Generic Makefile.include for Webots controllers, physics plugins, robot
### window libraries, remote control libraries and other libraries
### to be used with GNU make
###
### Platforms: Windows, macOS, Linux
### Languages: C, C++
###
### Authors: Olivier Michel, Yvan Bourquin, Fabien Rohrer
### Edmund Ronald, Sergei Poskriakov
###
###-----------------------------------------------------------------------------
###
### This file is meant to be included from the Makefile files located in the
### Webots projects subdirectories. It is possible to set a number of variables
### to customize the build process, i.e., add source files, compilation flags,
### include paths, libraries, etc. These variables should be set in your local
### Makefile just before including this Makefile.include. This Makefile.include
### should never be modified.
###
### Here is a description of the variables you may set in your local Makefile:
###
### ---- C Sources ----
### if your program uses several C source files:
### C_SOURCES = my_plugin.c my_clever_algo.c my_graphics.c
###
### ---- C++ Sources ----
### if your program uses several C++ source files:
### CXX_SOURCES = my_plugin.cc my_clever_algo.cpp my_graphics.c++
###
### ---- Compilation options ----
### if special compilation flags are necessary:
### CFLAGS = -Wno-unused-result
###
### ---- Linked libraries ----
### if your program needs additional libraries:
### INCLUDE = -I"/my_library_path/include"
### LIBRARIES = -L"/path/to/my/library" -lmy_library -lmy_other_library
###
### ---- Linking options ----
### if special linking flags are needed:
### LFLAGS = -s
###
### ---- Webots included libraries ----
### if you want to use the Webots C API in your C++ controller program:
### USE_C_API = true
### if you want to link with the Qt framework embedded in Webots:
### QT = core gui widgets network
###
### ---- Debug mode ----
### if you want to display the gcc command line for compilation and link, as
### well as the rm command details used for cleaning:
### VERBOSE = 1
###
###-----------------------------------------------------------------------------
C_SOURCES = ardupilot_SITL_QUAD.c sockets.c sensors.c
INCLUDE = -I"./"
### Do not modify: this includes Webots global Makefile.include
space :=
space +=
WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME))))
include $(WEBOTS_HOME_PATH)/resources/Makefile.include

BIN
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD Executable file
View File

Binary file not shown.

497
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.c Normal file
View File

@ -0,0 +1,497 @@
/*
* File: ardupilot_SITL_QUAD.c
* Date: 29 July 2019
* Description: integration with ardupilot SITL simulation.
* Author: M.S.Hefny (HefnySco)
* Modifications:
*/
/*
Data is sent in format:
{"timestamp": 1561043647.7598028,
"vehicle.imu": {"timestamp": 1561043647.7431362,
"angular_velocity": [-8.910427823138889e-06, 1.6135254554683343e-06, 0.0005768465343862772],
"linear_acceleration": [-0.06396577507257462, 0.22235631942749023, 9.807276725769043],
"magnetic_field": [23662.052734375, 2878.55859375, -53016.55859375]},
"vehicle.gps": {"timestamp": 1561043647.7431362, "x": -0.0027823783457279205, "y": -0.026340210810303688, "z": 0.159392312169075},
"vehicle.velocity": {"timestamp": 1561043647.7431362, "linear_velocity": [-6.0340113122947514e-05, -2.264878639834933e-05, 9.702569059300004e-07],
"angular_velocity": [-8.910427823138889e-06, 1.6135254554683343e-06, 0.0005768465343862772],
"world_linear_velocity": [-5.9287678595865145e-05, -2.5280191039200872e-05, 8.493661880493164e-07]},
"vehicle.pose": {"timestamp": 1561043647.7431362, "x": -0.0027823783457279205, "y": -0.026340210810303688, "z": 0.159392312169075, "yaw": 0.04371734336018562, "pitch": 0.0065115075558424, "roll": 0.022675735875964165}}
*/
/*
* You may need to add include files like <webots/distance_sensor.h> or
* <webots/differential_wheels.h>, etc.
*/
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <webots/robot.h>
#include <webots/supervisor.h>
#include "ardupilot_SITL_QUAD.h"
#include "sockets.h"
#include "sensors.h"
#define MOTOR_NUM 4
static WbDeviceTag motors[MOTOR_NUM];
static WbDeviceTag gyro;
static WbDeviceTag accelerometer;
static WbDeviceTag compass;
static WbDeviceTag gps;
static WbDeviceTag camera;
static WbDeviceTag inertialUnit;
static WbNodeRef world_info;
static const double *northDirection;
static double v[MOTOR_NUM];
int port;
static int timestep;
#ifdef DEBUG_USE_KB
/*
// Code used tp simulae motors using keys to make sure that sensors directions and motor torques and thrusts are all correct.
// You can start this controller and use telnet instead of SITL to start the simulator.
Then you can use Keyboard to emulate motor input.
*/
void process_keyboard ()
{
switch (wb_keyboard_get_key())
{
case 'Q': // Q key -> up & left
v[0] = 0.0;
v[1] = 0.0;
v[2] = 0.0;
v[3] = 0.0;
break;
case 'Y':
v[0] = v[0] + 0.01;
v[2] = v[2] - 0.01;
break;
case 'H':
v[0] = v[0] - 0.01;
v[2] = v[2] + 0.01;
break;
case 'G':
v[1] = v[1] + 0.01;
v[3] = v[3] - 0.01;
break;
case 'J':
v[1] = v[1] - 0.01;
v[3] = v[3] + 0.01;
break;
case 'W':
for (int i=0; i<MOTOR_NUM;++i)
{
v[i] += 0.01;
}
break;
case 'S':
for (int i=0; i<MOTOR_NUM;++i)
{
v[i] -= 0.01;
}
break;
case 'A':
v[1] = v[1] + 0.01;
v[3] = v[3] + 0.01;
v[0] = v[0] - 0.01;
v[2] = v[2] - 0.01;
break;
case 'D':
v[1] = v[1] - 0.01;
v[3] = v[3] - 0.01;
v[0] = v[0] + 0.01;
v[2] = v[2] + 0.01;
break;
}
for (int i=0; i< MOTOR_NUM; ++i)
{
if (v[i] <=0) v[i]=0;
if (v[i] >=600) v[i]=10;
wb_motor_set_position(motors[i], INFINITY);
wb_motor_set_velocity(motors[i], v[i]);
}
printf ("Motors Internal %f %f %f %f\n", v[0],v[1],v[2],v[3]);
}
#endif
/*
// apply motor thrust.
*/
void update_controls()
{
/*
1 N = 101.9716213 grams force
Thrust = t1 * |omega| * omega - t2 * |omega| * V
Where t1 and t2 are the constants specified in the thrustConstants field,
omega is the motor angular velocity
and V is the component of the linear velocity of the center of thrust along the shaft axis.
if Vehicle mass = 1 Kg. and we want omega = 1.0 to hover
then (mass / 0.10197) / (4 motors) = t1
LINEAR_THRUST
we also want throttle to be linear with thrust so we use sqrt to calculate omega from input.
*/
static float factor = 1.0f;
static float offset = 0.0f;
static float v[4];
#ifdef LINEAR_THRUST
v[0] = sqrt(state.motors.w ) * factor + offset;
v[1] = sqrt(state.motors.x ) * factor + offset;
v[2] = sqrt(state.motors.y ) * factor + offset;
v[3] = sqrt(state.motors.z ) * factor + offset;
#else
v[0] = (state.motors.w ) * factor + offset;
v[1] = (state.motors.x ) * factor + offset;
v[2] = (state.motors.y ) * factor + offset;
v[3] = (state.motors.z ) * factor + offset;
#endif
for ( int i=0; i<4; ++i)
{
wb_motor_set_position(motors[i], INFINITY);
wb_motor_set_velocity(motors[i], v[i]);
}
#ifdef DEBUG_MOTORS
printf ("RAW F:%f R:%f B:%f L:%f\n", state.motors.w, state.motors.x, state.motors.y, state.motors.z);
printf ("Motors F:%f R:%f B:%f L:%f\n", v[0], v[1], v[2], v[3]);
#endif
}
// data example: [my_controller_SITL] {"engines": [0.000, 0.000, 0.000, 0.000]}
// the JSON parser is directly inspired by https://github.com/ArduPilot/ardupilot/blob/master/libraries/SITL/SIM_Morse.cpp
bool parse_controls(const char *json)
{
//state.timestamp = 1.0;
#ifdef DEBUG_INPUT_DATA
printf("%s\n", json);
#endif
for (uint16_t i=0; i < ARRAY_SIZE(keytable); i++) {
struct keytable *key;
key = &keytable[i];
//printf("search %s/%s\n", key->section, key->key);
// look for section header
const char *p = strstr(json, key->section);
if (!p) {
// we don't have this sensor
continue;
}
p += strlen(key->section)+1;
// find key inside section
p = strstr(p, key->key);
if (!p) {
printf("Failed to find key %s/%s\n", key->section, key->key);
return false;
}
p += strlen(key->key)+3;
switch (key->type)
{
case DATA_FLOAT:
*((float *)key->ptr) = atof(p);
#ifdef DEBUG_INPUT_DATA
printf("GOT %s/%s\n", key->section, key->key);
#endif
break;
case DATA_DOUBLE:
*((double *)key->ptr) = atof(p);
#ifdef DEBUG_INPUT_DATA
printf("GOT %s/%s\n", key->section, key->key);
#endif
break;
case DATA_VECTOR4F: {
VECTOR4F *v = (VECTOR4F *)key->ptr;
if (sscanf(p, "[%f, %f, %f, %f]", &(v->w), &(v->x), &(v->y), &(v->z)) != 4) {
printf("Failed to parse Vector3f for %s %s/%s\n",p, key->section, key->key);
return false;
}
else
{
#ifdef DEBUG_INPUT_DATA
printf("GOT %s/%s\n[%f, %f, %f, %f]\n ", key->section, key->key,v->w,v->x,v->y,v->z);
#endif
}
break;
}
}
}
return true;
}
void run ()
{
char send_buf[1000]; //1000 just a safe margin
char command_buffer[200];
fd_set rfds;
while (wb_robot_step(timestep) != -1)
{
#ifdef DEBUG_USE_KB
process_keyboard();
#endif
if (fd == 0)
{
// if no socket wait till you get a socket
fd = socket_accept(sfd);
if (fd > 0)
socket_set_non_blocking(fd);
else if (fd < 0)
break;
}
getAllSensors ((char *)send_buf, northDirection, gyro,accelerometer,compass,gps, inertialUnit);
#ifdef DEBUG_SENSORS
printf("%s\n",send_buf);
#endif
if (write(fd,send_buf,strlen(send_buf)) <= 0)
{
printf ("Send Data Error\n");
}
if (fd)
{
FD_ZERO(&rfds);
FD_SET(fd, &rfds);
struct timeval tv;
tv.tv_sec = 0.05;
tv.tv_usec = 0;
int number = select(fd + 1, &rfds, NULL, NULL, &tv);
if (number != 0)
{
// there is a valid connection
int n = recv(fd, (char *)command_buffer, 200, 0);
if (n < 0) {
#ifdef _WIN32
int e = WSAGetLastError();
if (e == WSAECONNABORTED)
fprintf(stderr, "Connection aborted.\n");
else if (e == WSAECONNRESET)
fprintf(stderr, "Connection reset.\n");
else
fprintf(stderr, "Error reading from socket: %d.\n", e);
#else
if (errno)
fprintf(stderr, "Error reading from socket: %d.\n", errno);
#endif
break;
}
if (n==0)
{
break;
}
if (command_buffer[0] == 'e')
{
break;
}
if (n > 0)
{
//printf("Received %d bytes:\n", n);
command_buffer[n] = 0;
parse_controls (command_buffer);
update_controls();
}
}
}
}
socket_cleanup();
}
void initialize (int argc, char *argv[])
{
fd_set rfds;
port = 5599; // default port
for (int i = 0; i < argc; ++i)
{
if (strcmp (argv[i],"-p")==0)
{
if (argc > i+1 )
{
port = atoi (argv[i+1]);
}
}
}
sfd = create_socket_server(port);
/* necessary to initialize webots stuff */
wb_robot_init();
WbNodeRef root, node;
WbFieldRef children, field;
int n, i;
root = wb_supervisor_node_get_root();
children = wb_supervisor_node_get_field(root, "children");
n = wb_supervisor_field_get_count(children);
printf("This world contains %d nodes:\n", n);
for (i = 0; i < n; i++) {
node = wb_supervisor_field_get_mf_node(children, i);
if (wb_supervisor_node_get_type(node) == WB_NODE_WORLD_INFO)
{
world_info = node;
break;
}
}
printf("\n");
node = wb_supervisor_field_get_mf_node(children, 0);
field = wb_supervisor_node_get_field(node, "northDirection");
northDirection = wb_supervisor_field_get_sf_vec3f(field);
if (northDirection[0] == 1)
{
printf ("Axis Default Directions");
}
printf("WorldInfo.northDirection = %g %g %g\n\n", northDirection[0], northDirection[1], northDirection[2]);
// keybaard
timestep = (int)wb_robot_get_basic_time_step();
wb_keyboard_enable(timestep);
// inertialUnit
inertialUnit = wb_robot_get_device("inertial_unit");
wb_inertial_unit_enable(inertialUnit, timestep);
// gyro
gyro = wb_robot_get_device("gyro1");
wb_gyro_enable(gyro, timestep);
// accelerometer
accelerometer = wb_robot_get_device("accelerometer1");
wb_accelerometer_enable(accelerometer, timestep);
// compass
compass = wb_robot_get_device("compass1");
wb_compass_enable(compass, timestep);
// gps
gps = wb_robot_get_device("gps1");
wb_gps_enable(gps, timestep);
// camera
camera = wb_robot_get_device("camera1");
wb_camera_enable(camera, timestep);
const char *MOTOR_NAMES[] = {"motor1", "motor2", "motor3", "motor4"};
// get motor device tags
for (i = 0; i < MOTOR_NUM; i++) {
motors[i] = wb_robot_get_device(MOTOR_NAMES[i]);
v[i] = 0.0f;
//assert(motors[i]);
}
FD_ZERO(&rfds);
FD_SET(sfd, &rfds);
}
/*
* This is the main program.
* The arguments of the main function can be specified by the
* "controllerArgs" field of the Robot node
*/
int main(int argc, char **argv)
{
initialize( argc, argv);
/*
* You should declare here WbDeviceTag variables for storing
* robot devices like this:
* WbDeviceTag my_sensor = wb_robot_get_device("my_sensor");
* WbDeviceTag my_actuator = wb_robot_get_device("my_actuator");
*/
/* main loop
* Perform simulation steps of TIME_STEP milliseconds
* and leave the loop when the simulation is over
*/
/*
* Read the sensors :
* Enter here functions to read sensor data, like:
* double val = wb_distance_sensor_get_value(my_sensor);
*/
/* Process sensor data here */
/*
* Enter here functions to send actuator commands, like:
* wb_differential_wheels_set_speed(100.0,100.0);
*/
run();
/* Enter your cleanup code here */
/* This is necessary to cleanup webots resources */
wb_robot_cleanup();
return 0;
}

41
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.h Normal file
View File

@ -0,0 +1,41 @@
// #define DEBUG_MOTORS
// #define DEBUG_SENSORS
#define DEBUG_USE_KB
// #define DEBUG_INPUT_DATA
// #define LINEAR_THRUST
#define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0]))
enum data_type {
DATA_FLOAT,
DATA_DOUBLE,
DATA_VECTOR4F,
};
struct vector4f
{
float w,x,y,z;
};
typedef struct vector4f VECTOR4F;
struct {
double timestamp;
VECTOR4F motors;
} state, last_state;
// table to aid parsing of JSON sensor data
struct keytable {
const char *section;
const char *key;
void *ptr;
enum data_type type;
} keytable[1] = {
//{ "", "timestamp", &state.timestamp, DATA_DOUBLE },
{ "", "engines", &state.motors, DATA_VECTOR4F }
};

194
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/sensors.c Normal file
View File

@ -0,0 +1,194 @@
#include <stdio.h>
#include <sys/time.h>
#include <webots/supervisor.h>
#include "sensors.h"
#define M_PI 3.14159265358979323846
#define M_PI2 6.28318530718
/*
https://discuss.ardupilot.org/t/copter-x-y-z-which-is-which/6823/2
Copy pasted what’s important:
NED Coordinate System:
The x axis is aligned with the vector to the north pole (tangent to meridians).
The y axis points to the east side (tangent to parallels)
The z axis points to the center of the earth
There is also Body Fixed Frame:
Body Fixed Frame (Attached to the aircraft)
The x axis points in forward (defined by geometry and not by movement) direction. (= roll axis)
The y axis points to the right (geometrically) (= pitch axis)
The z axis points downwards (geometrically) (= yaw axis)
In order to convert from Body Frame to NED what you need to call this function:
copter.rotate_body_frame_to_NE(vel_vector.x, vel_vector.y);
*/
/*
returns: "yaw":_6.594794831471518e-05,"pitch":_-0.0005172680830582976,"roll":_0.022908752784132957}}
*/
void getInertia (const WbDeviceTag inertialUnit, const double *northDirection, char *buf)
{
const double *inertial_directions = wb_inertial_unit_get_roll_pitch_yaw (inertialUnit);
if (northDirection[0] == 1)
{
sprintf(buf,"\"roll\": %f,\"pitch\": %f,\"yaw\": %f",inertial_directions[0], inertial_directions[1], inertial_directions[2]);
}
else
{
sprintf(buf,"\"roll\": %f,\"pitch\": %f,\"yaw\": %f",inertial_directions[1], -inertial_directions[0], inertial_directions[2]);
}
return ;
}
/*
returns: "magnetic_field":_[23088.669921875,_3876.001220703125,_-53204.57421875]
*/
void getCompass (const WbDeviceTag compass, const double *northDirection, char *buf)
{
const double *north3D = wb_compass_get_values(compass);
if (northDirection[0] == 1)
{
sprintf(buf,"[%f, %f, %f]",north3D[0], north3D[2], north3D[1]);
}
else
{
sprintf(buf,"[%f, %f, %f]",north3D[2], -north3D[0], north3D[1]);
}
return ;
}
/*
returns: "vehicle.gps":{"timestamp":_1563301031.055164,"x":_5.5127296946011484e-05,"y":_-0.0010968948481604457,"z":_0.037179552018642426},
*/
void getGPS (const WbDeviceTag gps, const double *northDirection, char *buf)
{
const double *north3D = wb_gps_get_values(gps);
if (northDirection[0] == 1)
{
sprintf(buf,"\"x\": %f,\"y\": %f,\"z\": %f", north3D[0], north3D[2], north3D[1]);
}
else
{
sprintf(buf,"\"x\": %f,\"y\": %f,\"z\": %f", north3D[2], -north3D[0], north3D[1]);
}
return ;
}
/*
returns: "linear_acceleration": [0.005074390675872564, 0.22471477091312408, 9.80740737915039]
*/
void getAcc (const WbDeviceTag accelerometer, const double *northDirection, char *buf)
{
//SHOULD BE CORRECT
const double *a = wb_accelerometer_get_values(accelerometer);
if (northDirection[0] == 1)
{
sprintf(buf,"[%f, %f, %f]",a[0], a[2], a[1]);
}
else
{
sprintf(buf,"[%f, %f, %f]",a[0], a[2], a[1]);
}
//sprintf(buf,"[0.0, 0.0, 0.0]");
return ;
}
/*
returns: "angular_velocity": [-1.0255117643964695e-07, -8.877226775894087e-08, 2.087078510015772e-09]
*/
void getGyro (const WbDeviceTag gyro, const double *northDirection, char *buf)
{
const double *g = wb_gyro_get_values(gyro);
if (northDirection[0] == 1)
{
sprintf(buf,"[%f, %f, %f]",g[0], g[2], g[1]);
}
else
{
sprintf(buf,"[%f, %f, %f]",g[0], g[2], g[1]);
}
return ;
}
void getLinearVelocity (WbNodeRef nodeRef, const double *northDirection, char * buf)
{
const double * vel = wb_supervisor_node_get_velocity (nodeRef);
if (vel != NULL)
{
if (northDirection[0] == 1)
{
sprintf (buf,"[%f, %f, %f]", vel[0], vel[2], vel[1]);
}
else
{
sprintf (buf,"[%f, %f, %f]", vel[2], -vel[0], vel[1]);
}
}
}
void getAllSensors (char *buf, const double *northDirection, WbDeviceTag gyro, WbDeviceTag accelerometer, WbDeviceTag compass, const WbDeviceTag gps, WbDeviceTag inertial_unit)
{
/*
{"timestamp": 1563544049.2840538,
"vehicle.imu": {"timestamp": 1563544049.2673872,
"angular_velocity": [-2.0466000023589004e-06, 3.1428675129063777e-07, -6.141597896913709e-09],
"linear_acceleration": [0.005077465437352657, 0.22471386194229126, 9.807389259338379],
"magnetic_field": [23088.71484375, 3875.498046875, -53204.48046875]},
"vehicle.gps": {
"timestamp": 1563544049.2673872,
"x": 4.985610576113686e-05, "y": -0.0010973707539960742, "z": 0.037179529666900635},
"vehicle.velocity": {"timestamp": 1563544049.2673872,
"linear_velocity": [-3.12359499377024e-10, -1.3824124955874595e-08, -6.033386625858839e-07],
"angular_velocity": [-2.0466000023589004e-06, 3.1428675129063777e-07, -6.141597896913709e-09],
"world_linear_velocity": [0.0, 0.0, -6.034970851942489e-07]},
"vehicle.pose": {"timestamp": 1563544049.2673872,
"x": 4.985610576113686e-05, "y": -0.0010973707539960742, "z": 0.037179529666900635, "yaw": 8.899446402210742e-05, "pitch": -0.0005175824626348913, "roll": 0.022908702492713928}
}
*/
static char compass_buf [150];
static char acc_buf [150];
static char gyro_buf [150];
static char gps_buf [150];
static char inertial_buf [150];
static char linear_velocity_buf [150];
char szTime[21];
double time = wb_robot_get_time(); // current simulation time in [s]
sprintf(szTime,"%f", time);
getGyro(gyro, northDirection, gyro_buf);
getAcc(accelerometer, northDirection, acc_buf);
getCompass(compass, northDirection, compass_buf);
getGPS(gps, northDirection, gps_buf);
getInertia (inertial_unit, northDirection, inertial_buf);
getLinearVelocity(wb_supervisor_node_get_self(), northDirection, linear_velocity_buf);
sprintf (buf,"{\"ts\": %s,\"vehicle.imu\": {\"av\": %s,\"la\": %s,\"mf\": %s,\"vehicle.gps\":{%s},\"vehicle.velocity\":{\"wlv\": %s},\"vehicle.pose\":{%s,%s}}\r\n"
, szTime, gyro_buf, acc_buf, compass_buf, gps_buf, linear_velocity_buf, gps_buf, inertial_buf );
}

22
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/sensors.h Normal file
View File

@ -0,0 +1,22 @@
#include <webots/robot.h>
#include <webots/keyboard.h>
#include <webots/compass.h>
#include <webots/accelerometer.h>
#include <webots/inertial_unit.h>
#include <webots/gps.h>
#include <webots/gyro.h>
#include <webots/motor.h>
#include <webots/camera.h>
void getInertia (const WbDeviceTag inertialUnit, const double *northDirection, char *buf);
void getLinearVelocity (WbNodeRef nodeRef, const double *northDirection, char * buf);
void getCompass (const WbDeviceTag compass, const double *northDirection, char *buf);
void getAcc (const WbDeviceTag accelerometer, const double *northDirection, char *buf);
void getGyro (const WbDeviceTag gyro, const double *northDirection, char *buf);
void getGPS (const WbDeviceTag gps, const double *northDirection, char *buf);
void getAllSensors (char *buf, const double *northDirection, WbDeviceTag gyro, WbDeviceTag accelerometer, WbDeviceTag compass, const WbDeviceTag gps, WbDeviceTag inertial_unit);

112
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/sockets.c Normal file
View File

@ -0,0 +1,112 @@
#include "sockets.h"
bool socket_init() {
#ifdef _WIN32 /* initialize the socket API */
WSADATA info;
if (WSAStartup(MAKEWORD(1, 1), &info) != 0) {
fprintf(stderr, "Cannot initialize Winsock.\n");
return false;
}
#endif
return true;
}
bool socket_set_non_blocking(int fd) {
if (fd < 0)
return false;
#ifdef _WIN32
unsigned long mode = 1;
return (ioctlsocket(fd, FIONBIO, &mode) == 0) ? true : false;
#else
int flags = fcntl(fd, F_GETFL, 0) | O_NONBLOCK;
return (fcntl(fd, F_SETFL, flags) == 0) ? true : false;
#endif
}
int socket_accept(int server_fd) {
int cfd;
struct sockaddr_in client;
struct hostent *client_info;
#ifndef _WIN32
socklen_t asize;
#else
int asize;
#endif
asize = sizeof(struct sockaddr_in);
cfd = accept(server_fd, (struct sockaddr *)&client, &asize);
if (cfd == -1) {
#ifdef _WIN32
int e = WSAGetLastError();
if (e == WSAEWOULDBLOCK)
return 0;
fprintf(stderr, "Accept error: %d.\n", e);
#else
if (errno == EWOULDBLOCK)
return 0;
fprintf(stderr, "Accept error: %d.\n", errno);
#endif
return -1;
}
client_info = gethostbyname((char *)inet_ntoa(client.sin_addr));
printf("Accepted connection from: %s.\n", client_info->h_name);
return cfd;
}
bool socket_close(int fd) {
#ifdef _WIN32
return (closesocket(fd) == 0) ? true : false;
#else
return (close(fd) == 0) ? true : false;
#endif
}
bool socket_cleanup() {
#ifdef _WIN32
return (WSACleanup() == 0) ? true : false;
#else
return true;
#endif
}
/*
Creates a socket and bind it to port.
*/
int create_socket_server(int port) {
int sfd, rc;
struct sockaddr_in address;
if (!socket_init())
{
fprintf (stderr, "socket_init failed");
return -1;
}
sfd = socket(AF_INET, SOCK_STREAM, 0);
if (sfd == -1) {
fprintf(stderr, "Cannot create socket.\n");
return -1;
}
int one = 1;
setsockopt(sfd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
memset(&address, 0, sizeof(struct sockaddr_in));
address.sin_family = AF_INET;
address.sin_port = htons((unsigned short)port);
address.sin_addr.s_addr = INADDR_ANY;
rc = bind(sfd, (struct sockaddr *)&address, sizeof(struct sockaddr));
if (rc == -1) {
fprintf(stderr, "Cannot bind port %d.\n", port);
socket_close(sfd);
return -1;
}
if (listen(sfd, 1) == -1) {
fprintf(stderr, "Cannot listen for connections.\n");
socket_close(sfd);
return -1;
}
printf ("socket initialized at port %d.\n", port);
return sfd;
}

28
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/sockets.h Normal file
View File

@ -0,0 +1,28 @@
#include <stdbool.h>
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#ifdef _WIN32
#include <winsock.h>
#else
#include <arpa/inet.h> /* definition of inet_ntoa */
#include <errno.h>
#include <fcntl.h>
#include <netdb.h> /* definition of gethostbyname */
#include <netinet/in.h> /* definition of struct sockaddr_in */
#include <stdlib.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <unistd.h> /* definition of close */
#endif
int create_socket_server(int port);
bool socket_cleanup();
int socket_accept(int server_fd);
bool socket_set_non_blocking(int fd);
int fd;
int sfd;

77
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_ROVER/Makefile Normal file
View File

@ -0,0 +1,77 @@
# Copyright 1996-2019 Cyberbotics Ltd.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
### Generic Makefile.include for Webots controllers, physics plugins, robot
### window libraries, remote control libraries and other libraries
### to be used with GNU make
###
### Platforms: Windows, macOS, Linux
### Languages: C, C++
###
### Authors: Olivier Michel, Yvan Bourquin, Fabien Rohrer
### Edmund Ronald, Sergei Poskriakov
###
###-----------------------------------------------------------------------------
###
### This file is meant to be included from the Makefile files located in the
### Webots projects subdirectories. It is possible to set a number of variables
### to customize the build process, i.e., add source files, compilation flags,
### include paths, libraries, etc. These variables should be set in your local
### Makefile just before including this Makefile.include. This Makefile.include
### should never be modified.
###
### Here is a description of the variables you may set in your local Makefile:
###
### ---- C Sources ----
### if your program uses several C source files:
### C_SOURCES = my_plugin.c my_clever_algo.c my_graphics.c
###
### ---- C++ Sources ----
### if your program uses several C++ source files:
### CXX_SOURCES = my_plugin.cc my_clever_algo.cpp my_graphics.c++
###
### ---- Compilation options ----
### if special compilation flags are necessary:
### CFLAGS = -Wno-unused-result
###
### ---- Linked libraries ----
### if your program needs additional libraries:
### INCLUDE = -I"/my_library_path/include"
### LIBRARIES = -L"/path/to/my/library" -lmy_library -lmy_other_library
###
### ---- Linking options ----
### if special linking flags are needed:
### LFLAGS = -s
###
### ---- Webots included libraries ----
### if you want to use the Webots C API in your C++ controller program:
### USE_C_API = true
### if you want to link with the Qt framework embedded in Webots:
### QT = core gui widgets network
###
### ---- Debug mode ----
### if you want to display the gcc command line for compilation and link, as
### well as the rm command details used for cleaning:
### VERBOSE = 1
###
###-----------------------------------------------------------------------------
### Do not modify: this includes Webots global Makefile.include
C_SOURCES = ardupilot_SITL_ROVER.c sockets.c sensors.c
INCLUDE = -I"./"
LIBRARIES = -ldriver -lcar
space :=
space +=
WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME))))
include $(WEBOTS_HOME_PATH)/resources/Makefile.include

BIN
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_ROVER/ardupilot_SITL_ROVER Executable file
View File

Binary file not shown.

401
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_ROVER/ardupilot_SITL_ROVER.c Normal file
View File

@ -0,0 +1,401 @@
/*
* File: ardupilot_SITL_ROV.c
* Date:
* Description:
* Author:
* Modifications:
*/
/*
* You may need to add include files like <webots/distance_sensor.h> or
* <webots/motor.h>, etc.
*/
#include <webots/robot.h>
#include <webots/supervisor.h>
#include <webots/vehicle/car.h>
#include <webots/vehicle/driver.h>
#include "ardupilot_SITL_ROVER.h"
#include "sockets.h"
#include "sensors.h"
#define MOTOR_NUM 2
static WbDeviceTag gyro;
static WbDeviceTag accelerometer;
static WbDeviceTag compass;
static WbDeviceTag gps;
static WbDeviceTag camera;
static WbDeviceTag inertialUnit;
static WbDeviceTag car;
static WbNodeRef world_info;
static const double *northDirection;
const float max_speed = 27; //m/s
static double v[MOTOR_NUM];
int port;
static int timestep;
#ifdef DEBUG_USE_KB
/*
// Code used tp simulae motors using keys to make sure that sensors directions and motor torques and thrusts are all correct.
// You can start this controller and use telnet instead of SITL to start the simulator.
Then you can use Keyboard to emulate motor input.
*/
void process_keyboard ()
{
switch (wb_keyboard_get_key())
{
case 'Q': // Q key -> up & left
v[0] = 0.0;
v[1] = 0.0;
break;
case 'W':
v[1] += 0.01;
break;
case 'S':
v[1] -= 0.01;
break;
case 'A':
v[0] = v[0] + 0.01;
break;
case 'D':
v[0] = v[0] - 0.01;
break;
}
wbu_driver_set_cruising_speed (v[1]);
wbu_driver_set_steering_angle (v[0]);
printf ("Motors Internal %f %f\n", v[0],v[1]);
}
#endif
/*
// apply motor thrust.
*/
void update_controls()
{
float cruise_speed = state.rover.y * max_speed * 3.6f;
float steer_angle = state.rover.x * 0.7f;
wbu_driver_set_cruising_speed (cruise_speed + v[1]);
wbu_driver_set_steering_angle (steer_angle + v[0]);
#ifdef DEBUG_MOTORS
printf("cruise speed: %f steering angle: %f\n", cruise_speed, steer_angle);
#endif
}
bool parse_controls(const char *json)
{
//state.timestamp = 1.0;
#ifdef DEBUG_INPUT_DATA
printf("%s\n", json);
#endif
for (uint16_t i=0; i < ARRAY_SIZE(keytableROV); i++) {
struct keytableROV *key;
key = &keytableROV[i];
//printf("search %s/%s\n", key->section, key->key);
// look for section header
const char *p = strstr(json, key->section);
if (!p) {
// we don't have this sensor
continue;
}
p += strlen(key->section)+1;
// find key inside section
p = strstr(p, key->key);
if (!p) {
printf("Failed to find key %s/%s\n", key->section, key->key);
return false;
}
p += strlen(key->key)+3;
switch (key->type)
{
case DATA_FLOAT:
*((float *)key->ptr) = atof(p);
#ifdef DEBUG_INPUT_DATA
printf("GOT %s/%s\n", key->section, key->key);
#endif
break;
case DATA_DOUBLE:
*((double *)key->ptr) = atof(p);
#ifdef DEBUG_INPUT_DATA
printf("GOT %s/%s\n", key->section, key->key);
#endif
break;
case DATA_VECTOR2F:
{
VECTOR2F *v = (VECTOR2F *)key->ptr;
if (sscanf(p, "[%f, %f]", &(v->x), &(v->y)) != 2) {
printf("Failed to parse Vector2f for %s %s/%s\n",p, key->section, key->key);
return false;
}
else
{
#ifdef DEBUG_INPUT_DATA
printf("GOT %s/%s [%f, %f]\n ", key->section, key->key,v->x,v->y);
#endif
}
break;
}
case DATA_VECTOR4F: {
VECTOR4F *v = (VECTOR4F *)key->ptr;
if (sscanf(p, "[%f, %f, %f, %f]", &(v->w), &(v->x), &(v->y), &(v->z)) != 4) {
printf("Failed to parse Vector4f for %s %s/%s\n",p, key->section, key->key);
return false;
}
else
{
#ifdef DEBUG_INPUT_DATA
printf("GOT %s/%s\n[%f, %f, %f, %f]\n ", key->section, key->key,v->w,v->x,v->y,v->z);
#endif
}
break;
}
}
}
return true;
}
void run ()
{
char send_buf[1000]; //1000 just a safe margin
char command_buffer[200];
fd_set rfds;
while (wb_robot_step(timestep) != -1)
{
#ifdef DEBUG_USE_KB
process_keyboard();
#endif
if (fd == 0)
{
// if no socket wait till you get a socket
fd = socket_accept(sfd);
if (fd > 0)
socket_set_non_blocking(fd);
else if (fd < 0)
break;
}
getAllSensors ((char *)send_buf, northDirection, gyro,accelerometer,compass,gps, inertialUnit);
#ifdef DEBUG_SENSORS
printf("%s\n",send_buf);
#endif
if (write(fd,send_buf,strlen(send_buf)) <= 0)
{
printf ("Send Data Error\n");
}
if (fd)
{
FD_ZERO(&rfds);
FD_SET(fd, &rfds);
struct timeval tv;
tv.tv_sec = 0.05;
tv.tv_usec = 0;
int number = select(fd + 1, &rfds, NULL, NULL, &tv);
if (number != 0)
{
// there is a valid connection
int n = recv(fd, (char *)command_buffer, 200, 0);
if (n < 0) {
#ifdef _WIN32
int e = WSAGetLastError();
if (e == WSAECONNABORTED)
fprintf(stderr, "Connection aborted.\n");
else if (e == WSAECONNRESET)
fprintf(stderr, "Connection reset.\n");
else
fprintf(stderr, "Error reading from socket: %d.\n", e);
#else
if (errno)
fprintf(stderr, "Error reading from socket: %d.\n", errno);
#endif
break;
}
if (n==0)
{
break;
}
if (command_buffer[0] == 'e')
{
break;
}
if (n > 0)
{
//printf("Received %d bytes:\n", n);
command_buffer[n] = 0;
parse_controls (command_buffer);
update_controls();
}
}
}
}
socket_cleanup();
}
void initialize (int argc, char *argv[])
{
fd_set rfds;
port = 5599; // default port
for (int i = 0; i < argc; ++i)
{
if (strcmp (argv[i],"-p")==0)
{
if (argc > i+1 )
{
port = atoi (argv[i+1]);
}
}
}
sfd = create_socket_server(port);
/* necessary to initialize webots stuff */
wb_robot_init();
wbu_driver_init ();
WbNodeRef root, node;
WbFieldRef children, field;
int n, i;
root = wb_supervisor_node_get_root();
children = wb_supervisor_node_get_field(root, "children");
n = wb_supervisor_field_get_count(children);
printf("This world contains %d nodes:\n", n);
for (i = 0; i < n; i++) {
node = wb_supervisor_field_get_mf_node(children, i);
if (wb_supervisor_node_get_type(node) == WB_NODE_WORLD_INFO)
{
world_info = node;
break;
}
}
printf("\n");
node = wb_supervisor_field_get_mf_node(children, 0);
field = wb_supervisor_node_get_field(node, "northDirection");
northDirection = wb_supervisor_field_get_sf_vec3f(field);
if (northDirection[0] == 1)
{
printf ("Axis Default Directions");
}
printf("WorldInfo.northDirection = %g %g %g\n\n", northDirection[0], northDirection[1], northDirection[2]);
// keybaard
timestep = (int)wb_robot_get_basic_time_step();
wb_keyboard_enable(timestep);
// inertialUnit
inertialUnit = wb_robot_get_device("inertial_unit");
wb_inertial_unit_enable(inertialUnit, timestep);
// gyro
gyro = wb_robot_get_device("gyro1");
wb_gyro_enable(gyro, timestep);
// accelerometer
accelerometer = wb_robot_get_device("accelerometer1");
wb_accelerometer_enable(accelerometer, timestep);
// compass
compass = wb_robot_get_device("compass1");
wb_compass_enable(compass, timestep);
// gps
gps = wb_robot_get_device("gps1");
wb_gps_enable(gps, timestep);
// camera
camera = wb_robot_get_device("camera1");
wb_camera_enable(camera, timestep);
car = wb_robot_get_device ("rover");
FD_ZERO(&rfds);
FD_SET(sfd, &rfds);
}
/*
* This is the main program.
* The arguments of the main function can be specified by the
* "controllerArgs" field of the Robot node
*/
int main(int argc, char **argv) {
/* necessary to initialize webots stuff */
wb_robot_init();
initialize( argc, argv);
/*
* You should declare here WbDeviceTag variables for storing
* robot devices like this:
* WbDeviceTag my_sensor = wb_robot_get_device("my_sensor");
* WbDeviceTag my_actuator = wb_robot_get_device("my_actuator");
*/
/* main loop
* Perform simulation steps of TIME_STEP milliseconds
* and leave the loop when the simulation is over
*/
run();
/* Enter your cleanup code here */
wbu_driver_cleanup();
/* This is necessary to cleanup webots resources */
wb_robot_cleanup();
return 0;
}

48
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_ROVER/ardupilot_SITL_ROVER.h Normal file
View File

@ -0,0 +1,48 @@
#define DEBUG_USE_KB
#define DEBUG_MOTORS
#define DEBUG_INPUT_DATA
#define LINEAR_THRUST
#define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0]))
enum data_type {
DATA_FLOAT,
DATA_DOUBLE,
DATA_VECTOR4F,
DATA_VECTOR2F,
};
struct vector4f
{
float w,x,y,z;
};
struct vector2f
{
float x,y;
};
typedef struct vector4f VECTOR4F;
typedef struct vector2f VECTOR2F;
struct {
double timestamp;
VECTOR2F rover;
} state, last_state;
// table to aid parsing of JSON sensor data
struct keytableROV {
const char *section;
const char *key;
void *ptr;
enum data_type type;
} keytableROV[1] = {
//{ "", "timestamp", &state.timestamp, DATA_DOUBLE },
{ "", "rover", &state.rover, DATA_VECTOR2F }
};

194
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_ROVER/sensors.c Normal file
View File

@ -0,0 +1,194 @@
#include <stdio.h>
#include <sys/time.h>
#include <webots/supervisor.h>
#include "sensors.h"
#define M_PI 3.14159265358979323846
#define M_PI2 6.28318530718
/*
https://discuss.ardupilot.org/t/copter-x-y-z-which-is-which/6823/2
Copy pasted what’s important:
NED Coordinate System:
The x axis is aligned with the vector to the north pole (tangent to meridians).
The y axis points to the east side (tangent to parallels)
The z axis points to the center of the earth
There is also Body Fixed Frame:
Body Fixed Frame (Attached to the aircraft)
The x axis points in forward (defined by geometry and not by movement) direction. (= roll axis)
The y axis points to the right (geometrically) (= pitch axis)
The z axis points downwards (geometrically) (= yaw axis)
In order to convert from Body Frame to NED what you need to call this function:
copter.rotate_body_frame_to_NE(vel_vector.x, vel_vector.y);
*/
/*
returns: "yaw":_6.594794831471518e-05,"pitch":_-0.0005172680830582976,"roll":_0.022908752784132957}}
*/
void getInertia (const WbDeviceTag inertialUnit, const double *northDirection, char *buf)
{
const double *inertial_directions = wb_inertial_unit_get_roll_pitch_yaw (inertialUnit);
if (northDirection[0] == 1)
{
sprintf(buf,"\"roll\": %f,\"pitch\": %f,\"yaw\": %f",inertial_directions[0], inertial_directions[1], inertial_directions[2]);
}
else
{
sprintf(buf,"\"roll\": %f,\"pitch\": %f,\"yaw\": %f",inertial_directions[1], -inertial_directions[0], inertial_directions[2]);
}
return ;
}
/*
returns: "magnetic_field":_[23088.669921875,_3876.001220703125,_-53204.57421875]
*/
void getCompass (const WbDeviceTag compass, const double *northDirection, char *buf)
{
const double *north3D = wb_compass_get_values(compass);
if (northDirection[0] == 1)
{
sprintf(buf,"[%f, %f, %f]",north3D[0], north3D[2], north3D[1]);
}
else
{
sprintf(buf,"[%f, %f, %f]",north3D[2], -north3D[0], north3D[1]);
}
return ;
}
/*
returns: "vehicle.gps":{"timestamp":_1563301031.055164,"x":_5.5127296946011484e-05,"y":_-0.0010968948481604457,"z":_0.037179552018642426},
*/
void getGPS (const WbDeviceTag gps, const double *northDirection, char *buf)
{
const double *north3D = wb_gps_get_values(gps);
if (northDirection[0] == 1)
{
sprintf(buf,"\"x\": %f,\"y\": %f,\"z\": %f", north3D[0], north3D[2], north3D[1]);
}
else
{
sprintf(buf,"\"x\": %f,\"y\": %f,\"z\": %f", north3D[2], -north3D[0], north3D[1]);
}
return ;
}
/*
returns: "linear_acceleration": [0.005074390675872564, 0.22471477091312408, 9.80740737915039]
*/
void getAcc (const WbDeviceTag accelerometer, const double *northDirection, char *buf)
{
//SHOULD BE CORRECT
const double *a = wb_accelerometer_get_values(accelerometer);
if (northDirection[0] == 1)
{
sprintf(buf,"[%f, %f, %f]",a[0], a[2], a[1]);
}
else
{
sprintf(buf,"[%f, %f, %f]",a[0], a[2], a[1]);
}
//sprintf(buf,"[0.0, 0.0, 0.0]");
return ;
}
/*
returns: "angular_velocity": [-1.0255117643964695e-07, -8.877226775894087e-08, 2.087078510015772e-09]
*/
void getGyro (const WbDeviceTag gyro, const double *northDirection, char *buf)
{
const double *g = wb_gyro_get_values(gyro);
if (northDirection[0] == 1)
{
sprintf(buf,"[%f, %f, %f]",g[0], g[2], g[1]);
}
else
{
sprintf(buf,"[%f, %f, %f]",g[0], g[2], g[1]);
}
return ;
}
void getLinearVelocity (WbNodeRef nodeRef, const double *northDirection, char * buf)
{
const double * vel = wb_supervisor_node_get_velocity (nodeRef);
if (vel != NULL)
{
if (northDirection[0] == 1)
{
sprintf (buf,"[%f, %f, %f]", vel[0], vel[2], vel[1]);
}
else
{
sprintf (buf,"[%f, %f, %f]", vel[2], -vel[0], vel[1]);
}
}
}
void getAllSensors (char *buf, const double *northDirection, WbDeviceTag gyro, WbDeviceTag accelerometer, WbDeviceTag compass, const WbDeviceTag gps, WbDeviceTag inertial_unit)
{
/*
{"timestamp": 1563544049.2840538,
"vehicle.imu": {"timestamp": 1563544049.2673872,
"angular_velocity": [-2.0466000023589004e-06, 3.1428675129063777e-07, -6.141597896913709e-09],
"linear_acceleration": [0.005077465437352657, 0.22471386194229126, 9.807389259338379],
"magnetic_field": [23088.71484375, 3875.498046875, -53204.48046875]},
"vehicle.gps": {
"timestamp": 1563544049.2673872,
"x": 4.985610576113686e-05, "y": -0.0010973707539960742, "z": 0.037179529666900635},
"vehicle.velocity": {"timestamp": 1563544049.2673872,
"linear_velocity": [-3.12359499377024e-10, -1.3824124955874595e-08, -6.033386625858839e-07],
"angular_velocity": [-2.0466000023589004e-06, 3.1428675129063777e-07, -6.141597896913709e-09],
"world_linear_velocity": [0.0, 0.0, -6.034970851942489e-07]},
"vehicle.pose": {"timestamp": 1563544049.2673872,
"x": 4.985610576113686e-05, "y": -0.0010973707539960742, "z": 0.037179529666900635, "yaw": 8.899446402210742e-05, "pitch": -0.0005175824626348913, "roll": 0.022908702492713928}
}
*/
static char compass_buf [150];
static char acc_buf [150];
static char gyro_buf [150];
static char gps_buf [150];
static char inertial_buf [150];
static char linear_velocity_buf [150];
char szTime[21];
double time = wb_robot_get_time(); // current simulation time in [s]
sprintf(szTime,"%f", time);
getGyro(gyro, northDirection, gyro_buf);
getAcc(accelerometer, northDirection, acc_buf);
getCompass(compass, northDirection, compass_buf);
getGPS(gps, northDirection, gps_buf);
getInertia (inertial_unit, northDirection, inertial_buf);
getLinearVelocity(wb_supervisor_node_get_self(), northDirection, linear_velocity_buf);
sprintf (buf,"{\"ts\": %s,\"vehicle.imu\": {\"av\": %s,\"la\": %s,\"mf\": %s,\"vehicle.gps\":{%s},\"vehicle.velocity\":{\"wlv\": %s},\"vehicle.pose\":{%s,%s}}\r\n"
, szTime, gyro_buf, acc_buf, compass_buf, gps_buf, linear_velocity_buf, gps_buf, inertial_buf );
}

22
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_ROVER/sensors.h Normal file
View File

@ -0,0 +1,22 @@
#include <webots/robot.h>
#include <webots/keyboard.h>
#include <webots/compass.h>
#include <webots/accelerometer.h>
#include <webots/inertial_unit.h>
#include <webots/gps.h>
#include <webots/gyro.h>
#include <webots/motor.h>
#include <webots/camera.h>
void getInertia (const WbDeviceTag inertialUnit, const double *northDirection, char *buf);
void getLinearVelocity (WbNodeRef nodeRef, const double *northDirection, char * buf);
void getCompass (const WbDeviceTag compass, const double *northDirection, char *buf);
void getAcc (const WbDeviceTag accelerometer, const double *northDirection, char *buf);
void getGyro (const WbDeviceTag gyro, const double *northDirection, char *buf);
void getGPS (const WbDeviceTag gps, const double *northDirection, char *buf);
void getAllSensors (char *buf, const double *northDirection, WbDeviceTag gyro, WbDeviceTag accelerometer, WbDeviceTag compass, const WbDeviceTag gps, WbDeviceTag inertial_unit);

124
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_ROVER/sockets.c Normal file
View File

@ -0,0 +1,124 @@
/*
* File: ardupilot_SITL_ROV.c
* Date: 03 Aug 2019
* Description: integration with ardupilot SITL simulation.
* Author: M.S.Hefny (HefnySco)
* Modifications:
*/
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <webots/supervisor.h>
#include "sockets.h"
#include "sensors.h"
bool socket_init() {
#ifdef _WIN32 /* initialize the socket API */
WSADATA info;
if (WSAStartup(MAKEWORD(1, 1), &info) != 0) {
fprintf(stderr, "Cannot initialize Winsock.\n");
return false;
}
#endif
return true;
}
bool socket_set_non_blocking(int fd) {
if (fd < 0)
return false;
#ifdef _WIN32
unsigned long mode = 1;
return (ioctlsocket(fd, FIONBIO, &mode) == 0) ? true : false;
#else
int flags = fcntl(fd, F_GETFL, 0) | O_NONBLOCK;
return (fcntl(fd, F_SETFL, flags) == 0) ? true : false;
#endif
}
int socket_accept(int server_fd) {
int cfd;
struct sockaddr_in client;
struct hostent *client_info;
#ifndef _WIN32
socklen_t asize;
#else
int asize;
#endif
asize = sizeof(struct sockaddr_in);
cfd = accept(server_fd, (struct sockaddr *)&client, &asize);
if (cfd == -1) {
#ifdef _WIN32
int e = WSAGetLastError();
if (e == WSAEWOULDBLOCK)
return 0;
fprintf(stderr, "Accept error: %d.\n", e);
#else
if (errno == EWOULDBLOCK)
return 0;
fprintf(stderr, "Accept error: %d.\n", errno);
#endif
return -1;
}
client_info = gethostbyname((char *)inet_ntoa(client.sin_addr));
printf("Accepted connection from: %s.\n", client_info->h_name);
return cfd;
}
bool socket_close(int fd) {
#ifdef _WIN32
return (closesocket(fd) == 0) ? true : false;
#else
return (close(fd) == 0) ? true : false;
#endif
}
bool socket_cleanup() {
#ifdef _WIN32
return (WSACleanup() == 0) ? true : false;
#else
return true;
#endif
}
/*
Creates a socket and bind it to port.
*/
int create_socket_server(int port) {
int sfd, rc;
struct sockaddr_in address;
if (!socket_init())
{
fprintf (stderr, "socket_init failed");
return -1;
}
sfd = socket(AF_INET, SOCK_STREAM, 0);
if (sfd == -1) {
fprintf(stderr, "Cannot create socket.\n");
return -1;
}
int one = 1;
setsockopt(sfd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
memset(&address, 0, sizeof(struct sockaddr_in));
address.sin_family = AF_INET;
address.sin_port = htons((unsigned short)port);
address.sin_addr.s_addr = INADDR_ANY;
rc = bind(sfd, (struct sockaddr *)&address, sizeof(struct sockaddr));
if (rc == -1) {
fprintf(stderr, "Cannot bind port %d.\n", port);
socket_close(sfd);
return -1;
}
if (listen(sfd, 1) == -1) {
fprintf(stderr, "Cannot listen for connections.\n");
socket_close(sfd);
return -1;
}
printf ("socket initialized at port %d.\n", port);
return sfd;
}

28
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_ROVER/sockets.h Normal file
View File

@ -0,0 +1,28 @@
#include <stdbool.h>
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#ifdef _WIN32
#include <winsock.h>
#else
#include <arpa/inet.h> /* definition of inet_ntoa */
#include <errno.h>
#include <fcntl.h>
#include <netdb.h> /* definition of gethostbyname */
#include <netinet/in.h> /* definition of struct sockaddr_in */
#include <stdlib.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <unistd.h> /* definition of close */
#endif
int create_socket_server(int port);
bool socket_cleanup();
int socket_accept(int server_fd);
bool socket_set_non_blocking(int fd);
int fd;
int sfd;

5
libraries/SITL/examples/Webots/dronePlus.sh Executable file
View File

@ -0,0 +1,5 @@
#!/bin/bash
# assume we start the script from the root directory
ROOTDIR=$PWD
$PWD/Tools/autotest/sim_vehicle.py -v ArduCopter -w --model webots-quad --add-param-file=libraries/SITL/examples/Webots/quadPlus.parm

5
libraries/SITL/examples/Webots/droneX.sh Executable file
View File

@ -0,0 +1,5 @@
#!/bin/bash
# assume we start the script from the root directory
ROOTDIR=$PWD
$PWD/Tools/autotest/sim_vehicle.py -v ArduCopter -w --model webots-quad --add-param-file=libraries/SITL/examples/Webots/quadX.parm

142
libraries/SITL/examples/Webots/mission_test_sitl.plan Normal file
View File

@ -0,0 +1,142 @@
{
"fileType": "Plan",
"geoFence": {
"circles": [
],
"polygons": [
],
"version": 2
},
"groundStation": "QGroundControl",
"mission": {
"cruiseSpeed": 15,
"firmwareType": 3,
"hoverSpeed": 5,
"items": [
{
"autoContinue": true,
"command": 22,
"doJumpId": 1,
"frame": 3,
"params": [
15,
0,
0,
null,
0,
0,
5
],
"type": "SimpleItem"
},
{
"AMSLAltAboveTerrain": 5,
"Altitude": 5,
"AltitudeMode": 1,
"autoContinue": true,
"command": 16,
"doJumpId": 2,
"frame": 3,
"params": [
0,
0,
0,
null,
-35.36256542276401,
149.16525136037512,
5
],
"type": "SimpleItem"
},
{
"AMSLAltAboveTerrain": 5,
"Altitude": 5,
"AltitudeMode": 1,
"autoContinue": true,
"command": 16,
"doJumpId": 3,
"frame": 3,
"params": [
0,
0,
0,
null,
-35.36266166697202,
149.1641248325614,
5
],
"type": "SimpleItem"
},
{
"AMSLAltAboveTerrain": 5,
"Altitude": 5,
"AltitudeMode": 1,
"autoContinue": true,
"command": 16,
"doJumpId": 4,
"frame": 3,
"params": [
0,
0,
0,
null,
-35.362008003718415,
149.16351540625385,
5
],
"type": "SimpleItem"
},
{
"AMSLAltAboveTerrain": null,
"Altitude": 5,
"AltitudeMode": 1,
"autoContinue": true,
"command": 16,
"doJumpId": 5,
"frame": 3,
"params": [
0,
0,
0,
null,
-35.362273115811924,
149.164369628518,
5
],
"type": "SimpleItem"
},
{
"AMSLAltAboveTerrain": null,
"Altitude": 5,
"AltitudeMode": 1,
"autoContinue": true,
"command": 16,
"doJumpId": 6,
"frame": 3,
"params": [
0,
0,
0,
null,
-35.36200381,
149.16511265,
5
],
"type": "SimpleItem"
}
],
"plannedHomePosition": [
-35.363261,
149.1652299,
584.04
],
"vehicleType": 2,
"version": 2
},
"rallyPoints": {
"points": [
],
"version": 2
},
"version": 1
}

5
libraries/SITL/examples/Webots/pyramids_dronePlus.sh Executable file
View File

@ -0,0 +1,5 @@
#!/bin/bash
# assume we start the script from the root directory
ROOTDIR=$PWD
$PWD/Tools/autotest/sim_vehicle.py -v ArduCopter -w --model webots-quad --add-param-file=libraries/SITL/examples/Webots/quadPlus.parm -L Pyramid

27
libraries/SITL/examples/Webots/quadPlus.parm Normal file
View File

@ -0,0 +1,27 @@
FRAME_CLASS 1.000000
FRAME_TYPE 0.000000
ATC_ANG_PIT_P 1.0
ATC_ANG_RLL_P 1.0
ATC_ANG_YAW_P 1.000000
ATC_RAT_PIT_FF 0.000000
ATC_RAT_PIT_FILT 50.000000
ATC_RAT_PIT_IMAX 0.500000
ATC_RAT_RLL_FF 0.000000
ATC_RAT_RLL_FILT 50.000000
ATC_RAT_RLL_IMAX 0.500000
ATC_RAT_PIT_P 8.0
ATC_RAT_RLL_P 8.0
ATC_RAT_PIT_I 0.030000
ATC_RAT_RLL_I 0.0300000
ATC_RAT_PIT_D 0.001
ATC_RAT_RLL_D 0.001
ATC_RAT_YAW_D 0.0000100
ATC_RAT_YAW_FF 0.000000
ATC_RAT_YAW_FILT 5.000000
ATC_RAT_YAW_I 0.03
ATC_RAT_YAW_IMAX 0.50000
ATC_RAT_YAW_P 0.60
EK2_LOG_MASK 7
LOG_BITMASK 978943
LOG_MAV_BUFSIZE 32
LOG_REPLAY 1

27
libraries/SITL/examples/Webots/quadX.parm Normal file
View File

@ -0,0 +1,27 @@
FRAME_CLASS 1.000000
FRAME_TYPE 1.000000
ATC_ANG_PIT_P 1.0
ATC_ANG_RLL_P 1.0
ATC_ANG_YAW_P 1.000000
ATC_RAT_PIT_FF 0.000000
ATC_RAT_PIT_FILT 50.000000
ATC_RAT_PIT_IMAX 0.500000
ATC_RAT_RLL_FF 0.000000
ATC_RAT_RLL_FILT 50.000000
ATC_RAT_RLL_IMAX 0.500000
ATC_RAT_PIT_P 8.0
ATC_RAT_RLL_P 8.0
ATC_RAT_PIT_I 0.030000
ATC_RAT_RLL_I 0.0300000
ATC_RAT_PIT_D 0.001
ATC_RAT_RLL_D 0.001
ATC_RAT_YAW_D 0.0000100
ATC_RAT_YAW_FF 0.000000
ATC_RAT_YAW_FILT 5.000000
ATC_RAT_YAW_I 0.03
ATC_RAT_YAW_IMAX 0.50000
ATC_RAT_YAW_P 0.60
EK2_LOG_MASK 7
LOG_BITMASK 978943
LOG_MAV_BUFSIZE 32
LOG_REPLAY 1

4
libraries/SITL/examples/Webots/rover.parm Normal file
View File

@ -0,0 +1,4 @@
# setup rover for skid steering
SERVO1_FUNCTION 73
SERVO3_FUNCTION 74
TURN_RADIUS 2

5
libraries/SITL/examples/Webots/rover.sh Executable file
View File

@ -0,0 +1,5 @@
#!/bin/bash
# assume we start the script from the root directory
ROOTDIR=$PWD
$PWD/Tools/autotest/sim_vehicle.py -v APMrover2 -w --model webots-rover --add-param-file=libraries/SITL/examples/Webots/rover.parm

12
libraries/SITL/examples/Webots/worlds/.pyramidMapReduced2.wbproj Normal file
View File

@ -0,0 +1,12 @@
Webots Project File version R2019b
perspectives: 000000ff00000000fd00000003000000000000021d000003d5fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000005400ffffff000000010000007f0000036ffc0200000001fb0000001400540065007800740045006400690074006f007201000000160000036f000000a200ffffff000000030000039c0000005efc0100000001fb0000000e0043006f006e0073006f006c006501000000000000039c0000005400ffffff000003170000036f00000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff000000010000000200000118000001fc0100000006010000000100
sceneTreePerspectives: 000000ff0000000100000002000000c0000000fc0100000006010000000200
maximizedDockId: -1
centralWidgetVisible: 1
renderingMode: PLAIN
selectionDisabled: 0
viewpointLocked: 0
orthographicViewHeight: 1
textFiles: 1 "../../../../../../Work/projeKt/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.c" "controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.c"
renderingDevicePerspectives: quad_plus_sitl:camera1;1;0.773438;0;0

12
libraries/SITL/examples/Webots/worlds/.webots_quadPlus.wbproj Normal file
View File

@ -0,0 +1,12 @@
Webots Project File version R2019b
perspectives: 000000ff00000000fd00000003000000000000021d000003d5fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000005400ffffff00000001000000ed000002e6fc0200000001fb0000001400540065007800740045006400690074006f00720100000016000002e6000000a200ffffff000000030000039c000000e7fc0100000001fb0000000e0043006f006e0073006f006c006501000000000000039c0000005400ffffff000002a9000002e600000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff000000010000000200000118000001af0100000006010000000100
sceneTreePerspectives: 000000ff0000000100000002000000c0000000fc0100000006010000000200
maximizedDockId: -1
centralWidgetVisible: 1
renderingMode: PLAIN
selectionDisabled: 0
viewpointLocked: 0
orthographicViewHeight: 1
textFiles: 0 "controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.c"
renderingDevicePerspectives: quad_plus_sitl:camera1;1;1.07031;0;0

11
libraries/SITL/examples/Webots/worlds/.webots_quadX.wbproj Normal file
View File

@ -0,0 +1,11 @@
Webots Project File version R2019b
perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000005400ffffff0000000100000348000002f7fc0200000001fb0000001400540065007800740045006400690074006f00720100000016000002f7000000a200ffffff000000030000073d000000d8fc0100000001fb0000000e0043006f006e0073006f006c006501000000000000073d0000005400ffffff000003ef000002f700000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff000000010000000200000166000001960100000006010000000100
sceneTreePerspectives: 000000ff0000000100000002000000c0000000fc0100000006010000000200
maximizedDockId: -1
centralWidgetVisible: 1
selectionDisabled: 0
viewpointLocked: 0
orthographicViewHeight: 1
textFiles: 0 "controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.c"
renderingDevicePerspectives: quad_x_sitl:camera1;1;1.03125;0;0

12
libraries/SITL/examples/Webots/worlds/.webots_rover.wbproj Normal file
View File

@ -0,0 +1,12 @@
Webots Project File version R2019b
perspectives: 000000ff00000000fd00000003000000000000021d000003d5fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000005400ffffff00000001000000cd00000261fc0200000001fb0000001400540065007800740045006400690074006f0072010000001600000261000000a200ffffff000000030000039f0000016efc0100000001fb0000000e0043006f006e0073006f006c006501000000000000039f0000005400ffffff000002cc0000026100000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff000000010000000200000118000001340100000006010000000100
sceneTreePerspectives: 000000ff0000000100000002000000c0000000fc0100000006010000000200
maximizedDockId: -1
centralWidgetVisible: 1
renderingMode: WIREFRAME
selectionDisabled: 0
viewpointLocked: 0
orthographicViewHeight: 1
textFiles: 0 "controllers/ardupilot_SITL_ROVER/ardupilot_SITL_ROVER.c"
renderingDevicePerspectives: rover:camera1;1;1;0;0

6690
libraries/SITL/examples/Webots/worlds/pyramidMapReduced2.wbt Normal file
View File

File diff suppressed because it is too large Load Diff

379
libraries/SITL/examples/Webots/worlds/webots_quadPlus.wbt Normal file
View File

@ -0,0 +1,379 @@
#VRML_SIM R2019b utf8
WorldInfo {
gravity 0 -9.80665 0
basicTimeStep 2
FPS 15
optimalThreadCount 4
randomSeed 52
}
DogHouse {
translation 34.82 0.76 -24.56
name "dog house(1)"
}
DogHouse {
translation 161.819 0.75 -152.174
name "dog house(2)"
}
DogHouse {
translation 185.42 0.77 48.97
name "dog house(5)"
}
Viewpoint {
orientation 0.9206416250609748 0.36103346961097993 0.14857264898272837 5.442484201093071
position -1.9234783002250637 6.507213279340571 5.620669046678885
follow "quad_plus_sitl"
followOrientation TRUE
}
Background {
skyColor [
0.15 0.5 1
]
cubemap Cubemap {
}
}
Solid {
translation 36.93 0.77 -37.93
children [
HouseWithGarage {
}
]
}
Solid {
translation 192.76999999999998 0 64.98
rotation 0 1 0 -1.5707963071795863
children [
HouseWithGarage {
}
]
name "solid(1)"
}
Robot {
translation -0.027600999999999997 0.674307 0.005031
rotation -0.012461000916064287 0.999885073506054 -0.008635000634797779 -0.22761130717958622
children [
Shape {
appearance Appearance {
material Material {
}
}
geometry Box {
size 0.1 0.1 0.1
}
}
Camera {
translation 0 0.12 0
rotation 0.12942795977353752 0.9831056944488316 0.12942795977353752 -1.5878343071795866
name "camera1"
width 128
height 128
}
Compass {
name "compass1"
}
GPS {
name "gps1"
}
Accelerometer {
name "accelerometer1"
}
Gyro {
name "gyro1"
}
InertialUnit {
name "inertial_unit"
}
Transform {
translation -0.09999999999999999 0 0
rotation -0.5773502691896258 0.5773502691896258 0.5773502691896258 2.094395
children [
Solid {
translation 0 0.35 0
rotation 1 0 0 1.5707959999999999
children [
Propeller {
shaftAxis 0 -1 0
thrustConstants -12.2583125 0
torqueConstants 18 0
device RotationalMotor {
name "motor3"
controlPID 10.001 0 0
maxVelocity 1000
}
fastHelix Solid {
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 0 0.1
}
}
geometry Cylinder {
height 0.002
radius 0.02
}
}
]
}
slowHelix Solid {
rotation 0 1 0 1.1667874781290464
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 0 0.1
}
}
geometry Cylinder {
height 0.002
radius 0.02
}
}
]
}
}
]
physics Physics {
mass 0.25
}
}
Shape {
appearance Appearance {
material Material {
}
}
geometry DEF DEF_ARM Cylinder {
height 0.1
radius 0.01
}
}
]
}
Transform {
translation 0 0 0.09999999999999999
rotation 0 0.7071067811865476 0.7071067811865476 -3.1415923071795864
children [
Solid {
translation 0 0.35 0
rotation 1 0 0 1.5707959999999999
children [
Propeller {
shaftAxis 0 1 0
thrustConstants 12.2583125 0
torqueConstants 18 0
device RotationalMotor {
name "motor2"
controlPID 10.001 0 0
maxVelocity 1000
}
fastHelix Solid {
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 0 0.1
}
}
geometry Cylinder {
height 0.002
radius 0.02
}
}
]
}
slowHelix Solid {
rotation 0 -1 0 5.370767303526115
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 0 0.1
}
}
geometry Cylinder {
height 0.002
radius 0.02
}
}
]
}
}
]
name "solid(2)"
physics Physics {
mass 0.25
}
}
Shape {
appearance Appearance {
material Material {
}
}
geometry USE DEF_ARM
}
]
}
Transform {
translation 0.09999999999999999 0 0
rotation 0.5773502691896258 0.5773502691896258 0.5773502691896258 -2.094395307179586
children [
Solid {
translation 0 0.35 0
rotation 1 0 0 1.5707959999999999
children [
Propeller {
shaftAxis 0 -1 0
thrustConstants -12.2583125 0
torqueConstants 18 0
device RotationalMotor {
name "motor1"
controlPID 10.001 0 0
maxVelocity 1000
}
fastHelix Solid {
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 0 0.1
}
}
geometry Cylinder {
height 0.002
radius 0.02
}
}
]
}
slowHelix Solid {
rotation 0 1 0 5.486397909883531
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 0 0.1
}
}
geometry Cylinder {
height 0.002
radius 0.02
}
}
]
}
}
]
name "solid(1)"
physics Physics {
mass 0.25
}
}
Shape {
appearance Appearance {
material Material {
diffuseColor 1 0.09999999999999999 0
}
}
geometry USE DEF_ARM
}
]
}
Transform {
translation 0 0 -0.09999999999999999
rotation 1 0 0 -1.5707963071795863
children [
Solid {
translation 0 0.35 0
rotation 1 0 0 1.5707959999999999
children [
Propeller {
shaftAxis 0 1 0
thrustConstants 12.2583125 0
torqueConstants 18 0
device RotationalMotor {
name "motor4"
controlPID 10.001 0 0
maxVelocity 1000
}
fastHelix Solid {
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 0 0.1
}
}
geometry Cylinder {
height 0.002
radius 0.02
}
}
]
}
slowHelix Solid {
rotation 0 -1 0 5.350616673324008
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 0 0.1
}
}
geometry Cylinder {
height 0.002
radius 0.02
}
}
]
}
}
]
name "solid(3)"
physics Physics {
mass 0.25
}
}
Shape {
appearance Appearance {
material Material {
diffuseColor 0.7999999999999999 0.7999999999999999 0.7999999999999999
}
}
geometry USE DEF_ARM
}
]
}
]
name "quad_plus_sitl"
boundingObject Box {
size 0.1 0.1 0.1
}
physics Physics {
density -1
mass 1.5
centerOfMass [
0 0 0
]
}
rotationStep 0.261799
controller "ardupilot_SITL_QUAD"
controllerArgs "-p 5599"
supervisor TRUE
}
DirectionalLight {
direction 0 -1 0
}
UnevenTerrain {
size 500 1 500
}
HouseWithGarage {
translation 174.25 1.88 -157.5
rotation 0 1 0 -1.5707963071795863
}
AdvertisingBoard {
translation 0 2.35 -5.71
}
AdvertisingBoard {
translation 84.03999999999999 2.35 -5.81
rotation 0 1 0 -1.5707963071795863
name "advertising board(1)"
}

385
libraries/SITL/examples/Webots/worlds/webots_quadX.wbt Normal file
View File

@ -0,0 +1,385 @@
#VRML_SIM R2019b utf8
WorldInfo {
gravity 0 -9.80665 0
basicTimeStep 2
FPS 25
optimalThreadCount 4
randomSeed 52
}
DogHouse {
translation 34.82 0.76 -24.56
name "dog house(1)"
}
DogHouse {
translation 161.819 0.75 -152.174
name "dog house(2)"
}
DogHouse {
translation 185.42 0.77 48.97
name "dog house(5)"
}
Viewpoint {
orientation 0.8271274640436935 0.5437841938242927 0.1419820720073933 5.671643293801365
position -7.234434459826133 13.00762277807382 18.43704042041417
follow "quad_x_sitl"
followOrientation TRUE
}
Background {
skyColor [
0.15 0.5 1
]
cubemap Cubemap {
}
}
Solid {
translation 36.93 0.77 -37.93
children [
HouseWithGarage {
}
]
}
Solid {
translation 192.76999999999998 0 64.98
rotation 0 1 0 -1.5707963071795863
children [
HouseWithGarage {
}
]
name "solid(1)"
}
Robot {
translation -0.027601 0.674307 0.005031
rotation 0 1 0 0.785388
children [
Shape {
appearance Appearance {
material Material {
}
}
geometry Box {
size 0.1 0.1 0.1
}
}
Camera {
translation 0 0.12 0
rotation 0 -1 0 2.356195
name "camera1"
width 128
height 128
}
Compass {
rotation 0 1 0 -0.7853983071795865
name "compass1"
}
GPS {
rotation 0 1 0 -0.785398
name "gps1"
}
Accelerometer {
rotation 0 1 0 -0.785398
name "accelerometer1"
}
Gyro {
rotation 0 1 0 -0.785398
name "gyro1"
}
InertialUnit {
rotation 0 -1 0 0.7853979999999999
name "inertial_unit"
}
Transform {
translation -0.09999999999999999 0 0
rotation -0.5773502691896258 0.5773502691896258 0.5773502691896258 2.094395
children [
Solid {
translation 0 0.35 0
rotation 1 0 0 1.5707959999999999
children [
Propeller {
shaftAxis 0 -1 0
thrustConstants -12.2583125 0
torqueConstants 18 0
device RotationalMotor {
name "motor3"
controlPID 10.001 0 0
maxVelocity 1000
}
fastHelix Solid {
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 0 0.1
}
}
geometry Cylinder {
height 0.002
radius 0.02
}
}
]
}
slowHelix Solid {
rotation 0 1 0 1.1667874781290464
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 0 0.1
}
}
geometry Cylinder {
height 0.002
radius 0.02
}
}
]
}
}
]
physics Physics {
mass 0.25
}
}
Shape {
appearance Appearance {
material Material {
}
}
geometry DEF DEF_ARM Cylinder {
height 0.1
radius 0.01
}
}
]
}
Transform {
translation 0 0 0.09999999999999999
rotation 0 0.7071067811865476 0.7071067811865476 -3.1415923071795864
children [
Solid {
translation 0 0.35 0
rotation 1 0 0 1.5707959999999999
children [
Propeller {
shaftAxis 0 1 0
thrustConstants 12.2583125 0
torqueConstants 18 0
device RotationalMotor {
name "motor2"
controlPID 10.001 0 0
maxVelocity 1000
}
fastHelix Solid {
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 0 0.1
}
}
geometry Cylinder {
height 0.002
radius 0.02
}
}
]
}
slowHelix Solid {
rotation 0 -1 0 5.370767303526115
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 0 0.1
}
}
geometry Cylinder {
height 0.002
radius 0.02
}
}
]
}
}
]
name "solid(2)"
physics Physics {
mass 0.25
}
}
Shape {
appearance Appearance {
material Material {
diffuseColor 1 0.09999999999999999 0
}
}
geometry USE DEF_ARM
}
]
}
Transform {
translation 0.09999999999999999 0 0
rotation 0.5773502691896258 0.5773502691896258 0.5773502691896258 -2.094395307179586
children [
Solid {
translation 0 0.35 0
rotation 1 0 0 1.5707959999999999
children [
Propeller {
shaftAxis 0 -1 0
thrustConstants -12.2583125 0
torqueConstants 18 0
device RotationalMotor {
name "motor1"
controlPID 10.001 0 0
maxVelocity 1000
}
fastHelix Solid {
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 0 0.1
}
}
geometry Cylinder {
height 0.002
radius 0.02
}
}
]
}
slowHelix Solid {
rotation 0 1 0 5.486397909883531
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 0 0.1
}
}
geometry Cylinder {
height 0.002
radius 0.02
}
}
]
}
}
]
name "solid(1)"
physics Physics {
mass 0.25
}
}
Shape {
appearance Appearance {
material Material {
diffuseColor 1 0.09999999999999999 0
}
}
geometry USE DEF_ARM
}
]
}
Transform {
translation 0 0 -0.09999999999999999
rotation 1 0 0 -1.5707963071795863
children [
Solid {
translation 0 0.35 0
rotation 1 0 0 1.5707959999999999
children [
Propeller {
shaftAxis 0 1 0
thrustConstants 12.2583125 0
torqueConstants 18 0
device RotationalMotor {
name "motor4"
controlPID 10.001 0 0
maxVelocity 1000
}
fastHelix Solid {
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 0 0.1
}
}
geometry Cylinder {
height 0.002
radius 0.02
}
}
]
}
slowHelix Solid {
rotation 0 -1 0 5.350616673324008
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 0 0.1
}
}
geometry Cylinder {
height 0.002
radius 0.02
}
}
]
}
}
]
name "solid(3)"
physics Physics {
mass 0.25
}
}
Shape {
appearance Appearance {
material Material {
diffuseColor 0.7999999999999999 0.7999999999999999 0.7999999999999999
}
}
geometry USE DEF_ARM
}
]
}
]
name "quad_x_sitl"
boundingObject Box {
size 0.1 0.1 0.1
}
physics Physics {
density -1
mass 1.5
centerOfMass [
0 0 0
]
}
rotationStep 0.261799
controller "ardupilot_SITL_QUAD"
controllerArgs "-p 5599"
supervisor TRUE
}
DirectionalLight {
direction 0 -1 0
}
UnevenTerrain {
size 500 1 500
}
HouseWithGarage {
translation 174.25 1.88 -157.5
rotation 0 1 0 -1.5707963071795863
}
AdvertisingBoard {
translation 0 2.35 -5.71
}
AdvertisingBoard {
translation 84.03999999999999 2.35 -5.81
rotation 0 1 0 -1.5707963071795863
name "advertising board(1)"
}

120
libraries/SITL/examples/Webots/worlds/webots_rover.wbt Normal file
View File

@ -0,0 +1,120 @@
#VRML_SIM R2019b utf8
WorldInfo {
gravity 0 -9.80665 0
basicTimeStep 2
FPS 25
optimalThreadCount 4
randomSeed 52
}
DogHouse {
translation 34.82 0.76 -24.56
name "dog house(1)"
}
DogHouse {
translation 161.819 0.75 -152.174
name "dog house(2)"
}
DogHouse {
translation 185.42 0.77 48.97
name "dog house(5)"
}
Viewpoint {
orientation -0.4802960487107921 -0.8160262481931511 -0.32158493100985075 1.374242044385044
position -34.80383906285916 39.60501890601726 13.953518107115032
followOrientation TRUE
}
Background {
skyColor [
0.15 0.5 1
]
cubemap Cubemap {
}
}
Solid {
translation 36.93 0.77 -37.93
children [
HouseWithGarage {
}
]
}
Solid {
translation 192.76999999999998 0 64.98
rotation 0 1 0 -1.5707963071795863
children [
HouseWithGarage {
}
]
name "solid(1)"
}
Car {
translation -0.0010326953082284774 1.0033229120626295 2.2409311299249333
rotation 0.00024190667924360975 0.9999990672138167 0.0013442665863175399 1.5707409280078055
name "rover"
controller "ardupilot_SITL_ROVER"
supervisor TRUE
wheelbase 2
extensionSlot [
Camera {
rotation 0 1 0 -3.1415923071795864
name "camera1"
}
InertialUnit {
rotation 0 -1 0 1.5707959999999999
name "inertial_unit"
}
Compass {
rotation 0 -1 0 1.5707959999999999
name "compass1"
}
Gyro {
rotation 0 -1 0 1.5707959999999999
name "gyro1"
}
Accelerometer {
rotation 0 -1 0 1.5707959999999999
name "accelerometer1"
}
GPS {
rotation 0 -1 0 1.5707959999999999
name "gps1"
}
VehicleLights {
}
]
physics Physics {
mass 3.5
}
wheelFrontRight VehicleWheel {
thickness 0.2
tireRadius 0.3
}
wheelFrontLeft VehicleWheel {
name "vehicle wheel(1)"
thickness 0.2
tireRadius 0.3
}
wheelRearRight VehicleWheel {
}
wheelRearLeft VehicleWheel {
name "wheel4"
}
engineType "electric"
}
DirectionalLight {
direction 0 -1 0
}
UnevenTerrain {
size 500 1 500
}
HouseWithGarage {
translation 174.25 1.88 -157.5
rotation 0 1 0 -1.5707963071795863
}
AdvertisingBoard {
translation 0 2.35 -5.71
}
AdvertisingBoard {
translation 84.03999999999999 2.35 -5.81
rotation 0 1 0 -1.5707963071795863
name "advertising board(1)"
}