10 #include <boost/lexical_cast.hpp> 11 #include <boost/regex.hpp> 12 #include <boost/thread/xtime.hpp> 13 #include <boost/date_time/local_time/local_time.hpp> 25 std::stringstream msg;
26 const boost::posix_time::ptime now = boost::posix_time::second_clock::local_time();
27 boost::posix_time::time_facet *
const f =
new boost::posix_time::time_facet(
"%Y-%m-%d-%H-%M-%S");
28 msg.imbue(std::locale(msg.getloc(),f));
48 std::map<SbgEComMagCalibBandwidth, std::string> SbgDevice::g_mag_calib_bandwidth = {{
SBG_ECOM_MAG_CALIB_HIGH_BW,
"High Bandwidth"},
60 m_ref_node_(ref_node_handle),
61 m_mag_calibration_ongoing_(false),
62 m_mag_calibration_done_(false)
90 ROS_ERROR(
"SBG DRIVER - Unable to close the communication interface.");
147 throw ros::Exception(
"Invalid interface type for the SBG device.");
194 return std::string(version);
221 ref_ros_response.success =
true;
222 ref_ros_response.message =
"Magnetometer calibration is finished. See the output console to get calibration informations.";
226 ref_ros_response.success =
false;
227 ref_ros_response.message =
"Unable to end the calibration.";
237 ref_ros_response.success =
true;
238 ref_ros_response.message =
"Magnetometer calibration process started.";
242 ref_ros_response.success =
false;
243 ref_ros_response.message =
"Unable to start magnetometers calibration.";
249 return ref_ros_response.success;
258 ref_ros_response.success =
false;
259 ref_ros_response.message =
"Magnetometer calibration process is still ongoing, finish it before trying to save it.";
265 ref_ros_response.success =
true;
266 ref_ros_response.message =
"Magnetometer calibration has been uploaded to the device.";
270 ref_ros_response.success =
false;
271 ref_ros_response.message =
"Magnetometer calibration has not been uploaded to the device.";
276 ref_ros_response.success =
false;
277 ref_ros_response.message =
"No magnetometer calibration has been done.";
280 return ref_ros_response.success;
301 ROS_INFO(
"SBG DRIVER [Mag Calib] - Start calibration");
338 ROS_WARN(
"SBG DRIVER [Mag Calib] - Unable to set the magnetometers calibration data to the device : %s",
sbgErrorCodeToString(error_code));
343 ROS_INFO(
"SBG DRIVER [Mag Calib] - Saving data to the device");
351 ROS_ERROR(
"SBG DRIVER [Mag Calib] - The calibration was invalid, it can't be uploaded on the device.");
370 ROS_WARN(
"SBG DRIVER [Mag Calib] - Not enough valid points. Maybe you are moving too fast");
374 ROS_WARN(
"SBG DRIVER [Mag Calib] - Unable to find a calibration solution. Maybe there are too much non static distortions");
378 ROS_WARN(
"SBG DRIVER [Mag Calib] - The magnetic calibration has troubles to correct the magnetometers and inertial frame alignment");
384 ROS_WARN(
"SBG DRIVER [Mag Calib] - Too much roll motion for a 2D magnetic calibration");
388 ROS_WARN(
"SBG DRIVER [Mag Calib] - Too much pitch motion for a 2D magnetic calibration");
395 ROS_WARN(
"SBG DRIVER [Mag Calib] - Not enough roll motion for a 3D magnetic calibration");
399 ROS_WARN(
"SBG DRIVER [Mag Calib] - Not enough pitch motion for a 3D magnetic calibration.");
404 ROS_WARN(
"SBG DRIVER [Mag Calib] - Not enough yaw motion to compute a valid magnetic calibration");
412 ostringstream mag_results_stream;
413 string output_filename;
418 mag_results_stream <<
"SBG DRIVER [Mag Calib]" << endl;
419 mag_results_stream <<
"======= Parameters =======" << endl;
420 mag_results_stream <<
"* CALIB_MODE = " <<
g_mag_calib_mode_[mag_calib_mode] << endl;
423 mag_results_stream <<
"======= Results =======" << endl;
426 mag_results_stream <<
"======= Infos =======" << endl;
428 mag_results_stream <<
"* Mean, Std, Max" << endl;
434 mag_results_stream <<
"* Matrix" << endl;
440 ofstream output_file(output_filename);
441 output_file << mag_results_stream.str();
444 ROS_INFO(
"%s", mag_results_stream.str().c_str());
445 ROS_INFO(
"SBG DRIVER [Mag Calib] - Magnetometers calibration results saved to file %s", output_filename.c_str());
480 ROS_INFO(
"SBG DRIVER [Init] - SBG device is initialized for magnetometers calibration.");
const SbgEComMagCalibMode & getMagnetometerCalibMode(void) const
SbgEComMagCalibConfidence confidence
Helper methods and definitions used to handle version.
MessagePublisher m_message_publisher_
bool isInterfaceUdp(void) const
uint32_t getReadingRateFrequency(void) const
uint8_t productCode[SBG_ECOM_INFO_PRODUCT_CODE_LENGTH]
ros::ServiceServer m_calib_save_service_
SBG_COMMON_LIB_API SbgErrorCode sbgVersionToStringEncoded(uint32_t version, char *pBuffer, uint32_t sizeOfBuffer)
bool processMagCalibration(std_srvs::Trigger::Request &ref_ros_request, std_srvs::Trigger::Response &ref_ros_response)
void exportMagCalibrationResults(void) const
SbgEComMagCalibQuality quality
void periodicHandle(void)
void onLogReceived(SbgEComClass msg_class, SbgEComMsgId msg, const SbgBinaryLogData &ref_sbg_data)
static const char * sbgErrorCodeToString(SbgErrorCode errorCode)
bool isInterfaceSerial(void) const
enum _SbgEComMagCalibMode SbgEComMagCalibMode
SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceSerialDestroy(SbgInterface *pHandle)
SbgErrorCode sbgEComCmdMagStartCalib(SbgEComHandle *pHandle, SbgEComMagCalibMode mode, SbgEComMagCalibBandwidth bandwidth)
bool uploadMagCalibrationToDevice(void)
bool checkConfigWithRos(void) const
uint32_t getBaudRate(void) const
enum _SbgEComMagCalibBandwidth SbgEComMagCalibBandwidth
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
uint32_t getUpdateFrequency(void) const
#define SBG_ECOM_MAG_CALIB_Y_MOTION_ISSUE
ros::NodeHandle & m_ref_node_
void publish(SbgEComClass sbg_msg_class, SbgEComMsgId sbg_msg_id, const SbgBinaryLogData &ref_sbg_log)
void displayMagCalibrationStatusResult(void) const
SBG_INLINE float sbgRadToDegF(float angle)
SbgInterface m_sbg_interface_
static std::map< SbgEComMagCalibQuality, std::string > g_mag_calib_quality_
ConfigStore m_config_store_
void saveConfiguration(void)
uint32_t getInputPortAddress(void) const
#define SBG_ECOM_MAG_CALIB_TOO_MUCH_DISTORTIONS
SbgErrorCode sbgInterfaceUdpCreate(SbgInterface *pHandle, sbgIpAddress remoteAddr, uint32 remotePort, uint32 localPort)
sbgIpAddress getIpAddress(void) const
SbgEComHandle m_com_handle_
void loadParameters(void)
SbgErrorCode sbgEComHandle(SbgEComHandle *pHandle)
SBG_COMMON_LIB_API SbgErrorCode sbgInterfaceSerialCreate(SbgInterface *pHandle, const char *deviceName, uint32_t baudRate)
std::string getVersionAsString(uint32 sbg_version_enc) const
void readDeviceInfo(void)
void loadFromRosNodeHandle(const ros::NodeHandle &ref_node_handle)
bool startMagCalibration(void)
const std::string & getUartPortName(void) const
static std::map< SbgEComMagCalibConfidence, std::string > g_mag_calib_confidence_
SbgEComMagCalibResults m_magCalibResults
SbgErrorCode sbgEComSetReceiveLogCallback(SbgEComHandle *pHandle, SbgEComReceiveLogFunc pReceiveLogCallback, void *pUserArg)
static std::map< SbgEComMagCalibMode, std::string > g_mag_calib_mode_
#define SBG_ECOM_MAG_CALIB_NOT_ENOUGH_POINTS
SbgErrorCode sbgEComCmdMagComputeCalib(SbgEComHandle *pHandle, SbgEComMagCalibResults *pCalibResults)
std::string timeToStr(ros::WallTime ros_t)
uint32_t m_rate_frequency_
bool endMagCalibration(void)
void applyConfiguration(const ConfigStore &ref_config_store)
#define SBG_ECOM_MAG_CALIB_X_MOTION_ISSUE
SbgErrorCode sbgEComCmdMagSetCalibData(SbgEComHandle *pHandle, const float offset[3], const float matrix[9])
#define SBG_UNUSED_PARAMETER(x)
static std::map< SbgEComMagCalibBandwidth, std::string > g_mag_calib_bandwidth
void initPublishers(ros::NodeHandle &ref_ros_node_handle, const ConfigStore &ref_config_store)
#define SBG_ECOM_MAG_CALIB_Z_MOTION_ISSUE
enum _SbgEComClass SbgEComClass
SbgErrorCode sbgEComCmdGetInfo(SbgEComHandle *pHandle, SbgEComDeviceInfo *pInfo)
Handle a connected SBG device.
bool saveMagCalibration(std_srvs::Trigger::Request &ref_ros_request, std_srvs::Trigger::Response &ref_ros_response)
bool m_mag_calibration_done_
#define SBG_ECOM_MAG_CALIB_ALIGNMENT_ISSUE
void initDeviceForReceivingData(void)
void initDeviceForMagCalibration(void)
bool m_mag_calibration_ongoing_
void initPublishers(void)
SbgErrorCode sbgEComInit(SbgEComHandle *pHandle, SbgInterface *pInterface)
const SbgEComMagCalibBandwidth & getMagnetometerCalibBandwidth(void) const
enum _SbgErrorCode SbgErrorCode
SbgErrorCode sbgEComClose(SbgEComHandle *pHandle)
uint32_t getOutputPortAddress(void) const
SbgErrorCode sbgInterfaceUdpDestroy(SbgInterface *pHandle)
static SbgErrorCode onLogReceivedCallback(SbgEComHandle *p_handle, SbgEComClass msg_class, SbgEComMsgId msg, const SbgBinaryLogData *p_log_data, void *p_user_arg)
ros::ServiceServer m_calib_service_