mirror of https://github.com/ArduPilot/ardupilot
SITL: Add Webots 2023a support and examples
The various C Webots controllers are replaced by a single Python controller - More readable (in my opinion) - Does not require compilation - Easily modifiable to run user code - Can be blackboxed and configured via arguments when designing a robot model - Optionally provides the ability to stream camera images over TCP - Generalizable to copters and rovers (and probably more) - Supports multi-vehicle simulation (including of multiple types) - Requires no non-standard libraries (neither does current) Higher fidelity example worlds - Iris quadcopter demo world similar to gazebo - Crazyflie quadcopter demo world (crazyflie models baked into webots) - Pioneer3at rover demo world (pioneer models baked into webots)
This commit is contained in:
Ian
Peter Barker
parent
773129bab4
commit
d358ca1b32
|
|
@ -0,0 +1,172 @@
|
|||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
simulator connection for Webots 2023a
|
||||
*/
|
||||
|
||||
#include "SIM_Webots_Python.h"
|
||||
|
||||
#if HAL_SIM_WEBOTSPYTHON_ENABLED
|
||||
|
||||
#include <stdio.h>
|
||||
#include <errno.h>
|
||||
|
||||
namespace SITL {
|
||||
|
||||
WebotsPython::WebotsPython(const char *frame_str) :
|
||||
Aircraft(frame_str),
|
||||
last_timestamp(0),
|
||||
socket_sitl{true}{
|
||||
|
||||
// disable time sync and sensor smoothing to allow for faster than realtime simulation
|
||||
use_time_sync = false;
|
||||
use_smoothing = false;
|
||||
|
||||
printf("Starting SITL Webots\n");
|
||||
}
|
||||
|
||||
/*
|
||||
Create and set in/out socket
|
||||
*/
|
||||
void WebotsPython::set_interface_ports(const char* address, const int port_in, const int port_out)
|
||||
{
|
||||
// try to bind to a specific port so that if we restart ArduPilot
|
||||
// Webots keeps sending us packets. Not strictly necessary but
|
||||
// useful for debugging
|
||||
if (!socket_sitl.bind("0.0.0.0", port_in)) {
|
||||
fprintf(stderr, "SITL: socket in bind failed on sim in : %d - %s\n", port_in, strerror(errno));
|
||||
fprintf(stderr, "Aborting launch...\n");
|
||||
exit(1);
|
||||
}
|
||||
printf("Bind %s:%d for SITL in\n", "127.0.0.1", port_in);
|
||||
socket_sitl.reuseaddress();
|
||||
socket_sitl.set_blocking(false);
|
||||
|
||||
_webots_address = address;
|
||||
_webots_port = port_out;
|
||||
printf("Setting Webots interface to %s:%d \n", _webots_address, _webots_port);
|
||||
}
|
||||
|
||||
/*
|
||||
Decode and send SITL outputs to FDM aka Webots (scaled to be 0-1 instead of 1000-2000)
|
||||
*/
|
||||
void WebotsPython::send_servos(const struct sitl_input &input)
|
||||
{
|
||||
servo_packet pkt;
|
||||
for (unsigned i = 0; i < 16; ++i){
|
||||
pkt.motor_speed[i] = (input.servos[i]-1000) / 1000.0f;
|
||||
}
|
||||
socket_sitl.sendto(&pkt, sizeof(pkt), _webots_address, _webots_port);
|
||||
}
|
||||
|
||||
/*
|
||||
Receive sensor data from Webots (the Flight Dynamics Model)
|
||||
*/
|
||||
void WebotsPython::recv_fdm(const struct sitl_input &input)
|
||||
{
|
||||
fdm_packet pkt;
|
||||
|
||||
/*
|
||||
we re-send the servo packet every 0.1 seconds until we get a
|
||||
reply. This allows us to cope with some packet loss to the FDM
|
||||
*/
|
||||
while (socket_sitl.recv(&pkt, sizeof(pkt), 100) != sizeof(pkt)) {
|
||||
send_servos(input);
|
||||
// Reset the timestamp after a long disconnection, also catch webots reset
|
||||
if (get_wall_time_us() > last_wall_time_us + WEBOTS_TIMEOUT_US) {
|
||||
last_timestamp = 0;
|
||||
}
|
||||
}
|
||||
|
||||
const double deltat = pkt.timestamp - last_timestamp; // in seconds
|
||||
if (deltat < 0) { // don't use old packet
|
||||
time_now_us += 1;
|
||||
return;
|
||||
}
|
||||
// get imu stuff
|
||||
accel_body = Vector3f(static_cast<float>(pkt.imu_linear_acceleration_xyz[0]),
|
||||
static_cast<float>(pkt.imu_linear_acceleration_xyz[1]),
|
||||
static_cast<float>(pkt.imu_linear_acceleration_xyz[2]));
|
||||
|
||||
gyro = Vector3f(static_cast<float>(pkt.imu_angular_velocity_rpy[0]),
|
||||
static_cast<float>(pkt.imu_angular_velocity_rpy[1]),
|
||||
static_cast<float>(pkt.imu_angular_velocity_rpy[2]));
|
||||
|
||||
// compute dcm from imu orientation
|
||||
dcm.from_euler(static_cast<float>(pkt.imu_orientation_rpy[0]),
|
||||
static_cast<float>(pkt.imu_orientation_rpy[1]),
|
||||
static_cast<float>(pkt.imu_orientation_rpy[2]));
|
||||
|
||||
velocity_ef = Vector3f(static_cast<float>(pkt.velocity_xyz[0]),
|
||||
static_cast<float>(pkt.velocity_xyz[1]),
|
||||
static_cast<float>(pkt.velocity_xyz[2]));
|
||||
|
||||
position = Vector3d(pkt.position_xyz[0],
|
||||
pkt.position_xyz[1],
|
||||
pkt.position_xyz[2]);
|
||||
position.xy() += origin.get_distance_NE_double(home);
|
||||
|
||||
// auto-adjust to simulation frame rate
|
||||
time_now_us += static_cast<uint64_t>(deltat * 1.0e6);
|
||||
|
||||
if (deltat < 0.01 && deltat > 0) {
|
||||
adjust_frame_time(static_cast<float>(1.0/deltat));
|
||||
}
|
||||
last_timestamp = pkt.timestamp;
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
Drain remaining data on the socket to prevent phase lag
|
||||
*/
|
||||
void WebotsPython::drain_sockets()
|
||||
{
|
||||
const uint16_t buflen = 1024;
|
||||
char buf[buflen];
|
||||
ssize_t received;
|
||||
errno = 0;
|
||||
do {
|
||||
received = socket_sitl.recv(buf, buflen, 0);
|
||||
if (received < 0) {
|
||||
if (errno != EAGAIN && errno != EWOULDBLOCK && errno != 0) {
|
||||
fprintf(stderr, "error recv on socket in: %s \n",
|
||||
strerror(errno));
|
||||
}
|
||||
} else {
|
||||
// fprintf(stderr, "received from control socket: %s\n", buf);
|
||||
}
|
||||
} while (received > 0);
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
Update the Webots simulation by one time step
|
||||
*/
|
||||
void WebotsPython::update(const struct sitl_input &input)
|
||||
{
|
||||
send_servos(input);
|
||||
recv_fdm(input);
|
||||
update_position();
|
||||
|
||||
time_advance();
|
||||
// update magnetic field
|
||||
update_mag_field_bf();
|
||||
drain_sockets();
|
||||
}
|
||||
|
||||
} // namespace SITL
|
||||
|
||||
|
||||
#endif // HAL_SIM_WEBOTSPYTHON_ENABLED
|
||||
|
|
@ -0,0 +1,90 @@
|
|||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
GNU General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
/*
|
||||
simulator connection for Webots 2023a
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <AP_HAL/AP_HAL_Boards.h>
|
||||
|
||||
#ifndef HAL_SIM_WEBOTSPYTHON_ENABLED
|
||||
#define HAL_SIM_WEBOTSPYTHON_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL)
|
||||
#endif
|
||||
|
||||
#if HAL_SIM_WEBOTSPYTHON_ENABLED
|
||||
|
||||
#include "SIM_Aircraft.h"
|
||||
#include <AP_HAL/utility/Socket.h>
|
||||
|
||||
namespace SITL {
|
||||
|
||||
/*
|
||||
WebotsPython simulator
|
||||
*/
|
||||
class WebotsPython : public Aircraft {
|
||||
public:
|
||||
WebotsPython(const char *frame_str);
|
||||
|
||||
/* update model by one time step */
|
||||
void update(const struct sitl_input &input) override;
|
||||
|
||||
/* static object creator */
|
||||
static Aircraft *create(const char *frame_str) {
|
||||
return new WebotsPython(frame_str);
|
||||
}
|
||||
|
||||
/* Create and set in/out socket for Webots simulator */
|
||||
void set_interface_ports(const char* address, const int port_in, const int port_out) override;
|
||||
|
||||
private:
|
||||
/// @brief packet to send to Webots
|
||||
/// python struct code 'f'*16
|
||||
struct servo_packet {
|
||||
float motor_speed[16];
|
||||
};
|
||||
|
||||
/// @brief packet from Webots (the Flight Dynamics Model)
|
||||
/// python struct code 'd'*(1+3+3+3+3+3)
|
||||
struct fdm_packet {
|
||||
double timestamp; // in seconds
|
||||
double imu_angular_velocity_rpy[3];
|
||||
double imu_linear_acceleration_xyz[3];
|
||||
double imu_orientation_rpy[3];
|
||||
double velocity_xyz[3];
|
||||
double position_xyz[3];
|
||||
};
|
||||
|
||||
/// @brief receive sensor packet from Webots
|
||||
void recv_fdm(const struct sitl_input &input);
|
||||
|
||||
/// @brief send servo packet to Webots
|
||||
void send_servos(const struct sitl_input &input);
|
||||
|
||||
/// @brief drain all pending packets from Webots
|
||||
void drain_sockets();
|
||||
|
||||
double last_timestamp;
|
||||
|
||||
SocketAPM socket_sitl;
|
||||
const char* _webots_address = "127.0.0.1";
|
||||
int _webots_port = 9002;
|
||||
static const uint64_t WEBOTS_TIMEOUT_US = 5000000;
|
||||
};
|
||||
|
||||
} // namespace SITL
|
||||
|
||||
|
||||
#endif // HAL_SIM_WEBOTSPYTHON_ENABLED
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
*.wbproj
|
||||
__pycache__
|
||||
worlds/.*
|
||||
|
|
@ -0,0 +1,127 @@
|
|||
#!/usr/bin/env python3
|
||||
'''
|
||||
General ardupilot vehicle controller for Webots 2023a
|
||||
|
||||
AP_FLAKE8_CLEAN
|
||||
'''
|
||||
|
||||
|
||||
import time
|
||||
import argparse
|
||||
from webots_vehicle import WebotsArduVehicle
|
||||
|
||||
|
||||
def get_args():
|
||||
parser = argparse.ArgumentParser()
|
||||
|
||||
parser.add_argument("--motors", "-m",
|
||||
type=str,
|
||||
default="m1_motor, m2_motor, m3_motor, m4_motor",
|
||||
help="Comma spaced list of motor names in ardupilot numerical order (ex --motors \"m1,m2,m3, m4\")")
|
||||
parser.add_argument("--reversed-motors", "-r",
|
||||
type=str,
|
||||
default=None,
|
||||
help="Comma spaced list of motors to reverse (starting from 1, in ardupilot order)")
|
||||
parser.add_argument("--bidirectional-motors",
|
||||
type=bool,
|
||||
default=False,
|
||||
help="If the motors are bidirectional (as is the case for Rovers usually)")
|
||||
parser.add_argument("--uses-propellers",
|
||||
type=bool,
|
||||
default=True,
|
||||
help="Whether the vehicle uses propellers. This is important as we need to linearize thrust if so")
|
||||
parser.add_argument("--motor-cap",
|
||||
type=float,
|
||||
default=float('inf'),
|
||||
help="Motor velocity cap. This is useful for the crazyflie which default has way too much power")
|
||||
|
||||
parser.add_argument("--accel",
|
||||
type=str,
|
||||
default="accelerometer",
|
||||
help="Webots accelerometer name")
|
||||
parser.add_argument("--imu",
|
||||
type=str,
|
||||
default="inertial unit",
|
||||
help="Webots IMU name")
|
||||
parser.add_argument("--gyro",
|
||||
type=str,
|
||||
default="gyro",
|
||||
help="Webots gyro name")
|
||||
parser.add_argument("--gps",
|
||||
type=str,
|
||||
default="gps",
|
||||
help="Webots GPS name")
|
||||
|
||||
parser.add_argument("--camera",
|
||||
type=str,
|
||||
default=None,
|
||||
help="Webots Camera name (optional)")
|
||||
parser.add_argument("--camera-fps",
|
||||
type=int,
|
||||
default=10,
|
||||
help="Camera FPS. Note lower FPS is faster")
|
||||
parser.add_argument("--camera-port",
|
||||
type=int,
|
||||
default=None,
|
||||
help="Port to stream grayscale camera images to. "
|
||||
"If no port is supplied the camera will not be streamed.")
|
||||
|
||||
parser.add_argument("--rangefinder",
|
||||
type=str,
|
||||
default=None,
|
||||
help="Webots RangeFinder name (optional)")
|
||||
parser.add_argument("--rangefinder-fps",
|
||||
type=int,
|
||||
default=10,
|
||||
help="rangefinder FPS. Note lower FPS is faster")
|
||||
parser.add_argument("--rangefinder-port",
|
||||
type=int,
|
||||
default=None,
|
||||
help="Port to stream grayscale rangefinder images to. "
|
||||
"If no port is supplied the rangefinder will not be streamed.")
|
||||
|
||||
parser.add_argument("--instance", "-i",
|
||||
type=int,
|
||||
default=0,
|
||||
help="Drone instance to match the SITL. This allows multiple vehicles")
|
||||
parser.add_argument("--sitl-address",
|
||||
type=str,
|
||||
default="127.0.0.1",
|
||||
help="IP address of the SITL (useful with WSL2 eg \"172.24.220.98\")")
|
||||
|
||||
return parser.parse_args()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
args = get_args()
|
||||
|
||||
# parse string arguments into lists
|
||||
motors = [x.strip() for x in args.motors.split(',')]
|
||||
if args.reversed_motors:
|
||||
reversed_motors = [int(x) for x in args.reversed_motors.split(",")]
|
||||
else:
|
||||
reversed_motors = []
|
||||
|
||||
vehicle = WebotsArduVehicle(motor_names=motors,
|
||||
reversed_motors=reversed_motors,
|
||||
accel_name=args.accel,
|
||||
imu_name=args.imu,
|
||||
gyro_name=args.gyro,
|
||||
gps_name=args.gps,
|
||||
camera_name=args.camera,
|
||||
camera_fps=args.camera_fps,
|
||||
camera_stream_port=args.camera_port,
|
||||
rangefinder_name=args.rangefinder,
|
||||
rangefinder_fps=args.rangefinder_fps,
|
||||
rangefinder_stream_port=args.rangefinder_port,
|
||||
instance=args.instance,
|
||||
motor_velocity_cap=args.motor_cap,
|
||||
bidirectional_motors=args.bidirectional_motors,
|
||||
uses_propellers=args.uses_propellers,
|
||||
sitl_address=args.sitl_address)
|
||||
|
||||
# User code (ex: connect via drone kit and take off)
|
||||
# ...
|
||||
|
||||
while vehicle.webots_connected():
|
||||
time.sleep(1)
|
||||
|
|
@ -0,0 +1,392 @@
|
|||
#!/usr/bin/env python3
|
||||
'''
|
||||
This file implements a class that acts as a bridge between ArduPilot SITL and Webots
|
||||
|
||||
AP_FLAKE8_CLEAN
|
||||
'''
|
||||
|
||||
# Imports
|
||||
import os
|
||||
import sys
|
||||
import time
|
||||
import socket
|
||||
import select
|
||||
import struct
|
||||
import numpy as np
|
||||
from threading import Thread
|
||||
from typing import List, Union
|
||||
|
||||
# Here we set up environment variables so we can run this script
|
||||
# as an external controller outside of Webots (useful for debugging)
|
||||
# https://cyberbotics.com/doc/guide/running-extern-robot-controllers
|
||||
if sys.platform.startswith("win"):
|
||||
WEBOTS_HOME = "C:\\Program Files\\Webots"
|
||||
elif sys.platform.startswith("darwin"):
|
||||
WEBOTS_HOME = "/Applications/Webots.app"
|
||||
elif sys.platform.startswith("linux"):
|
||||
WEBOTS_HOME = "/usr/local/webots"
|
||||
else:
|
||||
raise Exception("Unsupported OS")
|
||||
|
||||
if os.environ.get("WEBOTS_HOME") is None:
|
||||
os.environ["WEBOTS_HOME"] = WEBOTS_HOME
|
||||
else:
|
||||
WEBOTS_HOME = os.environ.get("WEBOTS_HOME")
|
||||
|
||||
os.environ["PYTHONIOENCODING"] = "UTF-8"
|
||||
sys.path.append(f"{WEBOTS_HOME}/lib/controller/python")
|
||||
|
||||
from controller import Robot, Camera, RangeFinder # noqa: E401, E402
|
||||
|
||||
|
||||
class WebotsArduVehicle():
|
||||
"""Class representing an ArduPilot controlled Webots Vehicle"""
|
||||
|
||||
controls_struct_format = 'f'*16
|
||||
controls_struct_size = struct.calcsize(controls_struct_format)
|
||||
fdm_struct_format = 'd'*(1+3+3+3+3+3)
|
||||
fdm_struct_size = struct.calcsize(fdm_struct_format)
|
||||
|
||||
def __init__(self,
|
||||
motor_names: List[str],
|
||||
accel_name: str = "accelerometer",
|
||||
imu_name: str = "inertial unit",
|
||||
gyro_name: str = "gyro",
|
||||
gps_name: str = "gps",
|
||||
camera_name: str = None,
|
||||
camera_fps: int = 10,
|
||||
camera_stream_port: int = None,
|
||||
rangefinder_name: str = None,
|
||||
rangefinder_fps: int = 10,
|
||||
rangefinder_stream_port: int = None,
|
||||
instance: int = 0,
|
||||
motor_velocity_cap: float = float('inf'),
|
||||
reversed_motors: List[int] = None,
|
||||
bidirectional_motors: bool = False,
|
||||
uses_propellers: bool = True,
|
||||
sitl_address: str = "127.0.0.1"):
|
||||
"""WebotsArduVehicle constructor
|
||||
|
||||
Args:
|
||||
motor_names (List[str]): Motor names in ArduPilot numerical order (first motor is SERVO1 etc).
|
||||
accel_name (str, optional): Webots accelerometer name. Defaults to "accelerometer".
|
||||
imu_name (str, optional): Webots imu name. Defaults to "inertial unit".
|
||||
gyro_name (str, optional): Webots gyro name. Defaults to "gyro".
|
||||
gps_name (str, optional): Webots GPS name. Defaults to "gps".
|
||||
camera_name (str, optional): Webots camera name. Defaults to None.
|
||||
camera_fps (int, optional): Camera FPS. Lower FPS runs better in sim. Defaults to 10.
|
||||
camera_stream_port (int, optional): Port to stream grayscale camera images to.
|
||||
If no port is supplied the camera will not be streamed. Defaults to None.
|
||||
rangefinder_name (str, optional): Webots RangeFinder name. Defaults to None.
|
||||
rangefinder_fps (int, optional): RangeFinder FPS. Lower FPS runs better in sim. Defaults to 10.
|
||||
rangefinder_stream_port (int, optional): Port to stream rangefinder images to.
|
||||
If no port is supplied the camera will not be streamed. Defaults to None.
|
||||
instance (int, optional): Vehicle instance number to match the SITL. This allows multiple vehicles. Defaults to 0.
|
||||
motor_velocity_cap (float, optional): Motor velocity cap. This is useful for the crazyflie
|
||||
which default has way too much power. Defaults to float('inf').
|
||||
reversed_motors (list[int], optional): Reverse the motors (indexed from 1). Defaults to None.
|
||||
bidirectional_motors (bool, optional): Enable bidirectional motors. Defaults to False.
|
||||
uses_propellers (bool, optional): Whether the vehicle uses propellers.
|
||||
This is important as we need to linearize thrust if so. Defaults to True.
|
||||
sitl_address (str, optional): IP address of the SITL (useful with WSL2 eg \"172.24.220.98\").
|
||||
Defaults to "127.0.0.1".
|
||||
"""
|
||||
# init class variables
|
||||
self.motor_velocity_cap = motor_velocity_cap
|
||||
self._instance = instance
|
||||
self._reversed_motors = reversed_motors
|
||||
self._bidirectional_motors = bidirectional_motors
|
||||
self._uses_propellers = uses_propellers
|
||||
self._webots_connected = True
|
||||
|
||||
# setup Webots robot instance
|
||||
self.robot = Robot()
|
||||
|
||||
# set robot time step relative to sim time step
|
||||
self._timestep = int(self.robot.getBasicTimeStep())
|
||||
|
||||
# init sensors
|
||||
self.accel = self.robot.getDevice(accel_name)
|
||||
self.imu = self.robot.getDevice(imu_name)
|
||||
self.gyro = self.robot.getDevice(gyro_name)
|
||||
self.gps = self.robot.getDevice(gps_name)
|
||||
|
||||
self.accel.enable(self._timestep)
|
||||
self.imu.enable(self._timestep)
|
||||
self.gyro.enable(self._timestep)
|
||||
self.gps.enable(self._timestep)
|
||||
|
||||
# init camera
|
||||
if camera_name is not None:
|
||||
self.camera = self.robot.getDevice(camera_name)
|
||||
self.camera.enable(1000//camera_fps) # takes frame period in ms
|
||||
|
||||
# start camera streaming thread if requested
|
||||
if camera_stream_port is not None:
|
||||
self._camera_thread = Thread(daemon=True,
|
||||
target=self._handle_image_stream,
|
||||
args=[self.camera, camera_stream_port])
|
||||
self._camera_thread.start()
|
||||
|
||||
# init rangefinder
|
||||
if rangefinder_name is not None:
|
||||
self.rangefinder = self.robot.getDevice(rangefinder_name)
|
||||
self.rangefinder.enable(1000//rangefinder_fps) # takes frame period in ms
|
||||
|
||||
# start rangefinder streaming thread if requested
|
||||
if rangefinder_stream_port is not None:
|
||||
self._rangefinder_thread = Thread(daemon=True,
|
||||
target=self._handle_image_stream,
|
||||
args=[self.rangefinder, rangefinder_stream_port])
|
||||
self._rangefinder_thread.start()
|
||||
|
||||
# init motors (and setup velocity control)
|
||||
self._motors = [self.robot.getDevice(n) for n in motor_names]
|
||||
for m in self._motors:
|
||||
m.setPosition(float('inf'))
|
||||
m.setVelocity(0)
|
||||
|
||||
# start ArduPilot SITL communication thread
|
||||
self._sitl_thread = Thread(daemon=True, target=self._handle_sitl, args=[sitl_address, 9002+10*instance])
|
||||
self._sitl_thread.start()
|
||||
|
||||
def _handle_sitl(self, sitl_address: str = "127.0.0.1", port: int = 9002):
|
||||
"""Handles all communications with the ArduPilot SITL
|
||||
|
||||
Args:
|
||||
port (int, optional): Port to listen for SITL on. Defaults to 9002.
|
||||
"""
|
||||
|
||||
# create a local UDP socket server to listen for SITL
|
||||
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) # SOCK_STREAM
|
||||
s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
||||
s.bind(('0.0.0.0', port))
|
||||
|
||||
# wait for SITL to connect
|
||||
print(f"Listening for ardupilot SITL (I{self._instance}) at 127.0.0.1:{port}")
|
||||
self.robot.step(self._timestep) # flush print in webots console
|
||||
|
||||
while not select.select([s], [], [], 0)[0]: # wait for socket to be readable
|
||||
# if webots is closed, close the socket and exit
|
||||
if self.robot.step(self._timestep) == -1:
|
||||
s.close()
|
||||
self._webots_connected = False
|
||||
return
|
||||
|
||||
print(f"Connected to ardupilot SITL (I{self._instance})")
|
||||
|
||||
# main loop handling communications
|
||||
while True:
|
||||
# check if the socket is ready to send/receive
|
||||
readable, writable, _ = select.select([s], [s], [], 0)
|
||||
|
||||
# send data to SITL port (one lower than its output port as seen in SITL_cmdline.cpp)
|
||||
if writable:
|
||||
fdm_struct = self._get_fdm_struct()
|
||||
s.sendto(fdm_struct, (sitl_address, port+1))
|
||||
|
||||
# receive data from SITL port
|
||||
if readable:
|
||||
data = s.recv(512)
|
||||
if not data or len(data) < self.controls_struct_size:
|
||||
continue
|
||||
|
||||
# parse a single struct
|
||||
command = struct.unpack(self.controls_struct_format, data[:self.controls_struct_size])
|
||||
self._handle_controls(command)
|
||||
|
||||
# wait until the next Webots time step as no new sensor data will be available until then
|
||||
step_success = self.robot.step(self._timestep)
|
||||
if step_success == -1: # webots closed
|
||||
break
|
||||
|
||||
# if we leave the main loop then Webots must have closed
|
||||
s.close()
|
||||
self._webots_connected = False
|
||||
print(f"Lost connection to Webots (I{self._instance})")
|
||||
|
||||
def _get_fdm_struct(self) -> bytes:
|
||||
"""Form the Flight Dynamics Model struct (aka sensor data) to send to the SITL
|
||||
|
||||
Returns:
|
||||
bytes: bytes representing the struct to send to SITL
|
||||
"""
|
||||
# get data from Webots
|
||||
i = self.imu.getRollPitchYaw()
|
||||
g = self.gyro.getValues()
|
||||
a = self.accel.getValues()
|
||||
gps_pos = self.gps.getValues()
|
||||
gps_vel = self.gps.getSpeedVector()
|
||||
|
||||
# pack the struct, converting ENU to NED (ish)
|
||||
# https://discuss.ardupilot.org/t/copter-x-y-z-which-is-which/6823/3
|
||||
# struct fdm_packet {
|
||||
# double timestamp;
|
||||
# double imu_angular_velocity_rpy[3];
|
||||
# double imu_linear_acceleration_xyz[3];
|
||||
# double imu_orientation_rpy[3];
|
||||
# double velocity_xyz[3];
|
||||
# double position_xyz[3];
|
||||
# };
|
||||
return struct.pack(self.fdm_struct_format,
|
||||
self.robot.getTime(),
|
||||
g[0], -g[1], -g[2],
|
||||
a[0], -a[1], -a[2],
|
||||
i[0], -i[1], -i[2],
|
||||
gps_vel[0], -gps_vel[1], -gps_vel[2],
|
||||
gps_pos[0], -gps_pos[1], -gps_pos[2])
|
||||
|
||||
def _handle_controls(self, command: tuple):
|
||||
"""Set the motor speeds based on the SITL command
|
||||
|
||||
Args:
|
||||
command (tuple): tuple of motor speeds 0.0-1.0 where -1.0 is unused
|
||||
"""
|
||||
|
||||
# get only the number of motors we have
|
||||
command_motors = command[:len(self._motors)]
|
||||
if -1 in command_motors:
|
||||
print(f"Warning: SITL provided {command.index(-1)} motors "
|
||||
f"but model specifies {len(self._motors)} (I{self._instance})")
|
||||
|
||||
# scale commands to -1.0-1.0 if the motors are bidirectional (ex rover wheels)
|
||||
if self._bidirectional_motors:
|
||||
command_motors = [v*2-1 for v in command_motors]
|
||||
|
||||
# linearize propeller thrust for `MOT_THST_EXPO=0`
|
||||
if self._uses_propellers:
|
||||
# `Thrust = thrust_constant * |omega| * omega` (ref https://cyberbotics.com/doc/reference/propeller)
|
||||
# if we set `omega = sqrt(input_thottle)` then `Thrust = thrust_constant * input_thottle`
|
||||
linearized_motor_commands = [np.sqrt(np.abs(v))*np.sign(v) for v in command_motors]
|
||||
|
||||
# reverse motors if desired
|
||||
if self._reversed_motors:
|
||||
for m in self._reversed_motors:
|
||||
linearized_motor_commands[m-1] *= -1
|
||||
|
||||
# set velocities of the motors in Webots
|
||||
for i, m in enumerate(self._motors):
|
||||
m.setVelocity(linearized_motor_commands[i] * min(m.getMaxVelocity(), self.motor_velocity_cap))
|
||||
|
||||
def _handle_image_stream(self, camera: Union[Camera, RangeFinder], port: int):
|
||||
"""Stream grayscale images over TCP
|
||||
|
||||
Args:
|
||||
camera (Camera or RangeFinder): the camera to get images from
|
||||
port (int): port to send images over
|
||||
"""
|
||||
|
||||
# get camera info
|
||||
# https://cyberbotics.com/doc/reference/camera
|
||||
if isinstance(camera, Camera):
|
||||
cam_sample_period = self.camera.getSamplingPeriod()
|
||||
cam_width = self.camera.getWidth()
|
||||
cam_height = self.camera.getHeight()
|
||||
print(f"Camera stream started at 127.0.0.1:{port} (I{self._instance}) "
|
||||
f"({cam_width}x{cam_height} @ {1000/cam_sample_period:0.2f}fps)")
|
||||
elif isinstance(camera, RangeFinder):
|
||||
cam_sample_period = self.rangefinder.getSamplingPeriod()
|
||||
cam_width = self.rangefinder.getWidth()
|
||||
cam_height = self.rangefinder.getHeight()
|
||||
print(f"RangeFinder stream started at 127.0.0.1:{port} (I{self._instance}) "
|
||||
f"({cam_width}x{cam_height} @ {1000/cam_sample_period:0.2f}fps)")
|
||||
else:
|
||||
print(sys.stderr, f"Error: camera passed to _handle_image_stream is of invalid type "
|
||||
f"'{type(camera)}' (I{self._instance})")
|
||||
return
|
||||
|
||||
# create a local TCP socket server
|
||||
server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
server.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
||||
server.bind(('127.0.0.1', port))
|
||||
server.listen(1)
|
||||
|
||||
# continuously send images
|
||||
while self._webots_connected:
|
||||
# wait for incoming connection
|
||||
conn, _ = server.accept()
|
||||
print(f"Connected to camera client (I{self._instance})")
|
||||
|
||||
# send images to client
|
||||
try:
|
||||
while self._webots_connected:
|
||||
# delay at sample rate
|
||||
start_time = self.robot.getTime()
|
||||
|
||||
# get image
|
||||
if isinstance(camera, Camera):
|
||||
img = self.get_camera_gray_image()
|
||||
elif isinstance(camera, RangeFinder):
|
||||
img = self.get_rangefinder_image()
|
||||
|
||||
if img is None:
|
||||
print(f"No image received (I{self._instance})")
|
||||
time.sleep(cam_sample_period/1000)
|
||||
continue
|
||||
|
||||
# create a header struct with image size
|
||||
header = struct.pack("=HH", cam_width, cam_height)
|
||||
|
||||
# pack header and image and send
|
||||
data = header + img.tobytes()
|
||||
conn.sendall(data)
|
||||
|
||||
# delay at sample rate
|
||||
while self.robot.getTime() - start_time < cam_sample_period/1000:
|
||||
time.sleep(0.001)
|
||||
|
||||
except ConnectionResetError:
|
||||
pass
|
||||
except BrokenPipeError:
|
||||
pass
|
||||
finally:
|
||||
conn.close()
|
||||
print(f"Camera client disconnected (I{self._instance})")
|
||||
|
||||
def get_camera_gray_image(self) -> np.ndarray:
|
||||
"""Get the grayscale image from the camera as a numpy array of bytes"""
|
||||
img = self.get_camera_image()
|
||||
img_gray = np.average(img, axis=2).astype(np.uint8)
|
||||
return img_gray
|
||||
|
||||
def get_camera_image(self) -> np.ndarray:
|
||||
"""Get the RGB image from the camera as a numpy array of bytes"""
|
||||
img = self.camera.getImage()
|
||||
img = np.frombuffer(img, np.uint8).reshape((self.camera.getHeight(), self.camera.getWidth(), 4))
|
||||
return img[:, :, :3] # RGB only, no Alpha
|
||||
|
||||
def get_rangefinder_image(self, use_int16: bool = False) -> np.ndarray:
|
||||
"""Get the rangefinder depth image as a numpy array of int8 or int16"""\
|
||||
|
||||
# get range image size
|
||||
height = self.rangefinder.getHeight()
|
||||
width = self.rangefinder.getWidth()
|
||||
|
||||
# get image, and convert raw ctypes array to numpy array
|
||||
# https://cyberbotics.com/doc/reference/rangefinder
|
||||
image_c_ptr = self.rangefinder.getRangeImage(data_type="buffer")
|
||||
img_arr = np.ctypeslib.as_array(image_c_ptr, (width*height,))
|
||||
img_floats = img_arr.reshape((height, width))
|
||||
|
||||
# normalize and set unknown values to max range
|
||||
range_range = self.rangefinder.getMaxRange() - self.rangefinder.getMinRange()
|
||||
img_normalized = (img_floats - self.rangefinder.getMinRange()) / range_range
|
||||
img_normalized[img_normalized == float('inf')] = 1
|
||||
|
||||
# convert to int8 or int16, allowing for the option of higher precision if desired
|
||||
if use_int16:
|
||||
img = (img_normalized * 65535).astype(np.uint16)
|
||||
else:
|
||||
img = (img_normalized * 255).astype(np.uint8)
|
||||
|
||||
return img
|
||||
|
||||
def stop_motors(self):
|
||||
"""Set all motors to zero velocity"""
|
||||
for m in self._motors:
|
||||
m.setPosition(float('inf'))
|
||||
m.setVelocity(0)
|
||||
|
||||
def webots_connected(self) -> bool:
|
||||
"""Check if Webots client is connected"""
|
||||
return self._webots_connected
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
FRAME_CLASS 1 # Multirotor
|
||||
FRAME_TYPE 3 # H class
|
||||
|
||||
ATC_ANG_PIT_P 0.5
|
||||
ATC_ANG_RLL_P 0.5
|
||||
ATC_ANG_YAW_P 0.50000
|
||||
|
||||
ATC_RAT_PIT_P 0.05
|
||||
ATC_RAT_RLL_P 0.05
|
||||
ATC_RAT_YAW_P 0.02
|
||||
|
||||
ATC_RAT_PIT_I 0
|
||||
ATC_RAT_RLL_I 0
|
||||
|
||||
ATC_RAT_PIT_D 0.0003
|
||||
ATC_RAT_RLL_D 0.0003
|
||||
ATC_RAT_YAW_D 0.000001
|
||||
|
||||
AHRS_EKF_TYPE 10 # Use sim AHRS
|
||||
ARMING_CHECK 1045534 # No RC requirements
|
||||
MOT_THST_EXPO 0 # No motor exponential
|
||||
SCHED_LOOP_RATE 300
|
||||
INS_GYR_CAL 0
|
||||
|
|
@ -0,0 +1,23 @@
|
|||
FRAME_CLASS 1 # Multirotor
|
||||
FRAME_TYPE 1 # X class
|
||||
|
||||
ATC_ANG_PIT_P 0.5
|
||||
ATC_ANG_RLL_P 0.5
|
||||
ATC_ANG_YAW_P 0.5
|
||||
|
||||
ATC_RAT_PIT_P 0.05
|
||||
ATC_RAT_RLL_P 0.05
|
||||
ATC_RAT_YAW_P 0.02
|
||||
|
||||
ATC_RAT_PIT_I 0
|
||||
ATC_RAT_RLL_I 0
|
||||
|
||||
ATC_RAT_PIT_D 0.0003
|
||||
ATC_RAT_RLL_D 0.0003
|
||||
ATC_RAT_YAW_D 0.000001
|
||||
|
||||
AHRS_EKF_TYPE 10 # Use sim AHRS
|
||||
ARMING_CHECK 1045534 # No RC requirements
|
||||
MOT_THST_EXPO 0 # No motor exponential
|
||||
SCHED_LOOP_RATE 300
|
||||
INS_GYR_CAL 0
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
FRAME_CLASS 1 # Rover
|
||||
|
||||
SERVO1_FUNCTION 73 # (Throttle Left)
|
||||
SERVO2_FUNCTION 73
|
||||
SERVO3_FUNCTION 74 # (Throttle Right)
|
||||
SERVO4_FUNCTION 74
|
||||
|
||||
AHRS_EKF_TYPE 10 # Use sim AHRS
|
||||
ARMING_CHECK 1045534 # No RC requirements
|
||||
MOT_THST_EXPO 0 # No motor exponential
|
||||
SCHED_LOOP_RATE 300
|
||||
INS_GYR_CAL 0
|
||||
|
|
@ -0,0 +1,57 @@
|
|||
# VRML_SIM R2022b utf8
|
||||
# template language: javascript
|
||||
|
||||
PROTO ArucoMarker [
|
||||
field SFVec3f translation 0 0 0
|
||||
field SFRotation rotation 0 0 1 0
|
||||
field SFString name "aruco" # Defines the name of the aruco board.
|
||||
field SFFloat size 0.1 # Defines size of the markers.
|
||||
field SFString{"0", "1", "2", "3", "4", "5", "6", "7", "8"}
|
||||
markerId "0" # Defines the marker id.
|
||||
]
|
||||
{
|
||||
Solid {
|
||||
translation IS translation
|
||||
rotation IS rotation
|
||||
name IS name
|
||||
children [
|
||||
# marker slightly in front of the border
|
||||
Transform {
|
||||
translation 0 0 1e-3
|
||||
children [
|
||||
Shape {
|
||||
appearance PBRAppearance {
|
||||
baseColor 1 1 1
|
||||
metalness 0
|
||||
roughness 0.6
|
||||
|
||||
%< let markerTexture = "textures/aruco_" + fields.markerId.value + ".png"; >%
|
||||
baseColorMap ImageTexture {
|
||||
url "%<= markerTexture >%"
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
geometry Plane {
|
||||
size %<= fields.size.value >% %<= fields.size.value >%
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
|
||||
|
||||
# while boarder
|
||||
Shape {
|
||||
appearance PBRAppearance {
|
||||
baseColor 1 1 1
|
||||
metalness 0
|
||||
roughness 0.6
|
||||
}
|
||||
geometry Plane {
|
||||
size %<= fields.size.value * 8/6 >% %<= fields.size.value * 8/6 >%
|
||||
}
|
||||
}
|
||||
|
||||
]
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,161 @@
|
|||
# VRML_SIM R2022b utf8
|
||||
# Iris drone model. Note that the model is not 1-to-1 with the
|
||||
# real drone as motor/propeller constants were empirically determined.
|
||||
|
||||
PROTO Iris [
|
||||
field SFVec3f translation 0 0 0
|
||||
field SFRotation rotation 0 0 1 0
|
||||
field SFString name "Iris" # Is `Robot.name`.
|
||||
field SFString controller "void" # Is `Robot.controller`.
|
||||
field MFString controllerArgs [] # Is `Robot.controllerArgs`.
|
||||
field SFString customData "" # Is `Robot.customData`.
|
||||
field SFBool supervisor FALSE # Is `Robot.supervisor`.
|
||||
field SFBool synchronization TRUE # Is `Robot.synchronization`.
|
||||
field MFNode extensionSlot [] # Extends the robot with new nodes in the extension slot.
|
||||
]
|
||||
{
|
||||
Robot {
|
||||
translation IS translation
|
||||
rotation IS rotation
|
||||
controller IS controller
|
||||
controllerArgs IS controllerArgs
|
||||
customData IS customData
|
||||
supervisor IS supervisor
|
||||
synchronization IS synchronization
|
||||
name IS name
|
||||
children [
|
||||
Group {
|
||||
children IS extensionSlot
|
||||
}
|
||||
DEF IRIS_MESH Shape {
|
||||
appearance PBRAppearance {
|
||||
baseColor 0.05 0.05 0.05
|
||||
roughness 1.000000
|
||||
metalness 0.2
|
||||
}
|
||||
geometry Mesh {
|
||||
url "meshes/iris.dae"
|
||||
}
|
||||
castShadows FALSE
|
||||
}
|
||||
Propeller {
|
||||
shaftAxis 0 0 1
|
||||
centerOfThrust 0.130000 -0.220000 0.023000
|
||||
thrustConstants 0.0012 0
|
||||
torqueConstants 5.0e-04 0
|
||||
device RotationalMotor {
|
||||
name "m1_motor"
|
||||
maxVelocity 100
|
||||
maxTorque 30
|
||||
multiplier 1
|
||||
}
|
||||
slowHelix Solid {
|
||||
translation 0.130000 -0.220000 0.023000
|
||||
children [
|
||||
Shape {
|
||||
appearance DEF PROP_BLUE PBRAppearance {
|
||||
baseColor 0 0 0.75
|
||||
metalness 0.3
|
||||
}
|
||||
geometry DEF CCW_PROP Mesh {
|
||||
url [
|
||||
"meshes/iris_prop_ccw.dae"
|
||||
]
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
Propeller {
|
||||
shaftAxis 0 0 1
|
||||
centerOfThrust -0.130000 0.200000 0.023000
|
||||
thrustConstants 0.0012 0
|
||||
torqueConstants 5.0e-04 0
|
||||
device RotationalMotor {
|
||||
name "m2_motor"
|
||||
maxVelocity 100
|
||||
maxTorque 30
|
||||
multiplier 1
|
||||
}
|
||||
slowHelix Solid {
|
||||
translation -0.130000 0.200000 0.023000
|
||||
children [
|
||||
Shape {
|
||||
appearance DEF PROP_BLACK PBRAppearance {
|
||||
baseColor 0 0 0
|
||||
metalness 0.3
|
||||
}
|
||||
geometry USE CCW_PROP
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
Propeller {
|
||||
shaftAxis 0 0 1
|
||||
centerOfThrust 00.130000 0.220000 0.023000
|
||||
thrustConstants -0.0012 0
|
||||
torqueConstants 5.0e-04 0
|
||||
device RotationalMotor {
|
||||
name "m3_motor"
|
||||
maxVelocity 100
|
||||
maxTorque 30
|
||||
multiplier -1
|
||||
}
|
||||
slowHelix Solid {
|
||||
translation 0.130000 0.220000 0.023000
|
||||
children [
|
||||
Shape {
|
||||
appearance USE PROP_BLUE
|
||||
geometry DEF CW_PROP Mesh {
|
||||
url [
|
||||
"meshes/iris_prop_cw.dae"
|
||||
]
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
Propeller {
|
||||
shaftAxis 0 0 1
|
||||
centerOfThrust -0.130000 -0.200000 0.023000
|
||||
thrustConstants -0.0012 0
|
||||
torqueConstants 5.0e-04 0
|
||||
device RotationalMotor {
|
||||
name "m4_motor"
|
||||
maxVelocity 100
|
||||
maxTorque 30
|
||||
multiplier -1
|
||||
}
|
||||
slowHelix Solid {
|
||||
translation -0.130000 -0.200000 0.023000
|
||||
children [
|
||||
Shape {
|
||||
appearance USE PROP_BLACK
|
||||
geometry USE CW_PROP
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
Accelerometer {
|
||||
}
|
||||
GPS {
|
||||
}
|
||||
Gyro {
|
||||
}
|
||||
InertialUnit {
|
||||
}
|
||||
]
|
||||
boundingObject Box {
|
||||
size 0.470000 0.470000 0.110000
|
||||
}
|
||||
physics Physics {
|
||||
density -1
|
||||
mass 1.500000
|
||||
centerOfMass [ 0.000000 0.000000 0.000000 ]
|
||||
inertiaMatrix [
|
||||
2.912500e-02 2.912500e-02 5.522500e-02
|
||||
0.000000e+00 0.000000e+00 0.000000e+00
|
||||
]
|
||||
}
|
||||
}
|
||||
}
|
||||
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
Binary file not shown.
|
After Width: | Height: | Size: 194 B |
Binary file not shown.
|
After Width: | Height: | Size: 177 B |
Binary file not shown.
|
After Width: | Height: | Size: 189 B |
Binary file not shown.
|
After Width: | Height: | Size: 189 B |
Binary file not shown.
|
After Width: | Height: | Size: 197 B |
Binary file not shown.
|
After Width: | Height: | Size: 190 B |
Binary file not shown.
|
After Width: | Height: | Size: 191 B |
Binary file not shown.
|
After Width: | Height: | Size: 186 B |
Binary file not shown.
|
After Width: | Height: | Size: 192 B |
|
|
@ -0,0 +1,84 @@
|
|||
#!/usr/bin/env python3
|
||||
|
||||
#
|
||||
# An example script that receives images from a WebotsArduVehicle on port 5599
|
||||
# and displays them overlayed with any ArUco markers using OpenCV.
|
||||
# Requires opencv-python (`pip3 install opencv-python`)
|
||||
#
|
||||
|
||||
import cv2
|
||||
import socket
|
||||
import struct
|
||||
import numpy as np
|
||||
|
||||
# connect to WebotsArduVehicle
|
||||
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
s.connect(("127.0.0.1", 5599))
|
||||
|
||||
# ArUco setup
|
||||
aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_50)
|
||||
aruco_params = cv2.aruco.DetectorParameters_create()
|
||||
|
||||
header_size = struct.calcsize("=HH")
|
||||
while True:
|
||||
# receive header
|
||||
header = s.recv(header_size)
|
||||
if len(header) != header_size:
|
||||
print("Header size mismatch")
|
||||
break
|
||||
|
||||
# parse header
|
||||
width, height = struct.unpack("=HH", header)
|
||||
|
||||
# for CV applications we may want camera intrinsics such as focal length:
|
||||
# https://stackoverflow.com/questions/61555182/webot-camera-default-parameters-like-pixel-size-and-focus
|
||||
# cam_focal_length = 2 * np.arctan(np.tan(cam_fov * 0.5) / (cam_width / cam_height))
|
||||
|
||||
# receive image
|
||||
bytes_to_read = width * height
|
||||
img = bytes()
|
||||
while len(img) < bytes_to_read:
|
||||
img += s.recv(min(bytes_to_read - len(img), 4096))
|
||||
|
||||
# convert to numpy array
|
||||
img = np.frombuffer(img, np.uint8).reshape((height, width))
|
||||
img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
|
||||
|
||||
# detect ArUco markers
|
||||
(corners, ids, rejected) = cv2.aruco.detectMarkers(img, aruco_dict, parameters=aruco_params)
|
||||
if len(corners) > 0:
|
||||
# flatten the ArUco IDs list
|
||||
ids = ids.flatten()
|
||||
# loop over the detected ArUCo corners
|
||||
for (markerCorner, markerID) in zip(corners, ids):
|
||||
# extract the marker corners (which are always returned in
|
||||
# top-left, top-right, bottom-right, and bottom-left order)
|
||||
corners = markerCorner.reshape((4, 2))
|
||||
(topLeft, topRight, bottomRight, bottomLeft) = corners
|
||||
# convert each of the (x, y)-coordinate pairs to integers
|
||||
topRight = (int(topRight[0]), int(topRight[1]))
|
||||
bottomRight = (int(bottomRight[0]), int(bottomRight[1]))
|
||||
bottomLeft = (int(bottomLeft[0]), int(bottomLeft[1]))
|
||||
topLeft = (int(topLeft[0]), int(topLeft[1]))
|
||||
|
||||
# draw the bounding box of the ArUCo detection
|
||||
cv2.line(img, topLeft, topRight, (0, 255, 0), 2)
|
||||
cv2.line(img, topRight, bottomRight, (0, 255, 0), 2)
|
||||
cv2.line(img, bottomRight, bottomLeft, (0, 255, 0), 2)
|
||||
cv2.line(img, bottomLeft, topLeft, (0, 255, 0), 2)
|
||||
# compute and draw the center (x, y)-coordinates of the ArUco
|
||||
# marker
|
||||
cX = int((topLeft[0] + bottomRight[0]) / 2.0)
|
||||
cY = int((topLeft[1] + bottomRight[1]) / 2.0)
|
||||
cv2.circle(img, (cX, cY), 4, (0, 0, 255), -1)
|
||||
# draw the ArUco marker ID on the image
|
||||
cv2.putText(img, str(markerID),
|
||||
(topLeft[0], topLeft[1] - 15), cv2.FONT_HERSHEY_SIMPLEX,
|
||||
0.5, (0, 255, 0), 2)
|
||||
|
||||
# display image
|
||||
cv2.imshow("image", img)
|
||||
if cv2.waitKey(1) == ord("q"):
|
||||
break
|
||||
|
||||
s.close()
|
||||
|
|
@ -0,0 +1,46 @@
|
|||
#!/usr/bin/env python3
|
||||
|
||||
#
|
||||
# An example script that receives images from a WebotsArduVehicle
|
||||
# on port 5599 and displays using OpenCV.
|
||||
# Requires opencv-python (`pip3 install opencv-python`)
|
||||
#
|
||||
|
||||
import cv2
|
||||
import socket
|
||||
import struct
|
||||
import numpy as np
|
||||
|
||||
# connect to WebotsArduVehicle
|
||||
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
s.connect(("127.0.0.1", 5599))
|
||||
|
||||
header_size = struct.calcsize("=HH")
|
||||
while True:
|
||||
# receive header
|
||||
header = s.recv(header_size)
|
||||
if len(header) != header_size:
|
||||
print("Header size mismatch")
|
||||
break
|
||||
|
||||
# parse header
|
||||
width, height = struct.unpack("=HH", header)
|
||||
|
||||
# receive image
|
||||
bytes_to_read = width * height
|
||||
img = bytes()
|
||||
while len(img) < bytes_to_read:
|
||||
img += s.recv(min(bytes_to_read - len(img), 4096))
|
||||
|
||||
# convert incoming bytes to a numpy array (a grayscale image)
|
||||
img = np.frombuffer(img, np.uint8).reshape((height, width))
|
||||
|
||||
# Do cool stuff with the image here
|
||||
# ...
|
||||
|
||||
# display image
|
||||
cv2.imshow("image", img)
|
||||
if cv2.waitKey(1) == ord("q"):
|
||||
break
|
||||
|
||||
s.close()
|
||||
|
|
@ -0,0 +1,51 @@
|
|||
#VRML_SIM R2023a utf8
|
||||
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/robots/bitcraze/crazyflie/protos/Crazyflie.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackground.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/floors/protos/Floor.proto"
|
||||
|
||||
WorldInfo {
|
||||
title "Crazyflie Ardupilot"
|
||||
basicTimeStep 2
|
||||
FPS 20
|
||||
}
|
||||
Viewpoint {
|
||||
orientation -0.16896949154992233 0.7737576492025086 0.6105312532753491 0.687001624474616
|
||||
position -0.398945807007221 -0.19824950788611448 0.29284320157231725
|
||||
follow "Crazyflie"
|
||||
followSmoothness 0.01
|
||||
}
|
||||
TexturedBackground {
|
||||
}
|
||||
TexturedBackgroundLight {
|
||||
}
|
||||
Floor {
|
||||
}
|
||||
Crazyflie {
|
||||
controller "ardupilot_vehicle_controller"
|
||||
controllerArgs [
|
||||
"--motors"
|
||||
"m1_motor, m3_motor, m4_motor, m2_motor"
|
||||
"--reversed-motors"
|
||||
"1, 2"
|
||||
"--camera-fps"
|
||||
"10"
|
||||
"--camera"
|
||||
"camera"
|
||||
"--accel"
|
||||
"accelerometer"
|
||||
"--imu"
|
||||
"inertial unit"
|
||||
"--gyro"
|
||||
"gyro"
|
||||
"--gps"
|
||||
"gps"
|
||||
"--motor-cap"
|
||||
"100"
|
||||
]
|
||||
extensionSlot [
|
||||
Accelerometer {
|
||||
}
|
||||
]
|
||||
}
|
||||
|
|
@ -0,0 +1,76 @@
|
|||
#VRML_SIM R2023a utf8
|
||||
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/robots/bitcraze/crazyflie/protos/Crazyflie.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackground.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/floors/protos/Floor.proto"
|
||||
|
||||
WorldInfo {
|
||||
title "Crazyflie Ardupilot"
|
||||
basicTimeStep 2
|
||||
FPS 30
|
||||
}
|
||||
Viewpoint {
|
||||
orientation -0.0593532354440745 0.9880332410323718 0.14236399845953884 0.798588127989691
|
||||
position -0.5248689288960577 0.1043842791985661 0.5678333324847978
|
||||
follow "Crazyflie 0"
|
||||
followSmoothness 0.01
|
||||
}
|
||||
TexturedBackground {
|
||||
}
|
||||
TexturedBackgroundLight {
|
||||
}
|
||||
Floor {
|
||||
}
|
||||
Crazyflie {
|
||||
hidden linearVelocity_0 -4.637218965709596e-19 -4.196252875038606e-19 3.352873534367973e-16
|
||||
hidden angularVelocity_0 4.956352788505185e-17 6.199137112878587e-17 -7.59922590549506e-18
|
||||
translation -1.2560172892842563e-17 0.2 0.014998773749999996
|
||||
rotation 0.0006423841504381482 0.0009061380955760532 -0.9999993831279872 3.0862235845634213e-16
|
||||
name "Crazyflie 0"
|
||||
controller "ardupilot_vehicle_controller"
|
||||
controllerArgs [
|
||||
"--reversed-motors"
|
||||
"1, 2"
|
||||
"--camera"
|
||||
"camera"
|
||||
"--camera-fps"
|
||||
"10"
|
||||
"--motors"
|
||||
"m1_motor, m3_motor, m4_motor, m2_motor"
|
||||
"--motor-cap"
|
||||
"100"
|
||||
"--instance"
|
||||
"0"
|
||||
]
|
||||
extensionSlot [
|
||||
Accelerometer {
|
||||
}
|
||||
]
|
||||
}
|
||||
Crazyflie {
|
||||
hidden linearVelocity_0 1.1290302625479896e-18 -4.326730364482081e-19 3.352873534367973e-16
|
||||
hidden angularVelocity_0 5.546676845014852e-31 2.0628843433230186e-17 -7.9532317962812e-18
|
||||
translation -3.7742147020501796e-18 -1.921003416687696e-17 0.014998773749999996
|
||||
rotation -0.0017125758688741218 0.0002306643848911165 -0.9999985069378028 3.183501628356045e-16
|
||||
name "Crazyflie 1"
|
||||
controller "ardupilot_vehicle_controller"
|
||||
controllerArgs [
|
||||
"--reversed-motors"
|
||||
"1, 2"
|
||||
"--camera-fps"
|
||||
"10"
|
||||
"--camera"
|
||||
"camera"
|
||||
"--motors"
|
||||
"m1_motor, m3_motor, m4_motor, m2_motor"
|
||||
"--motor-cap"
|
||||
"100"
|
||||
"--instance"
|
||||
"1"
|
||||
]
|
||||
extensionSlot [
|
||||
Accelerometer {
|
||||
}
|
||||
]
|
||||
}
|
||||
|
|
@ -0,0 +1,63 @@
|
|||
# VRML_SIM R2023a utf8
|
||||
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackground.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/road/protos/StraightRoadSegment.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/floors/protos/Floor.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/appearances/protos/Grass.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/advertising_board/protos/AdvertisingBoard.proto"
|
||||
EXTERNPROTO "../protos/Iris.proto"
|
||||
|
||||
WorldInfo {
|
||||
title "Iris Ardupilot"
|
||||
basicTimeStep 2
|
||||
FPS 20
|
||||
}
|
||||
Viewpoint {
|
||||
orientation 0.15554153350599548 -0.8879632385984604 -0.4328141844398555 5.514055516141922
|
||||
position -2.719806534182175 -0.9451744343683601 2.1727182111093915
|
||||
follow "Iris"
|
||||
followSmoothness 0.01
|
||||
}
|
||||
TexturedBackground {
|
||||
}
|
||||
TexturedBackgroundLight {
|
||||
}
|
||||
Floor {
|
||||
size 300 100
|
||||
appearance Grass {
|
||||
}
|
||||
}
|
||||
AdvertisingBoard {
|
||||
translation 13.1012 17.8267 0
|
||||
rotation 0 0 1 -1.469
|
||||
frontTexture [
|
||||
"../../../../../Tools/autotest/web-firmware/images/logo.png"
|
||||
]
|
||||
backTexture [
|
||||
"../../../../../Tools/autotest/web-firmware/images/logo.png"
|
||||
]
|
||||
displayWidth 16.28
|
||||
displayHeight 6
|
||||
poleNumber 2
|
||||
poleType "H-shape"
|
||||
poleHeight 10
|
||||
}
|
||||
StraightRoadSegment {
|
||||
translation -15 0.3 0.02
|
||||
rotation 0 0 1 0.105
|
||||
width 16.5
|
||||
rightBorder FALSE
|
||||
leftBorder FALSE
|
||||
length 140
|
||||
}
|
||||
Iris {
|
||||
translation 0 0 0.09
|
||||
controller "ardupilot_vehicle_controller"
|
||||
controllerArgs [
|
||||
"--motors"
|
||||
"m1_motor, m2_motor, m3_motor, m4_motor"
|
||||
]
|
||||
extensionSlot [
|
||||
]
|
||||
}
|
||||
|
|
@ -0,0 +1,118 @@
|
|||
#VRML_SIM R2023a utf8
|
||||
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackground.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/road/protos/StraightRoadSegment.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/floors/protos/Floor.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/appearances/protos/Grass.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/tables/protos/Table.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/advertising_board/protos/AdvertisingBoard.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/obstacles/protos/OilBarrel.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/animals/protos/Dog.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/animals/protos/Rabbit.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/garden/protos/Gnome.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/factory/forklift/protos/Forklift.proto"
|
||||
EXTERNPROTO "../protos/Iris.proto"
|
||||
EXTERNPROTO "../protos/ArucoMarker.proto"
|
||||
|
||||
WorldInfo {
|
||||
title "Iris Ardupilot"
|
||||
basicTimeStep 2
|
||||
FPS 20
|
||||
}
|
||||
Viewpoint {
|
||||
orientation 0.10264123808602478 -0.7648124131744727 -0.6360241732028804 5.867275660921915
|
||||
position -2.488925008027789 0.29237831017431365 2.067754838508868
|
||||
follow "Iris"
|
||||
followSmoothness 0.01
|
||||
}
|
||||
TexturedBackground {
|
||||
}
|
||||
TexturedBackgroundLight {
|
||||
}
|
||||
Floor {
|
||||
size 300 100
|
||||
appearance Grass {
|
||||
}
|
||||
}
|
||||
StraightRoadSegment {
|
||||
translation -15 0.3 0.02
|
||||
rotation 0 0 1 0.105
|
||||
width 16.5
|
||||
rightBorder FALSE
|
||||
leftBorder FALSE
|
||||
length 140
|
||||
}
|
||||
Rabbit {
|
||||
translation 4.68362 1.88178 0.88
|
||||
rotation 0 0 1 2.61799
|
||||
name "nibbles"
|
||||
}
|
||||
Gnome {
|
||||
translation 5.87588 0.590355 3.6788e-21
|
||||
rotation 0 0 1 2.61799
|
||||
}
|
||||
Forklift {
|
||||
translation 9.19827 2.56008 0.81
|
||||
rotation 0 0 1 -2.3561953071795863
|
||||
enablePhysics FALSE
|
||||
}
|
||||
Dog {
|
||||
translation 5.97592 3.52 1.77636e-15
|
||||
rotation 0 0 1 -2.6179953071795863
|
||||
name "bentley"
|
||||
}
|
||||
OilBarrel {
|
||||
translation 4.54625 1.79835 0.44
|
||||
}
|
||||
AdvertisingBoard {
|
||||
translation 13.1012 17.8267 0
|
||||
rotation 0 0 1 -1.469
|
||||
frontTexture [
|
||||
"../../../../../Tools/autotest/web-firmware/images/logo.png"
|
||||
]
|
||||
backTexture [
|
||||
"../../../../../Tools/autotest/web-firmware/images/logo.png"
|
||||
]
|
||||
displayWidth 16.28
|
||||
displayHeight 6
|
||||
poleNumber 2
|
||||
poleType "H-shape"
|
||||
poleHeight 10
|
||||
}
|
||||
Table {
|
||||
translation 0 0.92 0
|
||||
}
|
||||
Iris {
|
||||
translation 0.0702495 1.45039 0.8
|
||||
controller "ardupilot_vehicle_controller"
|
||||
controllerArgs [
|
||||
"--motors"
|
||||
"m1_motor, m2_motor, m3_motor, m4_motor"
|
||||
"--camera"
|
||||
"camera"
|
||||
"--camera-port"
|
||||
"5599"
|
||||
]
|
||||
extensionSlot [
|
||||
Camera {
|
||||
translation 0.05 0 0.05
|
||||
rotation 0 1 0 0
|
||||
width 640
|
||||
height 480
|
||||
}
|
||||
]
|
||||
}
|
||||
ArucoMarker {
|
||||
translation 4.23295 1.80428 0.72
|
||||
rotation 0 1 0 -1.5707953071795862
|
||||
name "aruco 0"
|
||||
size 0.2
|
||||
}
|
||||
ArucoMarker {
|
||||
translation 4.39446 1.53182 0.44
|
||||
rotation 0.44721372362178063 -0.7745965212991105 0.44721372362178063 1.82348
|
||||
name "aruco 1"
|
||||
size 0.2
|
||||
markerId "1"
|
||||
}
|
||||
|
|
@ -0,0 +1,97 @@
|
|||
#VRML_SIM R2023a utf8
|
||||
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackground.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/road/protos/StraightRoadSegment.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/floors/protos/Floor.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/appearances/protos/Grass.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/humans/pedestrian/protos/Pedestrian.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/buildings/protos/Warehouse.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/advertising_board/protos/AdvertisingBoard.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/tables/protos/Table.proto"
|
||||
EXTERNPROTO "../protos/Iris.proto"
|
||||
|
||||
WorldInfo {
|
||||
title "Iris Ardupilot"
|
||||
basicTimeStep 2
|
||||
FPS 20
|
||||
}
|
||||
Viewpoint {
|
||||
orientation -0.08123827592100609 -0.8844702112073417 0.45947011656061265 5.888579928177443
|
||||
position -13.153423548832741 2.5290211453100055 6.931773501208396
|
||||
follow "Iris"
|
||||
followSmoothness 0.01
|
||||
}
|
||||
TexturedBackground {
|
||||
}
|
||||
TexturedBackgroundLight {
|
||||
}
|
||||
Floor {
|
||||
size 300 100
|
||||
appearance Grass {
|
||||
}
|
||||
}
|
||||
StraightRoadSegment {
|
||||
translation -15 0.3 0.02
|
||||
rotation 0 0 1 0.105
|
||||
width 16.5
|
||||
rightBorder FALSE
|
||||
leftBorder FALSE
|
||||
length 140
|
||||
}
|
||||
Warehouse {
|
||||
translation 22.0473 -12.6792 0
|
||||
rotation 0 0 1 1.671
|
||||
}
|
||||
Warehouse {
|
||||
translation 61.8844 -8.67468 0
|
||||
rotation 0 0 1 1.671
|
||||
name "warehouse(1)"
|
||||
}
|
||||
Pedestrian {
|
||||
translation 2.49 0 1.27
|
||||
controllerArgs [
|
||||
"--speed=0.75"
|
||||
"--trajectory= 2.5 0, 5 2.5, 7.5 0, 5 -2.5"
|
||||
]
|
||||
}
|
||||
Table {
|
||||
translation 0 0.92 0
|
||||
}
|
||||
AdvertisingBoard {
|
||||
translation 13.1012 17.8267 0
|
||||
rotation 0 0 1 -1.469
|
||||
frontTexture [
|
||||
"../../../../../Tools/autotest/web-firmware/images/logo.png"
|
||||
]
|
||||
backTexture [
|
||||
"../../../../../Tools/autotest/web-firmware/images/logo.png"
|
||||
]
|
||||
displayWidth 16.28
|
||||
displayHeight 6
|
||||
poleNumber 2
|
||||
poleType "H-shape"
|
||||
poleHeight 10
|
||||
}
|
||||
Iris {
|
||||
translation 0.0702495 1.45039 0.8
|
||||
rotation 0 0 1 -0.523595307179586
|
||||
controller "ardupilot_vehicle_controller"
|
||||
controllerArgs [
|
||||
"--motors"
|
||||
"m1_motor, m2_motor, m3_motor, m4_motor"
|
||||
"--rangefinder"
|
||||
"range-finder"
|
||||
"--rangefinder-port"
|
||||
"5599"
|
||||
]
|
||||
extensionSlot [
|
||||
RangeFinder {
|
||||
translation 0.1 0 0
|
||||
fieldOfView 1.57
|
||||
width 640
|
||||
height 480
|
||||
maxRange 20
|
||||
}
|
||||
]
|
||||
}
|
||||
|
|
@ -0,0 +1,62 @@
|
|||
#VRML_SIM R2023a utf8
|
||||
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/released/projects/robots/adept/pioneer3/protos/Pioneer3at.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackground.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/road/protos/StraightRoadSegment.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/floors/protos/Floor.proto"
|
||||
EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/appearances/protos/Grass.proto"
|
||||
|
||||
WorldInfo {
|
||||
title "Pioneer3at Ardupilot"
|
||||
basicTimeStep 2
|
||||
FPS 20
|
||||
}
|
||||
Viewpoint {
|
||||
orientation 0.12905547789670888 -0.9264705889754452 -0.353549050894418 5.532540449151996
|
||||
position -2.921033113316591 -0.8540816786630955 2.693602881281485
|
||||
follow "Pioneer 3-AT"
|
||||
followSmoothness 0.01
|
||||
}
|
||||
TexturedBackground {
|
||||
}
|
||||
TexturedBackgroundLight {
|
||||
}
|
||||
Floor {
|
||||
size 300 100
|
||||
appearance Grass {
|
||||
}
|
||||
}
|
||||
StraightRoadSegment {
|
||||
translation -15 0.3 0.02
|
||||
rotation 0 0 1 0.105
|
||||
width 16.5
|
||||
rightBorder FALSE
|
||||
leftBorder FALSE
|
||||
length 140
|
||||
}
|
||||
Pioneer3at {
|
||||
controller "ardupilot_vehicle_controller"
|
||||
controllerArgs [
|
||||
"--motors"
|
||||
"front left wheel, back left wheel, front right wheel, back right wheel"
|
||||
"--camera-fps"
|
||||
"10"
|
||||
"--motor-cap"
|
||||
"100"
|
||||
"--bidirectional-motors"
|
||||
"true"
|
||||
"--uses-propellers"
|
||||
"false"
|
||||
]
|
||||
extensionSlot [
|
||||
Accelerometer {
|
||||
}
|
||||
InertialUnit {
|
||||
}
|
||||
Gyro {
|
||||
}
|
||||
GPS {
|
||||
}
|
||||
]
|
||||
}
|
||||
Loading…
Reference in New Issue