Skip to content

Commit

Permalink
fix
Browse files Browse the repository at this point in the history
Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
  • Loading branch information
soblin committed Dec 13, 2024
1 parent 0767098 commit 1548a30
Show file tree
Hide file tree
Showing 5 changed files with 27 additions and 16 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -102,7 +102,7 @@ void plot_footprint(
axes.plot(Args(xs, ys), Kwargs("color"_a = color, "linestyle"_a = "dotted"));
}

void plot_lanelet_polyogn(matplotlibcpp17::axes::Axes & axes, const lanelet::BasicPolygon2d polygon)
void plot_lanelet_polygon(matplotlibcpp17::axes::Axes & axes, const lanelet::BasicPolygon2d polygon)
{
std::vector<double> xs, ys;
for (const auto & p : polygon) {
Expand Down Expand Up @@ -353,7 +353,7 @@ std::vector<PullOverPath> selectPullOverPaths(
std::map<size_t, double> path_id_to_rough_margin_map;
const auto & target_objects = autoware_perception_msgs::msg::PredictedObjects{};
for (const size_t i : sorted_path_indices) {
const auto & path = pull_over_path_candidates[i];
const auto & path = pull_over_path_candidates[i]; // cppcheck-suppress containerOutOfBounds
const double distance =
autoware::behavior_path_planner::utils::path_safety_checker::calculateRoughDistanceToObjects(
path.parking_path(), target_objects, planner_data->parameters, false, "max");
Expand Down Expand Up @@ -663,7 +663,7 @@ int main(int argc, char ** argv)
const auto bus_stop_area_polygons =
getBusStopAreaPolygons(planner_data, goal_planner_parameter);
for (const auto & bus_stop_area_polygon : bus_stop_area_polygons) {
plot_lanelet_polyogn(ax1, bus_stop_area_polygon);
plot_lanelet_polygon(ax1, bus_stop_area_polygon);
}
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -45,10 +45,17 @@
#include <pybind11/pytypes.h>
#include <yaml-cpp/yaml.h>

#include <algorithm>
#include <chrono>
#include <cmath>
#include <functional>
#include <iostream>

#include <map>
#include <memory>
#include <string>
#include <tuple>
#include <unordered_map>
#include <vector>
using namespace std::chrono_literals; // NOLINT

using autoware::behavior_path_planner::BehaviorModuleOutput;
Expand Down Expand Up @@ -94,7 +101,7 @@ void plot_footprint(
axes.plot(Args(xs, ys), Kwargs("color"_a = color, "linestyle"_a = "dotted"));
}

void plot_lanelet_polyogn(matplotlibcpp17::axes::Axes & axes, const lanelet::BasicPolygon2d polygon)
void plot_lanelet_polygon(matplotlibcpp17::axes::Axes & axes, const lanelet::BasicPolygon2d polygon)
{
std::vector<double> xs, ys;
for (const auto & p : polygon) {
Expand Down Expand Up @@ -353,7 +360,7 @@ std::vector<PullOverPath> selectPullOverPaths(
std::map<size_t, double> path_id_to_rough_margin_map;
const auto & target_objects = autoware_perception_msgs::msg::PredictedObjects{};
for (const size_t i : sorted_path_indices) {
const auto & path = pull_over_path_candidates[i];
const auto & path = pull_over_path_candidates[i]; // cppcheck-suppress containerOutOfBounds
const double distance =
autoware::behavior_path_planner::utils::path_safety_checker::calculateRoughDistanceToObjects(
path.parking_path(), target_objects, planner_data->parameters, false, "max");
Expand Down Expand Up @@ -657,7 +664,7 @@ int main(int argc, char ** argv)
const auto bus_stop_area_polygons =
getBusStopAreaPolygons(planner_data, goal_planner_parameter);
for (const auto & bus_stop_area_polygon : bus_stop_area_polygons) {
plot_lanelet_polyogn(ax1, bus_stop_area_polygon);
plot_lanelet_polygon(ax1, bus_stop_area_polygon);
}
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -20,9 +20,9 @@
<buildtool_depend>autoware_cmake</buildtool_depend>
<buildtool_depend>eigen3_cmake_module</buildtool_depend>

<depend>auotware_bezier_sampler</depend>
<depend>autoware_behavior_path_planner</depend>
<depend>autoware_behavior_path_planner_common</depend>
<depend>autoware_bezier_sampler</depend>
<depend>autoware_motion_utils</depend>
<depend>autoware_rtc_interface</depend>
<depend>autoware_test_utils</depend>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -23,8 +23,12 @@
#include <autoware_bezier_sampler/bezier_sampling.hpp>
#include <autoware_lanelet2_extension/utility/query.hpp>

#include <algorithm>
#include <iterator>

#include <limits>
#include <memory>
#include <utility>
#include <vector>
namespace autoware::behavior_path_planner
{
BezierPullOver::BezierPullOver(
Expand Down Expand Up @@ -223,7 +227,7 @@ std::vector<PullOverPath> BezierPullOver::generateBezierPath(
tf2::getYaw(from_pose.orientation)};
const autoware::sampler_common::State final{
{to_pose.position.x, to_pose.position.y}, {0.0, 0.0}, 0.0, tf2::getYaw(to_pose.orientation)};
// setting the initial velocity to higher gives straight forwared path (the steering does not
// setting the initial velocity to higher gives straight forward path (the steering does not
// change)
const auto bezier_path =
bezier_sampler::sample(initial, final, v_init_coeff, v_final_coeff, acc_coeff);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,7 @@ Bezier sample(
const autoware::sampler_common::State & initial, const autoware::sampler_common::State & final,
const double initial_velocity, const double final_velocity, const double acceleration)
{
double distance_initial_to_final = std::sqrt(
const double distance_initial_to_final = std::sqrt(
(initial.pose.x() - final.pose.x()) * (initial.pose.x() - final.pose.x()) +
(initial.pose.y() - final.pose.y()) * (initial.pose.y() - final.pose.y()));
// Tangent vectors
Expand All @@ -71,13 +71,13 @@ Bezier sample(
// Unit vectors
const Eigen::Vector2d initial_normal_unit = initial_tangent_unit.unitOrthogonal();
const Eigen::Vector2d final_normal_unit = final_tangent_unit.unitOrthogonal();
double initial_tangent_length = initial_velocity * distance_initial_to_final;
double final_tangent_length = final_velocity * distance_initial_to_final;
double acceleration_length = acceleration * distance_initial_to_final;
Eigen::Vector2d initial_acceleration =
const double initial_tangent_length = initial_velocity * distance_initial_to_final;
const double final_tangent_length = final_velocity * distance_initial_to_final;
const double acceleration_length = acceleration * distance_initial_to_final;
const Eigen::Vector2d initial_acceleration =
acceleration_length * initial_tangent_unit +
initial.curvature * initial_tangent_length * initial_tangent_length * initial_normal_unit;
Eigen::Vector2d final_acceleration =
const Eigen::Vector2d final_acceleration =
acceleration_length * final_tangent_unit +
final.curvature * final_tangent_length * final_tangent_length * final_normal_unit;
return generate(
Expand Down

0 comments on commit 1548a30

Please sign in to comment.