SITL: enable multi sitl for gazebo

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;

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);

libraries/SITL/SIM_Aircraft.cpp

@@ -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
  */

libraries/SITL/SIM_Aircraft.h

@@ -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,

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);
     }
 

libraries/SITL/SIM_Gazebo.h

@@ -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