ArduPilot Sensor Updates

AirLib/include/common/AirSimSettings.hpp

@@ -215,6 +215,7 @@ struct AirSimSettings {
         Vector3r position = VectorMath::nanVector();
         Rotation rotation = Rotation::nanRotation();
         bool draw_debug_points = false;
+        bool external_controller = true;
     };
 
     struct LidarSetting : SensorSetting {
@@ -235,6 +236,8 @@ struct AirSimSettings {
 
         bool draw_debug_points = false;
         std::string data_frame = AirSimSettings::kVehicleInertialFrame;
+
+        bool external_controller = true;
     };
 
     struct VehicleSetting {
@@ -1237,6 +1240,7 @@ struct AirSimSettings {
         distance_setting.min_distance = settings_json.getFloat("MinDistance", distance_setting.min_distance);
         distance_setting.max_distance = settings_json.getFloat("MaxDistance", distance_setting.max_distance);
         distance_setting.draw_debug_points = settings_json.getBool("DrawDebugPoints", distance_setting.draw_debug_points);
+        distance_setting.external_controller = settings_json.getBool("ExternalController", distance_setting.external_controller);
 
         distance_setting.position = createVectorSetting(settings_json, distance_setting.position);
         distance_setting.rotation = createRotationSetting(settings_json, distance_setting.rotation);
@@ -1250,6 +1254,7 @@ struct AirSimSettings {
         lidar_setting.horizontal_rotation_frequency = settings_json.getInt("RotationsPerSecond", lidar_setting.horizontal_rotation_frequency);
         lidar_setting.draw_debug_points = settings_json.getBool("DrawDebugPoints", lidar_setting.draw_debug_points);
         lidar_setting.data_frame = settings_json.getString("DataFrame", lidar_setting.data_frame);
+        lidar_setting.external_controller = settings_json.getBool("ExternalController", lidar_setting.external_controller);
 
         lidar_setting.vertical_FOV_upper = settings_json.getFloat("VerticalFOVUpper", lidar_setting.vertical_FOV_upper);
         lidar_setting.vertical_FOV_lower = settings_json.getFloat("VerticalFOVLower", lidar_setting.vertical_FOV_lower);

AirLib/include/sensors/distance/DistanceSimpleParams.hpp

@@ -21,6 +21,7 @@ struct DistanceSimpleParams {
     };
 
     bool draw_debug_points = false;
+    bool external_controller = true;
 
 /*
     Ref: A Stochastic Approach to Noise Modeling for Barometric Altimeters
@@ -52,6 +53,7 @@ struct DistanceSimpleParams {
         max_distance = settings.max_distance;
 
         draw_debug_points = settings.draw_debug_points;
+        external_controller = settings.external_controller;
 
         relative_pose.position = settings.position;
         if (std::isnan(relative_pose.position.x()))

AirLib/include/sensors/lidar/LidarSimpleParams.hpp

@@ -34,6 +34,8 @@ struct LidarSimpleParams {
     bool draw_debug_points = false;
     std::string data_frame = AirSimSettings::kVehicleInertialFrame;
 
+    bool external_controller = true;
+
     real_T update_frequency = 10;             // Hz
     real_T startup_delay = 0;                 // sec
 
@@ -90,6 +92,7 @@ struct LidarSimpleParams {
 
         draw_debug_points = settings.draw_debug_points;
         data_frame = settings.data_frame;
+        external_controller = settings.external_controller;
     }
 };
 

AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp

@@ -15,7 +15,8 @@
 #include "sensors/gps/GpsBase.hpp"
 #include "sensors/magnetometer/MagnetometerBase.hpp"
 #include "sensors/barometer/BarometerBase.hpp"
-#include "sensors/lidar/LidarBase.hpp"
+#include "sensors/distance/DistanceSimple.hpp"
+#include "sensors/lidar/LidarSimple.hpp"
 
 #include "UdpSocket.hpp"
 
@@ -122,8 +123,8 @@ class ArduRoverApi : public CarApiBase {
 protected:
     void closeConnections()
     {
-        if (udpSocket_ != nullptr)
-            udpSocket_->close();
+        if (udp_socket_ != nullptr)
+            udp_socket_->close();
     }
 
     void connect()
@@ -144,24 +145,24 @@ class ArduRoverApi : public CarApiBase {
         Utils::log(Utils::stringf("Using UDP port %d, local IP %s, remote IP %s for sending sensor data", port_, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo);
         Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo);
 
-        udpSocket_ = std::make_shared<mavlinkcom::UdpSocket>();
-        udpSocket_->bind(connection_info_.local_host_ip, connection_info_.control_port);
+        udp_socket_ = std::make_unique<mavlinkcom::UdpSocket>();
+        udp_socket_->bind(connection_info_.local_host_ip, connection_info_.control_port);
     }
 
 private:
     void recvRoverControl()
     {
         // Receive motor data
         RoverControlMessage pkt;
-        int recv_ret = udpSocket_->recv(&pkt, sizeof(pkt), 100);
+        int recv_ret = udp_socket_->recv(&pkt, sizeof(pkt), 100);
         while (recv_ret != sizeof(pkt)) {
             if (recv_ret <= 0) {
                 Utils::log(Utils::stringf("Error while receiving rotor control data - ErrorNo: %d", recv_ret), Utils::kLogLevelInfo);
             } else {
                 Utils::log(Utils::stringf("Received %d bytes instead of %zu bytes", recv_ret, sizeof(pkt)), Utils::kLogLevelInfo);
             }
 
-            recv_ret = udpSocket_->recv(&pkt, sizeof(pkt), 100);
+            recv_ret = udp_socket_->recv(&pkt, sizeof(pkt), 100);
         }
 
         last_controls_.throttle = pkt.throttle;
@@ -181,83 +182,123 @@ class ArduRoverApi : public CarApiBase {
         if (sensors_ == nullptr)
             return;
 
-        const auto& gps_output = getGpsData("");
-        const auto& imu_output = getImuData("");
-
         std::ostringstream oss;
 
-        const uint count_lidars = getSensors().size(SensorBase::SensorType::Lidar);
-        // TODO: Add bool value in settings to check whether to send lidar data or not
-        // Since it's possible that we don't want to send the lidar data to Ardupilot but still have the lidar (maybe as a ROS topic)
+        const uint count_gps_sensors = sensors_->size(SensorBase::SensorType::Gps);
+        if (count_gps_sensors != 0) {
+            const auto& gps_output = getGpsData("");
+
+            oss << ","
+                   "\"gps\": {"
+                << std::fixed << std::setprecision(7)
+                << "\"lat\": " << gps_output.gnss.geo_point.latitude << ","
+                << "\"lon\": " << gps_output.gnss.geo_point.longitude << ","
+                << std::setprecision(3) << "\"alt\": " << gps_output.gnss.geo_point.altitude
+                << "},"
+
+                << "\"velocity\": {"
+                << std::setprecision(12)
+                << "\"world_linear_velocity\": [" 
+                << gps_output.gnss.velocity[0] << ","
+                << gps_output.gnss.velocity[1] << ","
+                << gps_output.gnss.velocity[2] << "]"
+                "}";
+        }
+
+        // Send Distance Sensors data if present
+        const uint count_distance_sensors = sensors_->size(SensorBase::SensorType::Distance);
+        if (count_distance_sensors != 0) {
+            // Start JSON element
+            oss << ","
+                    "\"rng\": {"
+                    "\"distances\": [";
+
+            // Used to avoid trailing comma
+            std::string sep = "";
+
+            // Add sensor outputs in the array
+            for (uint i=0; i<count_distance_sensors; ++i) {
+                const auto* distance_sensor = static_cast<const DistanceSimple*>(
+                                                sensors_->getByType(SensorBase::SensorType::Distance, i));
+                // Don't send the data if sending to external controller is disabled in settings
+                if (distance_sensor && distance_sensor->getParams().external_controller) {
+                    const auto& distance_output = distance_sensor->getOutput();
+                    // AP uses meters so no need to convert here
+                    oss << sep << distance_output.distance;
+                    sep = ",";
+                }
+            }
+
+            // Close JSON array & element
+            oss << "]}";
+        }
+
+        const uint count_lidars = sensors_->size(SensorBase::SensorType::Lidar);
         if (count_lidars != 0) {
-            const auto& lidar_output = getLidarData("");
             oss << ","
                    "\"lidar\": {"
                    "\"point_cloud\": [";
 
-            std::copy(lidar_output.point_cloud.begin(), lidar_output.point_cloud.end(), std::ostream_iterator<real_T>(oss, ","));
+            // Add sensor outputs in the array
+            for (uint i=0; i<count_lidars; ++i) {
+                const auto* lidar = static_cast<const LidarSimple*>(sensors_->getByType(SensorBase::SensorType::Lidar, i));
+
+                if (lidar && lidar->getParams().external_controller) {
+                    const auto& lidar_output = lidar->getOutput();
+                    std::copy(lidar_output.point_cloud.begin(), lidar_output.point_cloud.end(), std::ostream_iterator<real_T>(oss, ","));
+                    // AP backend only takes in a single Lidar sensor data currently
+                    break;
+                }
+            }
+
             oss << "]}";
         }
 
+        const auto& imu_output = getImuData("");
+
         float yaw;
         float pitch;
         float roll;
         VectorMath::toEulerianAngle(imu_output.orientation, pitch, roll, yaw);
 
+        // UDP packets have a maximum size limit of 65kB
         char buf[65000];
 
-        // TODO: Split the following sensor packet formation into different parts for individual sensors
-
-        // UDP packets have a maximum size limit of 65kB
         int ret = snprintf(buf, sizeof(buf),
                            "{"
                            "\"timestamp\": %" PRIu64 ","
                            "\"imu\": {"
                            "\"angular_velocity\": [%.12f, %.12f, %.12f],"
                            "\"linear_acceleration\": [%.12f, %.12f, %.12f]"
                            "},"
-                           "\"gps\": {"
-                           "\"lat\": %.7f,"
-                           "\"lon\": %.7f,"
-                           "\"alt\": %.3f"
-                           "},"
-                           "\"velocity\": {"
-                           "\"world_linear_velocity\": [%.12f, %.12f, %.12f]"
-                           "},"
                            "\"pose\": {"
                            "\"roll\": %.12f,"
                            "\"pitch\": %.12f,"
                            "\"yaw\": %.12f"
                            "}"
                            "%s"
                            "}\n",
-                           static_cast<uint64_t>(msr::airlib::ClockFactory::get()->nowNanos() / 1.0E3),
+                           static_cast<uint64_t>(ClockFactory::get()->nowNanos() / 1.0E3),
                            imu_output.angular_velocity[0],
                            imu_output.angular_velocity[1],
                            imu_output.angular_velocity[2],
                            imu_output.linear_acceleration[0],
                            imu_output.linear_acceleration[1],
                            imu_output.linear_acceleration[2],
-                           gps_output.gnss.geo_point.latitude,
-                           gps_output.gnss.geo_point.longitude,
-                           gps_output.gnss.geo_point.altitude,
-                           gps_output.gnss.velocity[0],
-                           gps_output.gnss.velocity[1],
-                           gps_output.gnss.velocity[2],
                            roll, pitch, yaw,
                            oss.str().c_str());
 
         if (ret == -1) {
-            Utils::log("Error while allocating memory for sensor message", Utils::kLogLevelInfo);
+            Utils::log("Error while allocating memory for sensor message", Utils::kLogLevelError);
             return;
         }
         else if (static_cast<uint>(ret) >= sizeof(buf)) {
-            Utils::log(Utils::stringf("Sensor message truncated, lost %d bytes", ret - sizeof(buf)), Utils::kLogLevelInfo);
+            Utils::log(Utils::stringf("Sensor message truncated, lost %d bytes", ret - sizeof(buf)), Utils::kLogLevelWarn);
         }
 
         // Send data
-        if (udpSocket_ != nullptr) {
-            udpSocket_->sendto(buf, strlen(buf), ip_, port_);
+        if (udp_socket_ != nullptr) {
+            udp_socket_->sendto(buf, strlen(buf), ip_, port_);
         }
     }
 
@@ -269,7 +310,7 @@ class ArduRoverApi : public CarApiBase {
 
     AirSimSettings::MavLinkConnectionInfo connection_info_;
 
-    std::shared_ptr<mavlinkcom::UdpSocket> udpSocket_;
+    std::unique_ptr<mavlinkcom::UdpSocket> udp_socket_;
 
     uint16_t port_;
     std::string ip_;