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

refactor(motion_utils): cherry picked from a merged PR #1090

Merged
merged 1 commit into from
Jan 11, 2024
Merged
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
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,10 @@ std::vector<geometry_msgs::msg::Quaternion> slerp(
const std::vector<double> & base_keys,
const std::vector<geometry_msgs::msg::Quaternion> & base_values,
const std::vector<double> & query_keys);

geometry_msgs::msg::Quaternion lerpOrientation(
const geometry_msgs::msg::Quaternion & o_from, const geometry_msgs::msg::Quaternion & o_to,
const double ratio);
} // namespace interpolation

#endif // INTERPOLATION__SPHERICAL_LINEAR_INTERPOLATION_HPP_
11 changes: 11 additions & 0 deletions common/interpolation/src/spherical_linear_interpolation.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -57,4 +57,15 @@ std::vector<geometry_msgs::msg::Quaternion> slerp(
return query_values;
}

geometry_msgs::msg::Quaternion lerpOrientation(
const geometry_msgs::msg::Quaternion & o_from, const geometry_msgs::msg::Quaternion & o_to,
const double ratio)
{
tf2::Quaternion q_from, q_to;
tf2::fromMsg(o_from, q_from);
tf2::fromMsg(o_to, q_to);

const auto q_interpolated = q_from.slerp(q_to, ratio);
return tf2::toMsg(q_interpolated);
}
} // namespace interpolation
71 changes: 71 additions & 0 deletions common/motion_utils/include/motion_utils/trajectory/conversion.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,8 @@
#ifndef MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_
#define MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_

#include "autoware_auto_planning_msgs/msg/detail/path__struct.hpp"
#include "autoware_auto_planning_msgs/msg/detail/path_with_lane_id__struct.hpp"
#include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp"
#include "autoware_auto_planning_msgs/msg/detail/trajectory_point__struct.hpp"
#include "std_msgs/msg/header.hpp"
Expand All @@ -23,6 +25,8 @@

namespace motion_utils
{
using TrajectoryPoints = std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint>;

/**
* @brief Convert std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> to
* autoware_auto_planning_msgs::msg::Trajectory. This function is temporarily added for porting to
Expand All @@ -45,6 +49,73 @@ autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory(
std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> convertToTrajectoryPointArray(
const autoware_auto_planning_msgs::msg::Trajectory & trajectory);

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

template <>
inline autoware_auto_planning_msgs::msg::Path convertToPath(
const autoware_auto_planning_msgs::msg::PathWithLaneId & input)
{
autoware_auto_planning_msgs::msg::Path output{};
output.header = input.header;
output.left_bound = input.left_bound;
output.right_bound = input.right_bound;
output.points.resize(input.points.size());
for (size_t i = 0; i < input.points.size(); ++i) {
output.points.at(i) = input.points.at(i).point;
}
return output;
}

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

template <>
inline TrajectoryPoints convertToTrajectoryPoints(
const autoware_auto_planning_msgs::msg::PathWithLaneId & input)
{
TrajectoryPoints output{};
for (const auto & p : input.points) {
autoware_auto_planning_msgs::msg::TrajectoryPoint tp;
tp.pose = p.point.pose;
tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps;
// since path point doesn't have acc for now
tp.acceleration_mps2 = 0;
output.emplace_back(tp);
}
return output;
}

template <class T>
autoware_auto_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 autoware_auto_planning_msgs::msg::PathWithLaneId convertToPathWithLaneId(
const TrajectoryPoints & input)
{
autoware_auto_planning_msgs::msg::PathWithLaneId output{};
for (const auto & p : input) {
autoware_auto_planning_msgs::msg::PathPointWithLaneId pp;
pp.point.pose = p.pose;
pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps;
output.points.emplace_back(pp);
}
return output;
}

} // namespace motion_utils

#endif // MOTION_UTILS__TRAJECTORY__CONVERSION_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,8 @@
#define PID_LONGITUDINAL_CONTROLLER__LONGITUDINAL_CONTROLLER_UTILS_HPP_

#include "interpolation/linear_interpolation.hpp"
#include "interpolation/spherical_linear_interpolation.hpp"
#include "motion_utils/trajectory/conversion.hpp"
#include "motion_utils/trajectory/trajectory.hpp"
#include "tf2/utils.h"

Expand Down Expand Up @@ -74,14 +76,6 @@ Pose calcPoseAfterTimeDelay(
const Pose & current_pose, const double delay_time, const double current_vel,
const double current_acc);

/**
* @brief apply linear interpolation to orientation
* @param [in] o_from first orientation
* @param [in] o_to second orientation
* @param [in] ratio ratio between o_from and o_to for interpolation
*/
Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio);

/**
* @brief apply linear interpolation to trajectory point that is nearest to a certain point
* @param [in] points trajectory points
Expand All @@ -107,7 +101,7 @@ TrajectoryPoint lerpTrajectoryPoint(
points.at(i).pose.position.x, points.at(i + 1).pose.position.x, interpolate_ratio);
interpolated_point.pose.position.y = interpolation::lerp(
points.at(i).pose.position.y, points.at(i + 1).pose.position.y, interpolate_ratio);
interpolated_point.pose.orientation = lerpOrientation(
interpolated_point.pose.orientation = interpolation::lerpOrientation(
points.at(i).pose.orientation, points.at(i + 1).pose.orientation, interpolate_ratio);
interpolated_point.longitudinal_velocity_mps = interpolation::lerp(
points.at(i).longitudinal_velocity_mps, points.at(i + 1).longitudinal_velocity_mps,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -141,16 +141,6 @@ double lerp(const double v_from, const double v_to, const double ratio)
return v_from + (v_to - v_from) * ratio;
}

Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio)
{
tf2::Quaternion q_from, q_to;
tf2::fromMsg(o_from, q_from);
tf2::fromMsg(o_to, q_to);

const auto q_interpolated = q_from.slerp(q_to, ratio);
return tf2::toMsg(q_interpolated);
}

double applyDiffLimitFilter(
const double input_val, const double prev_val, const double dt, const double max_val,
const double min_val)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,8 @@
// limitations under the License.

#include "gtest/gtest.h"
#include "interpolation/spherical_linear_interpolation.hpp"
#include "motion_utils/trajectory/conversion.hpp"
#include "pid_longitudinal_controller/longitudinal_controller_utils.hpp"
#include "tf2/LinearMath/Quaternion.h"

Expand Down Expand Up @@ -303,15 +305,15 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation)
o_to.setRPY(M_PI_4, M_PI_4, M_PI_4);

ratio = 0.0;
result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
tf2::convert(result, o_result);
tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw);
EXPECT_DOUBLE_EQ(roll, 0.0);
EXPECT_DOUBLE_EQ(pitch, 0.0);
EXPECT_DOUBLE_EQ(yaw, 0.0);

ratio = 1.0;
result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
tf2::convert(result, o_result);
tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw);
EXPECT_DOUBLE_EQ(roll, M_PI_4);
Expand All @@ -320,23 +322,23 @@ TEST(TestLongitudinalControllerUtils, lerpOrientation)

ratio = 0.5;
o_to.setRPY(M_PI_4, 0.0, 0.0);
result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
tf2::convert(result, o_result);
tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw);
EXPECT_DOUBLE_EQ(roll, M_PI_4 / 2);
EXPECT_DOUBLE_EQ(pitch, 0.0);
EXPECT_DOUBLE_EQ(yaw, 0.0);

o_to.setRPY(0.0, M_PI_4, 0.0);
result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
tf2::convert(result, o_result);
tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw);
EXPECT_DOUBLE_EQ(roll, 0.0);
EXPECT_DOUBLE_EQ(pitch, M_PI_4 / 2);
EXPECT_DOUBLE_EQ(yaw, 0.0);

o_to.setRPY(0.0, 0.0, M_PI_4);
result = longitudinal_utils::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
result = interpolation::lerpOrientation(tf2::toMsg(o_from), tf2::toMsg(o_to), ratio);
tf2::convert(result, o_result);
tf2::Matrix3x3(o_result).getRPY(roll, pitch, yaw);
EXPECT_DOUBLE_EQ(roll, 0.0);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
#include "behavior_path_planner_common/marker_utils/utils.hpp"
#include "behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp"
#include "behavior_path_planner_common/utils/path_utils.hpp"
#include "motion_utils/trajectory/conversion.hpp"

#include <tier4_autoware_utils/ros/update_param.hpp>
#include <vehicle_info_util/vehicle_info_util.hpp>
Expand Down Expand Up @@ -700,7 +701,8 @@ Path BehaviorPathPlannerNode::convertToPath(
return output;
}

output = utils::toPath(*path_candidate_ptr);
output = motion_utils::convertToPath<autoware_auto_planning_msgs::msg::PathWithLaneId>(
*path_candidate_ptr);
// header is replaced by the input one, so it is substituted again
output.header = planner_data->route_handler->getRouteHeader();
output.header.stamp = this->now();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -58,8 +58,6 @@ PathWithLaneId resamplePathWithSpline(
const PathWithLaneId & path, const double interval, const bool keep_input_points = false,
const std::pair<double, double> target_section = {0.0, std::numeric_limits<double>::max()});

Path toPath(const PathWithLaneId & input);

size_t getIdxByArclength(
const PathWithLaneId & path, const size_t target_idx, const double signed_arc);

Expand Down
13 changes: 0 additions & 13 deletions planning/behavior_path_planner_common/src/utils/path_utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -155,19 +155,6 @@ PathWithLaneId resamplePathWithSpline(
return motion_utils::resamplePath(path, s_out);
}

Path toPath(const PathWithLaneId & input)
{
Path output{};
output.header = input.header;
output.left_bound = input.left_bound;
output.right_bound = input.right_bound;
output.points.resize(input.points.size());
for (size_t i = 0; i < input.points.size(); ++i) {
output.points.at(i) = input.points.at(i).point;
}
return output;
}

size_t getIdxByArclength(
const PathWithLaneId & path, const size_t target_idx, const double signed_arc)
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -35,11 +35,6 @@ using TrajectoryPoints = std::vector<TrajectoryPoint>;
using geometry_msgs::msg::Quaternion;
using TrajectoryPointWithIdx = std::pair<TrajectoryPoint, size_t>;

TrajectoryPoints convertPathToTrajectoryPoints(const PathWithLaneId & path);
PathWithLaneId convertTrajectoryPointsToPath(const TrajectoryPoints & trajectory);

Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio);

//! smooth path point with lane id starts from ego position on path to the path end
bool smoothPath(
const PathWithLaneId & in_path, PathWithLaneId & out_path,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,8 @@
// limitations under the License.

// #include <behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
#include "motion_utils/trajectory/conversion.hpp"

#include <behavior_velocity_planner_common/utilization/trajectory_utils.hpp>
#include <motion_utils/trajectory/trajectory.hpp>
#include <motion_velocity_smoother/trajectory_utils.hpp>
Expand Down Expand Up @@ -41,41 +43,6 @@ using TrajectoryPoints = std::vector<TrajectoryPoint>;
using geometry_msgs::msg::Quaternion;
using TrajectoryPointWithIdx = std::pair<TrajectoryPoint, size_t>;

TrajectoryPoints convertPathToTrajectoryPoints(const PathWithLaneId & path)
{
TrajectoryPoints tps;
for (const auto & p : path.points) {
TrajectoryPoint tp;
tp.pose = p.point.pose;
tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps;
// since path point doesn't have acc for now
tp.acceleration_mps2 = 0;
tps.emplace_back(tp);
}
return tps;
}
PathWithLaneId convertTrajectoryPointsToPath(const TrajectoryPoints & trajectory)
{
PathWithLaneId path;
for (const auto & p : trajectory) {
PathPointWithLaneId pp;
pp.point.pose = p.pose;
pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps;
path.points.emplace_back(pp);
}
return path;
}

Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio)
{
tf2::Quaternion q_from, q_to;
tf2::fromMsg(o_from, q_from);
tf2::fromMsg(o_to, q_to);

const auto q_interpolated = q_from.slerp(q_to, ratio);
return tf2::toMsg(q_interpolated);
}

//! smooth path point with lane id starts from ego position on path to the path end
bool smoothPath(
const PathWithLaneId & in_path, PathWithLaneId & out_path,
Expand All @@ -87,7 +54,9 @@ bool smoothPath(
const auto & external_v_limit = planner_data->external_velocity_limit;
const auto & smoother = planner_data->velocity_smoother_;

auto trajectory = convertPathToTrajectoryPoints(in_path);
auto trajectory =
motion_utils::convertToTrajectoryPoints<autoware_auto_planning_msgs::msg::PathWithLaneId>(
in_path);
const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory);

const auto traj_steering_rate_limited =
Expand Down Expand Up @@ -117,7 +86,7 @@ bool smoothPath(
motion_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit(
traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed);
}
out_path = convertTrajectoryPointsToPath(traj_smoothed);
out_path = motion_utils::convertToPathWithLaneId<TrajectoryPoints>(traj_smoothed);
return true;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -492,19 +492,6 @@ void updateNodeOptions(
node_options.arguments(std::vector<std::string>{arguments.begin(), arguments.end()});
}

Path toPath(const PathWithLaneId & input)
{
Path output{};
output.header = input.header;
output.left_bound = input.left_bound;
output.right_bound = input.right_bound;
output.points.resize(input.points.size());
for (size_t i = 0; i < input.points.size(); ++i) {
output.points.at(i) = input.points.at(i).point;
}
return output;
}

PathWithLaneId loadPathWithLaneIdInYaml()
{
const auto planning_test_utils_dir =
Expand Down
1 change: 1 addition & 0 deletions planning/planning_test_utils/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
<depend>component_interface_utils</depend>
<depend>lanelet2_extension</depend>
<depend>lanelet2_io</depend>
<depend>motion_utils</depend>
<depend>nav_msgs</depend>
<depend>rclcpp</depend>
<depend>route_handler</depend>
Expand Down
Loading
Loading