slight refactoring of simulated sensors, added untested noise

umrover · Feb 16, 2024 · 8ef4b5d · qhdwight · Feb 16, 2024 · 8ef4b5d
1 parent a45e9b9
commit 8ef4b5d
Show file tree

Hide file tree

Showing 3 changed files with 39 additions and 9 deletions.
diff --git a/src/simulator/pch.hpp b/src/simulator/pch.hpp
@@ -15,6 +15,7 @@
 #include <stdexcept>
 #include <thread>
 #include <unordered_set>
+#include <random>
 
 #include <boost/circular_buffer.hpp>
 #include <boost/container/static_vector.hpp>

diff --git a/src/simulator/simulator.hpp b/src/simulator/simulator.hpp
@@ -225,6 +225,10 @@ namespace mrover {
         R3 mGpsLinerizationReferencePoint{};
         double mGpsLinerizationReferenceHeading{};
 
+        // TODO: make variances configurable
+        std::default_random_engine mRNG;
+        std::normal_distribution<double> mGPSDist{0, 0.5}, mAccelDist{0, 0.1}, mGyroDist{0, 0.1}, mMagDist{0, 0.1}, mOrientationDist{0, 0.1};
+
         PeriodicTask mGpsTask;
         PeriodicTask mImuTask;
 

diff --git a/src/simulator/simulator.sensors.cpp b/src/simulator/simulator.sensors.cpp
@@ -111,18 +111,18 @@ namespace mrover {
         return {lat, lon, alt};
     }
 
-    auto computeNavSatFix(SE3d const& gpsInMap, Eigen::Vector3d const& referenceGeodetic, double referenceHeadingDegrees) -> sensor_msgs::NavSatFix {
+    auto computeNavSatFix(R3 const& gpsInMap, Eigen::Vector3d const& referenceGeodetic, double referenceHeadingDegrees) -> sensor_msgs::NavSatFix {
         sensor_msgs::NavSatFix gpsMessage;
         gpsMessage.header.stamp = ros::Time::now();
         gpsMessage.header.frame_id = "map";
-        auto geodetic = cartesianToGeodetic(gpsInMap.translation(), referenceGeodetic, referenceHeadingDegrees);
+        auto geodetic = cartesianToGeodetic(gpsInMap, referenceGeodetic, referenceHeadingDegrees);
         gpsMessage.latitude = geodetic(0);
         gpsMessage.longitude = geodetic(1);
         gpsMessage.altitude = geodetic(2);
         return gpsMessage;
     }
 
-    auto computeImu(SE3d const& imuInMap, R3 const& imuAngularVelocity, R3 const& linearAcceleration, R3 const& magneticField) -> ImuAndMag {
+    auto computeImu(SO3d const& imuInMap, R3 const& imuAngularVelocity, R3 const& linearAcceleration, R3 const& magneticField) -> ImuAndMag {
         ImuAndMag imuMessage;
         imuMessage.header.stamp = ros::Time::now();
         imuMessage.header.frame_id = "map";
@@ -165,20 +165,45 @@ namespace mrover {
                 odometry.pose.pose.orientation.x = q.x();
                 odometry.pose.pose.orientation.y = q.y();
                 odometry.pose.pose.orientation.z = q.z();
-                // TODO(quintin, riley): fill in twist?
+                R3 v = btVector3ToR3(rover.physics->getBaseVel());
+                odometry.twist.twist.linear.x = v.x();
+                odometry.twist.twist.linear.y = v.y();
+                odometry.twist.twist.linear.z = v.z();
+                R3 w = btVector3ToR3(rover.physics->getBaseOmega());
+                odometry.twist.twist.angular.x = w.x();
+                odometry.twist.twist.angular.y = w.y();
+                odometry.twist.twist.angular.z = w.z();
                 mGroundTruthPub.publish(odometry);
             }
             if (mGpsTask.shouldUpdate()) {
-                SE3d gpsInMap = rover.linkInWorld("chassis_link");
+                R3 gpsInMap = rover.linkInWorld("chassis_link").translation();
+                R3 gpsNoise;
+                gpsNoise << mGPSDist(mRNG), mGPSDist(mRNG), mGPSDist(mRNG);
+                gpsInMap += gpsNoise;
+
                 mGpsPub.publish(computeNavSatFix(gpsInMap, mGpsLinerizationReferencePoint, mGpsLinerizationReferenceHeading));
             }
             if (mImuTask.shouldUpdate()) {
-                R3 imuAngularVelocity = btVector3ToR3(rover.physics->getBaseOmega());
+                R3 roverAngularVelocity = btVector3ToR3(rover.physics->getBaseOmega());
                 R3 roverLinearVelocity = btVector3ToR3(rover.physics->getBaseVel());
-                R3 imuLinearAcceleration = (roverLinearVelocity - mRoverLinearVelocity) / std::chrono::duration_cast<std::chrono::duration<float>>(dt).count();
+                R3 roverLinearAcceleration = (roverLinearVelocity - mRoverLinearVelocity) / std::chrono::duration_cast<std::chrono::duration<float>>(dt).count();
                 mRoverLinearVelocity = roverLinearVelocity;
-                SE3d imuInMap = rover.linkInWorld("imu");
-                mImuPub.publish(computeImu(imuInMap, imuAngularVelocity, imuLinearAcceleration, imuInMap.asSO3().inverse().rotation().col(1)));
+                SO3d imuInMap = rover.linkInWorld("imu").asSO3();
+                R3 roverMagVector = imuInMap.inverse().rotation().col(1);
+
+                R3 accelNoise, gyroNoise, magNoise;
+                accelNoise << mAccelDist(mRNG), mAccelDist(mRNG), mAccelDist(mRNG);
+                gyroNoise << mGyroDist(mRNG), mGyroDist(mRNG), mGyroDist(mRNG);
+                magNoise << mMagDist(mRNG), mMagDist(mRNG), mMagDist(mRNG);
+                roverLinearAcceleration += accelNoise;
+                roverAngularVelocity += gyroNoise;
+                roverMagVector += magNoise;
+
+                SO3d::Tangent orientationNoise;
+                orientationNoise << mOrientationDist(mRNG), mOrientationDist(mRNG), mOrientationDist(mRNG);
+                imuInMap += orientationNoise;
+
+                mImuPub.publish(computeImu(imuInMap, roverAngularVelocity, roverLinearAcceleration, roverMagVector));
             }
         }
     }