
# This block should only store calibration data.  (i.e. matt antone owns this
# block)

# Calibration from 2007-06-07 weymouth data set
# Ad-hoc methods used; assumed skirt lidars and velodyne perfectly level
# Used some manually-measured quantities
calibration {

# Bounds are specified in body coordinates (center of rear axle=origin)

# **NOTE: these bounds are tight, and were derived from the original CAD
#  model, so do NOT accommodate the extra equipment on the front and rear.
# (with stock bumper)
#    vehicle_bounds {
#        front_left = [3.8,1.0];
#        front_right = [3.8,-1.0];
#        rear_left = [-1.1,1.0];
#        rear_right = [-1.1,-1.0];
#    }
#
# Vehicle, as modified, including rear sick
    vehicle_bounds {
        front_left = [4.04,1.0];
        front_right = [4.04,-1.0];
        rear_left = [-1.36,1.0];
        rear_right = [-1.36,-1.0];
    }

    calibration_to_body {
#        position = [1.86,0,1.7];
        position = [0,0,0];
        orientation = [1,0,0,0];
    }
    
    imu {
        serial_number = 1;
        body_frame {
#            orientation = [0.999801,-0.000144,0.018324,0.007853];
#            orientation = [0.99988348461239673, 0.0001027368008877986, -0.013089192388242812, 0.0078536416485787096];
            orientation = [0.99996580052287631, 2.03424139882388e-05, -0.0025898866052970551, 0.0078542891594992493];
#            orientation = [0.9990482215818578, 0.0, -0.043619387365336, 0.0];
#            orientation = [1, 0, 0, 0];
            position = [-0.8, 0, 0.9];
        }
    }
    
    gps {
        body_frame {
            position = [-0.8, 0, 0.9];
        }
	#nad83_offset = [ 0, 0, 0];
	# Weymouth
	#nad83_offset = [ 12.1944e-6, 2.6528e-6, -1.2280];
	# Victorville from cors station:
	nad83_offset = [ 4.46e-6, -1.15e-5, -0.729];
	# ElToro (John Wayne) cors station:
	#nad83_offset = [ 3.58e-6, -1.08e-5, -0.723];
	# ElToro (John Wayne) cors station with sign error works best with Caltech RNDF :|
	#nad83_offset = [ -3.58e-6, -1.08e-5, -0.723];
    }

VELODYNE {
   position = [  1.56, -0.00408,  2.550000];
   rpy      = [  2.683627, -0.076601, 91.026067];
   relative_to = "body";
}

BROOM_C {
   position = [  2.013933,  0.047059,  2.100000];
   rpy      = [-179.138828, 23.745276,  0.060704];
   relative_to = "body";
}

BROOM_CL {
   position = [  1.819529,  0.375364,  2.100000];
   rpy      = [-178.709200,  8.355239,  0.440671];
   relative_to = "body";
}

BROOM_CR {
   position = [  1.901997, -0.399339,  2.100000];
   rpy      = [-179.963066, 12.068070,  0.750713];
   relative_to = "body";
}

BROOM_L {
   position = [  1.860818,  0.720426,  2.100000];
   rpy      = [-177.623234, 17.592725, 45.234690];
   relative_to = "body";
}

BROOM_R {
   position = [  1.779955, -0.506827,  2.100000];
   rpy      = [178.842287, 16.295419,-44.257785];
   relative_to = "body";
}

SKIRT_FC {
   position = [  3.852147,  0.048174,  0.870000];
   rpy      = [-180.830635,  2.685923, -0.962376];
   relative_to = "body";
}

SKIRT_FL {
   position = [  3.813821,  0.731976,  0.685000];
   rpy      = [-179.048230,  2.159054, 45.540754];
   relative_to = "body";
}

SKIRT_FR {
   position = [  3.812421, -0.701704,  0.520000];
   rpy      = [177.945370,  0.578049,-46.905670];
   relative_to = "body";
}

SKIRT_RC_HI {
   position = [ -.990800,  0.546515,  0.960000];
   rpy      = [180.491037, -2.433785,175.662131];
   relative_to = "body";
}

SKIRT_RC_LO {
   position = [ -1.061562,  0.148386,  0.490000];
   rpy      = [  0.306163, -2.863488,178.947666];
   relative_to = "body";
}

SKIRT_RL {
   position = [ -0.523749,  1.018553,  0.530000];
   rpy      = [  2.204727, -0.874988, 94.712140];
   relative_to = "body";
}

SKIRT_RR {
   position = [ -0.525306, -0.788682,  0.540000];
   rpy      = [ -1.890840, -2.421132,-96.142552];
   relative_to = "body";
}

# NOTE (2007-10-14): physical measurements of camera positions are really
# about 15cm off in x.
# So the fc camera labeled as 1.6m in x is physically more like 1.75m.
# The 15cm is a fudge factor used to align the data better with the lasers.
    cameras {
        cam_roof_fc {
            width = 752;
            height = 480;
            distortion_model = "spherical";
            distortion_center = [-0.020410,0.033517];
            distortion_params = [1.074213];
            pinhole = [479.867758,479.867758,0.000000,360.036957,267.139665];
            #position = [0.020000,0.100000,0.170000];
            position = [2.0, 0.10, 1.87];
#            orientation = [0.494631,-0.514317,0.509386,-0.480983];
            orientation = [0.49910037152507131, -0.50998100140449931, 0.51356391936558043, -0.47651951062979958];
	    relative_to = "body";
        }

        cam_roof_fl {
            width = 752;
            height = 480;
            distortion_model = "spherical";
            distortion_center = [-0.021519,-0.031968];
            distortion_params = [1.072291];
            pinhole = [479.567636,479.567636,0.000000,359.563721,216.757192];
            #position = [-0.130000,0.660000,0.130000];
            position = [1.70, 0.66, 1.83];
            orientation = [0.598504,-0.772675,0.176464,-0.116734];
	    relative_to = "body";
        }

        cam_roof_fr {
            width = 752;
            height = 480;
            distortion_model = "spherical";
            distortion_center = [0.011822,-0.039534];
            distortion_params = [1.071786];
            pinhole = [482.093010,482.093010,0.000000,384.128030,211.236693];
            #position = [-0.130000,-0.660000,0.130000];
            position = [1.60, -0.66, 1.83];
            orientation = [0.123769,-0.183141,0.770141,-0.598351];
	    relative_to = "body";
        }
        cam_roof_rc {
            width = 752;
            height = 480;
            distortion_model = "spherical";
            distortion_center = [-0.05616908,-0.005963998];
            distortion_params = [1.085173436];
            pinhole = [473.1791,473.1791,0.0000,333.0504,237.5875];
            #position = [-2.50000,0.3560000,0.170000];
#            position = [-0.78, 0.36, 1.87];
            position = [-0.72, 0.58, 1.87];
#            orientation = [0.497204417,-0.480581100,-0.500506042,0.520887007];
#            orientation = [0.49981823297819244, -0.47784716678428141, -0.49789583684278083, 0.52339617206745337];
            #orientation = [0.50662645655357041, -0.48432348359917721, -0.49159835474042768, 0.5168089129187684];
            orientation = [0.5096029747,-0.490046465,-0.4884064966,0.5114864264];
	    relative_to = "body";
        }

        cam_roof_fc_6mm {
            width = 752;
            height = 480;
            distortion_model = "spherical";
            distortion_center = [-0.001310066976,-0.01458390567];
            distortion_params = [0.68462101929529];
            pinhole = [1024.920603,1024.920603,0.000000,373.788212,233.819293];
            #position = [0.020000,0.100000,0.170000];
            position = [2.0, 0.10, 1.87];
            orientation = [0.496964,-0.507504,0.518219,-0.476356];
	    relative_to = "body";
        }

        cam_roof_fc_9mm {
            width = 752;
            height = 480;
            distortion_model = "spherical";
            distortion_center = [-0.0179985975,-0.01234489871];
            distortion_params = [0.594448022015905];
            pinhole = [1537.197190,1537.197190,0.000000,361.827044,231.833107];
            #position = [0.020000,0.100000,0.170000];
            position = [2.0, 0.10, 1.87];
            orientation = [0.499054,-0.513152,0.488003,-0.499473];
	    relative_to = "body";
        }

    }
    
    RADAR_ROOF_0 {
        #position = [0.02,-0.07,0.17];
        position = [1.88, -0.07, 1.87];
        angleaxis = [0.0, 0,0,1];
        relative_to = "body";
    }
    RADAR_ROOF_17 {
        #position = [0.02,-0.29,0.17];
        position = [1.88, -0.29, 1.87];
        angleaxis = [0.297, 0,0,1];
	relative_to = "body";
    }
    RADAR_ROOF_34 {
        #position = [0.02,-0.68,0.17];
        position = [1.88, -0.68, 1.87];
        angleaxis = [0.593, 0,0,1];
	relative_to = "body";
	}
    RADAR_ROOF_51 {
        #position = [-0.04,-0.92,0.30];
        position = [1.82, -0.92, 2.00];
        angleaxis = [0.890, 0, 0, 1];
	relative_to = "body";
    }
    RADAR_FRONT_68 {
        #position = [2.14,0.27,-0.78];
        position = [4.00, 0.27, 0.92];
        angleaxis = [1.187, 0,0,1];
	relative_to = "body";
    }
    RADAR_FRONT_85 {
        #position = [2.12,0.78,-0.53];
        position = [3.98, 0.78, 1.17];
        angleaxis = [1.484, 0,0,1];
	relative_to = "body";
    }
    RADAR_FRONT_102 {
        #position = [2.06,0.27,-0.53];
        position = [3.92, 0.27, 1.17];
        angleaxis = [1.780, 0,0,1];
	relative_to = "body";
    }
    RADAR_ROOF_119 {
        #position = [-0.40,0.93,0.17];
        position = [1.46, 0.93, 1.87];
        angleaxis = [2.077, 0,0,1];
	relative_to = "body";
    }
    RADAR_ROOF_-17 {
        #position = [0.02,0.41,0.17];
        position = [1.88, 0.41, 1.87];
        angleaxis = [-0.297, 0,0,1];
	relative_to = "body";
    }
    RADAR_ROOF_-34 {
        #position = [-0.04,0.61,0.30];
        position = [1.82, 0.61, 2.00];
        angleaxis = [-0.593, 0,0,1];
	relative_to = "body";
    }
    RADAR_ROOF_-51 {
        #position = [0.02,0.75,0.17];
        position = [1.88, 0.75, 1.87];
        angleaxis = [-0.890, 0,0,1];  
	relative_to = "body";
    }
    RADAR_FRONT_-68 {
        #position = [2.14,-0.27,-0.78];
        position = [4.00, -0.27, 0.92];
        angleaxis = [-1.187, 0,0,1];
	relative_to = "body";
    }
    RADAR_FRONT_-85 {
        #position = [2.12,-0.79,-0.53];
        position = [3.98, -0.79, 1.17];
        angleaxis = [-1.484, 0,0,1];
	relative_to = "body";
    }
    RADAR_FRONT_-102 {
        #position = [2.06,-0.27,-0.53];
        position = [3.92, -0.27, 1.17];
        angleaxis = [-1.780, 0,0,1];
	relative_to = "body";
    }
    RADAR_ROOF_-119 {
        #position = [-0.40,-0.93,0.17];
        position = [1.46, -0.93, 1.87];
        angleaxis = [-2.077, 0,0,1];
	relative_to = "body";
    }

    IBEO {
        #position = [2.180000,0.000000,-0.8];
        position = [4.04, 0.00, 0.90];
	rpy = [ 0, 0, -4];
	relative_to = "body";
    }
}

renderer_car {
    chassis_model = "Landrover_LR3_2006_chassis.rwx";
    chassis_translate = [-50533, 15912, 53209];
    chassis_xyz_rotate = [90, 0, 180];
    chassis_scale = 0.00008533;
    wheel_model = "Landrover_LR3_2006_wheel.rwx";
    wheel_translate = [-16911, 11472, 43815];
    wheel_xyz_rotate = [90, 0, 180];
    wheel_scale = 0.00008533;

    wheel_radius = 0.35;
    wheel_base = 2.885;
    wheel_track = 1.605;
    steering_ratio = 17.7;
}

obstacles {
    velodyne_mask_length = 5.5;
    velodyne_mask_width = 2.5;
    velodyne_mask_length_offset = -1;
    velodyne_mask_width_offset = 0;
    velodyne_mask_rotation = 1.571;

    global_mask_forward = 4.0;
    global_mask_backward = 1.5;
    global_mask_width = 2.3;

    persist_mask_forward = 5.5;   #[m] fwd of rear axle
    persist_mask_backward = 3.5;  #[m] bwd of rear axle
    persist_mask_width = 5.0;     #[m] total width centered at middle of rear axel
    persist_mask_expire_sec = 5;  #[s] time to persist objects within mask if unseen
}

# this block stores all lidar configuration params except for calibration

# If a sensor is listed as disabled = "true" (case-insensitive) or disabled = 1
# (or any number other than 0), it wont be searched for by the carcheck script.
# disabled = 0 is valid, but ignored: sensors are enabled by default.

RADAR_TRACKS {
	     viewer_color = [1,0,0];
}

LASER_TRACKS {
	     viewer_color = [0,0,1];
}

SKIRT_FC {
    device="moxa:moxa:1";
    viewer_color = [ 1.0, 1.0, 0.0 ];
    # mask off occluded parts of the sensor field. This is a list
    # of pairs of doubles: any theta (in the sicks coordinates)
    # is eliminated if it is between the two as you rotated in
    # +theta direction from the first to the second.
    verifiers=["SKIRT_FL","SKIRT_FR"];
    mask = [1.5, 1.6];
}

SKIRT_FL {
    device="moxa:moxa2:7";
    viewer_color = [ 1.0, 0.0, 1.0 ];
    verifiers=["SKIRT_FC","SKIRT_FR"];
}

SKIRT_FR {
    device="moxa:moxa2:2";
    viewer_color = [ 0.0, 1.0, 1.0 ];
    verifiers=["SKIRT_FC","SKIRT_FL"];
}

SKIRT_RR {
    device="moxa:moxa2:4";
    viewer_color = [1, 0, 0];
    verifiers=["SKIRT_FR","SKIRT_RC_LO","SKIRT_RC_HI"];
}

SKIRT_RL {
    device="moxa:moxa:4";
    mask = [-1.6, -1.50];
    verifiers=["SKIRT_FL","SKIRT_RC_LO","SKIRT_RC_HI"];
    viewer_color = [0, 1, 0];
}	

SKIRT_RC_HI {
    device="moxa:moxa2:0";
    viewer_color = [ 0.3, 0.3, 0.6 ];
    verifiers=["SKIRT_RL","SKIRT_RR","SKIRT_RC_LO"];
}

SKIRT_RC_LO {
    device="moxa:moxa:0";
    viewer_color = [ 0.3, 0.6, 0.3 ];
    verifiers=["SKIRT_RL","SKIRT_RR","SKIRT_RC_HI"];
}

BROOM_C {
    device="moxa:moxa2:1";
    viewer_color = [ 0.0, 0.0, 1 ];
    verifiers=["BROOM_CL", "BROOM_CR"];
    mask = [1.45, 1.6];
}

BROOM_L {
    device="moxa:moxa2:3";
    viewer_color = [ 0.0, 1.0, 1 ];
    mask = [1.45, 1.6];
}

BROOM_R {
    device="moxa:moxa:2";
    viewer_color = [ 1.0, 0.0, 1 ];
    mask = [ -1.6, -1.45];
}

BROOM_CL {
    device="moxa:moxa:7";
    viewer_color = [ 1.0, 1.0, 0 ];
    mask = [0.9, 1.6];
    verifiers=["BROOM_C", "BROOM_CR"];
}

BROOM_CR {
    device="moxa:moxa:6";
    viewer_color = [ 0.0, 1.0, 0 ];
    mask = [-1.6, -0.9];
    verifiers=["BROOM_CL", "BROOM_C"];
}

VERT_L {
    disabled = 1;
    device="moxa:---:3";
    viewer_color = [ 0.3, 0.3, 0.3 ];
}

VERT_R {
    disabled = 1;
    device="moxa:---:3";
    viewer_color = [ 0.3, 0.3, 0.3 ];
}

RADAR_ROOF_0 {
    device="ethcan11:1";
}

RADAR_ROOF_17 {
    device="ethcan14:1";
}

RADAR_ROOF_34 {    
    device="ethcan13:0";
}

RADAR_ROOF_51 {
    device="ethcan12:0";
}

RADAR_FRONT_68 {
    device="ethcan16:1";
}

RADAR_FRONT_85 {
    device="ethcan17:0";
}

RADAR_FRONT_102 {
    device="ethcan14:0";
}

RADAR_ROOF_119 {
    device="ethcan17:1";
}

RADAR_ROOF_-17 {
    device="ethcan11:0";
}

RADAR_ROOF_-34 {
    device="ethcan16:0";
}

RADAR_ROOF_-51 {
    device="ethcan15:0";
}

RADAR_FRONT_-68 {
    device="ethcan10:0";
}

RADAR_FRONT_-85 {
    device="ethcan15:1";
}

RADAR_FRONT_-102 {
    device="ethcan10:1";
}

RADAR_ROOF_-119 {
    device="ethcan12:1";
}



# this block stores all camera configuration params except for calibration
cameras {
    #Roof front center
    cam_roof_fc {
        mobileye_bottom = -83;
        mobileye_left = -160;
        uid = "0xb09d010062bd06";
        thumbnail_channel = "CAM_THUMB_RFC";
        image_url_channel = "CAM_URL_RFC";
        full_frame_channel = "CAM_FRAME_RFC";
        short_name = "RFC";
        vehicle_body = [ 147, 479,
                         147, 445,
                         202, 408,
                         568, 396,
                         621, 426,
                         621, 479 ];
    }

    #Roof front left
    cam_roof_fl {
        uid = "0xb09d010062bce5";
        thumbnail_channel = "CAM_THUMB_RFL";
        image_url_channel = "CAM_URL_RFL";
        full_frame_channel = "CAM_FRAME_RFL";
        short_name = "RFL";
        vehicle_body = [ 0, 139,
                     83, 136,
                     102, 23,
                     0, 23 ];
    }

    #Roof front right
    cam_roof_fr {
        uid = "0xb09d010062bcdc";
        thumbnail_channel = "CAM_THUMB_RFR";
        image_url_channel = "CAM_URL_RFR";
        full_frame_channel = "CAM_FRAME_RFR";
        short_name = "RFR";
        vehicle_body = [ 0, 225,
                     100, 191,
                     90, 99,
                     0, 99 ];
    }

    #Roof rear center
    cam_roof_rc {
        uid = "0xb09d010062bd0a";
        thumbnail_channel = "CAM_THUMB_RRC";
        image_url_channel = "CAM_URL_RRC";
        full_frame_channel = "CAM_FRAME_RRC";
        short_name = "RRC";
    }

    cam_roof_fc_6mm {
        uid = "0xb09d010062bce9";
            thumbnail_channel = "CAM_THUMB_RFC_6mm";
            image_url_channel = "CAM_URL_RFC_6mm";
            full_frame_channel = "CAM_FRAME_RFC_6mm";
            short_name = "RFC6";
        }

        cam_roof_fc_9mm {
            uid = "0xb09d010062bd12";
            thumbnail_channel = "CAM_THUMB_RFC_9mm";
            image_url_channel = "CAM_URL_RFC_9mm";
            full_frame_channel = "CAM_FRAME_RFC_9mm";
            short_name = "RFC9";
        }

}

mobileye {
    refPointX = 0;
    refPointY = 0;
    refPointZ = 0;
    minLaneWidth = 2.5;
    leftLDWsens = 1;
    rightLDWsens = 1;
    runningMode = 0;
}

# this block stores all controller related params
control {
    sc {
        Kp_fw   = 0.2;          # [/(m/s)] P gain for forwards drive (FD)
        Ki_fw   = 0.04;         # [/m] I gain for FD
        Kd_fw   = 0.0;          # [/(m/(s^2))] D gain for FD
        Kwu_fw  = 0.1;          # [m] anti-windup gain for FD
        Kp_rv   = 0.2;          # [/(m/s)] P gain for reverse drive (RD)
        Ki_rv   = 0.04;         # [/m] I gain for RD
        Kd_rv   = 0.0;          # [/(m/(s^2))] D gain for RD
        Kwu_rv  = 0.1;          # [m] anti-windup gain for RD

        use_acc_ff  = 0;        # [.] use acceleration feedforward
        use_dec_ff  = 0;        # [.] use deceleration feedforward
        use_vel_ff  = 0;        # [.] use speed feedforward
        acc_ff_n    = 2;            # [.] numer of points
        acc_ff_key  = [0.0, 2.0];   # [m/(s^2)] commanded acceleration
        acc_ff_val  = [0.0, 0.02];  # [.] u_speed
        dec_ff_n    = 2;            # [.] numer of points
        dec_ff_key  = [-2.0, 0.0];  # [m/(s^2)] commanded deceleration
        dec_ff_val  = [-0.1, 0.0];  # [.] u_speed
        vel_ff_n    = 9;            # [.] number of points
        vel_ff_key  = [ 0.0,    1.5,    2.0,    2.6,    3.3,    7.2,    9.4, 
                          13.5,  100.0];
        # [m/s] commanded speed
        vel_ff_val  = [ 0.0, 0.0500, 0.0817, 0.1133, 0.1292, 0.1450, 0.1900, 
                        0.2000, 0.2000];
        # [.] u_speed
    }
    pp {
        fw_ref_dist = 1.4425;   # [m] distance of reference point from rear 
                                # axle, positive towards nose, for forward 
                                # drive (FD)
        fw_min_L1   = 3.0;      # [m] minimum L1 distance for FD
        fw_max_L1   = 12.0;     # [m] maximum L1 distance for FD
        fw_min_L1_v = 1.34;     # [m/s] speed corresponding to fw_min_L1
        fw_max_L1_v = 5.36;     # [m/s] speed corresponding to fw_max_L1

        rv_ref_dist = -1.4425;  # [m] distance of reference point from rear
                                # axle, positive towards nose, for reverse 
                                # drive (RD)
        rv_min_L1   = 3.0;      # [m] minimum L1 distance for RD
        rv_max_L1   = 12.0;     # [m] maximum L1 distance for RD
        rv_min_L1_v = 1.34;     # [m/s] speed corresponding to rv_min_L1
        rv_max_L1_v = 5.36;     # [m/s] speed corresponding to rv_max_L1
    }
    controller {
        stopped_gas_brake_mv    = 1400;     # [mV] gas to apply when stopped
        vel_stop_threshold      = 0.05;     # [m/s] velocity below which the 
                                            # car can be considered stopped
        acc_stop_threshold      = 0.1;      # [m/s^2] acceleration below which 
                                            # the car can be considered stopped
        stopped_hysteresis_factor   = 3.0;  # [.] this is the factor, 
                                            # multiplied to the nominal 
                                            # parameters, to determine whether 
                                            # the car is considered stopped, 
                                            # after it had actually stopped.

    ###########################################################################
    # PLEASE INCREASE THE THRESHOLD BELOW BEFORE THE RACE                     #
    #   max_safe_roll, max_safe_pitch, pose_update_max_delay_us,              #
    #   motion_plan_timeout                                                   #
    #       20[deg] = 0.349[rad]    30[deg] = 0.524[rad]                      #
    ###########################################################################
        pose_update_max_delay_us    = 200000;   # [us] if the interval from the 
                                                # last received pose_t is 
                                                # greater than this, consider 
                                                # that pose_t message as 
                                                # invalid
        max_safe_roll   = 0.349;    # [rad] maximum tolerable vehicle roll to 
                                    # be considered safe
        max_safe_pitch  = 0.785;    # [rad] maximum tolerable vehicle pitch to 
                                    # be considered safe
        v_body_x_tau    = 0.08;     # [s] filter time constant for longitudinal 
                                    # velocity
        a_body_x_tau    = 0.08;     # [s] filter time constant for longitudinal 
                                    # acceleration
        a_body_y_tau    = 0.08;     # [s] filter time constant for lateral 
                                    # acceleration
        # Note: Bandwidth = 1.9894 Hz for tau == 0.08
        max_engine_rpm  = 7000.0;   # [rpm] maximum allowable engine rpm
        max_steer_wheel_speed   = 0.02; # [rad/s] maximum rate of change of raw 
                                        # steering wheel data, below which, the 
                                        # steering wheel is considered 
                                        # stationary. This is used to determine 
                                        # whether the steer/shift plan has been 
                                        # executed to completion
    }
    scpp {
        min_pt_sep          = 0.08;     # [m] minimum allowable separation 
                                        # between points of the predicted 
                                        # trajectory path
        update_interval_ms  = 40;       # [ms] loop update interval
        final_brake_u_step  = 0.05;     # [.] if final u_speed is greater than 
                                        # the equivalent stopped_gas_brake_mv, 
                                        # reduce u_speed at this level at every 
                                        # iteration until stopped_gas_brake_mv 
                                        # is reached
        default_delay_us    = 1000000;  # [us] default delay interval
        shift_delay_us      = 1000000;  # [us] delay interval after shift
        execution_delay_us  = 200000;   # [us] delay interval after finish 
                                        # executing a plan
        safety_override_delay_us    = 10000000; # [us] delay interval after 
                                                # safety override has been 
                                                # triggered
        max_steer_interval_us   = 3300000;  # [us] interval to allow steer to 
                                            # complete when shifting
        motion_plan_timeout_us  = 300000;   # [us] motion plan timeout
        max_v_cmd_discontinuity = 2.0;      # [m/s] maximum discontinuity of 
                                            # v_cmd used
        vel_advisor_override_acc    = 1.0;  # [m/s^2] the acceleration to use 
                                            # when overriding the planned v_cmd 
                                            # based on the velocity advisor
        vel_advisor_override_dec    = 2.5;  # [m/s^2] the deceleration to use 
                                            # when overriding the planned v_cmd 
                                            # based on the velocity advisor
    }
    sclsc {
        l1_fw       = 4.1;      # [m] signed displacement from rear axle that 
                                # defines reference point
        Ke_fw       = 0.05;     # [rad/m] gain for cross-track error term
        Klambda_fw  = 0.0;      # [rad/rad] gain for heading error term
        Komega_fw   = 0.0;      # [rad/(rad/m)] gain for path curvature term
        l1_rv       = -1.1;     # [m] signed displacement from rear axle that 
                                # defines reference point
        Ke_rv       = 0.5;      # [rad/m] gain for cross-track error term
        Klambda_rv  = 0.0;      # [rad/rad] gain for heading error term
        Komega_rv   = 0.0;      # [rad/(rad/m)] gain for path curvature term
        min_pt_sep          = 0.08;     # [m] minimum allowable separation 
                                        # between points of the predicted 
                                        # trajectory path
        update_interval_ms  = 40;       # [ms] loop update interval
        final_brake_u_step  = 0.005;    # [.] if final u_speed is greater than 
                                        # the equivalent stopped_gas_brake_mv, 
                                        # reduce u_speed at this level at every 
                                        # iteration until stopped_gas_brake_mv 
                                        # is reached
        default_delay_us    = 1000000;  # [us] default delay interval
        shift_delay_us      = 1000000;  # [us] delay interval after shift
        execution_delay_us  = 200000;   # [us] delay interval after finish 
                                        # executing a plan
        safety_override_delay_us    = 300000000;    # [us] delay interval after 
                                                    # safety override has been 
                                                    # triggered
        motion_plan_timeout_us  = 300000;   # [us] motion plan timeout
        max_v_cmd_discontinuity = 1.0;      # [m/s] maximum discontinuity of 
                                            # v_cmd used
    }
    vehicle_constants {
        wheel_base  = 2.885;    # [m] wheelbase
        wheel_track = 1.605;    # [m] smallest wheel track (front) 
                                # [for rollover prevention]
        height_cg   = 1.5;      # [m] height of C.G. [for rollover prevention]
                                # Crude estimate of C.G. height with all 
                                # equipment and 4 passengers is 1.207m. 
                                # ALWAYS SPECIFY a number greater than 1.207m 
                                # with some margin
        lat_accel_safety_factor = 1.0;  # [.] a multiplicative factor ~ 1.0 to 
                                        # tune conservativeness of rollover
        rear_axle_to_front_bumper   = 4.1;  # [m] = 3.574 to original bumper
        rear_bumper_to_rear_axle    = 1.1;  # [m] = 1.027 to original bumper
        center_to_mirror_tip        = 1.1;  # [m] = 1.000 bare minimum
        max_steer       = 0.5435;   # [rad] maximum steer angle of wheels
        max_steer_rate  = 0.3294;   # [rad/s] at wheel on ground

        characteristic_velocity = 20.0;     # [m/s]
        time_const_brake        = 0.3;      # [s]
        time_const_gas          = 0.3;      # [s]
        time_const_steer        = 0.05;     # [s] Derived by Stefan
        delay_actuator_switch   = 0.00;     # [s]
        max_accel   = 1.8;                  # [m/s^2] cap
        max_decel   = 6.0;                  # [m/s^2]

        # parameters for map from controls to ADU voltage
        mv_max  = 4700;     # [mV] upper limit of throttle command
        mv_pos  = 3200;     # [mV] start applying the gas (RPM starts 
                            # responding)
        u_pos   = 0.05;     # [dimensionless] normalized u corresponding to 
                            # mv_pos
        mv_neg  = 1850;     # [mV] start applying the brake (steady state speed 
                            # is slower than coasting)
        u_neg   = -0.05;    # [dimensionless] normalized u corresponding to 
                            # mv_neg
        mv_min  = 300;      # [mV] lower limit of brake command
        steer_center_volt   = 2440;     # [mV] steer center voltage measured in 
                                        # field
        steer_range_volt    = 4400;     # [mV] range of steer voltage == 
                                        # max_steer_volt - min_steer_volt

        # Parameters for the emprical LPV model for longitudinal motion.
        # This model is used for velocity prediction in 
        # /planning3/vdh_model_prop.c
        lpv_model_a0    = 49.1208;
        lpv_model_a1    = 0.5788;
        lpv_model_a2    = 0.1013;
        lpv_model_tau   = 12.0;

        # mv to gas and brake mappings 
        # these mappings are used by the LPV model for longitudinal motion
        map_mv_to_gas_x1    = 3250.0;
        map_mv_to_gas_x2    = 3700.0;
        map_mv_to_gas_y1    = 0.0;
        map_mv_to_gas_y2    = 0.26;

        map_mv_to_brake_x1  = 1800.0;
        map_mv_to_brake_x2  = 900.0;
        map_mv_to_brake_y1  = 0.0;
        map_mv_to_brake_y2  = 0.6;

        # input scaling parameters of LPV model for longitudinal motion
        brake_coef  = 3.0;
        brake_const = 0.2;
    }
}

lane_tracker {
    # this parameter is a rough knob on the curvature the lane tracker uses
    # when interpolating the positions of low-confidence control points.
    #
    # acceptable values are from 0.0 (linear interpolation) to 1.0
    # (low-curvature cubic spline)
    interpolation_scale_factor = 0.0;

    # if set to 1, then the lane detectors will compute the position of the
    # sun and attempt to reject images with significant sun dazzle.
    reject_sun = 1;
}

# this block stores all situational interpreter/planner params
# TODO: separate the parameters for the SP from those for the SI
motion_planner {
    target_threshold_heading_deg = 180.0;    # [deg] +/- of this
    
    # Prediction related
    speed_design {
        min_v_cmd           = 0.5;  # [m/s] Minimum commanded speed to move forward
        max_v_cmd           = 11.176; # [m/s] 30mph = 13.41m/s, 20mph = 8.94m/s, 10mph = 4.47m/s
        ramp_accel          = 1.0;  # [m/s^2]
        ramp_decel          = 2.5;  # [m/s^2]
        slow_ramp_accel     = 0.5;  # [m/s^2] Slow ramp for initial acceleration
        slow_ramp_max_v_cmd = 1.0;  # [m/s] Slow ramp ends here
        min_duration        = 1.0;  # [s]
        #  decel  :   v0,     v1,      v2 
        # 1.8m/s^2: -0.3523, 1.2175, -0.0337
        # 2.0m/s^2: -0.4101, 1.2265, -0.0311
        # 2.5m/s^2: -0.5347, 1.2344, -0.0252
        dist_overshoot_per_v0 = -0.5347;    # Overshooting distance is
        dist_overshoot_per_v1 =  1.2344;    # per_v0 + v * per_v1 + v^2 * per_v2
        dist_overshoot_per_v2 = -0.0252;
    }
    
    max_duration_gear_shift = 7.0;     # Bound on how long the shifting takes    
    prop_heading_diff_tol_deg = 20.0;  # heading diff between the car and the
                                       # motion plan at the stopped state
    prop_steering_diff_tol_deg = 30.0; # steering diff between the car and the
                                       # motion plan at the stopped state
    speed_sampling_accel = 4.0;        # [m/s^2]
    max_side_accel             = 4.0;  # [m/s^2]  9.8m/s^ is 1g
    max_side_accel_loose       = 4.7;  # [m/s^2]  9.8m/s^ is 1g
    vlim_retry_ratio           = 0.6;  # try with (vlim * ratio) on too much amax
 
    # Collision check related
    max_violation_dist = 3.0;           # [m] initial violation
    max_constraint_tighten = 0.0;       # tightening
    dist_reach_max_tighten = 2.0;       # where max is reached
    max_dist_obst_cost  = 40.0;         # 
    
    rrt {
        min_dist_ctrl_ipts = 1.0; # min dist allowed between ctrler inputs, m
        num_branch_pt = 4;
        branch_min_sep_L1 = 0.5;        # [m]
        branch_min_sep_dt = 0.2;        # [sec]
        # add branches once the following is not satisfied
        min_duration_to_root = 0.2;     # [sec]
        min_dist_to_root     = 1.0;     # [m]
        
        num_node_try_connect = 10;      # after sorting
        num_node_try_target  = 300;     # on change_target
        use_pruning          = 0;       # flag
        pruning_threshold_diff = 0.0;   # pruning
        
        # for trashing the tree -- to be removed
        exec_threshold_cross_track = 2.0;    # [m]
        exec_threshold_speed = 12.5;    # [m/s]
        exec_threshold_heading_deg = 45;    # [deg]
    }

    gridmap {
    	    convolve_line_searches = 4; # (an integer)
	    convolve_total_width = 2.1; # width of car + .1m

# most conservative value
#	    obsmap_offsets =  [ -1.1, 3.8 ]; # centers of outermost 

#	    obsmap_offsets = [-.8375, 3.5375];
	    obsmap_offsets = [-.95, 3.65];
#	    lanemap_offsets = [ 1.1, 1.785 ];  # centers of outermost 
#	    lanemap_offsets = [ 0.0, 2.885 ];  # centers of outermost 
	    lanemap_offsets_always_feasible = [ 0.0, 2.885 ];  # centers of outermost 
	    lanemap_offsets_forward = [ 1.442, 2.885 ];  # centers of outermost 
	    lanemap_offsets_reverse = [ 0.0, 1.442 ];  # centers of outermost 
                                               # convolution circles
    }
    
    lcgl_channels = "RRT-ESTOP,RRT-DESCENDENT,RRT-REPROPAGATE,RRT-TARGET"; 

    # add whichever you always want to see
    # RRT-CHECKGRID-POS,RRT-CHECKGRID1,RRT-CHECKGRID,RRT-CHECKGRID-DBG,
    # RRT-OUTSIDE,RRT-SEP_LINES,RRT-PREDICTION-DATA,RRT-BAD-PREDICTION,RRT-NOT_OK,RRT-MIN-STATE,
    # RRT-TARGET,RRT-EXPAND,RRT-TREE-CTRL,RRT-TREE,RRT-PLAN-DBG-PRED,RRT-PLAN-DBG-COMM,RRT-PLAN-DBG-TARGET
    # RRT-PLAN-DBG-ROOT,RRT:REACHING,RRT:SPEED,RRT:SPEED_CMD,RRT-HAZARD,RRT-DESCENDENT,RRT-REPROPAGATE
    # RRT-PRED,RRT-ESTOP"
}
high_bandwidth_multicast_address = "239.4.107.68";
high_bandwidth_multicast_port = 7698;

vehicle {
      ############################################
      # car physical characteristics

#      max_turn_rate_rad = 0.2041;  # turning radius is the reciprocal 
      max_turn_rate_rad = 0.2094;   # tan(0.5435) / 2.885

      # maximum STEER radians is different, and can be computed
      # as max_steer_rate_rad = atan(max_turn_rate_rad * wheel_base)

      wheel_base = 2.885;         # front to back distance
      wheel_width = 1.605;        # distance between left and right wheels
      rear_axle_to_nose = 3.8;    # distance from rear axle to nose of car
      overall_width = 2.0; # 2.0
      overall_length = 4.9;

      #############################################
      # EMC characteristics

      gas_none_mv = 3400;
      gas_full_mv = 3900;

      brake_none_mv = 1750;
      brake_full_mv = 1350;

      steer_left_mv = 200;
      steer_center_mv = 2440;
      steer_right_mv = 4600;

      steer_time = 3.75;           # how many seconds does it take to go from full left to full right
      gas_brake_time = .5;         # how many seconds to go from full gas to full brake?
}

rndf = "rndf/uce_rndf_dense.txt";

#rndf = "rndf/fake_nqe/NQE_Area_A_in_PA2_RNDF.txt";
#rndf = "rndf/fake_nqe/NQE_Scout_RNDF.txt";
#rndf = "rndf/nqe_rndf.txt";
#rndf = "rndf/fake_nqe/Cobalt_RNDF.txt";
#rndf = "rndf/fake_nqe/Cobalt_RNDF.txt";
#rndf = "rndf/uce_rndf.txt";

#rndf = "rndf/fake_nqe/Alert_Area_A_RNDF.txt";
#rndf = "rndf/fake_nqe/NQE_Practice_Area_1_RNDF.txt";
#rndf = "rndf/fake_nqe/NQE_Practice_Area_2_RNDF_krail.txt";
#rndf = "rndf/estop_rndf.txt";

#rndf = "rndf/Ford_Irvine_RNDF.txt";
#rndf = "rndf/ElToro_ITRF00_RNDF.txt";
#rndf = "rndf/ElToro_Start_RNDF.txt";
#rndf = "rndf/Weymouth_NQE_RNDF_3.txt";
#rndf = "rndf/Weymouth_NQE_RNDF_2.txt";
#rndf = "rndf/Olin_RNDF_dense.txt";
#rndf = "rndf/Dearborn_RNDF.txt";
#rndf = "rndf/Cambridge_RNDF.txt";
#rndf = "rndf/Sample_RNDF.txt";
#rndf = "rndf/Site_Visit_RNDF.txt";
#rndf = "rndf/Site_Visit_RNDF_Sparse.txt";
#rndf = "rndf/Site_Visit_20_mph_RNDF.txt";
#rndf = "rndf/Natick_RNDF.txt";
#rndf = "rndf/Natick_tweaked_RNDF.txt";

