.. _program_listing_file_applications_rover_app.cc:

Program Listing for File rover_app.cc
=====================================

|exhale_lsh| :ref:`Return to documentation for file <file_applications_rover_app.cc>` (``applications/rover_app.cc``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   #include "lupnt/applications/rover_app.h"
   
   #include "lupnt/agents/agent.h"
   #include "lupnt/core/asset_factory.h"
   #include "lupnt/core/logger.h"
   #include "lupnt/devices/clock.h"
   #include "lupnt/filters/filter.h"
   #include "lupnt/states/state.h"
   
   namespace lupnt {
   
     // Constructor
     RoverApp::RoverApp(Config& config) : Application(config) {
       Logger::Debug(fmt::format("Creating {}", name_), "RoverApp");
   
       // Filter dynamics
       LUPNT_CHECK(config["dynamics"], "No dynamics found", "RoverApp");
       Config dyn_config(config["dynamics"]);
       if (!dyn_config["name"]) dyn_config["name"] = "dynamics";
       dyn_config["name"] = name_ + "/" + dyn_config["name"].as<std::string>();
       std::string dynamics_class = dyn_config["class"].as<std::string>();
       dynamics_ = AssetFactory<Dynamics, Config&>::Create(dynamics_class, dyn_config);
   
       // Filter
       LUPNT_CHECK(config["filter"], "No filter found", "RoverApp");
       Config filter_config(config["filter"]);
       if (!filter_config["name"]) filter_config["name"] = "filter";
       filter_config["name"] = name_ + "/" + filter_config["name"].as<std::string>();
       std::string filter_class = filter_config["class"].as<std::string>();
       filter_ = AssetFactory<Filter, Config&>::Create(filter_class, filter_config);
     }
   
     // Setup
     void RoverApp::Setup() {
       Logger::Debug(fmt::format("Setting up {}", name_), "RoverApp");
   
       // Checks
       LUPNT_CHECK(filter_, "Filter not set", "RoverApp");
       LUPNT_CHECK(config_["initial_state"], "No initial state found", "RoverApp");
       LUPNT_CHECK(agent_, "Agent not set", "RoverApp");
   
       // Initial state
       Real sigma_r = config_["initial_state"]["sigma_r"].as<Real>();
       Real sigma_theta = RAD * config_["initial_state"]["sigma_theta"].as<Real>();
       MatXd P0 = Mat3d::Zero();
       P0(0, 0) = sigma_r * sigma_r;
       P0(1, 1) = sigma_r * sigma_r;
       P0(2, 2) = sigma_theta * sigma_theta;
   
       // Filter
       Real t = agent_->GetTime();
       Cart6 rv = agent_->GetState();
   
       // Dynamics function
       FilterDynamicsFunction f_dyn
           = [this](const State& x, Real t0, Real tf, const State* u, MatXd* F) {
               return dynamics_->Propagate(x, t0, tf, u, F);
             };
   
       // Process noise function
       MatXd Q = P0;
       ProcessNoiseFunction f_proc = [Q](const State& x, Real t0, Real tf) {
         (void)x;
         (void)t0;
         (void)tf;
         return Q;
       };
   
       filter_->SetTime(t);
       filter_->SetState(rv);
       filter_->SetCovariance(P0);
       filter_->SetDynamicsFunction(f_dyn);
       filter_->SetProcessNoiseFunction(f_proc);
   
       // Log after setup
       Log(0.0);
     }
   
     // Step
     void RoverApp::Step(Real t) {
       Logger::Debug("Step", "RoverApp", t);
       LUPNT_CHECK(agent_, "Agent not set", "RoverApp");
       LUPNT_CHECK(filter_, "Filter not set", "RoverApp");
   
       // auto clk = dynamic_cast<Clock*>(agent_->GetDevice("clock"))->Read(t);
   
       // Control
       // auto u = agent_->GetControl();
       State u = Vec2::Zero();
       if (u.size() == 0) {
         u = State::Zero(2);
         u.SetNames({"v", "w"});
         u.SetUnits({"m/s", "rad/s"});
       }
       // filter_->Predict(t, &u);
   
       // Control
       Real R = config_["trajectory"]["R"].as<Real>();
       Real v = config_["trajectory"]["v"].as<Real>();
       Real w = v / R;
       State u_new(2);
       u_new << v, w;
       u_new.SetNames({"v", "w"});
       u_new.SetUnits({"m/s", "rad/s"});
       // agent_->SetControl(u_new);
       Logger::Debug(fmt::format("Control: {}", u_new), "RoverApp");
   
       Log(t);
     }
   
     // Log
     void RoverApp::Log(Real t) {
       Logger::Debug(fmt::format("Logging {}", name_), "RoverApp", t);
       filter_->Log(t);
   
       // Errors
     }
   
     REGISTER_FACTORY_CLASS(Application, RoverApp)
   
   }  // namespace lupnt