updated the mpc params to the cfg

mpc_ros/cfg/MPCPlanner.cfg

@@ -10,5 +10,29 @@ gen = ParameterGenerator()
 # that concern all localplanners
 add_generic_localplanner_params(gen)
 
+gen.add("debug_info", bool_t, 0, "Debug information", False)
+gen.add("delay_mode", bool_t, 0, "Delay mode", True)
+gen.add("max_speed", double_t, 0, "Maximum speed [m/s]", 0.50, 0.01, 5.0)
+gen.add("waypoints_dist", double_t, 0, "Waypoint distance [m]", -1, -1.0, 10.0)
+gen.add("path_length", double_t, 0, "Path length [m]", 5.0, 0.0, 10.0)
+gen.add("controller_freq", double_t, 0, "Controller Frequency", 10.0, 0.01, 100.0)
+
+gen.add("steps", double_t, 0, "Steps", 20.0, 0.0, 100.0)
+gen.add("ref_cte", double_t, 0, "Reference Cross Track Error", 0.0, 0.0, 10.0)
+gen.add("ref_vel", double_t, 0, "Reference Velocity", 1.0, 0.0, 5.0)
+gen.add("ref_etheta", double_t, 0, "Reference Error theta", 0.0, 0.01, 5.0)
+
+gen.add("w_cte", double_t, 0, "Weight of Cross Track Error", 1000.0, 0.01, 1000.0)
+gen.add("w_etheta", double_t, 0, "Weight of Error Theta", 1000.0, 0.01, 1000.0)
+gen.add("w_vel", double_t, 0, "Weight of Velocity", 100.0, 0.01, 1000.0)
+gen.add("w_angvel", double_t, 0, "Weight of Angular velocity", 100.0, 0.01, 1000.0)
+gen.add("w_angvel_d", double_t, 0, "Weight of Angular velocity difference", 0.0, 0.01, 1000.0)
+gen.add("w_accel", double_t, 0,"Weight of Acceleration", 50.0, 0.01, 1000.0)
+gen.add("w_accel_d", double_t, 0,"Weight of Acceleration difference", 10.0, 0.01, 1000.0)
+
+gen.add("max_angvel", double_t, 0, "Maximum Angular velocity", 1.0, 0.01, 5.0)
+gen.add("max_throttle", double_t, 0, "Maximum Throttle", 1.0, 0.01, 5.0)
+gen.add("bound_value", double_t, 0, "Bound value", 1000.0, 0.01, 1000.0)
+
 
 exit(gen.generate("mpc_ros", "mpc_ros", "MPCPlanner"))

mpc_ros/include/mpc_plannner_ros.h

@@ -79,7 +79,6 @@ namespace mpc_ros{
             void publishLocalPlan(std::vector<geometry_msgs::PoseStamped>& path);
             void publishGlobalPlan(std::vector<geometry_msgs::PoseStamped>& path);
 
-
             void LoadParams(const std::map<string, double> &params);
 
             // Local planner plugin functions
@@ -115,14 +114,12 @@ namespace mpc_ros{
             std::string robot_base_frame_; ///< @brief Used as the base frame id of the robot
             std::vector<geometry_msgs::Point> footprint_spec_;
             std::vector<geometry_msgs::PoseStamped> global_plan_;
-
 
             base_local_planner::LocalPlannerUtil planner_util_;
             base_local_planner::LatchedStopRotateController latchedStopRotateController_;
             base_local_planner::OdometryHelperRos odom_helper_;
             geometry_msgs::PoseStamped current_pose_;
 
-            double forward_point_distance_;
             base_local_planner::SimpleTrajectoryGenerator generator_;
             base_local_planner::SimpleScoredSamplingPlanner scored_sampling_planner_;
             dynamic_reconfigure::Server<MPCPlannerConfig> *dsrv_;
@@ -139,48 +136,28 @@ namespace mpc_ros{
 
             // Flags
             std::string odom_topic_;
-            bool reached_goal_;
             bool initialized_;
 
-
-        public:
-            int get_thread_numbers();
         private:
 
             // Solve the model given an initial state and polynomial coefficients.
             // Return the first actuatotions.
-            vector<double> Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs);
+            //vector<double> Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs);
             vector<double> mpc_x;
             vector<double> mpc_y;
             vector<double> mpc_theta;
 
             ros::NodeHandle _nh;
             ros::Subscriber _sub_odom;
-            ros::Publisher _pub_globalpath,_pub_odompath, _pub_twist, _pub_mpctraj;
-            ros::Timer _timer1;
+            ros::Publisher _pub_odompath, _pub_mpctraj;
             tf2_ros::Buffer *tf_;  ///
-            ros::Time tracking_stime;
-            ros::Time tracking_etime;
-            ros::Time tracking_time;
-            int tracking_time_sec;
-            int tracking_time_nsec;
 
-
-            std::ofstream file;
-            unsigned int idx = 0;
-
-            //time flag
-            bool start_timef = false;
-            bool end_timef = false;
-
-            geometry_msgs::Point _goal_pos;
             nav_msgs::Odometry _odom;
             nav_msgs::Path _odom_path, _mpc_traj; 
             //ackermann_msgs::AckermannDriveStamped _ackermann_msg;
             geometry_msgs::Twist _twist_msg;
 
-            string _globalPath_topic, _goal_topic;
-            string _map_frame, _odom_frame, _car_frame;
+            string _map_frame, _odom_frame;
 
             MPC _mpc;
             map<string, double> _mpc_params;
@@ -190,8 +167,8 @@ namespace mpc_ros{
             //double _Lf; 
             double _dt, _w, _throttle, _speed, _max_speed;
             double _pathLength, _goalRadius, _waypointsDist;
-            int _controller_freq, _downSampling, _thread_numbers;
-            bool _goal_received, _goal_reached, _path_computed, _pub_twist_flag, _debug_info, _delay_mode;
+            int _controller_freq, _downSampling;
+            bool _debug_info, _delay_mode;
             double polyeval(Eigen::VectorXd coeffs, double x);
             Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order);
 

mpc_ros/params/mpc_last_params.yaml

@@ -1,13 +1,11 @@
 
 
 # Parameters for control loop
-pub_twist_cmd: true
 debug_info: false
 delay_mode: true
 max_speed: 0.5 # unit: m/s #0.8
 waypoints_dist: -1.0 # unit: m, set < 0 means computed by node
 path_length: 5.0 # unit: m
-goal_radius: 0.5 # unit: m
 controller_freq: 10
 
 # Parameter for MPC solver

mpc_ros/params/total_rviz_navigation.rviz

@@ -7,8 +7,9 @@ Panels:
         - /Global Options1
         - /TF1/Frames1
         - /Local Planner1
+        - /Local Planner1/MPC_Reference1
       Splitter Ratio: 0.43225806951522827
-    Tree Height: 723
+    Tree Height: 715
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -399,7 +400,7 @@ Visualization Manager:
           Head Length: 0.20000000298023224
           Length: 0.30000001192092896
           Line Style: Billboards
-          Line Width: 0.30000001192092896
+          Line Width: 0.10000000149011612
           Name: MPC_Reference
           Offset:
             X: 0
@@ -569,16 +570,16 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 29.701488494873047
+      Distance: 22.66962242126465
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 1.7857226133346558
-        Y: -6.595728874206543
-        Z: -6.072860240936279
+        X: 1.6164482831954956
+        Y: -4.018435001373291
+        Z: -5.246603488922119
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
@@ -592,10 +593,10 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1023
+  Height: 1015
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd0000000400000000000001d300000361fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000049200000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000361000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004380000003efc0100000002fb0000000800540069006d0065010000000000000438000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000025f0000036100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001d300000359fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed0000049200000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000359000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004380000003efc0100000002fb0000000800540069006d0065010000000000000438000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000025f0000035900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -605,5 +606,5 @@ Window Geometry:
   Views:
     collapsed: true
   Width: 1080
-  X: 2565
-  Y: 369
+  X: 3848
+  Y: 426

mpc_ros/src/mpc_plannner_ros.cpp

@@ -58,14 +58,11 @@ namespace mpc_ros{
         ros::NodeHandle pn("~");
 
         //Parameters for control loop
-        pn.param("thread_numbers", _thread_numbers, 2); // number of threads for this ROS node
-        pn.param("pub_twist_cmd", _pub_twist_flag, true);
         pn.param("debug_info", _debug_info, true);
         pn.param("delay_mode", _delay_mode, true);
         pn.param("max_speed", _max_speed, 0.50); // unit: m/s
         pn.param("waypoints_dist", _waypointsDist, -1.0); // unit: m
         pn.param("path_length", _pathLength, 8.0); // unit: m
-        pn.param("goal_radius", _goalRadius, 0.5); // unit: m
         pn.param("controller_freq", _controller_freq, 10);
         //pn.param("vehicle_Lf", _Lf, 0.290); // distance between the front of the vehicle and its center of gravity
         _dt = double(1.0/_controller_freq); // time step duration dt in s 
@@ -87,15 +84,11 @@ namespace mpc_ros{
         pn.param("mpc_bound_value", _bound_value, 1.0e3); // Bound value for other variables
 
         //Parameter for topics & Frame name
-        pn.param<std::string>("global_path_topic", _globalPath_topic, "/move_base/TrajectoryPlannerROS/global_plan" );
-        pn.param<std::string>("goal_topic", _goal_topic, "/move_base_simple/goal" );
         pn.param<std::string>("map_frame", _map_frame, "map" ); //*****for mpc, "odom"
         pn.param<std::string>("odom_frame", _odom_frame, "odom");
-        pn.param<std::string>("car_frame", _car_frame, "base_footprint" );
 
         //Display the parameters
         cout << "\n===== Parameters =====" << endl;
-        cout << "pub_twist_cmd: "  << _pub_twist_flag << endl;
         cout << "debug_info: "  << _debug_info << endl;
         cout << "delay_mode: "  << _delay_mode << endl;
         //cout << "vehicle_Lf: "  << _Lf << endl;
@@ -115,13 +108,8 @@ namespace mpc_ros{
         _pub_mpctraj   = _nh.advertise<nav_msgs::Path>("/mpc_trajectory", 1);// MPC trajectory output
         _pub_odompath  = _nh.advertise<nav_msgs::Path>("/mpc_reference", 1); // reference path for MPC ///mpc_reference 
 
-        if(_pub_twist_flag)
-            _pub_twist = _nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1); //for stage (Ackermann msg non-supported)
-
 
         //Init variables
-        _goal_reached  = false;
-        _path_computed = false;
         _throttle = 0.0; 
         _w = 0.0;
         _speed = 0.0;
@@ -177,6 +165,30 @@ namespace mpc_ros{
       limits.prune_plan = config.prune_plan;
       limits.trans_stopped_vel = config.trans_stopped_vel;
       limits.theta_stopped_vel = config.theta_stopped_vel;
+
+      //Parameter for MPC solver
+      _debug_info = config.debug_info;
+      _delay_mode = config.delay_mode;
+      _max_speed = config.max_speed;
+      _waypointsDist = config.waypoints_dist;
+      _pathLength = config.path_length;
+      _controller_freq = config.controller_freq;
+      _mpc_steps = config.steps;
+      _ref_cte = config.ref_cte;
+      _ref_vel = config.ref_vel;
+      _ref_etheta = config.ref_etheta;
+      _w_cte = config.w_cte;
+      _w_etheta = config.w_etheta;
+      _w_vel = config.w_vel;
+      _w_angvel = config.w_angvel;
+      _w_angvel_d = config.w_angvel_d;
+      _w_accel_d = config.w_accel_d;
+      _w_accel = config.w_accel;
+      _max_angvel = config.max_angvel;
+      _max_throttle = config.max_throttle;
+      _bound_value = config.bound_value;
+
+
       planner_util_.reconfigureCB(limits, false);
 
   }
@@ -196,7 +208,6 @@ namespace mpc_ros{
         }
 
         latchedStopRotateController_.resetLatching();
-        reached_goal_ = false;
         planner_util_.setPlan(orig_global_plan);
 
     }
@@ -683,12 +694,6 @@ namespace mpc_ros{
         }
     }
 
-    // Public: return _thread_numbers
-    int MPCPlannerROS::get_thread_numbers()
-    {
-        return _thread_numbers;
-    }
-
     // Evaluate a polynomial.
     double MPCPlannerROS::polyeval(Eigen::VectorXd coeffs, double x) 
     {