diff --git a/planner_cspace/CMakeLists.txt b/planner_cspace/CMakeLists.txt index 5ef773b6..3b5902da 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 42f8ceea..9b8f8aee 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,9 @@ class GridAstarModel3D : public GridAstarModelBase<3, 2> const BlockMemGridmapBase& cm_hyst, const BlockMemGridmapBase& cm_rough, const CostCoeff& cc, - const int range); + const int range, + 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; @@ -125,6 +125,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 ccc7de46..773a2f23 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,6 +50,7 @@ class MotionCache friend class MotionCache; std::vector> motion_; + std::vector> interpolated_motion_; float distance_; public: @@ -60,6 +62,10 @@ class MotionCache { return motion_; } + const std::vector>& getInterpolatedMotion() const + { + return interpolated_motion_; + } }; using Cache = @@ -76,6 +82,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 +108,11 @@ 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_resolution, + const float grid_enumeration_resolution); + + 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 e0224417..00000000 --- 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 068f6f7b..f330a9db 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,9 @@ GridAstarModel3D::GridAstarModel3D( const BlockMemGridmapBase& cm_hyst, const BlockMemGridmapBase& cm_rough, const CostCoeff& cc, - const int range) + const int range, + const float path_interpolation_resolution, + const float grid_enumeration_resolution) : hysteresis_(false) , map_info_(map_info) , euclid_cost_coef_(euclid_cost_coef) @@ -81,12 +83,16 @@ GridAstarModel3D::GridAstarModel3D( map_info_linear.linear_resolution, map_info_linear.angular_resolution, range_, - cm_rough_.getAddressor()); + cm_rough_.getAddressor(), + path_interpolation_resolution, + grid_enumeration_resolution); motion_cache_.reset( map_info_.linear_resolution, map_info_.angular_resolution, range_, - cm_.getAddressor()); + cm_.getAddressor(), + path_interpolation_resolution, + grid_enumeration_resolution); // Make boundary check threshold min_boundary_ = motion_cache_.getMaxRange(); @@ -112,7 +118,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 +324,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 +368,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 f43a0a60..51718474 100644 --- a/planner_cspace/src/motion_cache.cpp +++ b/planner_cspace/src/motion_cache.cpp @@ -29,9 +29,12 @@ #include #include +#include #include #include +#include + #include #include @@ -43,7 +46,9 @@ 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_resolution, + const float grid_enumeration_resolution) { const int angle = std::lround(M_PI * 2 / angular_resolution); @@ -80,25 +85,32 @@ 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 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; 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; - CyclicVecInt<3, 2> pos( - 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); - 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; } + if (step % interpolation_step == 0) + { + page.interpolated_motion_.push_back(posf); + } } page.distance_ = d.len(); cache_[syaw][d] = page; @@ -124,28 +136,30 @@ void MotionCache::reset( CyclicVecFloat<3, 2> posf_prev(0, 0, 0); - for (float i = 0; i < 1.0; 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 posf_raw[3] = - { - (cx2 - r * cosf(cyaw + M_PI / 2)) / linear_resolution, - (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]); - CyclicVecInt<3, 2> pos(posf_raw[0], posf_raw[1], posf_raw[2]); + 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 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 (registered.find(pos) == registered.end()) + if ((pos != d) && (registered.find(pos) == registered.end())) { page.motion_.push_back(pos); registered[pos] = true; } - 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(); @@ -174,5 +188,45 @@ 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) + { + 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(); + 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 + { + 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])); + } + } + } + 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 5bb2ea50..00000000 --- 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 4affd184..eee4b91b 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,8 @@ class Planner3dNode int tolerance_angle_; double tolerance_range_f_; double tolerance_angle_f_; + double path_interpolation_resolution_; + double grid_enumeration_resolution_; int unknown_cost_; bool overwrite_cost_; bool has_map_; @@ -365,8 +366,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 +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("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_)); + cc_, range_, path_interpolation_resolution_, grid_enumeration_resolution_)); } void cbParameter(const Planner3DConfig& config, const uint32_t /* level */) @@ -1959,8 +1966,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 e78c749e..ea959527 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 645489bf..ed9944c7 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(), 0.5, 0.1); // 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 839e8129..00000000 --- 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 7ed3f191..f355811b 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, 4, cm_.getAddressor(), 0.1, 0.1); const GridAstar<3, 2>::Vec size3d( static_cast(map_info_.width), @@ -108,7 +108,7 @@ 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); + const auto path_interpolated = motion_cache_.interpolatePath(path_grid); nav_msgs::Path result_path; result_path.header.frame_id = "map"; @@ -138,7 +138,7 @@ class StartPosePredictorTester : public ::testing::Test StartPosePredictor::Config config_; GridAstar<3, 2>::Gridmap cm_; costmap_cspace_msgs::MapMetaData3D map_info_; - PathInterpolator interpolator_; + MotionCache motion_cache_; std::unordered_map, nav_msgs::Path>> paths_; }; @@ -231,16 +231,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);