Skip to content

Commit

Permalink
feat!: replace tier4_planning_msgs/PathWithLaneId with autoware_inter…
Browse files Browse the repository at this point in the history
…nal_planning_msgs/PathWithLaneId

Signed-off-by: mitsudome-r <[email protected]>
  • Loading branch information
mitsudome-r committed Jan 27, 2025
1 parent 32e3402 commit 9ad78e6
Show file tree
Hide file tree
Showing 217 changed files with 810 additions and 814 deletions.
2 changes: 1 addition & 1 deletion common/autoware_motion_utils/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ The second function finds the nearest index in the lane whose id is `lane_id`.
```cpp
size_t findNearestIndexFromLaneId(
const tier4_planning_msgs::msg::PathWithLaneId & path,
const autoware_internal_planning_msgs::msg::PathWithLaneId & path,
const geometry_msgs::msg::Point & pos, const int64_t lane_id);
```

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@

#include "autoware_planning_msgs/msg/path.hpp"
#include "autoware_planning_msgs/msg/trajectory.hpp"
#include "tier4_planning_msgs/msg/path_with_lane_id.hpp"
#include "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp"

#include <vector>

Expand Down Expand Up @@ -113,8 +113,8 @@ std::vector<geometry_msgs::msg::Pose> resamplePoseVector(
* longitudinal and lateral velocity. Otherwise, it uses linear interpolation
* @return resampled path
*/
tier4_planning_msgs::msg::PathWithLaneId resamplePath(
const tier4_planning_msgs::msg::PathWithLaneId & input_path,
autoware_internal_planning_msgs::msg::PathWithLaneId resamplePath(
const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path,
const std::vector<double> & resampled_arclength, const bool use_akima_spline_for_xy = false,
const bool use_lerp_for_z = true, const bool use_zero_order_hold_for_v = true);

Expand All @@ -136,8 +136,8 @@ tier4_planning_msgs::msg::PathWithLaneId resamplePath(
* @param resample_input_path_stop_point If true, resample closest stop point in input path
* @return resampled path
*/
tier4_planning_msgs::msg::PathWithLaneId resamplePath(
const tier4_planning_msgs::msg::PathWithLaneId & input_path, const double resample_interval,
autoware_internal_planning_msgs::msg::PathWithLaneId resamplePath(
const autoware_internal_planning_msgs::msg::PathWithLaneId & input_path, const double resample_interval,
const bool use_akima_spline_for_xy = false, const bool use_lerp_for_z = true,
const bool use_zero_order_hold_for_v = true, const bool resample_input_path_stop_point = true);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@
#include "autoware_planning_msgs/msg/detail/trajectory__struct.hpp"
#include "autoware_planning_msgs/msg/detail/trajectory_point__struct.hpp"
#include "std_msgs/msg/header.hpp"
#include "tier4_planning_msgs/msg/detail/path_with_lane_id__struct.hpp"
#include "autoware_internal_planning_msgs/msg/detail/path_with_lane_id__struct.hpp"

#include <vector>

Expand Down Expand Up @@ -58,7 +58,7 @@ autoware_planning_msgs::msg::Path convertToPath([[maybe_unused]] const T & input

template <>
inline autoware_planning_msgs::msg::Path convertToPath(
const tier4_planning_msgs::msg::PathWithLaneId & input)
const autoware_internal_planning_msgs::msg::PathWithLaneId & input)
{
autoware_planning_msgs::msg::Path output{};
output.header = input.header;
Expand All @@ -80,7 +80,7 @@ TrajectoryPoints convertToTrajectoryPoints([[maybe_unused]] const T & input)

template <>
inline TrajectoryPoints convertToTrajectoryPoints(
const tier4_planning_msgs::msg::PathWithLaneId & input)
const autoware_internal_planning_msgs::msg::PathWithLaneId & input)
{
TrajectoryPoints output{};
for (const auto & p : input.points) {
Expand All @@ -95,19 +95,19 @@ inline TrajectoryPoints convertToTrajectoryPoints(
}

template <class T>
tier4_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId([[maybe_unused]] const T & input)
autoware_internal_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId([[maybe_unused]] const T & input)
{
static_assert(sizeof(T) == 0, "Only specializations of convertToPathWithLaneId can be used.");
throw std::logic_error("Only specializations of convertToPathWithLaneId can be used.");
}

template <>
inline tier4_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId(
inline autoware_internal_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId(
const TrajectoryPoints & input)
{
tier4_planning_msgs::msg::PathWithLaneId output{};
autoware_internal_planning_msgs::msg::PathWithLaneId output{};
for (const auto & p : input) {
tier4_planning_msgs::msg::PathPointWithLaneId pp;
autoware_internal_planning_msgs::msg::PathPointWithLaneId pp;
pp.point.pose = p.pose;
pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps;
output.points.emplace_back(pp);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@
#include "autoware/universe_utils/geometry/geometry.hpp"

#include "autoware_planning_msgs/msg/trajectory.hpp"
#include "tier4_planning_msgs/msg/path_with_lane_id.hpp"
#include "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp"

#include <boost/optional.hpp>

Expand Down Expand Up @@ -51,8 +51,8 @@ autoware_planning_msgs::msg::TrajectoryPoint calcInterpolatedPoint(
* twist information
* @return resampled path(poses)
*/
tier4_planning_msgs::msg::PathPointWithLaneId calcInterpolatedPoint(
const tier4_planning_msgs::msg::PathWithLaneId & path,
autoware_internal_planning_msgs::msg::PathPointWithLaneId calcInterpolatedPoint(
const autoware_internal_planning_msgs::msg::PathWithLaneId & path,
const geometry_msgs::msg::Pose & target_pose, const bool use_zero_order_hold_for_twist = false,
const double dist_threshold = std::numeric_limits<double>::max(),
const double yaw_threshold = std::numeric_limits<double>::max());
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,31 +15,31 @@
#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_
#define AUTOWARE__MOTION_UTILS__TRAJECTORY__PATH_WITH_LANE_ID_HPP_

#include "tier4_planning_msgs/msg/path_with_lane_id.hpp"
#include "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp"
#include <geometry_msgs/msg/point.hpp>

#include <optional>
#include <utility>
namespace autoware::motion_utils
{
std::optional<std::pair<size_t, size_t>> getPathIndexRangeWithLaneId(
const tier4_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id);
const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const int64_t target_lane_id);

size_t findNearestIndexFromLaneId(
const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos,
const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos,
const int64_t lane_id);

size_t findNearestSegmentIndexFromLaneId(
const tier4_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos,
const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Point & pos,
const int64_t lane_id);

// @brief Calculates the path to be followed by the rear wheel center in order to make the vehicle
// center follow the input path
// @param [in] path with position to be followed by the center of the vehicle
// @param [out] PathWithLaneId to be followed by the rear wheel center follow to make the vehicle
// center follow the input path NOTE: rear_to_cog is supposed to be positive
tier4_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter(
const tier4_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog,
autoware_internal_planning_msgs::msg::PathWithLaneId convertToRearWheelCenter(
const autoware_internal_planning_msgs::msg::PathWithLaneId & path, const double rear_to_cog,
const bool enable_last_point_compensation = true);
} // namespace autoware::motion_utils

Expand Down
Loading

0 comments on commit 9ad78e6

Please sign in to comment.