fix autonomous: SQUASH ME

huskyroboticsteam · Nov 4, 2023 · ab0eb15 · ab0eb15
1 parent 34be8ec
commit ab0eb15
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 10 deletions.
diff --git a/src/autonomous/AutonomousTask.cpp b/src/autonomous/AutonomousTask.cpp
@@ -38,7 +38,7 @@ void AutonomousTask::navigate() {
 
 		if (latestGPS.isFresh(2000ms) && latestHeading.isFresh(2000ms)) {
 			auto gpsData = latestGPS.getData();
-			navtypes::pose_t latestPos(gpsData.x(), gpsData.y(), latestHeading);
+			navtypes::pose_t latestPos(gpsData.x(), gpsData.y(), latestHeading.getData());
 			cmd.setState(latestPos, robot::types::dataclock::now());
 			commands::command_t output = cmd.getOutput();
 			robot::setCmdVel(output.thetaVel, output.xVel);

diff --git a/src/network/MissionControlProtocol.cpp b/src/network/MissionControlProtocol.cpp
@@ -224,19 +224,20 @@ void MissionControlProtocol::sendRoverPos() {
 
 static bool validateWaypointNavRequest(const json& j) {
 	return util::validateKey(j, "latitude", val_t::string) &&
-		   util::validateKey(j, "longitude", val_t::string) &&
+		   util::validateKey(j, "longitude", val_t::string);
+           // FIXME: Mission Control does not always send the below two values
 		   util::validateKey(j, "isApproximate", val_t::boolean) &&
 		   util::validateKey(j, "isGate", val_t::boolean);
 }
 
 void MissionControlProtocol::handleWaypointNavRequest(const json& j) {
-	double latitude = j["latitude"];
-	double longitude = j["longitude"];
-	bool isApproximate = j["isApproximate"];
-	bool isGate = j["isGate"];
+    std::string latitude = j["latitude"];
+    std::string longitude = j["longitude"];
+	//bool isApproximate = j["isApproximate"];
+	//bool isGate = j["isGate"];
 
-	if (Globals::AUTONOMOUS && !isApproximate && !isGate) {
-		navtypes::point_t waypointCoords(latitude, longitude, 1);
+	if (Globals::AUTONOMOUS) { // && !isApproximate && !isGate) {
+		navtypes::point_t waypointCoords(std::stod(latitude), std::stod(longitude), 1);
 		_autonomous_task.start(waypointCoords);
 	}
 }
@@ -413,8 +414,9 @@ MissionControlProtocol::MissionControlProtocol(SingleClientWSServer& server)
 		validateCameraStreamCloseRequest);
 	this->addMessageHandler(
 		WAYPOINT_NAV_REQ_TYPE,
-		std::bind(&MissionControlProtocol::handleWaypointNavRequest, this, _1),
-		validateWaypointNavRequest);
+		std::bind(&MissionControlProtocol::handleWaypointNavRequest, this, _1));
+        // TODO: Add this validation back in once message is fixed
+		//validateWaypointNavRequest);
 	this->addConnectionHandler(std::bind(&MissionControlProtocol::handleConnection, this));
 	this->addDisconnectionHandler(
 		std::bind(&MissionControlProtocol::stopAndShutdownPowerRepeat, this));