Create AutonomousTask to handle autonomous navigation

Use DriveToWaypointCommand functionality to execute an autonomous task.

Add necessary constants to Constants.h

Start autonomous task from MissionControlProtocol.cpp

src/CMakeLists.txt

@@ -255,9 +255,13 @@ add_library(control SHARED
 )
 target_link_libraries(control utils)
 
-add_library(commands STATIC
+add_library(commands SHARED
   commands/DriveToWaypointCommand.cpp)
 
+add_library(autonomous SHARED
+  autonomous/AutonomousTask.cpp)
+target_link_libraries(autonomous commands)
+
 list(APPEND simulator_libs
   simulator_interface)
 

src/Constants.h

@@ -2,6 +2,7 @@
 
 #include "kinematics/DiffDriveKinematics.h"
 #include "world_interface/data.h"
+#include "utils/time.h"
 
 #include <array>
 #include <chrono>
@@ -184,3 +185,12 @@ constexpr frozen::unordered_map<robot::types::motorid_t, double, IK_MOTORS.size(
 
 constexpr double CONTROL_HZ = 10.0;
 } // namespace Constants
+
+namespace autonomous {
+constexpr double THETA_KP = 2.0;
+constexpr double DRIVE_VEL = 1.5;
+constexpr double SLOW_DRIVE_VEL = 1.0;
+constexpr double DONE_THRESHOLD = 0.5;
+// Duration long enough to confirm we are there, not so long that time is wasted
+constexpr util::dseconds CLOSE_TO_TARGET_DUR_VAL = std::chrono::seconds(1);
+} // namespace autonomous

src/autonomous/AutonomousTask.cpp

@@ -0,0 +1,70 @@
+#include "AutonomousTask.h"
+
+#include "../Constants.h"
+#include "../Globals.h"
+#include "../commands/DriveToWaypointCommand.h"
+#include "../world_interface/world_interface.h"
+
+using namespace std::chrono_literals;
+
+namespace autonomous {
+
+AutonomousTask::AutonomousTask(){};
+
+AutonomousTask::~AutonomousTask() {
+	if (_autonomous_task_thread.joinable()) {
+		_autonomous_task_thread.join();
+	}
+}
+
+void AutonomousTask::start(const navtypes::point_t& waypointCoords) {
+	if (_autonomous_task_thread.joinable()) {
+		kill();
+	}
+
+	_waypoint_coords = waypointCoords;
+	_kill_called = false;
+	_autonomous_task_thread = std::thread(&autonomous::AutonomousTask::navigate, this);
+}
+
+void AutonomousTask::navigate() {
+	commands::DriveToWaypointCommand cmd(_waypoint_coords, THETA_KP, DRIVE_VEL, SLOW_DRIVE_VEL,
+										 DONE_THRESHOLD, CLOSE_TO_TARGET_DUR_VAL);
+
+	auto sleepUntil = std::chrono::steady_clock().now();
+	while (!cmd.isDone()) {
+		auto latestGPS = robot::readGPS();
+		auto latestHeading = robot::readIMUHeading();
+
+		if (latestGPS.isFresh(2000ms) && latestHeading.isFresh(2000ms)) {
+			auto gpsData = latestGPS.getData();
+			navtypes::pose_t latestPos(gpsData.x(), gpsData.y(), latestHeading);
+			cmd.setState(latestPos, robot::types::dataclock::now());
+			commands::command_t output = cmd.getOutput();
+			robot::setCmdVel(output.thetaVel, output.xVel);
+		}
+
+		std::unique_lock autonomousTaskLock(_autonomous_task_mutex);
+		sleepUntil += 500ms;
+		// Wait 500ms or return out of while loop if kill called
+		if (_autonomous_task_cv.wait_until(autonomousTaskLock, sleepUntil,
+										   [&] { return _kill_called; })) {
+			return;
+		}
+	}
+}
+
+void AutonomousTask::kill() {
+	{
+		std::lock_guard lock(_autonomous_task_mutex);
+		_kill_called = true;
+	}
+
+	_autonomous_task_cv.notify_all();
+
+	if (_autonomous_task_thread.joinable()) {
+		_autonomous_task_thread.join();
+	}
+}
+
+} // namespace autonomous

src/autonomous/AutonomousTask.h

@@ -0,0 +1,53 @@
+#pragma once
+
+#include "../navtypes.h"
+
+#include <cmath>
+#include <condition_variable>
+#include <mutex>
+#include <thread>
+
+namespace autonomous {
+
+/*
+ * @brief This class facilitates autonomous navigation to a specified waypoint.
+ */
+class AutonomousTask {
+public:
+	/*
+	 * @brief Constructs a new AutonomousTask
+	 */
+	AutonomousTask();
+
+	/*
+	 * @brief Destructs AutonomousTask object, joins _autonomous_task_thread
+	 */
+	~AutonomousTask();
+
+	/*
+	 * @brief Starts an autonomous task to the provided waypoint
+	 *
+	 * @param navtypes::pose_t The waypoint to navigate to
+	 */
+	void start(const navtypes::point_t&);
+
+	/*
+	 * @brief Halts autonomous navigation by exiting the navigate() function and joining the
+	 *        autonomous task thread
+	 */
+	void kill();
+
+private:
+	/*
+	 * @brief Handles navigation to waypoint, called by start()
+	 */
+	void navigate();
+
+	navtypes::point_t _waypoint_coords;
+	std::mutex _autonomous_task_mutex;
+	std::thread _autonomous_task_thread;
+	std::condition_variable _autonomous_task_cv;
+	bool _kill_called;
+};
+
+} // namespace autonomous

src/network/CMakeLists.txt

@@ -8,6 +8,6 @@ target_link_libraries(websocket_utils
 
 add_library(mission_control_interface STATIC
   MissionControlProtocol.cpp)
-target_link_libraries(mission_control_interface utils video)
+target_link_libraries(mission_control_interface utils video autonomous)
 set_target_properties(mission_control_interface PROPERTIES POSITION_INDEPENDENT_CODE ON)
 

src/network/MissionControlProtocol.cpp

@@ -2,6 +2,7 @@
 
 #include "../Constants.h"
 #include "../Globals.h"
+#include "../autonomous/AutonomousTask.h"
 #include "../base64/base64_img.h"
 #include "../log.h"
 #include "../utils/core.h"
@@ -45,6 +46,7 @@ constexpr const char* MOTOR_POSITION_REQ_TYPE = "motorPositionRequest";
 constexpr const char* JOINT_POSITION_REQ_TYPE = "jointPositionRequest";
 constexpr const char* CAMERA_STREAM_OPEN_REQ_TYPE = "cameraStreamOpenRequest";
 constexpr const char* CAMERA_STREAM_CLOSE_REQ_TYPE = "cameraStreamCloseRequest";
+constexpr const char* WAYPOINT_NAV_REQ_TYPE = "waypointNavRequest";
 
 // report keys
 constexpr const char* MOTOR_STATUS_REP_TYPE = "motorStatusReport";
@@ -100,6 +102,7 @@ void MissionControlProtocol::handleOperationModeRequest(const json& j) {
 		// if we have entered autonomous mode, we need to stop all the power repeater stuff.
 		this->stopAndShutdownPowerRepeat();
 	} else {
+		_autonomous_task.kill();
 		// if we have left autonomous mode, we need to start the power repeater again.
 		this->startPowerRepeat();
 	}
@@ -221,6 +224,25 @@ 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);
+}
+
+void MissionControlProtocol::handleWaypointNavRequest(const json& j) {
+	double latitude = j["latitude"];
+	double longitude = j["longitude"];
+	bool isApproximate = j["isApproximate"];
+	bool isGate = j["isGate"];
+
+	if (Globals::AUTONOMOUS && !isApproximate && !isGate) {
+		navtypes::point_t waypointCoords(latitude, longitude, 1);
+		_autonomous_task.start(waypointCoords);
+	}
+}
+
 static bool validateJointPositionRequest(const json& j) {
 	return validateJoint(j) && util::validateKey(j, "position", val_t::number_integer);
 }
@@ -347,8 +369,8 @@ void MissionControlProtocol::stopAndShutdownPowerRepeat() {
 }
 
 MissionControlProtocol::MissionControlProtocol(SingleClientWSServer& server)
-	: WebSocketProtocol(Constants::MC_PROTOCOL_NAME), _server(server), _open_streams(),
-	  _last_joint_power(), _joint_repeat_running(false) {
+	: WebSocketProtocol(Constants::MC_PROTOCOL_NAME), _server(server), _autonomous_task(),
+	  _open_streams(), _last_joint_power(), _joint_repeat_running(false) {
 	// TODO: Add support for tank drive requests
 	// TODO: add support for science station requests (lazy susan, lazy susan lid, drill,
 	// syringe)
@@ -391,6 +413,10 @@ MissionControlProtocol::MissionControlProtocol(SingleClientWSServer& server)
 		CAMERA_STREAM_CLOSE_REQ_TYPE,
 		std::bind(&MissionControlProtocol::handleCameraStreamCloseRequest, this, _1),
 		validateCameraStreamCloseRequest);
+	this->addMessageHandler(
+		WAYPOINT_NAV_REQ_TYPE,
+		std::bind(&MissionControlProtocol::handleWaypointNavRequest, this, _1),
+		validateWaypointNavRequest);
 	this->addConnectionHandler(std::bind(&MissionControlProtocol::handleConnection, this));
 	this->addDisconnectionHandler(
 		std::bind(&MissionControlProtocol::stopAndShutdownPowerRepeat, this));

src/network/MissionControlProtocol.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include "../autonomous/AutonomousTask.h"
 #include "../video/H264Encoder.h"
 #include "../world_interface/world_interface.h"
 #include "websocket/WebSocketProtocol.h"
@@ -37,6 +38,7 @@ class MissionControlProtocol : public WebSocketProtocol { // TODO: add documenta
 	void updateArmIKRepeatTask();
 	void telemReportTask();
 	SingleClientWSServer& _server;
+	autonomous::AutonomousTask _autonomous_task;
 	std::shared_mutex _stream_mutex;
 	std::unordered_map<CameraID, uint32_t> _open_streams;
 	std::unordered_map<CameraID, std::shared_ptr<video::H264Encoder>> _camera_encoders;
@@ -62,6 +64,7 @@ class MissionControlProtocol : public WebSocketProtocol { // TODO: add documenta
 	void handleCameraStreamOpenRequest(const json& j);
 	void handleCameraStreamCloseRequest(const json& j);
 	void handleJointPowerRequest(const json& j);
+	void handleWaypointNavRequest(const json& j);
 	void handleDriveRequest(const json& j);
 	void handleSetArmIKEnabled(const json& j);
 	void sendCameraStreamReport(const CameraID& cam,