Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

SITL: enable multi sitl for gazebo

Browse Source
This commit is contained in:
Pierre Kancir 2016-09-21 22:46:12 +02:00 committed by Andrew Tridgell
parent fc4caf51e9
commit 63ea8c6f21
6 changed files with 81 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_HAL_SITL/SITL_State.h
View File

@ -170,6 +170,9 @@ private:
bool _use_fg_view;
const char *_fdm_address;
const char *_gazebo_address;
int _gazebo_port_in;
int _gazebo_port_out;
// delay buffer variables
static const uint8_t mag_buffer_length = 250;

56
libraries/AP_HAL_SITL/SITL_cmdline.cpp
View File

@ -43,21 +43,24 @@ static void _sig_fpe(int signum)
void SITL_State::_usage(void)
{
printf("Options:\n"
"\t--home HOME set home location (lat,lng,alt,yaw)\n"
"\t--model MODEL set simulation model\n"
"\t--wipe wipe eeprom and dataflash\n"
"\t--unhide-groups parameter enumeration ignores AP_PARAM_FLAG_ENABLE\n"
"\t--rate RATE set SITL framerate\n"
"\t--console use console instead of TCP ports\n"
"\t--instance N set instance of SITL (adds 10*instance to all port numbers)\n"
"\t--speedup SPEEDUP set simulation speedup\n"
"\t--gimbal enable simulated MAVLink gimbal\n"
"\t--autotest-dir DIR set directory for additional files\n"
"\t--uartA device set device string for UARTA\n"
"\t--uartB device set device string for UARTB\n"
"\t--uartC device set device string for UARTC\n"
"\t--uartD device set device string for UARTD\n"
"\t--uartE device set device string for UARTE\n"
"\t--home HOME set home location (lat,lng,alt,yaw)\n"
"\t--model MODEL set simulation model\n"
"\t--wipe wipe eeprom and dataflash\n"
"\t--unhide-groups parameter enumeration ignores AP_PARAM_FLAG_ENABLE\n"
"\t--rate RATE set SITL framerate\n"
"\t--console use console instead of TCP ports\n"
"\t--instance N set instance of SITL (adds 10*instance to all port numbers)\n"
"\t--speedup SPEEDUP set simulation speedup\n"
"\t--gimbal enable simulated MAVLink gimbal\n"
"\t--autotest-dir DIR set directory for additional files\n"
"\t--uartA device set device string for UARTA\n"
"\t--uartB device set device string for UARTB\n"
"\t--uartC device set device string for UARTC\n"
"\t--uartD device set device string for UARTD\n"
"\t--uartE device set device string for UARTE\n"
"\t--gazebo-address ADDR set address string for gazebo\n"
"\t--gazebo-port-in PORT set port num for gazebo in\n"
"\t--gazebo-port-out PORT set port num for gazebo out\n"
"\t--defaults path set path to defaults file\n"
);
}
@ -136,6 +139,9 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
_fdm_address = "127.0.0.1";
_client_address = nullptr;
_use_fg_view = true;
_gazebo_address = "127.0.0.1";
_gazebo_port_in = 9003;
_gazebo_port_out = 9002;
_instance = 0;
enum long_options {
@ -150,6 +156,9 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
CMDLINE_UARTF,
CMDLINE_RTSCTS,
CMDLINE_FGVIEW,
CMDLINE_GAZEBO_ADDRESS,
CMDLINE_GAZEBO_PORT_IN,
CMDLINE_GAZEBO_PORT_OUT,
CMDLINE_DEFAULTS
};
@ -176,6 +185,9 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
{"defaults", true, 0, CMDLINE_DEFAULTS},
{"rtscts", false, 0, CMDLINE_RTSCTS},
{"disable-fgview", false, 0, CMDLINE_FGVIEW},
{"gazebo-address", true, 0, CMDLINE_GAZEBO_ADDRESS},
{"gazebo-port-in", true, 0, CMDLINE_GAZEBO_PORT_IN},
{"gazebo-port-out", true, 0, CMDLINE_GAZEBO_PORT_OUT},
{0, false, 0, 0}
};
@ -203,6 +215,8 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
_rcout_port += _instance * 10;
_rcin_port += _instance * 10;
_fg_view_port += _instance * 10;
_gazebo_port_in += _instance * 10;
_gazebo_port_out += _instance * 10;
}
break;
case 'P':
@ -250,6 +264,15 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
case CMDLINE_FGVIEW:
_use_fg_view = false;
break;
case CMDLINE_GAZEBO_ADDRESS:
_gazebo_address = gopt.optarg;
break;
case CMDLINE_GAZEBO_PORT_IN:
_gazebo_port_in = atoi(gopt.optarg);
break;
case CMDLINE_GAZEBO_PORT_OUT:
_gazebo_port_out = atoi(gopt.optarg);
break;
default:
_usage();
exit(1);
@ -264,6 +287,9 @@ void SITL_State::_parse_command_line(int argc, char * const argv[])
for (uint8_t i=0; i < ARRAY_SIZE(model_constructors); i++) {
if (strncasecmp(model_constructors[i].name, model_str, strlen(model_constructors[i].name)) == 0) {
sitl_model = model_constructors[i].constructor(home_str, model_str);
if (strncasecmp("gazebo", model_str, strlen("gazebo")) == 0) {
sitl_model->set_interface_ports(_gazebo_address, _gazebo_port_in, _gazebo_port_out);
}
sitl_model->set_speedup(speedup);
sitl_model->set_instance(_instance);
sitl_model->set_autotest_dir(autotest_dir);

24
libraries/SITL/SIM_Aircraft.cpp
View File

@ -21,6 +21,8 @@
#include <stdio.h>
#include <sys/time.h>
#include <unistd.h>
#include <errno.h>
#ifdef __CYGWIN__
#include <windows.h>
@ -53,6 +55,8 @@ Aircraft::Aircraft(const char *home_str, const char *frame_str) :
rate_hz(1200.0f),
autotest_dir(nullptr),
frame(frame_str),
socket_in(true),
socket_out(true),
#ifdef __CYGWIN__
min_sleep_time(20000)
#else
@ -408,6 +412,26 @@ void Aircraft::set_speedup(float speedup)
setup_frame_time(rate_hz, speedup);
}
/*
Create and set in/out socket
*/
void Aircraft::set_interface_ports(const char* address, const int port_in, const int port_out)
{
if (!socket_in.bind("127.0.0.1", port_in)) {
fprintf(stderr, "SITL: socket in bind failed - %s\n", strerror(errno));
exit(1);
}
socket_in.reuseaddress();
socket_in.set_blocking(false);
if (!socket_out.connect(address, port_out)) {
fprintf(stderr, "SITL: socket out bind failed - %s\n", strerror(errno));
exit(1);
}
socket_out.reuseaddress();
socket_out.set_blocking(false);
}
/*
update the simulation attitude and relative position
*/

8
libraries/SITL/SIM_Aircraft.h
View File

@ -20,9 +20,11 @@
#include <AP_Math/AP_Math.h>
#include <AP_HAL/utility/Socket.h>
#include "SITL.h"
#include <AP_Terrain/AP_Terrain.h>
namespace SITL {
/*
@ -66,6 +68,9 @@ public:
autotest_dir = _autotest_dir;
}
/* Create and set in/out socket */
void set_interface_ports(const char* address, const int port_in, const int port_out);
/*
step the FDM by one time step
*/
@ -159,6 +164,9 @@ protected:
bool use_time_sync = true;
float last_speedup = -1;
SocketAPM socket_in;
SocketAPM socket_out;
enum {
GROUND_BEHAVIOR_NONE = 0,
GROUND_BEHAVIOR_NO_MOVEMENT,

12
libraries/SITL/SIM_Gazebo.cpp
View File

@ -28,17 +28,13 @@ namespace SITL {
Gazebo::Gazebo(const char *home_str, const char *frame_str) :
Aircraft(home_str, frame_str),
last_timestamp(0),
sock(true)
last_timestamp(0)
{
// try to bind to a specific port so that if we restart ArduPilot
// Gazebo keeps sending us packets. Not strictly necessary but
// useful for debugging
sock.bind("127.0.0.1", 9003);
fprintf(stdout, "Starting SITL Gazebo\n");
sock.reuseaddress();
sock.set_blocking(false);
fprintf(stdout, "bind\n");
}
/*
@ -53,7 +49,7 @@ void Gazebo::send_servos(const struct sitl_input &input)
{
pkt.motor_speed[i] = (input.servos[i]-1000) / 1000.0f;
}
sock.sendto(&pkt, sizeof(servo_packet), "127.0.0.1", 9002);
socket_out.send(&pkt, sizeof(pkt));
}
/*
@ -68,7 +64,7 @@ void Gazebo::recv_fdm(const struct sitl_input &input)
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 (sock.recv(&pkt, sizeof(pkt), 100) != sizeof(pkt)) {
while (socket_in.recv(&pkt, sizeof(pkt), 100) != sizeof(pkt)) {
send_servos(input);
}

6
libraries/SITL/SIM_Gazebo.h
View File

@ -18,8 +18,6 @@
#pragma once
#include <AP_HAL/utility/Socket.h>
#include "SIM_Aircraft.h"
namespace SITL {
@ -60,13 +58,11 @@ private:
double position_xyz[3];
};
void send_servos_heli(const struct sitl_input &input);
void send_servos_fixed_wing(const struct sitl_input &input);
void recv_fdm(const struct sitl_input &input);
void send_servos(const struct sitl_input &input);
double last_timestamp;
SocketAPM sock;
};
} // namespace SITL