Method setPlan

controllers/src/lqr_controller.cpp:100–113

  void cleanup() override {}

  void setPlan(const nav_msgs::msg::Path & path) override
  {
    auto node_shared = node_.lock();
    if (!node_shared) {
      throw std::runtime_error{"Could not acquire node."};
    }

    trajectory_.clear();
    std::transform(
      path.poses.begin(), path.poses.end(), std::back_inserter(
        trajectory_), StateFromMsg);
    path_start_time_ = node_shared->now();
    resetStates(Eigen::Vector3d::Zero());
  }

  void resetStates(Eigen::Vector3d init_state)
  {