Skip to content

Commit

Permalink
perf(motion_utils): faster removeOverlapPoints and calcLateralOffset …
Browse files Browse the repository at this point in the history
…functions (awf#5385) (#1066)

* faster removeOverlapPoints and calcLateralOffset functions

Signed-off-by: Owen-Liuyuxuan <[email protected]>

* style(pre-commit): autofix

---------

Signed-off-by: Owen-Liuyuxuan <[email protected]>
Co-authored-by: Owen-Liuyuxuan <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
3 people authored and satoshi-ota committed Jan 14, 2024
1 parent 0ba179a commit c76e0b9
Show file tree
Hide file tree
Showing 2 changed files with 78 additions and 0 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -132,6 +132,7 @@ T removeOverlapPoints(const T & points, const size_t & start_idx = 0)
}

T dst;
dst.reserve(points.size());

for (size_t i = 0; i <= start_idx; ++i) {
dst.push_back(points.at(i));
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,77 @@
// Copyright 2023 TIER IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "motion_utils/trajectory/trajectory.hpp"

#include <gtest/gtest.h>
#include <gtest/internal/gtest-port.h>
#include <tf2/LinearMath/Quaternion.h>

#include <random>

namespace
{
using autoware_auto_planning_msgs::msg::Trajectory;
using tier4_autoware_utils::createPoint;
using tier4_autoware_utils::createQuaternionFromRPY;

constexpr double epsilon = 1e-6;

geometry_msgs::msg::Pose createPose(
double x, double y, double z, double roll, double pitch, double yaw)
{
geometry_msgs::msg::Pose p;
p.position = createPoint(x, y, z);
p.orientation = createQuaternionFromRPY(roll, pitch, yaw);
return p;
}

template <class T>
T generateTestTrajectory(
const size_t num_points, const double point_interval, const double vel = 0.0,
const double init_theta = 0.0, const double delta_theta = 0.0)
{
using Point = typename T::_points_type::value_type;

T traj;
for (size_t i = 0; i < num_points; ++i) {
const double theta = init_theta + i * delta_theta;
const double x = i * point_interval * std::cos(theta);
const double y = i * point_interval * std::sin(theta);

Point p;
p.pose = createPose(x, y, 0.0, 0.0, 0.0, theta);
p.longitudinal_velocity_mps = vel;
traj.points.push_back(p);
}

return traj;
}
} // namespace

TEST(trajectory_benchmark, DISABLED_calcLateralOffset)
{
std::random_device r;
std::default_random_engine e1(r());
std::uniform_real_distribution<double> uniform_dist(0.0, 1000.0);

using motion_utils::calcLateralOffset;

const auto traj = generateTestTrajectory<Trajectory>(1000, 1.0, 0.0, 0.0, 0.1);
constexpr auto nb_iteration = 10000;
for (auto i = 0; i < nb_iteration; ++i) {
const auto point = createPoint(uniform_dist(e1), uniform_dist(e1), 0.0);
calcLateralOffset(traj.points, point);
}
}

0 comments on commit c76e0b9

Please sign in to comment.