17 divided_map_point_(math_util::Squared(kDividedNum)),
18 divided_map_top_z_(math_util::Squared(kDividedNum)) {
24 const Vector3 global_robot_com) {
31 global_robot_com_ = global_robot_com;
36 for (
size_t i = 0; i < kMapPointSize; ++i) {
49 if (IsValidIndex(x) && IsValidIndex(y)) {
50 divided_map_point_[GetDividedMapIndex(x, y)].push_back(point);
53 divided_map_top_z_[GetDividedMapIndex(x, y)] =
54 (std::max)(point.
z, divided_map_top_z_[GetDividedMapIndex(x, y)]);
61 for (
auto& i : divided_map_point_) {
66 for (
auto& i : divided_map_top_z_) {
72 const int x_index,
const int y_index)
const {
74 if (!IsValidIndex(x_index) || !IsValidIndex(y_index)) {
76 "DividedMapState::GetPointNum: Invalid index.");
79 return static_cast<int>(
80 divided_map_point_[GetDividedMapIndex(x_index, y_index)].size());
84 const int x_index,
const int y_index,
const int divide_map_index)
const {
86 if (!IsValidIndex(x_index) || !IsValidIndex(y_index)) {
88 "DividedMapState::GetPointPos: Invalid index.");
92 divided_map_point_[GetDividedMapIndex(x_index, y_index)].size();
94 if (divide_map_index < 0 ||
static_cast<int>(size) <= divide_map_index) {
99 return divided_map_point_[GetDividedMapIndex(x_index, y_index)]
104 std::vector<Vector3>* point_vec)
const {
105 if (point_vec ==
nullptr) {
110 if (!IsValidIndex(x_index) || !IsValidIndex(y_index)) {
114 (*point_vec) = divided_map_point_[GetDividedMapIndex(x_index, y_index)];
119 if (!IsValidIndex(x_index) || !IsValidIndex(y_index)) {
123 return divided_map_top_z_[GetDividedMapIndex(x_index, y_index)];
constexpr bool IsInMap(const float x, const float y) const noexcept
指定した座標がマップの範囲内に存在するかどうかを返す.
constexpr int GetDividedMapIndexX(const float pos_x) const noexcept
指定した座標を DividedMap のインデックスに変換する. 範囲外の値を指定した場合でも,値を丸めずに返す. そのため,IsInMap で範囲内に存在するかどうかを確認する必要がある.
nostd::expected< Vector3, std::string > GetPointPos(int x_index, int y_index, int divided_map_index) const
格子状に切り分けられたマップから,脚設置可能点の実際の座標を取得する. 範囲外の値を指定した場合は, unexpected を返す.
void Init(const MapState &map_state, const Vector3 global_robot_com)
マップのデータを初期化する. ロボットの重心座標を中心にマップのデータを格子状に分割し, その中に存在する脚設置可能点を集める.
void Clear()
マップのデータを消去する.
constexpr int GetDividedMapIndexY(const float pos_y) const noexcept
指定した座標を DividedMap のインデックスに変換する. 範囲外の値を指定した場合でも,値を丸めずに返す. そのため,IsInMap で範囲内に存在するかどうかを確認する必要がある.
float GetTopZ(int x_index, int y_index) const
格子状に切り分けられたマップから,最も高いZ座標を返す.
nostd::expected< int, std::string > GetPointNum(int x_index, int y_index) const
格子状に切り分けられたマップから,脚設置可能点の数を取得する. 範囲外の値を指定した場合は, unexpected を返す.
void GetPointVector(int x_index, int y_index, std::vector< Vector3 > *point_vec) const
格子状に切り分けられたマップから,脚設置可能点の vector を取得する 範囲外の値を指定した場合は,空の vector を返す.
DividedMapState(float min_z=MapState::kMapMinZ)
static constexpr int kDividedNum
格子の数.
size_t GetMapPointSize() const noexcept
脚設置可能点の総数を返す.
Vector3 GetMapPoint(const size_t index) const noexcept
脚設置可能点の座標を返す.
constexpr T Squared(const T num) noexcept
2乗した値を返す関数.
static constexpr Vector3 GetZeroVec() noexcept
零ベクトルを返す. 静的な関数なので,Vector3::GetZeroVec() と呼び出せる.