void RollPitchYawrateThrustControllerNode::InitializeParams() { ros::NodeHandle pnh("~"); // Read parameters from rosparam. GetRosParameter(pnh, "attitude_gain/x", roll_pitch_yawrate_thrust_controller_.controller_parameters_.attitude_gain_.x(), &roll_pitch_yawrate_thrust_controller_.controller_parameters_.attitude_gain_.x()); GetRosParameter(pnh, "attitude_gain/y", roll_pitch_yawrate_thrust_controller_.controller_parameters_.attitude_gain_.y(), &roll_pitch_yawrate_thrust_controller_.controller_parameters_.attitude_gain_.y()); GetRosParameter(pnh, "attitude_gain/z", roll_pitch_yawrate_thrust_controller_.controller_parameters_.attitude_gain_.z(), &roll_pitch_yawrate_thrust_controller_.controller_parameters_.attitude_gain_.z()); GetRosParameter(pnh, "angular_rate_gain/x", roll_pitch_yawrate_thrust_controller_.controller_parameters_.angular_rate_gain_.x(), &roll_pitch_yawrate_thrust_controller_.controller_parameters_.angular_rate_gain_.x()); GetRosParameter(pnh, "angular_rate_gain/y", roll_pitch_yawrate_thrust_controller_.controller_parameters_.angular_rate_gain_.y(), &roll_pitch_yawrate_thrust_controller_.controller_parameters_.angular_rate_gain_.y()); GetRosParameter(pnh, "angular_rate_gain/z", roll_pitch_yawrate_thrust_controller_.controller_parameters_.angular_rate_gain_.z(), &roll_pitch_yawrate_thrust_controller_.controller_parameters_.angular_rate_gain_.z()); GetVehicleParameters(pnh, &roll_pitch_yawrate_thrust_controller_.vehicle_parameters_); roll_pitch_yawrate_thrust_controller_.InitializeParameters(); }
void LeePositionControllerNode::InitializeParams() { ros::NodeHandle pnh("~"); // Read parameters from rosparam. GetRosParameter(pnh, "position_gain/x", lee_position_controller_.controller_parameters_.position_gain_.x(), &lee_position_controller_.controller_parameters_.position_gain_.x()); GetRosParameter(pnh, "position_gain/y", lee_position_controller_.controller_parameters_.position_gain_.y(), &lee_position_controller_.controller_parameters_.position_gain_.y()); GetRosParameter(pnh, "position_gain/z", lee_position_controller_.controller_parameters_.position_gain_.z(), &lee_position_controller_.controller_parameters_.position_gain_.z()); GetRosParameter(pnh, "velocity_gain/x", lee_position_controller_.controller_parameters_.velocity_gain_.x(), &lee_position_controller_.controller_parameters_.velocity_gain_.x()); GetRosParameter(pnh, "velocity_gain/y", lee_position_controller_.controller_parameters_.velocity_gain_.y(), &lee_position_controller_.controller_parameters_.velocity_gain_.y()); GetRosParameter(pnh, "velocity_gain/z", lee_position_controller_.controller_parameters_.velocity_gain_.z(), &lee_position_controller_.controller_parameters_.velocity_gain_.z()); GetRosParameter(pnh, "attitude_gain/x", lee_position_controller_.controller_parameters_.attitude_gain_.x(), &lee_position_controller_.controller_parameters_.attitude_gain_.x()); GetRosParameter(pnh, "attitude_gain/y", lee_position_controller_.controller_parameters_.attitude_gain_.y(), &lee_position_controller_.controller_parameters_.attitude_gain_.y()); GetRosParameter(pnh, "attitude_gain/z", lee_position_controller_.controller_parameters_.attitude_gain_.z(), &lee_position_controller_.controller_parameters_.attitude_gain_.z()); GetRosParameter(pnh, "angular_rate_gain/x", lee_position_controller_.controller_parameters_.angular_rate_gain_.x(), &lee_position_controller_.controller_parameters_.angular_rate_gain_.x()); GetRosParameter(pnh, "angular_rate_gain/y", lee_position_controller_.controller_parameters_.angular_rate_gain_.y(), &lee_position_controller_.controller_parameters_.angular_rate_gain_.y()); GetRosParameter(pnh, "angular_rate_gain/z", lee_position_controller_.controller_parameters_.angular_rate_gain_.z(), &lee_position_controller_.controller_parameters_.angular_rate_gain_.z()); GetVehicleParameters(pnh, &lee_position_controller_.vehicle_parameters_); lee_position_controller_.InitializeParameters(); }