Tick Messaging (#125)

* start

* rework to use msg passing instead of blocking Ref acquisition

* fix future blocking destructor

* remove debugging print & pass lint

* refactor to locking ptr class

* fix lint

include/core/mission_state.hpp

@@ -7,10 +7,14 @@
 #include <chrono>
 #include <vector>
 #include <optional>
+#include <queue>
 
 #include "core/mission_config.hpp"
 #include "utilities/datatypes.hpp"
 #include "utilities/constants.hpp"
+#include "utilities/locks.hpp"
+#include "utilities/lockptr.hpp"
+#include "utilities/logging.hpp"
 #include "protos/obc.pb.h"
 #include "pathing/cartesian.hpp"
 #include "ticks/ids.hpp"
@@ -28,18 +32,28 @@ class MissionState {
  void setCartesianConverter(CartesianConverter<GPSProtoVec>);
 
  std::chrono::milliseconds doTick();
- // For external use, acquires the tick mutex
- // In contrast to the private version of the function,
- // which is for internal use and does not acquire
- // the mutex. This version should not be called from
- // within another function that already acquires the tick_mut
- void setTick(Tick* newTick);
  TickID getTickID();
 
+ void setTick(Tick* newTick);
+
  void setInitPath(std::vector<GPSCoord> init_path);
  const std::vector<GPSCoord>& getInitPath();
- bool isInitPathValidated();
- void validateInitPath();
+
+ /*
+ * Gets a locking reference to the underlying tick for the given tick subclass T.
+ * 
+ * Needs to be defined in the header file unless we want to manually list out
+ * template derivations.
+ */
+ template <typename T>
+ std::optional<LockPtr<T>> getTickLockPtr() {
+ try {
+ return LockPtr(std::dynamic_pointer_cast<T>(this->tick), &this->tick_mut);
+ } catch (std::bad_cast ex) {
+ LOG_F(ERROR, "Error creating TickLockRef: %s", ex.what());
+ return {};
+ }
+ }
 
  /*
  * Gets a shared_ptr to the mavlink client. 
@@ -66,17 +80,15 @@ class MissionState {
  std::optional<CartesianConverter<GPSProtoVec>> converter;
 
  std::mutex tick_mut; // for reading/writing tick
- std::unique_ptr<Tick> tick;
+ std::shared_ptr<Tick> tick;
 
  std::mutex init_path_mut; // for reading/writing the initial path
  std::vector<GPSCoord> init_path;
- bool init_path_validated = false; // true when the operator has validated the initial path
 
  std::shared_ptr<MavlinkClient> mav;
  std::shared_ptr<AirdropClient> airdrop;
 
  void _setTick(Tick* newTick); // does not acquire the tick_mut
 };
 
-
 #endif // INCLUDE_CORE_MISSION_STATE_HPP_

include/core/obc.hpp

@@ -25,6 +25,9 @@ class OBC {
 
  std::unique_ptr<GCSServer> gcs_server;
 
+ std::thread connectMavThread;
+ std::thread connectAirdropThread;
+
  void connectMavlink();
  void connectAirdrop();
 };

include/ticks/path_validate.hpp

@@ -3,6 +3,7 @@
 
 #include <memory>
 #include <chrono>
+#include <mutex>
 
 #include "ticks/tick.hpp"
 #include "core/mission_state.hpp"
@@ -13,11 +14,22 @@
  */
 class PathValidateTick : public Tick {
  public:
+ enum class Status {
+ Rejected = -1, // user rejects the path, return to path generation and try again
+ None = 0,
+ Validated = 1, // user accepts the path, continue to mission upload
+ };
+
  explicit PathValidateTick(std::shared_ptr<MissionState> state);
 
  std::chrono::milliseconds getWait() const override;
 
+ void setStatus(Status status);
+
  Tick* tick() override;
+
+ private:
+ Status status;
 };
 
 #endif // INCLUDE_TICKS_PATH_VALIDATE_HPP_

include/ticks/tick.hpp

@@ -4,18 +4,18 @@
 #include <memory>
 #include <chrono>
 #include <string>
+#include <mutex>
 
 #include "core/mission_state.hpp"
 #include "ticks/ids.hpp"
-
-// When writing tick functions... Absolutely do not do not do not
-// delete the pointer that is being passed in.
+#include "utilities/logging.hpp"
 
 class Tick {
  public:
- explicit Tick(std::shared_ptr<MissionState> state, TickID id) {
+ Tick(std::shared_ptr<MissionState> state, TickID id) {
  this->state = state;
  this->id = id;
+ this->already_validated = false;
  }
  virtual ~Tick() = default;
 
@@ -32,8 +32,7 @@ class Tick {
  protected:
  std::shared_ptr<MissionState> state;
  TickID id;
+ bool already_validated;
 };
 
-
-
 #endif // INCLUDE_TICKS_TICK_HPP_

include/utilities/lockptr.hpp

@@ -0,0 +1,24 @@
+#ifndef INCLUDE_UTILITIES_LOCKPTR_HPP_
+#define INCLUDE_UTILITIES_LOCKPTR_HPP_
+
+#include <memory>
+#include <mutex>
+
+template <typename T>
+class LockPtr {
+ public:
+ std::shared_ptr<T> ptr;
+
+ LockPtr(std::shared_ptr<T> ptr, std::mutex* mut): mut {mut}, ptr {ptr} {
+ mut->lock();
+ }
+
+ ~LockPtr() {
+ this->mut->unlock();
+ }
+
+ private:
+ std::mutex* mut;
+};
+
+#endif // INCLUDE_UTILITIES_LOCKPTR_HPP_

src/core/mission_state.cpp

@@ -69,16 +69,6 @@ const std::vector<GPSCoord>& MissionState::getInitPath() {
  return this->init_path;
 }
 
-bool MissionState::isInitPathValidated() {
- Lock lock(this->init_path_mut);
- return this->init_path_validated;
-}
-
-void MissionState::validateInitPath() {
- Lock lock(this->init_path_mut);
- this->init_path_validated = true;
-}
-
 std::shared_ptr<MavlinkClient> MissionState::getMav() {
  return this->mav;
 }

src/core/obc.cpp

@@ -23,11 +23,8 @@ OBC::OBC(uint16_t gcs_port) {
 
  this->gcs_server = std::make_unique<GCSServer>(gcs_port, this->state);
 
- // Don't need to look at these futures at all because the connect functions
- // will set the global mission state themselves when connected, which everything
- // else can check.
- auto _fut1 = std::async(std::launch::async, &OBC::connectMavlink, this);
- auto _fut2 = std::async(std::launch::async, &OBC::connectAirdrop, this);
+ this->connectMavThread = std::thread([this]{this->connectMavlink();});
+ this->connectAirdropThread = std::thread([this]{this->connectAirdrop();});
 }
 
 void OBC::run() {

src/network/gcs_routes.cpp

@@ -14,6 +14,7 @@
 #include "network/gcs_macros.hpp"
 #include "ticks/tick.hpp"
 #include "ticks/path_gen.hpp"
+#include "ticks/path_validate.hpp"
 
 /*
  * This file defines all of the GCS handler functions for every route
@@ -93,7 +94,12 @@ DEF_GCS_HANDLE(Get, path, initial) {
 DEF_GCS_HANDLE(Get, path, initial, new) {
  LOG_REQUEST("GET", "/path/initial/new");
 
- state->setTick(new PathGenTick(state));
+ auto lock_ptr = state->getTickLockPtr<PathValidateTick>();
+ if (!lock_ptr.has_value()) {
+ LOG_RESPONSE(WARNING, "Not currently in PathValidate Tick", BAD_REQUEST);
+ return;
+ }
+ lock_ptr->ptr->setStatus(PathValidateTick::Status::Rejected);
 
  LOG_RESPONSE(INFO, "Started generating new initial path", OK);
 }
@@ -103,10 +109,17 @@ DEF_GCS_HANDLE(Post, path, initial, validate) {
 
  if (state->getInitPath().empty()) {
  LOG_RESPONSE(WARNING, "No initial path generated", BAD_REQUEST);
- } else {
- state->validateInitPath();
- LOG_RESPONSE(INFO, "Initial path validated", OK);
+ return;
  }
+
+ auto lock_ptr = state->getTickLockPtr<PathValidateTick>();
+ if (!lock_ptr.has_value()) {
+ LOG_RESPONSE(WARNING, "Not currently in PathValidate Tick", BAD_REQUEST);
+ return;
+ }
+ lock_ptr->ptr->setStatus(PathValidateTick::Status::Validated);
+
+ LOG_RESPONSE(INFO, "Initial path validated", OK);
 }
 
 DEF_GCS_HANDLE(Get, camera, status) {

src/ticks/path_validate.cpp

@@ -5,18 +5,31 @@
 #include "core/mission_state.hpp"
 #include "ticks/ids.hpp"
 #include "ticks/mission_upload.hpp"
+#include "ticks/path_gen.hpp"
+#include "utilities/locks.hpp"
 
 PathValidateTick::PathValidateTick(std::shared_ptr<MissionState> state)
- :Tick(state, TickID::PathValidate) {}
+ :Tick(state, TickID::PathValidate), status(PathValidateTick::Status::None) {}
 
 std::chrono::milliseconds PathValidateTick::getWait() const {
  return PATH_VALIDATE_TICK_WAIT;
 }
 
 Tick* PathValidateTick::tick() {
- if (this->state->isInitPathValidated() && this->state->getMav() != nullptr) {
- return new MissionUploadTick(this->state);
- } else {
- return nullptr;
+ if (status == Status::Validated) {
+ if (this->state->getMav() != nullptr) {
+ return new MissionUploadTick(this->state);
+ } else {
+ LOG_F(WARNING, "Path Validated, but cannot continue because mavlink not connected.");
+ return nullptr;
+ }
+ } else if (status == Status::Rejected) {
+ return new PathGenTick(this->state);
  }
+
+ return nullptr;
+}
+
+void PathValidateTick::setStatus(PathValidateTick::Status status) {
+ this->status = status;
 }