diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index 4e8ae809db..219cf2abfd 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -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; diff --git a/libraries/AP_HAL_SITL/SITL_cmdline.cpp b/libraries/AP_HAL_SITL/SITL_cmdline.cpp index 8cf0446d1c..b1283e8f4c 100644 --- a/libraries/AP_HAL_SITL/SITL_cmdline.cpp +++ b/libraries/AP_HAL_SITL/SITL_cmdline.cpp @@ -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); diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 8f15f12e91..97f18bc915 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -21,6 +21,8 @@ #include #include #include +#include + #ifdef __CYGWIN__ #include @@ -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 */ diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 11a4c5f17b..fb4c0391d5 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -20,9 +20,11 @@ #include +#include #include "SITL.h" #include + 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, diff --git a/libraries/SITL/SIM_Gazebo.cpp b/libraries/SITL/SIM_Gazebo.cpp index 2113f9742f..43c1a44a17 100644 --- a/libraries/SITL/SIM_Gazebo.cpp +++ b/libraries/SITL/SIM_Gazebo.cpp @@ -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); } diff --git a/libraries/SITL/SIM_Gazebo.h b/libraries/SITL/SIM_Gazebo.h index 6fd0a7785a..438f9b45d6 100644 --- a/libraries/SITL/SIM_Gazebo.h +++ b/libraries/SITL/SIM_Gazebo.h @@ -18,8 +18,6 @@ #pragma once -#include - #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