diff --git a/mpc/src/mpc.cpp b/mpc/src/mpc.cpp index e270208..9a05c68 100644 --- a/mpc/src/mpc.cpp +++ b/mpc/src/mpc.cpp @@ -51,6 +51,7 @@ Controls MPC::getControls(Eigen::VectorXd pathCoeffs, const VectorXd &state) assert(N > 0); //Return predicted path std::vector poses(N); + std::vector polynomial_poses(N); for (int i = 0; i < N; i++) { @@ -60,6 +61,21 @@ Controls MPC::getControls(Eigen::VectorXd pathCoeffs, const VectorXd &state) poses[i].pose.position.y = solution[2 * i + 3]; } + double x = 0.1; + + for (int i = 0; i < N; i++) + { + polynomial_poses[i].header.stamp = ros::Time::now(); + polynomial_poses[i].header.frame_id = "base_link"; + x += x; + polynomial_poses[i].pose.position.x = x; + polynomial_poses[i].pose.position.y = pathCoeffs[0] + pathCoeffs[1] * x + pathCoeffs[2]* x * x; + } + + std::swap(ret.polynomial_path.poses, polynomial_poses); + ret.polynomial_path.header.frame_id = "base_link"; + ret.polynomial_path.header.stamp = ros::Time::now(); + std::swap(ret.predicted_path.poses, poses); ret.predicted_path.header.frame_id = "base_link"; ret.predicted_path.header.stamp = ros::Time::now(); @@ -235,6 +251,12 @@ void FG_eval::operator()(ADvector& fg, const ADvector& vars) fg[0] += params.diff_v_weight * CppAD::pow((vars[v_start + t + 1] - vars[v_start + t])/params.delta_time, 2); } + // Make cornering safer + for (unsigned int t = 0; t < N - 1; ++t) + { + fg[0] += params.cornering_safety_weight * CppAD::pow(vars[v_start + t] * vars[v_start + t] * vars[delta_start + t], 2); + } + //Optimizer constraints - g(x) fg[1 + x_start] = 0; fg[1 + y_start] = 0; diff --git a/mpc/src/mpc.h b/mpc/src/mpc.h index f2f5d1f..9942c39 100644 --- a/mpc/src/mpc.h +++ b/mpc/src/mpc.h @@ -42,6 +42,7 @@ struct Params double ref_v; double max_v; double min_v; + double cornering_safety_weight; }; // Struct that is returned by MPC @@ -50,6 +51,7 @@ struct Controls double velocity; double delta; nav_msgs::Path predicted_path; + nav_msgs::Path polynomial_path; }; //Interface for interactions with ros diff --git a/mpc/src/mpc_node.cpp b/mpc/src/mpc_node.cpp index a9cd6e8..0b9ab97 100644 --- a/mpc/src/mpc_node.cpp +++ b/mpc/src/mpc_node.cpp @@ -32,6 +32,7 @@ int main(int argc, char** argv) ros::Publisher target_speed = nh.advertise("target_speed", 1000); ros::Publisher steering_angle = nh.advertise("steering_angle", 1000); ros::Publisher optimal_path = nh.advertise("optimal_path", 1000); + ros::Publisher polynomial_path = nh.advertise("polynomial_path", 1000); ros::Subscriber speed_sub = nh.subscribe("speed", 1000, speedCallback); ros::Subscriber closest_path_points = nh.subscribe("closest_path_points", 1000, pathCallback); @@ -58,6 +59,7 @@ int main(int argc, char** argv) pnh.param("ref_v", p.ref_v, 4.0); pnh.param("max_v", p.max_v, 0.5); pnh.param("min_v", p.min_v, -0.1); + pnh.param("cornering_safety_weight", p.cornering_safety_weight, 1.0); MPC mpc(p); @@ -111,11 +113,13 @@ int main(int argc, char** argv) std_msgs::Float64 target_speed_msg; std_msgs::Float64 steering_angle_msg; nav_msgs::Path optimal_path_msg; + nav_msgs::Path polynomial_path_msg; target_speed_msg.data = controls.velocity; //target_speed_msg.data = min(max_vel, max(-1*max_vel, target_speed_msg.data)); steering_angle_msg.data = controls.delta; optimal_path_msg = controls.predicted_path; + polynomial_path_msg = controls.polynomial_path; geometry_msgs::Twist velocity_msg = getTwist(target_speed_msg.data, steering_angle_msg.data, yaw); @@ -123,6 +127,7 @@ int main(int argc, char** argv) target_speed.publish(target_speed_msg); steering_angle.publish(steering_angle_msg); optimal_path.publish(optimal_path_msg); + polynomial_path.publish(polynomial_path_msg); ros::spinOnce(); rate.sleep(); @@ -157,20 +162,38 @@ VectorXd polyfit(const VectorXd &xvals, const VectorXd &yvals, int order) assert(xvals.size() == yvals.size()); assert(order >= 1 && order <= xvals.size() - 1); - Eigen::MatrixXd A(xvals.size(), order + 1); + double x_max = xvals[0]; + int valid_points = 1; + for(int i = 1; i < xvals.size(); ++i) + { + if(xvals[i] > xvals[i - 1]) + { + ++valid_points; + x_max = xvals[i]; + } + } + - for (int i = 0; i < xvals.size(); ++i) { + Eigen::MatrixXd A(valid_points, order + 1); + + for (int i = 0; i < valid_points; ++i) { A(i, 0) = 1.0; } - for (int j = 0; j < xvals.size(); ++j) { + for (int j = 0; j < valid_points; ++j) { for (int i = 0; i < order; ++i) { A(j, i + 1) = A(j, i) * xvals(j); } } + VectorXd yvals1(valid_points); + for(int i = 0; i < valid_points; ++i) + { + yvals1[i] = yvals[i]; + } + auto Q = A.householderQr(); - auto result = Q.solve(yvals); + auto result = Q.solve(yvals1); return result; } diff --git a/selfie_launch/launch/control/mpc.launch b/selfie_launch/launch/control/mpc.launch index 0fa54c8..b05cbe9 100644 --- a/selfie_launch/launch/control/mpc.launch +++ b/selfie_launch/launch/control/mpc.launch @@ -1,18 +1,19 @@ - + - + - + - - + + diff --git a/selfie_launch/launch/subsystems/lidars.launch b/selfie_launch/launch/subsystems/lidars.launch index a9cae3e..5be4ef8 100644 --- a/selfie_launch/launch/subsystems/lidars.launch +++ b/selfie_launch/launch/subsystems/lidars.launch @@ -1,6 +1,6 @@ - + @@ -10,7 +10,7 @@ - +