Skip to content

Commit

Permalink
refactor(motion_utils): clear the repeat definitions and correct the …
Browse files Browse the repository at this point in the history
…dependencies (autowarefoundation#5909)

* change .hpp name

Signed-off-by: Zhe Shen <[email protected]>

* change .cpp name

Signed-off-by: Zhe Shen <[email protected]>

* correct the #inlcude and #ifndef

Signed-off-by: Zhe Shen <[email protected]>

* toPath(): move the prototypes and the implementations in .hpp and .cpp

Signed-off-by: Zhe Shen <[email protected]>

* correct the dependency for /home/shen/autoware.universe/planning/behavior_path_planner/src/behavior_path_planner_node.cpp

Signed-off-by: Zhe Shen <[email protected]>

* toPath(): deleted the repeat definition in planning_interface_test_manager_utils.hpp

Signed-off-by: Zhe Shen <[email protected]>

* toPath(): corrected the call and dependency in planning_interface_test_manager.cpp

Signed-off-by: Zhe Shen <[email protected]>

* convertPathToTrajectoryPoints(), convertTrajectoryPointsToPath(), lerpOrientation(): moved to conversion.hpp and conversion.cpp

Signed-off-by: Zhe Shen <[email protected]>

* convertPathToTrajectoryPoints(): corrected the call and dependency

Signed-off-by: Zhe Shen <[email protected]>

* convertTrajectoryPointsToPath(): Corrected the call

Signed-off-by: Zhe Shen <[email protected]>

* lerpOrientation(): deleted the repeat definition in longitudinal_controller_utils.hpp and longitudinal_controller_utils.cpp

Signed-off-by: Zhe Shen <[email protected]>

* lerpOrientation(): Correct the call in longitudinal_controller_utils.hpp

Signed-off-by: Zhe Shen <[email protected]>

* lerpOrientation(): Corrected the dependency and call in test_longitudinal_controller_utils.cpp

Signed-off-by: Zhe Shen <[email protected]>

* name of conversion.cpp: modified the CMakeLists

Signed-off-by: Zhe Shen <[email protected]>

* namespace updated, but maybe not correct, will be tested in the next commit

Signed-off-by: Zhe Shen <[email protected]>

* Correct the dependencies the test_trajectory.cpp and test_interpolation.cpp

Signed-off-by: Zhe Shen <[email protected]>

* style(pre-commit): autofix

* [namespace problems fixed] conversion.cpp & conversion.hpp: The dependencies have been added. Also, the namespaces have been corrected.

Signed-off-by: Zhe Shen <[email protected]>

* style(pre-commit): autofix

* correct all the dependencies in #include

Signed-off-by: Zhe Shen <[email protected]>

* style(pre-commit): autofix

* avoid using the ../../ in the paths

Signed-off-by: Zhe Shen <[email protected]>

* dependencies fixed and package added in .xml

Signed-off-by: Zhe Shen <[email protected]>

* style(pre-commit): autofix

* change toPath() to convertToPath()

Signed-off-by: Zhe Shen <[email protected]>

* lerpOrientation(): move from conversion to spherical_linear_interpolation

Signed-off-by: Zhe Shen <[email protected]>

* style(pre-commit): autofix

* fix the missing relative path definition. ../../ will not be used.

Signed-off-by: Zhe Shen <[email protected]>

* the unneccessary dependencies eleminated. In specific, the lerpOrientation() related dependencies in coversion.hpp and unneccessary dependencies in conversion.cpp are deleted.

Signed-off-by: Zhe Shen <[email protected]>

* change the function to template in .cpp and .hpp

Signed-off-by: Zhe Shen <[email protected]>

* style(pre-commit): autofix

* change the corresponding calls to template

Signed-off-by: Zhe Shen <[email protected]>

* style(pre-commit): autofix

* change name to convertToTrajectory()

Signed-off-by: Zhe Shen <[email protected]>

* style(pre-commit): autofix

* change the template of convertToPathWithLaneId()

Signed-off-by: Zhe Shen <[email protected]>

* fix the dependencies

Signed-off-by: Zhe Shen <[email protected]>

* refactor(motion_utils): specialize class

Signed-off-by: satoshi-ota <[email protected]>

* fix(motion_utils): remove unnecessary header

Signed-off-by: satoshi-ota <[email protected]>

---------

Signed-off-by: Zhe Shen <[email protected]>
Signed-off-by: satoshi-ota <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Satoshi OTA <[email protected]>
Co-authored-by: satoshi-ota <[email protected]>
  • Loading branch information
4 people authored and karishma1911 committed May 28, 2024
1 parent 6695018 commit b8e1cde
Show file tree
Hide file tree
Showing 14 changed files with 111 additions and 96 deletions.
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

0 comments on commit b8e1cde

Please sign in to comment.