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

[pull] main from autowarefoundation:main #142

Merged
merged 9 commits into from
Mar 11, 2023
Merged

[pull] main from autowarefoundation:main #142

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
2ace0d2
fix(obstacle_avoidance_planner): remove the option march=native (#3009)
takayuki5168 Mar 10, 2023
fa9b018
feat(diagnostic_converter): add converter to use planning_evaluator's…
kyoichi-sugahara Mar 10, 2023
d8c826c
fix(avoidance): fix unexpected behavior in yield scenario (#3033)
satoshi-ota Mar 10, 2023
def79f3
fix(mpc): optimization failure due to steer limit (#3044)
TakaHoribe Mar 10, 2023
1b35a6f
refactor(mpc): clean up code (#3046)
TakaHoribe Mar 10, 2023
e4bcc86
feat: merge traffic signals from perception and V2X (#2903)
isamu-takagi Mar 10, 2023
85821ea
fix(tier4_planning_rviz_plugin): supress initial warning message (#2960)
takayuki5168 Mar 10, 2023
e92be64
refactor(pull_over): move parameter declare pull over (#3048)
satoshi-ota Mar 10, 2023
0056a2a
fix(lane_change): use previous module output (#3042)
satoshi-ota Mar 11, 2023
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
35 changes: 15 additions & 20 deletions common/tier4_planning_rviz_plugin/include/path/display.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -84,19 +84,14 @@ class AutowarePathWithDrivableAreaDisplay : public AutowarePathBaseDisplay<T>
{
public:
AutowarePathWithDrivableAreaDisplay()
: property_drivable_area_view_{"View Drivable Area", true, "", this},
property_drivable_area_alpha_{"Alpha", 0.999, "", &property_drivable_area_view_},
property_drivable_area_color_{"Color", QColor(0, 148, 205), "", &property_drivable_area_view_},
property_drivable_area_width_{"Width", 0.3f, "", &property_drivable_area_view_}
{
property_drivable_area_view_ = new rviz_common::properties::BoolProperty(
"View Drivable Area", true, "", this, SLOT(updateVisualization()));
property_drivable_area_alpha_ = new rviz_common::properties::FloatProperty(
"Alpha", 0.999, "", property_drivable_area_view_, SLOT(updateVisualization()), this);
property_drivable_area_alpha_->setMin(0.0);
property_drivable_area_alpha_->setMax(1.0);
property_drivable_area_color_ = new rviz_common::properties::ColorProperty(
"Color", QColor(0, 148, 205), "", property_drivable_area_view_, SLOT(updateVisualization()),
this);
property_drivable_area_width_ = new rviz_common::properties::FloatProperty(
"Width", 0.3f, "", property_drivable_area_view_, SLOT(updateVisualization()), this);
property_drivable_area_width_->setMin(0.001);
property_drivable_area_alpha_.setMin(0.0);
property_drivable_area_alpha_.setMax(1.0);
property_drivable_area_width_.setMin(0.001);
}

~AutowarePathWithDrivableAreaDisplay()
Expand Down Expand Up @@ -137,11 +132,11 @@ class AutowarePathWithDrivableAreaDisplay : public AutowarePathBaseDisplay<T>
return;
}

if (property_drivable_area_view_->getBool()) {
if (property_drivable_area_view_.getBool()) {
Ogre::ColourValue color =
rviz_common::properties::qtToOgre(property_drivable_area_color_->getColor());
color.a = property_drivable_area_alpha_->getFloat();
const auto line_width = property_drivable_area_width_->getFloat();
rviz_common::properties::qtToOgre(property_drivable_area_color_.getColor());
color.a = property_drivable_area_alpha_.getFloat();
const auto line_width = property_drivable_area_width_.getFloat();
visualizeBound(msg_ptr->left_bound, color, line_width, left_bound_object_);
visualizeBound(msg_ptr->right_bound, color, line_width, right_bound_object_);
}
Expand All @@ -151,10 +146,10 @@ class AutowarePathWithDrivableAreaDisplay : public AutowarePathBaseDisplay<T>
Ogre::ManualObject * left_bound_object_{nullptr};
Ogre::ManualObject * right_bound_object_{nullptr};

rviz_common::properties::BoolProperty * property_drivable_area_view_;
rviz_common::properties::ColorProperty * property_drivable_area_color_;
rviz_common::properties::FloatProperty * property_drivable_area_alpha_;
rviz_common::properties::FloatProperty * property_drivable_area_width_;
rviz_common::properties::BoolProperty property_drivable_area_view_;
rviz_common::properties::FloatProperty property_drivable_area_alpha_;
rviz_common::properties::ColorProperty property_drivable_area_color_;
rviz_common::properties::FloatProperty property_drivable_area_width_;
};

class AutowarePathWithLaneIdDisplay
Expand Down
67 changes: 33 additions & 34 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 @@ -46,6 +46,12 @@
namespace autoware::motion::control::mpc_lateral_controller
{

using autoware_auto_control_msgs::msg::AckermannLateralCommand;
using autoware_auto_planning_msgs::msg::Trajectory;
using autoware_auto_vehicle_msgs::msg::SteeringReport;
using geometry_msgs::msg::Pose;
using tier4_debug_msgs::msg::Float32MultiArrayStamped;

struct MPCParam
{
//!< @brief prediction horizon step
Expand Down Expand Up @@ -112,7 +118,7 @@ struct MPCData
{
int nearest_idx;
double nearest_time;
geometry_msgs::msg::Pose nearest_pose;
Pose nearest_pose;
double steer;
double predicted_steer;
double lateral_err;
Expand Down Expand Up @@ -147,15 +153,15 @@ class MPC
//!< @brief vehicle model type for MPC
std::string m_vehicle_model_type;
//!< @brief vehicle model for MPC
std::shared_ptr<mpc_lateral_controller::VehicleModelInterface> m_vehicle_model_ptr;
std::shared_ptr<VehicleModelInterface> m_vehicle_model_ptr;
//!< @brief qp solver for MPC
std::shared_ptr<mpc_lateral_controller::QPSolverInterface> m_qpsolver_ptr;
std::shared_ptr<QPSolverInterface> m_qpsolver_ptr;
//!< @brief lowpass filter for steering command
mpc_lateral_controller::Butterworth2dFilter m_lpf_steering_cmd;
Butterworth2dFilter m_lpf_steering_cmd;
//!< @brief lowpass filter for lateral error
mpc_lateral_controller::Butterworth2dFilter m_lpf_lateral_error;
Butterworth2dFilter m_lpf_lateral_error;
//!< @brief lowpass filter for heading error
mpc_lateral_controller::Butterworth2dFilter m_lpf_yaw_error;
Butterworth2dFilter m_lpf_yaw_error;

//!< @brief raw output computed two iterations ago
double m_raw_steer_cmd_pprev = 0.0;
Expand All @@ -170,17 +176,16 @@ class MPC
//!< @brief shift is forward or not.
bool m_is_forward_shift = true;
//!< @brief buffer of sent command
std::vector<autoware_auto_control_msgs::msg::AckermannLateralCommand> m_ctrl_cmd_vec;
std::vector<AckermannLateralCommand> m_ctrl_cmd_vec;
//!< @brief minimum prediction distance
double m_min_prediction_length = 5.0;

/**
* @brief get variables for mpc calculation
*/
bool getData(
const mpc_lateral_controller::MPCTrajectory & traj,
const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer,
const geometry_msgs::msg::Pose & current_pose, MPCData * data);
const MPCTrajectory & traj, const SteeringReport & current_steer, const Pose & current_pose,
MPCData * data);
/**
* @brief calculate predicted steering
*/
Expand All @@ -197,7 +202,7 @@ class MPC
/**
* @brief reset previous result of MPC
*/
void resetPrevResult(const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer);
void resetPrevResult(const SteeringReport & current_steer);
/**
* @brief set initial condition for mpc
* @param [in] data mpc data
Expand All @@ -210,14 +215,13 @@ class MPC
* @param [out] x updated state at delayed_time
*/
bool updateStateForDelayCompensation(
const mpc_lateral_controller::MPCTrajectory & traj, const double & start_time,
Eigen::VectorXd * x);
const MPCTrajectory & traj, const double & start_time, Eigen::VectorXd * x);
/**
* @brief generate MPC matrix with trajectory and vehicle model
* @param [in] reference_trajectory used for linearization around reference trajectory
*/
MPCMatrix generateMPCMatrix(
const mpc_lateral_controller::MPCTrajectory & reference_trajectory, const double prediction_dt);
const MPCTrajectory & reference_trajectory, const double prediction_dt);
/**
* @brief generate MPC matrix with trajectory and vehicle model
* @param [in] mpc_matrix parameters matrix to use for optimization
Expand All @@ -232,22 +236,19 @@ class MPC
* @brief resample trajectory with mpc resampling time
*/
bool resampleMPCTrajectoryByTime(
const double start_time, const double prediction_dt,
const mpc_lateral_controller::MPCTrajectory & input,
mpc_lateral_controller::MPCTrajectory * output) const;
const double start_time, const double prediction_dt, const MPCTrajectory & input,
MPCTrajectory * output) const;
/**
* @brief apply velocity dynamics filter with v0 from closest index
*/
mpc_lateral_controller::MPCTrajectory applyVelocityDynamicsFilter(
const mpc_lateral_controller::MPCTrajectory & trajectory,
const geometry_msgs::msg::Pose & current_pose, const double v0) const;
MPCTrajectory applyVelocityDynamicsFilter(
const MPCTrajectory & trajectory, const Pose & current_pose, const double v0) const;
/**
* @brief get prediction delta time of mpc.
* If trajectory length is shorter than min_prediction length, adjust delta time.
*/
double getPredictionDeltaTime(
const double start_time, const mpc_lateral_controller::MPCTrajectory & input,
const geometry_msgs::msg::Pose & current_pose) const;
const double start_time, const MPCTrajectory & input, const Pose & current_pose) const;
/**
* @brief add weights related to lateral_jerk, steering_rate, steering_acc into R
*/
Expand Down Expand Up @@ -342,7 +343,7 @@ class MPC

public:
//!< @brief reference trajectory to be followed
mpc_lateral_controller::MPCTrajectory m_ref_traj;
MPCTrajectory m_ref_traj;
//!< @brief MPC design parameter
MPCParam m_param;
//!< @brief mpc_output buffer for delay time compensation
Expand Down Expand Up @@ -381,24 +382,22 @@ class MPC
* @param [out] diagnostic diagnostic msg to be filled-out
*/
bool calculateMPC(
const autoware_auto_vehicle_msgs::msg::SteeringReport & current_steer,
const double current_velocity, const geometry_msgs::msg::Pose & current_pose,
autoware_auto_control_msgs::msg::AckermannLateralCommand & ctrl_cmd,
autoware_auto_planning_msgs::msg::Trajectory & predicted_traj,
tier4_debug_msgs::msg::Float32MultiArrayStamped & diagnostic);
const SteeringReport & current_steer, const double current_velocity, const Pose & current_pose,
AckermannLateralCommand & ctrl_cmd, Trajectory & predicted_traj,
Float32MultiArrayStamped & diagnostic);
/**
* @brief set the reference trajectory to follow
*/
void setReferenceTrajectory(
const autoware_auto_planning_msgs::msg::Trajectory & trajectory_msg,
const double traj_resample_dist, const bool enable_path_smoothing,
const int path_filter_moving_ave_num, const int curvature_smoothing_num_traj,
const int curvature_smoothing_num_ref_steer, const bool extend_trajectory_for_end_yaw_control);
const Trajectory & trajectory_msg, const double traj_resample_dist,
const bool enable_path_smoothing, const int path_filter_moving_ave_num,
const int curvature_smoothing_num_traj, const int curvature_smoothing_num_ref_steer,
const bool extend_trajectory_for_end_yaw_control);
/**
* @brief set the vehicle model of this MPC
*/
inline void setVehicleModel(
std::shared_ptr<mpc_lateral_controller::VehicleModelInterface> vehicle_model_ptr,
std::shared_ptr<VehicleModelInterface> vehicle_model_ptr,
const std::string & vehicle_model_type)
{
m_vehicle_model_ptr = vehicle_model_ptr;
Expand All @@ -407,7 +406,7 @@ class MPC
/**
* @brief set the QP solver of this MPC
*/
inline void setQPSolver(std::shared_ptr<mpc_lateral_controller::QPSolverInterface> qpsolver_ptr)
inline void setQPSolver(std::shared_ptr<QPSolverInterface> qpsolver_ptr)
{
m_qpsolver_ptr = qpsolver_ptr;
}
Expand Down
2 changes: 1 addition & 1 deletion control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -107,7 +107,7 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase
std::deque<autoware_auto_planning_msgs::msg::Trajectory> m_trajectory_buffer;

// MPC object
mpc_lateral_controller::MPC m_mpc;
MPC m_mpc;

//!< @brief measured kinematic state
nav_msgs::msg::Odometry m_current_kinematic_state;
Expand Down
Loading