Report full attitude, fix lat/lon bug, report attitude even if no gps…

… fix (#264)

* Report full attitude, fix lat/lon bug, report attitude even if no gps fix

* Address comments

.clang-format

@@ -17,3 +17,4 @@ IncludeCategories:
     Priority:	3
 IndentCaseLabels: true
 PointerAlignment: Left
+AlwaysBreakTemplateDeclarations: Yes

src/ardupilot/ArduPilotInterface.cpp

@@ -25,11 +25,7 @@ DataPoint<gpscoords_t> readGPSCoords() {
 } // namespace gps
 
 namespace robot {
-DataPoint<double> readIMUHeading() {
-	auto imu_heading = ardupilot_protocol->getIMU();
-	if (imu_heading.isValid()) {
-		return DataPoint<double>(imu_heading.getTime(), imu_heading.getData().yaw);
-	}
-	return DataPoint<double>();
+DataPoint<eulerangles_t> readIMU() {
+	return ardupilot_protocol->getIMU();
 }
 } // namespace robot

src/ardupilot/ArduPilotProtocol.cpp

@@ -65,7 +65,7 @@ bool ArduPilotProtocol::validateGPSRequest(const json& j) {
 
 void ArduPilotProtocol::handleGPSRequest(const json& j) {
 	double lat = j["lat"];
-	double lon = j["lat"];
+	double lon = j["lon"];
 	{
 		std::lock_guard<std::mutex> lock(_lastGPSMutex);
 		_lastGPS = gpscoords_t{lat, lon};

src/network/MissionControlProtocol.cpp

@@ -4,6 +4,7 @@
 #include "../Globals.h"
 #include "../base64/base64_img.h"
 #include "../log.h"
+#include "../utils/core.h"
 #include "../utils/json.h"
 #include "../world_interface/data.h"
 #include "../world_interface/world_interface.h"
@@ -185,24 +186,32 @@ void MissionControlProtocol::handleJointPowerRequest(const json& j) {
 }
 
 void MissionControlProtocol::sendRoverPos() {
-	auto heading = robot::readIMUHeading();
-	auto gps = robot::readGPS();
-	if (gps.isValid() && heading.isValid()) {
-		double orientX = 0.0;
-		double orientY = 0.0;
-		double orientZ = std::sin(heading.getData() / 2);
-		double orientW = std::cos(heading.getData() / 2);
-		double posX = gps.getData()[0];
-		double posY = gps.getData()[1];
+	auto imu = robot::readIMU();
+	auto gps = gps::readGPSCoords();
+	log(LOG_DEBUG, "imu_valid=%d, gps_valid=%d\n", util::to_string(imu.isValid()),
+		util::to_string(gps.isValid()));
+	if (imu.isValid()) {
+		auto rpy = imu.getData();
+		Eigen::Quaterniond quat(Eigen::AngleAxisd(rpy.roll, Eigen::Vector3d::UnitX()) *
+								Eigen::AngleAxisd(rpy.pitch, Eigen::Vector3d::UnitY()) *
+								Eigen::AngleAxisd(rpy.yaw, Eigen::Vector3d::UnitZ()));
+		double posX = 0, posY = 0;
+		if (gps.isValid()) {
+			posX = gps.getData().lon;
+			posY = gps.getData().lat;
+		}
 		double posZ = 0.0;
-		double gpsRecency = util::durationToSec(dataclock::now() - gps.getTime());
-		double headingRecency = util::durationToSec(dataclock::now() - heading.getTime());
-		double recency = std::max(gpsRecency, headingRecency);
+		double imuRecency = util::durationToSec(dataclock::now() - imu.getTime());
+		double recency = imuRecency;
+		if (gps.isValid()) {
+			double gpsRecency = util::durationToSec(dataclock::now() - gps.getTime());
+			recency = std::max(recency, gpsRecency);
+		}
 		json msg = {{"type", ROVER_POS_REP_TYPE},
-					{"orientW", orientW},
-					{"orientX", orientX},
-					{"orientY", orientY},
-					{"orientZ", orientZ},
+					{"orientW", quat.w()},
+					{"orientX", quat.x()},
+					{"orientY", quat.y()},
+					{"orientZ", quat.z()},
 					{"posX", posX},
 					{"posY", posY},
 					{"posZ", posZ},

src/utils/core.cpp

@@ -6,6 +6,11 @@ bool almostEqual(double a, double b, double threshold) {
 	return std::abs(a - b) < threshold;
 }
 
+template <>
+std::string to_string<bool>(const bool& val) {
+	return val ? "true" : "false";
+}
+
 frozen::string freezeStr(const std::string& str) {
 	return frozen::string(str.c_str(), str.size());
 }

src/utils/core.h

@@ -44,10 +44,20 @@ std::unordered_set<K> keySet(const std::unordered_map<K, V>& input) {
  * @return A string representation of that value, as a std::string. The exact representation of
  * the value is up to the implementation.
  */
-template <typename T> std::string to_string(const T& val) {
+template <typename T>
+std::string to_string(const T& val) {
 	return std::to_string(val);
 }
 
+/**
+ * @brief Converts a boolean to a string.
+ *
+ * @param val The value to get the string representation of.
+ * @return "true" iff val, otherwise "false".
+ */
+template <>
+std::string to_string<bool>(const bool& val);
+
 /**
  * @brief Convert the given std::string to a frozen::string.
  */

src/world_interface/data.h

@@ -6,6 +6,7 @@
 #include <bitset>
 #include <chrono>
 #include <optional>
+#include <type_traits>
 #include <vector>
 
 #include <frozen/string.h>
@@ -29,7 +30,8 @@ using dataclock = std::chrono::steady_clock;
 /** @brief A point in time as measured by dataclock */
 using datatime_t = std::chrono::time_point<dataclock>;
 
-template <typename T> class DataPoint;
+template <typename T>
+class DataPoint;
 
 /**
  * @brief A data structure that represents when each landmark was seen.
@@ -108,7 +110,8 @@ constexpr auto name_to_jointid = frozen::make_unordered_map<frozen::string, join
  *
  * @tparam T The type of data measured from the sensor.
  */
-template <typename T> class DataPoint {
+template <typename T>
+class DataPoint {
 public:
 	/**
 	 * @brief Construct an invalid DataPoint, holding no data
@@ -200,6 +203,24 @@ template <typename T> class DataPoint {
 		return isValid() ? getData() : defaultData;
 	}
 
+	/**
+	 * @brief Transforms the data in this datapoint by the given function.
+	 *
+	 * @tparam F A callable type that accepts @p T and outputs some other type.
+	 * @param f The function that transforms data.
+	 * @return DataPoint<std::invoke_result_t<F, T>> A new datapoint that holds
+	 * the output of `f(getData())` and has the same timestamp as this datapoint
+	 * if it's valid, otherwise an empty datapoint.
+	 */
+	template <typename F>
+	DataPoint<std::invoke_result_t<F, T>> transform(const F& f) {
+		if (isValid()) {
+			return DataPoint<std::invoke_result_t<F, T>>(getTime(), f(getData()));
+		} else {
+			return {};
+		}
+	}
+
 private:
 	/**
 	 * @brief The time and measurement data.

src/world_interface/gps_common_interface.cpp

@@ -10,6 +10,10 @@ static std::optional<GPSToMetersConverter> converter;
 
 namespace robot {
 
+DataPoint<double> readIMUHeading() {
+	return readIMU().transform([](const navtypes::eulerangles_t& e) { return e.yaw; });
+}
+
 DataPoint<point_t> readGPS() {
 	DataPoint<gpscoords_t> coords = gps::readGPSCoords();
 	if (!coords) {

src/world_interface/noop_world.cpp

@@ -43,7 +43,7 @@ landmarks_t readLandmarks() {
 	return landmarks_t{};
 }
 
-DataPoint<double> readIMUHeading() {
+DataPoint<navtypes::eulerangles_t> readIMU() {
 	return {};
 }
 

src/world_interface/simulator_interface.cpp

@@ -41,8 +41,8 @@ const std::map<motorid_t, std::string> motorNameMap = {
 	{motorid_t::wrist, "wrist"},
 	{motorid_t::hand, "hand"}};
 
-DataPoint<double> lastHeading;
-std::mutex headingMutex;
+DataPoint<navtypes::eulerangles_t> lastAttitude;
+std::mutex attitudeMutex;
 
 DataPoint<gpscoords_t> lastGPS;
 std::mutex gpsMutex;
@@ -149,8 +149,12 @@ void handleIMU(json msg) {
 	double qw = msg["w"];
 
 	double heading = util::quatToHeading(qw, qx, qy, qz);
-	std::lock_guard<std::mutex> guard(headingMutex);
-	lastHeading = {heading};
+	Eigen::Quaterniond q(qw, qx, qy, qz);
+	q.normalize();
+	Eigen::Vector3d euler = q.toRotationMatrix().eulerAngles(0, 1, 2);
+	std::lock_guard<std::mutex> guard(attitudeMutex);
+	lastAttitude =
+		navtypes::eulerangles_t{.roll = euler(0), .pitch = euler(1), .yaw = euler(2)};
 }
 
 void handleCamFrame(json msg) {
@@ -364,9 +368,9 @@ DataPoint<pose_t> readVisualOdomVel() {
 	return DataPoint<pose_t>{};
 }
 
-DataPoint<double> readIMUHeading() {
-	std::lock_guard<std::mutex> guard(headingMutex);
-	return lastHeading;
+DataPoint<navtypes::eulerangles_t> readIMU() {
+	std::lock_guard<std::mutex> guard(attitudeMutex);
+	return lastAttitude;
 }
 
 DataPoint<pose_t> getTruePose() {

src/world_interface/world_interface.h

@@ -165,6 +165,17 @@ types::DataPoint<navtypes::point_t> readGPS();
  */
 types::DataPoint<double> readIMUHeading();
 
+/**
+ * @brief Read the rover attitude from the IMU.
+ *
+ * Euler angles are in radians, in RPY format (i.e. XYZ extrinsic)
+ *
+ * @see https://en.wikipedia.org/wiki/Euler_angles#Tait%E2%80%93Bryan_angles
+ *
+ * @return types::DataPoint<navtypes::eulerangles_t>
+ */
+types::DataPoint<navtypes::eulerangles_t> readIMU();
+
 /**
  * @brief Get the ground truth pose, if available. This is only available in simulation.
  *