Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: Revamp Examples to support multiple drones

Browse Source
This commit is contained in:
mhefny 2020-05-14 08:26:46 +02:00 committed by Peter Barker
parent 5a64156862
commit 9f56b656e0
31 changed files with 9365 additions and 342 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/SITL/examples/Webots/.gitignore vendored
View File

@ -6,3 +6,5 @@
/controllers/ardupilot_SITL_ROVER/ardupilot_SITL_ROVER /controllers/ardupilot_SITL_ROVER/ardupilot_SITL_ROVER
/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD /controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD
/controllers/ardupilot_SITL_TRICOPTER/ardupilot_SITL_TRICOPTER /controllers/ardupilot_SITL_TRICOPTER/ardupilot_SITL_TRICOPTER
/controllers/ardupilot_SITL_Supervisor/ardupilot_SITL_Supervisor

182
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.c
View File

@ -4,6 +4,8 @@
* Description: integration with ardupilot SITL simulation. * Description: integration with ardupilot SITL simulation.
* Author: M.S.Hefny (HefnySco) * Author: M.S.Hefny (HefnySco)
* Modifications: * Modifications:
* - Blocking sockets
* - Advance simulation time only when receive motor data.
*/ */
@ -34,6 +36,7 @@
#include <webots/robot.h> #include <webots/robot.h>
#include <webots/supervisor.h> #include <webots/supervisor.h>
#include <webots/emitter.h> #include <webots/emitter.h>
#include <webots/receiver.h>
#include "ardupilot_SITL_QUAD.h" #include "ardupilot_SITL_QUAD.h"
#include "sockets.h" #include "sockets.h"
#include "sensors.h" #include "sensors.h"
@ -51,9 +54,10 @@ static WbDeviceTag gps;
static WbDeviceTag camera; static WbDeviceTag camera;
static WbDeviceTag inertialUnit; static WbDeviceTag inertialUnit;
static WbDeviceTag emitter; static WbDeviceTag emitter;
static WbNodeRef world_info; static WbDeviceTag receiver;
static const double *northDirection; static double _linear_velocity[3] = {0.0,0.0,0.0};
static double northDirection = 1;
static double v[MOTOR_NUM]; static double v[MOTOR_NUM];
int port; int port;
float dragFactor = VEHICLE_DRAG_FACTOR; float dragFactor = VEHICLE_DRAG_FACTOR;
@ -61,11 +65,6 @@ float dragFactor = VEHICLE_DRAG_FACTOR;
static int timestep; static int timestep;
// maxSimCycleTime limits the fasts execution path. it is very useful in SLOW MOTION.
// Increasing simulation speed using ">>" button on webots may not be effective
// if this value > 0.
int maxSimCycleTime = 0; // no delay
#ifdef DEBUG_USE_KB #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. // Code used tp simulae motors using keys to make sure that sensors directions and motor torques and thrusts are all correct.
@ -151,7 +150,22 @@ void process_keyboard ()
#endif #endif
// Read all messages and empty the Q and keep last value as the valid one.
static void read_incoming_messages()
{
// read while queue not empty
while (wb_receiver_get_queue_length(receiver) > 0) {
// I'm only expecting ascii messages
double * data = (double *) wb_receiver_get_data(receiver);
_linear_velocity[0] = data[0];
_linear_velocity[1] = data[1];
_linear_velocity[2] = data[2];
northDirection = data[3];
//printf("RAW Data [%f, %f, %f]\n", linear_velocity[0], linear_velocity[2], linear_velocity[1]);
wb_receiver_next_packet(receiver);
}
}
/* /*
// apply motor thrust. // apply motor thrust.
@ -209,7 +223,7 @@ void update_controls()
A is the cross section of our quad in m³ in the direction of movement A is the cross section of our quad in m³ in the direction of movement
v is the velocity in m/s v is the velocity in m/s
*/ */
if (northDirection[0] == 1) if (northDirection == 1)
{ {
wind_webots_axis.x = state.wind.x - linear_velocity[0]; wind_webots_axis.x = state.wind.x - linear_velocity[0];
wind_webots_axis.z = -state.wind.y - linear_velocity[2]; // "-state.wind.y" as angle 90 wind is from EAST. wind_webots_axis.z = -state.wind.y - linear_velocity[2]; // "-state.wind.y" as angle 90 wind is from EAST.
@ -241,7 +255,6 @@ void update_controls()
// the JSON parser is directly inspired by https://github.com/ArduPilot/ardupilot/blob/master/libraries/SITL/SIM_Morse.cpp // 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) bool parse_controls(const char *json)
{ {
//state.timestamp = 1.0;
#ifdef DEBUG_INPUT_DATA #ifdef DEBUG_INPUT_DATA
printf("%s\n", json); printf("%s\n", json);
#endif #endif
@ -249,7 +262,6 @@ bool parse_controls(const char *json)
for (uint16_t i=0; i < ARRAY_SIZE(keytable); i++) { for (uint16_t i=0; i < ARRAY_SIZE(keytable); i++) {
struct keytable *key; struct keytable *key;
key = &keytable[i]; key = &keytable[i];
//printf("search %s/%s\n", key->section, key->key);
// look for section header // look for section header
const char *p = strstr(json, key->section); const char *p = strstr(json, key->section);
if (!p) { if (!p) {
@ -261,7 +273,7 @@ bool parse_controls(const char *json)
// find key inside section // find key inside section
p = strstr(p, key->key); p = strstr(p, key->key);
if (!p) { if (!p) {
printf("Failed to find key %s/%s\n", key->section, key->key); fprintf(stderr,"Failed to find key %s/%s DATA:%s\n", key->section, key->key, json);
return false; return false;
} }
@ -286,7 +298,7 @@ bool parse_controls(const char *json)
case DATA_VECTOR4F: { case DATA_VECTOR4F: {
VECTOR4F *v = (VECTOR4F *)key->ptr; VECTOR4F *v = (VECTOR4F *)key->ptr;
if (sscanf(p, "[%f, %f, %f, %f]", &(v->w), &(v->x), &(v->y), &(v->z)) != 4) { 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); fprintf(stderr,"Failed to parse Vector3f for %s %s/%s\n",p, key->section, key->key);
return false; return false;
} }
else else
@ -306,16 +318,15 @@ bool parse_controls(const char *json)
void run () void run ()
{ {
char send_buf[1000]; //1000 just a safe margin char send_buf[1000];
char command_buffer[200]; char command_buffer[1000];
fd_set rfds; fd_set rfds;
while (wb_robot_step(timestep) != -1) // calculate initial sensor values.
wb_robot_step(timestep);
while (true)
{ {
for (int i=0;i<maxSimCycleTime;++i)
{
usleep(1000);
}
#ifdef DEBUG_USE_KB #ifdef DEBUG_USE_KB
process_keyboard(); process_keyboard();
#endif #endif
@ -324,21 +335,23 @@ void run ()
{ {
// if no socket wait till you get a socket // if no socket wait till you get a socket
fd = socket_accept(sfd); fd = socket_accept(sfd);
if (fd > 0) if (fd < 0)
socket_set_non_blocking(fd);
else if (fd < 0)
break; break;
} }
read_incoming_messages();
// trigget ArduPilot to send motor data
getAllSensors ((char *)send_buf, northDirection, gyro,accelerometer,compass,gps, inertialUnit); getAllSensors ((char *)send_buf, northDirection, gyro,accelerometer,compass,gps, inertialUnit);
#ifdef DEBUG_SENSORS #ifdef DEBUG_SENSORS
printf("%s\n",send_buf); printf("at %lf %s\n",wb_robot_get_time(), send_buf);
#endif #endif
if (write(fd,send_buf,strlen(send_buf)) <= 0) if (write(fd,send_buf,strlen(send_buf)) <= 0)
{ {
printf ("Send Data Error\n"); fprintf (stderr,"Send Data Error\n");
} }
if (fd) if (fd)
@ -352,7 +365,7 @@ void run ()
if (number != 0) if (number != 0)
{ {
// there is a valid connection // there is a valid connection
int n = recv(fd, (char *)command_buffer, 200, 0); int n = recv(fd, (char *)command_buffer, 1000, 0);
if (n < 0) { if (n < 0) {
#ifdef _WIN32 #ifdef _WIN32
@ -369,21 +382,21 @@ void run ()
#endif #endif
break; break;
} }
if (n==0) if (n==0)
{ {
break; break;
} }
if (command_buffer[0] == 'e')
{
break;
}
if (n > 0) if (n > 0)
{ {
command_buffer[n] = 0; command_buffer[n] = 0;
parse_controls (command_buffer); if (parse_controls (command_buffer))
{
update_controls(); update_controls();
//https://cyberbotics.com/doc/reference/robot#wb_robot_step
// this is used to force webots not to execute untill it receives feedback from simulator.
wb_robot_step(timestep);
}
} }
} }
@ -395,7 +408,7 @@ void run ()
} }
void initialize (int argc, char *argv[]) bool initialize (int argc, char *argv[])
{ {
fd_set rfds; fd_set rfds;
@ -404,7 +417,7 @@ void initialize (int argc, char *argv[])
for (int i = 0; i < argc; ++i) for (int i = 0; i < argc; ++i)
{ {
if (strcmp (argv[i],"-p")==0) if (strcmp (argv[i],"-p")==0)
{ { // specify port for SITL.
if (argc > i+1 ) if (argc > i+1 )
{ {
port = atoi (argv[i+1]); port = atoi (argv[i+1]);
@ -412,21 +425,18 @@ void initialize (int argc, char *argv[])
} }
} }
else if (strcmp (argv[i],"-df")==0) else if (strcmp (argv[i],"-df")==0)
{ { // specify drag functor used to simulate air resistance.
if (argc > i+1 ) if (argc > i+1 )
{ {
dragFactor = atof (argv[i+1]); dragFactor = atof (argv[i+1]);
printf("drag Factor %f\n",dragFactor); printf("drag Factor %f\n",dragFactor);
} }
} else
else if (strcmp (argv[i],"-d")==0)
{ {
if (argc > i+1 ) fprintf(stderr,"Missing drag factor value.\n");
{ return false;
// extra delay in milliseconds
maxSimCycleTime = atoi (argv[i+1]);
printf("max simulation cycle time is %d ms\n",maxSimCycleTime);
} }
} }
} }
@ -436,45 +446,37 @@ void initialize (int argc, char *argv[])
/* necessary to initialize webots stuff */ /* necessary to initialize webots stuff */
wb_robot_init(); wb_robot_init();
// Get WorldInfo Node. timestep = (int)wb_robot_get_basic_time_step();
WbNodeRef root, node;
WbFieldRef children, field; // init receiver from Supervisor
int n, i; receiver = wb_robot_get_device("receiver_main");
root = wb_supervisor_node_get_root(); if (receiver ==0)
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; fprintf(stderr,"Receiver not found\n");
break; fprintf(stderr,"EXIT with error\n");
} return false;
} }
node = wb_supervisor_field_get_mf_node(children, 0); // read robot number and set receiver channel accordingly
field = wb_supervisor_node_get_field(node, "northDirection"); const char * customData = wb_robot_get_custom_data();
northDirection = wb_supervisor_field_get_sf_vec3f(field); if (customData != NULL)
if (northDirection[0] == 1)
{ {
printf ("Axis Default Directions\n"); int channel = atoi(customData);
wb_receiver_set_channel(receiver,channel);
wb_receiver_enable(receiver,timestep);
printf("Receiver Channel at %d \n", channel);
}
else
{
fprintf(stderr, "MISSING Channel NO. in Custom Data");
return false;
} }
printf("WorldInfo.northDirection = %g %g %g\n\n", northDirection[0], northDirection[1], northDirection[2]);
// get Self Node
self_node = wb_supervisor_node_get_self();
// keybaard // keybaard
timestep = (int)wb_robot_get_basic_time_step(); #ifdef DEBUG_USE_KB
wb_keyboard_enable(timestep); wb_keyboard_enable(timestep);
#endif
// inertialUnit // inertialUnit
@ -499,24 +501,30 @@ void initialize (int argc, char *argv[])
// camera // camera
camera = wb_robot_get_device("camera1"); camera = wb_robot_get_device("camera1");
wb_camera_enable(camera, timestep); wb_camera_enable(camera, CAMERA_FRAME_RATE_FACTOR * timestep);
#ifdef WIND_SIMULATION #ifdef WIND_SIMULATION
// emitter // emitter
emitter = wb_robot_get_device("emitter_plugin"); emitter = wb_robot_get_device("emitter_plugin");
#endif #endif
// names of motor should be the same as name of motor in the robot.
const char *MOTOR_NAMES[] = {"motor1", "motor2", "motor3", "motor4"}; const char *MOTOR_NAMES[] = {"motor1", "motor2", "motor3", "motor4"};
// get motor device tags // get motor device tags
for (i = 0; i < MOTOR_NUM; i++) { for (int i = 0; i < MOTOR_NUM; i++) {
motors[i] = wb_robot_get_device(MOTOR_NAMES[i]); motors[i] = wb_robot_get_device(MOTOR_NAMES[i]);
v[i] = 0.0f; v[i] = 0.0f;
//assert(motors[i]);
} }
FD_ZERO(&rfds); FD_ZERO(&rfds);
FD_SET(sfd, &rfds); FD_SET(sfd, &rfds);
// init linear_velocity untill we receive valid data from Supervisor.
linear_velocity = &_linear_velocity[0] ;
return true;
} }
/* /*
* This is the main program. * This is the main program.
@ -528,35 +536,15 @@ int main(int argc, char **argv)
initialize( argc, argv); if (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: * Enter here functions to send actuator commands, like:
* wb_differential_wheels_set_speed(100.0,100.0); * wb_differential_wheels_set_speed(100.0,100.0);
*/ */
run(); run();
}
/* Enter your cleanup code here */ /* Enter your cleanup code here */

9
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.h
View File

@ -4,11 +4,16 @@
// #define DEBUG_USE_KB // #define DEBUG_USE_KB
// #define DEBUG_INPUT_DATA // #define DEBUG_INPUT_DATA
// #define LINEAR_THRUST // #define LINEAR_THRUST
#define WIND_SIMULATION
#define DEBUG_SOCKETS #define DEBUG_SOCKETS
#define WIND_SIMULATION
#define VEHICLE_DRAG_FACTOR 0.001 #define VEHICLE_DRAG_FACTOR 0.001
// # of simulation steps between two image frames.
#define CAMERA_FRAME_RATE_FACTOR 50
#define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0])) #define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0]))
@ -58,5 +63,5 @@ struct keytable {
w: wind speed w: wind speed
x , y, z: wind direction. x , y, z: wind direction.
*/ */
VECTOR4F __attribute__((packed, aligned(1))) wind_webots_axis; VECTOR4F wind_webots_axis;

33
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/sensors.c
View File

@ -33,10 +33,10 @@ copter.rotate_body_frame_to_NE(vel_vector.x, vel_vector.y);
/* /*
returns: "yaw":_6.594794831471518e-05,"pitch":_-0.0005172680830582976,"roll":_0.022908752784132957}} returns: "yaw":_6.594794831471518e-05,"pitch":_-0.0005172680830582976,"roll":_0.022908752784132957}}
*/ */
void getInertia (const WbDeviceTag inertialUnit, const double *northDirection, char *buf) void getInertia (const WbDeviceTag inertialUnit, const double northDirection, char *buf)
{ {
const double *inertial_directions = wb_inertial_unit_get_roll_pitch_yaw (inertialUnit); const double *inertial_directions = wb_inertial_unit_get_roll_pitch_yaw (inertialUnit);
if (northDirection[0] == 1) if (northDirection == 1)
{ {
sprintf(buf,"\"roll\": %f,\"pitch\": %f,\"yaw\": %f",inertial_directions[0], inertial_directions[1], inertial_directions[2]); sprintf(buf,"\"roll\": %f,\"pitch\": %f,\"yaw\": %f",inertial_directions[0], inertial_directions[1], inertial_directions[2]);
} }
@ -51,10 +51,10 @@ void getInertia (const WbDeviceTag inertialUnit, const double *northDirection, c
/* /*
returns: "magnetic_field":_[23088.669921875,_3876.001220703125,_-53204.57421875] returns: "magnetic_field":_[23088.669921875,_3876.001220703125,_-53204.57421875]
*/ */
void getCompass (const WbDeviceTag compass, const double *northDirection, char *buf) void getCompass (const WbDeviceTag compass, const double northDirection, char *buf)
{ {
const double *north3D = wb_compass_get_values(compass); const double *north3D = wb_compass_get_values(compass);
if (northDirection[0] == 1) if (northDirection == 1)
{ {
sprintf(buf,"[%f, %f, %f]",north3D[0], north3D[2], north3D[1]); sprintf(buf,"[%f, %f, %f]",north3D[0], north3D[2], north3D[1]);
} }
@ -71,11 +71,11 @@ void getCompass (const WbDeviceTag compass, const double *northDirection, char *
/* /*
returns: "vehicle.gps":{"timestamp":_1563301031.055164,"x":_5.5127296946011484e-05,"y":_-0.0010968948481604457,"z":_0.037179552018642426}, 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) void getGPS (const WbDeviceTag gps, const double northDirection, char *buf)
{ {
const double *north3D = wb_gps_get_values(gps); const double *north3D = wb_gps_get_values(gps);
if (northDirection[0] == 1) if (northDirection == 1)
{ {
sprintf(buf,"\"x\": %f,\"y\": %f,\"z\": %f", north3D[0], north3D[2], north3D[1]); sprintf(buf,"\"x\": %f,\"y\": %f,\"z\": %f", north3D[0], north3D[2], north3D[1]);
} }
@ -91,11 +91,11 @@ void getGPS (const WbDeviceTag gps, const double *northDirection, char *buf)
/* /*
returns: "linear_acceleration": [0.005074390675872564, 0.22471477091312408, 9.80740737915039] returns: "linear_acceleration": [0.005074390675872564, 0.22471477091312408, 9.80740737915039]
*/ */
void getAcc (const WbDeviceTag accelerometer, const double *northDirection, char *buf) void getAcc (const WbDeviceTag accelerometer, const double northDirection, char *buf)
{ {
//SHOULD BE CORRECT //SHOULD BE CORRECT
const double *a = wb_accelerometer_get_values(accelerometer); const double *a = wb_accelerometer_get_values(accelerometer);
if (northDirection[0] == 1) if (northDirection == 1)
{ {
sprintf(buf,"[%f, %f, %f]",a[0], a[2], a[1]); sprintf(buf,"[%f, %f, %f]",a[0], a[2], a[1]);
} }
@ -114,11 +114,11 @@ void getAcc (const WbDeviceTag accelerometer, const double *northDirection, char
/* /*
returns: "angular_velocity": [-1.0255117643964695e-07, -8.877226775894087e-08, 2.087078510015772e-09] returns: "angular_velocity": [-1.0255117643964695e-07, -8.877226775894087e-08, 2.087078510015772e-09]
*/ */
void getGyro (const WbDeviceTag gyro, const double *northDirection, char *buf) void getGyro (const WbDeviceTag gyro, const double northDirection, char *buf)
{ {
const double *g = wb_gyro_get_values(gyro); const double *g = wb_gyro_get_values(gyro);
if (northDirection[0] == 1) if (northDirection == 1)
{ {
sprintf(buf,"[%f, %f, %f]",g[0], g[2], g[1]); sprintf(buf,"[%f, %f, %f]",g[0], g[2], g[1]);
} }
@ -131,24 +131,23 @@ void getGyro (const WbDeviceTag gyro, const double *northDirection, char *buf)
} }
void getLinearVelocity (WbNodeRef nodeRef, const double *northDirection, char * buf) void getLinearVelocity (WbNodeRef nodeRef, const double northDirection, char * buf)
{ {
linear_velocity = wb_supervisor_node_get_velocity (nodeRef);
if (linear_velocity != NULL) if (linear_velocity != NULL)
{ {
if (northDirection[0] == 1) if (northDirection == 1)
{ { // local map northDirection [1,0,0]
sprintf (buf,"[%f, %f, %f]", linear_velocity[0], linear_velocity[2], linear_velocity[1]); sprintf (buf,"[%f, %f, %f]", linear_velocity[0], linear_velocity[2], linear_velocity[1]);
} }
else else
{ { // openstreet map northDirection [0,0,1]
sprintf (buf,"[%f, %f, %f]", linear_velocity[2], -linear_velocity[0], linear_velocity[1]); sprintf (buf,"[%f, %f, %f]", linear_velocity[2], -linear_velocity[0], linear_velocity[1]);
} }
} }
} }
void getAllSensors (char *buf, const double *northDirection, WbDeviceTag gyro, WbDeviceTag accelerometer, WbDeviceTag compass, const WbDeviceTag gps, WbDeviceTag inertial_unit) void getAllSensors (char *buf, const double northDirection, WbDeviceTag gyro, WbDeviceTag accelerometer, WbDeviceTag compass, const WbDeviceTag gps, const WbDeviceTag inertial_unit)
{ {
/* /*
@ -179,7 +178,7 @@ void getAllSensors (char *buf, const double *northDirection, WbDeviceTag gyro, W
char szTime[21]; char szTime[21];
double time = wb_robot_get_time(); // current simulation time in [s] double time = wb_robot_get_time(); // current simulation time in [s]
sprintf(szTime,"%f", time); sprintf(szTime,"%lf", time);
getGyro(gyro, northDirection, gyro_buf); getGyro(gyro, northDirection, gyro_buf);
getAcc(accelerometer, northDirection, acc_buf); getAcc(accelerometer, northDirection, acc_buf);

14
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/sensors.h
View File

@ -14,10 +14,10 @@
WbNodeRef self_node; WbNodeRef self_node;
double *linear_velocity; double *linear_velocity;
void getInertia (const WbDeviceTag inertialUnit, const double *northDirection, char *buf); void getInertia (const WbDeviceTag inertialUnit, const double northDirection, char *buf);
void getLinearVelocity (WbNodeRef nodeRef, 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 getCompass (const WbDeviceTag compass, const double northDirection, char *buf);
void getAcc (const WbDeviceTag accelerometer, 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 getGyro (const WbDeviceTag gyro, const double northDirection, char *buf);
void getGPS (const WbDeviceTag gps, 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); void getAllSensors (char *buf, const double northDirection, WbDeviceTag gyro, WbDeviceTag accelerometer, WbDeviceTag compass, const WbDeviceTag gps, const WbDeviceTag inertial_unit);

34
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_ROVER/ardupilot_SITL_ROVER.c
View File

@ -1,9 +1,11 @@
/* /*
* File: ardupilot_SITL_ROV.c * File: ardupilot_SITL_ROV.c
* Date: * Date: July 2019
* Description: * Description: integration with ardupilot SITL simulation.
* Author: * Author: M.S.Hefny (HefnySco)
* Modifications: * Modifications:
* - Blocking sockets
* - Advance simulation time only when receive motor data.
*/ */
/* /*
@ -128,7 +130,7 @@ bool parse_controls(const char *json)
// find key inside section // find key inside section
p = strstr(p, key->key); p = strstr(p, key->key);
if (!p) { if (!p) {
printf("Failed to find key %s/%s\n", key->section, key->key); printf("Failed to find key %s/%s DATA:%s\n", key->section, key->key, json);
return false; return false;
} }
@ -189,9 +191,13 @@ void run ()
{ {
char send_buf[1000]; //1000 just a safe margin char send_buf[1000]; //1000 just a safe margin
char command_buffer[200]; char command_buffer[1000];
fd_set rfds; fd_set rfds;
while (wb_robot_step(timestep) != -1)
// trigget ArduPilot to send motor data
wb_robot_step(timestep);
while (true)
{ {
#ifdef DEBUG_USE_KB #ifdef DEBUG_USE_KB
process_keyboard(); process_keyboard();
@ -202,7 +208,9 @@ void run ()
// if no socket wait till you get a socket // if no socket wait till you get a socket
fd = socket_accept(sfd); fd = socket_accept(sfd);
if (fd > 0) if (fd > 0)
socket_set_non_blocking(fd); {
//socket_set_non_blocking(fd);
}
else if (fd < 0) else if (fd < 0)
break; break;
} }
@ -230,7 +238,7 @@ void run ()
{ {
// there is a valid connection // there is a valid connection
int n = recv(fd, (char *)command_buffer, 200, 0); int n = recv(fd, (char *)command_buffer, 1000, 0);
if (n < 0) { if (n < 0) {
#ifdef _WIN32 #ifdef _WIN32
int e = WSAGetLastError(); int e = WSAGetLastError();
@ -257,10 +265,14 @@ void run ()
if (n > 0) if (n > 0)
{ {
//printf("Received %d bytes:\n", n);
command_buffer[n] = 0; command_buffer[n] = 0;
parse_controls (command_buffer); if (parse_controls (command_buffer))
{
update_controls(); update_controls();
//https://cyberbotics.com/doc/reference/robot#wb_robot_step
wb_robot_step(timestep);
}
} }
} }
@ -356,7 +368,7 @@ void initialize (int argc, char *argv[])
// camera // camera
camera = wb_robot_get_device("camera1"); camera = wb_robot_get_device("camera1");
wb_camera_enable(camera, timestep); wb_camera_enable(camera, CAMERA_FRAME_RATE_FACTOR * timestep);
car = wb_robot_get_device ("rover"); car = wb_robot_get_device ("rover");

3
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_ROVER/ardupilot_SITL_ROVER.h
View File

@ -3,6 +3,9 @@
#define DEBUG_INPUT_DATA #define DEBUG_INPUT_DATA
#define LINEAR_THRUST #define LINEAR_THRUST
#define CAMERA_FRAME_RATE_FACTOR 50
#define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0])) #define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0]))

74
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_Supervisor/Makefile Normal file
View File

@ -0,0 +1,74 @@
# Copyright 1996-2020 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
###
### ---- 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_Supervisor.c
INCLUDE = -I"./"
space :=
space +=
WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME))))
include $(WEBOTS_HOME_PATH)/resources/Makefile.include

191
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_Supervisor/ardupilot_SITL_Supervisor.c Normal file
View File

@ -0,0 +1,191 @@
/*
* File: ardupilot_SITL_Supervisor.c
* Date:
* Description:
* Author:
* Modifications:
*/
/*
* You may need to add include files like <webots/distance_sensor.h> or
* <webots/motor.h>, etc.
*/
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <sys/types.h>
#include <webots/robot.h>
#include <webots/supervisor.h>
#include <webots/emitter.h>
#define MAX_NUM_ROBOTS 4
//define DEBUG_DATA
typedef struct {
WbNodeRef robot_node; // to track the robot node
int receiver_channel; // receiver channel
double velocity[3]; // to track robot's position
} Robot;
static WbDeviceTag emitter;
static WbNodeRef world_info, self_node;
static Robot *robots[MAX_NUM_ROBOTS];
static int actualRobots = 0;
double *linear_velocity;
static double northDirection[3] = {1.0,0.0,0.0};
/*
* You may want to add macros here.
*/
static int timestep;
void initialize (int argc, char *argv[])
{
WbNodeRef root, node;
WbFieldRef children, field;
int n, i , note_type;
/* necessary to initialize webots stuff */
wb_robot_init();
timestep = (int)wb_robot_get_basic_time_step();
self_node = wb_supervisor_node_get_self();
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, actualRobots=0; i < n; i++) {
node = wb_supervisor_field_get_mf_node(children, i);
field = wb_supervisor_node_get_field(node, "name");
note_type = wb_supervisor_node_get_type(node);
if ( note_type == WB_NODE_WORLD_INFO)
{
world_info = node;
//break;
}
else if ((note_type == WB_NODE_ROBOT) && (node != self_node))
{
/* code */
if (actualRobots < MAX_NUM_ROBOTS)
{
WbFieldRef channel = wb_supervisor_node_get_field(node, "customData");
Robot *robot = malloc(sizeof(Robot));
robots[actualRobots] = robot;
robots[actualRobots]->robot_node = node;
robots[actualRobots]->receiver_channel = atoi((const char *)(wb_supervisor_field_get_sf_string(channel)));
printf("Robot %s with ch %d is added \n",wb_supervisor_field_get_sf_string(field),robots[actualRobots]->receiver_channel);
actualRobots++;
}
else
{
printf("Robot %s is ignored \n",wb_supervisor_field_get_sf_string(field));
}
}
}
node = wb_supervisor_field_get_mf_node(children, 0);
field = wb_supervisor_node_get_field(node, "northDirection");
memcpy(&northDirection,wb_supervisor_field_get_sf_vec3f(field),sizeof(double)*3);
if (northDirection[0] == 1)
{
printf ("Axis Default Directions\n");
}
printf("WorldInfo.northDirection = %g %g %g\n\n", northDirection[0], northDirection[1], northDirection[2]);
printf("SUPVERVISOR timestep %d\n", timestep);
emitter = wb_robot_get_device("emitter");
}
/*
* 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
*/
while (true) {
wb_robot_step(timestep);
//printf("timestep %f\n", wb_robot_get_time());
/*
* 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_motor_set_position(my_actuator, 10.0);
*/
static double outputData[4];
for (int i=0; i < actualRobots; ++i)
{
linear_velocity = (double *) wb_supervisor_node_get_velocity (robots[i]->robot_node);
//sprintf (buf,"[%lf, %lf, %lf]", linear_velocity[0], linear_velocity[1], linear_velocity[2]);
//WbFieldRef channel = wb_supervisor_node_get_field(robots[i]->robot_node, "customData");
//wb_supervisor_field_set_sf_string (channel, &buf[0]);
//printf("%s",buf);
memcpy(outputData,linear_velocity, sizeof(double) * 3);
/*
* This is used to handle sensors axis when northDirection is different than [1,0,0]
* Note: that only two values are handled here.
* Local map northDirection [1,0,0]
* OpenStreetView map northDirection [0,0,1]
*/
outputData[3] = northDirection[0]; // send Map indicator;
wb_emitter_set_channel(emitter, robots[i]->receiver_channel);
wb_emitter_send(emitter, (const void *) outputData, sizeof(double) * 4);
#ifdef DEBUG_DATA
printf ("#%d at %lf Robot#%d [%lf, %lf, %lf, %lf]\n", test, wb_robot_get_time(), i, outputData[0], outputData[1], outputData[2], outputData[3]);
#endif
}
};
/* Enter your cleanup code here */
printf("OUT OF LOOP\n");
/* This is necessary to cleanup webots resources */
wb_robot_cleanup();
return 0;
}

230
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/ardupilot_SITL_TRICOPTER.c
View File

@ -1,12 +1,31 @@
/* /*
* File: ardupilot_SITL_TRICOPTER.c * File: ardupilot_SITL_TRICOPTER.c
* Date: 18 Aug 2019 * Date: 18 Aug 2019
* Description: integration with ardupilot SITL simulation. * Description: integration with ardupilot SITL simulation.
* Author: M.S.Hefny (HefnySco) * Author: M.S.Hefny (HefnySco)
* Modifications: * Modifications:
* - Blocking sockets
* - Advance simulation time only when receive motor data.
*/ */
/*
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 * You may need to add include files like <webots/distance_sensor.h> or
* <webots/differential_wheels.h>, etc. * <webots/differential_wheels.h>, etc.
@ -18,12 +37,12 @@
#include <webots/robot.h> #include <webots/robot.h>
#include <webots/supervisor.h> #include <webots/supervisor.h>
#include <webots/emitter.h> #include <webots/emitter.h>
#include <webots/receiver.h>
#include "ardupilot_SITL_TRICOPTER.h" #include "ardupilot_SITL_TRICOPTER.h"
#include "sockets.h" #include "sockets.h"
#include "sensors.h" #include "sensors.h"
#define MOTOR_NUM 3 #define MOTOR_NUM 3
static WbDeviceTag motors[MOTOR_NUM]; static WbDeviceTag motors[MOTOR_NUM];
@ -34,23 +53,22 @@ static WbDeviceTag compass;
static WbDeviceTag gps; static WbDeviceTag gps;
static WbDeviceTag camera; static WbDeviceTag camera;
static WbDeviceTag inertialUnit; static WbDeviceTag inertialUnit;
static WbDeviceTag emitter; static WbDeviceTag receiver;
static WbNodeRef world_info;
static const double *northDirection; #ifdef WIND_SIMULATION
static WbDeviceTag emitter;
#endif
static double _linear_velocity[3] = {0.0,0.0,0.0};
static double northDirection = 1;
static double v[MOTOR_NUM]; static double v[MOTOR_NUM];
static double servo_value = 0; static double servo_value = 0;
#ifdef DEBUG_USE_KB
static double servo_value_extra = 0; static double servo_value_extra = 0;
#endif
int port; int port;
int timestep;
// delatTime limits the fasts execution path. it is very useful in SLOW MOTION.
// Increasing simulation speed using ">>" button on webots may not be effective
// if this value > 0.
int maxSimCycleTime = 0; // no delay
static int timestep;
#ifdef DEBUG_USE_KB #ifdef DEBUG_USE_KB
@ -135,8 +153,22 @@ void process_keyboard ()
} }
#endif #endif
// Read all messages and empty the Q and keep last value as the valid one.
static void read_incoming_messages()
{
// read while queue not empty
while (wb_receiver_get_queue_length(receiver) > 0) {
// I'm only expecting ascii messages
double * data = (double *) wb_receiver_get_data(receiver);
_linear_velocity[0] = data[0];
_linear_velocity[1] = data[1];
_linear_velocity[2] = data[2];
northDirection = data[3];
//printf("RAW Data [%f, %f, %f]\n", linear_velocity[0], linear_velocity[2], linear_velocity[1]);
wb_receiver_next_packet(receiver);
}
}
/* /*
// apply motor thrust. // apply motor thrust.
@ -196,7 +228,7 @@ void update_controls()
double linear_speed = sqrt(linear_velocity[0] * linear_velocity[0] + linear_velocity[1] * linear_velocity[1] + linear_velocity[2] * linear_velocity[2]); double linear_speed = sqrt(linear_velocity[0] * linear_velocity[0] + linear_velocity[1] * linear_velocity[1] + linear_velocity[2] * linear_velocity[2]);
wind_webots_axis.w = state.wind.w + 0.01 * linear_speed * linear_speed; wind_webots_axis.w = state.wind.w + 0.01 * linear_speed * linear_speed;
if (northDirection[0] == 1) if (northDirection == 1)
{ {
wind_webots_axis.x = state.wind.x - linear_velocity[0]; wind_webots_axis.x = state.wind.x - linear_velocity[0];
wind_webots_axis.z = -state.wind.y - linear_velocity[2]; // "-state.wind.y" as angle 90 wind is from EAST. wind_webots_axis.z = -state.wind.y - linear_velocity[2]; // "-state.wind.y" as angle 90 wind is from EAST.
@ -219,7 +251,6 @@ void update_controls()
// the JSON parser is directly inspired by https://github.com/ArduPilot/ardupilot/blob/master/libraries/SITL/SIM_Morse.cpp // 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) bool parse_controls(const char *json)
{ {
//state.timestamp = 1.0;
#ifdef DEBUG_INPUT_DATA #ifdef DEBUG_INPUT_DATA
printf("%s\n", json); printf("%s\n", json);
#endif #endif
@ -227,7 +258,6 @@ bool parse_controls(const char *json)
for (uint16_t i=0; i < ARRAY_SIZE(keytable); i++) { for (uint16_t i=0; i < ARRAY_SIZE(keytable); i++) {
struct keytable *key; struct keytable *key;
key = &keytable[i]; key = &keytable[i];
//printf("search %s/%s\n", key->section, key->key);
// look for section header // look for section header
const char *p = strstr(json, key->section); const char *p = strstr(json, key->section);
if (!p) { if (!p) {
@ -239,7 +269,7 @@ bool parse_controls(const char *json)
// find key inside section // find key inside section
p = strstr(p, key->key); p = strstr(p, key->key);
if (!p) { if (!p) {
printf("Failed to find key %s/%s\n", key->section, key->key); printf("Failed to find key %s/%s DATA:%s\n", key->section, key->key, json);
return false; return false;
} }
@ -281,20 +311,19 @@ bool parse_controls(const char *json)
return true; return true;
} }
#define TIME_DIV 1000.0
void run () void run ()
{ {
char send_buf[1000]; //1000 just a safe margin char send_buf[1000];
char command_buffer[200]; char command_buffer[1000];
fd_set rfds; fd_set rfds;
while (wb_robot_step(timestep) != -1)
{
for (int i=0;i<maxSimCycleTime;++i)
{
usleep(1000);
}
// calculate initial sensor values.
wb_robot_step(timestep);
while (true)
{
#ifdef DEBUG_USE_KB #ifdef DEBUG_USE_KB
process_keyboard(); process_keyboard();
#endif #endif
@ -303,21 +332,26 @@ void run ()
{ {
// if no socket wait till you get a socket // if no socket wait till you get a socket
fd = socket_accept(sfd); fd = socket_accept(sfd);
if (fd > 0) if (fd < 0)
socket_set_non_blocking(fd);
else if (fd < 0)
break; break;
} }
read_incoming_messages();
// trigget ArduPilot to send motor data
getAllSensors ((char *)send_buf, northDirection, gyro,accelerometer,compass,gps, inertialUnit); getAllSensors ((char *)send_buf, northDirection, gyro,accelerometer,compass,gps, inertialUnit);
#ifdef DEBUG_SENSORS #ifdef DEBUG_SENSORS
printf("%s\n",send_buf); printf("at %lf %s\n",wb_robot_get_time(), send_buf);
#endif #endif
if (write(fd,send_buf,strlen(send_buf)) <= 0) if (write(fd,send_buf,strlen(send_buf)) <= 0)
{ {
printf ("Send Data Error\n"); fprintf (stderr,"Send Data Error\n");
} }
if (fd) if (fd)
@ -331,8 +365,8 @@ void run ()
if (number != 0) if (number != 0)
{ {
// there is a valid connection // there is a valid connection
int n = recv(fd, (char *)command_buffer, 1000, 0);
int n = recv(fd, (char *)command_buffer, 200, 0);
if (n < 0) { if (n < 0) {
#ifdef _WIN32 #ifdef _WIN32
int e = WSAGetLastError(); int e = WSAGetLastError();
@ -352,16 +386,16 @@ void run ()
{ {
break; break;
} }
if (command_buffer[0] == 'e')
{
break;
}
if (n > 0) if (n > 0)
{ {
command_buffer[n] = 0; command_buffer[n] = 0;
parse_controls (command_buffer); if (parse_controls (command_buffer))
{
update_controls(); update_controls();
//https://cyberbotics.com/doc/reference/robot#wb_robot_step
// this is used to force webots not to execute untill it receives feedback from simulator.
wb_robot_step(timestep);
}
} }
} }
@ -373,7 +407,7 @@ void run ()
} }
void initialize (int argc, char *argv[]) bool initialize (int argc, char *argv[])
{ {
fd_set rfds; fd_set rfds;
@ -386,64 +420,51 @@ void initialize (int argc, char *argv[])
if (argc > i+1 ) if (argc > i+1 )
{ {
port = atoi (argv[i+1]); port = atoi (argv[i+1]);
printf("socket port %d\n",port);
} }
} }
else if (strcmp (argv[i],"-d")==0) else if (strcmp (argv[i],"-df")==0)
{ {
if (argc > i+1 ) //TODO: to be implemented later. use same logic as in Quad file.
{
// extra delay in milliseconds
maxSimCycleTime = atoi (argv[i+1]);
printf("max simulation cycle time is %d ms\n",maxSimCycleTime);
}
}
}
}
}
sfd = create_socket_server(port); sfd = create_socket_server(port);
/* necessary to initialize webots stuff */ /* necessary to initialize webots stuff */
wb_robot_init(); wb_robot_init();
// Get WorldInfo Node.
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]);
// get Self Node
self_node = wb_supervisor_node_get_self();
// keybaard
timestep = (int)wb_robot_get_basic_time_step(); timestep = (int)wb_robot_get_basic_time_step();
// init receiver from Supervisor
receiver = wb_robot_get_device("receiver_main");
if (receiver ==0)
{
fprintf(stderr,"Receiver not found\n");
fprintf(stderr,"EXIT with error\n");
return false;
}
// read robot number and set receiver channel accordingly
const char * customData = wb_robot_get_custom_data();
if (customData != NULL)
{
int channel = atoi(customData);
wb_receiver_set_channel(receiver,channel);
wb_receiver_enable(receiver,timestep);
printf("Receiver Channel at %d \n", channel);
}
else
{
fprintf(stderr, "MISSING Channel NO. in Custom Data");
return false;
}
#ifdef DEBUG_USE_KB
wb_keyboard_enable(timestep); wb_keyboard_enable(timestep);
#endif
// inertialUnit // inertialUnit
inertialUnit = wb_robot_get_device("inertial_unit"); inertialUnit = wb_robot_get_device("inertial_unit");
@ -467,26 +488,32 @@ void initialize (int argc, char *argv[])
// camera // camera
camera = wb_robot_get_device("camera1"); camera = wb_robot_get_device("camera1");
wb_camera_enable(camera, timestep); wb_camera_enable(camera, CAMERA_FRAME_RATE_FACTOR * timestep);
#ifdef WIND_SIMULATION #ifdef WIND_SIMULATION
// emitter // emitter
emitter = wb_robot_get_device("emitter_plugin"); emitter = wb_robot_get_device("emitter_plugin");
#endif #endif
// names of motor should be the same as name of motor in the robot.
const char *MOTOR_NAMES[] = {"motor1", "motor2", "motor3"}; const char *MOTOR_NAMES[] = {"motor1", "motor2", "motor3"};
// get motor device tags // get motor device tags
for (i = 0; i < MOTOR_NUM; i++) { for (int i = 0; i < MOTOR_NUM; i++) {
motors[i] = wb_robot_get_device(MOTOR_NAMES[i]); motors[i] = wb_robot_get_device(MOTOR_NAMES[i]);
v[i] = 0.0f; v[i] = 0.0f;
//assert(motors[i]);
} }
// tricopter servo name
servo = wb_robot_get_device("servo_tail"); servo = wb_robot_get_device("servo_tail");
FD_ZERO(&rfds); FD_ZERO(&rfds);
FD_SET(sfd, &rfds); FD_SET(sfd, &rfds);
// init linear_velocity untill we receive valid data from Supervisor.
linear_velocity = &_linear_velocity[0] ;
return true;
} }
/* /*
* This is the main program. * This is the main program.
@ -496,36 +523,15 @@ void initialize (int argc, char *argv[])
int main(int argc, char **argv) int main(int argc, char **argv)
{ {
if (initialize( argc, 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: * Enter here functions to send actuator commands, like:
* wb_differential_wheels_set_speed(100.0,100.0); * wb_differential_wheels_set_speed(100.0,100.0);
*/ */
run(); run();
}
/* Enter your cleanup code here */ /* Enter your cleanup code here */

9
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/ardupilot_SITL_TRICOPTER.h
View File

@ -3,9 +3,16 @@
//#define DEBUG_USE_KB //#define DEBUG_USE_KB
//#define DEBUG_INPUT_DATA //#define DEBUG_INPUT_DATA
// #define LINEAR_THRUST // #define LINEAR_THRUST
//#define WIND_SIMULATION //#define WIND_SIMULATION
// # of simulation steps between two image frames.
#define CAMERA_FRAME_RATE_FACTOR 50
#define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0])) #define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0]))
@ -57,5 +64,5 @@ struct keytable {
w: wind speed w: wind speed
x , y, z: wind direction. x , y, z: wind direction.
*/ */
VECTOR4F __attribute__((packed, aligned(1))) wind_webots_axis; VECTOR4F wind_webots_axis;

33
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/sensors.c
View File

@ -33,10 +33,10 @@ copter.rotate_body_frame_to_NE(vel_vector.x, vel_vector.y);
/* /*
returns: "yaw":_6.594794831471518e-05,"pitch":_-0.0005172680830582976,"roll":_0.022908752784132957}} returns: "yaw":_6.594794831471518e-05,"pitch":_-0.0005172680830582976,"roll":_0.022908752784132957}}
*/ */
void getInertia (const WbDeviceTag inertialUnit, const double *northDirection, char *buf) void getInertia (const WbDeviceTag inertialUnit, const double northDirection, char *buf)
{ {
const double *inertial_directions = wb_inertial_unit_get_roll_pitch_yaw (inertialUnit); const double *inertial_directions = wb_inertial_unit_get_roll_pitch_yaw (inertialUnit);
if (northDirection[0] == 1) if (northDirection == 1)
{ {
sprintf(buf,"\"roll\": %f,\"pitch\": %f,\"yaw\": %f",inertial_directions[0], inertial_directions[1], inertial_directions[2]); sprintf(buf,"\"roll\": %f,\"pitch\": %f,\"yaw\": %f",inertial_directions[0], inertial_directions[1], inertial_directions[2]);
} }
@ -51,10 +51,10 @@ void getInertia (const WbDeviceTag inertialUnit, const double *northDirection, c
/* /*
returns: "magnetic_field":_[23088.669921875,_3876.001220703125,_-53204.57421875] returns: "magnetic_field":_[23088.669921875,_3876.001220703125,_-53204.57421875]
*/ */
void getCompass (const WbDeviceTag compass, const double *northDirection, char *buf) void getCompass (const WbDeviceTag compass, const double northDirection, char *buf)
{ {
const double *north3D = wb_compass_get_values(compass); const double *north3D = wb_compass_get_values(compass);
if (northDirection[0] == 1) if (northDirection == 1)
{ {
sprintf(buf,"[%f, %f, %f]",north3D[0], north3D[2], north3D[1]); sprintf(buf,"[%f, %f, %f]",north3D[0], north3D[2], north3D[1]);
} }
@ -71,11 +71,11 @@ void getCompass (const WbDeviceTag compass, const double *northDirection, char *
/* /*
returns: "vehicle.gps":{"timestamp":_1563301031.055164,"x":_5.5127296946011484e-05,"y":_-0.0010968948481604457,"z":_0.037179552018642426}, 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) void getGPS (const WbDeviceTag gps, const double northDirection, char *buf)
{ {
const double *north3D = wb_gps_get_values(gps); const double *north3D = wb_gps_get_values(gps);
if (northDirection[0] == 1) if (northDirection == 1)
{ {
sprintf(buf,"\"x\": %f,\"y\": %f,\"z\": %f", north3D[0], north3D[2], north3D[1]); sprintf(buf,"\"x\": %f,\"y\": %f,\"z\": %f", north3D[0], north3D[2], north3D[1]);
} }
@ -91,11 +91,11 @@ void getGPS (const WbDeviceTag gps, const double *northDirection, char *buf)
/* /*
returns: "linear_acceleration": [0.005074390675872564, 0.22471477091312408, 9.80740737915039] returns: "linear_acceleration": [0.005074390675872564, 0.22471477091312408, 9.80740737915039]
*/ */
void getAcc (const WbDeviceTag accelerometer, const double *northDirection, char *buf) void getAcc (const WbDeviceTag accelerometer, const double northDirection, char *buf)
{ {
//SHOULD BE CORRECT //SHOULD BE CORRECT
const double *a = wb_accelerometer_get_values(accelerometer); const double *a = wb_accelerometer_get_values(accelerometer);
if (northDirection[0] == 1) if (northDirection == 1)
{ {
sprintf(buf,"[%f, %f, %f]",a[0], a[2], a[1]); sprintf(buf,"[%f, %f, %f]",a[0], a[2], a[1]);
} }
@ -114,11 +114,11 @@ void getAcc (const WbDeviceTag accelerometer, const double *northDirection, char
/* /*
returns: "angular_velocity": [-1.0255117643964695e-07, -8.877226775894087e-08, 2.087078510015772e-09] returns: "angular_velocity": [-1.0255117643964695e-07, -8.877226775894087e-08, 2.087078510015772e-09]
*/ */
void getGyro (const WbDeviceTag gyro, const double *northDirection, char *buf) void getGyro (const WbDeviceTag gyro, const double northDirection, char *buf)
{ {
const double *g = wb_gyro_get_values(gyro); const double *g = wb_gyro_get_values(gyro);
if (northDirection[0] == 1) if (northDirection == 1)
{ {
sprintf(buf,"[%f, %f, %f]",g[0], g[2], g[1]); sprintf(buf,"[%f, %f, %f]",g[0], g[2], g[1]);
} }
@ -131,24 +131,23 @@ void getGyro (const WbDeviceTag gyro, const double *northDirection, char *buf)
} }
void getLinearVelocity (WbNodeRef nodeRef, const double *northDirection, char * buf) void getLinearVelocity (WbNodeRef nodeRef, const double northDirection, char * buf)
{ {
linear_velocity = wb_supervisor_node_get_velocity (nodeRef);
if (linear_velocity != NULL) if (linear_velocity != NULL)
{ {
if (northDirection[0] == 1) if (northDirection == 1)
{ { // local map northDirection [1,0,0]
sprintf (buf,"[%f, %f, %f]", linear_velocity[0], linear_velocity[2], linear_velocity[1]); sprintf (buf,"[%f, %f, %f]", linear_velocity[0], linear_velocity[2], linear_velocity[1]);
} }
else else
{ { // openstreet map northDirection [0,0,1]
sprintf (buf,"[%f, %f, %f]", linear_velocity[2], -linear_velocity[0], linear_velocity[1]); sprintf (buf,"[%f, %f, %f]", linear_velocity[2], -linear_velocity[0], linear_velocity[1]);
} }
} }
} }
void getAllSensors (char *buf, const double *northDirection, WbDeviceTag gyro, WbDeviceTag accelerometer, WbDeviceTag compass, const WbDeviceTag gps, WbDeviceTag inertial_unit) void getAllSensors (char *buf, const double northDirection, WbDeviceTag gyro, WbDeviceTag accelerometer, WbDeviceTag compass, const WbDeviceTag gps, const WbDeviceTag inertial_unit)
{ {
/* /*
@ -179,7 +178,7 @@ void getAllSensors (char *buf, const double *northDirection, WbDeviceTag gyro, W
char szTime[21]; char szTime[21];
double time = wb_robot_get_time(); // current simulation time in [s] double time = wb_robot_get_time(); // current simulation time in [s]
sprintf(szTime,"%f", time); sprintf(szTime,"%lf", time);
getGyro(gyro, northDirection, gyro_buf); getGyro(gyro, northDirection, gyro_buf);
getAcc(accelerometer, northDirection, acc_buf); getAcc(accelerometer, northDirection, acc_buf);

14
libraries/SITL/examples/Webots/controllers/ardupilot_SITL_TRICOPTER/sensors.h
View File

@ -14,10 +14,10 @@
WbNodeRef self_node; WbNodeRef self_node;
double *linear_velocity; double *linear_velocity;
void getInertia (const WbDeviceTag inertialUnit, const double *northDirection, char *buf); void getInertia (const WbDeviceTag inertialUnit, const double northDirection, char *buf);
void getLinearVelocity (WbNodeRef nodeRef, 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 getCompass (const WbDeviceTag compass, const double northDirection, char *buf);
void getAcc (const WbDeviceTag accelerometer, 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 getGyro (const WbDeviceTag gyro, const double northDirection, char *buf);
void getGPS (const WbDeviceTag gps, 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); void getAllSensors (char *buf, const double northDirection, WbDeviceTag gyro, WbDeviceTag accelerometer, WbDeviceTag compass, const WbDeviceTag gps, const WbDeviceTag inertial_unit);

11
libraries/SITL/examples/Webots/droneTwoTricopters.sh Executable file
View File

@ -0,0 +1,11 @@
#!/bin/bash
# assume we start the script from the root directory
ROOTDIR=$PWD
xterm -title "TriCopter 1" -e "$PWD/Tools/autotest/sim_vehicle.py -v ArduCopter -w --instance 10 --out=udpout:127.0.0.1:14450 --model webots-tri:127.0.0.1:5599 --add-param-file=libraries/SITL/examples/Webots/tricopter.parm " &
xterm -title "TriCopter 2" -e "$PWD/Tools/autotest/sim_vehicle.py -v ArduCopter -w --instance 20 --out=udpout:127.0.0.1:14550 --model webots-tri:127.0.0.1:5598 --add-param-file=libraries/SITL/examples/Webots/tricopter2.parm " &

2
libraries/SITL/examples/Webots/droneX.sh
View File

@ -2,4 +2,4 @@
# assume we start the script from the root directory # assume we start the script from the root directory
ROOTDIR=$PWD ROOTDIR=$PWD
$PWD/Tools/autotest/sim_vehicle.py -v ArduCopter -w --model webots-quad --add-param-file=libraries/SITL/examples/Webots/quadX.parm $PWD/Tools/autotest/sim_vehicle.py -v ArduCopter -w --model webots-quad:127.0.0.1:5577 --add-param-file=libraries/SITL/examples/Webots/quadX.parm

6
libraries/SITL/examples/Webots/droneX_TwoDrones.sh Executable file
View File

@ -0,0 +1,6 @@
#!/bin/bash
# assume we start the script from the root directory
ROOTDIR=$PWD
xterm -title "QuadX 1" -e "$PWD/Tools/autotest/sim_vehicle.py -v ArduCopter -w --instance 10 --out=udpout:127.0.0.1:14450 --model webots-quad:127.0.0.1:5599 --add-param-file=libraries/SITL/examples/Webots/quadX.parm " &
xterm -title "QuadX 2" -e "$PWD/Tools/autotest/sim_vehicle.py -v ArduCopter -w --instance 20 --out=udpout:127.0.0.1:14550 --model webots-quad:127.0.0.1:5598 --add-param-file=libraries/SITL/examples/Webots/quadX2.parm " &

218
libraries/SITL/examples/Webots/mission_pyramids.plan Normal file
View File

@ -0,0 +1,218 @@
{
"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,
50
],
"type": "SimpleItem"
},
{
"AMSLAltAboveTerrain": null,
"Altitude": 50,
"AltitudeMode": 1,
"autoContinue": true,
"command": 16,
"doJumpId": 2,
"frame": 3,
"params": [
0,
0,
0,
null,
29.97690345,
31.13300264,
50
],
"type": "SimpleItem"
},
{
"AMSLAltAboveTerrain": null,
"Altitude": 50,
"AltitudeMode": 1,
"autoContinue": true,
"command": 16,
"doJumpId": 3,
"frame": 3,
"params": [
0,
0,
0,
null,
29.97715437,
31.13230527,
50
],
"type": "SimpleItem"
},
{
"AMSLAltAboveTerrain": null,
"Altitude": 50,
"AltitudeMode": 1,
"autoContinue": true,
"command": 16,
"doJumpId": 4,
"frame": 3,
"params": [
0,
0,
0,
null,
29.97829748,
31.13269151,
50
],
"type": "SimpleItem"
},
{
"AMSLAltAboveTerrain": null,
"Altitude": 50,
"AltitudeMode": 1,
"autoContinue": true,
"command": 16,
"doJumpId": 5,
"frame": 3,
"params": [
0,
0,
0,
null,
29.97797221,
31.13128603,
50
],
"type": "SimpleItem"
},
{
"AMSLAltAboveTerrain": null,
"Altitude": 50,
"AltitudeMode": 1,
"autoContinue": true,
"command": 16,
"doJumpId": 6,
"frame": 3,
"params": [
0,
0,
0,
null,
29.97728448,
31.13107145,
50
],
"type": "SimpleItem"
},
{
"AMSLAltAboveTerrain": null,
"Altitude": 50,
"AltitudeMode": 1,
"autoContinue": true,
"command": 16,
"doJumpId": 7,
"frame": 3,
"params": [
0,
0,
0,
null,
29.97694991386429,
31.13263786259614,
50
],
"type": "SimpleItem"
},
{
"AMSLAltAboveTerrain": null,
"Altitude": 50,
"AltitudeMode": 1,
"autoContinue": true,
"command": 16,
"doJumpId": 8,
"frame": 3,
"params": [
0,
0,
0,
null,
29.97597408724931,
31.132434016983865,
50
],
"type": "SimpleItem"
},
{
"AMSLAltAboveTerrain": null,
"Altitude": 50,
"AltitudeMode": 1,
"autoContinue": true,
"command": 16,
"doJumpId": 9,
"frame": 3,
"params": [
0,
0,
0,
null,
29.97537928,
31.13303483,
50
],
"type": "SimpleItem"
},
{
"AMSLAltAboveTerrain": null,
"Altitude": 50,
"AltitudeMode": 1,
"autoContinue": true,
"command": 16,
"doJumpId": 10,
"frame": 3,
"params": [
0,
0,
0,
null,
29.97716367,
31.13395751,
50
],
"type": "SimpleItem"
}
],
"plannedHomePosition": [
29.976462,
31.1338234,
50
],
"vehicleType": 2,
"version": 2
},
"rallyPoints": {
"points": [
],
"version": 2
},
"version": 1
}

6
libraries/SITL/examples/Webots/pyramids_dronePlus_twoDrones.sh Executable file
View File

@ -0,0 +1,6 @@
#!/bin/bash
# assume we start the script from the root directory
ROOTDIR=$PWD
xterm -title "Quad 1" -e "$PWD/Tools/autotest/sim_vehicle.py -v ArduCopter -w --instance 10 --out=udpout:127.0.0.1:14450 --model webots-quad:127.0.0.1:5599 --add-param-file=libraries/SITL/examples/Webots/quadX.parm -L Pyramid " &
xterm -title "Quad 2" -e "$PWD/Tools/autotest/sim_vehicle.py -v ArduCopter -w --instance 20 --out=udpout:127.0.0.1:14550 --model webots-quad:127.0.0.1:5598 --add-param-file=libraries/SITL/examples/Webots/quadX2.parm -L Pyramid " &

1
libraries/SITL/examples/Webots/quadPlus.parm
View File

@ -1,3 +1,4 @@
SYSID_THISMAV 1
FRAME_CLASS 1.000000 FRAME_CLASS 1.000000
FRAME_TYPE 0.000000 FRAME_TYPE 0.000000
ATC_ANG_PIT_P 1.0 ATC_ANG_PIT_P 1.0

32
libraries/SITL/examples/Webots/quadPlus2.parm Normal file
View File

@ -0,0 +1,32 @@
SYSID_THISMAV 2
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
WPNAV_SPEED 800
SIM_WIND_DIR 90.0
SIM_WIND_SPD 6.0
SIM_WIND_TURB 0.0

1
libraries/SITL/examples/Webots/quadX.parm
View File

@ -1,3 +1,4 @@
SYSID_THISMAV 1
FRAME_CLASS 1.000000 FRAME_CLASS 1.000000
FRAME_TYPE 1.000000 FRAME_TYPE 1.000000
ATC_ANG_PIT_P 1.0 ATC_ANG_PIT_P 1.0

31
libraries/SITL/examples/Webots/quadX2.parm Normal file
View File

@ -0,0 +1,31 @@
SYSID_THISMAV 2
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
SIM_WIND_DIR 90.0
SIM_WIND_SPD 5.0
SIM_WIND_TURB 0.0

11
libraries/SITL/examples/Webots/tricopter.parm
View File

@ -1,12 +1,13 @@
SYSID_THISMAV 1
FRAME_CLASS 7.000000 FRAME_CLASS 7.000000
FRAME_TYPE 0.000000 FRAME_TYPE 0.000000
ATC_ANG_PIT_P 1.0 ATC_ANG_PIT_P 1.0
ATC_ANG_RLL_P 1.0 ATC_ANG_RLL_P 1.0
ATC_ANG_YAW_P 0.5 ATC_ANG_YAW_P 0.50
ATC_RAT_PIT_P 3.5 ATC_RAT_PIT_P 3.6
ATC_RAT_RLL_P 3.5 ATC_RAT_RLL_P 3.6
ATC_RAT_YAW_P 1.5 ATC_RAT_YAW_P 1.9
ATC_RAT_YAW_I 0.03 ATC_RAT_YAW_I 0.05
ATC_RAT_YAW_IMAX 0.50000 ATC_RAT_YAW_IMAX 0.50000
SIM_WIND_DIR 90.0 SIM_WIND_DIR 90.0
SIM_WIND_SPD 0.0 SIM_WIND_SPD 0.0

14
libraries/SITL/examples/Webots/tricopter2.parm Normal file
View File

@ -0,0 +1,14 @@
SYSID_THISMAV 2
FRAME_CLASS 7.000000
FRAME_TYPE 0.000000
ATC_ANG_PIT_P 1.0
ATC_ANG_RLL_P 1.0
ATC_ANG_YAW_P 0.5
ATC_RAT_PIT_P 3.6
ATC_RAT_RLL_P 3.6
ATC_RAT_YAW_P 1.9
ATC_RAT_YAW_I 0.05
ATC_RAT_YAW_IMAX 0.50000
SIM_WIND_DIR 90.0
SIM_WIND_SPD 0.0
SIM_WIND_TURB 0.0

35
libraries/SITL/examples/Webots/worlds/pyramidMapReduced2.wbt
View File

@ -1,4 +1,4 @@
#VRML_SIM R2019b utf8 #VRML_SIM R2020a utf8
WorldInfo { WorldInfo {
info [ info [
"World generated using the Open Street Map to Webots importer" "World generated using the Open Street Map to Webots importer"
@ -12,12 +12,30 @@ WorldInfo {
northDirection 0 0 1 northDirection 0 0 1
randomSeed 52 randomSeed 52
} }
Robot {
translation -1.949999999999999 0.23391237384410402 0
rotation 0 0 1 2.533555103014215e-15
children [
Emitter {
type "serial"
channel 1
}
]
name "supervisor_sync"
boundingObject Box {
size 0.1 0.1 0.1
}
physics Physics {
}
controller "ardupilot_SITL_Supervisor"
supervisor TRUE
}
Viewpoint { Viewpoint {
orientation -0.008430334675332775 -0.9889506387233125 -0.1480052824260481 2.792301901785628 orientation -0.008430334675332775 -0.9889506387233125 -0.1480052824260481 2.792301901785628
position -44.85708948922592 41.47847179839409 -120.78477309902298 position -2.7392169823215062 2.3278150473447536 -7.277902320169236
near 3 near 3
follow "quad_plus_sitl" follow "quad_plus_sitl"
followOrientation TRUE followType "Mounted Shot"
} }
TexturedBackground { TexturedBackground {
} }
@ -36,8 +54,15 @@ Floor {
} }
} }
DEF DEF_VEHICLE Robot { DEF DEF_VEHICLE Robot {
translation 7.15062e-12 0.06 1.08491e-11
rotation -1.6967771456756733e-10 1 2.4151813868919206e-10 -0.7512383045779282 rotation -1.6967771456756733e-10 1 2.4151813868919206e-10 -0.7512383045779282
children [ children [
Receiver {
name "receiver_main"
type "serial"
channel 1
bufferSize 32
}
Emitter { Emitter {
name "emitter_plugin" name "emitter_plugin"
description "commuicates with physics plugin" description "commuicates with physics plugin"
@ -348,8 +373,8 @@ DEF DEF_VEHICLE Robot {
} }
rotationStep 0.261799 rotationStep 0.261799
controller "ardupilot_SITL_QUAD" controller "ardupilot_SITL_QUAD"
controllerArgs "-p 5599 -df 0.02" controllerArgs "-p 5599 -df 0.01"
supervisor TRUE customData "1"
} }
Road { Road {
translation -972.826351 0.01 137.636697 translation -972.826351 0.01 137.636697

7043
libraries/SITL/examples/Webots/worlds/pyramidMapReduced2_two_quads.wbt Normal file
View File

File diff suppressed because it is too large Load Diff

38
libraries/SITL/examples/Webots/worlds/webots_quadPlus.wbt
View File

@ -1,17 +1,35 @@
#VRML_SIM R2019b utf8 #VRML_SIM R2020a utf8
WorldInfo { WorldInfo {
gravity 0 -9.80665 0 gravity 0 -9.80665 0
physics "sitl_physics_env" physics "sitl_physics_env"
basicTimeStep 2 basicTimeStep 1
FPS 15 FPS 15
optimalThreadCount 4 optimalThreadCount 4
randomSeed 52 randomSeed 52
} }
Robot {
translation -1.949999999999999 0.23391237384410402 0
rotation 0 0 1 2.533555103014215e-15
children [
Emitter {
type "serial"
channel 1
}
]
name "supervisor_sync"
boundingObject Box {
size 0.1 0.1 0.1
}
physics Physics {
}
controller "ardupilot_SITL_Supervisor"
supervisor TRUE
}
Viewpoint { Viewpoint {
orientation 0.5542589845891367 0.8247263718920818 0.11235385844706426 5.801159266912386 orientation 0.9997978097496996 0.017446300161270606 0.009998311376764756 5.242214502651711
position -4.295082945651872 3.093288102690911 9.827811912728198 position -0.10961792118984595 8.021071573852062 4.261928842800047
follow "quad_plus_sitl" follow "quad_plus_sitl"
followOrientation TRUE followType "Mounted Shot"
} }
DogHouse { DogHouse {
translation 34.82 0.76 -24.56 translation 34.82 0.76 -24.56
@ -29,8 +47,6 @@ Background {
skyColor [ skyColor [
0.15 0.5 1 0.15 0.5 1
] ]
cubemap Cubemap {
}
} }
Solid { Solid {
translation 36.93 0.77 -37.93 translation 36.93 0.77 -37.93
@ -52,6 +68,12 @@ DEF DEF_VEHICLE Robot {
translation -0.027601 0.674307 0.005031 translation -0.027601 0.674307 0.005031
rotation -0.012461000916064287 0.999885073506054 -0.008635000634797779 -0.22761130717958622 rotation -0.012461000916064287 0.999885073506054 -0.008635000634797779 -0.22761130717958622
children [ children [
Receiver {
name "receiver_main"
type "serial"
channel 1
bufferSize 32
}
Emitter { Emitter {
name "emitter_plugin" name "emitter_plugin"
description "commuicates with physics plugin" description "commuicates with physics plugin"
@ -362,7 +384,7 @@ DEF DEF_VEHICLE Robot {
rotationStep 0.261799 rotationStep 0.261799
controller "ardupilot_SITL_QUAD" controller "ardupilot_SITL_QUAD"
controllerArgs "-p 5599 -df 0.01" controllerArgs "-p 5599 -df 0.01"
supervisor TRUE customData "1"
} }
DirectionalLight { DirectionalLight {
direction 0 -1 0 direction 0 -1 0

50
libraries/SITL/examples/Webots/worlds/webots_quadX.wbt
View File

@ -1,12 +1,36 @@
#VRML_SIM R2019b utf8 #VRML_SIM R2020a utf8
WorldInfo { WorldInfo {
gravity 0 -9.80665 0 gravity 0 -9.80665 0
physics "sitl_physics_env" physics "sitl_physics_env"
basicTimeStep 2 basicTimeStep 2
FPS 25 FPS 15
optimalThreadCount 4 optimalThreadCount 4
randomSeed 52 randomSeed 52
} }
Robot {
translation -1.949999999999999 0.23391237384410402 0
rotation 0 0 1 2.533555103014215e-15
children [
Emitter {
type "serial"
channel 1
}
]
name "supervisor_sync"
boundingObject Box {
size 0.1 0.1 0.1
}
physics Physics {
}
controller "ardupilot_SITL_Supervisor"
supervisor TRUE
}
Viewpoint {
orientation -0.9919331177114086 -0.11439530181889061 -0.054611399076472875 0.8971103745745552
position -0.9553539884549295 11.277792894156029 8.463112348647094
follow "quad_plus_sitl"
followType "Mounted Shot"
}
DogHouse { DogHouse {
translation 34.82 0.76 -24.56 translation 34.82 0.76 -24.56
name "dog house(1)" name "dog house(1)"
@ -19,18 +43,10 @@ DogHouse {
translation 185.42 0.77 48.97 translation 185.42 0.77 48.97
name "dog house(5)" 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 { Background {
skyColor [ skyColor [
0.15 0.5 1 0.15 0.5 1
] ]
cubemap Cubemap {
}
} }
Solid { Solid {
translation 36.93 0.77 -37.93 translation 36.93 0.77 -37.93
@ -49,9 +65,15 @@ Solid {
name "solid(1)" name "solid(1)"
} }
DEF DEF_VEHICLE Robot { DEF DEF_VEHICLE Robot {
translation -0.027601 0.674307 0.005031 translation -0.027601 0.694307 0.005031
rotation 0 1 0 0.785388 rotation 0 1 0 0.785388
children [ children [
Receiver {
name "receiver_main"
type "serial"
channel 1
bufferSize 32
}
Emitter { Emitter {
name "emitter_plugin" name "emitter_plugin"
description "commuicates with physics plugin" description "commuicates with physics plugin"
@ -216,7 +238,7 @@ DEF DEF_VEHICLE Robot {
Shape { Shape {
appearance Appearance { appearance Appearance {
material Material { material Material {
diffuseColor 1 0.09999999999999999 0 diffuseColor 1 0.09 0
} }
} }
geometry USE DEF_ARM geometry USE DEF_ARM
@ -367,8 +389,8 @@ DEF DEF_VEHICLE Robot {
} }
rotationStep 0.261799 rotationStep 0.261799
controller "ardupilot_SITL_QUAD" controller "ardupilot_SITL_QUAD"
controllerArgs "-p 5599 -df 0.01" controllerArgs "-p 5577 -df 0.01"
supervisor TRUE customData "1"
} }
DirectionalLight { DirectionalLight {
direction 0 -1 0 direction 0 -1 0

44
libraries/SITL/examples/Webots/worlds/webots_tricopter.wbt
View File

@ -1,12 +1,29 @@
#VRML_SIM R2019b utf8 #VRML_SIM R2020a utf8
WorldInfo { WorldInfo {
gravity 0 -9.80665 0 gravity 0 -9.80665 0
physics "sitl_physics_env" physics "sitl_physics_env"
basicTimeStep 2 basicTimeStep 1
FPS 25 FPS 25
optimalThreadCount 4
randomSeed 52 randomSeed 52
} }
Robot {
translation -1.949999999999999 0.23391237384410402 0
rotation 0 0 1 2.533555103014215e-15
children [
Emitter {
type "serial"
channel 1
}
]
name "supervisor_sync"
boundingObject Box {
size 0.1 0.1 0.1
}
physics Physics {
}
controller "ardupilot_SITL_Supervisor"
supervisor TRUE
}
DogHouse { DogHouse {
translation 34.82 0.76 -24.56 translation 34.82 0.76 -24.56
name "dog house(1)" name "dog house(1)"
@ -20,16 +37,14 @@ DogHouse {
name "dog house(5)" name "dog house(5)"
} }
Viewpoint { Viewpoint {
orientation 0.9997750421775041 -0.014997750480821456 -0.01499775048082146 4.712163997257285 orientation 0.9952082156446058 0.09310572995296737 0.02986520656892867 5.659589519324221
position 0.17712971811531414 14.83048200793912 -1.4201693307676047 position -0.1971228135348432 5.281009887921962 4.9181113380378845
follow "tricopter" follow "tricopter"
} }
Background { Background {
skyColor [ skyColor [
0.15 0.5 1 0.15 0.5 1
] ]
cubemap Cubemap {
}
} }
Solid { Solid {
translation 36.93 0.77 -37.93 translation 36.93 0.77 -37.93
@ -51,6 +66,12 @@ DEF DEF_VEHICLE Robot {
translation -8.233889875751989e-05 0.666500515499142 -1.3598750857814472 translation -8.233889875751989e-05 0.666500515499142 -1.3598750857814472
rotation 0.00514799893982893 0.9999767940663002 0.004461999081102697 0.261804 rotation 0.00514799893982893 0.9999767940663002 0.004461999081102697 0.261804
children [ children [
Receiver {
name "receiver_main"
type "serial"
channel 1
bufferSize 32
}
Compass { Compass {
name "compass1" name "compass1"
} }
@ -75,8 +96,8 @@ DEF DEF_VEHICLE Robot {
} }
] ]
endPoint Solid { endPoint Solid {
translation -4.884982810326628e-15 -4.1022629410960365e-12 1.8091922030330322e-13 translation -4.8849852365954544e-15 -4.102262941093136e-12 1.8091922030330322e-13
rotation 2.3470124341273664e-11 1 -3.708275057969111e-10 1.5707963071795863 rotation 3.4266344293343444e-10 1 -6.900208306501498e-10 1.5707963071795863
children [ children [
Propeller { Propeller {
shaftAxis 0 1 0 shaftAxis 0 1 0
@ -255,7 +276,7 @@ DEF DEF_VEHICLE Robot {
size 0.1 0.1 0.1 size 0.1 0.1 0.1
} }
physics Physics { physics Physics {
mass 0.6 mass 1
} }
} }
] ]
@ -264,7 +285,8 @@ DEF DEF_VEHICLE Robot {
mass 0.001 mass 0.001
} }
controller "ardupilot_SITL_TRICOPTER" controller "ardupilot_SITL_TRICOPTER"
supervisor TRUE controllerArgs "-p 5599 -df 0.01"
customData "1"
} }
DirectionalLight { DirectionalLight {
direction 0 -1 0 direction 0 -1 0

744
libraries/SITL/examples/Webots/worlds/webots_two_quadX.wbt Normal file
View File

@ -0,0 +1,744 @@
#VRML_SIM R2020a utf8
WorldInfo {
gravity 0 -9.80665 0
physics "sitl_physics_env"
basicTimeStep 2
FPS 15
optimalThreadCount 4
randomSeed 52
}
Robot {
translation -1.9499999999999953 0.3722134041213816 0
rotation 0 0 1 1.4890101503307464e-14
children [
Emitter {
type "serial"
channel 2
}
]
name "supervisor_sync"
boundingObject Box {
size 0.1 0.1 0.1
}
physics Physics {
}
controller "ardupilot_SITL_Supervisor"
supervisor TRUE
linearVelocity 7.084819282124348e-13 24.393373207754877 0
angularVelocity 0 0 2.254085857556385e-12
}
Viewpoint {
orientation -0.9482622824196637 -0.31233593113705876 -0.05696411028881694 0.3800228985388573
position -1.546983649438513 5.685333881651476 12.687075631078367
follow "quad_plus_sitl"
followType "Mounted Shot"
}
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)"
}
Background {
skyColor [
0.15 0.5 1
]
}
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)"
}
DEF DEF_VEHICLE Robot {
translation -0.027601000000000004 0.6940716404 0.005030999999999995
rotation 0 1 0 0.785388
children [
Receiver {
name "receiver_main"
type "serial"
channel 1
bufferSize 32
}
Emitter {
name "emitter_plugin"
description "commuicates with physics plugin"
}
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.09 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_1"
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 5598 -df 0.01"
customData "1"
linearVelocity 0 -0.0588399 0
}
DEF DEF_VEHICLE2 Robot {
translation 1.58556 0.6941503202 1.61821
rotation 0 1 0 0.785388
children [
Receiver {
name "receiver_main"
type "serial"
channel 2
bufferSize 32
}
Emitter {
name "emitter_plugin"
description "commuicates with physics plugin"
}
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.09 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_2"
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 -df 0.01"
customData "2"
linearVelocity 0 -0.0392266 0
}
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)"
}

538
libraries/SITL/examples/Webots/worlds/webots_two_tricopter.wbt Normal file
View File

@ -0,0 +1,538 @@
#VRML_SIM R2020a utf8
WorldInfo {
gravity 0 -9.80665 0
physics "sitl_physics_env"
basicTimeStep 1
FPS 25
optimalThreadCount 4
randomSeed 52
}
DogHouse {
name "dog house(1)"
}
DogHouse {
name "dog house(2)"
}
DogHouse {
name "dog house(5)"
}
Viewpoint {
orientation 0.7086803181760043 0.6899712774114916 0.14734939082708026 5.697800962651686
position -9.937979108654101 14.58080553998484 26.61082096142779
}
Background {
skyColor [
0.15 0.5 1
]
}
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)"
}
DEF DEF_VEHICLE Robot {
translation -6.12062 0.6576871933499999 0.4069059999999998
rotation 0.005134141913449905 0.9999763391777008 0.0045783918111282325 0.26180410065967374
children [
Receiver {
name "receiver_main"
type "serial"
channel 1
bufferSize 32
}
Compass {
name "compass1"
}
Camera {
translation 0 0.25 0
name "camera1"
}
Transform {
translation -0.34 0 0
rotation 0 1 0 1.5707959999999999
children [
HingeJoint {
jointParameters HingeJointParameters {
position -9.387964099116676e-12
axis 0 0 1
}
device [
RotationalMotor {
name "servo_tail"
maxVelocity 50000
maxTorque 1000
}
]
endPoint Solid {
translation -4.884981346861648e-15 -4.102163053687445e-12 1.8030021919912542e-13
rotation 1.422280360462637e-09 1 -1.7696377693563738e-09 1.5707963071795863
children [
Propeller {
shaftAxis 0 1 0
thrustConstants 11.44 0
torqueConstants 1e-05 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 2.238367478129037
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 0 1 0.09999999999999999
}
}
geometry Cylinder {
height 0.002
radius 0.02
}
}
]
}
}
]
name "solid(1)"
boundingObject Box {
size 0.01 0.01 0.01
}
physics Physics {
mass 0.001
}
}
}
]
}
Transform {
translation 0.17 0 0.3
children [
Propeller {
shaftAxis 0 -1 0
thrustConstants -11.443 0
torqueConstants 1e-05 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 0.012993
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 0 0.1
}
}
geometry Cylinder {
height 0.002
radius 0.02
}
}
]
}
}
]
}
Transform {
translation 0.16 0 -0.3
children [
Propeller {
shaftAxis 0 1 0
thrustConstants 11.443 0
torqueConstants 1e-05 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 0.012993
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 0 0.1
}
}
geometry Cylinder {
height 0.002
radius 0.02
}
}
]
}
}
]
}
Emitter {
rotation 0 1 0 -1.5707963071795863
name "emitter_plugin"
}
InertialUnit {
name "inertial_unit"
}
Gyro {
name "gyro1"
}
Accelerometer {
name "accelerometer1"
}
GPS {
name "gps1"
}
Solid {
children [
Shape {
appearance Appearance {
material Material {
}
}
geometry Box {
size 0.1 0.1 0.1
}
}
]
boundingObject Box {
size 0.1 0.1 0.1
}
physics Physics {
mass 1
}
}
]
name "tricopter1"
physics Physics {
mass 0.001
}
controller "ardupilot_SITL_TRICOPTER"
controllerArgs "-p 5599 -df 0.01 "
customData "1"
supervisor TRUE
linearVelocity 2.688242491099282e-27 -0.009806650000000002 4.892319843481843e-25
angularVelocity -1.555631679291012e-22 -3.6929757828058823e-22 -5.825152319722367e-22
}
DEF DEF_VEHICLE2 Robot {
translation -4.8145 0.65222219335 5.00811
rotation 0.005134141913449905 0.9999763391777008 0.0045783918111282325 0.26180410065967374
children [
Receiver {
name "receiver_main"
type "serial"
channel 2
bufferSize 32
}
Compass {
name "compass1"
}
Camera {
translation 0 0.25 0
name "camera1"
}
Transform {
translation -0.34 0 0
rotation 0 1 0 1.5707959999999999
children [
HingeJoint {
jointParameters HingeJointParameters {
position -9.38790858754193e-12
axis 0 0 1
}
device [
RotationalMotor {
name "servo_tail"
maxVelocity 50000
maxTorque 1000
}
]
endPoint Solid {
translation -4.440892137013443e-15 -4.102385098292374e-12 1.8118839761882555e-13
rotation 1.426974259244834e-09 1 -1.7743316682306419e-09 1.5707963071795863
children [
Propeller {
shaftAxis 0 1 0
thrustConstants 11.44 0
torqueConstants 1e-05 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 2.238367478129037
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 0 1 0.09999999999999999
}
}
geometry Cylinder {
height 0.002
radius 0.02
}
}
]
}
}
]
name "solid(1)"
boundingObject Box {
size 0.01 0.01 0.01
}
physics Physics {
mass 0.001
}
}
}
]
}
Transform {
translation 0.17 0 0.3
children [
Propeller {
shaftAxis 0 -1 0
thrustConstants -11.443 0
torqueConstants 1e-05 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 0.012993
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 0 0.1
}
}
geometry Cylinder {
height 0.002
radius 0.02
}
}
]
}
}
]
}
Transform {
translation 0.16 0 -0.3
children [
Propeller {
shaftAxis 0 1 0
thrustConstants 11.443 0
torqueConstants 1e-05 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 0.012993
children [
Shape {
appearance Appearance {
material Material {
diffuseColor 1 0 0.1
}
}
geometry Cylinder {
height 0.002
radius 0.02
}
}
]
}
}
]
}
Emitter {
rotation 0 1 0 -1.5707963071795863
name "emitter_plugin"
}
InertialUnit {
name "inertial_unit"
}
Gyro {
name "gyro1"
}
Accelerometer {
name "accelerometer1"
}
GPS {
name "gps1"
}
Solid {
children [
Shape {
appearance Appearance {
material Material {
}
}
geometry Box {
size 0.1 0.1 0.1
}
}
]
boundingObject Box {
size 0.1 0.1 0.1
}
physics Physics {
mass 1
}
}
]
name "tricopter2"
physics Physics {
mass 0.001
}
controller "ardupilot_SITL_TRICOPTER"
controllerArgs "-p 5598 -df 0.01 "
customData "2"
supervisor TRUE
linearVelocity 1.891022353254736e-26 -0.009806650000000002 5.497731765059014e-25
angularVelocity -1.554888292972712e-22 -5.540057171831428e-22 -5.832083344416216e-22
}
Robot {
translation -6.43222 1.6878201933499999 0.218095
children [
Emitter {
type "serial"
channel 2
}
]
name "supervisor_sync"
boundingObject Box {
size 0.1 0.1 0.1
}
physics Physics {
}
controller "ardupilot_SITL_Supervisor"
supervisor TRUE
linearVelocity 0 -0.00980665 0
}
DirectionalLight {
direction 0 -1 0
}
UnevenTerrain {
translation 0 -0.85 0
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)"
}