mhefny
5 years ago
committed by
Francisco Ferreira
13 changed files with 1354 additions and 1 deletions
@ -0,0 +1,76 @@ |
|||||||
|
# Copyright 1996-2019 Cyberbotics Ltd.
|
||||||
|
#
|
||||||
|
# Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
# you may not use this file except in compliance with the License.
|
||||||
|
# You may obtain a copy of the License at
|
||||||
|
#
|
||||||
|
# http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
#
|
||||||
|
# Unless required by applicable law or agreed to in writing, software
|
||||||
|
# distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
# See the License for the specific language governing permissions and
|
||||||
|
# limitations under the License.
|
||||||
|
|
||||||
|
### Generic Makefile.include for Webots controllers, physics plugins, robot
|
||||||
|
### window libraries, remote control libraries and other libraries
|
||||||
|
### to be used with GNU make
|
||||||
|
###
|
||||||
|
### Platforms: Windows, macOS, Linux
|
||||||
|
### Languages: C, C++
|
||||||
|
###
|
||||||
|
### Authors: Olivier Michel, Yvan Bourquin, Fabien Rohrer
|
||||||
|
### Edmund Ronald, Sergei Poskriakov
|
||||||
|
###
|
||||||
|
###-----------------------------------------------------------------------------
|
||||||
|
###
|
||||||
|
### This file is meant to be included from the Makefile files located in the
|
||||||
|
### Webots projects subdirectories. It is possible to set a number of variables
|
||||||
|
### to customize the build process, i.e., add source files, compilation flags,
|
||||||
|
### include paths, libraries, etc. These variables should be set in your local
|
||||||
|
### Makefile just before including this Makefile.include. This Makefile.include
|
||||||
|
### should never be modified.
|
||||||
|
###
|
||||||
|
### Here is a description of the variables you may set in your local Makefile:
|
||||||
|
###
|
||||||
|
### ---- C Sources ----
|
||||||
|
### if your program uses several C source files:
|
||||||
|
### C_SOURCES = my_plugin.c my_clever_algo.c my_graphics.c
|
||||||
|
###
|
||||||
|
### ---- C++ Sources ----
|
||||||
|
### if your program uses several C++ source files:
|
||||||
|
### CXX_SOURCES = my_plugin.cc my_clever_algo.cpp my_graphics.c++
|
||||||
|
###
|
||||||
|
### ---- Compilation options ----
|
||||||
|
### if special compilation flags are necessary:
|
||||||
|
### CFLAGS = -Wno-unused-result
|
||||||
|
###
|
||||||
|
### ---- Linked libraries ----
|
||||||
|
### if your program needs additional libraries:
|
||||||
|
### INCLUDE = -I"/my_library_path/include"
|
||||||
|
### LIBRARIES = -L"/path/to/my/library" -lmy_library -lmy_other_library
|
||||||
|
###
|
||||||
|
### ---- Linking options ----
|
||||||
|
### if special linking flags are needed:
|
||||||
|
### LFLAGS = -s
|
||||||
|
###
|
||||||
|
### ---- Webots included libraries ----
|
||||||
|
### if you want to use the Webots C API in your C++ controller program:
|
||||||
|
### USE_C_API = true
|
||||||
|
### if you want to link with the Qt framework embedded in Webots:
|
||||||
|
### QT = core gui widgets network
|
||||||
|
###
|
||||||
|
### ---- Debug mode ----
|
||||||
|
### if you want to display the gcc command line for compilation and link, as
|
||||||
|
### well as the rm command details used for cleaning:
|
||||||
|
### VERBOSE = 1
|
||||||
|
###
|
||||||
|
###-----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
C_SOURCES = ardupilot_SITL_TRICOPTER.c sockets.c sensors.c
|
||||||
|
INCLUDE = -I"./"
|
||||||
|
### Do not modify: this includes Webots global Makefile.include
|
||||||
|
space :=
|
||||||
|
space +=
|
||||||
|
WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME))))
|
||||||
|
include $(WEBOTS_HOME_PATH)/resources/Makefile.include |
||||||
Binary file not shown.
@ -0,0 +1,520 @@ |
|||||||
|
/*
|
||||||
|
* File: ardupilot_SITL_TRICOPTER.c |
||||||
|
* Date: 18 Aug 2019 |
||||||
|
* Description: integration with ardupilot SITL simulation. |
||||||
|
* Author: M.S.Hefny (HefnySco) |
||||||
|
* Modifications: |
||||||
|
*/ |
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
* You may need to add include files like <webots/distance_sensor.h> or |
||||||
|
* <webots/differential_wheels.h>, etc. |
||||||
|
*/ |
||||||
|
#include <math.h> |
||||||
|
#include <stdio.h> |
||||||
|
#include <string.h> |
||||||
|
#include <sys/types.h> |
||||||
|
#include <webots/robot.h> |
||||||
|
#include <webots/supervisor.h> |
||||||
|
#include <webots/emitter.h> |
||||||
|
#include "ardupilot_SITL_TRICOPTER.h" |
||||||
|
#include "sockets.h" |
||||||
|
#include "sensors.h" |
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#define MOTOR_NUM 3 |
||||||
|
|
||||||
|
static WbDeviceTag motors[MOTOR_NUM]; |
||||||
|
static WbDeviceTag servo; |
||||||
|
static WbDeviceTag gyro; |
||||||
|
static WbDeviceTag accelerometer; |
||||||
|
static WbDeviceTag compass; |
||||||
|
static WbDeviceTag gps; |
||||||
|
static WbDeviceTag camera; |
||||||
|
static WbDeviceTag inertialUnit; |
||||||
|
static WbDeviceTag emitter; |
||||||
|
static WbNodeRef world_info; |
||||||
|
|
||||||
|
static const double *northDirection; |
||||||
|
static double v[MOTOR_NUM]; |
||||||
|
static double servo_value = 0; |
||||||
|
static double servo_value_extra = 0; |
||||||
|
|
||||||
|
int port; |
||||||
|
|
||||||
|
|
||||||
|
static int timestep; |
||||||
|
|
||||||
|
|
||||||
|
#ifdef DEBUG_USE_KB |
||||||
|
/*
|
||||||
|
// Code used tp simulae motors using keys to make sure that sensors directions and motor torques and thrusts are all correct.
|
||||||
|
// You can start this controller and use telnet instead of SITL to start the simulator.
|
||||||
|
Then you can use Keyboard to emulate motor input. |
||||||
|
*/ |
||||||
|
void process_keyboard () |
||||||
|
{ |
||||||
|
switch (wb_keyboard_get_key())
|
||||||
|
{ |
||||||
|
case 'Q': // Q key -> up & left
|
||||||
|
v[0] = 0.0; |
||||||
|
v[1] = 0.0; |
||||||
|
v[2] = 0.0; |
||||||
|
servo_value_extra = 0.0; |
||||||
|
break; |
||||||
|
|
||||||
|
case 'Y': |
||||||
|
v[0] = v[0] + 0.01; |
||||||
|
v[1] = v[1] + 0.01; |
||||||
|
v[2] = v[2] - 0.02; |
||||||
|
break; |
||||||
|
|
||||||
|
case 'H': |
||||||
|
v[0] = v[0] - 0.01; |
||||||
|
v[1] = v[1] - 0.01; |
||||||
|
v[2] = v[2] + 0.02; |
||||||
|
break; |
||||||
|
|
||||||
|
case 'G': |
||||||
|
v[0] = v[0] + 0.01; |
||||||
|
v[1] = v[1] - 0.01; |
||||||
|
break; |
||||||
|
|
||||||
|
case 'J': |
||||||
|
v[0] = v[0] - 0.01; |
||||||
|
v[1] = v[1] + 0.01; |
||||||
|
break; |
||||||
|
|
||||||
|
case 'W': |
||||||
|
for (int i=0; i<MOTOR_NUM;++i) |
||||||
|
{ |
||||||
|
v[i] += 0.01; |
||||||
|
} |
||||||
|
break; |
||||||
|
|
||||||
|
case 'S': |
||||||
|
for (int i=0; i<MOTOR_NUM;++i) |
||||||
|
{ |
||||||
|
v[i] -= 0.01; |
||||||
|
} |
||||||
|
break; |
||||||
|
|
||||||
|
case 'A': |
||||||
|
servo_value_extra = servo_value_extra + 0.01; |
||||||
|
break; |
||||||
|
|
||||||
|
case 'D': |
||||||
|
servo_value_extra = servo_value_extra - 0.01; |
||||||
|
break; |
||||||
|
|
||||||
|
|
||||||
|
} |
||||||
|
|
||||||
|
for (int i=0; i< MOTOR_NUM; ++i) |
||||||
|
{ |
||||||
|
if (v[i] <=0) v[i]=0; |
||||||
|
if (v[i] >=600) v[i]=10; |
||||||
|
|
||||||
|
wb_motor_set_position(motors[i], INFINITY); |
||||||
|
wb_motor_set_velocity(motors[i], v[i]);
|
||||||
|
} |
||||||
|
|
||||||
|
wb_motor_set_position (servo, servo_value_extra); |
||||||
|
wb_motor_set_velocity (servo, 100); |
||||||
|
|
||||||
|
|
||||||
|
printf ("Motors Internal right:%f left:%f back:%f servo:%f\n", v[0],v[1],v[2],servo_value); |
||||||
|
|
||||||
|
} |
||||||
|
#endif |
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
// apply motor thrust.
|
||||||
|
*/ |
||||||
|
void update_controls() |
||||||
|
{ |
||||||
|
/*
|
||||||
|
1 N = 101.9716213 grams force |
||||||
|
Thrust = t1 * |omega| * omega - t2 * |omega| * V |
||||||
|
Where t1 and t2 are the constants specified in the thrustConstants field, |
||||||
|
omega is the motor angular velocity
|
||||||
|
and V is the component of the linear velocity of the center of thrust along the shaft axis. |
||||||
|
|
||||||
|
if Vehicle mass = 1 Kg. and we want omega = 1.0 to hover |
||||||
|
then (mass / 0.10197) / (4 motors) = t1 |
||||||
|
|
||||||
|
LINEAR_THRUST |
||||||
|
we also want throttle to be linear with thrust so we use sqrt to calculate omega from input. |
||||||
|
*/ |
||||||
|
static float factor = 1.0f; |
||||||
|
static float offset = 0.0f; |
||||||
|
static float v[MOTOR_NUM]; |
||||||
|
|
||||||
|
#ifdef LINEAR_THRUST |
||||||
|
v[0] = sqrt(state.motors.w ) * factor + offset; |
||||||
|
v[1] = sqrt(state.motors.x ) * factor + offset; |
||||||
|
v[2] = 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.z ) * factor + offset; |
||||||
|
#endif |
||||||
|
|
||||||
|
servo_value = -state.motors.y ; |
||||||
|
|
||||||
|
for ( int i=0; i<MOTOR_NUM; ++i) |
||||||
|
{ |
||||||
|
wb_motor_set_position(motors[i], INFINITY); |
||||||
|
wb_motor_set_velocity(motors[i], v[i]);
|
||||||
|
} |
||||||
|
|
||||||
|
#ifdef DEBUG_USE_KB |
||||||
|
wb_motor_set_position(servo, servo_value + servo_value_extra); |
||||||
|
#else |
||||||
|
wb_motor_set_position(servo, servo_value); |
||||||
|
#endif |
||||||
|
wb_motor_set_velocity(servo, 1000);
|
||||||
|
|
||||||
|
#ifdef DEBUG_MOTORS |
||||||
|
printf ("RAW R:%f L:%f SRV:%f B:%f\n", state.motors.w, state.motors.x, state.motors.y, state.motors.z); |
||||||
|
printf ("Motors R:%f L:%f SRV:%f B:%f\n", v[0], v[1], servo_value, v[2]); |
||||||
|
#endif |
||||||
|
|
||||||
|
|
||||||
|
#ifdef WIND_SIMULATION |
||||||
|
|
||||||
|
double linear_speed = sqrt(linear_velocity[0] * linear_velocity[0] + linear_velocity[1] * linear_velocity[1] + linear_velocity[2] * linear_velocity[2]); |
||||||
|
wind_webots_axis.w = state.wind.w + 0.01 * linear_speed * linear_speed; |
||||||
|
|
||||||
|
if (northDirection[0] == 1) |
||||||
|
{ |
||||||
|
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]; |
||||||
|
} |
||||||
|
|
||||||
|
wb_emitter_send(emitter, &wind_webots_axis, sizeof(VECTOR4F)); |
||||||
|
|
||||||
|
#endif |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
// data example: [my_controller_SITL] {"engines": [0.000, 0.000, 0.000, 0.000]}
|
||||||
|
// the JSON parser is directly inspired by https://github.com/ArduPilot/ardupilot/blob/master/libraries/SITL/SIM_Morse.cpp
|
||||||
|
bool parse_controls(const char *json) |
||||||
|
{ |
||||||
|
//state.timestamp = 1.0;
|
||||||
|
#ifdef DEBUG_INPUT_DATA |
||||||
|
printf("%s\n", json); |
||||||
|
#endif |
||||||
|
|
||||||
|
for (uint16_t i=0; i < ARRAY_SIZE(keytable); i++) { |
||||||
|
struct keytable *key; |
||||||
|
key = &keytable[i]; |
||||||
|
//printf("search %s/%s\n", key->section, key->key);
|
||||||
|
// look for section header
|
||||||
|
const char *p = strstr(json, key->section); |
||||||
|
if (!p) { |
||||||
|
// we don't have this sensor
|
||||||
|
continue; |
||||||
|
} |
||||||
|
p += strlen(key->section)+1; |
||||||
|
|
||||||
|
// find key inside section
|
||||||
|
p = strstr(p, key->key); |
||||||
|
if (!p) { |
||||||
|
printf("Failed to find key %s/%s\n", key->section, key->key); |
||||||
|
return false; |
||||||
|
} |
||||||
|
|
||||||
|
p += strlen(key->key)+3; |
||||||
|
|
||||||
|
switch (key->type)
|
||||||
|
{ |
||||||
|
case DATA_FLOAT: |
||||||
|
*((float *)key->ptr) = atof(p); |
||||||
|
#ifdef DEBUG_INPUT_DATA |
||||||
|
printf("GOT %s/%s\n", key->section, key->key); |
||||||
|
#endif |
||||||
|
break; |
||||||
|
|
||||||
|
case DATA_DOUBLE: |
||||||
|
*((double *)key->ptr) = atof(p); |
||||||
|
#ifdef DEBUG_INPUT_DATA |
||||||
|
printf("GOT %s/%s\n", key->section, key->key); |
||||||
|
#endif |
||||||
|
break; |
||||||
|
|
||||||
|
case DATA_VECTOR4F: { |
||||||
|
VECTOR4F *v = (VECTOR4F *)key->ptr; |
||||||
|
if (sscanf(p, "[%f, %f, %f, %f]", &(v->w), &(v->x), &(v->y), &(v->z)) != 4) { |
||||||
|
printf("Failed to parse Vector3f for %s %s/%s\n",p, key->section, key->key); |
||||||
|
return false; |
||||||
|
} |
||||||
|
else |
||||||
|
{ |
||||||
|
#ifdef DEBUG_INPUT_DATA |
||||||
|
printf("GOT %s/%s\n[%f, %f, %f, %f]\n ", key->section, key->key,v->w,v->x,v->y,v->z); |
||||||
|
#endif |
||||||
|
} |
||||||
|
|
||||||
|
break; |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
return true; |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
void run () |
||||||
|
{ |
||||||
|
|
||||||
|
char send_buf[1000]; //1000 just a safe margin
|
||||||
|
char command_buffer[200]; |
||||||
|
fd_set rfds; |
||||||
|
while (wb_robot_step(timestep) != -1)
|
||||||
|
{ |
||||||
|
#ifdef DEBUG_USE_KB |
||||||
|
process_keyboard(); |
||||||
|
#endif |
||||||
|
|
||||||
|
if (fd == 0)
|
||||||
|
{ |
||||||
|
// if no socket wait till you get a socket
|
||||||
|
fd = socket_accept(sfd); |
||||||
|
if (fd > 0) |
||||||
|
socket_set_non_blocking(fd); |
||||||
|
else if (fd < 0) |
||||||
|
break; |
||||||
|
} |
||||||
|
|
||||||
|
getAllSensors ((char *)send_buf, northDirection, gyro,accelerometer,compass,gps, inertialUnit); |
||||||
|
|
||||||
|
#ifdef DEBUG_SENSORS |
||||||
|
printf("%s\n",send_buf); |
||||||
|
#endif |
||||||
|
|
||||||
|
if (write(fd,send_buf,strlen(send_buf)) <= 0) |
||||||
|
{ |
||||||
|
printf ("Send Data Error\n"); |
||||||
|
} |
||||||
|
|
||||||
|
if (fd)
|
||||||
|
{ |
||||||
|
FD_ZERO(&rfds); |
||||||
|
FD_SET(fd, &rfds); |
||||||
|
struct timeval tv; |
||||||
|
tv.tv_sec = 0.05; |
||||||
|
tv.tv_usec = 0; |
||||||
|
int number = select(fd + 1, &rfds, NULL, NULL, &tv); |
||||||
|
if (number != 0)
|
||||||
|
{
|
||||||
|
// there is a valid connection
|
||||||
|
|
||||||
|
int n = recv(fd, (char *)command_buffer, 200, 0); |
||||||
|
if (n < 0) { |
||||||
|
#ifdef _WIN32 |
||||||
|
int e = WSAGetLastError(); |
||||||
|
if (e == WSAECONNABORTED) |
||||||
|
fprintf(stderr, "Connection aborted.\n"); |
||||||
|
else if (e == WSAECONNRESET) |
||||||
|
fprintf(stderr, "Connection reset.\n"); |
||||||
|
else |
||||||
|
fprintf(stderr, "Error reading from socket: %d.\n", e); |
||||||
|
#else |
||||||
|
if (errno) |
||||||
|
fprintf(stderr, "Error reading from socket: %d.\n", errno); |
||||||
|
#endif |
||||||
|
break; |
||||||
|
} |
||||||
|
if (n==0) |
||||||
|
{ |
||||||
|
break; |
||||||
|
} |
||||||
|
if (command_buffer[0] == 'e') |
||||||
|
{ |
||||||
|
break; |
||||||
|
} |
||||||
|
if (n > 0) |
||||||
|
{ |
||||||
|
|
||||||
|
//printf("Received %d bytes:\n", n);
|
||||||
|
command_buffer[n] = 0; |
||||||
|
parse_controls (command_buffer); |
||||||
|
update_controls(); |
||||||
|
|
||||||
|
|
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
socket_cleanup(); |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
void initialize (int argc, char *argv[]) |
||||||
|
{ |
||||||
|
|
||||||
|
fd_set rfds; |
||||||
|
|
||||||
|
port = 5599; // default port
|
||||||
|
for (int i = 0; i < argc; ++i) |
||||||
|
{ |
||||||
|
if (strcmp (argv[i],"-p")==0) |
||||||
|
{ |
||||||
|
if (argc > i+1 ) |
||||||
|
{ |
||||||
|
port = atoi (argv[i+1]); |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
sfd = create_socket_server(port); |
||||||
|
|
||||||
|
/* necessary to initialize webots stuff */ |
||||||
|
wb_robot_init(); |
||||||
|
|
||||||
|
// Get WorldInfo Node.
|
||||||
|
WbNodeRef root, node; |
||||||
|
WbFieldRef children, field; |
||||||
|
int n, i; |
||||||
|
root = wb_supervisor_node_get_root(); |
||||||
|
children = wb_supervisor_node_get_field(root, "children"); |
||||||
|
n = wb_supervisor_field_get_count(children); |
||||||
|
printf("This world contains %d nodes:\n", n); |
||||||
|
for (i = 0; i < n; i++) { |
||||||
|
node = wb_supervisor_field_get_mf_node(children, i); |
||||||
|
if (wb_supervisor_node_get_type(node) == WB_NODE_WORLD_INFO) |
||||||
|
{ |
||||||
|
world_info = node;
|
||||||
|
break; |
||||||
|
} |
||||||
|
} |
||||||
|
|
||||||
|
printf("\n"); |
||||||
|
node = wb_supervisor_field_get_mf_node(children, 0); |
||||||
|
field = wb_supervisor_node_get_field(node, "northDirection"); |
||||||
|
northDirection = wb_supervisor_field_get_sf_vec3f(field); |
||||||
|
|
||||||
|
if (northDirection[0] == 1) |
||||||
|
{ |
||||||
|
printf ("Axis Default Directions"); |
||||||
|
} |
||||||
|
|
||||||
|
printf("WorldInfo.northDirection = %g %g %g\n\n", northDirection[0], northDirection[1], northDirection[2]); |
||||||
|
|
||||||
|
|
||||||
|
// get Self Node
|
||||||
|
self_node = wb_supervisor_node_get_self(); |
||||||
|
|
||||||
|
|
||||||
|
// keybaard
|
||||||
|
timestep = (int)wb_robot_get_basic_time_step(); |
||||||
|
wb_keyboard_enable(timestep); |
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// inertialUnit
|
||||||
|
inertialUnit = wb_robot_get_device("inertial_unit"); |
||||||
|
wb_inertial_unit_enable(inertialUnit, timestep); |
||||||
|
|
||||||
|
// gyro
|
||||||
|
gyro = wb_robot_get_device("gyro1"); |
||||||
|
wb_gyro_enable(gyro, timestep); |
||||||
|
|
||||||
|
// accelerometer
|
||||||
|
accelerometer = wb_robot_get_device("accelerometer1"); |
||||||
|
wb_accelerometer_enable(accelerometer, timestep); |
||||||
|
|
||||||
|
// compass
|
||||||
|
compass = wb_robot_get_device("compass1"); |
||||||
|
wb_compass_enable(compass, timestep); |
||||||
|
|
||||||
|
// gps
|
||||||
|
gps = wb_robot_get_device("gps1"); |
||||||
|
wb_gps_enable(gps, timestep); |
||||||
|
|
||||||
|
// camera
|
||||||
|
camera = wb_robot_get_device("camera1"); |
||||||
|
wb_camera_enable(camera, timestep); |
||||||
|
|
||||||
|
#ifdef WIND_SIMULATION |
||||||
|
// emitter
|
||||||
|
emitter = wb_robot_get_device("emitter_plugin"); |
||||||
|
#endif |
||||||
|
|
||||||
|
const char *MOTOR_NAMES[] = {"motor1", "motor2", "motor3"}; |
||||||
|
|
||||||
|
// get motor device tags
|
||||||
|
for (i = 0; i < MOTOR_NUM; i++) { |
||||||
|
motors[i] = wb_robot_get_device(MOTOR_NAMES[i]); |
||||||
|
v[i] = 0.0f; |
||||||
|
//assert(motors[i]);
|
||||||
|
} |
||||||
|
|
||||||
|
servo = wb_robot_get_device("servo_tail"); |
||||||
|
|
||||||
|
FD_ZERO(&rfds); |
||||||
|
FD_SET(sfd, &rfds); |
||||||
|
} |
||||||
|
/*
|
||||||
|
* This is the main program. |
||||||
|
* The arguments of the main function can be specified by the |
||||||
|
* "controllerArgs" field of the Robot node |
||||||
|
*/ |
||||||
|
int main(int argc, char **argv) |
||||||
|
{ |
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
initialize( argc, argv); |
||||||
|
|
||||||
|
/*
|
||||||
|
* You should declare here WbDeviceTag variables for storing |
||||||
|
* robot devices like this: |
||||||
|
* WbDeviceTag my_sensor = wb_robot_get_device("my_sensor"); |
||||||
|
* WbDeviceTag my_actuator = wb_robot_get_device("my_actuator"); |
||||||
|
*/ |
||||||
|
|
||||||
|
/* main loop
|
||||||
|
* Perform simulation steps of TIME_STEP milliseconds |
||||||
|
* and leave the loop when the simulation is over |
||||||
|
*/ |
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Read the sensors : |
||||||
|
* Enter here functions to read sensor data, like: |
||||||
|
* double val = wb_distance_sensor_get_value(my_sensor); |
||||||
|
*/ |
||||||
|
|
||||||
|
/* Process sensor data here */ |
||||||
|
|
||||||
|
/*
|
||||||
|
* Enter here functions to send actuator commands, like: |
||||||
|
* wb_differential_wheels_set_speed(100.0,100.0); |
||||||
|
*/ |
||||||
|
run(); |
||||||
|
|
||||||
|
|
||||||
|
/* Enter your cleanup code here */ |
||||||
|
|
||||||
|
/* This is necessary to cleanup webots resources */ |
||||||
|
wb_robot_cleanup(); |
||||||
|
|
||||||
|
return 0; |
||||||
|
} |
||||||
@ -0,0 +1,61 @@ |
|||||||
|
//#define DEBUG_MOTORS
|
||||||
|
// #define DEBUG_SENSORS
|
||||||
|
//#define DEBUG_USE_KB
|
||||||
|
//#define DEBUG_INPUT_DATA
|
||||||
|
// #define LINEAR_THRUST
|
||||||
|
//#define WIND_SIMULATION
|
||||||
|
|
||||||
|
|
||||||
|
#define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0])) |
||||||
|
|
||||||
|
|
||||||
|
enum data_type { |
||||||
|
DATA_FLOAT, |
||||||
|
DATA_DOUBLE, |
||||||
|
DATA_VECTOR4F, |
||||||
|
}; |
||||||
|
|
||||||
|
struct vector4f
|
||||||
|
{ |
||||||
|
float w,x,y,z; |
||||||
|
}; |
||||||
|
|
||||||
|
|
||||||
|
typedef struct vector4f VECTOR4F; |
||||||
|
|
||||||
|
struct { |
||||||
|
double timestamp; |
||||||
|
VECTOR4F motors; |
||||||
|
VECTOR4F wind;
|
||||||
|
/*
|
||||||
|
struct { |
||||||
|
float speed; // m/s
|
||||||
|
float direction; // degrees 0..360
|
||||||
|
float turbulence; |
||||||
|
float dir_z; //degrees -90..90
|
||||||
|
} wind; |
||||||
|
*/ |
||||||
|
} state, last_state; |
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// table to aid parsing of JSON sensor data
|
||||||
|
struct keytable { |
||||||
|
const char *section; |
||||||
|
const char *key; |
||||||
|
void *ptr; |
||||||
|
enum data_type type; |
||||||
|
|
||||||
|
} keytable[2] = { |
||||||
|
//{ "", "timestamp", &state.timestamp, DATA_DOUBLE },
|
||||||
|
{ "", "eng", &state.motors, DATA_VECTOR4F }, /*right,left,servo,back*/ |
||||||
|
{ "", "wnd", &state.wind, DATA_VECTOR4F } /*speed,x,y,z in Ardupilot AXIS*/
|
||||||
|
}; |
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
w: wind speed |
||||||
|
x , y, z: wind direction. |
||||||
|
*/ |
||||||
|
VECTOR4F __attribute__((packed, aligned(1))) wind_webots_axis; |
||||||
|
|
||||||
@ -0,0 +1,194 @@ |
|||||||
|
#include <stdio.h> |
||||||
|
#include <sys/time.h> |
||||||
|
#include <webots/supervisor.h> |
||||||
|
#include "sensors.h" |
||||||
|
|
||||||
|
#define M_PI 3.14159265358979323846 |
||||||
|
#define M_PI2 6.28318530718 |
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
https://discuss.ardupilot.org/t/copter-x-y-z-which-is-which/6823/2
|
||||||
|
|
||||||
|
Copy pasted what’s important: |
||||||
|
NED Coordinate System: |
||||||
|
|
||||||
|
The x axis is aligned with the vector to the north pole (tangent to meridians). |
||||||
|
The y axis points to the east side (tangent to parallels) |
||||||
|
The z axis points to the center of the earth |
||||||
|
There is also Body Fixed Frame: |
||||||
|
Body Fixed Frame (Attached to the aircraft) |
||||||
|
|
||||||
|
The x axis points in forward (defined by geometry and not by movement) direction. (= roll axis) |
||||||
|
The y axis points to the right (geometrically) (= pitch axis) |
||||||
|
The z axis points downwards (geometrically) (= yaw axis) |
||||||
|
In order to convert from Body Frame to NED what you need to call this function: |
||||||
|
|
||||||
|
copter.rotate_body_frame_to_NE(vel_vector.x, vel_vector.y); |
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
*/ |
||||||
|
/*
|
||||||
|
returns: "yaw":_6.594794831471518e-05,"pitch":_-0.0005172680830582976,"roll":_0.022908752784132957}} |
||||||
|
*/ |
||||||
|
void getInertia (const WbDeviceTag inertialUnit, const double *northDirection, char *buf) |
||||||
|
{ |
||||||
|
const double *inertial_directions = wb_inertial_unit_get_roll_pitch_yaw (inertialUnit); |
||||||
|
if (northDirection[0] == 1) |
||||||
|
{ |
||||||
|
sprintf(buf,"\"roll\": %f,\"pitch\": %f,\"yaw\": %f",inertial_directions[0], inertial_directions[1], inertial_directions[2]); |
||||||
|
} |
||||||
|
else |
||||||
|
{ |
||||||
|
sprintf(buf,"\"roll\": %f,\"pitch\": %f,\"yaw\": %f",inertial_directions[1], -inertial_directions[0], inertial_directions[2]); |
||||||
|
} |
||||||
|
|
||||||
|
return ; |
||||||
|
} |
||||||
|
|
||||||
|
/*
|
||||||
|
returns: "magnetic_field":_[23088.669921875,_3876.001220703125,_-53204.57421875] |
||||||
|
*/ |
||||||
|
void getCompass (const WbDeviceTag compass, const double *northDirection, char *buf) |
||||||
|
{ |
||||||
|
const double *north3D = wb_compass_get_values(compass); |
||||||
|
if (northDirection[0] == 1) |
||||||
|
{ |
||||||
|
sprintf(buf,"[%f, %f, %f]",north3D[0], north3D[2], north3D[1]); |
||||||
|
} |
||||||
|
else |
||||||
|
{ |
||||||
|
sprintf(buf,"[%f, %f, %f]",north3D[2], -north3D[0], north3D[1]); |
||||||
|
} |
||||||
|
|
||||||
|
return ; |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
returns: "vehicle.gps":{"timestamp":_1563301031.055164,"x":_5.5127296946011484e-05,"y":_-0.0010968948481604457,"z":_0.037179552018642426},
|
||||||
|
*/ |
||||||
|
void getGPS (const WbDeviceTag gps, const double *northDirection, char *buf) |
||||||
|
{ |
||||||
|
|
||||||
|
const double *north3D = wb_gps_get_values(gps); |
||||||
|
if (northDirection[0] == 1) |
||||||
|
{ |
||||||
|
sprintf(buf,"\"x\": %f,\"y\": %f,\"z\": %f", north3D[0], north3D[2], north3D[1]); |
||||||
|
} |
||||||
|
else |
||||||
|
{ |
||||||
|
sprintf(buf,"\"x\": %f,\"y\": %f,\"z\": %f", north3D[2], -north3D[0], north3D[1]); |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
return ; |
||||||
|
} |
||||||
|
|
||||||
|
/*
|
||||||
|
returns: "linear_acceleration": [0.005074390675872564, 0.22471477091312408, 9.80740737915039] |
||||||
|
*/ |
||||||
|
void getAcc (const WbDeviceTag accelerometer, const double *northDirection, char *buf) |
||||||
|
{ |
||||||
|
//SHOULD BE CORRECT
|
||||||
|
const double *a = wb_accelerometer_get_values(accelerometer); |
||||||
|
if (northDirection[0] == 1) |
||||||
|
{ |
||||||
|
sprintf(buf,"[%f, %f, %f]",a[0], a[2], a[1]); |
||||||
|
} |
||||||
|
else |
||||||
|
{ |
||||||
|
sprintf(buf,"[%f, %f, %f]",a[0], a[2], a[1]); |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
//sprintf(buf,"[0.0, 0.0, 0.0]");
|
||||||
|
|
||||||
|
return ; |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
returns: "angular_velocity": [-1.0255117643964695e-07, -8.877226775894087e-08, 2.087078510015772e-09] |
||||||
|
*/ |
||||||
|
void getGyro (const WbDeviceTag gyro, const double *northDirection, char *buf) |
||||||
|
{ |
||||||
|
|
||||||
|
const double *g = wb_gyro_get_values(gyro); |
||||||
|
if (northDirection[0] == 1) |
||||||
|
{ |
||||||
|
sprintf(buf,"[%f, %f, %f]",g[0], g[2], g[1]); |
||||||
|
} |
||||||
|
else |
||||||
|
{ |
||||||
|
sprintf(buf,"[%f, %f, %f]",g[0], g[2], g[1]); |
||||||
|
} |
||||||
|
|
||||||
|
return ; |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
void getLinearVelocity (WbNodeRef nodeRef, const double *northDirection, char * buf) |
||||||
|
{ |
||||||
|
linear_velocity = wb_supervisor_node_get_velocity (nodeRef); |
||||||
|
if (linear_velocity != NULL) |
||||||
|
{ |
||||||
|
if (northDirection[0] == 1) |
||||||
|
{ |
||||||
|
sprintf (buf,"[%f, %f, %f]", linear_velocity[0], linear_velocity[2], linear_velocity[1]); |
||||||
|
} |
||||||
|
else |
||||||
|
{ |
||||||
|
sprintf (buf,"[%f, %f, %f]", linear_velocity[2], -linear_velocity[0], linear_velocity[1]); |
||||||
|
}
|
||||||
|
} |
||||||
|
|
||||||
|
} |
||||||
|
|
||||||
|
void getAllSensors (char *buf, const double *northDirection, WbDeviceTag gyro, WbDeviceTag accelerometer, WbDeviceTag compass, const WbDeviceTag gps, WbDeviceTag inertial_unit) |
||||||
|
{ |
||||||
|
|
||||||
|
/*
|
||||||
|
{"timestamp": 1563544049.2840538,
|
||||||
|
"vehicle.imu": {"timestamp": 1563544049.2673872,
|
||||||
|
"angular_velocity": [-2.0466000023589004e-06, 3.1428675129063777e-07, -6.141597896913709e-09], |
||||||
|
"linear_acceleration": [0.005077465437352657, 0.22471386194229126, 9.807389259338379],
|
||||||
|
"magnetic_field": [23088.71484375, 3875.498046875, -53204.48046875]},
|
||||||
|
"vehicle.gps": { |
||||||
|
"timestamp": 1563544049.2673872,
|
||||||
|
"x": 4.985610576113686e-05, "y": -0.0010973707539960742, "z": 0.037179529666900635},
|
||||||
|
"vehicle.velocity": {"timestamp": 1563544049.2673872,
|
||||||
|
"linear_velocity": [-3.12359499377024e-10, -1.3824124955874595e-08, -6.033386625858839e-07], |
||||||
|
"angular_velocity": [-2.0466000023589004e-06, 3.1428675129063777e-07, -6.141597896913709e-09],
|
||||||
|
"world_linear_velocity": [0.0, 0.0, -6.034970851942489e-07]},
|
||||||
|
"vehicle.pose": {"timestamp": 1563544049.2673872,
|
||||||
|
"x": 4.985610576113686e-05, "y": -0.0010973707539960742, "z": 0.037179529666900635, "yaw": 8.899446402210742e-05, "pitch": -0.0005175824626348913, "roll": 0.022908702492713928} |
||||||
|
} |
||||||
|
*/ |
||||||
|
|
||||||
|
|
||||||
|
static char compass_buf [150]; |
||||||
|
static char acc_buf [150]; |
||||||
|
static char gyro_buf [150]; |
||||||
|
static char gps_buf [150]; |
||||||
|
static char inertial_buf [150]; |
||||||
|
static char linear_velocity_buf [150]; |
||||||
|
|
||||||
|
char szTime[21]; |
||||||
|
double time = wb_robot_get_time(); // current simulation time in [s]
|
||||||
|
sprintf(szTime,"%f", time); |
||||||
|
|
||||||
|
getGyro(gyro, northDirection, gyro_buf); |
||||||
|
getAcc(accelerometer, northDirection, acc_buf); |
||||||
|
getCompass(compass, northDirection, compass_buf); |
||||||
|
getGPS(gps, northDirection, gps_buf); |
||||||
|
getInertia (inertial_unit, northDirection, inertial_buf); |
||||||
|
getLinearVelocity(self_node, northDirection, linear_velocity_buf); |
||||||
|
|
||||||
|
sprintf (buf,"{\"ts\": %s,\"vehicle.imu\": {\"av\": %s,\"la\": %s,\"mf\": %s,\"vehicle.gps\":{%s},\"vehicle.velocity\":{\"wlv\": %s},\"vehicle.pose\":{%s,%s}}\r\n" |
||||||
|
, szTime, gyro_buf, acc_buf, compass_buf, gps_buf, linear_velocity_buf, gps_buf, inertial_buf ); |
||||||
|
|
||||||
|
} |
||||||
@ -0,0 +1,23 @@ |
|||||||
|
|
||||||
|
|
||||||
|
#include <webots/robot.h> |
||||||
|
#include <webots/keyboard.h> |
||||||
|
#include <webots/compass.h> |
||||||
|
#include <webots/accelerometer.h> |
||||||
|
#include <webots/inertial_unit.h> |
||||||
|
#include <webots/gps.h> |
||||||
|
#include <webots/gyro.h> |
||||||
|
#include <webots/motor.h> |
||||||
|
#include <webots/camera.h> |
||||||
|
|
||||||
|
|
||||||
|
WbNodeRef self_node; |
||||||
|
double *linear_velocity; |
||||||
|
|
||||||
|
void getInertia (const WbDeviceTag inertialUnit, const double *northDirection, char *buf); |
||||||
|
void getLinearVelocity (WbNodeRef nodeRef, const double *northDirection, char * buf); |
||||||
|
void getCompass (const WbDeviceTag compass, const double *northDirection, char *buf); |
||||||
|
void getAcc (const WbDeviceTag accelerometer, const double *northDirection, char *buf); |
||||||
|
void getGyro (const WbDeviceTag gyro, const double *northDirection, char *buf); |
||||||
|
void getGPS (const WbDeviceTag gps, const double *northDirection, char *buf); |
||||||
|
void getAllSensors (char *buf, const double *northDirection, WbDeviceTag gyro, WbDeviceTag accelerometer, WbDeviceTag compass, const WbDeviceTag gps, WbDeviceTag inertial_unit); |
||||||
@ -0,0 +1,112 @@ |
|||||||
|
|
||||||
|
#include "sockets.h" |
||||||
|
|
||||||
|
|
||||||
|
bool socket_init() { |
||||||
|
#ifdef _WIN32 /* initialize the socket API */ |
||||||
|
WSADATA info; |
||||||
|
if (WSAStartup(MAKEWORD(1, 1), &info) != 0) { |
||||||
|
fprintf(stderr, "Cannot initialize Winsock.\n"); |
||||||
|
return false; |
||||||
|
} |
||||||
|
#endif |
||||||
|
return true; |
||||||
|
} |
||||||
|
|
||||||
|
bool socket_set_non_blocking(int fd) { |
||||||
|
if (fd < 0) |
||||||
|
return false; |
||||||
|
#ifdef _WIN32 |
||||||
|
unsigned long mode = 1; |
||||||
|
return (ioctlsocket(fd, FIONBIO, &mode) == 0) ? true : false; |
||||||
|
#else |
||||||
|
int flags = fcntl(fd, F_GETFL, 0) | O_NONBLOCK; |
||||||
|
return (fcntl(fd, F_SETFL, flags) == 0) ? true : false; |
||||||
|
#endif |
||||||
|
} |
||||||
|
|
||||||
|
int socket_accept(int server_fd) { |
||||||
|
int cfd; |
||||||
|
struct sockaddr_in client; |
||||||
|
struct hostent *client_info; |
||||||
|
#ifndef _WIN32 |
||||||
|
socklen_t asize; |
||||||
|
#else |
||||||
|
int asize; |
||||||
|
#endif |
||||||
|
asize = sizeof(struct sockaddr_in); |
||||||
|
cfd = accept(server_fd, (struct sockaddr *)&client, &asize); |
||||||
|
if (cfd == -1) { |
||||||
|
#ifdef _WIN32 |
||||||
|
int e = WSAGetLastError(); |
||||||
|
if (e == WSAEWOULDBLOCK) |
||||||
|
return 0; |
||||||
|
fprintf(stderr, "Accept error: %d.\n", e); |
||||||
|
#else |
||||||
|
if (errno == EWOULDBLOCK) |
||||||
|
return 0; |
||||||
|
fprintf(stderr, "Accept error: %d.\n", errno); |
||||||
|
#endif |
||||||
|
return -1; |
||||||
|
} |
||||||
|
client_info = gethostbyname((char *)inet_ntoa(client.sin_addr)); |
||||||
|
printf("Accepted connection from: %s.\n", client_info->h_name); |
||||||
|
return cfd; |
||||||
|
} |
||||||
|
|
||||||
|
bool socket_close(int fd) { |
||||||
|
#ifdef _WIN32 |
||||||
|
return (closesocket(fd) == 0) ? true : false; |
||||||
|
#else |
||||||
|
return (close(fd) == 0) ? true : false; |
||||||
|
#endif |
||||||
|
} |
||||||
|
|
||||||
|
bool socket_cleanup() { |
||||||
|
#ifdef _WIN32 |
||||||
|
return (WSACleanup() == 0) ? true : false; |
||||||
|
#else |
||||||
|
return true; |
||||||
|
#endif |
||||||
|
} |
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
|
Creates a socket and bind it to port. |
||||||
|
*/ |
||||||
|
int create_socket_server(int port) { |
||||||
|
int sfd, rc; |
||||||
|
struct sockaddr_in address; |
||||||
|
if (!socket_init()) |
||||||
|
{ |
||||||
|
fprintf (stderr, "socket_init failed"); |
||||||
|
return -1; |
||||||
|
} |
||||||
|
sfd = socket(AF_INET, SOCK_STREAM, 0); |
||||||
|
if (sfd == -1) { |
||||||
|
fprintf(stderr, "Cannot create socket.\n"); |
||||||
|
return -1; |
||||||
|
} |
||||||
|
int one = 1; |
||||||
|
setsockopt(sfd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); |
||||||
|
memset(&address, 0, sizeof(struct sockaddr_in)); |
||||||
|
address.sin_family = AF_INET; |
||||||
|
address.sin_port = htons((unsigned short)port); |
||||||
|
address.sin_addr.s_addr = INADDR_ANY; |
||||||
|
rc = bind(sfd, (struct sockaddr *)&address, sizeof(struct sockaddr)); |
||||||
|
if (rc == -1) { |
||||||
|
fprintf(stderr, "Cannot bind port %d.\n", port); |
||||||
|
socket_close(sfd); |
||||||
|
return -1; |
||||||
|
} |
||||||
|
if (listen(sfd, 1) == -1) { |
||||||
|
fprintf(stderr, "Cannot listen for connections.\n"); |
||||||
|
socket_close(sfd); |
||||||
|
return -1; |
||||||
|
} |
||||||
|
|
||||||
|
printf ("socket initialized at port %d.\n", port); |
||||||
|
return sfd; |
||||||
|
} |
||||||
@ -0,0 +1,28 @@ |
|||||||
|
|
||||||
|
#include <stdbool.h> |
||||||
|
#include <stdio.h> |
||||||
|
#include <string.h> |
||||||
|
#include <sys/types.h> |
||||||
|
|
||||||
|
|
||||||
|
#ifdef _WIN32 |
||||||
|
#include <winsock.h> |
||||||
|
#else |
||||||
|
#include <arpa/inet.h> /* definition of inet_ntoa */ |
||||||
|
#include <errno.h> |
||||||
|
#include <fcntl.h> |
||||||
|
#include <netdb.h> /* definition of gethostbyname */ |
||||||
|
#include <netinet/in.h> /* definition of struct sockaddr_in */ |
||||||
|
#include <stdlib.h> |
||||||
|
#include <sys/socket.h> |
||||||
|
#include <sys/time.h> |
||||||
|
#include <unistd.h> /* definition of close */ |
||||||
|
#endif |
||||||
|
|
||||||
|
int create_socket_server(int port); |
||||||
|
bool socket_cleanup(); |
||||||
|
int socket_accept(int server_fd); |
||||||
|
bool socket_set_non_blocking(int fd); |
||||||
|
|
||||||
|
int fd; |
||||||
|
int sfd; |
||||||
@ -0,0 +1,5 @@ |
|||||||
|
#!/bin/bash |
||||||
|
|
||||||
|
# assume we start the script from the root directory |
||||||
|
ROOTDIR=$PWD |
||||||
|
$PWD/Tools/autotest/sim_vehicle.py -v ArduCopter -w --model webots-tri --add-param-file=libraries/SITL/examples/Webots/tricopter.parm |
||||||
@ -0,0 +1,13 @@ |
|||||||
|
FRAME_CLASS 7.000000 |
||||||
|
FRAME_TYPE 0.000000 |
||||||
|
ATC_ANG_PIT_P 1.0 |
||||||
|
ATC_ANG_RLL_P 1.0 |
||||||
|
ATC_ANG_YAW_P 0.5 |
||||||
|
ATC_RAT_PIT_P 3.5 |
||||||
|
ATC_RAT_RLL_P 3.5 |
||||||
|
ATC_RAT_YAW_P 1.5 |
||||||
|
ATC_RAT_YAW_I 0.03 |
||||||
|
ATC_RAT_YAW_IMAX 0.50000 |
||||||
|
SIM_WIND_DIR 90.0 |
||||||
|
SIM_WIND_SPD 0.0 |
||||||
|
SIM_WIND_TURB 0.0 |
||||||
@ -0,0 +1,286 @@ |
|||||||
|
#VRML_SIM R2019b utf8 |
||||||
|
WorldInfo { |
||||||
|
gravity 0 -9.80665 0 |
||||||
|
physics "sitl_physics_env" |
||||||
|
basicTimeStep 2 |
||||||
|
FPS 25 |
||||||
|
optimalThreadCount 4 |
||||||
|
randomSeed 52 |
||||||
|
} |
||||||
|
DogHouse { |
||||||
|
translation 34.82 0.76 -24.56 |
||||||
|
name "dog house(1)" |
||||||
|
} |
||||||
|
DogHouse { |
||||||
|
translation 161.819 0.75 -152.174 |
||||||
|
name "dog house(2)" |
||||||
|
} |
||||||
|
DogHouse { |
||||||
|
translation 185.42 0.77 48.97 |
||||||
|
name "dog house(5)" |
||||||
|
} |
||||||
|
Viewpoint { |
||||||
|
orientation 0.9997750421775041 -0.014997750480821456 -0.01499775048082146 4.712163997257285 |
||||||
|
position 0.17712971811531414 14.83048200793912 -1.4201693307676047 |
||||||
|
follow "tricopter" |
||||||
|
} |
||||||
|
Background { |
||||||
|
skyColor [ |
||||||
|
0.15 0.5 1 |
||||||
|
] |
||||||
|
cubemap Cubemap { |
||||||
|
} |
||||||
|
} |
||||||
|
Solid { |
||||||
|
translation 36.93 0.77 -37.93 |
||||||
|
children [ |
||||||
|
HouseWithGarage { |
||||||
|
} |
||||||
|
] |
||||||
|
} |
||||||
|
Solid { |
||||||
|
translation 192.76999999999998 0 64.98 |
||||||
|
rotation 0 1 0 -1.5707963071795863 |
||||||
|
children [ |
||||||
|
HouseWithGarage { |
||||||
|
} |
||||||
|
] |
||||||
|
name "solid(1)" |
||||||
|
} |
||||||
|
DEF DEF_VEHICLE Robot { |
||||||
|
translation -8.233889875751989e-05 0.666500515499142 -1.3598750857814472 |
||||||
|
rotation 0.00514799893982893 0.9999767940663002 0.004461999081102697 0.261804 |
||||||
|
children [ |
||||||
|
Compass { |
||||||
|
name "compass1" |
||||||
|
} |
||||||
|
Camera { |
||||||
|
translation 0 0.25 0 |
||||||
|
name "camera1" |
||||||
|
} |
||||||
|
Transform { |
||||||
|
translation -0.34 0 0 |
||||||
|
rotation 0 1 0 1.5707959999999999 |
||||||
|
children [ |
||||||
|
HingeJoint { |
||||||
|
jointParameters HingeJointParameters { |
||||||
|
position -9.388038782122357e-12 |
||||||
|
axis 0 0 1 |
||||||
|
} |
||||||
|
device [ |
||||||
|
RotationalMotor { |
||||||
|
name "servo_tail" |
||||||
|
maxVelocity 50000 |
||||||
|
maxTorque 1000 |
||||||
|
} |
||||||
|
] |
||||||
|
endPoint Solid { |
||||||
|
translation -4.884982810326628e-15 -4.1022629410960365e-12 1.8091922030330322e-13 |
||||||
|
rotation 2.3470124341273664e-11 1 -3.708275057969111e-10 1.5707963071795863 |
||||||
|
children [ |
||||||
|
Propeller { |
||||||
|
shaftAxis 0 1 0 |
||||||
|
thrustConstants 11.44 0 |
||||||
|
torqueConstants 1e-05 0 |
||||||
|
device RotationalMotor { |
||||||
|
name "motor3" |
||||||
|
controlPID 10.001 0 0 |
||||||
|
maxVelocity 1000 |
||||||
|
} |
||||||
|
fastHelix Solid { |
||||||
|
children [ |
||||||
|
Shape { |
||||||
|
appearance Appearance { |
||||||
|
material Material { |
||||||
|
diffuseColor 1 0 0.1 |
||||||
|
} |
||||||
|
} |
||||||
|
geometry Cylinder { |
||||||
|
height 0.002 |
||||||
|
radius 0.02 |
||||||
|
} |
||||||
|
} |
||||||
|
] |
||||||
|
} |
||||||
|
slowHelix Solid { |
||||||
|
rotation 0 1 0 2.238367478129037 |
||||||
|
children [ |
||||||
|
Shape { |
||||||
|
appearance Appearance { |
||||||
|
material Material { |
||||||
|
diffuseColor 0 1 0.09999999999999999 |
||||||
|
} |
||||||
|
} |
||||||
|
geometry Cylinder { |
||||||
|
height 0.002 |
||||||
|
radius 0.02 |
||||||
|
} |
||||||
|
} |
||||||
|
] |
||||||
|
} |
||||||
|
} |
||||||
|
] |
||||||
|
name "solid(1)" |
||||||
|
boundingObject Box { |
||||||
|
size 0.01 0.01 0.01 |
||||||
|
} |
||||||
|
physics Physics { |
||||||
|
mass 0.001 |
||||||
|
} |
||||||
|
} |
||||||
|
} |
||||||
|
] |
||||||
|
} |
||||||
|
Transform { |
||||||
|
translation 0.17 0 0.3 |
||||||
|
children [ |
||||||
|
Propeller { |
||||||
|
shaftAxis 0 -1 0 |
||||||
|
thrustConstants -11.443 0 |
||||||
|
torqueConstants 1e-05 0 |
||||||
|
device RotationalMotor { |
||||||
|
name "motor1" |
||||||
|
controlPID 10.001 0 0 |
||||||
|
maxVelocity 1000 |
||||||
|
} |
||||||
|
fastHelix Solid { |
||||||
|
children [ |
||||||
|
Shape { |
||||||
|
appearance Appearance { |
||||||
|
material Material { |
||||||
|
diffuseColor 1 0 0.1 |
||||||
|
} |
||||||
|
} |
||||||
|
geometry Cylinder { |
||||||
|
height 0.002 |
||||||
|
radius 0.02 |
||||||
|
} |
||||||
|
} |
||||||
|
] |
||||||
|
} |
||||||
|
slowHelix Solid { |
||||||
|
rotation 0 1 0 0.012993 |
||||||
|
children [ |
||||||
|
Shape { |
||||||
|
appearance Appearance { |
||||||
|
material Material { |
||||||
|
diffuseColor 1 0 0.1 |
||||||
|
} |
||||||
|
} |
||||||
|
geometry Cylinder { |
||||||
|
height 0.002 |
||||||
|
radius 0.02 |
||||||
|
} |
||||||
|
} |
||||||
|
] |
||||||
|
} |
||||||
|
} |
||||||
|
] |
||||||
|
} |
||||||
|
Transform { |
||||||
|
translation 0.16 0 -0.3 |
||||||
|
children [ |
||||||
|
Propeller { |
||||||
|
shaftAxis 0 1 0 |
||||||
|
thrustConstants 11.443 0 |
||||||
|
torqueConstants 1e-05 0 |
||||||
|
device RotationalMotor { |
||||||
|
name "motor2" |
||||||
|
controlPID 10.001 0 0 |
||||||
|
maxVelocity 1000 |
||||||
|
} |
||||||
|
fastHelix Solid { |
||||||
|
children [ |
||||||
|
Shape { |
||||||
|
appearance Appearance { |
||||||
|
material Material { |
||||||
|
diffuseColor 1 0 0.1 |
||||||
|
} |
||||||
|
} |
||||||
|
geometry Cylinder { |
||||||
|
height 0.002 |
||||||
|
radius 0.02 |
||||||
|
} |
||||||
|
} |
||||||
|
] |
||||||
|
} |
||||||
|
slowHelix Solid { |
||||||
|
rotation 0 1 0 0.012993 |
||||||
|
children [ |
||||||
|
Shape { |
||||||
|
appearance Appearance { |
||||||
|
material Material { |
||||||
|
diffuseColor 1 0 0.1 |
||||||
|
} |
||||||
|
} |
||||||
|
geometry Cylinder { |
||||||
|
height 0.002 |
||||||
|
radius 0.02 |
||||||
|
} |
||||||
|
} |
||||||
|
] |
||||||
|
} |
||||||
|
} |
||||||
|
] |
||||||
|
} |
||||||
|
Emitter { |
||||||
|
rotation 0 1 0 -1.5707963071795863 |
||||||
|
name "emitter_plugin" |
||||||
|
} |
||||||
|
InertialUnit { |
||||||
|
name "inertial_unit" |
||||||
|
} |
||||||
|
Gyro { |
||||||
|
name "gyro1" |
||||||
|
} |
||||||
|
Accelerometer { |
||||||
|
name "accelerometer1" |
||||||
|
} |
||||||
|
GPS { |
||||||
|
name "gps1" |
||||||
|
} |
||||||
|
Solid { |
||||||
|
children [ |
||||||
|
Shape { |
||||||
|
appearance Appearance { |
||||||
|
material Material { |
||||||
|
} |
||||||
|
} |
||||||
|
geometry Box { |
||||||
|
size 0.1 0.1 0.1 |
||||||
|
} |
||||||
|
} |
||||||
|
] |
||||||
|
boundingObject Box { |
||||||
|
size 0.1 0.1 0.1 |
||||||
|
} |
||||||
|
physics Physics { |
||||||
|
mass 0.6 |
||||||
|
} |
||||||
|
} |
||||||
|
] |
||||||
|
name "tricopter" |
||||||
|
physics Physics { |
||||||
|
mass 0.001 |
||||||
|
} |
||||||
|
controller "ardupilot_SITL_TRICOPTER" |
||||||
|
supervisor TRUE |
||||||
|
} |
||||||
|
DirectionalLight { |
||||||
|
direction 0 -1 0 |
||||||
|
} |
||||||
|
UnevenTerrain { |
||||||
|
size 500 1 500 |
||||||
|
} |
||||||
|
HouseWithGarage { |
||||||
|
translation 174.25 1.88 -157.5 |
||||||
|
rotation 0 1 0 -1.5707963071795863 |
||||||
|
} |
||||||
|
AdvertisingBoard { |
||||||
|
translation 0 2.35 -5.71 |
||||||
|
} |
||||||
|
AdvertisingBoard { |
||||||
|
translation 84.03999999999999 2.35 -5.81 |
||||||
|
rotation 0 1 0 -1.5707963071795863 |
||||||
|
name "advertising board(1)" |
||||||
|
} |
||||||
Loading…
Reference in new issue