GaitGeneration by Graph Search
読み取り中…
検索中…
一致する文字列を見つけられません
divided_map_state.cpp
[詳解]
1
3
4// Copyright(c) 2023-2025 Design Engineering Laboratory, Saitama University
5// Released under the MIT license
6// https://opensource.org/licenses/mit-license.php
7
8#include "divided_map_state.h"
9
10#include "math_util.h"
11
12namespace designlab {
13
15 : kMapMinZ(min_z),
16 global_robot_com_{},
17 divided_map_point_(math_util::Squared(kDividedNum)),
18 divided_map_top_z_(math_util::Squared(kDividedNum)) {
19 // コンストラクタで,配列のサイズを確保しておく.
20 Clear();
21}
22
23void DividedMapState::Init(const MapState& map_state,
24 const Vector3 global_robot_com) {
25 // 配列のサイズが確保されているか確認.
26 assert(divided_map_point_.size() == math_util::Squared(kDividedNum));
27 assert(divided_map_top_z_.size() == math_util::Squared(kDividedNum));
28
29 Clear();
30
31 global_robot_com_ = global_robot_com; // ロボットの位置を更新する.
32
33 // マップのデータ全てを参照し,切り分ける.
34 const size_t kMapPointSize = map_state.GetMapPointSize();
35
36 for (size_t i = 0; i < kMapPointSize; ++i) {
37 // xy方向のブロック番号をそれぞれ求める.
38 const designlab::Vector3 point = map_state.GetMapPoint(i);
39
40 // 範囲内にいないならば処理をやめ,続行.
41 if (!IsInMap(point)) {
42 continue;
43 }
44
45 const int x = GetDividedMapIndexX(point.x);
46 const int y = GetDividedMapIndexY(point.y);
47
48 // マップの範囲内にいる時のみ追加する.
49 if (IsValidIndex(x) && IsValidIndex(y)) {
50 divided_map_point_[GetDividedMapIndex(x, y)].push_back(point);
51
52 // 最大値を更新する.
53 divided_map_top_z_[GetDividedMapIndex(x, y)] =
54 (std::max)(point.z, divided_map_top_z_[GetDividedMapIndex(x, y)]);
55 }
56 }
57}
58
60 // vectorの中身を全てクリアする.
61 for (auto& i : divided_map_point_) {
62 i.clear();
63 }
64
65 // 最小値で埋める.
66 for (auto& i : divided_map_top_z_) {
67 i = kMapMinZ;
68 }
69}
70
72 const int x_index, const int y_index) const {
73 // 存在していなければ終了
74 if (!IsValidIndex(x_index) || !IsValidIndex(y_index)) {
76 "DividedMapState::GetPointNum: Invalid index.");
77 }
78
79 return static_cast<int>(
80 divided_map_point_[GetDividedMapIndex(x_index, y_index)].size());
81}
82
84 const int x_index, const int y_index, const int divide_map_index) const {
85 // 存在していなければ終了.
86 if (!IsValidIndex(x_index) || !IsValidIndex(y_index)) {
88 "DividedMapState::GetPointPos: Invalid index.");
89 }
90
91 const size_t size =
92 divided_map_point_[GetDividedMapIndex(x_index, y_index)].size();
93
94 if (divide_map_index < 0 || static_cast<int>(size) <= divide_map_index) {
95 return Vector3::GetZeroVec();
96 }
97
98 // 存在しているならば値を返す.
99 return divided_map_point_[GetDividedMapIndex(x_index, y_index)]
100 [divide_map_index];
101}
102
103void DividedMapState::GetPointVector(const int x_index, const int y_index,
104 std::vector<Vector3>* point_vec) const {
105 if (point_vec == nullptr) {
106 return;
107 }
108
109 // 存在していなければ終了.
110 if (!IsValidIndex(x_index) || !IsValidIndex(y_index)) {
111 return;
112 }
113
114 (*point_vec) = divided_map_point_[GetDividedMapIndex(x_index, y_index)];
115}
116
117float DividedMapState::GetTopZ(int x_index, int y_index) const {
118 // 存在していなければ終了.
119 if (!IsValidIndex(x_index) || !IsValidIndex(y_index)) {
120 return kMapMinZ;
121 }
122
123 return divided_map_top_z_[GetDividedMapIndex(x_index, y_index)];
124}
125
126} // namespace designlab
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
格子の数.
マップを表すクラス.
Definition map_state.h:29
size_t GetMapPointSize() const noexcept
脚設置可能点の総数を返す.
Definition map_state.h:50
Vector3 GetMapPoint(const size_t index) const noexcept
脚設置可能点の座標を返す.
Definition map_state.h:42
自作の expected クラス
Definition my_expected.h:46
constexpr T Squared(const T num) noexcept
2乗した値を返す関数.
Definition math_util.h:55
3次元の位置ベクトルを表す構造体.
float x
ロボットの正面方向に正.
float z
ロボットの上向きに正.
static constexpr Vector3 GetZeroVec() noexcept
零ベクトルを返す. 静的な関数なので,Vector3::GetZeroVec() と呼び出せる.
float y
ロボットの左向きに正.