Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : /*
      16             :   simulator connection for webots simulator https://github.com/omichel/webots
      17             : */
      18             : 
      19             : #pragma once
      20             : 
      21             : #include "SIM_config.h"
      22             : 
      23             : #if AP_SIM_WEBOTS_ENABLED
      24             : 
      25             : #include <cmath>
      26             : #include <AP_HAL/utility/Socket_native.h>
      27             : #include "SIM_Aircraft.h"
      28             : 
      29             : namespace SITL {
      30             : 
      31             : /*
      32             :   simulation interface
      33             :  */
      34             : class Webots : public Aircraft {
      35             : public:
      36             :     Webots(const char *frame_str);
      37             : 
      38             :     /* update model by one time step */
      39             :     void update(const struct sitl_input &input) override;
      40             : 
      41             :     /* output servo to simulator */
      42             :     void output(const struct sitl_input &input);
      43             : 
      44             :     /* static object creator */
      45           0 :     static Aircraft *create(const char *frame_str) {
      46           0 :         return NEW_NOTHROW Webots(frame_str);
      47             :     }
      48             : 
      49             :     
      50             : 
      51             : private:
      52             : 
      53             :     const char *webots_ip = "127.0.0.1";
      54             : 
      55             :     // assume sensors are streamed on port 5599
      56             :     uint16_t webots_sensors_port = 5599; 
      57             : 
      58             :     enum {
      59             :         OUTPUT_ROVER=1,
      60             :         OUTPUT_QUAD=2,
      61             :         OUTPUT_TRICOPTER=3,
      62             :         OUTPUT_PWM=4
      63             :     } output_type;
      64             : 
      65             :     bool connect_sockets(void);
      66             :     bool parse_sensors(const char *json);
      67             :     bool sensors_receive(void);
      68             :     void output_rover(const struct sitl_input &input);
      69             :     void output_quad(const struct sitl_input &input);
      70             :     void output_tricopter(const struct sitl_input &input);
      71             :     void output_pwm(const struct sitl_input &input);
      72             :     void report_FPS();
      73             : 
      74             :     // buffer for parsing pose data in JSON format
      75             :     uint8_t sensor_buffer[50000];
      76             :     uint32_t sensor_buffer_len;
      77             : 
      78             :     SocketAPM_native *sim_sock;
      79             : 
      80             :     uint32_t connect_counter;
      81             : 
      82             :     uint64_t socket_frame_counter;
      83             :     uint64_t last_socket_frame_counter;
      84             :     uint64_t frame_counter;
      85             :     
      86             :     double last_frame_count_s;
      87             : 
      88             :     enum data_type {
      89             :         DATA_FLOAT,
      90             :         DATA_DOUBLE,
      91             :         DATA_VECTOR3F,
      92             :         DATA_VECTOR3F_ARRAY,
      93             :         DATA_FLOAT_ARRAY,
      94             :     };
      95             : 
      96             :     struct {
      97             :         double timestamp;
      98             :         struct {
      99             :             Vector3f angular_velocity;
     100             :             Vector3f linear_acceleration;
     101             :             Vector3f magnetic_field;
     102             :         } imu;
     103             :         struct {
     104             :             float x, y, z;
     105             :         } gps;
     106             :         struct {
     107             :             float roll, pitch, yaw;
     108             :         } pose;
     109             :         struct {
     110             :             Vector3f world_linear_velocity;
     111             :         } velocity;
     112             :         struct {
     113             :             struct vector3f_array points;
     114             :             struct float_array ranges;
     115             :         } scanner;
     116             :     } state, last_state;
     117             : 
     118             :     // table to aid parsing of JSON sensor data
     119             :     struct keytable {
     120             :         const char *section;
     121             :         const char *key;
     122             :         void *ptr;
     123             :         enum data_type type;
     124             :     } keytable[13] = {
     125             :         { "", "ts", &state.timestamp, DATA_DOUBLE },
     126             :         { ".imu", "av",    &state.imu.angular_velocity, DATA_VECTOR3F },
     127             :         { ".imu", "la", &state.imu.linear_acceleration, DATA_VECTOR3F },
     128             :         { ".imu", "mf",      &state.imu.magnetic_field, DATA_VECTOR3F },
     129             :         { ".gps", "x", &state.gps.x, DATA_FLOAT },
     130             :         { ".gps", "y", &state.gps.y, DATA_FLOAT },
     131             :         { ".gps", "z", &state.gps.z, DATA_FLOAT },
     132             :         { ".pose", "roll",  &state.pose.roll, DATA_FLOAT },
     133             :         { ".pose", "pitch", &state.pose.pitch, DATA_FLOAT },
     134             :         { ".pose", "yaw",   &state.pose.yaw, DATA_FLOAT },
     135             :         { ".velocity", "wlv", &state.velocity.world_linear_velocity, DATA_VECTOR3F },
     136             :         { ".scan", "pl", &state.scanner.points, DATA_VECTOR3F_ARRAY },
     137             :         { ".scan", "rl", &state.scanner.ranges, DATA_FLOAT_ARRAY },
     138             :     };
     139             : };
     140             : 
     141             : 
     142             : } // namespace SITL
     143             : 
     144             : #endif // AP_SIM_WEBOTS_ENABLED