diff --git a/README.md b/README.md index ec117bc..33e9ac7 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,14 @@ +## usage + +To run the sbg_driver using a file as source instead of the device + +``` +roslaunch sbg_driver sbg_file.launch file_path:=path +``` + + +# below, the original README + # sbg_driver [![Build Status](https://build.ros.org/buildStatus/icon?job=Mdev__sbg_driver__ubuntu_bionic_amd64&build=lastBuild)](https://build.ros.org/job/Mdev__sbg_driver__ubuntu_bionic_amd64/lastBuild/) diff --git a/config/sbg_device_uart_default.yaml b/config/sbg_device_uart_default.yaml index cbf2ad6..8f957f3 100644 --- a/config/sbg_device_uart_default.yaml +++ b/config/sbg_device_uart_default.yaml @@ -11,8 +11,13 @@ driver: # Configuration of the device with ROS. confWithRos: false +# File configuration +# can be configured by command line as defined in README +#filesConf: +# path: 'sbg_data.dat' + # Uart configuration -uartConf: +uartConf_: # Port Name portName: "/dev/ttyUSB0" @@ -207,6 +212,8 @@ odom: # 2 Measurement is always accepted rejectMode: 1 + enable: true + # ToDo: event & CAN configuration ############################### Output configuration ############################### @@ -235,11 +242,11 @@ output: # # "ros" : ROS time (default) # "ins_unix" : INS absolute time referenced to UNIX epoch (00:00:00 UTC on 1 January 1970) - time_reference: "ros" + time_reference: "ins_unix" #"ros" # Ros standard output: # Note: If true publish ROS standard messages. - ros_standard: false + ros_standard: true # Frame convention: # Note: If true messages are expressed in the ENU convention. @@ -256,25 +263,25 @@ output: # Status general, clock, com aiding, solution, heave log_status: 200 # Includes IMU status, acc., gyro, temp delta speeds and delta angles values - log_imu_data: 8 + log_imu_data: 3 # Includes roll, pitch, yaw and their accuracies on each axis log_ekf_euler: 0 # Includes the 4 quaternions values log_ekf_quat: 8 # Position and velocities in NED coordinates with the accuracies on each axis - log_ekf_nav: 0 + log_ekf_nav: 8 # Heave, surge and sway and accelerations on each axis for up to 4 points log_ship_motion: 0 # Provides UTC time reference log_utc_time: 200 # Magnetic data with associated accelerometer on each axis - log_mag: 0 + log_mag: 4 # Magnetometer calibration data (raw buffer) - log_mag_calib: 0 + log_mag_calib: 5 # GPS velocities from primary or secondary GPS receiver log_gps1_vel: 0 # GPS positions from primary or secondary GPS receiver - log_gps1_pos: 0 + log_gps1_pos: 14 # GPS true heading from dual antenna system log_gps1_hdt: 0 # GPS 1 raw data for post processing. @@ -287,6 +294,10 @@ output: log_event_c: 0 log_event_d: 0 # Air data - log_air_data: 0 + log_air_data: 36 # Short IMU data log_imu_short: 8 + + +odometry: + enable: true \ No newline at end of file diff --git a/config/sbg_device_udp_default.yaml b/config/sbg_device_udp_default.yaml index 256dab4..195f0d8 100644 --- a/config/sbg_device_udp_default.yaml +++ b/config/sbg_device_udp_default.yaml @@ -199,6 +199,8 @@ odom: # 2 Measurement is always accepted rejectMode: 1 + enable: true + # ToDo: event & CAN configuration ############################### Output configuration ############################### @@ -227,11 +229,11 @@ output: # # "ros" : ROS time (default) # "ins_unix" : INS absolute time referenced to UNIX epoch (00:00:00 UTC on 1 January 1970) - time_reference: "ros" + time_reference: "ins_unix" #"ros" # Ros standard output: # Note: If true publish ROS standard messages. - ros_standard: false + ros_standard: true # Frame convention: # Note: If true messages are expressed in the ENU convention. @@ -248,25 +250,25 @@ output: # Status general, clock, com aiding, solution, heave log_status: 200 # Includes IMU status, acc., gyro, temp delta speeds and delta angles values - log_imu_data: 8 + log_imu_data: 3 # Includes roll, pitch, yaw and their accuracies on each axis log_ekf_euler: 0 # Includes the 4 quaternions values log_ekf_quat: 8 # Position and velocities in NED coordinates with the accuracies on each axis - log_ekf_nav: 0 + log_ekf_nav: 8 # Heave, surge and sway and accelerations on each axis for up to 4 points log_ship_motion: 0 # Provides UTC time reference log_utc_time: 200 # Magnetic data with associated accelerometer on each axis - log_mag: 0 + log_mag: 4 # Magnetometer calibration data (raw buffer) - log_mag_calib: 0 + log_mag_calib: 5 # GPS velocities from primary or secondary GPS receiver log_gps1_vel: 0 # GPS positions from primary or secondary GPS receiver - log_gps1_pos: 0 + log_gps1_pos: 14 # GPS true heading from dual antenna system log_gps1_hdt: 0 # GPS 1 raw data for post processing. @@ -279,6 +281,10 @@ output: log_event_c: 0 log_event_d: 0 # Air data - log_air_data: 0 + log_air_data: 36 # Short IMU data - log_imu_short: 0 + log_imu_short: 8 + + +odometry: + enable: true \ No newline at end of file diff --git a/include/sbg_driver/config_store.h b/include/sbg_driver/config_store.h index db4b2b5..2b1ef10 100644 --- a/include/sbg_driver/config_store.h +++ b/include/sbg_driver/config_store.h @@ -83,6 +83,9 @@ class ConfigStore uint32_t m_in_port_address_; bool m_upd_communication_; + std::string m_sbg_file_; + bool m_files_communication_; + bool m_configure_through_ros_; SbgEComInitConditionConf m_init_condition_conf_; @@ -317,6 +320,20 @@ class ConfigStore */ uint32_t getInputPortAddress(void) const; + /*! + * Check if the interface configuration is to read dump files. + * + * \return True if the interface is dump files, False otherwise. + */ + bool isInterfaceFiles(void) const; + + /*! + * Get the input files. + * + * \return Input files. + */ + const std::string &getFile(void) const; + /*! * Get the initial conditions configuration. * diff --git a/launch/sbg_device.launch b/launch/sbg_device.launch index 29fb59e..19e0539 100644 --- a/launch/sbg_device.launch +++ b/launch/sbg_device.launch @@ -1,5 +1,5 @@ - + \ No newline at end of file diff --git a/launch/sbg_file.launch b/launch/sbg_file.launch new file mode 100644 index 0000000..9d79d30 --- /dev/null +++ b/launch/sbg_file.launch @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/src/config_store.cpp b/src/config_store.cpp index a359f27..218581d 100644 --- a/src/config_store.cpp +++ b/src/config_store.cpp @@ -59,6 +59,11 @@ void ConfigStore::loadCommunicationParameters(const ros::NodeHandle& ref_node_ha m_out_port_address_ = getParameter(ref_node_handle, "ipConf/out_port", 0); m_in_port_address_ = getParameter(ref_node_handle, "ipConf/in_port", 0); } + else if (ref_node_handle.hasParam("filesConf")) + { + m_files_communication_ = true; + m_sbg_file_ = ref_node_handle.param("filesConf/path", std::string("*.*")); + } else { throw ros::Exception("SBG DRIVER - Invalid communication interface parameters."); @@ -240,6 +245,16 @@ uint32_t ConfigStore::getInputPortAddress(void) const return m_in_port_address_; } +bool ConfigStore::isInterfaceFiles(void) const +{ + return m_files_communication_; +} + +const std::string &ConfigStore::getFile(void) const +{ + return m_sbg_file_; +} + const SbgEComInitConditionConf &ConfigStore::getInitialConditions(void) const { return m_init_condition_conf_; diff --git a/src/sbg_device.cpp b/src/sbg_device.cpp index b900803..0f3f99a 100644 --- a/src/sbg_device.cpp +++ b/src/sbg_device.cpp @@ -115,6 +115,10 @@ void SbgDevice::onLogReceived(SbgEComClass msg_class, SbgEComMsgId msg, const Sb // Publish the received SBG log. // m_message_publisher_.publish(msg_class, msg, ref_sbg_data); + + // if Sbg driver is reading from file + if (m_config_store_.isInterfaceFiles()) + usleep(50); } void SbgDevice::loadParameters(void) @@ -142,6 +146,10 @@ void SbgDevice::connect(void) { error_code = sbgInterfaceUdpCreate(&m_sbg_interface_, m_config_store_.getIpAddress(), m_config_store_.getInputPortAddress(), m_config_store_.getOutputPortAddress()); } + else if (m_config_store_.isInterfaceFiles()) + { + error_code = sbgInterfaceFileOpen(&m_sbg_interface_, m_config_store_.getFile().c_str()); + } else { throw ros::Exception("Invalid interface type for the SBG device."); @@ -159,7 +167,8 @@ void SbgDevice::connect(void) throw ros::Exception("SBG_DRIVER - [Init] Unable to initialize the SbgECom protocol - " + std::string(sbgErrorCodeToString(error_code))); } - readDeviceInfo(); + if (!m_config_store_.isInterfaceFiles()) + readDeviceInfo(); } void SbgDevice::readDeviceInfo(void)