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 all 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
52 changes: 9 additions & 43 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 @@ -12,7 +12,10 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include <interpolation/linear_interpolation.hpp>
#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 +87,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 +137,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 All @@ -151,7 +154,7 @@ autoware_auto_planning_msgs::msg::Path interpolatePath(
return path;
}

double path_len = length;
double path_len = std::min(length, motion_utils::calcArcLength(path.points));
{
double s = 0.0;
for (size_t idx = 0; idx < path.points.size(); ++idx) {
Expand All @@ -162,7 +165,7 @@ autoware_auto_planning_msgs::msg::Path interpolatePath(
v.push_back(path_point.longitudinal_velocity_mps);
if (idx != 0) {
const auto path_point_prev = path.points.at(idx - 1);
s += tier4_autoware_utils::calcDistance3d(path_point_prev.pose, path_point.pose);
s += tier4_autoware_utils::calcDistance2d(path_point_prev.pose, path_point.pose);
}
if (s > path_len) {
break;
Expand Down Expand Up @@ -205,44 +208,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