From 5c9e66a708fa2dd2be4a161c0826d011135f764b Mon Sep 17 00:00:00 2001 From: Naotaka Hatao Date: Sat, 16 Mar 2024 00:22:57 +0900 Subject: [PATCH] Add multiple resolution --- .../planner_cspace/planner_3d/motion_cache.h | 8 +-- planner_cspace/src/motion_cache.cpp | 49 +++++++++++++------ .../test/src/test_start_pose_predictor.cpp | 2 +- 3 files changed, 39 insertions(+), 20 deletions(-) diff --git a/planner_cspace/include/planner_cspace/planner_3d/motion_cache.h b/planner_cspace/include/planner_cspace/planner_3d/motion_cache.h index 53d15246..638e3b90 100644 --- a/planner_cspace/include/planner_cspace/planner_3d/motion_cache.h +++ b/planner_cspace/include/planner_cspace/planner_3d/motion_cache.h @@ -31,6 +31,7 @@ #define PLANNER_CSPACE_PLANNER_3D_MOTION_CACHE_H #include +#include #include #include @@ -49,7 +50,7 @@ class MotionCache friend class MotionCache; std::vector> motion_; - std::vector> motion_f_; + std::vector> interpolated_motion_; float distance_; public: @@ -63,7 +64,7 @@ class MotionCache } const std::vector>& getInterpolatedMotion() const { - return motion_f_; + return interpolated_motion_; } }; @@ -108,7 +109,8 @@ class MotionCache const float angular_resolution, const int range, const std::function, size_t&, size_t&)> gm_addr, - const float interpolation_interval); + const float interpolation_resolution, + const float grid_enumeration_resolution = 0.1); std::list> interpolatePath(const std::list>& path_grid) const; diff --git a/planner_cspace/src/motion_cache.cpp b/planner_cspace/src/motion_cache.cpp index 0f7f86be..721e7274 100644 --- a/planner_cspace/src/motion_cache.cpp +++ b/planner_cspace/src/motion_cache.cpp @@ -29,6 +29,7 @@ #include #include +#include #include #include @@ -46,7 +47,8 @@ void MotionCache::reset( const float angular_resolution, const int range, const std::function, size_t&, size_t&)> gm_addr, - const float interpolation_interval) + const float interpolation_resolution, + const float grid_enumeration_resolution) { const int angle = std::lround(M_PI * 2 / angular_resolution); @@ -83,26 +85,32 @@ void MotionCache::reset( const float cos_v = cosf(motion[2]); const float sin_v = sinf(motion[2]); - const float inter = interpolation_interval / d.len(); - + const int total_step = static_cast(std::round(d.len() / grid_enumeration_resolution)); + const int interpolation_step = + std::max(1, static_cast(std::round(interpolation_resolution / grid_enumeration_resolution))); if (std::abs(sin_v) < 0.1) { - for (float i = 0; i < 1.0 - inter / 2; i += inter) + for (int step = 0; step < total_step; ++step) { - const float x = diff_val[0] * i; - const float y = diff_val[1] * i; + const float ratio = step / static_cast(total_step); + const float x = diff_val[0] * ratio; + const float y = diff_val[1] * ratio; CyclicVecFloat<3, 2> posf(x / linear_resolution, y / linear_resolution, yaw / angular_resolution); CyclicVecInt<3, 2> pos(posf[0], posf[1], posf[2]); pos.cycleUnsigned(angle); - if (registered.find(pos) == registered.end()) + + if ((pos != d) && (registered.find(pos) == registered.end())) { page.motion_.push_back(pos); for (int i = 0; i < 3; ++i) max_range[i] = std::max(max_range[i], std::abs(pos[i])); registered[pos] = true; } - page.motion_f_.push_back(posf); + if (step % interpolation_step == 0) + { + page.interpolated_motion_.push_back(posf); + } } page.distance_ = d.len(); cache_[syaw][d] = page; @@ -128,12 +136,13 @@ void MotionCache::reset( CyclicVecFloat<3, 2> posf_prev(0, 0, 0); - for (float i = 0; i < 1.0 - inter / 2; i += inter) + for (int step = 0; step < total_step; ++step) { - const float r = r1 * (1.0 - i) + r2 * i; - const float cx2 = cx_s * (1.0 - i) + cx * i; - const float cy2 = cy_s * (1.0 - i) + cy * i; - const float cyaw = yaw + i * dyaw; + const float ratio = step / static_cast(total_step); + const float r = r1 * (1.0 - ratio) + r2 * ratio; + const float cx2 = cx_s * (1.0 - ratio) + cx * ratio; + const float cy2 = cy_s * (1.0 - ratio) + cy * ratio; + const float cyaw = yaw + ratio * dyaw; const float posf_raw[3] = { @@ -144,13 +153,17 @@ void MotionCache::reset( CyclicVecFloat<3, 2> posf(posf_raw[0], posf_raw[1], posf_raw[2]); CyclicVecInt<3, 2> pos(posf_raw[0], posf_raw[1], posf_raw[2]); pos.cycleUnsigned(angle); - if (registered.find(pos) == registered.end()) + if ((pos != d) && (registered.find(pos) == registered.end())) { page.motion_.push_back(pos); registered[pos] = true; } - page.motion_f_.push_back(posf); - distf += (posf - posf_prev).len(); + if (step % interpolation_step == 0) + { + page.interpolated_motion_.push_back(posf); + } + if (ratio > 0) + distf += (posf - posf_prev).len(); posf_prev = posf; } distf += (CyclicVecFloat<3, 2>(d) - posf_prev).len(); @@ -185,6 +198,10 @@ std::list> MotionCache::interpolatePath(const std::list> result; if (grid_path.size() < 2) { + for (const auto& p : grid_path) + { + result.push_back(CyclicVecFloat<3, 2>(p[0], p[1], p[2])); + } return result; } auto it_prev = grid_path.begin(); diff --git a/planner_cspace/test/src/test_start_pose_predictor.cpp b/planner_cspace/test/src/test_start_pose_predictor.cpp index 49421e70..1c60264b 100644 --- a/planner_cspace/test/src/test_start_pose_predictor.cpp +++ b/planner_cspace/test/src/test_start_pose_predictor.cpp @@ -68,7 +68,7 @@ class StartPosePredictorTester : public ::testing::Test map_info_.origin.orientation.w = 1.0; map_info_.linear_resolution = 1.0; map_info_.angular_resolution = 2 * M_PI / map_info_.angle; - motion_cache_.reset(map_info_.linear_resolution, map_info_.angular_resolution, 10, cm_.getAddressor(), 0.1); + motion_cache_.reset(map_info_.linear_resolution, map_info_.angular_resolution, 4, cm_.getAddressor(), 0.1); const GridAstar<3, 2>::Vec size3d( static_cast(map_info_.width),