LegOdoCommon::LegOdoCommon(lcm::LCM* lcm_recv, lcm::LCM* lcm_pub, BotParam * param): lcm_recv(lcm_recv), lcm_pub(lcm_pub){ verbose = false; char* mode_str = bot_param_get_str_or_fail(param, "state_estimator.legodo.mode"); if (strcmp(mode_str, "lin_rot_rate") == 0) { mode_ = MODE_LIN_AND_ROT_RATE; std::cout << "LegOdo will provide velocity and rotation rates." << std::endl; }else if (strcmp(mode_str, "lin_rate") == 0){ mode_ = MODE_LIN_RATE; std::cout << "LegOdo will provide linear velocity rates but no rotation rates." << std::endl; }else if (strcmp(mode_str, "pos_and_lin_rate") == 0){ mode_ = MODE_POSITION_AND_LIN_RATE; std::cout << "LegOdo will provide positions as well as linear velocity rates." << std::endl; }else{ std::cout << "Legodo not understood! [LegOdoCommon]." << std::endl; } free(mode_str); R_legodo_xyz_ = bot_param_get_double_or_fail(param, "state_estimator.legodo.r_xyz"); R_legodo_vxyz_ = bot_param_get_double_or_fail(param, "state_estimator.legodo.r_vxyz"); R_legodo_vang_ = bot_param_get_double_or_fail(param, "state_estimator.legodo.r_vang"); R_legodo_vxyz_uncertain_ = bot_param_get_double_or_fail(param, "state_estimator.legodo.r_vxyz_uncertain"); R_legodo_vang_uncertain_ = bot_param_get_double_or_fail(param, "state_estimator.legodo.r_vang_uncertain"); }
ServoConverter::ServoConverter(BotParam *param) { rad_to_servo_.elevL_slope = bot_param_get_double_or_fail(param, "servo_commands.radians_to_servo.elevL_slope"); rad_to_servo_.elevL_y_intercept = bot_param_get_double_or_fail(param, "servo_commands.radians_to_servo.elevL_y_intercept"); rad_to_servo_.elevR_slope = bot_param_get_double_or_fail(param, "servo_commands.radians_to_servo.elevR_slope"); rad_to_servo_.elevR_y_intercept = bot_param_get_double_or_fail(param, "servo_commands.radians_to_servo.elevR_y_intercept"); rad_to_servo_.throttle_slope = bot_param_get_double_or_fail(param, "servo_commands.radians_to_servo.throttle_slope"); rad_to_servo_.throttle_y_intercept = bot_param_get_double_or_fail(param, "servo_commands.radians_to_servo.throttle_y_intercept"); servo_to_rad_.elevL_slope = bot_param_get_double_or_fail(param, "servo_commands.servo_to_radians.elevL_slope"); servo_to_rad_.elevL_y_intercept = bot_param_get_double_or_fail(param, "servo_commands.servo_to_radians.elevL_y_intercept"); servo_to_rad_.elevR_slope = bot_param_get_double_or_fail(param, "servo_commands.servo_to_radians.elevR_slope"); servo_to_rad_.elevR_y_intercept = bot_param_get_double_or_fail(param, "servo_commands.servo_to_radians.elevR_y_intercept"); servo_to_rad_.throttle_slope = bot_param_get_double_or_fail(param, "servo_commands.servo_to_radians.throttle_slope"); servo_to_rad_.throttle_y_intercept = bot_param_get_double_or_fail(param, "servo_commands.servo_to_radians.throttle_y_intercept"); elevL_min_ = bot_param_get_int_or_fail(param, "servo_commands.elevL_min"); elevL_max_ = bot_param_get_int_or_fail(param, "servo_commands.elevL_max"); elevR_min_ = bot_param_get_int_or_fail(param, "servo_commands.elevR_min"); elevR_max_ = bot_param_get_int_or_fail(param, "servo_commands.elevR_max"); throttle_min_ = bot_param_get_int_or_fail(param, "servo_commands.throttle_min"); throttle_max_ = bot_param_get_int_or_fail(param, "servo_commands.throttle_max"); elevL_trim_ = bot_param_get_int_or_fail(param, "servo_commands.elevL_flight_trim"); elevR_trim_ = bot_param_get_int_or_fail(param, "servo_commands.elevR_flight_trim"); throttle_trim_ = bot_param_get_int_or_fail(param, "servo_commands.throttle_flight_trim"); }