mirror of https://github.com/ArduPilot/ardupilot
SITL: Revamp Examples to support multiple drones
This commit is contained in:
mhefny
Peter Barker
parent
5a64156862
commit
9f56b656e0
|
|
@ -6,3 +6,5 @@
|
|||
/controllers/ardupilot_SITL_ROVER/ardupilot_SITL_ROVER
|
||||
/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD
|
||||
/controllers/ardupilot_SITL_TRICOPTER/ardupilot_SITL_TRICOPTER
|
||||
/controllers/ardupilot_SITL_Supervisor/ardupilot_SITL_Supervisor
|
||||
|
||||
|
|
|
|||
|
|
@ -4,6 +4,8 @@
|
|||
* Description: integration with ardupilot SITL simulation.
|
||||
* Author: M.S.Hefny (HefnySco)
|
||||
* Modifications:
|
||||
* - Blocking sockets
|
||||
* - Advance simulation time only when receive motor data.
|
||||
*/
|
||||
|
||||
|
||||
|
|
@ -34,6 +36,7 @@
|
|||
#include <webots/robot.h>
|
||||
#include <webots/supervisor.h>
|
||||
#include <webots/emitter.h>
|
||||
#include <webots/receiver.h>
|
||||
#include "ardupilot_SITL_QUAD.h"
|
||||
#include "sockets.h"
|
||||
#include "sensors.h"
|
||||
|
|
@ -51,9 +54,10 @@ static WbDeviceTag gps;
|
|||
static WbDeviceTag camera;
|
||||
static WbDeviceTag inertialUnit;
|
||||
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];
|
||||
int port;
|
||||
float dragFactor = VEHICLE_DRAG_FACTOR;
|
||||
|
|
@ -61,11 +65,6 @@ float dragFactor = VEHICLE_DRAG_FACTOR;
|
|||
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
|
||||
/*
|
||||
// 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
|
||||
|
||||
|
||||
|
||||
// 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.
|
||||
|
|
@ -209,7 +223,7 @@ void update_controls()
|
|||
A is the cross section of our quad in m³ in the direction of movement
|
||||
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.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
|
||||
bool parse_controls(const char *json)
|
||||
{
|
||||
//state.timestamp = 1.0;
|
||||
#ifdef DEBUG_INPUT_DATA
|
||||
printf("%s\n", json);
|
||||
#endif
|
||||
|
|
@ -249,7 +262,6 @@ bool parse_controls(const char *json)
|
|||
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) {
|
||||
|
|
@ -261,7 +273,7 @@ bool parse_controls(const char *json)
|
|||
// find key inside section
|
||||
p = strstr(p, key->key);
|
||||
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;
|
||||
}
|
||||
|
||||
|
|
@ -286,7 +298,7 @@ bool parse_controls(const char *json)
|
|||
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);
|
||||
fprintf(stderr,"Failed to parse Vector3f for %s %s/%s\n",p, key->section, key->key);
|
||||
return false;
|
||||
}
|
||||
else
|
||||
|
|
@ -306,16 +318,15 @@ bool parse_controls(const char *json)
|
|||
void run ()
|
||||
{
|
||||
|
||||
char send_buf[1000]; //1000 just a safe margin
|
||||
char command_buffer[200];
|
||||
char send_buf[1000];
|
||||
char command_buffer[1000];
|
||||
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
|
||||
process_keyboard();
|
||||
#endif
|
||||
|
|
@ -324,21 +335,23 @@ void run ()
|
|||
{
|
||||
// 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)
|
||||
if (fd < 0)
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
read_incoming_messages();
|
||||
|
||||
// trigget ArduPilot to send motor data
|
||||
getAllSensors ((char *)send_buf, northDirection, gyro,accelerometer,compass,gps, inertialUnit);
|
||||
|
||||
#ifdef DEBUG_SENSORS
|
||||
printf("%s\n",send_buf);
|
||||
printf("at %lf %s\n",wb_robot_get_time(), send_buf);
|
||||
#endif
|
||||
|
||||
if (write(fd,send_buf,strlen(send_buf)) <= 0)
|
||||
{
|
||||
printf ("Send Data Error\n");
|
||||
fprintf (stderr,"Send Data Error\n");
|
||||
}
|
||||
|
||||
if (fd)
|
||||
|
|
@ -352,7 +365,7 @@ void run ()
|
|||
if (number != 0)
|
||||
{
|
||||
// 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) {
|
||||
#ifdef _WIN32
|
||||
|
|
@ -369,22 +382,22 @@ void run ()
|
|||
#endif
|
||||
break;
|
||||
}
|
||||
|
||||
if (n==0)
|
||||
{
|
||||
break;
|
||||
}
|
||||
if (command_buffer[0] == 'e')
|
||||
{
|
||||
break;
|
||||
}
|
||||
if (n > 0)
|
||||
{
|
||||
|
||||
command_buffer[n] = 0;
|
||||
parse_controls (command_buffer);
|
||||
update_controls();
|
||||
|
||||
if (parse_controls (command_buffer))
|
||||
{
|
||||
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,40 +408,37 @@ void run ()
|
|||
}
|
||||
|
||||
|
||||
void initialize (int argc, char *argv[])
|
||||
bool 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 (strcmp (argv[i],"-p")==0)
|
||||
{ // specify port for SITL.
|
||||
if (argc > i+1 )
|
||||
{
|
||||
if (argc > i+1 )
|
||||
{
|
||||
port = atoi (argv[i+1]);
|
||||
printf("socket port %d\n",port);
|
||||
}
|
||||
port = atoi (argv[i+1]);
|
||||
printf("socket port %d\n",port);
|
||||
}
|
||||
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]);
|
||||
printf("drag Factor %f\n",dragFactor);
|
||||
}
|
||||
dragFactor = atof (argv[i+1]);
|
||||
printf("drag Factor %f\n",dragFactor);
|
||||
}
|
||||
else if (strcmp (argv[i],"-d")==0)
|
||||
else
|
||||
{
|
||||
if (argc > i+1 )
|
||||
{
|
||||
// extra delay in milliseconds
|
||||
maxSimCycleTime = atoi (argv[i+1]);
|
||||
printf("max simulation cycle time is %d ms\n",maxSimCycleTime);
|
||||
}
|
||||
fprintf(stderr,"Missing drag factor value.\n");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
sfd = create_socket_server(port);
|
||||
|
|
@ -436,45 +446,37 @@ void initialize (int argc, char *argv[])
|
|||
/* necessary to initialize webots stuff */
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
timestep = (int)wb_robot_get_basic_time_step();
|
||||
|
||||
if (northDirection[0] == 1)
|
||||
// init receiver from Supervisor
|
||||
receiver = wb_robot_get_device("receiver_main");
|
||||
if (receiver ==0)
|
||||
{
|
||||
printf ("Axis Default Directions\n");
|
||||
fprintf(stderr,"Receiver not found\n");
|
||||
fprintf(stderr,"EXIT with error\n");
|
||||
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();
|
||||
// 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;
|
||||
}
|
||||
|
||||
|
||||
// keybaard
|
||||
timestep = (int)wb_robot_get_basic_time_step();
|
||||
#ifdef DEBUG_USE_KB
|
||||
wb_keyboard_enable(timestep);
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
// inertialUnit
|
||||
|
|
@ -499,24 +501,30 @@ void initialize (int argc, char *argv[])
|
|||
|
||||
// camera
|
||||
camera = wb_robot_get_device("camera1");
|
||||
wb_camera_enable(camera, timestep);
|
||||
wb_camera_enable(camera, CAMERA_FRAME_RATE_FACTOR * timestep);
|
||||
|
||||
#ifdef WIND_SIMULATION
|
||||
// emitter
|
||||
emitter = wb_robot_get_device("emitter_plugin");
|
||||
#endif
|
||||
|
||||
|
||||
// names of motor should be the same as name of motor in the robot.
|
||||
const char *MOTOR_NAMES[] = {"motor1", "motor2", "motor3", "motor4"};
|
||||
|
||||
// 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]);
|
||||
v[i] = 0.0f;
|
||||
//assert(motors[i]);
|
||||
}
|
||||
|
||||
FD_ZERO(&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.
|
||||
|
|
@ -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:
|
||||
* wb_differential_wheels_set_speed(100.0,100.0);
|
||||
*/
|
||||
run();
|
||||
|
||||
}
|
||||
|
||||
/* Enter your cleanup code here */
|
||||
|
||||
|
|
|
|||
|
|
@ -4,11 +4,16 @@
|
|||
// #define DEBUG_USE_KB
|
||||
// #define DEBUG_INPUT_DATA
|
||||
// #define LINEAR_THRUST
|
||||
#define WIND_SIMULATION
|
||||
#define DEBUG_SOCKETS
|
||||
|
||||
|
||||
|
||||
#define WIND_SIMULATION
|
||||
#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]))
|
||||
|
||||
|
||||
|
|
@ -58,5 +63,5 @@ struct keytable {
|
|||
w: wind speed
|
||||
x , y, z: wind direction.
|
||||
*/
|
||||
VECTOR4F __attribute__((packed, aligned(1))) wind_webots_axis;
|
||||
VECTOR4F wind_webots_axis;
|
||||
|
||||
|
|
|
|||
|
|
@ -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}}
|
||||
*/
|
||||
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);
|
||||
if (northDirection[0] == 1)
|
||||
if (northDirection == 1)
|
||||
{
|
||||
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]
|
||||
*/
|
||||
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);
|
||||
if (northDirection[0] == 1)
|
||||
if (northDirection == 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},
|
||||
*/
|
||||
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);
|
||||
if (northDirection[0] == 1)
|
||||
if (northDirection == 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]
|
||||
*/
|
||||
void getAcc (const WbDeviceTag accelerometer, const double *northDirection, char *buf)
|
||||
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)
|
||||
if (northDirection == 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]
|
||||
*/
|
||||
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);
|
||||
if (northDirection[0] == 1)
|
||||
if (northDirection == 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 (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]);
|
||||
}
|
||||
else
|
||||
{
|
||||
{ // openstreet map northDirection [0,0,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];
|
||||
double time = wb_robot_get_time(); // current simulation time in [s]
|
||||
sprintf(szTime,"%f", time);
|
||||
sprintf(szTime,"%lf", time);
|
||||
|
||||
getGyro(gyro, northDirection, gyro_buf);
|
||||
getAcc(accelerometer, northDirection, acc_buf);
|
||||
|
|
|
|||
|
|
@ -14,10 +14,10 @@
|
|||
WbNodeRef self_node;
|
||||
double *linear_velocity;
|
||||
|
||||
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);
|
||||
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, const WbDeviceTag inertial_unit);
|
||||
|
|
@ -1,9 +1,11 @@
|
|||
/*
|
||||
* File: ardupilot_SITL_ROV.c
|
||||
* Date:
|
||||
* Description:
|
||||
* Author:
|
||||
* Date: July 2019
|
||||
* Description: integration with ardupilot SITL simulation.
|
||||
* Author: M.S.Hefny (HefnySco)
|
||||
* 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
|
||||
p = strstr(p, key->key);
|
||||
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;
|
||||
}
|
||||
|
||||
|
|
@ -189,9 +191,13 @@ void run ()
|
|||
{
|
||||
|
||||
char send_buf[1000]; //1000 just a safe margin
|
||||
char command_buffer[200];
|
||||
char command_buffer[1000];
|
||||
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
|
||||
process_keyboard();
|
||||
|
|
@ -202,7 +208,9 @@ void run ()
|
|||
// if no socket wait till you get a socket
|
||||
fd = socket_accept(sfd);
|
||||
if (fd > 0)
|
||||
socket_set_non_blocking(fd);
|
||||
{
|
||||
//socket_set_non_blocking(fd);
|
||||
}
|
||||
else if (fd < 0)
|
||||
break;
|
||||
}
|
||||
|
|
@ -230,7 +238,7 @@ void run ()
|
|||
{
|
||||
// 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) {
|
||||
#ifdef _WIN32
|
||||
int e = WSAGetLastError();
|
||||
|
|
@ -257,10 +265,14 @@ void run ()
|
|||
if (n > 0)
|
||||
{
|
||||
|
||||
//printf("Received %d bytes:\n", n);
|
||||
command_buffer[n] = 0;
|
||||
parse_controls (command_buffer);
|
||||
update_controls();
|
||||
if (parse_controls (command_buffer))
|
||||
{
|
||||
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 = 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");
|
||||
|
|
|
|||
|
|
@ -3,6 +3,9 @@
|
|||
#define DEBUG_INPUT_DATA
|
||||
#define LINEAR_THRUST
|
||||
|
||||
|
||||
#define CAMERA_FRAME_RATE_FACTOR 50
|
||||
|
||||
#define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0]))
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -1,12 +1,31 @@
|
|||
|
||||
/*
|
||||
* File: ardupilot_SITL_TRICOPTER.c
|
||||
* Date: 18 Aug 2019
|
||||
* Description: integration with ardupilot SITL simulation.
|
||||
* Author: M.S.Hefny (HefnySco)
|
||||
* 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
|
||||
* <webots/differential_wheels.h>, etc.
|
||||
|
|
@ -18,12 +37,12 @@
|
|||
#include <webots/robot.h>
|
||||
#include <webots/supervisor.h>
|
||||
#include <webots/emitter.h>
|
||||
#include <webots/receiver.h>
|
||||
#include "ardupilot_SITL_TRICOPTER.h"
|
||||
#include "sockets.h"
|
||||
#include "sensors.h"
|
||||
|
||||
|
||||
|
||||
#define MOTOR_NUM 3
|
||||
|
||||
static WbDeviceTag motors[MOTOR_NUM];
|
||||
|
|
@ -34,23 +53,22 @@ static WbDeviceTag compass;
|
|||
static WbDeviceTag gps;
|
||||
static WbDeviceTag camera;
|
||||
static WbDeviceTag inertialUnit;
|
||||
static WbDeviceTag emitter;
|
||||
static WbNodeRef world_info;
|
||||
static WbDeviceTag receiver;
|
||||
|
||||
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 servo_value = 0;
|
||||
#ifdef DEBUG_USE_KB
|
||||
static double servo_value_extra = 0;
|
||||
#endif
|
||||
|
||||
int port;
|
||||
|
||||
|
||||
// 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;
|
||||
int timestep;
|
||||
|
||||
|
||||
#ifdef DEBUG_USE_KB
|
||||
|
|
@ -135,8 +153,22 @@ void process_keyboard ()
|
|||
}
|
||||
#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.
|
||||
|
|
@ -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]);
|
||||
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.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
|
||||
bool parse_controls(const char *json)
|
||||
{
|
||||
//state.timestamp = 1.0;
|
||||
#ifdef DEBUG_INPUT_DATA
|
||||
printf("%s\n", json);
|
||||
#endif
|
||||
|
|
@ -227,7 +258,6 @@ bool parse_controls(const char *json)
|
|||
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) {
|
||||
|
|
@ -239,7 +269,7 @@ bool parse_controls(const char *json)
|
|||
// find key inside section
|
||||
p = strstr(p, key->key);
|
||||
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;
|
||||
}
|
||||
|
||||
|
|
@ -281,43 +311,47 @@ bool parse_controls(const char *json)
|
|||
return true;
|
||||
}
|
||||
|
||||
|
||||
#define TIME_DIV 1000.0
|
||||
void run ()
|
||||
{
|
||||
|
||||
char send_buf[1000]; //1000 just a safe margin
|
||||
char command_buffer[200];
|
||||
|
||||
char send_buf[1000];
|
||||
char command_buffer[1000];
|
||||
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
|
||||
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)
|
||||
if (fd < 0)
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
read_incoming_messages();
|
||||
|
||||
|
||||
// trigget ArduPilot to send motor data
|
||||
getAllSensors ((char *)send_buf, northDirection, gyro,accelerometer,compass,gps, inertialUnit);
|
||||
|
||||
#ifdef DEBUG_SENSORS
|
||||
printf("%s\n",send_buf);
|
||||
printf("at %lf %s\n",wb_robot_get_time(), send_buf);
|
||||
#endif
|
||||
|
||||
|
||||
if (write(fd,send_buf,strlen(send_buf)) <= 0)
|
||||
{
|
||||
printf ("Send Data Error\n");
|
||||
fprintf (stderr,"Send Data Error\n");
|
||||
}
|
||||
|
||||
if (fd)
|
||||
|
|
@ -331,8 +365,8 @@ void run ()
|
|||
if (number != 0)
|
||||
{
|
||||
// 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) {
|
||||
#ifdef _WIN32
|
||||
int e = WSAGetLastError();
|
||||
|
|
@ -352,16 +386,16 @@ void run ()
|
|||
{
|
||||
break;
|
||||
}
|
||||
if (command_buffer[0] == 'e')
|
||||
{
|
||||
break;
|
||||
}
|
||||
if (n > 0)
|
||||
{
|
||||
|
||||
command_buffer[n] = 0;
|
||||
parse_controls (command_buffer);
|
||||
update_controls();
|
||||
if (parse_controls (command_buffer))
|
||||
{
|
||||
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,77 +407,64 @@ void run ()
|
|||
}
|
||||
|
||||
|
||||
void initialize (int argc, char *argv[])
|
||||
bool 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]);
|
||||
printf("socket port %d\n",port);
|
||||
}
|
||||
}
|
||||
else if (strcmp (argv[i],"-d")==0)
|
||||
{
|
||||
if (argc > i+1 )
|
||||
{
|
||||
// extra delay in milliseconds
|
||||
maxSimCycleTime = atoi (argv[i+1]);
|
||||
printf("max simulation cycle time is %d ms\n",maxSimCycleTime);
|
||||
}
|
||||
else if (strcmp (argv[i],"-df")==0)
|
||||
{
|
||||
//TODO: to be implemented later. use same logic as in Quad file.
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
sfd = create_socket_server(port);
|
||||
|
||||
/* necessary to initialize webots stuff */
|
||||
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();
|
||||
|
||||
// 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);
|
||||
|
||||
|
||||
#endif
|
||||
|
||||
// inertialUnit
|
||||
inertialUnit = wb_robot_get_device("inertial_unit");
|
||||
|
|
@ -467,26 +488,32 @@ void initialize (int argc, char *argv[])
|
|||
|
||||
// camera
|
||||
camera = wb_robot_get_device("camera1");
|
||||
wb_camera_enable(camera, timestep);
|
||||
wb_camera_enable(camera, CAMERA_FRAME_RATE_FACTOR * timestep);
|
||||
|
||||
#ifdef WIND_SIMULATION
|
||||
// emitter
|
||||
emitter = wb_robot_get_device("emitter_plugin");
|
||||
#endif
|
||||
|
||||
// names of motor should be the same as name of motor in the robot.
|
||||
const char *MOTOR_NAMES[] = {"motor1", "motor2", "motor3"};
|
||||
|
||||
// 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]);
|
||||
v[i] = 0.0f;
|
||||
//assert(motors[i]);
|
||||
}
|
||||
|
||||
// tricopter servo name
|
||||
servo = wb_robot_get_device("servo_tail");
|
||||
|
||||
FD_ZERO(&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.
|
||||
|
|
@ -496,36 +523,15 @@ void initialize (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:
|
||||
* wb_differential_wheels_set_speed(100.0,100.0);
|
||||
*/
|
||||
run();
|
||||
}
|
||||
|
||||
|
||||
/* Enter your cleanup code here */
|
||||
|
|
|
|||
|
|
@ -3,9 +3,16 @@
|
|||
//#define DEBUG_USE_KB
|
||||
//#define DEBUG_INPUT_DATA
|
||||
// #define LINEAR_THRUST
|
||||
|
||||
|
||||
|
||||
|
||||
//#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]))
|
||||
|
||||
|
||||
|
|
@ -57,5 +64,5 @@ struct keytable {
|
|||
w: wind speed
|
||||
x , y, z: wind direction.
|
||||
*/
|
||||
VECTOR4F __attribute__((packed, aligned(1))) wind_webots_axis;
|
||||
VECTOR4F wind_webots_axis;
|
||||
|
||||
|
|
|
|||
|
|
@ -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}}
|
||||
*/
|
||||
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);
|
||||
if (northDirection[0] == 1)
|
||||
if (northDirection == 1)
|
||||
{
|
||||
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]
|
||||
*/
|
||||
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);
|
||||
if (northDirection[0] == 1)
|
||||
if (northDirection == 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},
|
||||
*/
|
||||
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);
|
||||
if (northDirection[0] == 1)
|
||||
if (northDirection == 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]
|
||||
*/
|
||||
void getAcc (const WbDeviceTag accelerometer, const double *northDirection, char *buf)
|
||||
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)
|
||||
if (northDirection == 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]
|
||||
*/
|
||||
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);
|
||||
if (northDirection[0] == 1)
|
||||
if (northDirection == 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 (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]);
|
||||
}
|
||||
else
|
||||
{
|
||||
{ // openstreet map northDirection [0,0,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];
|
||||
double time = wb_robot_get_time(); // current simulation time in [s]
|
||||
sprintf(szTime,"%f", time);
|
||||
sprintf(szTime,"%lf", time);
|
||||
|
||||
getGyro(gyro, northDirection, gyro_buf);
|
||||
getAcc(accelerometer, northDirection, acc_buf);
|
||||
|
|
|
|||
|
|
@ -14,10 +14,10 @@
|
|||
WbNodeRef self_node;
|
||||
double *linear_velocity;
|
||||
|
||||
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);
|
||||
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, const WbDeviceTag inertial_unit);
|
||||
|
|
@ -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,4 +2,4 @@
|
|||
|
||||
# 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
|
||||
$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
|
||||
|
|
|
|||
|
|
@ -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 " &
|
||||
|
|
@ -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
|
||||
}
|
||||
|
|
@ -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,3 +1,4 @@
|
|||
SYSID_THISMAV 1
|
||||
FRAME_CLASS 1.000000
|
||||
FRAME_TYPE 0.000000
|
||||
ATC_ANG_PIT_P 1.0
|
||||
|
|
|
|||
|
|
@ -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,3 +1,4 @@
|
|||
SYSID_THISMAV 1
|
||||
FRAME_CLASS 1.000000
|
||||
FRAME_TYPE 1.000000
|
||||
ATC_ANG_PIT_P 1.0
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -1,12 +1,13 @@
|
|||
SYSID_THISMAV 1
|
||||
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.5
|
||||
ATC_RAT_RLL_P 3.5
|
||||
ATC_RAT_YAW_P 1.5
|
||||
ATC_RAT_YAW_I 0.03
|
||||
ATC_ANG_YAW_P 0.50
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -1,4 +1,4 @@
|
|||
#VRML_SIM R2019b utf8
|
||||
#VRML_SIM R2020a utf8
|
||||
WorldInfo {
|
||||
info [
|
||||
"World generated using the Open Street Map to Webots importer"
|
||||
|
|
@ -12,12 +12,30 @@ WorldInfo {
|
|||
northDirection 0 0 1
|
||||
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.008430334675332775 -0.9889506387233125 -0.1480052824260481 2.792301901785628
|
||||
position -44.85708948922592 41.47847179839409 -120.78477309902298
|
||||
position -2.7392169823215062 2.3278150473447536 -7.277902320169236
|
||||
near 3
|
||||
follow "quad_plus_sitl"
|
||||
followOrientation TRUE
|
||||
followType "Mounted Shot"
|
||||
}
|
||||
TexturedBackground {
|
||||
}
|
||||
|
|
@ -36,8 +54,15 @@ Floor {
|
|||
}
|
||||
}
|
||||
DEF DEF_VEHICLE Robot {
|
||||
translation 7.15062e-12 0.06 1.08491e-11
|
||||
rotation -1.6967771456756733e-10 1 2.4151813868919206e-10 -0.7512383045779282
|
||||
children [
|
||||
Receiver {
|
||||
name "receiver_main"
|
||||
type "serial"
|
||||
channel 1
|
||||
bufferSize 32
|
||||
}
|
||||
Emitter {
|
||||
name "emitter_plugin"
|
||||
description "commuicates with physics plugin"
|
||||
|
|
@ -348,8 +373,8 @@ DEF DEF_VEHICLE Robot {
|
|||
}
|
||||
rotationStep 0.261799
|
||||
controller "ardupilot_SITL_QUAD"
|
||||
controllerArgs "-p 5599 -df 0.02"
|
||||
supervisor TRUE
|
||||
controllerArgs "-p 5599 -df 0.01"
|
||||
customData "1"
|
||||
}
|
||||
Road {
|
||||
translation -972.826351 0.01 137.636697
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load Diff
|
|
@ -1,17 +1,35 @@
|
|||
#VRML_SIM R2019b utf8
|
||||
#VRML_SIM R2020a utf8
|
||||
WorldInfo {
|
||||
gravity 0 -9.80665 0
|
||||
physics "sitl_physics_env"
|
||||
basicTimeStep 2
|
||||
basicTimeStep 1
|
||||
FPS 15
|
||||
optimalThreadCount 4
|
||||
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.5542589845891367 0.8247263718920818 0.11235385844706426 5.801159266912386
|
||||
position -4.295082945651872 3.093288102690911 9.827811912728198
|
||||
orientation 0.9997978097496996 0.017446300161270606 0.009998311376764756 5.242214502651711
|
||||
position -0.10961792118984595 8.021071573852062 4.261928842800047
|
||||
follow "quad_plus_sitl"
|
||||
followOrientation TRUE
|
||||
followType "Mounted Shot"
|
||||
}
|
||||
DogHouse {
|
||||
translation 34.82 0.76 -24.56
|
||||
|
|
@ -29,8 +47,6 @@ Background {
|
|||
skyColor [
|
||||
0.15 0.5 1
|
||||
]
|
||||
cubemap Cubemap {
|
||||
}
|
||||
}
|
||||
Solid {
|
||||
translation 36.93 0.77 -37.93
|
||||
|
|
@ -52,6 +68,12 @@ DEF DEF_VEHICLE Robot {
|
|||
translation -0.027601 0.674307 0.005031
|
||||
rotation -0.012461000916064287 0.999885073506054 -0.008635000634797779 -0.22761130717958622
|
||||
children [
|
||||
Receiver {
|
||||
name "receiver_main"
|
||||
type "serial"
|
||||
channel 1
|
||||
bufferSize 32
|
||||
}
|
||||
Emitter {
|
||||
name "emitter_plugin"
|
||||
description "commuicates with physics plugin"
|
||||
|
|
@ -362,7 +384,7 @@ DEF DEF_VEHICLE Robot {
|
|||
rotationStep 0.261799
|
||||
controller "ardupilot_SITL_QUAD"
|
||||
controllerArgs "-p 5599 -df 0.01"
|
||||
supervisor TRUE
|
||||
customData "1"
|
||||
}
|
||||
DirectionalLight {
|
||||
direction 0 -1 0
|
||||
|
|
|
|||
|
|
@ -1,12 +1,36 @@
|
|||
#VRML_SIM R2019b utf8
|
||||
#VRML_SIM R2020a utf8
|
||||
WorldInfo {
|
||||
gravity 0 -9.80665 0
|
||||
physics "sitl_physics_env"
|
||||
basicTimeStep 2
|
||||
FPS 25
|
||||
FPS 15
|
||||
optimalThreadCount 4
|
||||
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 {
|
||||
translation 34.82 0.76 -24.56
|
||||
name "dog house(1)"
|
||||
|
|
@ -19,18 +43,10 @@ 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
|
||||
|
|
@ -49,9 +65,15 @@ Solid {
|
|||
name "solid(1)"
|
||||
}
|
||||
DEF DEF_VEHICLE Robot {
|
||||
translation -0.027601 0.674307 0.005031
|
||||
translation -0.027601 0.694307 0.005031
|
||||
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"
|
||||
|
|
@ -216,7 +238,7 @@ DEF DEF_VEHICLE Robot {
|
|||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0.09999999999999999 0
|
||||
diffuseColor 1 0.09 0
|
||||
}
|
||||
}
|
||||
geometry USE DEF_ARM
|
||||
|
|
@ -367,8 +389,8 @@ DEF DEF_VEHICLE Robot {
|
|||
}
|
||||
rotationStep 0.261799
|
||||
controller "ardupilot_SITL_QUAD"
|
||||
controllerArgs "-p 5599 -df 0.01"
|
||||
supervisor TRUE
|
||||
controllerArgs "-p 5577 -df 0.01"
|
||||
customData "1"
|
||||
}
|
||||
DirectionalLight {
|
||||
direction 0 -1 0
|
||||
|
|
|
|||
|
|
@ -1,12 +1,29 @@
|
|||
#VRML_SIM R2019b utf8
|
||||
#VRML_SIM R2020a utf8
|
||||
WorldInfo {
|
||||
gravity 0 -9.80665 0
|
||||
physics "sitl_physics_env"
|
||||
basicTimeStep 2
|
||||
basicTimeStep 1
|
||||
FPS 25
|
||||
optimalThreadCount 4
|
||||
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 {
|
||||
translation 34.82 0.76 -24.56
|
||||
name "dog house(1)"
|
||||
|
|
@ -20,16 +37,14 @@ DogHouse {
|
|||
name "dog house(5)"
|
||||
}
|
||||
Viewpoint {
|
||||
orientation 0.9997750421775041 -0.014997750480821456 -0.01499775048082146 4.712163997257285
|
||||
position 0.17712971811531414 14.83048200793912 -1.4201693307676047
|
||||
orientation 0.9952082156446058 0.09310572995296737 0.02986520656892867 5.659589519324221
|
||||
position -0.1971228135348432 5.281009887921962 4.9181113380378845
|
||||
follow "tricopter"
|
||||
}
|
||||
Background {
|
||||
skyColor [
|
||||
0.15 0.5 1
|
||||
]
|
||||
cubemap Cubemap {
|
||||
}
|
||||
}
|
||||
Solid {
|
||||
translation 36.93 0.77 -37.93
|
||||
|
|
@ -51,6 +66,12 @@ DEF DEF_VEHICLE Robot {
|
|||
translation -8.233889875751989e-05 0.666500515499142 -1.3598750857814472
|
||||
rotation 0.00514799893982893 0.9999767940663002 0.004461999081102697 0.261804
|
||||
children [
|
||||
Receiver {
|
||||
name "receiver_main"
|
||||
type "serial"
|
||||
channel 1
|
||||
bufferSize 32
|
||||
}
|
||||
Compass {
|
||||
name "compass1"
|
||||
}
|
||||
|
|
@ -75,8 +96,8 @@ DEF DEF_VEHICLE Robot {
|
|||
}
|
||||
]
|
||||
endPoint Solid {
|
||||
translation -4.884982810326628e-15 -4.1022629410960365e-12 1.8091922030330322e-13
|
||||
rotation 2.3470124341273664e-11 1 -3.708275057969111e-10 1.5707963071795863
|
||||
translation -4.8849852365954544e-15 -4.102262941093136e-12 1.8091922030330322e-13
|
||||
rotation 3.4266344293343444e-10 1 -6.900208306501498e-10 1.5707963071795863
|
||||
children [
|
||||
Propeller {
|
||||
shaftAxis 0 1 0
|
||||
|
|
@ -255,7 +276,7 @@ DEF DEF_VEHICLE Robot {
|
|||
size 0.1 0.1 0.1
|
||||
}
|
||||
physics Physics {
|
||||
mass 0.6
|
||||
mass 1
|
||||
}
|
||||
}
|
||||
]
|
||||
|
|
@ -264,7 +285,8 @@ DEF DEF_VEHICLE Robot {
|
|||
mass 0.001
|
||||
}
|
||||
controller "ardupilot_SITL_TRICOPTER"
|
||||
supervisor TRUE
|
||||
controllerArgs "-p 5599 -df 0.01"
|
||||
customData "1"
|
||||
}
|
||||
DirectionalLight {
|
||||
direction 0 -1 0
|
||||
|
|
|
|||
|
|
@ -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)"
|
||||
}
|
||||
|
|
@ -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)"
|
||||
}
|
||||
Loading…
Reference in New Issue