00001 #ifndef INCLUDE_ROTORS_CONTROL_PARAMETERS_ROS_H_
00002 #define INCLUDE_ROTORS_CONTROL_PARAMETERS_ROS_H_
00003
00004 #include <ros/ros.h>
00005
00006 #include "rotors_control/parameters.h"
00007
00008 namespace rotors_control {
00009
00010 template<typename T> inline void GetRosParameter(const ros::NodeHandle& nh,
00011 const std::string& key,
00012 const T& default_value,
00013 T* value) {
00014 ROS_ASSERT(value != nullptr);
00015 bool have_parameter = nh.getParam(key, *value);
00016 if (!have_parameter) {
00017 ROS_WARN_STREAM("[rosparam]: could not find parameter " << nh.getNamespace()
00018 << "/" << key << ", setting to default: " << default_value);
00019 *value = default_value;
00020 }
00021 }
00022
00023 inline void GetRotorConfiguration(const ros::NodeHandle& nh,
00024 RotorConfiguration* rotor_configuration) {
00025 std::map<std::string, double> single_rotor;
00026 std::string rotor_configuration_string = "rotor_configuration/";
00027 unsigned int i = 0;
00028 while (nh.getParam(rotor_configuration_string + std::to_string(i), single_rotor)) {
00029 if (i == 0) {
00030 rotor_configuration->rotors.clear();
00031 }
00032 Rotor rotor;
00033 nh.getParam(rotor_configuration_string + std::to_string(i) + "/angle",
00034 rotor.angle);
00035 nh.getParam(rotor_configuration_string + std::to_string(i) + "/arm_length",
00036 rotor.arm_length);
00037 nh.getParam(rotor_configuration_string + std::to_string(i) + "/rotor_force_constant",
00038 rotor.rotor_force_constant);
00039 nh.getParam(rotor_configuration_string + std::to_string(i) + "/rotor_moment_constant",
00040 rotor.rotor_moment_constant);
00041 nh.getParam(rotor_configuration_string + std::to_string(i) + "/direction",
00042 rotor.direction);
00043 rotor_configuration->rotors.push_back(rotor);
00044 ++i;
00045 }
00046 }
00047
00048 inline void GetVehicleParameters(const ros::NodeHandle& nh, VehicleParameters* vehicle_parameters) {
00049 GetRosParameter(nh, "mass",
00050 vehicle_parameters->mass_,
00051 &vehicle_parameters->mass_);
00052 GetRosParameter(nh, "inertia/xx",
00053 vehicle_parameters->inertia_(0, 0),
00054 &vehicle_parameters->inertia_(0, 0));
00055 GetRosParameter(nh, "inertia/xy",
00056 vehicle_parameters->inertia_(0, 1),
00057 &vehicle_parameters->inertia_(0, 1));
00058 vehicle_parameters->inertia_(1, 0) = vehicle_parameters->inertia_(0, 1);
00059 GetRosParameter(nh, "inertia/xz",
00060 vehicle_parameters->inertia_(0, 2),
00061 &vehicle_parameters->inertia_(0, 2));
00062 vehicle_parameters->inertia_(2, 0) = vehicle_parameters->inertia_(0, 2);
00063 GetRosParameter(nh, "inertia/yy",
00064 vehicle_parameters->inertia_(1, 1),
00065 &vehicle_parameters->inertia_(1, 1));
00066 GetRosParameter(nh, "inertia/yz",
00067 vehicle_parameters->inertia_(1, 2),
00068 &vehicle_parameters->inertia_(1, 2));
00069 vehicle_parameters->inertia_(2, 1) = vehicle_parameters->inertia_(1, 2);
00070 GetRosParameter(nh, "inertia/zz",
00071 vehicle_parameters->inertia_(2, 2),
00072 &vehicle_parameters->inertia_(2, 2));
00073 GetRotorConfiguration(nh, &vehicle_parameters->rotor_configuration_);
00074 }
00075 }
00076
00077 #endif