SITL: Webots2021b Compatible
This commit is contained in:
Mohammad Hefny
Andrew Tridgell
parent
ae98545202
commit
ac30fbd7b0
1
libraries/SITL/examples/Webots/.gitignore
vendored
1
libraries/SITL/examples/Webots/.gitignore
vendored
@ -7,4 +7,5 @@
|
||||
/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD
|
||||
/controllers/ardupilot_SITL_TRICOPTER/ardupilot_SITL_TRICOPTER
|
||||
/controllers/ardupilot_SITL_Supervisor/ardupilot_SITL_Supervisor
|
||||
/plugins/physics/sitl_physics_env/build
|
||||
|
||||
|
||||
@ -29,14 +29,14 @@
|
||||
* You may need to add include files like <webots/distance_sensor.h> or
|
||||
* <webots/differential_wheels.h>, etc.
|
||||
*/
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <sys/types.h>
|
||||
#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"
|
||||
@ -54,16 +54,18 @@ static WbDeviceTag gps;
|
||||
static WbDeviceTag camera;
|
||||
static WbDeviceTag inertialUnit;
|
||||
static WbDeviceTag emitter;
|
||||
static WbDeviceTag receiver;
|
||||
|
||||
|
||||
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;
|
||||
|
||||
static int timestep;
|
||||
|
||||
#ifdef DEBUG_SENSORS
|
||||
FILE *fptr;
|
||||
#endif
|
||||
|
||||
#ifdef DEBUG_USE_KB
|
||||
/*
|
||||
@ -71,6 +73,8 @@ static int timestep;
|
||||
// You can start this controller and use telnet instead of SITL to start the simulator.
|
||||
Then you can use Keyboard to emulate motor input.
|
||||
*/
|
||||
static float factor = 1.0f;
|
||||
static float offset = 0.0f;
|
||||
void process_keyboard ()
|
||||
{
|
||||
switch (wb_keyboard_get_key())
|
||||
@ -102,32 +106,39 @@ void process_keyboard ()
|
||||
v[3] = v[3] + 0.01;
|
||||
break;
|
||||
|
||||
case 'E':
|
||||
for (int i=0; i<MOTOR_NUM;++i)
|
||||
{
|
||||
v[i] += 0.00002;
|
||||
}
|
||||
break;
|
||||
|
||||
case 'W':
|
||||
for (int i=0; i<MOTOR_NUM;++i)
|
||||
{
|
||||
v[i] += 0.01;
|
||||
v[i] += 0.0002;
|
||||
}
|
||||
break;
|
||||
|
||||
case 'S':
|
||||
for (int i=0; i<MOTOR_NUM;++i)
|
||||
{
|
||||
v[i] -= 0.01;
|
||||
v[i] -= 0.0002;
|
||||
}
|
||||
break;
|
||||
|
||||
case 'A':
|
||||
v[1] = v[1] + 0.01;
|
||||
v[3] = v[3] + 0.01;
|
||||
v[0] = v[0] - 0.01;
|
||||
v[2] = v[2] - 0.01;
|
||||
v[1] = v[1] + 0.001;
|
||||
v[3] = v[3] + 0.001;
|
||||
v[0] = v[0] - 0.001;
|
||||
v[2] = v[2] - 0.001;
|
||||
break;
|
||||
|
||||
case 'D':
|
||||
v[1] = v[1] - 0.01;
|
||||
v[3] = v[3] - 0.01;
|
||||
v[0] = v[0] + 0.01;
|
||||
v[2] = v[2] + 0.01;
|
||||
v[1] = v[1] - 0.001;
|
||||
v[3] = v[3] - 0.001;
|
||||
v[0] = v[0] + 0.001;
|
||||
v[2] = v[2] + 0.001;
|
||||
break;
|
||||
|
||||
|
||||
@ -135,11 +146,9 @@ void process_keyboard ()
|
||||
|
||||
for (int i=0; i< MOTOR_NUM; ++i)
|
||||
{
|
||||
if (v[i] <=0) v[i]=0;
|
||||
if (v[i] >=600) v[i]=10;
|
||||
|
||||
|
||||
wb_motor_set_position(motors[i], INFINITY);
|
||||
wb_motor_set_velocity(motors[i], v[i]);
|
||||
wb_motor_set_velocity(motors[i], factor* v[i] + offset);
|
||||
|
||||
|
||||
}
|
||||
@ -150,30 +159,13 @@ 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.
|
||||
*/
|
||||
void update_controls()
|
||||
{
|
||||
/*
|
||||
1 N = 101.9716213 grams force
|
||||
1 N = 101.97162129779 grams force
|
||||
Thrust = t1 * |omega| * omega - t2 * |omega| * V
|
||||
Where t1 and t2 are the constants specified in the thrustConstants field,
|
||||
omega is the motor angular velocity
|
||||
@ -184,22 +176,46 @@ void update_controls()
|
||||
|
||||
LINEAR_THRUST
|
||||
we also want throttle to be linear with thrust so we use sqrt to calculate omega from input.
|
||||
Check this doc: https://docs.google.com/spreadsheets/d/1eR4Fb6cgaTb-BHUKJbhAXPzyX0ZLtUcEE3EY-wQYvM8/edit?usp=sharing
|
||||
*/
|
||||
static float factor = 1.0f;
|
||||
static float offset = 0.0f;
|
||||
static float v[4];
|
||||
#ifndef DEBUG_USE_KB
|
||||
//static float factor = 2.1f;
|
||||
//static float offset = 0.0f;
|
||||
|
||||
|
||||
//static float factor = 1.0f;
|
||||
static float offset = 0.0f;
|
||||
|
||||
static float v[4];
|
||||
// pls check https://docs.google.com/spreadsheets/d/1eR4Fb6cgaTb-BHUKJbhAXPzyX0ZLtUcEE3EY-wQYvM8/edit?usp=sharing
|
||||
static float factorDyn[11] = {
|
||||
3.6f, // 0.0
|
||||
3.6f, // 0.1
|
||||
4.6f, // 0.2
|
||||
4.1f, // 0.3
|
||||
4.1f, // 0.4
|
||||
3.9f, // 0.5
|
||||
3.9f, // 0.6
|
||||
3.8f, // 0.7
|
||||
3.7f, // 0.8
|
||||
3.6f, // 0.9
|
||||
3.4f // 1.0
|
||||
};
|
||||
//#define LINEAR_THRUST
|
||||
|
||||
|
||||
|
||||
#ifdef LINEAR_THRUST
|
||||
v[0] = sqrt(state.motors.w ) * factor + offset;
|
||||
v[1] = sqrt(state.motors.x ) * factor + offset;
|
||||
v[2] = sqrt(state.motors.y ) * factor + offset;
|
||||
v[3] = sqrt(state.motors.z ) * factor + offset;
|
||||
v[0] = sqrt(state.motors.w) * factor + offset;
|
||||
v[1] = sqrt(state.motors.x) * factor + offset;
|
||||
v[2] = sqrt(state.motors.y) * factor + offset;
|
||||
v[3] = sqrt(state.motors.z) * factor + offset;
|
||||
#else
|
||||
v[0] = (state.motors.w ) * factor + offset;
|
||||
v[1] = (state.motors.x ) * factor + offset;
|
||||
v[2] = (state.motors.y ) * factor + offset;
|
||||
v[3] = (state.motors.z ) * factor + offset;
|
||||
#endif
|
||||
v[0] = (state.motors.w ) * factorDyn[(int)(10 * state.motors.w)] + offset;
|
||||
v[1] = (state.motors.x ) * factorDyn[(int)(10 * state.motors.x)] + offset;
|
||||
v[2] = (state.motors.y ) * factorDyn[(int)(10 * state.motors.y)] + offset;
|
||||
v[3] = (state.motors.z ) * factorDyn[(int)(10 * state.motors.z)] + offset;
|
||||
#endif //LINEAR_THRUST
|
||||
|
||||
for ( int i=0; i<4; ++i)
|
||||
{
|
||||
@ -209,9 +225,12 @@ void update_controls()
|
||||
|
||||
#ifdef DEBUG_MOTORS
|
||||
printf ("RAW F:%f R:%f B:%f L:%f\n", state.motors.w, state.motors.x, state.motors.y, state.motors.z);
|
||||
printf ("Factors F:%f \n", factorDyn[(int)(10 * state.motors.w)]);
|
||||
printf ("Motors F:%f R:%f B:%f L:%f\n", v[0], v[1], v[2], v[3]);
|
||||
#endif
|
||||
|
||||
#endif //DEBUG_MOTORS
|
||||
|
||||
#endif //DEBUG_USE_KB
|
||||
|
||||
#ifdef WIND_SIMULATION
|
||||
/*
|
||||
@ -223,18 +242,11 @@ 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 == 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.
|
||||
wind_webots_axis.y = state.wind.z - linear_velocity[1];
|
||||
}
|
||||
else
|
||||
{ // as in pyramids and any open map street world.
|
||||
wind_webots_axis.x = state.wind.y - linear_velocity[0]; // always add "linear_velocity" as there is no axis transformation here.
|
||||
wind_webots_axis.z = -state.wind.x - linear_velocity[2];
|
||||
wind_webots_axis.y = state.wind.z - linear_velocity[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.
|
||||
wind_webots_axis.y = state.wind.z - linear_velocity[1];
|
||||
|
||||
|
||||
wind_webots_axis.x = dragFactor * wind_webots_axis.x * abs(wind_webots_axis.x);
|
||||
wind_webots_axis.z = dragFactor * wind_webots_axis.z * abs(wind_webots_axis.z);
|
||||
@ -340,14 +352,18 @@ void run ()
|
||||
}
|
||||
|
||||
|
||||
read_incoming_messages();
|
||||
|
||||
// trigget ArduPilot to send motor data
|
||||
getAllSensors ((char *)send_buf, northDirection, gyro,accelerometer,compass,gps, inertialUnit);
|
||||
getAllSensors ((char *)send_buf, gyro,accelerometer,compass,gps, inertialUnit);
|
||||
|
||||
#ifdef DEBUG_SENSORS
|
||||
//printf("at %lf %s\n",wb_robot_get_time(), send_buf);
|
||||
printf("at %lf %s\n",wb_robot_get_time(), send_buf);
|
||||
if (strlen (pBug)> 5)
|
||||
{
|
||||
// fprintf(fptr, "%s\n",pBug);
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
if (write(fd,send_buf,strlen(send_buf)) <= 0)
|
||||
{
|
||||
@ -388,7 +404,6 @@ void run ()
|
||||
}
|
||||
if (n > 0)
|
||||
{
|
||||
|
||||
command_buffer[n] = 0;
|
||||
if (parse_controls (command_buffer))
|
||||
{
|
||||
@ -412,7 +427,9 @@ bool initialize (int argc, char *argv[])
|
||||
{
|
||||
|
||||
fd_set rfds;
|
||||
|
||||
#ifdef DEBUG_SENSORS
|
||||
fptr = fopen ("/tmp/log.txt","w");
|
||||
#endif
|
||||
port = 5599; // default port
|
||||
for (int i = 0; i < argc; ++i)
|
||||
{
|
||||
@ -447,31 +464,9 @@ bool initialize (int argc, char *argv[])
|
||||
wb_robot_init();
|
||||
|
||||
timestep = (int)wb_robot_get_basic_time_step();
|
||||
timestep_scale = timestep * 1000.0;
|
||||
printf("timestep_scale: %f \n", timestep_scale);
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
|
||||
// keybaard
|
||||
#ifdef DEBUG_USE_KB
|
||||
|
||||
@ -1,10 +1,10 @@
|
||||
// #define DEBUG_MOTORS
|
||||
//#define DEBUG_MOTORS
|
||||
// #define DEBUG_WIND
|
||||
// #define DEBUG_SENSORS
|
||||
// #define DEBUG_USE_KB
|
||||
//#define DEBUG_USE_KB
|
||||
// #define DEBUG_INPUT_DATA
|
||||
// #define LINEAR_THRUST
|
||||
#define DEBUG_SOCKETS
|
||||
// #define DEBUG_SOCKETS
|
||||
|
||||
|
||||
|
||||
|
||||
@ -33,77 +33,66 @@ 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, char *buf)
|
||||
{
|
||||
const double *inertial_directions = wb_inertial_unit_get_roll_pitch_yaw (inertialUnit);
|
||||
if (northDirection == 1)
|
||||
{
|
||||
sprintf(buf,"\"roll\": %f,\"pitch\": %f,\"yaw\": %f",inertial_directions[0], inertial_directions[1], inertial_directions[2]);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(buf,"\"roll\": %f,\"pitch\": %f,\"yaw\": %f",inertial_directions[1], -inertial_directions[0], inertial_directions[2]);
|
||||
}
|
||||
|
||||
return ;
|
||||
sprintf(buf,"\"roll\": %f,\"pitch\": %f,\"yaw\": %f",inertial_directions[0], inertial_directions[1], inertial_directions[2]);
|
||||
|
||||
return ;
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
returns: "magnetic_field":_[23088.669921875,_3876.001220703125,_-53204.57421875]
|
||||
*/
|
||||
void getCompass (const WbDeviceTag compass, const double northDirection, char *buf)
|
||||
void getCompass (const WbDeviceTag compass, char *buf)
|
||||
{
|
||||
const double *north3D = wb_compass_get_values(compass);
|
||||
if (northDirection == 1)
|
||||
{
|
||||
sprintf(buf,"[%f, %f, %f]",north3D[0], north3D[2], north3D[1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(buf,"[%f, %f, %f]",north3D[2], -north3D[0], north3D[1]);
|
||||
}
|
||||
|
||||
sprintf(buf,"[%f, %f, %f]",north3D[0], north3D[2], north3D[1]);
|
||||
|
||||
return ;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
double old_north3D[3];
|
||||
double lllinear_velocity[3];
|
||||
double llspeed;
|
||||
/*
|
||||
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, char *buf)
|
||||
{
|
||||
|
||||
const double *north3D = wb_gps_get_values(gps);
|
||||
if (northDirection == 1)
|
||||
llspeed = wb_gps_get_speed(gps);
|
||||
const double delta = (north3D[0] - old_north3D[0]);
|
||||
if (delta != 0.0)
|
||||
{
|
||||
sprintf(buf,"\"x\": %f,\"y\": %f,\"z\": %f", north3D[0], north3D[2], north3D[1]);
|
||||
lllinear_velocity[0] = (north3D[0] - old_north3D[0]) * timestep_scale;
|
||||
lllinear_velocity[1] = (north3D[1] - old_north3D[1]) * timestep_scale;
|
||||
lllinear_velocity[2] = (north3D[2] - old_north3D[2]) * timestep_scale;
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(buf,"\"x\": %f,\"y\": %f,\"z\": %f", north3D[2], -north3D[0], north3D[1]);
|
||||
}
|
||||
|
||||
old_north3D[0] = north3D[0];
|
||||
old_north3D[1] = north3D[1];
|
||||
old_north3D[2] = north3D[2];
|
||||
|
||||
sprintf(buf,"\"x\": %f,\"y\": %f,\"z\": %f", north3D[0], north3D[2], north3D[1]);
|
||||
|
||||
return ;
|
||||
}
|
||||
|
||||
/*
|
||||
returns: "linear_acceleration": [0.005074390675872564, 0.22471477091312408, 9.80740737915039]
|
||||
*/
|
||||
void getAcc (const WbDeviceTag accelerometer, const double northDirection, char *buf)
|
||||
void getAcc (const WbDeviceTag accelerometer, char *buf)
|
||||
{
|
||||
//SHOULD BE CORRECT
|
||||
const double *a = wb_accelerometer_get_values(accelerometer);
|
||||
if (northDirection == 1)
|
||||
{
|
||||
sprintf(buf,"[%f, %f, %f]",a[0], a[2], a[1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(buf,"[%f, %f, %f]",a[0], a[2], a[1]);
|
||||
}
|
||||
|
||||
sprintf(buf,"[%f, %f, %f]",a[0], a[2], a[1]);
|
||||
|
||||
//sprintf(buf,"[0.0, 0.0, 0.0]");
|
||||
|
||||
@ -114,40 +103,29 @@ 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, char *buf)
|
||||
{
|
||||
|
||||
const double *g = wb_gyro_get_values(gyro);
|
||||
if (northDirection == 1)
|
||||
{
|
||||
sprintf(buf,"[%f, %f, %f]",g[0], g[2], g[1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(buf,"[%f, %f, %f]",g[0], g[2], g[1]);
|
||||
}
|
||||
|
||||
sprintf(buf,"[%f, %f, %f]",g[0], g[2], g[1]);
|
||||
|
||||
return ;
|
||||
}
|
||||
|
||||
|
||||
void getLinearVelocity (WbNodeRef nodeRef, const double northDirection, char * buf)
|
||||
void getLinearVelocity (WbNodeRef nodeRef, char * buf)
|
||||
{
|
||||
if (linear_velocity != NULL)
|
||||
{
|
||||
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]);
|
||||
}
|
||||
sprintf (buf,"[%f, %f, %f]", lllinear_velocity[0], lllinear_velocity[2], lllinear_velocity[1]);
|
||||
//sprintf (pBug,"[%f, %f, %f]", lllinear_velocity[0], lllinear_velocity[2], lllinear_velocity[1]);
|
||||
}
|
||||
|
||||
|
||||
return ;
|
||||
}
|
||||
|
||||
void getAllSensors (char *buf, const double northDirection, WbDeviceTag gyro, WbDeviceTag accelerometer, WbDeviceTag compass, const WbDeviceTag gps, const WbDeviceTag inertial_unit)
|
||||
void getAllSensors (char *buf, WbDeviceTag gyro, WbDeviceTag accelerometer, WbDeviceTag compass, const WbDeviceTag gps, const WbDeviceTag inertial_unit)
|
||||
{
|
||||
|
||||
/*
|
||||
@ -180,14 +158,14 @@ void getAllSensors (char *buf, const double northDirection, WbDeviceTag gyro, Wb
|
||||
double time = wb_robot_get_time(); // current simulation time in [s]
|
||||
sprintf(szTime,"%lf", time);
|
||||
|
||||
getGyro(gyro, northDirection, gyro_buf);
|
||||
getAcc(accelerometer, northDirection, acc_buf);
|
||||
getCompass(compass, northDirection, compass_buf);
|
||||
getGPS(gps, northDirection, gps_buf);
|
||||
getInertia (inertial_unit, northDirection, inertial_buf);
|
||||
getLinearVelocity(self_node, northDirection, linear_velocity_buf);
|
||||
getGyro(gyro, gyro_buf);
|
||||
getAcc(accelerometer, acc_buf);
|
||||
getCompass(compass, compass_buf);
|
||||
getGPS(gps, gps_buf);
|
||||
getInertia (inertial_unit, inertial_buf);
|
||||
getLinearVelocity(self_node, linear_velocity_buf);
|
||||
|
||||
sprintf (buf,"{\"ts\": %s,\"vehicle.imu\": {\"av\": %s,\"la\": %s,\"mf\": %s,\"vehicle.gps\":{%s},\"vehicle.velocity\":{\"wlv\": %s},\"vehicle.pose\":{%s,%s}}\r\n"
|
||||
, szTime, gyro_buf, acc_buf, compass_buf, gps_buf, linear_velocity_buf, gps_buf, inertial_buf );
|
||||
, szTime, gyro_buf, acc_buf, compass_buf, gps_buf, linear_velocity_buf, gps_buf, inertial_buf );
|
||||
|
||||
}
|
||||
@ -13,11 +13,12 @@
|
||||
|
||||
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, const WbDeviceTag inertial_unit);
|
||||
double timestep_scale;
|
||||
char pBug[1024];
|
||||
void getInertia (const WbDeviceTag inertialUnit, char *buf);
|
||||
void getLinearVelocity (WbNodeRef nodeRef, char * buf);
|
||||
void getCompass (const WbDeviceTag compass, char *buf);
|
||||
void getAcc (const WbDeviceTag accelerometer, char *buf);
|
||||
void getGyro (const WbDeviceTag gyro, char *buf);
|
||||
void getGPS (const WbDeviceTag gps, char *buf);
|
||||
void getAllSensors (char *buf, WbDeviceTag gyro, WbDeviceTag accelerometer, WbDeviceTag compass, const WbDeviceTag gps, const WbDeviceTag inertial_unit);
|
||||
@ -215,7 +215,7 @@ void run ()
|
||||
break;
|
||||
}
|
||||
|
||||
getAllSensors ((char *)send_buf, northDirection, gyro,accelerometer,compass,gps, inertialUnit);
|
||||
getAllSensors ((char *)send_buf, gyro,accelerometer,compass,gps, inertialUnit);
|
||||
|
||||
#ifdef DEBUG_SENSORS
|
||||
printf("%s\n",send_buf);
|
||||
|
||||
@ -33,37 +33,27 @@ 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, char *buf)
|
||||
{
|
||||
const double *inertial_directions = wb_inertial_unit_get_roll_pitch_yaw (inertialUnit);
|
||||
if (northDirection[0] == 1)
|
||||
{
|
||||
sprintf(buf,"\"roll\": %f,\"pitch\": %f,\"yaw\": %f",inertial_directions[0], inertial_directions[1], inertial_directions[2]);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(buf,"\"roll\": %f,\"pitch\": %f,\"yaw\": %f",inertial_directions[1], -inertial_directions[0], inertial_directions[2]);
|
||||
}
|
||||
|
||||
return ;
|
||||
sprintf(buf,"\"roll\": %f,\"pitch\": %f,\"yaw\": %f",inertial_directions[0], inertial_directions[1], inertial_directions[2]);
|
||||
|
||||
return ;
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
returns: "magnetic_field":_[23088.669921875,_3876.001220703125,_-53204.57421875]
|
||||
*/
|
||||
void getCompass (const WbDeviceTag compass, const double *northDirection, char *buf)
|
||||
void getCompass (const WbDeviceTag compass, char *buf)
|
||||
{
|
||||
const double *north3D = wb_compass_get_values(compass);
|
||||
if (northDirection[0] == 1)
|
||||
{
|
||||
sprintf(buf,"[%f, %f, %f]",north3D[0], north3D[2], north3D[1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(buf,"[%f, %f, %f]",north3D[2], -north3D[0], north3D[1]);
|
||||
}
|
||||
|
||||
sprintf(buf,"[%f, %f, %f]",north3D[0], north3D[2], north3D[1]);
|
||||
|
||||
return ;
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -71,39 +61,25 @@ 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, char *buf)
|
||||
{
|
||||
|
||||
const double *north3D = wb_gps_get_values(gps);
|
||||
if (northDirection[0] == 1)
|
||||
{
|
||||
sprintf(buf,"\"x\": %f,\"y\": %f,\"z\": %f", north3D[0], north3D[2], north3D[1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(buf,"\"x\": %f,\"y\": %f,\"z\": %f", north3D[2], -north3D[0], north3D[1]);
|
||||
}
|
||||
|
||||
|
||||
sprintf(buf,"\"x\": %f,\"y\": %f,\"z\": %f", north3D[0], north3D[2], north3D[1]);
|
||||
|
||||
return ;
|
||||
}
|
||||
|
||||
/*
|
||||
returns: "linear_acceleration": [0.005074390675872564, 0.22471477091312408, 9.80740737915039]
|
||||
*/
|
||||
void getAcc (const WbDeviceTag accelerometer, const double *northDirection, char *buf)
|
||||
void getAcc (const WbDeviceTag accelerometer, char *buf)
|
||||
{
|
||||
//SHOULD BE CORRECT
|
||||
const double *a = wb_accelerometer_get_values(accelerometer);
|
||||
if (northDirection[0] == 1)
|
||||
{
|
||||
sprintf(buf,"[%f, %f, %f]",a[0], a[2], a[1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(buf,"[%f, %f, %f]",a[0], a[2], a[1]);
|
||||
}
|
||||
|
||||
sprintf(buf,"[%f, %f, %f]",a[0], a[2], a[1]);
|
||||
|
||||
//sprintf(buf,"[0.0, 0.0, 0.0]");
|
||||
|
||||
@ -114,41 +90,28 @@ 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, char *buf)
|
||||
{
|
||||
|
||||
const double *g = wb_gyro_get_values(gyro);
|
||||
if (northDirection[0] == 1)
|
||||
{
|
||||
sprintf(buf,"[%f, %f, %f]",g[0], g[2], g[1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(buf,"[%f, %f, %f]",g[0], g[2], g[1]);
|
||||
}
|
||||
|
||||
sprintf(buf,"[%f, %f, %f]",g[0], g[2], g[1]);
|
||||
|
||||
return ;
|
||||
}
|
||||
|
||||
|
||||
void getLinearVelocity (WbNodeRef nodeRef, const double *northDirection, char * buf)
|
||||
void getLinearVelocity (WbNodeRef nodeRef, char * buf)
|
||||
{
|
||||
const double * vel = wb_supervisor_node_get_velocity (nodeRef);
|
||||
if (vel != NULL)
|
||||
if (linear_velocity != NULL)
|
||||
{
|
||||
if (northDirection[0] == 1)
|
||||
{
|
||||
sprintf (buf,"[%f, %f, %f]", vel[0], vel[2], vel[1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf (buf,"[%f, %f, %f]", vel[2], -vel[0], vel[1]);
|
||||
}
|
||||
sprintf (buf,"[%f, %f, %f]", linear_velocity[0], linear_velocity[2], linear_velocity[1]);
|
||||
}
|
||||
|
||||
|
||||
return ;
|
||||
}
|
||||
|
||||
void getAllSensors (char *buf, const double *northDirection, WbDeviceTag gyro, WbDeviceTag accelerometer, WbDeviceTag compass, const WbDeviceTag gps, WbDeviceTag inertial_unit)
|
||||
void getAllSensors (char *buf, WbDeviceTag gyro, WbDeviceTag accelerometer, WbDeviceTag compass, const WbDeviceTag gps, const WbDeviceTag inertial_unit)
|
||||
{
|
||||
|
||||
/*
|
||||
@ -179,16 +142,16 @@ 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);
|
||||
getCompass(compass, northDirection, compass_buf);
|
||||
getGPS(gps, northDirection, gps_buf);
|
||||
getInertia (inertial_unit, northDirection, inertial_buf);
|
||||
getLinearVelocity(wb_supervisor_node_get_self(), northDirection, linear_velocity_buf);
|
||||
getGyro(gyro, gyro_buf);
|
||||
getAcc(accelerometer, acc_buf);
|
||||
getCompass(compass, compass_buf);
|
||||
getGPS(gps, gps_buf);
|
||||
getInertia (inertial_unit, inertial_buf);
|
||||
getLinearVelocity(self_node, linear_velocity_buf);
|
||||
|
||||
sprintf (buf,"{\"ts\": %s,\"vehicle.imu\": {\"av\": %s,\"la\": %s,\"mf\": %s,\"vehicle.gps\":{%s},\"vehicle.velocity\":{\"wlv\": %s},\"vehicle.pose\":{%s,%s}}\r\n"
|
||||
, szTime, gyro_buf, acc_buf, compass_buf, gps_buf, linear_velocity_buf, gps_buf, inertial_buf );
|
||||
, szTime, gyro_buf, acc_buf, compass_buf, gps_buf, linear_velocity_buf, gps_buf, inertial_buf );
|
||||
|
||||
}
|
||||
@ -13,10 +13,10 @@
|
||||
|
||||
|
||||
|
||||
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, char *buf);
|
||||
void getLinearVelocity (WbNodeRef nodeRef, char * buf);
|
||||
void getCompass (const WbDeviceTag compass, char *buf);
|
||||
void getAcc (const WbDeviceTag accelerometer, char *buf);
|
||||
void getGyro (const WbDeviceTag gyro, char *buf);
|
||||
void getGPS (const WbDeviceTag gps, char *buf);
|
||||
void getAllSensors (char *buf, WbDeviceTag gyro, WbDeviceTag accelerometer, WbDeviceTag compass, const WbDeviceTag gps, WbDeviceTag inertial_unit);
|
||||
@ -1,74 +0,0 @@
|
||||
# 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
|
||||
@ -1,191 +0,0 @@
|
||||
/*
|
||||
* 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,4 +1,3 @@
|
||||
|
||||
/*
|
||||
* File: ardupilot_SITL_TRICOPTER.c
|
||||
* Date: 18 Aug 2019
|
||||
@ -59,7 +58,6 @@ static WbDeviceTag receiver;
|
||||
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
|
||||
@ -67,6 +65,7 @@ static double servo_value_extra = 0;
|
||||
#endif
|
||||
|
||||
int port;
|
||||
float dragFactor = VEHICLE_DRAG_FACTOR;
|
||||
|
||||
int timestep;
|
||||
|
||||
@ -163,7 +162,6 @@ static void read_incoming_messages()
|
||||
_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);
|
||||
@ -269,7 +267,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 DATA:%s\n", key->section, key->key, json);
|
||||
fprintf(stderr,"Failed to find key %s/%s DATA:%s\n", key->section, key->key, json);
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -294,7 +292,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
|
||||
@ -311,7 +309,6 @@ bool parse_controls(const char *json)
|
||||
return true;
|
||||
}
|
||||
|
||||
#define TIME_DIV 1000.0
|
||||
void run ()
|
||||
{
|
||||
|
||||
@ -337,12 +334,10 @@ void run ()
|
||||
}
|
||||
|
||||
|
||||
|
||||
read_incoming_messages();
|
||||
|
||||
|
||||
// trigget ArduPilot to send motor data
|
||||
getAllSensors ((char *)send_buf, northDirection, gyro,accelerometer,compass,gps, inertialUnit);
|
||||
getAllSensors ((char *)send_buf, gyro,accelerometer,compass,gps, inertialUnit);
|
||||
|
||||
#ifdef DEBUG_SENSORS
|
||||
printf("at %lf %s\n",wb_robot_get_time(), send_buf);
|
||||
@ -416,7 +411,7 @@ bool initialize (int argc, char *argv[])
|
||||
for (int i = 0; i < argc; ++i)
|
||||
{
|
||||
if (strcmp (argv[i],"-p")==0)
|
||||
{
|
||||
{ // specify port for SITL.
|
||||
if (argc > i+1 )
|
||||
{
|
||||
port = atoi (argv[i+1]);
|
||||
@ -424,17 +419,30 @@ bool initialize (int argc, char *argv[])
|
||||
}
|
||||
}
|
||||
else if (strcmp (argv[i],"-df")==0)
|
||||
{
|
||||
//TODO: to be implemented later. use same logic as in Quad file.
|
||||
|
||||
{ // specify drag functor used to simulate air resistance.
|
||||
if (argc > i+1 )
|
||||
{
|
||||
dragFactor = atof (argv[i+1]);
|
||||
printf("drag Factor %f\n",dragFactor);
|
||||
}
|
||||
else
|
||||
{
|
||||
fprintf(stderr,"Missing drag factor value.\n");
|
||||
return false;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
sfd = create_socket_server(port);
|
||||
|
||||
/* necessary to initialize webots stuff */
|
||||
wb_robot_init();
|
||||
|
||||
timestep = (int)wb_robot_get_basic_time_step();
|
||||
timestep_scale = timestep * 1000.0;
|
||||
printf("timestep_scale: %f \n", timestep_scale);
|
||||
|
||||
// init receiver from Supervisor
|
||||
receiver = wb_robot_get_device("receiver_main");
|
||||
@ -460,12 +468,13 @@ bool initialize (int argc, char *argv[])
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
// keybaard
|
||||
#ifdef DEBUG_USE_KB
|
||||
wb_keyboard_enable(timestep);
|
||||
#endif
|
||||
|
||||
|
||||
// inertialUnit
|
||||
inertialUnit = wb_robot_get_device("inertial_unit");
|
||||
wb_inertial_unit_enable(inertialUnit, timestep);
|
||||
@ -513,6 +522,7 @@ bool initialize (int argc, char *argv[])
|
||||
// init linear_velocity untill we receive valid data from Supervisor.
|
||||
linear_velocity = &_linear_velocity[0] ;
|
||||
|
||||
|
||||
return true;
|
||||
}
|
||||
/*
|
||||
@ -523,6 +533,8 @@ bool initialize (int argc, char *argv[])
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
|
||||
|
||||
|
||||
if (initialize( argc, argv))
|
||||
{
|
||||
|
||||
@ -533,7 +545,6 @@ int main(int argc, char **argv)
|
||||
run();
|
||||
}
|
||||
|
||||
|
||||
/* Enter your cleanup code here */
|
||||
|
||||
/* This is necessary to cleanup webots resources */
|
||||
|
||||
@ -1,14 +1,11 @@
|
||||
//#define DEBUG_MOTORS
|
||||
// #define DEBUG_MOTORS
|
||||
// #define DEBUG_SENSORS
|
||||
//#define DEBUG_USE_KB
|
||||
//#define DEBUG_INPUT_DATA
|
||||
// #define DEBUG_USE_KB
|
||||
// #define DEBUG_INPUT_DATA
|
||||
// #define LINEAR_THRUST
|
||||
// #define WIND_SIMULATION
|
||||
|
||||
|
||||
|
||||
|
||||
//#define WIND_SIMULATION
|
||||
|
||||
#define VEHICLE_DRAG_FACTOR 0.001
|
||||
|
||||
// # of simulation steps between two image frames.
|
||||
#define CAMERA_FRAME_RATE_FACTOR 50
|
||||
|
||||
@ -33,77 +33,66 @@ 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, char *buf)
|
||||
{
|
||||
const double *inertial_directions = wb_inertial_unit_get_roll_pitch_yaw (inertialUnit);
|
||||
if (northDirection == 1)
|
||||
{
|
||||
sprintf(buf,"\"roll\": %f,\"pitch\": %f,\"yaw\": %f",inertial_directions[0], inertial_directions[1], inertial_directions[2]);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(buf,"\"roll\": %f,\"pitch\": %f,\"yaw\": %f",inertial_directions[1], -inertial_directions[0], inertial_directions[2]);
|
||||
}
|
||||
|
||||
return ;
|
||||
sprintf(buf,"\"roll\": %f,\"pitch\": %f,\"yaw\": %f",inertial_directions[0], inertial_directions[1], inertial_directions[2]);
|
||||
|
||||
return ;
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
returns: "magnetic_field":_[23088.669921875,_3876.001220703125,_-53204.57421875]
|
||||
*/
|
||||
void getCompass (const WbDeviceTag compass, const double northDirection, char *buf)
|
||||
void getCompass (const WbDeviceTag compass, char *buf)
|
||||
{
|
||||
const double *north3D = wb_compass_get_values(compass);
|
||||
if (northDirection == 1)
|
||||
{
|
||||
sprintf(buf,"[%f, %f, %f]",north3D[0], north3D[2], north3D[1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(buf,"[%f, %f, %f]",north3D[2], -north3D[0], north3D[1]);
|
||||
}
|
||||
|
||||
sprintf(buf,"[%f, %f, %f]",north3D[0], north3D[2], north3D[1]);
|
||||
|
||||
return ;
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
double old_north3D[3];
|
||||
double lllinear_velocity[3];
|
||||
double llspeed;
|
||||
/*
|
||||
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, char *buf)
|
||||
{
|
||||
|
||||
const double *north3D = wb_gps_get_values(gps);
|
||||
if (northDirection == 1)
|
||||
llspeed = wb_gps_get_speed(gps);
|
||||
const double delta = (north3D[0] - old_north3D[0]);
|
||||
if (delta != 0.0)
|
||||
{
|
||||
sprintf(buf,"\"x\": %f,\"y\": %f,\"z\": %f", north3D[0], north3D[2], north3D[1]);
|
||||
lllinear_velocity[0] = (north3D[0] - old_north3D[0]) * timestep_scale;
|
||||
lllinear_velocity[1] = (north3D[1] - old_north3D[1]) * timestep_scale;
|
||||
lllinear_velocity[2] = (north3D[2] - old_north3D[2]) * timestep_scale;
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(buf,"\"x\": %f,\"y\": %f,\"z\": %f", north3D[2], -north3D[0], north3D[1]);
|
||||
}
|
||||
|
||||
old_north3D[0] = north3D[0];
|
||||
old_north3D[1] = north3D[1];
|
||||
old_north3D[2] = north3D[2];
|
||||
|
||||
sprintf(buf,"\"x\": %f,\"y\": %f,\"z\": %f", north3D[0], north3D[2], north3D[1]);
|
||||
|
||||
return ;
|
||||
}
|
||||
|
||||
/*
|
||||
returns: "linear_acceleration": [0.005074390675872564, 0.22471477091312408, 9.80740737915039]
|
||||
*/
|
||||
void getAcc (const WbDeviceTag accelerometer, const double northDirection, char *buf)
|
||||
void getAcc (const WbDeviceTag accelerometer, char *buf)
|
||||
{
|
||||
//SHOULD BE CORRECT
|
||||
const double *a = wb_accelerometer_get_values(accelerometer);
|
||||
if (northDirection == 1)
|
||||
{
|
||||
sprintf(buf,"[%f, %f, %f]",a[0], a[2], a[1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(buf,"[%f, %f, %f]",a[0], a[2], a[1]);
|
||||
}
|
||||
|
||||
sprintf(buf,"[%f, %f, %f]",a[0], a[2], a[1]);
|
||||
|
||||
//sprintf(buf,"[0.0, 0.0, 0.0]");
|
||||
|
||||
@ -114,40 +103,29 @@ 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, char *buf)
|
||||
{
|
||||
|
||||
const double *g = wb_gyro_get_values(gyro);
|
||||
if (northDirection == 1)
|
||||
{
|
||||
sprintf(buf,"[%f, %f, %f]",g[0], g[2], g[1]);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(buf,"[%f, %f, %f]",g[0], g[2], g[1]);
|
||||
}
|
||||
|
||||
sprintf(buf,"[%f, %f, %f]",g[0], g[2], g[1]);
|
||||
|
||||
return ;
|
||||
}
|
||||
|
||||
|
||||
void getLinearVelocity (WbNodeRef nodeRef, const double northDirection, char * buf)
|
||||
void getLinearVelocity (WbNodeRef nodeRef, char * buf)
|
||||
{
|
||||
if (linear_velocity != NULL)
|
||||
{
|
||||
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]);
|
||||
}
|
||||
sprintf (buf,"[%f, %f, %f]", lllinear_velocity[0], lllinear_velocity[2], lllinear_velocity[1]);
|
||||
|
||||
}
|
||||
|
||||
|
||||
return ;
|
||||
}
|
||||
|
||||
void getAllSensors (char *buf, const double northDirection, WbDeviceTag gyro, WbDeviceTag accelerometer, WbDeviceTag compass, const WbDeviceTag gps, const WbDeviceTag inertial_unit)
|
||||
void getAllSensors (char *buf, WbDeviceTag gyro, WbDeviceTag accelerometer, WbDeviceTag compass, const WbDeviceTag gps, const WbDeviceTag inertial_unit)
|
||||
{
|
||||
|
||||
/*
|
||||
@ -180,14 +158,14 @@ void getAllSensors (char *buf, const double northDirection, WbDeviceTag gyro, Wb
|
||||
double time = wb_robot_get_time(); // current simulation time in [s]
|
||||
sprintf(szTime,"%lf", time);
|
||||
|
||||
getGyro(gyro, northDirection, gyro_buf);
|
||||
getAcc(accelerometer, northDirection, acc_buf);
|
||||
getCompass(compass, northDirection, compass_buf);
|
||||
getGPS(gps, northDirection, gps_buf);
|
||||
getInertia (inertial_unit, northDirection, inertial_buf);
|
||||
getLinearVelocity(self_node, northDirection, linear_velocity_buf);
|
||||
getGyro(gyro, gyro_buf);
|
||||
getAcc(accelerometer, acc_buf);
|
||||
getCompass(compass, compass_buf);
|
||||
getGPS(gps, gps_buf);
|
||||
getInertia (inertial_unit, inertial_buf);
|
||||
getLinearVelocity(self_node, linear_velocity_buf);
|
||||
|
||||
sprintf (buf,"{\"ts\": %s,\"vehicle.imu\": {\"av\": %s,\"la\": %s,\"mf\": %s,\"vehicle.gps\":{%s},\"vehicle.velocity\":{\"wlv\": %s},\"vehicle.pose\":{%s,%s}}\r\n"
|
||||
, szTime, gyro_buf, acc_buf, compass_buf, gps_buf, linear_velocity_buf, gps_buf, inertial_buf );
|
||||
, szTime, gyro_buf, acc_buf, compass_buf, gps_buf, linear_velocity_buf, gps_buf, inertial_buf );
|
||||
|
||||
}
|
||||
@ -13,11 +13,12 @@
|
||||
|
||||
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, const WbDeviceTag inertial_unit);
|
||||
double timestep_scale;
|
||||
char pBug[1024];
|
||||
void getInertia (const WbDeviceTag inertialUnit, char *buf);
|
||||
void getLinearVelocity (WbNodeRef nodeRef, char * buf);
|
||||
void getCompass (const WbDeviceTag compass, char *buf);
|
||||
void getAcc (const WbDeviceTag accelerometer, char *buf);
|
||||
void getGyro (const WbDeviceTag gyro, char *buf);
|
||||
void getGPS (const WbDeviceTag gps, char *buf);
|
||||
void getAllSensors (char *buf, WbDeviceTag gyro, WbDeviceTag accelerometer, WbDeviceTag compass, const WbDeviceTag gps, const WbDeviceTag inertial_unit);
|
||||
@ -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/quadPlus.parm -L Pyramid
|
||||
$PWD/Tools/autotest/sim_vehicle.py -v ArduCopter -w --model webots-quad --add-param-file=libraries/SITL/examples/Webots/quadX.parm -L Pyramid
|
||||
@ -1,32 +1,17 @@
|
||||
SYSID_THISMAV 1
|
||||
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_ANG_PIT_P 0.5
|
||||
ATC_ANG_RLL_P 0.5
|
||||
ATC_ANG_YAW_P 3.50000
|
||||
ATC_RAT_PIT_P 3.5
|
||||
ATC_RAT_RLL_P 3.5
|
||||
ATC_RAT_PIT_D 0.03
|
||||
ATC_RAT_RLL_D 0.03
|
||||
ATC_RAT_YAW_P 2.5
|
||||
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_PIT_D 0.003
|
||||
ATC_RAT_RLL_D 0.003
|
||||
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
|
||||
ARMING_CHECK 33554398
|
||||
@ -1,32 +1,17 @@
|
||||
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_ANG_PIT_P 0.5
|
||||
ATC_ANG_RLL_P 0.5
|
||||
ATC_ANG_YAW_P 3.50000
|
||||
ATC_RAT_PIT_P 3.5
|
||||
ATC_RAT_RLL_P 3.5
|
||||
ATC_RAT_PIT_D 0.03
|
||||
ATC_RAT_RLL_D 0.03
|
||||
ATC_RAT_YAW_P 2.5
|
||||
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_PIT_D 0.003
|
||||
ATC_RAT_RLL_D 0.003
|
||||
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
|
||||
ARMING_CHECK 33554398
|
||||
@ -1,31 +1,17 @@
|
||||
SYSID_THISMAV 1
|
||||
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_ANG_PIT_P 0.5
|
||||
ATC_ANG_RLL_P 0.5
|
||||
ATC_ANG_YAW_P 3.50000
|
||||
ATC_RAT_PIT_P 3.5
|
||||
ATC_RAT_RLL_P 3.5
|
||||
ATC_RAT_PIT_D 0.03
|
||||
ATC_RAT_RLL_D 0.03
|
||||
ATC_RAT_YAW_P 2.5
|
||||
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_PIT_D 0.003
|
||||
ATC_RAT_RLL_D 0.003
|
||||
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
|
||||
ARMING_CHECK 33554398
|
||||
@ -1,31 +1,20 @@
|
||||
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_ANG_PIT_P 0.5
|
||||
ATC_ANG_RLL_P 0.5
|
||||
ATC_ANG_YAW_P 3.50000
|
||||
ATC_RAT_PIT_P 3.5
|
||||
ATC_RAT_RLL_P 3.5
|
||||
ATC_RAT_PIT_D 0.03
|
||||
ATC_RAT_RLL_D 0.03
|
||||
ATC_RAT_YAW_P 2.5
|
||||
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_PIT_D 0.003
|
||||
ATC_RAT_RLL_D 0.003
|
||||
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
|
||||
ARMING_CHECK 33554398
|
||||
SIM_WIND_DIR 90.0
|
||||
SIM_WIND_SPD 5.0
|
||||
SIM_WIND_TURB 0.0
|
||||
17766
libraries/SITL/examples/Webots/worlds/pyramidMap.wbt
Normal file
17766
libraries/SITL/examples/Webots/worlds/pyramidMap.wbt
Normal file
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
18100
libraries/SITL/examples/Webots/worlds/pyramidMap_two_quads.wbt
Normal file
18100
libraries/SITL/examples/Webots/worlds/pyramidMap_two_quads.wbt
Normal file
File diff suppressed because it is too large
Load Diff
@ -1,33 +1,17 @@
|
||||
#VRML_SIM R2020a utf8
|
||||
#VRML_SIM R2021b utf8
|
||||
WorldInfo {
|
||||
gravity 0 -9.80665 0
|
||||
title "ArduPilot Webots Simulator"
|
||||
gravity 9.80665
|
||||
physics "sitl_physics_env"
|
||||
basicTimeStep 1
|
||||
FPS 15
|
||||
optimalThreadCount 4
|
||||
coordinateSystem "NUE"
|
||||
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.9997978097496996 0.017446300161270606 0.009998311376764756 5.242214502651711
|
||||
position -0.10961792118984595 8.021071573852062 4.261928842800047
|
||||
orientation -0.5970464555560485 -0.7963994022827472 -0.0963510350315054 0.3998584260381905
|
||||
position -5.109140421817085 5.5744493754094595 15.933129307498696
|
||||
follow "quad_plus_sitl"
|
||||
followType "Mounted Shot"
|
||||
}
|
||||
@ -65,15 +49,8 @@ Solid {
|
||||
name "solid(1)"
|
||||
}
|
||||
DEF DEF_VEHICLE Robot {
|
||||
translation -0.027601 0.674307 0.005031
|
||||
rotation -0.012461000916064287 0.999885073506054 -0.008635000634797779 -0.22761130717958622
|
||||
translation -0.027601 0.6742971933499999 0.005031
|
||||
children [
|
||||
Receiver {
|
||||
name "receiver_main"
|
||||
type "serial"
|
||||
channel 1
|
||||
bufferSize 32
|
||||
}
|
||||
Emitter {
|
||||
name "emitter_plugin"
|
||||
description "commuicates with physics plugin"
|
||||
@ -99,6 +76,7 @@ DEF DEF_VEHICLE Robot {
|
||||
}
|
||||
GPS {
|
||||
name "gps1"
|
||||
type "laser"
|
||||
}
|
||||
Accelerometer {
|
||||
name "accelerometer1"
|
||||
@ -114,17 +92,16 @@ DEF DEF_VEHICLE Robot {
|
||||
rotation -0.5773502691896258 0.5773502691896258 0.5773502691896258 2.094395
|
||||
children [
|
||||
Solid {
|
||||
translation 0 0.35 0
|
||||
translation 0 0.2 0
|
||||
rotation 1 0 0 1.5707959999999999
|
||||
children [
|
||||
Propeller {
|
||||
shaftAxis 0 -1 0
|
||||
thrustConstants -12.2583125 0
|
||||
torqueConstants 18 0
|
||||
thrustConstants -1.01 0
|
||||
device RotationalMotor {
|
||||
name "motor3"
|
||||
controlPID 10.001 0 0
|
||||
maxVelocity 1000
|
||||
maxVelocity 5000
|
||||
}
|
||||
fastHelix Solid {
|
||||
children [
|
||||
@ -136,7 +113,7 @@ DEF DEF_VEHICLE Robot {
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -147,12 +124,12 @@ DEF DEF_VEHICLE Robot {
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0 0.1
|
||||
diffuseColor 0.960784 0.47451 0
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -160,7 +137,7 @@ DEF DEF_VEHICLE Robot {
|
||||
}
|
||||
]
|
||||
physics Physics {
|
||||
mass 0.25
|
||||
mass 0.05
|
||||
}
|
||||
}
|
||||
Shape {
|
||||
@ -180,17 +157,16 @@ DEF DEF_VEHICLE Robot {
|
||||
rotation 0 0.7071067811865476 0.7071067811865476 -3.1415923071795864
|
||||
children [
|
||||
Solid {
|
||||
translation 0 0.35 0
|
||||
translation 0 0.2 0
|
||||
rotation 1 0 0 1.5707959999999999
|
||||
children [
|
||||
Propeller {
|
||||
shaftAxis 0 1 0
|
||||
thrustConstants 12.2583125 0
|
||||
torqueConstants 18 0
|
||||
thrustConstants 1.01 0
|
||||
device RotationalMotor {
|
||||
name "motor2"
|
||||
controlPID 10.001 0 0
|
||||
maxVelocity 1000
|
||||
maxVelocity 5000
|
||||
}
|
||||
fastHelix Solid {
|
||||
children [
|
||||
@ -202,23 +178,23 @@ DEF DEF_VEHICLE Robot {
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
slowHelix Solid {
|
||||
rotation 0 -1 0 5.370767303526115
|
||||
rotation 0 1 0 1.1667874781290464
|
||||
children [
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0 0.1
|
||||
diffuseColor 0.960784 0.47451 0
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -227,7 +203,7 @@ DEF DEF_VEHICLE Robot {
|
||||
]
|
||||
name "solid(2)"
|
||||
physics Physics {
|
||||
mass 0.25
|
||||
mass 0.05
|
||||
}
|
||||
}
|
||||
Shape {
|
||||
@ -244,17 +220,16 @@ DEF DEF_VEHICLE Robot {
|
||||
rotation 0.5773502691896258 0.5773502691896258 0.5773502691896258 -2.094395307179586
|
||||
children [
|
||||
Solid {
|
||||
translation 0 0.35 0
|
||||
translation 0 0.2 0
|
||||
rotation 1 0 0 1.5707959999999999
|
||||
children [
|
||||
Propeller {
|
||||
shaftAxis 0 -1 0
|
||||
thrustConstants -12.2583125 0
|
||||
torqueConstants 18 0
|
||||
thrustConstants -1.01 0
|
||||
device RotationalMotor {
|
||||
name "motor1"
|
||||
controlPID 10.001 0 0
|
||||
maxVelocity 1000
|
||||
maxVelocity 5000
|
||||
}
|
||||
fastHelix Solid {
|
||||
children [
|
||||
@ -266,23 +241,23 @@ DEF DEF_VEHICLE Robot {
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
slowHelix Solid {
|
||||
rotation 0 1 0 5.486397909883531
|
||||
rotation 0 1 0 1.1667874781290464
|
||||
children [
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0 0.1
|
||||
diffuseColor 0.960784 0.47451 0
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -291,7 +266,7 @@ DEF DEF_VEHICLE Robot {
|
||||
]
|
||||
name "solid(1)"
|
||||
physics Physics {
|
||||
mass 0.25
|
||||
mass 0.05
|
||||
}
|
||||
}
|
||||
Shape {
|
||||
@ -309,17 +284,16 @@ DEF DEF_VEHICLE Robot {
|
||||
rotation 1 0 0 -1.5707963071795863
|
||||
children [
|
||||
Solid {
|
||||
translation 0 0.35 0
|
||||
translation 0 0.2 0
|
||||
rotation 1 0 0 1.5707959999999999
|
||||
children [
|
||||
Propeller {
|
||||
shaftAxis 0 1 0
|
||||
thrustConstants 12.2583125 0
|
||||
torqueConstants 18 0
|
||||
thrustConstants 1.01 0
|
||||
device RotationalMotor {
|
||||
name "motor4"
|
||||
controlPID 10.001 0 0
|
||||
maxVelocity 1000
|
||||
maxVelocity 5000
|
||||
}
|
||||
fastHelix Solid {
|
||||
children [
|
||||
@ -331,23 +305,23 @@ DEF DEF_VEHICLE Robot {
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
slowHelix Solid {
|
||||
rotation 0 -1 0 5.350616673324008
|
||||
rotation 0 1 0 1.1667874781290464
|
||||
children [
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0 0.1
|
||||
diffuseColor 0.960784 0.47451 0
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -356,7 +330,7 @@ DEF DEF_VEHICLE Robot {
|
||||
]
|
||||
name "solid(3)"
|
||||
physics Physics {
|
||||
mass 0.25
|
||||
mass 0.05
|
||||
}
|
||||
}
|
||||
Shape {
|
||||
@ -376,14 +350,19 @@ DEF DEF_VEHICLE Robot {
|
||||
}
|
||||
physics Physics {
|
||||
density -1
|
||||
mass 1.5
|
||||
mass 1
|
||||
centerOfMass [
|
||||
0 0 0
|
||||
]
|
||||
}
|
||||
rotationStep 0.261799
|
||||
controller "ardupilot_SITL_QUAD"
|
||||
controllerArgs "-p 5599 -df 0.01"
|
||||
controllerArgs [
|
||||
"-p"
|
||||
"5599"
|
||||
"-df"
|
||||
"0.01"
|
||||
]
|
||||
customData "1"
|
||||
}
|
||||
DirectionalLight {
|
||||
@ -398,9 +377,15 @@ HouseWithGarage {
|
||||
}
|
||||
AdvertisingBoard {
|
||||
translation 0 2.35 -5.71
|
||||
frontTexture [
|
||||
"https://ardupilot.org/application/files/6315/7552/1962/ArduPilot-Motto.png"
|
||||
]
|
||||
}
|
||||
AdvertisingBoard {
|
||||
translation 84.03999999999999 2.35 -5.81
|
||||
rotation 0 1 0 -1.5707963071795863
|
||||
name "advertising board(1)"
|
||||
frontTexture [
|
||||
"https://ardupilot.org/application/files/6315/7552/1962/ArduPilot-Motto.png"
|
||||
]
|
||||
}
|
||||
|
||||
@ -1,34 +1,17 @@
|
||||
#VRML_SIM R2020a utf8
|
||||
#VRML_SIM R2021b utf8
|
||||
WorldInfo {
|
||||
gravity 0 -9.80665 0
|
||||
gravity 9.80665
|
||||
physics "sitl_physics_env"
|
||||
basicTimeStep 2
|
||||
basicTimeStep 1
|
||||
FPS 15
|
||||
optimalThreadCount 4
|
||||
optimalThreadCount 6
|
||||
coordinateSystem "NUE"
|
||||
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"
|
||||
orientation -0.876127950041513 -0.46933997493480994 -0.11008997721976312 0.5231847446439049
|
||||
position -3.507998466973923 7.686764746094164 13.66147356160692
|
||||
follow "quad_x_sitl"
|
||||
followType "Mounted Shot"
|
||||
}
|
||||
DogHouse {
|
||||
@ -68,12 +51,6 @@ DEF DEF_VEHICLE Robot {
|
||||
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"
|
||||
@ -115,21 +92,23 @@ DEF DEF_VEHICLE Robot {
|
||||
name "inertial_unit"
|
||||
}
|
||||
Transform {
|
||||
translation -0.09999999999999999 0 0
|
||||
translation -0.1 0 0
|
||||
rotation -0.5773502691896258 0.5773502691896258 0.5773502691896258 2.094395
|
||||
children [
|
||||
Solid {
|
||||
translation 0 0.35 0
|
||||
translation 0 0.2 0
|
||||
rotation 1 0 0 1.5707959999999999
|
||||
children [
|
||||
Propeller {
|
||||
shaftAxis 0 -1 0
|
||||
thrustConstants -12.2583125 0
|
||||
torqueConstants 18 0
|
||||
thrustConstants -1.00001 0
|
||||
torqueConstants 1.1 0
|
||||
fastHelixThreshold 1
|
||||
device RotationalMotor {
|
||||
name "motor3"
|
||||
controlPID 10.001 0 0
|
||||
maxVelocity 1000
|
||||
maxVelocity 50000
|
||||
maxTorque 90
|
||||
}
|
||||
fastHelix Solid {
|
||||
children [
|
||||
@ -141,7 +120,7 @@ DEF DEF_VEHICLE Robot {
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -152,12 +131,12 @@ DEF DEF_VEHICLE Robot {
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0 0.1
|
||||
diffuseColor 0.960784 0.47451 0
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -165,7 +144,7 @@ DEF DEF_VEHICLE Robot {
|
||||
}
|
||||
]
|
||||
physics Physics {
|
||||
mass 0.25
|
||||
mass 0.05
|
||||
}
|
||||
}
|
||||
Shape {
|
||||
@ -181,21 +160,23 @@ DEF DEF_VEHICLE Robot {
|
||||
]
|
||||
}
|
||||
Transform {
|
||||
translation 0 0 0.09999999999999999
|
||||
translation 0 0.01 0.1
|
||||
rotation 0 0.7071067811865476 0.7071067811865476 -3.1415923071795864
|
||||
children [
|
||||
Solid {
|
||||
translation 0 0.35 0
|
||||
translation 0 0.2 0
|
||||
rotation 1 0 0 1.5707959999999999
|
||||
children [
|
||||
Propeller {
|
||||
shaftAxis 0 1 0
|
||||
thrustConstants 12.2583125 0
|
||||
torqueConstants 18 0
|
||||
thrustConstants 1.00001 0
|
||||
torqueConstants 1.1 0
|
||||
fastHelixThreshold 1
|
||||
device RotationalMotor {
|
||||
name "motor2"
|
||||
controlPID 10.001 0 0
|
||||
maxVelocity 1000
|
||||
maxVelocity 50000
|
||||
maxTorque 90
|
||||
}
|
||||
fastHelix Solid {
|
||||
children [
|
||||
@ -207,7 +188,7 @@ DEF DEF_VEHICLE Robot {
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -218,12 +199,12 @@ DEF DEF_VEHICLE Robot {
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0 0.1
|
||||
diffuseColor 0.960784 0.47451 0
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -232,7 +213,7 @@ DEF DEF_VEHICLE Robot {
|
||||
]
|
||||
name "solid(2)"
|
||||
physics Physics {
|
||||
mass 0.25
|
||||
mass 0.05
|
||||
}
|
||||
}
|
||||
Shape {
|
||||
@ -250,17 +231,19 @@ DEF DEF_VEHICLE Robot {
|
||||
rotation 0.5773502691896258 0.5773502691896258 0.5773502691896258 -2.094395307179586
|
||||
children [
|
||||
Solid {
|
||||
translation 0 0.35 0
|
||||
translation 0 0.2 0
|
||||
rotation 1 0 0 1.5707959999999999
|
||||
children [
|
||||
Propeller {
|
||||
shaftAxis 0 -1 0
|
||||
thrustConstants -12.2583125 0
|
||||
torqueConstants 18 0
|
||||
thrustConstants -1.00001 0
|
||||
torqueConstants 1.1 0
|
||||
fastHelixThreshold 1
|
||||
device RotationalMotor {
|
||||
name "motor1"
|
||||
controlPID 10.001 0 0
|
||||
maxVelocity 1000
|
||||
maxVelocity 50000
|
||||
maxTorque 90
|
||||
}
|
||||
fastHelix Solid {
|
||||
children [
|
||||
@ -272,7 +255,7 @@ DEF DEF_VEHICLE Robot {
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -283,12 +266,12 @@ DEF DEF_VEHICLE Robot {
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0 0.1
|
||||
diffuseColor 0.960784 0.47451 0
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -297,7 +280,7 @@ DEF DEF_VEHICLE Robot {
|
||||
]
|
||||
name "solid(1)"
|
||||
physics Physics {
|
||||
mass 0.25
|
||||
mass 0.05
|
||||
}
|
||||
}
|
||||
Shape {
|
||||
@ -315,17 +298,19 @@ DEF DEF_VEHICLE Robot {
|
||||
rotation 1 0 0 -1.5707963071795863
|
||||
children [
|
||||
Solid {
|
||||
translation 0 0.35 0
|
||||
translation 0 0.2 0
|
||||
rotation 1 0 0 1.5707959999999999
|
||||
children [
|
||||
Propeller {
|
||||
shaftAxis 0 1 0
|
||||
thrustConstants 12.2583125 0
|
||||
torqueConstants 18 0
|
||||
thrustConstants 1.00001 0
|
||||
torqueConstants 1.1 0
|
||||
fastHelixThreshold 1
|
||||
device RotationalMotor {
|
||||
name "motor4"
|
||||
controlPID 10.001 0 0
|
||||
maxVelocity 1000
|
||||
maxVelocity 50000
|
||||
maxTorque 90
|
||||
}
|
||||
fastHelix Solid {
|
||||
children [
|
||||
@ -337,7 +322,7 @@ DEF DEF_VEHICLE Robot {
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -348,12 +333,12 @@ DEF DEF_VEHICLE Robot {
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0 0.1
|
||||
diffuseColor 0.960784 0.47451 0
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -362,7 +347,7 @@ DEF DEF_VEHICLE Robot {
|
||||
]
|
||||
name "solid(3)"
|
||||
physics Physics {
|
||||
mass 0.25
|
||||
mass 0.05
|
||||
}
|
||||
}
|
||||
Shape {
|
||||
@ -382,14 +367,19 @@ DEF DEF_VEHICLE Robot {
|
||||
}
|
||||
physics Physics {
|
||||
density -1
|
||||
mass 1.5
|
||||
mass 1
|
||||
centerOfMass [
|
||||
0 0 0
|
||||
]
|
||||
}
|
||||
rotationStep 0.261799
|
||||
controller "ardupilot_SITL_QUAD"
|
||||
controllerArgs "-p 5577 -df 0.01"
|
||||
controllerArgs [
|
||||
"-p"
|
||||
"5577"
|
||||
"-df"
|
||||
"0.01"
|
||||
]
|
||||
customData "1"
|
||||
}
|
||||
DirectionalLight {
|
||||
@ -404,6 +394,9 @@ HouseWithGarage {
|
||||
}
|
||||
AdvertisingBoard {
|
||||
translation 0 2.35 -5.71
|
||||
frontTexture [
|
||||
"https://ardupilot.org/application/files/6315/7552/1962/ArduPilot-Motto.png"
|
||||
]
|
||||
}
|
||||
AdvertisingBoard {
|
||||
translation 84.03999999999999 2.35 -5.81
|
||||
|
||||
@ -1,28 +1,17 @@
|
||||
#VRML_SIM R2020a utf8
|
||||
#VRML_SIM R2021b utf8
|
||||
WorldInfo {
|
||||
gravity 0 -9.80665 0
|
||||
gravity 9.80665
|
||||
physics "sitl_physics_env"
|
||||
basicTimeStep 1
|
||||
FPS 25
|
||||
FPS 15
|
||||
optimalThreadCount 4
|
||||
coordinateSystem "NUE"
|
||||
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.9817595579497901 0.18074833080110114 0.05897636210252833 5.641450773408579
|
||||
position -0.25435571209748176 3.4363486643124848 2.2561209069971624
|
||||
follow "tricopter"
|
||||
}
|
||||
DogHouse {
|
||||
translation 34.82 0.76 -24.56
|
||||
@ -36,11 +25,6 @@ DogHouse {
|
||||
translation 185.42 0.77 48.97
|
||||
name "dog house(5)"
|
||||
}
|
||||
Viewpoint {
|
||||
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
|
||||
@ -96,8 +80,8 @@ DEF DEF_VEHICLE Robot {
|
||||
}
|
||||
]
|
||||
endPoint Solid {
|
||||
translation -4.8849852365954544e-15 -4.102262941093136e-12 1.8091922030330322e-13
|
||||
rotation 3.4266344293343444e-10 1 -6.900208306501498e-10 1.5707963071795863
|
||||
translation -4.884985467668676e-15 -4.1022629410928594e-12 1.8091922030330322e-13
|
||||
rotation 3.80215598061924e-10 1 -7.275729865152367e-10 1.5707963071795863
|
||||
children [
|
||||
Propeller {
|
||||
shaftAxis 0 1 0
|
||||
@ -118,23 +102,23 @@ DEF DEF_VEHICLE Robot {
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
slowHelix Solid {
|
||||
rotation 0 1 0 2.238367478129037
|
||||
rotation 0 1 0 1.1667874781290464
|
||||
children [
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 0 1 0.09999999999999999
|
||||
diffuseColor 0.45098 0.823529 0.0862745
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -174,23 +158,23 @@ DEF DEF_VEHICLE Robot {
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
slowHelix Solid {
|
||||
rotation 0 1 0 0.012993
|
||||
rotation 0 1 0 1.1667874781290464
|
||||
children [
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0 0.1
|
||||
diffuseColor 0.960784 0.47451 0
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -220,23 +204,23 @@ DEF DEF_VEHICLE Robot {
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
slowHelix Solid {
|
||||
rotation 0 1 0 0.012993
|
||||
rotation 0 1 0 1.1667874781290464
|
||||
children [
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0 0.1
|
||||
diffuseColor 0.960784 0.47451 0
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -285,7 +269,12 @@ DEF DEF_VEHICLE Robot {
|
||||
mass 0.001
|
||||
}
|
||||
controller "ardupilot_SITL_TRICOPTER"
|
||||
controllerArgs "-p 5599 -df 0.01"
|
||||
controllerArgs [
|
||||
"-p"
|
||||
"5599"
|
||||
"-df"
|
||||
"0.01"
|
||||
]
|
||||
customData "1"
|
||||
}
|
||||
DirectionalLight {
|
||||
@ -305,4 +294,7 @@ AdvertisingBoard {
|
||||
translation 84.03999999999999 2.35 -5.81
|
||||
rotation 0 1 0 -1.5707963071795863
|
||||
name "advertising board(1)"
|
||||
frontTexture [
|
||||
"https://ardupilot.org/application/files/6315/7552/1962/ArduPilot-Motto.png"
|
||||
]
|
||||
}
|
||||
|
||||
@ -1,36 +1,17 @@
|
||||
#VRML_SIM R2020a utf8
|
||||
#VRML_SIM R2021b utf8
|
||||
WorldInfo {
|
||||
gravity 0 -9.80665 0
|
||||
gravity 9.80665
|
||||
physics "sitl_physics_env"
|
||||
basicTimeStep 2
|
||||
basicTimeStep 1
|
||||
FPS 15
|
||||
optimalThreadCount 4
|
||||
coordinateSystem "NUE"
|
||||
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"
|
||||
orientation -0.9809407353203402 -0.18800438009076653 -0.04908795021058695 0.520283069940929
|
||||
position -0.4263057978186248 9.362345229216729 15.402780917057946
|
||||
follow "quad_x_sitl_2"
|
||||
followType "Mounted Shot"
|
||||
}
|
||||
DogHouse {
|
||||
@ -67,15 +48,9 @@ Solid {
|
||||
name "solid(1)"
|
||||
}
|
||||
DEF DEF_VEHICLE Robot {
|
||||
translation -0.027601000000000004 0.6940716404 0.005030999999999995
|
||||
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"
|
||||
@ -117,21 +92,23 @@ DEF DEF_VEHICLE Robot {
|
||||
name "inertial_unit"
|
||||
}
|
||||
Transform {
|
||||
translation -0.09999999999999999 0 0
|
||||
translation -0.1 0 0
|
||||
rotation -0.5773502691896258 0.5773502691896258 0.5773502691896258 2.094395
|
||||
children [
|
||||
Solid {
|
||||
translation 0 0.35 0
|
||||
translation 0 0.2 0
|
||||
rotation 1 0 0 1.5707959999999999
|
||||
children [
|
||||
Propeller {
|
||||
shaftAxis 0 -1 0
|
||||
thrustConstants -12.2583125 0
|
||||
torqueConstants 18 0
|
||||
thrustConstants -1.00001 0
|
||||
torqueConstants 1.1 0
|
||||
fastHelixThreshold 1
|
||||
device RotationalMotor {
|
||||
name "motor3"
|
||||
controlPID 10.001 0 0
|
||||
maxVelocity 1000
|
||||
maxVelocity 50000
|
||||
maxTorque 90
|
||||
}
|
||||
fastHelix Solid {
|
||||
children [
|
||||
@ -143,7 +120,7 @@ DEF DEF_VEHICLE Robot {
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -154,12 +131,12 @@ DEF DEF_VEHICLE Robot {
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0 0.1
|
||||
diffuseColor 0.960784 0.47451 0
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -167,7 +144,7 @@ DEF DEF_VEHICLE Robot {
|
||||
}
|
||||
]
|
||||
physics Physics {
|
||||
mass 0.25
|
||||
mass 0.05
|
||||
}
|
||||
}
|
||||
Shape {
|
||||
@ -183,21 +160,23 @@ DEF DEF_VEHICLE Robot {
|
||||
]
|
||||
}
|
||||
Transform {
|
||||
translation 0 0 0.09999999999999999
|
||||
translation 0 0.01 0.1
|
||||
rotation 0 0.7071067811865476 0.7071067811865476 -3.1415923071795864
|
||||
children [
|
||||
Solid {
|
||||
translation 0 0.35 0
|
||||
translation 0 0.2 0
|
||||
rotation 1 0 0 1.5707959999999999
|
||||
children [
|
||||
Propeller {
|
||||
shaftAxis 0 1 0
|
||||
thrustConstants 12.2583125 0
|
||||
torqueConstants 18 0
|
||||
thrustConstants 1.00001 0
|
||||
torqueConstants 1.1 0
|
||||
fastHelixThreshold 1
|
||||
device RotationalMotor {
|
||||
name "motor2"
|
||||
controlPID 10.001 0 0
|
||||
maxVelocity 1000
|
||||
maxVelocity 50000
|
||||
maxTorque 90
|
||||
}
|
||||
fastHelix Solid {
|
||||
children [
|
||||
@ -209,7 +188,7 @@ DEF DEF_VEHICLE Robot {
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -220,12 +199,12 @@ DEF DEF_VEHICLE Robot {
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0 0.1
|
||||
diffuseColor 0.960784 0.47451 0
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -234,7 +213,7 @@ DEF DEF_VEHICLE Robot {
|
||||
]
|
||||
name "solid(2)"
|
||||
physics Physics {
|
||||
mass 0.25
|
||||
mass 0.05
|
||||
}
|
||||
}
|
||||
Shape {
|
||||
@ -252,17 +231,19 @@ DEF DEF_VEHICLE Robot {
|
||||
rotation 0.5773502691896258 0.5773502691896258 0.5773502691896258 -2.094395307179586
|
||||
children [
|
||||
Solid {
|
||||
translation 0 0.35 0
|
||||
translation 0 0.2 0
|
||||
rotation 1 0 0 1.5707959999999999
|
||||
children [
|
||||
Propeller {
|
||||
shaftAxis 0 -1 0
|
||||
thrustConstants -12.2583125 0
|
||||
torqueConstants 18 0
|
||||
thrustConstants -1.00001 0
|
||||
torqueConstants 1.1 0
|
||||
fastHelixThreshold 1
|
||||
device RotationalMotor {
|
||||
name "motor1"
|
||||
controlPID 10.001 0 0
|
||||
maxVelocity 1000
|
||||
maxVelocity 50000
|
||||
maxTorque 90
|
||||
}
|
||||
fastHelix Solid {
|
||||
children [
|
||||
@ -274,7 +255,7 @@ DEF DEF_VEHICLE Robot {
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -285,12 +266,12 @@ DEF DEF_VEHICLE Robot {
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0 0.1
|
||||
diffuseColor 0.960784 0.47451 0
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -299,7 +280,7 @@ DEF DEF_VEHICLE Robot {
|
||||
]
|
||||
name "solid(1)"
|
||||
physics Physics {
|
||||
mass 0.25
|
||||
mass 0.05
|
||||
}
|
||||
}
|
||||
Shape {
|
||||
@ -317,17 +298,19 @@ DEF DEF_VEHICLE Robot {
|
||||
rotation 1 0 0 -1.5707963071795863
|
||||
children [
|
||||
Solid {
|
||||
translation 0 0.35 0
|
||||
translation 0 0.2 0
|
||||
rotation 1 0 0 1.5707959999999999
|
||||
children [
|
||||
Propeller {
|
||||
shaftAxis 0 1 0
|
||||
thrustConstants 12.2583125 0
|
||||
torqueConstants 18 0
|
||||
thrustConstants 1.00001 0
|
||||
torqueConstants 1.1 0
|
||||
fastHelixThreshold 1
|
||||
device RotationalMotor {
|
||||
name "motor4"
|
||||
controlPID 10.001 0 0
|
||||
maxVelocity 1000
|
||||
maxVelocity 50000
|
||||
maxTorque 90
|
||||
}
|
||||
fastHelix Solid {
|
||||
children [
|
||||
@ -339,7 +322,7 @@ DEF DEF_VEHICLE Robot {
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -350,12 +333,12 @@ DEF DEF_VEHICLE Robot {
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0 0.1
|
||||
diffuseColor 0.960784 0.47451 0
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -364,7 +347,7 @@ DEF DEF_VEHICLE Robot {
|
||||
]
|
||||
name "solid(3)"
|
||||
physics Physics {
|
||||
mass 0.25
|
||||
mass 0.05
|
||||
}
|
||||
}
|
||||
Shape {
|
||||
@ -378,33 +361,31 @@ DEF DEF_VEHICLE Robot {
|
||||
]
|
||||
}
|
||||
]
|
||||
name "quad_x_sitl_1"
|
||||
name "quad_x_sitl"
|
||||
boundingObject Box {
|
||||
size 0.1 0.1 0.1
|
||||
}
|
||||
physics Physics {
|
||||
density -1
|
||||
mass 1.5
|
||||
mass 1
|
||||
centerOfMass [
|
||||
0 0 0
|
||||
]
|
||||
}
|
||||
rotationStep 0.261799
|
||||
controller "ardupilot_SITL_QUAD"
|
||||
controllerArgs "-p 5598 -df 0.01"
|
||||
controllerArgs [
|
||||
"-p"
|
||||
"5599"
|
||||
"-df"
|
||||
"0.01"
|
||||
]
|
||||
customData "1"
|
||||
linearVelocity 0 -0.0588399 0
|
||||
}
|
||||
DEF DEF_VEHICLE2 Robot {
|
||||
translation 1.58556 0.6941503202 1.61821
|
||||
DEF DEF_VEHICLE Robot {
|
||||
translation -2.21265 0.694307 2.19001
|
||||
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"
|
||||
@ -446,21 +427,23 @@ DEF DEF_VEHICLE2 Robot {
|
||||
name "inertial_unit"
|
||||
}
|
||||
Transform {
|
||||
translation -0.09999999999999999 0 0
|
||||
translation -0.1 0 0
|
||||
rotation -0.5773502691896258 0.5773502691896258 0.5773502691896258 2.094395
|
||||
children [
|
||||
Solid {
|
||||
translation 0 0.35 0
|
||||
translation 0 0.2 0
|
||||
rotation 1 0 0 1.5707959999999999
|
||||
children [
|
||||
Propeller {
|
||||
shaftAxis 0 -1 0
|
||||
thrustConstants -12.2583125 0
|
||||
torqueConstants 18 0
|
||||
thrustConstants -1.00001 0
|
||||
torqueConstants 1.1 0
|
||||
fastHelixThreshold 1
|
||||
device RotationalMotor {
|
||||
name "motor3"
|
||||
controlPID 10.001 0 0
|
||||
maxVelocity 1000
|
||||
maxVelocity 50000
|
||||
maxTorque 90
|
||||
}
|
||||
fastHelix Solid {
|
||||
children [
|
||||
@ -472,7 +455,7 @@ DEF DEF_VEHICLE2 Robot {
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -483,12 +466,12 @@ DEF DEF_VEHICLE2 Robot {
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0 0.1
|
||||
diffuseColor 0.960784 0.47451 0
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -496,7 +479,7 @@ DEF DEF_VEHICLE2 Robot {
|
||||
}
|
||||
]
|
||||
physics Physics {
|
||||
mass 0.25
|
||||
mass 0.05
|
||||
}
|
||||
}
|
||||
Shape {
|
||||
@ -512,21 +495,23 @@ DEF DEF_VEHICLE2 Robot {
|
||||
]
|
||||
}
|
||||
Transform {
|
||||
translation 0 0 0.09999999999999999
|
||||
translation 0 0.01 0.1
|
||||
rotation 0 0.7071067811865476 0.7071067811865476 -3.1415923071795864
|
||||
children [
|
||||
Solid {
|
||||
translation 0 0.35 0
|
||||
translation 0 0.2 0
|
||||
rotation 1 0 0 1.5707959999999999
|
||||
children [
|
||||
Propeller {
|
||||
shaftAxis 0 1 0
|
||||
thrustConstants 12.2583125 0
|
||||
torqueConstants 18 0
|
||||
thrustConstants 1.00001 0
|
||||
torqueConstants 1.1 0
|
||||
fastHelixThreshold 1
|
||||
device RotationalMotor {
|
||||
name "motor2"
|
||||
controlPID 10.001 0 0
|
||||
maxVelocity 1000
|
||||
maxVelocity 50000
|
||||
maxTorque 90
|
||||
}
|
||||
fastHelix Solid {
|
||||
children [
|
||||
@ -538,7 +523,7 @@ DEF DEF_VEHICLE2 Robot {
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -549,12 +534,12 @@ DEF DEF_VEHICLE2 Robot {
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0 0.1
|
||||
diffuseColor 0.960784 0.47451 0
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -563,7 +548,7 @@ DEF DEF_VEHICLE2 Robot {
|
||||
]
|
||||
name "solid(2)"
|
||||
physics Physics {
|
||||
mass 0.25
|
||||
mass 0.05
|
||||
}
|
||||
}
|
||||
Shape {
|
||||
@ -581,17 +566,19 @@ DEF DEF_VEHICLE2 Robot {
|
||||
rotation 0.5773502691896258 0.5773502691896258 0.5773502691896258 -2.094395307179586
|
||||
children [
|
||||
Solid {
|
||||
translation 0 0.35 0
|
||||
translation 0 0.2 0
|
||||
rotation 1 0 0 1.5707959999999999
|
||||
children [
|
||||
Propeller {
|
||||
shaftAxis 0 -1 0
|
||||
thrustConstants -12.2583125 0
|
||||
torqueConstants 18 0
|
||||
thrustConstants -1.00001 0
|
||||
torqueConstants 1.1 0
|
||||
fastHelixThreshold 1
|
||||
device RotationalMotor {
|
||||
name "motor1"
|
||||
controlPID 10.001 0 0
|
||||
maxVelocity 1000
|
||||
maxVelocity 50000
|
||||
maxTorque 90
|
||||
}
|
||||
fastHelix Solid {
|
||||
children [
|
||||
@ -603,7 +590,7 @@ DEF DEF_VEHICLE2 Robot {
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -614,12 +601,12 @@ DEF DEF_VEHICLE2 Robot {
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0 0.1
|
||||
diffuseColor 0.960784 0.47451 0
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -628,7 +615,7 @@ DEF DEF_VEHICLE2 Robot {
|
||||
]
|
||||
name "solid(1)"
|
||||
physics Physics {
|
||||
mass 0.25
|
||||
mass 0.05
|
||||
}
|
||||
}
|
||||
Shape {
|
||||
@ -646,17 +633,19 @@ DEF DEF_VEHICLE2 Robot {
|
||||
rotation 1 0 0 -1.5707963071795863
|
||||
children [
|
||||
Solid {
|
||||
translation 0 0.35 0
|
||||
translation 0 0.2 0
|
||||
rotation 1 0 0 1.5707959999999999
|
||||
children [
|
||||
Propeller {
|
||||
shaftAxis 0 1 0
|
||||
thrustConstants 12.2583125 0
|
||||
torqueConstants 18 0
|
||||
thrustConstants 1.00001 0
|
||||
torqueConstants 1.1 0
|
||||
fastHelixThreshold 1
|
||||
device RotationalMotor {
|
||||
name "motor4"
|
||||
controlPID 10.001 0 0
|
||||
maxVelocity 1000
|
||||
maxVelocity 50000
|
||||
maxTorque 90
|
||||
}
|
||||
fastHelix Solid {
|
||||
children [
|
||||
@ -668,7 +657,7 @@ DEF DEF_VEHICLE2 Robot {
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -679,12 +668,12 @@ DEF DEF_VEHICLE2 Robot {
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0 0.1
|
||||
diffuseColor 0.960784 0.47451 0
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -693,7 +682,7 @@ DEF DEF_VEHICLE2 Robot {
|
||||
]
|
||||
name "solid(3)"
|
||||
physics Physics {
|
||||
mass 0.25
|
||||
mass 0.05
|
||||
}
|
||||
}
|
||||
Shape {
|
||||
@ -707,22 +696,26 @@ DEF DEF_VEHICLE2 Robot {
|
||||
]
|
||||
}
|
||||
]
|
||||
name "quad_x_sitl_2"
|
||||
name "quad_x_sitl"
|
||||
boundingObject Box {
|
||||
size 0.1 0.1 0.1
|
||||
}
|
||||
physics Physics {
|
||||
density -1
|
||||
mass 1.5
|
||||
mass 1
|
||||
centerOfMass [
|
||||
0 0 0
|
||||
]
|
||||
}
|
||||
rotationStep 0.261799
|
||||
controller "ardupilot_SITL_QUAD"
|
||||
controllerArgs "-p 5599 -df 0.01"
|
||||
controllerArgs [
|
||||
"-p"
|
||||
"5598"
|
||||
"-df"
|
||||
"0.01"
|
||||
]
|
||||
customData "2"
|
||||
linearVelocity 0 -0.0392266 0
|
||||
}
|
||||
DirectionalLight {
|
||||
direction 0 -1 0
|
||||
@ -736,9 +729,15 @@ HouseWithGarage {
|
||||
}
|
||||
AdvertisingBoard {
|
||||
translation 0 2.35 -5.71
|
||||
frontTexture [
|
||||
"https://ardupilot.org/application/files/6315/7552/1962/ArduPilot-Motto.png"
|
||||
]
|
||||
}
|
||||
AdvertisingBoard {
|
||||
translation 84.03999999999999 2.35 -5.81
|
||||
rotation 0 1 0 -1.5707963071795863
|
||||
name "advertising board(1)"
|
||||
frontTexture [
|
||||
"https://ardupilot.org/application/files/6315/7552/1962/ArduPilot-Motto.png"
|
||||
]
|
||||
}
|
||||
|
||||
@ -1,24 +1,16 @@
|
||||
#VRML_SIM R2020a utf8
|
||||
#VRML_SIM R2021b utf8
|
||||
WorldInfo {
|
||||
gravity 0 -9.80665 0
|
||||
gravity 9.80665
|
||||
physics "sitl_physics_env"
|
||||
basicTimeStep 1
|
||||
FPS 25
|
||||
optimalThreadCount 4
|
||||
coordinateSystem "NUE"
|
||||
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
|
||||
orientation 0.7430204203343388 0.6136940340292428 0.2670211369219613 5.2236735858922065
|
||||
position -13.41495339376872 28.05967141215561 19.477401792573247
|
||||
}
|
||||
Background {
|
||||
skyColor [
|
||||
@ -42,8 +34,8 @@ Solid {
|
||||
name "solid(1)"
|
||||
}
|
||||
DEF DEF_VEHICLE Robot {
|
||||
translation -6.12062 0.6576871933499999 0.4069059999999998
|
||||
rotation 0.005134141913449905 0.9999763391777008 0.0045783918111282325 0.26180410065967374
|
||||
translation 0.241399 0.666833 -1.42457
|
||||
rotation 0.00514799893982893 0.9999767940663002 0.004461999081102697 0.261804
|
||||
children [
|
||||
Receiver {
|
||||
name "receiver_main"
|
||||
@ -64,7 +56,7 @@ DEF DEF_VEHICLE Robot {
|
||||
children [
|
||||
HingeJoint {
|
||||
jointParameters HingeJointParameters {
|
||||
position -9.387964099116676e-12
|
||||
position -9.388038782122357e-12
|
||||
axis 0 0 1
|
||||
}
|
||||
device [
|
||||
@ -75,8 +67,8 @@ DEF DEF_VEHICLE Robot {
|
||||
}
|
||||
]
|
||||
endPoint Solid {
|
||||
translation -4.884981346861648e-15 -4.102163053687445e-12 1.8030021919912542e-13
|
||||
rotation 1.422280360462637e-09 1 -1.7696377693563738e-09 1.5707963071795863
|
||||
translation -4.884985544693083e-15 -4.102262941092767e-12 1.8091922030330322e-13
|
||||
rotation 3.896036368440465e-10 1 -7.369610254815084e-10 1.5707963071795863
|
||||
children [
|
||||
Propeller {
|
||||
shaftAxis 0 1 0
|
||||
@ -97,23 +89,23 @@ DEF DEF_VEHICLE Robot {
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
slowHelix Solid {
|
||||
rotation 0 1 0 2.238367478129037
|
||||
rotation 0 1 0 1.1667874781290464
|
||||
children [
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 0 1 0.09999999999999999
|
||||
diffuseColor 0.45098 0.823529 0.0862745
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -153,23 +145,23 @@ DEF DEF_VEHICLE Robot {
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
slowHelix Solid {
|
||||
rotation 0 1 0 0.012993
|
||||
rotation 0 1 0 1.1667874781290464
|
||||
children [
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0 0.1
|
||||
diffuseColor 0.960784 0.47451 0
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -199,23 +191,23 @@ DEF DEF_VEHICLE Robot {
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
slowHelix Solid {
|
||||
rotation 0 1 0 0.012993
|
||||
rotation 0 1 0 1.1667874781290464
|
||||
children [
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0 0.1
|
||||
diffuseColor 0.960784 0.47451 0
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -259,25 +251,27 @@ DEF DEF_VEHICLE Robot {
|
||||
}
|
||||
}
|
||||
]
|
||||
name "tricopter1"
|
||||
name "tricopter"
|
||||
physics Physics {
|
||||
mass 0.001
|
||||
}
|
||||
controller "ardupilot_SITL_TRICOPTER"
|
||||
controllerArgs "-p 5599 -df 0.01 "
|
||||
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
|
||||
DEF DEF_VEHICLE Robot {
|
||||
translation -1.86439 0.663929 -0.860399
|
||||
rotation 0.00514799893982893 0.9999767940663002 0.004461999081102697 0.261804
|
||||
children [
|
||||
Receiver {
|
||||
name "receiver_main"
|
||||
type "serial"
|
||||
channel 2
|
||||
channel 1
|
||||
bufferSize 32
|
||||
}
|
||||
Compass {
|
||||
@ -293,7 +287,7 @@ DEF DEF_VEHICLE2 Robot {
|
||||
children [
|
||||
HingeJoint {
|
||||
jointParameters HingeJointParameters {
|
||||
position -9.38790858754193e-12
|
||||
position -9.388038782122357e-12
|
||||
axis 0 0 1
|
||||
}
|
||||
device [
|
||||
@ -304,8 +298,8 @@ DEF DEF_VEHICLE2 Robot {
|
||||
}
|
||||
]
|
||||
endPoint Solid {
|
||||
translation -4.440892137013443e-15 -4.102385098292374e-12 1.8118839761882555e-13
|
||||
rotation 1.426974259244834e-09 1 -1.7743316682306419e-09 1.5707963071795863
|
||||
translation -4.884985583205287e-15 -4.102262941092721e-12 1.8091922030330322e-13
|
||||
rotation 3.9429765623510775e-10 1 -7.416550449646442e-10 1.5707963071795863
|
||||
children [
|
||||
Propeller {
|
||||
shaftAxis 0 1 0
|
||||
@ -326,23 +320,23 @@ DEF DEF_VEHICLE2 Robot {
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
slowHelix Solid {
|
||||
rotation 0 1 0 2.238367478129037
|
||||
rotation 0 1 0 1.1667874781290464
|
||||
children [
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 0 1 0.09999999999999999
|
||||
diffuseColor 0.45098 0.823529 0.0862745
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -382,23 +376,23 @@ DEF DEF_VEHICLE2 Robot {
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
slowHelix Solid {
|
||||
rotation 0 1 0 0.012993
|
||||
rotation 0 1 0 1.1667874781290464
|
||||
children [
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0 0.1
|
||||
diffuseColor 0.960784 0.47451 0
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -428,23 +422,23 @@ DEF DEF_VEHICLE2 Robot {
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
slowHelix Solid {
|
||||
rotation 0 1 0 0.012993
|
||||
rotation 0 1 0 1.1667874781290464
|
||||
children [
|
||||
Shape {
|
||||
appearance Appearance {
|
||||
material Material {
|
||||
diffuseColor 1 0 0.1
|
||||
diffuseColor 0.960784 0.47451 0
|
||||
}
|
||||
}
|
||||
geometry Cylinder {
|
||||
height 0.002
|
||||
radius 0.02
|
||||
radius 0.1
|
||||
}
|
||||
}
|
||||
]
|
||||
@ -488,34 +482,18 @@ DEF DEF_VEHICLE2 Robot {
|
||||
}
|
||||
}
|
||||
]
|
||||
name "tricopter2"
|
||||
name "tricopter"
|
||||
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
|
||||
}
|
||||
controllerArgs [
|
||||
"-p"
|
||||
"5598"
|
||||
"-df"
|
||||
"0.01"
|
||||
]
|
||||
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
|
||||
customData "2"
|
||||
}
|
||||
DirectionalLight {
|
||||
direction 0 -1 0
|
||||
@ -530,9 +508,15 @@ HouseWithGarage {
|
||||
}
|
||||
AdvertisingBoard {
|
||||
translation 0 2.35 -5.71
|
||||
frontTexture [
|
||||
"https://ardupilot.org/application/files/6315/7552/1962/ArduPilot-Motto.png"
|
||||
]
|
||||
}
|
||||
AdvertisingBoard {
|
||||
translation 84.03999999999999 2.35 -5.81
|
||||
rotation 0 1 0 -1.5707963071795863
|
||||
name "advertising board(1)"
|
||||
frontTexture [
|
||||
"https://ardupilot.org/application/files/6315/7552/1962/ArduPilot-Motto.png"
|
||||
]
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user