Autonomous Task (#322)

* 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

* fix autonomous: SQUASH ME

* pretty much working + squash me

* Add message validation back in

* Run clang-format

* Use correct json message names

* MissionControl: Accept both floats and unsigned numbers in autonomous

JavaScript truncates any value such as 0.0 or 1.0 to 0 and 1,
respectively. This fails validation because we looks for floats and
those come across as unsigned ints. To fix this, we check if the
incoming number is either a float or an unsigned number. Both are
accepted.

* Change rot order

* format

* wip: Debugging and whatnot

* Improve logging, add slow distance threshold to dtwcommand

* Move autonomous constants to Constants.cpp to match new impl

* format

* Remove extraneous logging

---------

Co-authored-by: Abhay D <[email protected]>

src/CMakeLists.txt

@@ -250,9 +250,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.cpp

@@ -147,5 +147,14 @@ const std::array<robot::types::motorid_t, 2> IK_MOTORS = ([]() {
 })();
 } // namespace arm
 
+namespace autonomous {
+const double THETA_KP = 2.0;
+const double DRIVE_VEL = 1.5;
+const double SLOW_DRIVE_THRESHOLD = 3.0;
+const double DONE_THRESHOLD = 0.5;
+// Duration long enough to confirm we are there, not so long that time is wasted
+const util::dseconds CLOSE_TO_TARGET_DUR_VAL = std::chrono::milliseconds(750);
+} // namespace autonomous
+
 const double CONTROL_HZ = 10.0;
 } // namespace Constants

src/Constants.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include "utils/time.h"
 #include "world_interface/data.h"
 
 #include <array>
@@ -175,5 +176,15 @@ constexpr frozen::unordered_map<robot::types::motorid_t, double, IK_MOTORS.size(
 					{robot::types::motorid_t::elbow, 0.461264}};
 } // namespace arm
 
+namespace autonomous {
+extern const double THETA_KP;
+extern const double DRIVE_VEL;
+extern const double SLOW_DRIVE_THRESHOLD;
+extern const double DONE_THRESHOLD;
+// Duration long enough to confirm we are there, not so long that time is wasted
+extern const util::dseconds CLOSE_TO_TARGET_DUR_VAL;
+} // namespace autonomous
+
 extern const double CONTROL_HZ;
+
 } // namespace Constants

src/ardupilot/ArduPilotProtocol.cpp

@@ -80,6 +80,7 @@ void ArduPilotProtocol::handleIMURequest(const json& j) {
 	double roll = j["roll"];
 	double pitch = j["pitch"];
 	double yaw = j["yaw"];
+	yaw = -yaw;
 	eulerangles_t rpy{roll, pitch, yaw};
 
 	std::lock_guard lock(_lastOrientationMutex);

src/autonomous/AutonomousTask.cpp

@@ -0,0 +1,84 @@
+#include "AutonomousTask.h"
+
+#include "../Constants.h"
+#include "../commands/DriveToWaypointCommand.h"
+#include "../utils/transform.h"
+#include "../world_interface/world_interface.h"
+
+#include <loguru.hpp>
+
+using namespace std::chrono_literals;
+using namespace Constants::autonomous;
+
+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_THRESHOLD, 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)) {
+			LOG_SCOPE_F(INFO, "AutoNav");
+			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);
+			navtypes::point_t relTarget = util::toTransform(latestPos) * _waypoint_coords;
+			LOG_F(INFO, "Relative Target: (%lf, %lf)", relTarget(0), relTarget(1));
+			LOG_F(INFO, "thetaVel: %lf", scaledVels(2));
+			LOG_F(INFO, "xVel: %lf", scaledVels(0));
+			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/commands/DriveToWaypointCommand.cpp

@@ -9,12 +9,12 @@ using namespace std::chrono_literals;
 namespace commands {
 
 DriveToWaypointCommand::DriveToWaypointCommand(const point_t& target, double thetaKP,
-											   double driveVel, double slowDriveVel,
+											   double driveVel, double slowDriveThresh,
 											   double doneThresh,
 											   util::dseconds closeToTargetDur)
 	: target(target), pose(pose_t::Zero()), thetaKP(thetaKP), driveVel(driveVel),
-	  slowDriveVel(slowDriveVel), doneThresh(doneThresh), setStateCalledBeforeOutput(false),
-	  closeToTargetDur(closeToTargetDur) {}
+	  slowDriveThresh(slowDriveThresh), doneThresh(doneThresh),
+	  setStateCalledBeforeOutput(false), closeToTargetDur(closeToTargetDur) {}
 
 void DriveToWaypointCommand::setState(const navtypes::pose_t& pose,
 									  robot::types::datatime_t time) {
@@ -33,11 +33,13 @@ command_t DriveToWaypointCommand::getOutput() {
 	double targetAngle = std::atan2(toTarget(1), toTarget(0));
 	double angleDelta = targetAngle - pose(2);
 	double thetaErr = std::atan2(std::sin(angleDelta), std::cos(angleDelta));
-
 	double thetaVel = thetaKP * thetaErr;
 
 	double dist = (pose.topRows<2>() - target.topRows<2>()).norm();
-	double xVel = dist <= 2 * doneThresh ? slowDriveVel : driveVel;
+	double xVel = driveVel;
+	if (dist <= slowDriveThresh) {
+		xVel *= dist / slowDriveThresh;
+	}
 
 	return {.thetaVel = thetaVel, .xVel = xVel};
 }

src/commands/DriveToWaypointCommand.h

@@ -20,16 +20,16 @@ class DriveToWaypointCommand : CommandBase {
 	/**
 	 * Creates a new DriveToWaypointCommand with the specified targeting information.
 	 *
-	 * @param target, the position of the target in world coordinates.
-	 * @param thetaKP, the scaling factor to use for robot steering proportional control.
-	 * @param driveVel, the speed to drive the robot at.
-	 * @param slowDriveVel, the slower speed to drive the robot at when near the target.
-	 * @param doneThresh, the threshold for minimum robot-target distance to complete command.
-	 * @param closeToTargetDurVal, the duration within doneThresh after which robot navigation
+	 * @param target the position of the target in world coordinates.
+	 * @param thetaKP the scaling factor to use for robot steering proportional control.
+	 * @param driveVel the speed to drive the robot at.
+	 * @param slowDriveThresh the distance at which the robot will start slowing down.
+	 * @param doneThresh the threshold for minimum robot-target distance to complete command.
+	 * @param closeToTargetDurVal the duration within doneThresh after which robot navigation
 	 * 							   is considered done, seconds.
 	 */
 	DriveToWaypointCommand(const navtypes::point_t& target, double thetaKP, double driveVel,
-						   double slowDriveVel, double doneThresh,
+						   double slowDriveThresh, double doneThresh,
 						   util::dseconds closeToTargetDur);
 
 	/**
@@ -62,7 +62,7 @@ class DriveToWaypointCommand : CommandBase {
 	navtypes::pose_t pose;
 	double thetaKP;
 	double driveVel;
-	double slowDriveVel;
+	double slowDriveThresh;
 	double doneThresh;
 	bool setStateCalledBeforeOutput;
 	util::dseconds closeToTargetDur;

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();
 	}
@@ -152,6 +154,32 @@ 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::gpscoords_t coords = {latitude, longitude};
+		auto target = robot::gpsToMeters(coords);
+		if (target) {
+			_autonomous_task.start(target.value());
+		} else {
+			LOG_F(WARNING, "No GPS converter initialized!");
+		}
+	}
+}
+
 static bool validateJointPositionRequest(const json& j) {
 	return validateJoint(j) && util::validateKey(j, "position", val_t::number_integer);
 }
@@ -233,7 +261,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)
@@ -271,6 +300,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;
 }
 

src/world_interface/gps_common_interface.cpp

@@ -34,7 +34,7 @@ DataPoint<point_t> readGPS() {
 }
 
 std::optional<point_t> gpsToMeters(const gpscoords_t& coord) {
-	if (converter) {
+	if (gpsHasFix()) {
 		return converter->gpsToMeters(coord);
 	} else {
 		return {};