From cb101ede3f925ccf96890a55dacae9e251a2c06d Mon Sep 17 00:00:00 2001 From: Naotaka Hatao Date: Thu, 14 Mar 2024 20:25:00 +0900 Subject: [PATCH 1/6] Remove duplicated interpolation method --- planner_cspace/CMakeLists.txt | 1 - .../planner_3d/grid_astar_model.h | 9 +- .../planner_cspace/planner_3d/motion_cache.h | 18 ++- .../planner_3d/path_interpolator.h | 66 ---------- planner_cspace/src/grid_astar_model_3dof.cpp | 19 ++- planner_cspace/src/motion_cache.cpp | 56 +++++++-- planner_cspace/src/path_interpolator.cpp | 117 ------------------ planner_cspace/src/planner_3d.cpp | 11 +- planner_cspace/test/CMakeLists.txt | 9 +- planner_cspace/test/src/test_motion_cache.cpp | 2 +- .../test/src/test_path_interpolator.cpp | 113 ----------------- .../test/src/test_start_pose_predictor.cpp | 16 +-- 12 files changed, 101 insertions(+), 336 deletions(-) delete mode 100644 planner_cspace/include/planner_cspace/planner_3d/path_interpolator.h delete mode 100644 planner_cspace/src/path_interpolator.cpp delete mode 100644 planner_cspace/test/src/test_path_interpolator.cpp diff --git a/planner_cspace/CMakeLists.txt b/planner_cspace/CMakeLists.txt index 5ef773b65..3b5902dab 100644 --- a/planner_cspace/CMakeLists.txt +++ b/planner_cspace/CMakeLists.txt @@ -52,7 +52,6 @@ add_executable(planner_3d src/grid_astar_model_3dof.cpp src/motion_cache.cpp src/motion_primitive_builder.cpp - src/path_interpolator.cpp src/planner_3d.cpp src/distance_map.cpp src/rotation_cache.cpp diff --git a/planner_cspace/include/planner_cspace/planner_3d/grid_astar_model.h b/planner_cspace/include/planner_cspace/planner_3d/grid_astar_model.h index 42f8ceea7..515a95590 100644 --- a/planner_cspace/include/planner_cspace/planner_3d/grid_astar_model.h +++ b/planner_cspace/include/planner_cspace/planner_3d/grid_astar_model.h @@ -31,6 +31,7 @@ #define PLANNER_CSPACE_PLANNER_3D_GRID_ASTAR_MODEL_H #include +#include #include #include #include @@ -41,7 +42,6 @@ #include #include #include -#include #include namespace planner_cspace @@ -78,8 +78,6 @@ class GridAstarModel3D : public GridAstarModelBase<3, 2> using Vec = CyclicVecInt<3, 2>; using Vecf = CyclicVecFloat<3, 2>; - PathInterpolator path_interpolator_; - protected: bool hysteresis_; costmap_cspace_msgs::MapMetaData3D map_info_; @@ -111,7 +109,8 @@ class GridAstarModel3D : public GridAstarModelBase<3, 2> const BlockMemGridmapBase& cm_hyst, const BlockMemGridmapBase& cm_rough, const CostCoeff& cc, - const int range); + const int range, + const double interpolation_interval = 1.0); void enableHysteresis(const bool enable); void createEuclidCostCache(); float euclidCost(const Vec& v) const; @@ -125,6 +124,8 @@ class GridAstarModel3D : public GridAstarModelBase<3, 2> const Vec& p, const std::vector& ss, const Vec& es) const override; + std::list interpolatePath( + const std::list& path) const; }; class GridAstarModel2D : public GridAstarModelBase<3, 2> 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 ccc7de46a..53d152460 100644 --- a/planner_cspace/include/planner_cspace/planner_3d/motion_cache.h +++ b/planner_cspace/include/planner_cspace/planner_3d/motion_cache.h @@ -49,6 +49,7 @@ class MotionCache friend class MotionCache; std::vector> motion_; + std::vector> motion_f_; float distance_; public: @@ -60,6 +61,10 @@ class MotionCache { return motion_; } + const std::vector>& getInterpolatedMotion() const + { + return motion_f_; + } }; using Cache = @@ -76,6 +81,14 @@ class MotionCache i += page_size_; return cache_[i].find(goal); } + inline const typename Cache::const_iterator find( + const CyclicVecInt<3, 2>& from, + const CyclicVecInt<3, 2>& to) const + { + const int start_yaw = from[2]; + const CyclicVecInt<3, 2> goal(to[0] - from[0], to[1] - from[1], to[2]); + return find(start_yaw, goal); + } inline const typename Cache::const_iterator end( const int start_yaw) const { @@ -94,7 +107,10 @@ class MotionCache const float linear_resolution, const float angular_resolution, const int range, - const std::function, size_t&, size_t&)> gm_addr); + const std::function, size_t&, size_t&)> gm_addr, + const float interpolation_interval); + + std::list> interpolatePath(const std::list>& path_grid) const; protected: std::vector cache_; diff --git a/planner_cspace/include/planner_cspace/planner_3d/path_interpolator.h b/planner_cspace/include/planner_cspace/planner_3d/path_interpolator.h deleted file mode 100644 index e0224417c..000000000 --- a/planner_cspace/include/planner_cspace/planner_3d/path_interpolator.h +++ /dev/null @@ -1,66 +0,0 @@ -/* - * Copyright (c) 2019, the neonavigation authors - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef PLANNER_CSPACE_PLANNER_3D_PATH_INTERPOLATOR_H -#define PLANNER_CSPACE_PLANNER_3D_PATH_INTERPOLATOR_H - -#include - -#include -#include - -namespace planner_cspace -{ -namespace planner_3d -{ -class PathInterpolator -{ -private: - RotationCache rot_cache_; - int range_; - int angle_; - -public: - inline void reset( - const float angular_resolution, - const int range) - { - range_ = range; - angle_ = std::lround(M_PI * 2 / angular_resolution); - rot_cache_.reset(1.0, angular_resolution, range); - } - std::list> interpolate( - const std::list>& path_grid, - const float interval, - const int local_range) const; -}; -} // namespace planner_3d -} // namespace planner_cspace - -#endif // PLANNER_CSPACE_PLANNER_3D_PATH_INTERPOLATOR_H diff --git a/planner_cspace/src/grid_astar_model_3dof.cpp b/planner_cspace/src/grid_astar_model_3dof.cpp index 068f6f7bb..4dfdbea0e 100644 --- a/planner_cspace/src/grid_astar_model_3dof.cpp +++ b/planner_cspace/src/grid_astar_model_3dof.cpp @@ -29,6 +29,7 @@ #include #include +#include #include #include @@ -40,7 +41,6 @@ #include #include #include -#include #include namespace planner_cspace @@ -56,7 +56,8 @@ GridAstarModel3D::GridAstarModel3D( const BlockMemGridmapBase& cm_hyst, const BlockMemGridmapBase& cm_rough, const CostCoeff& cc, - const int range) + const int range, + const double interpolation_interval) : hysteresis_(false) , map_info_(map_info) , euclid_cost_coef_(euclid_cost_coef) @@ -71,6 +72,7 @@ GridAstarModel3D::GridAstarModel3D( , cm_rough_(cm_rough) , cc_(cc) , range_(range) + { rot_cache_.reset(map_info_.linear_resolution, map_info_.angular_resolution, range_); @@ -81,12 +83,14 @@ GridAstarModel3D::GridAstarModel3D( map_info_linear.linear_resolution, map_info_linear.angular_resolution, range_, - cm_rough_.getAddressor()); + cm_rough_.getAddressor(), + interpolation_interval); motion_cache_.reset( map_info_.linear_resolution, map_info_.angular_resolution, range_, - cm_.getAddressor()); + cm_.getAddressor(), + interpolation_interval); // Make boundary check threshold min_boundary_ = motion_cache_.getMaxRange(); @@ -112,7 +116,6 @@ GridAstarModel3D::GridAstarModel3D( search_list_rough_.push_back(d); } } - path_interpolator_.reset(map_info_.angular_resolution, range_); } void GridAstarModel3D::enableHysteresis(const bool enable) @@ -319,6 +322,11 @@ const std::vector& GridAstarModel3D::searchGrids( return search_list_rough_; } +std::list GridAstarModel3D::interpolatePath(const std::list& grid_path) const +{ + return motion_cache_.interpolatePath(grid_path); +} + float GridAstarModel2D::cost( const Vec& cur, const Vec& next, const std::vector& start, const Vec& goal) const { @@ -358,5 +366,6 @@ const std::vector& GridAstarModel2D::searchGrids( { return base_->search_list_rough_; } + } // namespace planner_3d } // namespace planner_cspace diff --git a/planner_cspace/src/motion_cache.cpp b/planner_cspace/src/motion_cache.cpp index f43a0a601..0f7f86be9 100644 --- a/planner_cspace/src/motion_cache.cpp +++ b/planner_cspace/src/motion_cache.cpp @@ -32,6 +32,8 @@ #include #include +#include + #include #include @@ -43,7 +45,8 @@ void MotionCache::reset( const float linear_resolution, const float angular_resolution, const int range, - const std::function, size_t&, size_t&)> gm_addr) + const std::function, size_t&, size_t&)> gm_addr, + const float interpolation_interval) { const int angle = std::lround(M_PI * 2 / angular_resolution); @@ -80,17 +83,17 @@ void MotionCache::reset( const float cos_v = cosf(motion[2]); const float sin_v = sinf(motion[2]); - const float inter = 1.0 / d.len(); + const float inter = interpolation_interval / d.len(); if (std::abs(sin_v) < 0.1) { - for (float i = 0; i < 1.0; i += inter) + for (float i = 0; i < 1.0 - inter / 2; i += inter) { const float x = diff_val[0] * i; const float y = diff_val[1] * i; - CyclicVecInt<3, 2> pos( - x / linear_resolution, y / linear_resolution, yaw / angular_resolution); + 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()) { @@ -99,6 +102,7 @@ void MotionCache::reset( max_range[i] = std::max(max_range[i], std::abs(pos[i])); registered[pos] = true; } + page.motion_f_.push_back(posf); } page.distance_ = d.len(); cache_[syaw][d] = page; @@ -124,7 +128,7 @@ void MotionCache::reset( CyclicVecFloat<3, 2> posf_prev(0, 0, 0); - for (float i = 0; i < 1.0; i += inter) + for (float i = 0; i < 1.0 - inter / 2; i += inter) { const float r = r1 * (1.0 - i) + r2 * i; const float cx2 = cx_s * (1.0 - i) + cx * i; @@ -137,7 +141,7 @@ void MotionCache::reset( (cy2 - r * sinf(cyaw + M_PI / 2)) / linear_resolution, cyaw / angular_resolution, }; - const CyclicVecFloat<3, 2> posf(posf_raw[0], posf_raw[1], posf_raw[2]); + 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()) @@ -145,6 +149,7 @@ void MotionCache::reset( page.motion_.push_back(pos); registered[pos] = true; } + page.motion_f_.push_back(posf); distf += (posf - posf_prev).len(); posf_prev = posf; } @@ -174,5 +179,42 @@ void MotionCache::reset( } max_range_ = max_range; } + +std::list> MotionCache::interpolatePath(const std::list>& grid_path) const +{ + std::list> result; + if (grid_path.size() < 2) + { + return result; + } + auto it_prev = grid_path.begin(); + for (auto it = std::next(it_prev); it != grid_path.end(); ++it, ++it_prev) + { + if (((*it_prev)[0] == (*it)[0]) && ((*it_prev)[1] == (*it)[1])) + { + result.push_back(CyclicVecFloat<3, 2>((*it_prev)[0], (*it_prev)[1], (*it_prev)[2])); + continue; + } + + const auto motion_it = find(*it_prev, *it); + if (motion_it == end((*it)[2])) + { + ROS_ERROR("Failed to find motion between [%d %d %d] and [%d %d %d]", + (*it_prev)[0], (*it_prev)[1], (*it_prev)[2], (*it)[0], (*it)[1], (*it)[2]); + result.push_back(CyclicVecFloat<3, 2>((*it_prev)[0], (*it_prev)[1], (*it_prev)[2])); + } + else + { + const auto& motion = motion_it->second.getInterpolatedMotion(); + for (const auto& pos_diff : motion) + { + result.push_back(CyclicVecFloat<3, 2>((*it_prev)[0] + pos_diff[0], (*it_prev)[1] + pos_diff[1], pos_diff[2])); + } + } + } + result.push_back(CyclicVecFloat<3, 2>(grid_path.back()[0], grid_path.back()[1], grid_path.back()[2])); + return result; +} + } // namespace planner_3d } // namespace planner_cspace diff --git a/planner_cspace/src/path_interpolator.cpp b/planner_cspace/src/path_interpolator.cpp deleted file mode 100644 index 5bb2ea502..000000000 --- a/planner_cspace/src/path_interpolator.cpp +++ /dev/null @@ -1,117 +0,0 @@ -/* - * Copyright (c) 2019, the neonavigation authors - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include - -#include -#include -#include - -namespace planner_cspace -{ -namespace planner_3d -{ -std::list> PathInterpolator::interpolate( - const std::list>& path_grid, - const float interval, - const int local_range) const -{ - CyclicVecInt<3, 2> p_prev(0, 0, 0); - bool init = false; - - std::list> path; - - for (auto p : path_grid) - { - p.cycleUnsigned(angle_); - - if (init) - { - const CyclicVecInt<3, 2> ds = path_grid.front() - p; - CyclicVecInt<3, 2> d = p - p_prev; - d.cycle(angle_); - const CyclicVecInt<3, 2> d2(d[0] + range_, d[1] + range_, p[2]); - - const float inter = interval / d.len(); - - if (d[0] == 0 && d[1] == 0) - { - const int yaw_inc = std::copysign(1, d[2]); - for (int yaw = p_prev[2]; yaw != p_prev[2] + d[2]; yaw += yaw_inc) - { - path.push_back(CyclicVecFloat<3, 2>(p[0], p[1], yaw)); - } - } - else if (d[2] == 0 || ds.sqlen() > local_range * local_range) - { - for (float i = 0; i < 1.0 - inter / 2; i += inter) - { - const float x2 = p_prev[0] * (1 - i) + p[0] * i; - const float y2 = p_prev[1] * (1 - i) + p[1] * i; - const float yaw2 = p_prev[2] + i * d[2]; - - path.push_back(CyclicVecFloat<3, 2>(x2, y2, yaw2)); - } - } - else - { - const auto& radiuses = rot_cache_.getRadiuses(p_prev[2], d2); - const float r1 = radiuses.first; - const float r2 = radiuses.second; - const float yawf = p[2] * M_PI * 2.0 / angle_; - const float yawf_prev = p_prev[2] * M_PI * 2.0 / angle_; - - const float cx = p[0] + r2 * cosf(yawf + M_PI / 2); - const float cy = p[1] + r2 * sinf(yawf + M_PI / 2); - const float cx_prev = p_prev[0] + r1 * cosf(yawf_prev + M_PI / 2); - const float cy_prev = p_prev[1] + r1 * sinf(yawf_prev + M_PI / 2); - - for (float i = 0; i < 1.0 - inter / 2; i += inter) - { - const float r = r1 * (1.0 - i) + r2 * i; - const float cx2 = cx_prev * (1.0 - i) + cx * i; - const float cy2 = cy_prev * (1.0 - i) + cy * i; - const float cyaw = p_prev[2] + i * d[2]; - const float cyawf = cyaw * M_PI * 2.0 / angle_; - - const float x2 = cx2 - r * cosf(cyawf + M_PI / 2); - const float y2 = cy2 - r * sinf(cyawf + M_PI / 2); - - path.push_back(CyclicVecFloat<3, 2>(x2, y2, cyaw)); - } - } - } - p_prev = p; - init = true; - } - path.push_back(CyclicVecFloat<3, 2>(path_grid.back())); - return path; -} -} // namespace planner_3d -} // namespace planner_cspace diff --git a/planner_cspace/src/planner_3d.cpp b/planner_cspace/src/planner_3d.cpp index 4affd184f..fc6fda3b9 100644 --- a/planner_cspace/src/planner_3d.cpp +++ b/planner_cspace/src/planner_3d.cpp @@ -77,7 +77,6 @@ #include #include #include -#include #include #include @@ -151,6 +150,7 @@ class Planner3dNode int tolerance_angle_; double tolerance_range_f_; double tolerance_angle_f_; + double interpolation_interval_; int unknown_cost_; bool overwrite_cost_; bool has_map_; @@ -365,8 +365,7 @@ class Planner3dNode path.header = map_header_; path.header.stamp = ros::Time::now(); - const std::list path_interpolated = - model_->path_interpolator_.interpolate(path_grid, 0.5, 0.0); + const std::list path_interpolated = model_->interpolatePath(path_grid); grid_metric_converter::appendGridPath2MetricPath(map_info_, path_interpolated, path); res.plan.header = map_header_; @@ -1202,6 +1201,7 @@ class Planner3dNode pnh_.param("esc_range", esc_range_f_, 0.25); pnh_.param("tolerance_range", tolerance_range_f_, 0.25); pnh_.param("tolerance_angle", tolerance_angle_f_, 0.0); + pnh_.param("interpolation_interval", interpolation_interval_, 0.5); pnh_.param("sw_wait", sw_wait_, 2.0f); pnh_.param("find_best", find_best_, true); @@ -1300,7 +1300,7 @@ class Planner3dNode ec_, local_range_, cost_estim_cache_.gridmap(), cm_, cm_hyst_, cm_rough_, - cc_, range_)); + cc_, range_, interpolation_interval_)); } void cbParameter(const Planner3DConfig& config, const uint32_t /* level */) @@ -1959,8 +1959,7 @@ class Planner3dNode { pub_preserved_path_poses_.publish(start_pose_predictor_.getPreservedPath()); } - const std::list path_interpolated = - model_->path_interpolator_.interpolate(path_grid, 0.5, local_range_); + const std::list path_interpolated = model_->interpolatePath(path_grid); path.poses = start_pose_predictor_.getPreservedPath().poses; grid_metric_converter::appendGridPath2MetricPath(map_info_, path_interpolated, path); diff --git a/planner_cspace/test/CMakeLists.txt b/planner_cspace/test/CMakeLists.txt index e78c749ee..ea959527b 100644 --- a/planner_cspace/test/CMakeLists.txt +++ b/planner_cspace/test/CMakeLists.txt @@ -60,16 +60,9 @@ catkin_add_gtest(test_motion_primitive_builder ) target_link_libraries(test_motion_primitive_builder ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -catkin_add_gtest(test_path_interpolator - src/test_path_interpolator.cpp - ../src/path_interpolator.cpp - ../src/rotation_cache.cpp -) -target_link_libraries(test_path_interpolator ${catkin_LIBRARIES} ${Boost_LIBRARIES}) - catkin_add_gtest(test_start_pose_predictor src/test_start_pose_predictor.cpp - ../src/path_interpolator.cpp + ../src/motion_cache.cpp ../src/rotation_cache.cpp ../src/start_pose_predictor.cpp ) diff --git a/planner_cspace/test/src/test_motion_cache.cpp b/planner_cspace/test/src/test_motion_cache.cpp index 645489bff..3cb3fb720 100644 --- a/planner_cspace/test/src/test_motion_cache.cpp +++ b/planner_cspace/test/src/test_motion_cache.cpp @@ -51,7 +51,7 @@ TEST(MotionCache, Generate) MotionCache cache; cache.reset( linear_resolution, angular_resolution, range, - gm.getAddressor()); + gm.getAddressor(), 1.0); // Straight motions const int xy_yaw_straight[][3] = diff --git a/planner_cspace/test/src/test_path_interpolator.cpp b/planner_cspace/test/src/test_path_interpolator.cpp deleted file mode 100644 index 839e8129e..000000000 --- a/planner_cspace/test/src/test_path_interpolator.cpp +++ /dev/null @@ -1,113 +0,0 @@ -/* - * Copyright (c) 2022, the neonavigation authors - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE - * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR - * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF - * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS - * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN - * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include - -#include - -#include - -#include -#include -#include - -namespace planner_cspace -{ -namespace planner_3d -{ -TEST(PathInterpolator, SimpleStraight) -{ - PathInterpolator pi; - pi.reset(4.0, 5); - const std::list> in = - { - CyclicVecInt<3, 2>(0, 0, 0), - CyclicVecInt<3, 2>(1, 0, 0), - CyclicVecInt<3, 2>(2, 0, 0), - }; - const std::list> out = pi.interpolate(in, 0.25, 0); - - const std::list> expected = - { - CyclicVecFloat<3, 2>(0.00f, 0.f, 0.f), - CyclicVecFloat<3, 2>(0.25f, 0.f, 0.f), - CyclicVecFloat<3, 2>(0.50f, 0.f, 0.f), - CyclicVecFloat<3, 2>(0.75f, 0.f, 0.f), - CyclicVecFloat<3, 2>(1.00f, 0.f, 0.f), - CyclicVecFloat<3, 2>(1.25f, 0.f, 0.f), - CyclicVecFloat<3, 2>(1.50f, 0.f, 0.f), - CyclicVecFloat<3, 2>(1.75f, 0.f, 0.f), - CyclicVecFloat<3, 2>(2.00f, 0.f, 0.f), - }; - - ASSERT_EQ(expected, out); -} - -TEST(PathInterpolator, DuplicatedPose) -{ - PathInterpolator pi; - pi.reset(4.0, 5); - const std::list> in = - { - CyclicVecInt<3, 2>(0, 0, 0), - CyclicVecInt<3, 2>(1, 0, 0), - CyclicVecInt<3, 2>(2, 0, 0), - }; - const std::list> out = pi.interpolate(in, 0.4999999, 0); - - costmap_cspace_msgs::MapMetaData3D map_info; - map_info.linear_resolution = 0.1; - map_info.angular_resolution = M_PI / 2; - map_info.origin.position.x = -100.5; - map_info.origin.position.y = -200.5; - - CyclicVecFloat<3, 2> m_prev; - for (const auto& g : out) - { - CyclicVecFloat<3, 2> m; - - // Introduce quantization error - grid_metric_converter::grid2Metric( - map_info, - g[0], g[1], g[2], - m[0], m[1], m[2]); - - ASSERT_NE(m_prev, m); - m_prev = m; - } -} -} // namespace planner_3d -} // namespace planner_cspace - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - - return RUN_ALL_TESTS(); -} diff --git a/planner_cspace/test/src/test_start_pose_predictor.cpp b/planner_cspace/test/src/test_start_pose_predictor.cpp index 7ed3f1918..205a4a54e 100644 --- a/planner_cspace/test/src/test_start_pose_predictor.cpp +++ b/planner_cspace/test/src/test_start_pose_predictor.cpp @@ -36,8 +36,8 @@ #include #include -#include #include +#include #include @@ -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; - interpolator_.reset(map_info_.linear_resolution, 1); + motion_cache_.reset(map_info_.linear_resolution, map_info_.angular_resolution, 10, cm_.getAddressor(), 0.1); const GridAstar<3, 2>::Vec size3d( static_cast(map_info_.width), @@ -108,12 +108,14 @@ class StartPosePredictorTester : public ::testing::Test nav_msgs::Path convertToMetricPath(const std::list::Vec>& path_grid) const { - const auto path_interpolated = interpolator_.interpolate(path_grid, 0.1, 10); + std::cerr << "convertToMetricPath" << std::endl; + const auto path_interpolated = motion_cache_.interpolatePath(path_grid); nav_msgs::Path result_path; result_path.header.frame_id = "map"; for (const auto& pose_grid : path_interpolated) { + std::cerr << "pose_grid: " << pose_grid[0] << ", " << pose_grid[1] << ", " << pose_grid[2] << std::endl; geometry_msgs::PoseStamped pose; pose.header.frame_id = "map"; float x, y, yaw; @@ -138,8 +140,8 @@ class StartPosePredictorTester : public ::testing::Test StartPosePredictor::Config config_; GridAstar<3, 2>::Gridmap cm_; costmap_cspace_msgs::MapMetaData3D map_info_; - PathInterpolator interpolator_; std::unordered_map, nav_msgs::Path>> paths_; + MotionCache motion_cache_; }; TEST_F(StartPosePredictorTester, EdgeCases) @@ -231,16 +233,16 @@ TEST_F(StartPosePredictorTester, SwitchBack) { StartPosePredictor::Astar::Vec start_grid; - EXPECT_TRUE(predictor_.process(getPose(0.55, 0.0, 0.0), cm_, map_info_, paths_["switch_back"].second, start_grid)); + EXPECT_TRUE(predictor_.process(getPose(0.65, 0.0, 0.0), cm_, map_info_, paths_["switch_back"].second, start_grid)); // The robot will reach the switch back within 2.0 seconds, and the start grid is the switch back point. EXPECT_EQ(StartPosePredictor::Astar::Vec(8, 6, 1), start_grid); const auto preserved_path = predictor_.getPreservedPath(); // A part of the first forward, the second forward, and the first turn. - ASSERT_EQ(preserved_path.poses.size(), 5 + 10 + 13); + ASSERT_EQ(preserved_path.poses.size(), 4 + 10 + 13); for (size_t i = 0; i < preserved_path.poses.size(); ++i) { const auto& path_pose = preserved_path.poses[i].pose; - const auto& expected_path_pose = paths_["switch_back"].second.poses[i + 6].pose; + const auto& expected_path_pose = paths_["switch_back"].second.poses[i + 7].pose; EXPECT_NEAR(path_pose.position.x, expected_path_pose.position.x, 1.0e-6); EXPECT_NEAR(path_pose.position.y, expected_path_pose.position.y, 1.0e-6); EXPECT_NEAR(tf2::getYaw(path_pose.orientation), tf2::getYaw(expected_path_pose.orientation), 1.0e-6); From 322ffbdf9f541836972543a1076bda6831ee89de Mon Sep 17 00:00:00 2001 From: Naotaka Hatao Date: Thu, 14 Mar 2024 21:49:29 +0900 Subject: [PATCH 2/6] Remove unnecessary change --- planner_cspace/test/src/test_start_pose_predictor.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/planner_cspace/test/src/test_start_pose_predictor.cpp b/planner_cspace/test/src/test_start_pose_predictor.cpp index 205a4a54e..49421e70a 100644 --- a/planner_cspace/test/src/test_start_pose_predictor.cpp +++ b/planner_cspace/test/src/test_start_pose_predictor.cpp @@ -108,14 +108,12 @@ class StartPosePredictorTester : public ::testing::Test nav_msgs::Path convertToMetricPath(const std::list::Vec>& path_grid) const { - std::cerr << "convertToMetricPath" << std::endl; const auto path_interpolated = motion_cache_.interpolatePath(path_grid); nav_msgs::Path result_path; result_path.header.frame_id = "map"; for (const auto& pose_grid : path_interpolated) { - std::cerr << "pose_grid: " << pose_grid[0] << ", " << pose_grid[1] << ", " << pose_grid[2] << std::endl; geometry_msgs::PoseStamped pose; pose.header.frame_id = "map"; float x, y, yaw; @@ -140,8 +138,8 @@ class StartPosePredictorTester : public ::testing::Test StartPosePredictor::Config config_; GridAstar<3, 2>::Gridmap cm_; costmap_cspace_msgs::MapMetaData3D map_info_; - std::unordered_map, nav_msgs::Path>> paths_; MotionCache motion_cache_; + std::unordered_map, nav_msgs::Path>> paths_; }; TEST_F(StartPosePredictorTester, EdgeCases) From 5c9e66a708fa2dd2be4a161c0826d011135f764b Mon Sep 17 00:00:00 2001 From: Naotaka Hatao Date: Sat, 16 Mar 2024 00:22:57 +0900 Subject: [PATCH 3/6] 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 53d152460..638e3b90d 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 0f7f86be9..721e7274f 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 49421e70a..1c60264b7 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), From 76c12c1310b110e8b6fd39518f41e60e1546b78b Mon Sep 17 00:00:00 2001 From: Naotaka Hatao Date: Tue, 19 Mar 2024 15:48:36 +0900 Subject: [PATCH 4/6] Small fixes --- planner_cspace/src/motion_cache.cpp | 3 +-- planner_cspace/src/planner_3d.cpp | 6 +++--- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/planner_cspace/src/motion_cache.cpp b/planner_cspace/src/motion_cache.cpp index 721e7274f..b1b162c0b 100644 --- a/planner_cspace/src/motion_cache.cpp +++ b/planner_cspace/src/motion_cache.cpp @@ -222,8 +222,7 @@ std::list> MotionCache::interpolatePath(const std::listsecond.getInterpolatedMotion(); - for (const auto& pos_diff : motion) + for (const auto& pos_diff : motion_it->second.getInterpolatedMotion()) { result.push_back(CyclicVecFloat<3, 2>((*it_prev)[0] + pos_diff[0], (*it_prev)[1] + pos_diff[1], pos_diff[2])); } diff --git a/planner_cspace/src/planner_3d.cpp b/planner_cspace/src/planner_3d.cpp index fc6fda3b9..062dbe16a 100644 --- a/planner_cspace/src/planner_3d.cpp +++ b/planner_cspace/src/planner_3d.cpp @@ -150,7 +150,7 @@ class Planner3dNode int tolerance_angle_; double tolerance_range_f_; double tolerance_angle_f_; - double interpolation_interval_; + double interpolation_resolution_; int unknown_cost_; bool overwrite_cost_; bool has_map_; @@ -1201,7 +1201,7 @@ class Planner3dNode pnh_.param("esc_range", esc_range_f_, 0.25); pnh_.param("tolerance_range", tolerance_range_f_, 0.25); pnh_.param("tolerance_angle", tolerance_angle_f_, 0.0); - pnh_.param("interpolation_interval", interpolation_interval_, 0.5); + pnh_.param("interpolation_resolution", interpolation_resolution_, 0.5); pnh_.param("sw_wait", sw_wait_, 2.0f); pnh_.param("find_best", find_best_, true); @@ -1300,7 +1300,7 @@ class Planner3dNode ec_, local_range_, cost_estim_cache_.gridmap(), cm_, cm_hyst_, cm_rough_, - cc_, range_, interpolation_interval_)); + cc_, range_, interpolation_resolution_)); } void cbParameter(const Planner3DConfig& config, const uint32_t /* level */) From 020516685eaa065f23fd616cce8e50017a5cbd11 Mon Sep 17 00:00:00 2001 From: Naotaka Hatao Date: Tue, 19 Mar 2024 16:01:05 +0900 Subject: [PATCH 5/6] Fix some parameter names --- .../planner_cspace/planner_3d/grid_astar_model.h | 3 ++- .../planner_cspace/planner_3d/motion_cache.h | 2 +- planner_cspace/src/grid_astar_model_3dof.cpp | 9 ++++++--- planner_cspace/src/planner_3d.cpp | 13 ++++++++++--- planner_cspace/test/src/test_motion_cache.cpp | 2 +- .../test/src/test_start_pose_predictor.cpp | 2 +- 6 files changed, 21 insertions(+), 10 deletions(-) diff --git a/planner_cspace/include/planner_cspace/planner_3d/grid_astar_model.h b/planner_cspace/include/planner_cspace/planner_3d/grid_astar_model.h index 515a95590..9b8f8aee4 100644 --- a/planner_cspace/include/planner_cspace/planner_3d/grid_astar_model.h +++ b/planner_cspace/include/planner_cspace/planner_3d/grid_astar_model.h @@ -110,7 +110,8 @@ class GridAstarModel3D : public GridAstarModelBase<3, 2> const BlockMemGridmapBase& cm_rough, const CostCoeff& cc, const int range, - const double interpolation_interval = 1.0); + const float path_interpolation_resolution = 0.5, + const float grid_enumeration_resolution = 0.1); void enableHysteresis(const bool enable); void createEuclidCostCache(); float euclidCost(const Vec& v) const; 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 638e3b90d..773a2f23d 100644 --- a/planner_cspace/include/planner_cspace/planner_3d/motion_cache.h +++ b/planner_cspace/include/planner_cspace/planner_3d/motion_cache.h @@ -110,7 +110,7 @@ class MotionCache const int range, const std::function, size_t&, size_t&)> gm_addr, const float interpolation_resolution, - const float grid_enumeration_resolution = 0.1); + const float grid_enumeration_resolution); std::list> interpolatePath(const std::list>& path_grid) const; diff --git a/planner_cspace/src/grid_astar_model_3dof.cpp b/planner_cspace/src/grid_astar_model_3dof.cpp index 4dfdbea0e..fb619d7b6 100644 --- a/planner_cspace/src/grid_astar_model_3dof.cpp +++ b/planner_cspace/src/grid_astar_model_3dof.cpp @@ -57,7 +57,8 @@ GridAstarModel3D::GridAstarModel3D( const BlockMemGridmapBase& cm_rough, const CostCoeff& cc, const int range, - const double interpolation_interval) + const float path_interpolation_resolution, + const float grid_enumeration_resolution) : hysteresis_(false) , map_info_(map_info) , euclid_cost_coef_(euclid_cost_coef) @@ -84,13 +85,15 @@ GridAstarModel3D::GridAstarModel3D( map_info_linear.angular_resolution, range_, cm_rough_.getAddressor(), - interpolation_interval); + path_interpolation_resolution, + grid_enumeration_resolution); motion_cache_.reset( map_info_.linear_resolution, map_info_.angular_resolution, range_, cm_.getAddressor(), - interpolation_interval); + path_interpolation_resolution, + grid_enumeration_resolution); // Make boundary check threshold min_boundary_ = motion_cache_.getMaxRange(); diff --git a/planner_cspace/src/planner_3d.cpp b/planner_cspace/src/planner_3d.cpp index 062dbe16a..eee4b91b5 100644 --- a/planner_cspace/src/planner_3d.cpp +++ b/planner_cspace/src/planner_3d.cpp @@ -150,7 +150,8 @@ class Planner3dNode int tolerance_angle_; double tolerance_range_f_; double tolerance_angle_f_; - double interpolation_resolution_; + double path_interpolation_resolution_; + double grid_enumeration_resolution_; int unknown_cost_; bool overwrite_cost_; bool has_map_; @@ -1201,7 +1202,13 @@ class Planner3dNode pnh_.param("esc_range", esc_range_f_, 0.25); pnh_.param("tolerance_range", tolerance_range_f_, 0.25); pnh_.param("tolerance_angle", tolerance_angle_f_, 0.0); - pnh_.param("interpolation_resolution", interpolation_resolution_, 0.5); + pnh_.param("path_interpolation_resolution", path_interpolation_resolution_, 0.5); + pnh_.param("grid_enumeration_resolution", grid_enumeration_resolution_, 0.1); + if (path_interpolation_resolution_ < grid_enumeration_resolution_) + { + ROS_ERROR("path_interpolation_resolution must be greater than or equal to grid_enumeration_resolution."); + path_interpolation_resolution_ = grid_enumeration_resolution_; + } pnh_.param("sw_wait", sw_wait_, 2.0f); pnh_.param("find_best", find_best_, true); @@ -1300,7 +1307,7 @@ class Planner3dNode ec_, local_range_, cost_estim_cache_.gridmap(), cm_, cm_hyst_, cm_rough_, - cc_, range_, interpolation_resolution_)); + cc_, range_, path_interpolation_resolution_, grid_enumeration_resolution_)); } void cbParameter(const Planner3DConfig& config, const uint32_t /* level */) diff --git a/planner_cspace/test/src/test_motion_cache.cpp b/planner_cspace/test/src/test_motion_cache.cpp index 3cb3fb720..ed9944c75 100644 --- a/planner_cspace/test/src/test_motion_cache.cpp +++ b/planner_cspace/test/src/test_motion_cache.cpp @@ -51,7 +51,7 @@ TEST(MotionCache, Generate) MotionCache cache; cache.reset( linear_resolution, angular_resolution, range, - gm.getAddressor(), 1.0); + gm.getAddressor(), 0.5, 0.1); // Straight motions const int xy_yaw_straight[][3] = diff --git a/planner_cspace/test/src/test_start_pose_predictor.cpp b/planner_cspace/test/src/test_start_pose_predictor.cpp index 1c60264b7..f355811b8 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, 4, cm_.getAddressor(), 0.1); + motion_cache_.reset(map_info_.linear_resolution, map_info_.angular_resolution, 4, cm_.getAddressor(), 0.1, 0.1); const GridAstar<3, 2>::Vec size3d( static_cast(map_info_.width), From 77e3ddf170022975eb73c6a8a7cc668d6866ebe8 Mon Sep 17 00:00:00 2001 From: Naotaka Hatao Date: Fri, 22 Mar 2024 10:20:36 +0900 Subject: [PATCH 6/6] Address comments --- planner_cspace/src/grid_astar_model_3dof.cpp | 1 - planner_cspace/src/motion_cache.cpp | 14 +++++--------- 2 files changed, 5 insertions(+), 10 deletions(-) diff --git a/planner_cspace/src/grid_astar_model_3dof.cpp b/planner_cspace/src/grid_astar_model_3dof.cpp index fb619d7b6..f330a9dbe 100644 --- a/planner_cspace/src/grid_astar_model_3dof.cpp +++ b/planner_cspace/src/grid_astar_model_3dof.cpp @@ -73,7 +73,6 @@ GridAstarModel3D::GridAstarModel3D( , cm_rough_(cm_rough) , cc_(cc) , range_(range) - { rot_cache_.reset(map_info_.linear_resolution, map_info_.angular_resolution, range_); diff --git a/planner_cspace/src/motion_cache.cpp b/planner_cspace/src/motion_cache.cpp index b1b162c0b..51718474e 100644 --- a/planner_cspace/src/motion_cache.cpp +++ b/planner_cspace/src/motion_cache.cpp @@ -96,7 +96,7 @@ void MotionCache::reset( 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); + const 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); @@ -144,14 +144,10 @@ void MotionCache::reset( const float cy2 = cy_s * (1.0 - ratio) + cy * ratio; const float cyaw = yaw + ratio * dyaw; - const float posf_raw[3] = - { - (cx2 - r * cosf(cyaw + M_PI / 2)) / linear_resolution, - (cy2 - r * sinf(cyaw + M_PI / 2)) / linear_resolution, - cyaw / angular_resolution, - }; - 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]); + const CyclicVecFloat<3, 2> posf((cx2 - r * cosf(cyaw + M_PI / 2)) / linear_resolution, + (cy2 - r * sinf(cyaw + M_PI / 2)) / linear_resolution, + cyaw / angular_resolution); + CyclicVecInt<3, 2> pos(posf[0], posf[1], posf[2]); pos.cycleUnsigned(angle); if ((pos != d) && (registered.find(pos) == registered.end())) {