Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat!: replace tier4_planning_msgs/PathWithLaneId with autoware_internal_planning_msgs/PathWithLaneId #10023

Open
wants to merge 3 commits into
base: main
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion build_depends.repos
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@ repositories:
core/autoware_internal_msgs:
type: git
url: https://github.com/autowarefoundation/autoware_internal_msgs.git
version: 1.3.0
version: 1.5.0
# universe
universe/external/tier4_autoware_msgs:
type: git
Expand Down
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 @@ -15,9 +15,9 @@
#ifndef AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_
#define AUTOWARE__MOTION_UTILS__RESAMPLE__RESAMPLE_HPP_

#include "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp"
#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 <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,10 +136,11 @@ 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,
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);
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);

/**
* @brief A resampling function for a path. Note that in a default setting, position xy are
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,11 +15,11 @@
#ifndef AUTOWARE__MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_
#define AUTOWARE__MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_

#include "autoware_internal_planning_msgs/msg/detail/path_with_lane_id__struct.hpp"
#include "autoware_planning_msgs/msg/detail/path__struct.hpp"
#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 <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,20 @@ 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 @@ -17,8 +17,8 @@

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

#include "autoware_internal_planning_msgs/msg/path_with_lane_id.hpp"
#include "autoware_planning_msgs/msg/trajectory.hpp"
#include "tier4_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 int64_t lane_id);
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 int64_t lane_id);
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
Loading