39 lines
1.1 KiB
C++
39 lines
1.1 KiB
C++
#pragma once
|
|
|
|
#include <optional>
|
|
#include "car_nav_lite/types.hpp"
|
|
#include "car_nav_lite/grid_map.hpp"
|
|
#include "car_nav_lite/localizer.hpp"
|
|
#include "car_nav_lite/global_planner.hpp"
|
|
#include "car_nav_lite/local_planner.hpp"
|
|
|
|
namespace car_nav_lite {
|
|
|
|
class BTExecutor {
|
|
public:
|
|
BTExecutor();
|
|
|
|
void setGoal(const Pose2D& goal);
|
|
void setTestPath(const std::vector<Pose2D>& path);
|
|
void setParams(double la, double ms, double mr, double sl, double cf);
|
|
void setFailCallback(std::function<void()> cb) { fail_callback_ = std::move(cb); }
|
|
bool hasGoal() const { return goal_.has_value(); }
|
|
|
|
Twist tick(const Pose2D& pose, const GridMap& map);
|
|
|
|
NavState state() const { return state_; }
|
|
const std::vector<Pose2D>& currentPath() const { return current_path_; }
|
|
|
|
private:
|
|
NavState state_ = NavState::IDLE;
|
|
std::optional<Pose2D> goal_;
|
|
std::vector<Pose2D> current_path_;
|
|
GlobalPlanner planner_;
|
|
LocalPlanner local_planner_;
|
|
std::function<void()> fail_callback_;
|
|
|
|
bool atGoal(const Pose2D& pose) const;
|
|
};
|
|
|
|
} // namespace car_nav_lite
|