Dynamic parameter server callback function
| 47 | |
| 48 | // Dynamic parameter server callback function |
| 49 | void dynamicParamCallback(fiss_planner::fiss_plannerConfig& config, uint32_t level) |
| 50 | { |
| 51 | // General Settings |
| 52 | CHECK_COLLISION = config.check_collision; |
| 53 | USE_ASYNC = config.use_async; |
| 54 | USE_HEURISTIC = config.use_heuristic; |
| 55 | SETTINGS.tick_t = config.tick_t; |
| 56 | |
| 57 | // Sampling parameters (lateral) |
| 58 | LANE_WIDTH = config.curr_lane_width; |
| 59 | LEFT_LANE_WIDTH = config.left_lane_width; |
| 60 | RIGHT_LANE_WIDTH = config.right_lane_width; |
| 61 | SETTINGS.center_offset = config.center_offset; |
| 62 | SETTINGS.num_width = config.num_width; |
| 63 | // Sampling parameters (longitudinal) |
| 64 | SETTINGS.max_t = config.max_t; |
| 65 | SETTINGS.min_t = config.min_t; |
| 66 | SETTINGS.num_t = config.num_t; |
| 67 | |
| 68 | SETTINGS.highest_speed = kph2mps(config.highest_speed); |
| 69 | SETTINGS.lowest_speed = kph2mps(config.lowest_speed); |
| 70 | SETTINGS.num_speed = config.num_speed; |
| 71 | // Constraints |
| 72 | // SETTINGS.max_speed = Vehicle::max_speed(); |
| 73 | // SETTINGS.max_accel = Vehicle::max_acceleration(); |
| 74 | // SETTINGS.max_decel = Vehicle::max_deceleration(); |
| 75 | // SETTINGS.max_curvature = Vehicle::max_curvature_front(); |
| 76 | // SETTINGS.steering_angle_rate = Vehicle::max_steering_rate(); |
| 77 | SETTINGS.max_speed = kph2mps(config.max_speed); |
| 78 | SETTINGS.max_accel = config.max_acceleration; |
| 79 | SETTINGS.max_decel = -config.max_deceleration; |
| 80 | SETTINGS.max_curvature = config.max_curvature; |
| 81 | SETTINGS.max_jerk_s = config.max_jerk_lon; |
| 82 | SETTINGS.max_jerk_d = config.max_jerk_lat; |
| 83 | // Cost Weights |
| 84 | SETTINGS.k_heuristic = config.k_heuristic; |
| 85 | SETTINGS.k_diff = config.k_diff; |
| 86 | SETTINGS.k_time = config.k_time; |
| 87 | SETTINGS.k_jerk = config.k_jerk; |
| 88 | SETTINGS.k_lat = config.k_lat; |
| 89 | SETTINGS.k_lon = config.k_lon; |
| 90 | // SETTINGS.k_obstacle = config.k_obstacle; |
| 91 | // Safety constraints |
| 92 | SETTINGS.vehicle_length = config.vehicle_length; |
| 93 | SETTINGS.vehicle_width = config.vehicle_width; |
| 94 | SETTINGS.safety_margin_lon = config.safety_margin_lon; |
| 95 | SETTINGS.safety_margin_lat = config.safety_margin_lat; |
| 96 | SETTINGS.safety_margin_soft = config.safety_margin_soft; |
| 97 | // PID and Stanley gains |
| 98 | PID_Kp = config.PID_Kp; |
| 99 | PID_Ki = config.PID_Ki; |
| 100 | PID_Kd = config.PID_Kd; |
| 101 | NUM_WP_LOOK_AHEAD = config.num_wp_look_ahead; |
| 102 | STANLEY_OVERALL_GAIN = config.stanley_overall_gain; |
| 103 | TRACK_ERROR_GAIN = config.track_error_gain; |
| 104 | // Hyperparameters for output path |
| 105 | TRAJ_MAX_SIZE = config.traj_max_size; |
| 106 | TRAJ_MIN_SIZE = config.traj_min_size; |