diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index dc172add03b..00df6c668c5 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -171,9 +171,9 @@ private: bool _use_fg_view; const char *_fdm_address; - const char *_gazebo_address; - int _gazebo_port_in; - int _gazebo_port_out; + const char *_simulator_address; + int _simulator_port_in; + int _simulator_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 cb3ada17b7e..ef514c8e1fe 100644 --- a/libraries/AP_HAL_SITL/SITL_cmdline.cpp +++ b/libraries/AP_HAL_SITL/SITL_cmdline.cpp @@ -58,9 +58,9 @@ void SITL_State::_usage(void) "\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--sim-address ADDR set address string for simulator\n" + "\t--sim-port-in PORT set port num for simulator in\n" + "\t--sim-port-out PORT set port num for simulator out\n" "\t--irlock-port PORT set port num for irlock\n" "\t--rc-in-port PORT set port num for rc in\n" "\t--rc-out-port PORT set port num for rc out\n" @@ -140,8 +140,8 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) const int RCOUT_PORT = 5502; const int RCIN_PORT = 5501; const int FG_VIEW_PORT = 5503; - const int GAZEBO_IN_PORT = 9003; - const int GAZEBO_OUT_PORT = 9002; + const int SIM_IN_PORT = 9003; + const int SIM_OUT_PORT = 9002; const int IRLOCK_PORT = 9005; _base_port = BASE_PORT; _rcout_port = RCOUT_PORT; @@ -150,9 +150,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 = GAZEBO_IN_PORT; - _gazebo_port_out = GAZEBO_OUT_PORT; + _simulator_address = "127.0.0.1"; + _simulator_port_in = SIM_IN_PORT; + _simulator_port_out = SIM_OUT_PORT; _irlock_port = IRLOCK_PORT; _instance = 0; @@ -168,9 +168,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_SIM_ADDRESS, + CMDLINE_SIM_PORT_IN, + CMDLINE_SIM_PORT_OUT, CMDLINE_BASE_PORT, CMDLINE_IRLOCK_PORT, CMDLINE_RCIN_PORT, @@ -201,9 +201,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}, + {"sim-address", true, 0, CMDLINE_SIM_ADDRESS}, + {"sim-port-in", true, 0, CMDLINE_SIM_PORT_IN}, + {"sim-port-out", true, 0, CMDLINE_SIM_PORT_OUT}, {"base-port", true, 0, CMDLINE_BASE_PORT}, {"irlock-port", true, 0, CMDLINE_IRLOCK_PORT}, {"rc-in-port", true, 0, CMDLINE_RCIN_PORT}, @@ -243,11 +243,11 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) if (_fg_view_port == FG_VIEW_PORT) { _fg_view_port += _instance * 10; } - if (_gazebo_port_in == GAZEBO_IN_PORT) { - _gazebo_port_in += _instance * 10; + if (_simulator_port_in == SIM_IN_PORT) { + _simulator_port_in += _instance * 10; } - if (_gazebo_port_out == GAZEBO_OUT_PORT) { - _gazebo_port_out += _instance * 10; + if (_simulator_port_out == SIM_OUT_PORT) { + _simulator_port_out += _instance * 10; } if (_irlock_port == IRLOCK_PORT) { _irlock_port += _instance * 10; @@ -299,14 +299,14 @@ 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; + case CMDLINE_SIM_ADDRESS: + _simulator_address = gopt.optarg; break; - case CMDLINE_GAZEBO_PORT_IN: - _gazebo_port_in = atoi(gopt.optarg); + case CMDLINE_SIM_PORT_IN: + _simulator_port_in = atoi(gopt.optarg); break; - case CMDLINE_GAZEBO_PORT_OUT: - _gazebo_port_out = atoi(gopt.optarg); + case CMDLINE_SIM_PORT_OUT: + _simulator_port_out = atoi(gopt.optarg); break; case CMDLINE_IRLOCK_PORT: _irlock_port = atoi(gopt.optarg); @@ -334,9 +334,7 @@ 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_interface_ports(_simulator_address, _simulator_port_in, _simulator_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 ec2f5be8a12..49d1ca98ced 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -55,8 +55,6 @@ 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 @@ -412,30 +410,6 @@ 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 on sim in : %d - %s\n", port_in, strerror(errno)); - fprintf(stderr, "Abording launch...\n"); - exit(1); - } - printf("Bind %s:%d for SITL in\n", "127.0.0.1", port_in); - socket_in.reuseaddress(); - socket_in.set_blocking(false); - - if (!socket_out.connect(address, port_out)) { - fprintf(stderr, "SITL: socket out bind failed on sim out : %d - %s\n", port_out, strerror(errno)); - fprintf(stderr, "Abording launch...\n"); - exit(1); - } - printf("Bind %s:%d for SITL out\n", address, port_out); - 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 fb4c0391d5d..eabdb1335e4 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -68,8 +68,8 @@ 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); + /* Create and set in/out socket for extenal simulator */ + virtual void set_interface_ports(const char* address, const int port_in, const int port_out) {}; /* step the FDM by one time step @@ -164,9 +164,6 @@ 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 4e58c2d36ba..4c03464d41b 100644 --- a/libraries/SITL/SIM_Gazebo.cpp +++ b/libraries/SITL/SIM_Gazebo.cpp @@ -29,7 +29,8 @@ namespace SITL { Gazebo::Gazebo(const char *home_str, const char *frame_str) : Aircraft(home_str, frame_str), - last_timestamp(0) + last_timestamp(0), + socket_sitl{true} { // try to bind to a specific port so that if we restart ArduPilot // Gazebo keeps sending us packets. Not strictly necessary but @@ -38,6 +39,25 @@ Gazebo::Gazebo(const char *home_str, const char *frame_str) : } +/* + Create and set in/out socket +*/ +void Gazebo::set_interface_ports(const char* address, const int port_in, const int port_out) +{ + 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, "Abording 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); + + _gazebo_address = address; + _gazebo_port = port_out; + printf("Setting Gazebo interface to %s:%d \n", _gazebo_address, _gazebo_port); +} + /* decode and send servos */ @@ -50,7 +70,7 @@ void Gazebo::send_servos(const struct sitl_input &input) { pkt.motor_speed[i] = (input.servos[i]-1000) / 1000.0f; } - socket_out.send(&pkt, sizeof(pkt)); + socket_sitl.sendto(&pkt, sizeof(pkt), _gazebo_address, _gazebo_port); } /* @@ -65,7 +85,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 (socket_in.recv(&pkt, sizeof(pkt), 100) != sizeof(pkt)) { + while (socket_sitl.recv(&pkt, sizeof(pkt), 100) != sizeof(pkt)) { send_servos(input); } @@ -123,7 +143,7 @@ void Gazebo::drain_sockets() ssize_t received; errno = 0; do { - received = socket_in.recv(buf, buflen, 0); + 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", diff --git a/libraries/SITL/SIM_Gazebo.h b/libraries/SITL/SIM_Gazebo.h index 457939291ca..e322f12f43e 100644 --- a/libraries/SITL/SIM_Gazebo.h +++ b/libraries/SITL/SIM_Gazebo.h @@ -37,6 +37,9 @@ public: return new Gazebo(home_str, frame_str); } + /* Create and set in/out socket for Gazebo simulator */ + void set_interface_ports(const char* address, const int port_in, const int port_out); + private: /* packet sent to Gazebo @@ -64,6 +67,9 @@ private: double last_timestamp; + SocketAPM socket_sitl; + const char *_gazebo_address = "127.0.0.1"; + int _gazebo_port = 9002; }; } // namespace SITL