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.

Sign up for GitHub

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

Jump to bottom

feat(behavior_velocity_planner): use-motion-utils #1493

Merged
merged 3 commits into from
Aug 2, 2022
Merged

feat(behavior_velocity_planner): use-motion-utils #1493

Changes from 2 commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
7fde960
feat(obstacle_velocity_planner): use-motion-utils
purewater0901 Aug 2, 2022
79ede01
change z interpolation to linear interpolation
purewater0901 Aug 2, 2022
5726e79
fix arclength calculation
purewater0901 Aug 2, 2022
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
47 changes: 6 additions & 41 deletions planning/behavior_velocity_planner/src/utilization/path_utilization.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,8 @@
// limitations under the License.

#include <interpolation/spline_interpolation.hpp>
#include <interpolation/zero_order_hold.hpp>
#include <motion_utils/resample/resample.hpp>
#include <rclcpp/rclcpp.hpp>
#include <utilization/path_utilization.hpp>

Expand Down Expand Up @@ -84,8 +86,9 @@ bool splineInterpolate(
// do spline for xy
const std::vector<double> resampled_x = ::interpolation::slerp(base_s, base_x, resampled_s);
const std::vector<double> resampled_y = ::interpolation::slerp(base_s, base_y, resampled_s);
const std::vector<double> resampled_z = ::interpolation::slerp(base_s, base_z, resampled_s);
const std::vector<double> resampled_v = ::interpolation::slerp(base_s, base_v, resampled_s);
const std::vector<double> resampled_z = ::interpolation::lerp(base_s, base_z, resampled_s);
const std::vector<double> resampled_v =
::interpolation::zero_order_hold(base_s, base_v, resampled_s);

// set xy
output->points.clear();
Expand Down Expand Up @@ -133,7 +136,6 @@ autoware_auto_planning_msgs::msg::Path interpolatePath(
const autoware_auto_planning_msgs::msg::Path & path, const double length, const double interval)
{
const auto logger{rclcpp::get_logger("behavior_velocity_planner").get_child("path_utilization")};
autoware_auto_planning_msgs::msg::Path interpolated_path;

const double epsilon = 0.01;
std::vector<double> x;
Expand Down Expand Up @@ -205,44 +207,7 @@ autoware_auto_planning_msgs::msg::Path interpolatePath(
return path;
}

// Interpolate
const auto x_interp = interpolation::slerp(s_in, x, s_out);
const auto y_interp = interpolation::slerp(s_in, y, s_out);
const auto z_interp = interpolation::slerp(s_in, z, s_out);

// Find a nearest segment for each point in s_out and use the velocity of the segment's beginning
// point. Note that if s_out is almost the same value as s_in within DOUBLE_EPSILON range, the
// velocity of s_out should be same as the one of s_in.
std::vector<double> v_interp;
size_t closest_segment_idx = 0;
for (size_t i = 0; i < s_out.size() - 1; ++i) {
for (size_t j = closest_segment_idx; j < s_in.size() - 1; ++j) {
if (s_in.at(j) - DOUBLE_EPSILON < s_out.at(i) && s_out.at(i) < s_in.at(j + 1)) {
// find closest segment in s_in
closest_segment_idx = j;
}
}
v_interp.push_back(v.at(closest_segment_idx));
}
v_interp.push_back(v.back());

// Insert path point to interpolated_path
for (size_t idx = 0; idx < v_interp.size() - 1; ++idx) {
autoware_auto_planning_msgs::msg::PathPoint path_point;
path_point.pose.position.x = x_interp.at(idx);
path_point.pose.position.y = y_interp.at(idx);
path_point.pose.position.z = z_interp.at(idx);
path_point.longitudinal_velocity_mps = v_interp.at(idx);
const double yaw =
std::atan2(y_interp.at(idx + 1) - y_interp.at(idx), x_interp.at(idx + 1) - x_interp.at(idx));
tf2::Quaternion quat;
quat.setRPY(0, 0, yaw);
path_point.pose.orientation = tf2::toMsg(quat);
interpolated_path.points.push_back(path_point);
}
interpolated_path.points.push_back(path.points.back());

return interpolated_path;
return motion_utils::resamplePath(path, s_out);
taikitanaka3 marked this conversation as resolved.
Show resolved Hide resolved
}

autoware_auto_planning_msgs::msg::Path filterLitterPathPoint(
Expand Down