Create AutonomousTask to handle autonomous navigation

src/CMakeLists.txt

@@ -254,9 +254,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

@@ -1,6 +1,7 @@
 #pragma once
 
 #include "kinematics/DiffDriveKinematics.h"
+#include "utils/time.h"
 #include "world_interface/data.h"
 
 #include <array>
@@ -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,74 @@
+#include "AutonomousTask.h"
+
+#include "../Constants.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);
+
+	DiffDriveKinematics diffDriveKinematics(Constants::EFF_WHEEL_BASE);
+
+	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.getData());
+			cmd.setState(latestPos, robot::types::dataclock::now());
+			commands::command_t output = cmd.getOutput();
+			auto scaledVels = diffDriveKinematics.ensureWithinWheelSpeedLimit(
+				DiffDriveKinematics::PreferredVelPreservation::PreferThetaVel, output.xVel,
+				output.thetaVel, Constants::MAX_WHEEL_VEL);
+			robot::setCmdVel(scaledVels(2), scaledVels(0));
+		}
+
+		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

@@ -9,6 +9,6 @@ target_link_libraries(websocket_utils
 add_library(mission_control_interface STATIC
   MissionControlProtocol.cpp
   MissionControlTasks.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/MissionControlMessages.h

@@ -14,6 +14,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";

src/network/MissionControlProtocol.cpp

@@ -2,6 +2,7 @@
 
 #include "../Constants.h"
 #include "../Globals.h"
+#include "../autonomous/AutonomousTask.h"
 #include "../utils/core.h"
 #include "../utils/json.h"
 #include "../world_interface/data.h"
@@ -71,6 +72,7 @@ void MissionControlProtocol::handleOperationModeRequest(const json& j) {
 		// if we have entered autonomous mode, we need to stop all the power repeater stuff.
 		this->stopAndShutdownPowerRepeat(true);
 	} else {
+		_autonomous_task.kill();
 		// if we have left autonomous mode, we need to start the power repeater again.
 		_power_repeat_task.start();
 	}
@@ -150,6 +152,27 @@ void MissionControlProtocol::handleJointPowerRequest(const json& j) {
 	}
 }
 
+static bool validateWaypointNavRequest(const json& j) {
+	bool lat_is_unsigned = util::validateKey(j, "latitude", val_t::number_unsigned);
+	bool lon_is_unsigned = util::validateKey(j, "longitude", val_t::number_unsigned);
+	return (lat_is_unsigned || util::validateKey(j, "latitude", val_t::number_float)) &&
+		   (lon_is_unsigned || util::validateKey(j, "longitude", val_t::number_float)) &&
+		   util::validateKey(j, "isApproximate", val_t::boolean) &&
+		   util::validateKey(j, "isGate", val_t::boolean);
+}
+
+void MissionControlProtocol::handleWaypointNavRequest(const json& j) {
+	float latitude = j["latitude"];
+	float 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);
 }
@@ -231,7 +254,8 @@ void MissionControlProtocol::stopAndShutdownPowerRepeat(bool sendDisableIK) {
 
 MissionControlProtocol::MissionControlProtocol(SingleClientWSServer& server)
 	: WebSocketProtocol(Constants::MC_PROTOCOL_NAME), _server(server),
-	  _camera_stream_task(server), _telem_report_task(server), _arm_ik_task(server) {
+	  _camera_stream_task(server), _telem_report_task(server), _arm_ik_task(server),
+	  _autonomous_task() {
 	// TODO: Add support for tank drive requests
 	// TODO: add support for science station requests (lazy susan, lazy susan lid, drill,
 	// syringe)
@@ -269,6 +293,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, false));

src/network/MissionControlProtocol.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include "../autonomous/AutonomousTask.h"
 #include "../utils/scheduler.h"
 #include "../video/H264Encoder.h"
 #include "../world_interface/world_interface.h"
@@ -38,12 +39,14 @@ class MissionControlProtocol : public WebSocketProtocol { // TODO: add documenta
 	tasks::CameraStreamTask _camera_stream_task;
 	tasks::TelemReportTask _telem_report_task;
 	tasks::ArmIKTask _arm_ik_task;
+	autonomous::AutonomousTask _autonomous_task;
 
 	void handleEmergencyStopRequest(const json& j);
 	void handleOperationModeRequest(const json& j);
 	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 handleRequestArmIKEnabled(const json& j);
 	void sendArmIKEnabledReport(bool enabled);

src/utils/transform.cpp

@@ -22,9 +22,9 @@ double quatToHeading(const Eigen::Quaterniond& quat) {
 }
 
 Eigen::Quaterniond eulerAnglesToQuat(const navtypes::eulerangles_t& rpy) {
-	Eigen::Quaterniond quat(Eigen::AngleAxisd(rpy.roll, Eigen::Vector3d::UnitX()) *
+	Eigen::Quaterniond quat(Eigen::AngleAxisd(rpy.yaw, Eigen::Vector3d::UnitZ()) *
 							Eigen::AngleAxisd(rpy.pitch, Eigen::Vector3d::UnitY()) *
-							Eigen::AngleAxisd(rpy.yaw, Eigen::Vector3d::UnitZ()));
+							Eigen::AngleAxisd(rpy.roll, Eigen::Vector3d::UnitX()));
 	return quat;
 }