Skip to content

Commit

Permalink
refactor(autoware_interpolation): prefix package and namespace with a…
Browse files Browse the repository at this point in the history
…utoware

Signed-off-by: Esteve Fernandez <[email protected]>
  • Loading branch information
esteve committed Aug 6, 2024
1 parent b29c84e commit eff4efe
Show file tree
Hide file tree
Showing 85 changed files with 220 additions and 205 deletions.
Original file line number Diff line number Diff line change
@@ -1,10 +1,10 @@
cmake_minimum_required(VERSION 3.14)
project(interpolation)
project(autoware_interpolation)

find_package(autoware_cmake REQUIRED)
autoware_package()

ament_auto_add_library(interpolation SHARED
ament_auto_add_library(autoware_interpolation SHARED
src/linear_interpolation.cpp
src/spline_interpolation.cpp
src/spline_interpolation_points_2d.cpp
Expand All @@ -17,7 +17,7 @@ if(BUILD_TESTING)
ament_add_ros_isolated_gtest(test_interpolation ${test_files})

target_link_libraries(test_interpolation
interpolation
autoware_interpolation
)
endif()

Expand Down
File renamed without changes.
Original file line number Diff line number Diff line change
Expand Up @@ -12,15 +12,15 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef INTERPOLATION__INTERPOLATION_UTILS_HPP_
#define INTERPOLATION__INTERPOLATION_UTILS_HPP_
#ifndef AUTOWARE__INTERPOLATION__INTERPOLATION_UTILS_HPP_
#define AUTOWARE__INTERPOLATION__INTERPOLATION_UTILS_HPP_

#include <algorithm>
#include <array>
#include <stdexcept>
#include <vector>

namespace interpolation_utils
namespace autoware::interpolation
{
inline bool isIncreasing(const std::vector<double> & x)
{
Expand Down Expand Up @@ -109,6 +109,6 @@ void validateKeysAndValues(
throw std::invalid_argument("The size of base_keys and base_values are not the same.");
}
}
} // namespace interpolation_utils
} // namespace autoware::interpolation

#endif // INTERPOLATION__INTERPOLATION_UTILS_HPP_
#endif // AUTOWARE__INTERPOLATION__INTERPOLATION_UTILS_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,14 +12,14 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef INTERPOLATION__LINEAR_INTERPOLATION_HPP_
#define INTERPOLATION__LINEAR_INTERPOLATION_HPP_
#ifndef AUTOWARE__INTERPOLATION__LINEAR_INTERPOLATION_HPP_
#define AUTOWARE__INTERPOLATION__LINEAR_INTERPOLATION_HPP_

#include "interpolation/interpolation_utils.hpp"
#include "autoware/interpolation/interpolation_utils.hpp"

#include <vector>

namespace interpolation
namespace autoware::interpolation
{
double lerp(const double src_val, const double dst_val, const double ratio);

Expand All @@ -30,7 +30,6 @@ std::vector<double> lerp(
double lerp(
const std::vector<double> & base_keys, const std::vector<double> & base_values,
const double query_key);
} // namespace autoware::interpolation

} // namespace interpolation

#endif // INTERPOLATION__LINEAR_INTERPOLATION_HPP_
#endif // AUTOWARE__INTERPOLATION__LINEAR_INTERPOLATION_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,10 +12,10 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_
#define INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_
#ifndef AUTOWARE__INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_
#define AUTOWARE__INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_

#include "interpolation/interpolation_utils.hpp"
#include "autoware/interpolation/interpolation_utils.hpp"

#include <geometry_msgs/msg/quaternion.hpp>

Expand All @@ -29,7 +29,7 @@

#include <vector>

namespace interpolation
namespace autoware::interpolation
{
geometry_msgs::msg::Quaternion slerp(
const geometry_msgs::msg::Quaternion & src_quat, const geometry_msgs::msg::Quaternion & dst_quat,
Expand All @@ -43,6 +43,6 @@ std::vector<geometry_msgs::msg::Quaternion> slerp(
geometry_msgs::msg::Quaternion lerpOrientation(
const geometry_msgs::msg::Quaternion & o_from, const geometry_msgs::msg::Quaternion & o_to,
const double ratio);
} // namespace interpolation
} // namespace autoware::interpolation

#endif // INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_
#endif // AUTOWARE__INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,19 +12,19 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef INTERPOLATION__SPLINE_INTERPOLATION_HPP_
#define INTERPOLATION__SPLINE_INTERPOLATION_HPP_
#ifndef AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_HPP_
#define AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_HPP_

#include "autoware/interpolation/interpolation_utils.hpp"
#include "autoware/universe_utils/geometry/geometry.hpp"
#include "interpolation/interpolation_utils.hpp"

#include <algorithm>
#include <cmath>
#include <iostream>
#include <numeric>
#include <vector>

namespace interpolation
namespace autoware::interpolation
{
// NOTE: X(s) = a_i (s - s_i)^3 + b_i (s - s_i)^2 + c_i (s - s_i) + d_i : (i = 0, 1, ... N-1)
struct MultiSplineCoef
Expand Down Expand Up @@ -52,7 +52,6 @@ std::vector<double> spline(
std::vector<double> splineByAkima(
const std::vector<double> & base_keys, const std::vector<double> & base_values,
const std::vector<double> & query_keys);
} // namespace interpolation

// non-static 1-dimensional spline interpolation
//
Expand Down Expand Up @@ -104,5 +103,6 @@ class SplineInterpolation
void calcSplineCoefficients(
const std::vector<double> & base_keys, const std::vector<double> & base_values);
};
} // namespace autoware::interpolation

#endif // INTERPOLATION__SPLINE_INTERPOLATION_HPP_
#endif // AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,22 +12,21 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_
#define INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_
#ifndef AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_
#define AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_

#include "interpolation/spline_interpolation.hpp"
#include "autoware/interpolation/spline_interpolation.hpp"

#include <vector>

namespace interpolation
namespace autoware::interpolation
{
std::array<std::vector<double>, 3> slerp2dFromXY(
const std::vector<double> & base_keys, const std::vector<double> & base_x_values,
const std::vector<double> & base_y_values, const std::vector<double> & query_keys);

template <typename T>
std::vector<double> splineYawFromPoints(const std::vector<T> & points);
} // namespace interpolation

// non-static points spline interpolation
// NOTE: We can calculate yaw from the x and y by interpolation derivatives.
Expand Down Expand Up @@ -88,5 +87,6 @@ class SplineInterpolationPoints2d

std::vector<double> base_s_vec_;
};
} // namespace autoware::interpolation

#endif // INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_
#endif // AUTOWARE__INTERPOLATION__SPLINE_INTERPOLATION_POINTS_2D_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,14 +12,14 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef INTERPOLATION__ZERO_ORDER_HOLD_HPP_
#define INTERPOLATION__ZERO_ORDER_HOLD_HPP_
#ifndef AUTOWARE__INTERPOLATION__ZERO_ORDER_HOLD_HPP_
#define AUTOWARE__INTERPOLATION__ZERO_ORDER_HOLD_HPP_

#include "interpolation/interpolation_utils.hpp"
#include "autoware/interpolation/interpolation_utils.hpp"

#include <vector>

namespace interpolation
namespace autoware::interpolation
{
inline std::vector<size_t> calc_closest_segment_indices(
const std::vector<double> & base_keys, const std::vector<double> & query_keys,
Expand Down Expand Up @@ -76,6 +76,6 @@ std::vector<T> zero_order_hold(
return zero_order_hold(
base_keys, base_values, calc_closest_segment_indices(base_keys, query_keys, overlap_threshold));
}
} // namespace interpolation
} // namespace autoware::interpolation

#endif // INTERPOLATION__ZERO_ORDER_HOLD_HPP_
#endif // AUTOWARE__INTERPOLATION__ZERO_ORDER_HOLD_HPP_
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>interpolation</name>
<name>autoware_interpolation</name>
<version>0.1.0</version>
<description>The spline interpolation package</description>
<maintainer email="[email protected]">Fumiya Watanabe</maintainer>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,11 +12,11 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include "interpolation/linear_interpolation.hpp"
#include "autoware/interpolation/linear_interpolation.hpp"

#include <vector>

namespace interpolation
namespace autoware::interpolation
{
double lerp(const double src_val, const double dst_val, const double ratio)
{
Expand Down Expand Up @@ -56,4 +56,4 @@ double lerp(
{
return lerp(base_keys, base_values, std::vector<double>{query_key}).front();
}
} // namespace interpolation
} // namespace autoware::interpolation
Original file line number Diff line number Diff line change
Expand Up @@ -12,9 +12,9 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include "interpolation/spherical_linear_interpolation.hpp"
#include "autoware/interpolation/spherical_linear_interpolation.hpp"

namespace interpolation
namespace autoware::interpolation
{
geometry_msgs::msg::Quaternion slerp(
const geometry_msgs::msg::Quaternion & src_quat, const geometry_msgs::msg::Quaternion & dst_quat,
Expand Down Expand Up @@ -68,4 +68,4 @@ geometry_msgs::msg::Quaternion lerpOrientation(
const auto q_interpolated = q_from.slerp(q_to, ratio);
return tf2::toMsg(q_interpolated);
}
} // namespace interpolation
} // namespace autoware::interpolation
Original file line number Diff line number Diff line change
Expand Up @@ -12,11 +12,11 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include "interpolation/spline_interpolation.hpp"
#include "autoware/interpolation/spline_interpolation.hpp"

#include <vector>

namespace
namespace autoware::interpolation
{
// solve Ax = d
// where A is tridiagonal matrix
Expand Down Expand Up @@ -77,7 +77,6 @@ inline std::vector<double> solveTridiagonalMatrixAlgorithm(const TDMACoef & tdma

return x;
}
} // namespace

namespace interpolation
{
Expand Down Expand Up @@ -287,3 +286,4 @@ std::vector<double> SplineInterpolation::getSplineInterpolatedQuadDiffValues(

return res;
}
} // namespace autoware::interpolation
Original file line number Diff line number Diff line change
Expand Up @@ -12,11 +12,11 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include "interpolation/spline_interpolation_points_2d.hpp"
#include "autoware/interpolation/spline_interpolation_points_2d.hpp"

#include <vector>

namespace
namespace autoware::interpolation
{
std::vector<double> calcEuclidDist(const std::vector<double> & x, const std::vector<double> & y)
{
Expand Down Expand Up @@ -66,10 +66,6 @@ std::array<std::vector<double>, 4> getBaseValues(

return {base_s, base_x, base_y, base_z};
}
} // namespace

namespace interpolation
{

std::array<std::vector<double>, 3> slerp2dFromXY(
const std::vector<double> & base_keys, const std::vector<double> & base_x_values,
Expand Down Expand Up @@ -237,3 +233,4 @@ void SplineInterpolationPoints2d::calcSplineCoefficientsInner(
spline_y_ = SplineInterpolation(base_s_vec_, base_y_vec);
spline_z_ = SplineInterpolation(base_s_vec_, base_z_vec);
}
} // namespace autoware::interpolation
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include "interpolation/interpolation_utils.hpp"
#include "autoware/interpolation/interpolation_utils.hpp"

#include <gtest/gtest.h>

Expand All @@ -24,43 +24,43 @@ TEST(interpolation_utils, isIncreasing)
{
// empty
const std::vector<double> empty_vec;
EXPECT_THROW(interpolation_utils::isIncreasing(empty_vec), std::invalid_argument);
EXPECT_THROW(autoware::interpolation::isIncreasing(empty_vec), std::invalid_argument);

// increase
const std::vector<double> increasing_vec{0.0, 1.5, 3.0, 4.5, 6.0};
EXPECT_EQ(interpolation_utils::isIncreasing(increasing_vec), true);
EXPECT_EQ(autoware::interpolation::isIncreasing(increasing_vec), true);

// not decrease
const std::vector<double> not_increasing_vec{0.0, 1.5, 1.5, 4.5, 6.0};
EXPECT_EQ(interpolation_utils::isIncreasing(not_increasing_vec), false);
EXPECT_EQ(autoware::interpolation::isIncreasing(not_increasing_vec), false);

// decrease
const std::vector<double> decreasing_vec{0.0, 1.5, 1.2, 4.5, 6.0};
EXPECT_EQ(interpolation_utils::isIncreasing(decreasing_vec), false);
EXPECT_EQ(autoware::interpolation::isIncreasing(decreasing_vec), false);
}

TEST(interpolation_utils, isNotDecreasing)
{
// empty
const std::vector<double> empty_vec;
EXPECT_THROW(interpolation_utils::isNotDecreasing(empty_vec), std::invalid_argument);
EXPECT_THROW(autoware::interpolation::isNotDecreasing(empty_vec), std::invalid_argument);

// increase
const std::vector<double> increasing_vec{0.0, 1.5, 3.0, 4.5, 6.0};
EXPECT_EQ(interpolation_utils::isNotDecreasing(increasing_vec), true);
EXPECT_EQ(autoware::interpolation::isNotDecreasing(increasing_vec), true);

// not decrease
const std::vector<double> not_increasing_vec{0.0, 1.5, 1.5, 4.5, 6.0};
EXPECT_EQ(interpolation_utils::isNotDecreasing(not_increasing_vec), true);
EXPECT_EQ(autoware::interpolation::isNotDecreasing(not_increasing_vec), true);

// decrease
const std::vector<double> decreasing_vec{0.0, 1.5, 1.2, 4.5, 6.0};
EXPECT_EQ(interpolation_utils::isNotDecreasing(decreasing_vec), false);
EXPECT_EQ(autoware::interpolation::isNotDecreasing(decreasing_vec), false);
}

TEST(interpolation_utils, validateKeys)
{
using interpolation_utils::validateKeys;
using autoware::interpolation::validateKeys;

const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0};
const std::vector<double> query_keys{0.0, 1.0, 2.0, 3.0};
Expand Down Expand Up @@ -116,7 +116,7 @@ TEST(interpolation_utils, validateKeys)

TEST(interpolation_utils, validateKeysAndValues)
{
using interpolation_utils::validateKeysAndValues;
using autoware::interpolation::validateKeysAndValues;

const std::vector<double> base_keys{0.0, 1.0, 2.0, 3.0};
const std::vector<double> base_values{0.0, 1.0, 2.0, 3.0};
Expand Down
Loading

0 comments on commit eff4efe

Please sign in to comment.