Apply changes from 07.09.2019 test session

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 <geometry_msgs::PoseStamped> poses(N);
+  std::vector <geometry_msgs::PoseStamped> 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;

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

mpc/src/mpc_node.cpp

@@ -32,6 +32,7 @@ int main(int argc, char** argv)
   ros::Publisher target_speed = nh.advertise<std_msgs::Float64>("target_speed", 1000);
   ros::Publisher steering_angle = nh.advertise<std_msgs::Float64>("steering_angle", 1000);
   ros::Publisher optimal_path = nh.advertise<nav_msgs::Path>("optimal_path", 1000);
+  ros::Publisher polynomial_path = nh.advertise<nav_msgs::Path>("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,18 +113,21 @@ 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);
 
     f1sim_cmd.publish(velocity_msg);
     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;
 }

selfie_launch/launch/control/mpc.launch

@@ -1,18 +1,19 @@
 <launch>
   <node pkg="mpc" type="mpc" name="mpc" output="screen">
     <param name="prediction_horizon" type="int" value="10" />
-    <param name="delta_time" type="double" value="0.2" />
+    <param name="delta_time" type="double" value="0.12" />
     <param name="loop_rate" type="int" value="10" />
     <param name="max_mod_delta" type="double" value="0.44" />
-    <param name="ref_v" type="double" value="0.8" />
+    <param name="ref_v" type="double" value="2.6" />
     <param name="cte_weight" type="int" value="85" />
     <param name="epsi_weight" type="int" value="40" />
     <param name="v_weight" type="int" value="150" />
     <param name="delta_weight" type="int" value="0" />
-    <param name="diff_delta_weight" type="int" value="40" />
+    <param name="diff_delta_weight" type="int" value="100" />
     <param name="diff_v_weight" type="int" value="10" />
-    <param name="max_v" type="double" value="1" />
-    <param name="min_v" type="double" value="-0.2"
+    <param name="max_v" type="double" value="3" />
+    <param name="min_v" type="double" value="-0.2" />
+    <param name="cornering_safety_weight" type="double" value="40" />
   </node>
 
   <node pkg="selfie_control" type="cmd_converter.py" name="cmd_converter" />

selfie_launch/launch/subsystems/lidars.launch

@@ -1,6 +1,6 @@
 <launch>
   <!-- RIGHT LASER -->
-  <node pkg="tf" type="static_transform_publisher" name="tf_base2rightlaser" args="0 -0.12 0  -1.57079632679 0 0  base_link right_laser 10"/>
+  <node pkg="tf" type="static_transform_publisher" name="tf_base2rightlaser" args="0 0.12 0  1.57079632679 0 0  base_link right_laser 10"/>
   <node name="hokuyo0" pkg="hokuyo_node" type="hokuyo_node">
        <param name="port" type="string" value="/dev/sensors/hokuyo_H1312790" />
   <param name="min_ang" type="double" value="-2.35619449019234492883" />
@@ -10,7 +10,7 @@
   </node>
 
   <!-- LEFT LASER -->
-  <node pkg="tf" type="static_transform_publisher" name="tf_base2leftlaser" args="-0.006 0.12 0  1.57079632679 0 0  base_link left_laser 10"/>
+  <node pkg="tf" type="static_transform_publisher" name="tf_base2leftlaser" args="-0.006 -0.12 0  -1.57079632679 0 0  base_link left_laser 10"/>
   <node name="hokuyo1" pkg="hokuyo_node" type="hokuyo_node">
        <param name="port" type="string" value="/dev/sensors/hokuyo_H1402546" />
        <param name="frame_id" value="left_laser" />