From 744c127bc45471c49b14691c11b6f87674e1ddfe Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Fri, 19 Aug 2022 14:58:38 +0300 Subject: [PATCH 1/6] refactor(motion_velocity_smoother): change curvature calculation Signed-off-by: Berkay Karaman --- .../src/trajectory_utils.cpp | 53 +++++++++---------- 1 file changed, 24 insertions(+), 29 deletions(-) diff --git a/planning/motion_velocity_smoother/src/trajectory_utils.cpp b/planning/motion_velocity_smoother/src/trajectory_utils.cpp index 0647600ba3e73..9d4452b6e30d9 100644 --- a/planning/motion_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/motion_velocity_smoother/src/trajectory_utils.cpp @@ -169,41 +169,36 @@ std::vector calcTrajectoryIntervalDistance(const TrajectoryPoints & traj std::vector calcTrajectoryCurvatureFrom3Points( const TrajectoryPoints & trajectory, size_t idx_dist) { - using tier4_autoware_utils::calcCurvature; - using tier4_autoware_utils::getPoint; + using tier4_autoware_utils::calcCurvature; + using tier4_autoware_utils::getPoint; - if (trajectory.size() < 3) { - const std::vector k_arr(trajectory.size(), 0.0); - return k_arr; - } + if (trajectory.size() < 3) { + const std::vector k_arr(trajectory.size(), 0.0); + return k_arr; + } - // if the idx size is not enough, change the idx_dist - const auto max_idx_dist = static_cast(std::floor((trajectory.size() - 1) / 2.0)); - idx_dist = std::max(1ul, std::min(idx_dist, max_idx_dist)); + // if the idx size is not enough, change the idx_dist + const auto max_idx_dist = static_cast(std::floor((trajectory.size() - 1) / 2.0)); + idx_dist = std::max(1ul, std::min(idx_dist, max_idx_dist)); - if (idx_dist < 1) { - throw std::logic_error("idx_dist less than 1 is not expected"); - } + if (idx_dist < 1) { + throw std::logic_error("idx_dist less than 1 is not expected"); + } - // calculate curvature by circle fitting from three points - std::vector k_arr; - for (size_t i = idx_dist; i < trajectory.size() - idx_dist; ++i) { - try { - const auto p0 = getPoint(trajectory.at(i - idx_dist)); - const auto p1 = getPoint(trajectory.at(i)); - const auto p2 = getPoint(trajectory.at(i + idx_dist)); - k_arr.push_back(calcCurvature(p0, p1, p2)); - } catch (...) { - k_arr.push_back(0.0); // points are too close. No curvature. + // calculate curvature by circle fitting from three points + std::vector k_arr; + // for first point curvature = 0; + k_arr.push_back(0.0); + for (size_t i = 1; i + 1 < trajectory.size(); i++) { + const auto p0 = getPoint(trajectory.at(i - std::min(idx_dist, i))); + const auto p1 = getPoint(trajectory.at(i)); + const auto p2 = getPoint(trajectory.at(i + std::min(idx_dist, trajectory.size() - 1 - i))); + k_arr.push_back(calcCurvature(p0, p1, p2)); } - } + // for last point curvature = 0; + k_arr.push_back(0.0); - // first and last curvature is copied from next value - for (size_t i = 0; i < idx_dist; ++i) { - k_arr.insert(k_arr.begin(), k_arr.front()); - k_arr.push_back(k_arr.back()); - } - return k_arr; + return k_arr; } void setZeroVelocity(TrajectoryPoints & trajectory) From 98630f4e0512dd4c5556ea6bf58997257d6353bf Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 19 Aug 2022 12:19:07 +0000 Subject: [PATCH 2/6] ci(pre-commit): autofix --- .../src/trajectory_utils.cpp | 50 +++++++++---------- 1 file changed, 25 insertions(+), 25 deletions(-) diff --git a/planning/motion_velocity_smoother/src/trajectory_utils.cpp b/planning/motion_velocity_smoother/src/trajectory_utils.cpp index 9d4452b6e30d9..b417b03666547 100644 --- a/planning/motion_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/motion_velocity_smoother/src/trajectory_utils.cpp @@ -169,36 +169,36 @@ std::vector calcTrajectoryIntervalDistance(const TrajectoryPoints & traj std::vector calcTrajectoryCurvatureFrom3Points( const TrajectoryPoints & trajectory, size_t idx_dist) { - using tier4_autoware_utils::calcCurvature; - using tier4_autoware_utils::getPoint; + using tier4_autoware_utils::calcCurvature; + using tier4_autoware_utils::getPoint; - if (trajectory.size() < 3) { - const std::vector k_arr(trajectory.size(), 0.0); - return k_arr; - } + if (trajectory.size() < 3) { + const std::vector k_arr(trajectory.size(), 0.0); + return k_arr; + } - // if the idx size is not enough, change the idx_dist - const auto max_idx_dist = static_cast(std::floor((trajectory.size() - 1) / 2.0)); - idx_dist = std::max(1ul, std::min(idx_dist, max_idx_dist)); + // if the idx size is not enough, change the idx_dist + const auto max_idx_dist = static_cast(std::floor((trajectory.size() - 1) / 2.0)); + idx_dist = std::max(1ul, std::min(idx_dist, max_idx_dist)); - if (idx_dist < 1) { - throw std::logic_error("idx_dist less than 1 is not expected"); - } + if (idx_dist < 1) { + throw std::logic_error("idx_dist less than 1 is not expected"); + } - // calculate curvature by circle fitting from three points - std::vector k_arr; - // for first point curvature = 0; - k_arr.push_back(0.0); - for (size_t i = 1; i + 1 < trajectory.size(); i++) { - const auto p0 = getPoint(trajectory.at(i - std::min(idx_dist, i))); - const auto p1 = getPoint(trajectory.at(i)); - const auto p2 = getPoint(trajectory.at(i + std::min(idx_dist, trajectory.size() - 1 - i))); - k_arr.push_back(calcCurvature(p0, p1, p2)); - } - // for last point curvature = 0; - k_arr.push_back(0.0); + // calculate curvature by circle fitting from three points + std::vector k_arr; + // for first point curvature = 0; + k_arr.push_back(0.0); + for (size_t i = 1; i + 1 < trajectory.size(); i++) { + const auto p0 = getPoint(trajectory.at(i - std::min(idx_dist, i))); + const auto p1 = getPoint(trajectory.at(i)); + const auto p2 = getPoint(trajectory.at(i + std::min(idx_dist, trajectory.size() - 1 - i))); + k_arr.push_back(calcCurvature(p0, p1, p2)); + } + // for last point curvature = 0; + k_arr.push_back(0.0); - return k_arr; + return k_arr; } void setZeroVelocity(TrajectoryPoints & trajectory) From 54b1cee6610f6aa446463dcd690b73963300ded7 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Wed, 24 Aug 2022 15:12:39 +0300 Subject: [PATCH 3/6] add error handler for too close points Signed-off-by: Berkay Karaman --- .../src/trajectory_utils.cpp | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/planning/motion_velocity_smoother/src/trajectory_utils.cpp b/planning/motion_velocity_smoother/src/trajectory_utils.cpp index b417b03666547..72d84171df6d9 100644 --- a/planning/motion_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/motion_velocity_smoother/src/trajectory_utils.cpp @@ -190,10 +190,25 @@ std::vector calcTrajectoryCurvatureFrom3Points( // for first point curvature = 0; k_arr.push_back(0.0); for (size_t i = 1; i + 1 < trajectory.size(); i++) { + double curvature = 0.0; const auto p0 = getPoint(trajectory.at(i - std::min(idx_dist, i))); const auto p1 = getPoint(trajectory.at(i)); const auto p2 = getPoint(trajectory.at(i + std::min(idx_dist, trajectory.size() - 1 - i))); - k_arr.push_back(calcCurvature(p0, p1, p2)); + try { + curvature = calcCurvature(p0, p1, p2); + } catch (std::exception const& e) { + // ...code that handles the error... + RCLCPP_WARN( + rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), + "%s", e.what()); + if(!k_arr.empty()) + { + curvature = k_arr.back(); // previous curvature + } else { + curvature = 0.0; + } + } + k_arr.push_back(curvature); } // for last point curvature = 0; k_arr.push_back(0.0); From 136c38be90efcafb1795b416eacc09c79f27cb56 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 24 Aug 2022 12:14:09 +0000 Subject: [PATCH 4/6] ci(pre-commit): autofix --- .../motion_velocity_smoother/src/trajectory_utils.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/planning/motion_velocity_smoother/src/trajectory_utils.cpp b/planning/motion_velocity_smoother/src/trajectory_utils.cpp index 72d84171df6d9..3d212a7560632 100644 --- a/planning/motion_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/motion_velocity_smoother/src/trajectory_utils.cpp @@ -196,13 +196,12 @@ std::vector calcTrajectoryCurvatureFrom3Points( const auto p2 = getPoint(trajectory.at(i + std::min(idx_dist, trajectory.size() - 1 - i))); try { curvature = calcCurvature(p0, p1, p2); - } catch (std::exception const& e) { + } catch (std::exception const & e) { // ...code that handles the error... RCLCPP_WARN( - rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), - "%s", e.what()); - if(!k_arr.empty()) - { + rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), "%s", + e.what()); + if (!k_arr.empty()) { curvature = k_arr.back(); // previous curvature } else { curvature = 0.0; From 7fd4a9ad067e94dbb8a31516bbcbd3aa70550b11 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Wed, 31 Aug 2022 02:17:36 +0300 Subject: [PATCH 5/6] take copy for first and last points Signed-off-by: Berkay Karaman --- .../src/trajectory_utils.cpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/planning/motion_velocity_smoother/src/trajectory_utils.cpp b/planning/motion_velocity_smoother/src/trajectory_utils.cpp index 3d212a7560632..6e0d6905dda82 100644 --- a/planning/motion_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/motion_velocity_smoother/src/trajectory_utils.cpp @@ -186,9 +186,9 @@ std::vector calcTrajectoryCurvatureFrom3Points( } // calculate curvature by circle fitting from three points - std::vector k_arr; - // for first point curvature = 0; - k_arr.push_back(0.0); + std::vector k_arr(trajectory.size(), 0.0); + + for (size_t i = 1; i + 1 < trajectory.size(); i++) { double curvature = 0.0; const auto p0 = getPoint(trajectory.at(i - std::min(idx_dist, i))); @@ -201,16 +201,17 @@ std::vector calcTrajectoryCurvatureFrom3Points( RCLCPP_WARN( rclcpp::get_logger("motion_velocity_smoother").get_child("trajectory_utils"), "%s", e.what()); - if (!k_arr.empty()) { - curvature = k_arr.back(); // previous curvature + if (i > 1) { + curvature = k_arr.at(i - 1); // previous curvature } else { curvature = 0.0; } } - k_arr.push_back(curvature); + k_arr.at(i) = curvature; } - // for last point curvature = 0; - k_arr.push_back(0.0); + // copy curvatures for the last and first points; + k_arr.at(0) = k_arr.at(1); + k_arr.back() = k_arr.at((trajectory.size() - 2)); return k_arr; } From a786a62d9532ca23db00e20c7f88c124e72dbab0 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 30 Aug 2022 23:19:11 +0000 Subject: [PATCH 6/6] ci(pre-commit): autofix --- planning/motion_velocity_smoother/src/trajectory_utils.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/planning/motion_velocity_smoother/src/trajectory_utils.cpp b/planning/motion_velocity_smoother/src/trajectory_utils.cpp index 6e0d6905dda82..629ba1b15040a 100644 --- a/planning/motion_velocity_smoother/src/trajectory_utils.cpp +++ b/planning/motion_velocity_smoother/src/trajectory_utils.cpp @@ -188,7 +188,6 @@ std::vector calcTrajectoryCurvatureFrom3Points( // calculate curvature by circle fitting from three points std::vector k_arr(trajectory.size(), 0.0); - for (size_t i = 1; i + 1 < trajectory.size(); i++) { double curvature = 0.0; const auto p0 = getPoint(trajectory.at(i - std::min(idx_dist, i)));