13 ConfigStore::ConfigStore(
void):
14 m_serial_communication_(false),
15 m_upd_communication_(false),
16 m_configure_through_ros_(false),
17 m_ros_standard_output_(false)
30 if (ref_node_handle.
hasParam(
"uartConf"))
35 m_uart_baud_rate_ = getParameter<uint32_t>(ref_node_handle,
"uartConf/baudRate", 0);
38 else if (ref_node_handle.
hasParam(
"ipConf"))
40 std::string ip_address;
41 ref_node_handle.
param<std::string>(
"ipConf/ipAddress", ip_address,
"0.0.0.0");
50 throw ros::Exception(
"SBG DRIVER - Invalid communication interface parameters.");
76 float sensor_level_arm[3];
77 ref_node_handle.param<
float>(
"imuAlignementLeverArm/leverArmX", sensor_level_arm[0], 0.0f);
78 ref_node_handle.param<
float>(
"imuAlignementLeverArm/leverArmY", sensor_level_arm[1], 0.0f);
79 ref_node_handle.param<
float>(
"imuAlignementLeverArm/leverArmZ", sensor_level_arm[2], 0.0f);
124 float odometer_level_arm_[3];
125 ref_node_handle.
param<
float>(
"odom/leverArmX", odometer_level_arm_[0], 0.0f);
126 ref_node_handle.
param<
float>(
"odom/leverArmY", odometer_level_arm_[1], 0.0f);
127 ref_node_handle.
param<
float>(
"odom/leverArmZ", odometer_level_arm_[2], 0.0f);
322 m_rate_frequency_ = getParameter<uint32_t>(ref_node_handle,
"output/frequency", 0);
const SbgEComInitConditionConf & getInitialConditions(void) const
SbgEComAxisDirection axisDirectionX
SbgEComOutputPort m_output_port_
SbgEComSensorAlignmentInfo m_sensor_alignement_info_
SbgEComRejectionMode velocity
SbgEComOutputMode output_mode
SbgEComModulePortAssignment gps1Port
sbgIpAddress getIpAddress(void) const
uint32_t m_out_port_address_
const std::string & getUartPortName(void) const
const std::vector< SbgLogOutput > & getOutputModes(void) const
SbgEComAxisDirection axisDirectionY
uint32_t m_uart_baud_rate_
enum _SbgEComMagCalibMode SbgEComMagCalibMode
void loadCommunicationParameters(const ros::NodeHandle &ref_node_handle)
const SbgEComGnssInstallation & getGnssInstallation(void) const
void loadOdometerParameters(const ros::NodeHandle &ref_node_handle)
SbgEComGnssRejectionConf m_gnss_rejection_conf_
SbgEComMagCalibBandwidth m_mag_calib_bandwidth_
SbgEComRejectionMode velocity
const SbgEComMagCalibBandwidth & getMagnetometerCalibBandwidth(void) const
uint32_t m_in_port_address_
enum _SbgEComMagCalibBandwidth SbgEComMagCalibBandwidth
uint32_t getReadingRateFrequency(void) const
const SbgEComGnssRejectionConf & getGnssRejection(void) const
const SbgEComMagCalibMode & getMagnetometerCalibMode(void) const
SbgEComModelInfo m_gnss_model_info_
bool m_configure_through_ros_
bool checkConfigWithRos(void) const
const SbgEComModelInfo & getMotionProfile(void) const
SbgEComGnssInstallationMode leverArmSecondaryMode
SbgEComModuleSyncAssignment gps1Sync
std::vector< SbgLogOutput > m_output_modes_
void loadGnssParameters(const ros::NodeHandle &ref_node_handle)
bool m_upd_communication_
void loadAidingAssignementParameters(const ros::NodeHandle &ref_node_handle)
SbgEComModulePortAssignment rtcmPort
SbgVector3< float > m_odometer_level_arm_
SbgEComModelInfo m_motion_profile_model_info_
Class to handle the device configuration.
float leverArmSecondary[3]
SbgEComOdoRejectionConf m_odometer_rejection_conf_
const SbgVector3< float > & getOdometerLevelArms(void) const
bool m_serial_communication_
SbgEComOdoConf m_odometer_conf_
bool leverArmPrimaryPrecise
void loadOutputConfiguration(const ros::NodeHandle &ref_node_handle, const std::string &ref_key, SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_id)
uint32_t getBaudRate(void) const
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
const SbgEComMagRejectionConf & getMagnetometerRejection(void) const
uint32_t getInputPortAddress(void) const
SbgEComMagRejectionConf m_mag_rejection_conf_
void loadSensorParameters(const ros::NodeHandle &ref_node_handle)
void loadFromRosNodeHandle(const ros::NodeHandle &ref_node_handle)
SbgEComOdometerPinAssignment odometerPinsConf
SbgVector3< float > m_sensor_lever_arm_
const SbgEComModelInfo & getMagnetometerModel(void) const
SbgEComMagCalibMode m_mag_calib_mode_
uint32_t getOutputPortAddress(void) const
sbgIpAddress m_sbg_ip_address_
void loadMagnetometersParameters(const ros::NodeHandle &ref_node_handle)
void loadImuAlignementParameters(const ros::NodeHandle &ref_node_handle)
const SbgEComOdoConf & getOdometerConf(void) const
bool isInterfaceUdp(void) const
const SbgVector3< float > & getSensorLevelArms(void) const
const SbgEComAidingAssignConf & getAidingAssignement(void) const
std::string m_uart_port_name_
const SbgEComOdoRejectionConf & getOdometerRejection(void) const
enum _SbgEComOutputPort SbgEComOutputPort
bool isInterfaceSerial(void) const
SbgEComClass message_class
enum _SbgEComClass SbgEComClass
SbgEComAidingAssignConf m_aiding_assignement_conf_
bool hasParam(const std::string &key) const
SbgEComRejectionMode magneticField
SBG_COMMON_LIB_API sbgIpAddress sbgNetworkIpFromString(const char *pBuffer)
SbgEComRejectionMode position
bool m_ros_standard_output_
const SbgEComModelInfo & getGnssModel(void) const
SbgEComGnssInstallation m_gnss_installation_
SbgEComOutputPort getOutputPort(void) const
SbgEComInitConditionConf m_init_condition_conf_
SbgEComModelInfo m_mag_model_info_
const SbgEComSensorAlignmentInfo & getSensorAlignement(void) const
uint32_t m_rate_frequency_
bool checkRosStandardMessages(void) const