diff --git a/src/autonomous/AutonomousTask.cpp b/src/autonomous/AutonomousTask.cpp index dc0c53206..994e4fe81 100644 --- 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 index 010e8ede1..c735787c7 100644 --- a/src/network/MissionControlProtocol.cpp +++ b/src/network/MissionControlProtocol.cpp @@ -226,19 +226,19 @@ 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, "isApproximate", val_t::boolean) && - util::validateKey(j, "isGate", val_t::boolean); + util::validateKey(j, "longitude", val_t::string); + 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"]; + 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); } }