diff --git a/mpc_ros/cfg/MPCPlanner.cfg b/mpc_ros/cfg/MPCPlanner.cfg index 5450915..fee17e9 100644 --- a/mpc_ros/cfg/MPCPlanner.cfg +++ b/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")) \ No newline at end of file diff --git a/mpc_ros/include/mpc_plannner_ros.h b/mpc_ros/include/mpc_plannner_ros.h index 71a6aca..3e3e76b 100644 --- a/mpc_ros/include/mpc_plannner_ros.h +++ b/mpc_ros/include/mpc_plannner_ros.h @@ -79,7 +79,6 @@ namespace mpc_ros{ void publishLocalPlan(std::vector& path); void publishGlobalPlan(std::vector& path); - void LoadParams(const std::map ¶ms); // 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 footprint_spec_; std::vector 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 *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 Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs); + //vector Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs); vector mpc_x; vector mpc_y; vector 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 _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); diff --git a/mpc_ros/params/mpc_last_params.yaml b/mpc_ros/params/mpc_last_params.yaml index ba8f3e4..3f02134 100644 --- a/mpc_ros/params/mpc_last_params.yaml +++ b/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 diff --git a/mpc_ros/params/total_rviz_navigation.rviz b/mpc_ros/params/total_rviz_navigation.rviz index 8305a9f..1f49b15 100644 --- a/mpc_ros/params/total_rviz_navigation.rviz +++ b/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 diff --git a/mpc_ros/src/mpc_plannner_ros.cpp b/mpc_ros/src/mpc_plannner_ros.cpp index 43f02f6..29917bb 100644 --- a/mpc_ros/src/mpc_plannner_ros.cpp +++ b/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("global_path_topic", _globalPath_topic, "/move_base/TrajectoryPlannerROS/global_plan" ); - pn.param("goal_topic", _goal_topic, "/move_base_simple/goal" ); pn.param("map_frame", _map_frame, "map" ); //*****for mpc, "odom" pn.param("odom_frame", _odom_frame, "odom"); - pn.param("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("/mpc_trajectory", 1);// MPC trajectory output _pub_odompath = _nh.advertise("/mpc_reference", 1); // reference path for MPC ///mpc_reference - if(_pub_twist_flag) - _pub_twist = _nh.advertise("/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) {