Use Sensor APIs for getting data

Firmwares: PX4, ArduPilot-Copter,Rover
microsoft · Mar 27, 2020 · f74faaa · f74faaa
1 parent ecb024f
commit f74faaa
Show file tree

Hide file tree

Showing 4 changed files with 23 additions and 92 deletions.
diff --git a/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp b/AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp
@@ -148,27 +148,6 @@ class ArduRoverApi : public CarApiBase {
         udpSocket_->bind(connection_info_.local_host_ip, connection_info_.control_port);
     }
 
-    const GpsBase* getGps() const
-    {
-        return static_cast<const GpsBase*>(sensors_->getByType(SensorBase::SensorType::Gps));
-    }
-    const ImuBase* getImu() const
-    {
-        return static_cast<const ImuBase*>(sensors_->getByType(SensorBase::SensorType::Imu));
-    }
-    const MagnetometerBase* getMagnetometer() const
-    {
-        return static_cast<const MagnetometerBase*>(sensors_->getByType(SensorBase::SensorType::Magnetometer));
-    }
-    const BarometerBase* getBarometer() const
-    {
-        return static_cast<const BarometerBase*>(sensors_->getByType(SensorBase::SensorType::Barometer));
-    }
-    const LidarBase* getLidar() const
-    {
-        return static_cast<const LidarBase*>(sensors_->getByType(SensorBase::SensorType::Lidar));
-    }
-
 private:
     void recvRoverControl()
     {
@@ -202,18 +181,16 @@ class ArduRoverApi : public CarApiBase {
         if (sensors_ == nullptr)
             return;
 
-        const auto& gps_output = getGps()->getOutput();
-        const auto& imu_output = getImu()->getOutput();
-        // const auto& baro_output = getBarometer()->getOutput();
-        // const auto& mag_output = getMagnetometer()->getOutput();
+        const auto& gps_output = getGpsData("");
+        const auto& imu_output = getImuData("");
 
         std::ostringstream oss;
 
-        const auto lidar = getLidar();
+        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)
-        if (lidar != nullptr) {
-            const auto& lidar_output = lidar->getOutput();
+        if (count_lidars != 0) {
+            const auto& lidar_output = getLidarData("");
             oss << ","
                    "\"lidar\": {"
                    "\"point_cloud\": [";

diff --git a/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp
@@ -324,27 +324,6 @@ class ArduCopterApi : public MultirotorApiBase {
         udpSocket_->bind(connection_info_.local_host_ip, connection_info_.control_port);
     }
 
-    const GpsBase* getGps() const
-    {
-        return static_cast<const GpsBase*>(sensors_->getByType(SensorBase::SensorType::Gps));
-    }
-    const ImuBase* getImu() const
-    {
-        return static_cast<const ImuBase*>(sensors_->getByType(SensorBase::SensorType::Imu));
-    }
-    const MagnetometerBase* getMagnetometer() const
-    {
-        return static_cast<const MagnetometerBase*>(sensors_->getByType(SensorBase::SensorType::Magnetometer));
-    }
-    const BarometerBase* getBarometer() const
-    {
-        return static_cast<const BarometerBase*>(sensors_->getByType(SensorBase::SensorType::Barometer));
-    }
-    const LidarBase* getLidar() const
-    {
-        return static_cast<const LidarBase*>(sensors_->getByType(SensorBase::SensorType::Lidar));
-    }
-
 private:
     virtual void normalizeRotorControls()
     {
@@ -359,10 +338,8 @@ class ArduCopterApi : public MultirotorApiBase {
         if (sensors_ == nullptr)
             return;
 
-        const auto& gps_output = getGps()->getOutput();
-        const auto& imu_output = getImu()->getOutput();
-        // const auto& baro_output = getBarometer()->getOutput();
-        // const auto& mag_output = getMagnetometer()->getOutput();
+        const auto& gps_output = getGpsData("");
+        const auto& imu_output = getImuData("");
 
         std::ostringstream oss;
 
@@ -382,11 +359,11 @@ class ArduCopterApi : public MultirotorApiBase {
                 << "]}";
         }
 
-        const auto lidar = getLidar();
+        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)
-        if (lidar != nullptr) {
-            const auto& lidar_output = lidar->getOutput();
+        if (count_lidars != 0) {
+            const auto& lidar_output = getLidarData("");
             oss << ","
                    "\"lidar\": {"
                    "\"point_cloud\": [";

diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp
@@ -22,12 +22,10 @@ class ArduCopterSoloApi : public MavLinkMultirotorApi
 			return;
 
 		// send GPS and other sensor updates
-		const auto gps = getGps();
-		if (gps != nullptr) {
-			const auto& gps_output = gps->getOutput();
-			const auto& imu_output = getImu()->getOutput();
-			//                    const auto& mag_output = getMagnetometer()->getOutput();
-			//                    const auto& baro_output = getBarometer()->getOutput();
+        const uint count_gps_sensors = getSensors().size(SensorBase::SensorType::Gps);
+		if (count_gps_sensors != 0) {
+            const auto& gps_output = getGpsData("");
+            const auto& imu_output = getImuData("");
 
 			SensorMessage packet;
 			packet.timestamp = Utils::getTimeSinceEpochNanos() / 1000;

diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
@@ -104,19 +104,19 @@ class MavLinkMultirotorApi : public MultirotorApiBase
             }
 
             //send sensor updates
-            const auto& imu_output = getImu()->getOutput();
-            const auto& mag_output = getMagnetometer()->getOutput();
-            const auto& baro_output = getBarometer()->getOutput();
+            const auto& imu_output = getImuData("");
+            const auto& mag_output = getMagnetometerData("");
+            const auto& baro_output = getBarometerData("");
 
             sendHILSensor(imu_output.linear_acceleration,
                 imu_output.angular_velocity,
                 mag_output.magnetic_field_body,
                 baro_output.pressure * 0.01f /*Pa to Millibar */, baro_output.altitude);
 
 
-            const auto * distance = getDistance();
-            if (distance) {
-                const auto& distance_output = distance->getOutput();
+            const uint count_distance_sensors = getSensors().size(SensorBase::SensorType::Distance);
+            if (count_distance_sensors != 0) {
+                const auto& distance_output = getDistanceSensorData("");
                 float pitch, roll, yaw;
                 VectorMath::toEulerianAngle(distance_output.relative_pose.orientation, pitch, roll, yaw);
 
@@ -128,9 +128,9 @@ class MavLinkMultirotorApi : public MultirotorApiBase
                     pitch); //TODO: convert from radians to degrees?
             }
 
-            const auto gps = getGps();
-            if (gps != nullptr) {
-                const auto& gps_output = gps->getOutput();
+            const uint count_gps_sensors = getSensors().size(SensorBase::SensorType::Gps);
+            if (count_gps_sensors != 0) {
+                const auto& gps_output = getGpsData("");
 
                 //send GPS
                 if (gps_output.is_valid && gps_output.gnss.time_utc > last_gps_time_) {
@@ -669,27 +669,6 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         disconnect();
     }
 
-    const ImuBase* getImu() const
-    {
-        return static_cast<const ImuBase*>(sensors_->getByType(SensorBase::SensorType::Imu));
-    }
-    const MagnetometerBase* getMagnetometer() const
-    {
-        return static_cast<const MagnetometerBase*>(sensors_->getByType(SensorBase::SensorType::Magnetometer));
-    }
-    const BarometerBase* getBarometer() const
-    {
-        return static_cast<const BarometerBase*>(sensors_->getByType(SensorBase::SensorType::Barometer));
-    }
-    const DistanceBase* getDistance() const
-    {
-        return static_cast<const DistanceBase*>(sensors_->getByType(SensorBase::SensorType::Distance));
-    }
-    const GpsBase* getGps() const
-    {
-        return static_cast<const GpsBase*>(sensors_->getByType(SensorBase::SensorType::Gps));
-    }
-
     void closeAllConnection()
     {
         close();