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

refactor(mpc): remove old library dependency #3165

Merged
merged 1 commit into from
Mar 27, 2023
+11 −15
Merged

refactor(mpc): remove old library dependency #3165

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
2 changes: 0 additions & 2 deletions control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,6 @@
#ifndef MPC_LATERAL_CONTROLLER__MPC_HPP_
#define MPC_LATERAL_CONTROLLER__MPC_HPP_

#include "geometry/common_2d.hpp"
#include "helper_functions/angle_utils.hpp"
#include "mpc_lateral_controller/interpolate.hpp"
#include "mpc_lateral_controller/lowpass_filter.hpp"
#include "mpc_lateral_controller/mpc_trajectory.hpp"
Expand Down
1 change: 0 additions & 1 deletion control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,6 @@
#define MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_

#include "eigen3/Eigen/Core"
#include "geometry/common_2d.hpp"
#include "helper_functions/angle_utils.hpp"
#include "interpolation/linear_interpolation.hpp"
#include "interpolation/spline_interpolation.hpp"
Expand Down
1 change: 0 additions & 1 deletion control/mpc_lateral_controller/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,6 @@
<build_depend>autoware_cmake</build_depend>

<depend>autoware_auto_control_msgs</depend>
<depend>autoware_auto_geometry</depend>
<depend>autoware_auto_planning_msgs</depend>
<depend>autoware_auto_vehicle_msgs</depend>
<depend>diagnostic_msgs</depend>
Expand Down
4 changes: 2 additions & 2 deletions control/mpc_lateral_controller/src/mpc.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -284,8 +284,8 @@ bool MPC::getData(
*m_steer_prediction_prev = data->predicted_steer;

/* check error limit */
const double dist_err = autoware::common::geometry::distance_2d<double>(
current_pose.position, data->nearest_pose.position);
const double dist_err =
tier4_autoware_utils::calcDistance2d(current_pose.position, data->nearest_pose.position);
if (dist_err > m_admissible_position_error) {
RCLCPP_WARN_SKIPFIRST_THROTTLE(
m_logger, *m_clock, duration, "position error is over limit. error = %fm, limit: %fm",
Expand Down
18 changes: 9 additions & 9 deletions control/mpc_lateral_controller/src/mpc_utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@ namespace autoware::motion::control::mpc_lateral_controller
{
namespace MPCUtils
{
using autoware::common::geometry::distance_2d;
using tier4_autoware_utils::calcDistance2d;

geometry_msgs::msg::Quaternion getQuaternionFromYaw(const double & yaw)
{
Expand Down Expand Up @@ -200,12 +200,12 @@ std::vector<double> calcTrajectoryCurvature(
p1.y = traj.y[prev_idx];
p2.y = traj.y[curr_idx];
p3.y = traj.y[next_idx];
const double den = std::max(
distance_2d<double>(p1, p2) * distance_2d<double>(p2, p3) * distance_2d<double>(p3, p1),
std::numeric_limits<double>::epsilon());
const double curvature =
2.0 * ((p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x)) / den;
curvature_vec.at(curr_idx) = curvature;
try {
curvature_vec.at(curr_idx) = tier4_autoware_utils::calcCurvature(p1, p2, p3);
} catch (...) {
std::cerr << "[MPC] 2 points are too close to calculate curvature." << std::endl;
curvature_vec.at(curr_idx) = 0.0;
}
}

/* first and last curvature is copied from next value */
Expand Down Expand Up @@ -380,7 +380,7 @@ double calcStopDistance(
for (int i = origin + 1; i < static_cast<int>(current_trajectory.points.size()) - 1; ++i) {
const auto & p0 = current_trajectory.points.at(static_cast<size_t>(i));
const auto & p1 = current_trajectory.points.at(static_cast<size_t>(i - 1));
stop_dist += distance_2d<double>(p0, p1);
stop_dist += calcDistance2d(p0, p1);
if (std::fabs(p0.longitudinal_velocity_mps) < zero_velocity) {
break;
}
Expand All @@ -395,7 +395,7 @@ double calcStopDistance(
if (std::fabs(p0.longitudinal_velocity_mps) > zero_velocity) {
break;
}
stop_dist -= distance_2d<double>(p0, p1);
stop_dist -= calcDistance2d(p0, p1);
}
return stop_dist;
}
Expand Down