diff --git a/CMakeLists.txt b/CMakeLists.txt index 66369ae..1b6474d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,73 +1,37 @@ cmake_minimum_required(VERSION 2.8.3) project(imu_vn_100) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall") - -find_package(catkin REQUIRED COMPONENTS - roscpp - sensor_msgs - diagnostic_updater -) -find_package(Boost REQUIRED) - -catkin_package( - INCLUDE_DIRS include vncpplib/include - LIBRARIES imu_vn_100 - CATKIN_DEPENDS diagnostic_updater roscpp sensor_msgs - DEPENDS Boost -) - -include_directories( - include - vncpplib/include - ${catkin_INCLUDE_DIRS} -) - -add_library(imu_vn_100 - vncpplib/src/arch/linux/vncp_services.c - vncpplib/src/vndevice.c - vncpplib/src/vn100.c - src/imu_vn_100.cpp -) -target_link_libraries(imu_vn_100 - ${catkin_LIBRARIES} -) -add_dependencies(imu_vn_100 - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) - -add_executable(imu_vn_100_node - src/imu_vn_100_node.cpp) -target_link_libraries(imu_vn_100_node - ${catkin_LIBRARIES} - imu_vn_100 -) -add_dependencies(imu_vn_100_node - ${${PROJECT_NAME}_EXPORTED_TARGETS} - ${catkin_EXPORTED_TARGETS} -) - -install(TARGETS imu_vn_100 imu_vn_100_node - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} - FILES_MATCHING PATTERN "*.h**" -) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 11) +endif() + +find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs diagnostic_updater) + +catkin_package(CATKIN_DEPENDS + diagnostic_updater + roscpp + sensor_msgs) + +add_executable(${PROJECT_NAME}_node + src/imu_vn_100.cpp + vncpplib/src/arch/linux/vncp_services.c + vncpplib/src/vn100.c + vncpplib/src/vndevice.c) +target_include_directories(${PROJECT_NAME}_node + PUBLIC src vncpplib/include ${catkin_INCLUDE_DIRS}) +target_link_libraries(${PROJECT_NAME}_node PUBLIC ${catkin_LIBRARIES}) + +install(TARGETS ${PROJECT_NAME}_node + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) install(FILES vncpplib/include/vn100.h - vncpplib/include/vncp_services.h - vncpplib/include/vndevice.h - vncpplib/include/vn_errorCodes.h - vncpplib/include/vn_kinematics.h - vncpplib/include/vn_linearAlgebra.h - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) + vncpplib/include/vncp_services.h + vncpplib/include/vndevice.h + vncpplib/include/vn_errorCodes.h + vncpplib/include/vn_kinematics.h + vncpplib/include/vn_linearAlgebra.h + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/README.md b/README.md index 4ff95b4..9de26d4 100644 --- a/README.md +++ b/README.md @@ -45,13 +45,10 @@ The baud rate of the serial port. The available baud rates can be checked on the The rate of the IMU data. ``` -enable_magn (bool, default: true) -enable_press (bool, default: true) -enable_temp (bool, default: true) -enable_rpy (bool, default: false) +enable_mag_pres (bool, default: true) ``` -Enable other possible messages that the driver is available to send. Note that the frequency of the data for each of these messages will be the same with the IMU data, if the topic is enabled. +Calibrated magnetic (compensated), temperature, and pressure measurements. `sync_rate` (`int`, `20`) @@ -68,9 +65,13 @@ Set serial port for binary messages to one of: * `1` - Serial port 1 * `2` - Serial port 2 -`imu_compensated` (`boolean`, `default: true`) +`compensated` (`boolean`, `default: true`) -Use *compensated* IMU measurements (i.e. angular velocity, linear acceleration, magnetic field). +Use *compensated* IMU measurements (i.e. angular velocity, linear acceleration). + +`time_alpha` (`double`, `default: 0`) + +Used when computing time stamp. `t1 = a * dt_ros + (1 - a) * dt_dev + t0 `vpe_enable` (`boolean`, `default: true`) @@ -102,11 +103,11 @@ Set VPE tuning mode to one of: `imu/imu` (`sensor_msgs/Imu`) -If `imu_compensated` is `false`, the default, then the message contains the *uncompensated* (for the definition of UNCOMPENSATED, please refer to the [user manual](http://www.vectornav.com/docs/default-source/documentation/vn-100-documentation/UM001.pdf?sfvrsn=10)) angular velocity and linear acceleration. Otherwise both are *compensated*. +If `compensated` is `false`, the default, then the message contains the *uncompensated* (for the definition of UNCOMPENSATED, please refer to the [user manual](http://www.vectornav.com/docs/default-source/documentation/vn-100-documentation/UM001.pdf?sfvrsn=10)) angular velocity and linear acceleration. Otherwise both are *compensated*. `imu/magnetic_field` (`sensor_msgs/MagneticField`) -Magnetic field. If `imu_compensated` is `false` then it is *uncompensated* otherwise it is *compensated*. +Magnetic field. Always *compensated*. `imu/pressure` (`sensor_msgs/FluidPressure`) @@ -116,16 +117,12 @@ Pressure. Temperature in degree Celsius -`imu/rpy` (`geometry_msgs/Vector3Stamped`) - -Estimated roll (`x`), pitch (`y`) and yaw (`z`) angles in radians given as a 3,2,1 Euler angle rotation sequence describing the orientation of the sensor with respect to the inertial North East Down (NED) frame. - **Node** With the provided launch file, do ``` -roslaunch imu_vn_100 vn_100_cont.launch +roslaunch imu_vn_100 imu.launch ``` ## FAQ diff --git a/launch/vn_100_cont.launch b/launch/imu.launch similarity index 75% rename from launch/vn_100_cont.launch rename to launch/imu.launch index f7c00a1..29fe39e 100644 --- a/launch/vn_100_cont.launch +++ b/launch/imu.launch @@ -9,14 +9,15 @@ - + - + + @@ -26,9 +27,7 @@ - - - + @@ -37,13 +36,14 @@ - - - - - + + + + + + diff --git a/package.xml b/package.xml index 70e03f4..b0b3df7 100644 --- a/package.xml +++ b/package.xml @@ -5,6 +5,7 @@ The imu_vn_100 package Ke Sun + Chao Qu Apache 2.0 Ke Sun @@ -15,9 +16,6 @@ roscpp sensor_msgs - - - diff --git a/src/imu_vn_100.cpp b/src/imu_vn_100.cpp index a23ccea..446dfef 100644 --- a/src/imu_vn_100.cpp +++ b/src/imu_vn_100.cpp @@ -14,35 +14,59 @@ * limitations under the License. */ -#include +#include "imu_vn_100.h" #include +#include +#include +#include +#include +#include namespace imu_vn_100 { -// LESS HACK IS STILL HACK ImuVn100* imu_vn_100_ptr; -using geometry_msgs::Vector3Stamped; +using namespace geometry_msgs; +using namespace sensor_msgs; -using sensor_msgs::Imu; -using sensor_msgs::MagneticField; -using sensor_msgs::FluidPressure; -using sensor_msgs::Temperature; +using VnErrorCode = VN_ERROR_CODE; +void VnEnsure(const VnErrorCode& error_code); -void RosVector3FromVnVector3(geometry_msgs::Vector3& ros_vec3, - const VnVector3& vn_vec3); -void RosQuaternionFromVnQuaternion(geometry_msgs::Quaternion& ros_quat, - const VnQuaternion& vn_quat); +std::ostream& operator<<(std::ostream& os, const VnVector3& v) { + os << "(" << v.c0 << ", " << v.c1 << ", " << v.c2 << ")"; + return os; +} + +std::string VectorToString(const std::vector& vec) { + std::string str; + for (const auto& v : vec) { + str += v + " | "; + } + return str; +} + +Vector3 RosVecFromVnVec(const VnVector3& u) { + Vector3 v; + v.x = u.c0; + v.y = u.c1; + v.z = u.c2; + return v; +} + +Quaternion RosQuatFromVnQuat(const VnQuaternion& p) { + Quaternion q; + q.x = p.x; + q.y = p.y; + q.z = p.z; + q.w = p.w; + return q; +} void AsyncListener(void* sender, VnDeviceCompositeData* data) { imu_vn_100_ptr->PublishData(*data); } -constexpr int ImuVn100::kBaseImuRate; -constexpr int ImuVn100::kDefaultImuRate; -constexpr int ImuVn100::kDefaultSyncOutRate; - void ImuVn100::SyncInfo::Update(const unsigned sync_count, const ros::Time& sync_time) { if (rate <= 0) return; @@ -58,14 +82,12 @@ bool ImuVn100::SyncInfo::SyncEnabled() const { return rate > 0; } void ImuVn100::SyncInfo::FixSyncRate() { // Check the sync out rate if (SyncEnabled()) { - if (ImuVn100::kBaseImuRate % rate != 0) { - rate = ImuVn100::kBaseImuRate / (ImuVn100::kBaseImuRate / rate); + if (kBaseImuRate % rate != 0) { + rate = kBaseImuRate / (kBaseImuRate / rate); ROS_INFO("Set SYNC_OUT_RATE to %d", rate); } skip_count = - (std::floor(ImuVn100::kBaseImuRate / static_cast(rate) + - 0.5f)) - - 1; + (std::floor(kBaseImuRate / static_cast(rate) + 0.5f)) - 1; if (pulse_width_us > 10000) { ROS_INFO("Sync out pulse with is over 10ms. Reset to 1ms"); @@ -84,6 +106,7 @@ ImuVn100::ImuVn100(const ros::NodeHandle& pnh) frame_id_(std::string("imu")) { Initialize(); imu_vn_100_ptr = this; + ROS_INFO_STREAM("Diagnostic period: " << updater_.getPeriod() << " s"); } ImuVn100::~ImuVn100() { Disconnect(); } @@ -108,11 +131,6 @@ void ImuVn100::LoadParameters() { pnh_.param("baudrate", baudrate_, 115200); pnh_.param("imu_rate", imu_rate_, kDefaultImuRate); - pnh_.param("enable_mag", enable_mag_, true); - pnh_.param("enable_pres", enable_pres_, true); - pnh_.param("enable_temp", enable_temp_, true); - pnh_.param("enable_rpy", enable_rpy_, false); - pnh_.param("sync_rate", sync_info_.rate, kDefaultSyncOutRate); pnh_.param("sync_pulse_width_us", sync_info_.pulse_width_us, 1000); @@ -120,10 +138,12 @@ void ImuVn100::LoadParameters() { pnh_.param("binary_async_mode", binary_async_mode_, BINARY_ASYNC_MODE_SERIAL_2); - pnh_.param("imu_compensated", imu_compensated_, false); + pnh_.param("compensated", compensated_, false); + pnh_.param("time_alpha", time_alpha_, 0.0); + ROS_INFO("time_alpha: %f", time_alpha_); + time_alpha_ = std::max(0.0, std::min(time_alpha_, 1.0)); pnh_.param("vpe/enable", vpe_enable_, true); - pnh_.param("vpe/heading_mode", vpe_heading_mode_, 1); pnh_.param("vpe/filtering_mode", vpe_filtering_mode_, 1); pnh_.param("vpe/tuning_mode", vpe_tuning_mode_, 1); @@ -131,44 +151,58 @@ void ImuVn100::LoadParameters() { pnh_.param("vpe/mag_tuning/base_tuning/x", vpe_mag_base_tuning_.c0, 4.0); pnh_.param("vpe/mag_tuning/base_tuning/y", vpe_mag_base_tuning_.c1, 4.0); pnh_.param("vpe/mag_tuning/base_tuning/z", vpe_mag_base_tuning_.c2, 4.0); - pnh_.param("vpe/mag_tuning/adaptive_tuning/x", vpe_mag_adaptive_tuning_.c0, 5.0); - pnh_.param("vpe/mag_tuning/adaptive_tuning/y", vpe_mag_adaptive_tuning_.c1, 5.0); - pnh_.param("vpe/mag_tuning/adaptive_tuning/z", vpe_mag_adaptive_tuning_.c2, 5.0); - pnh_.param("vpe/mag_tuning/adaptive_filtering/x", vpe_mag_adaptive_filtering_.c0, 5.5); - pnh_.param("vpe/mag_tuning/adaptive_filtering/y", vpe_mag_adaptive_filtering_.c1, 5.5); - pnh_.param("vpe/mag_tuning/adaptive_filtering/z", vpe_mag_adaptive_filtering_.c2, 5.5); + pnh_.param("vpe/mag_tuning/adaptive_tuning/x", vpe_mag_adaptive_tuning_.c0, + 5.0); + pnh_.param("vpe/mag_tuning/adaptive_tuning/y", vpe_mag_adaptive_tuning_.c1, + 5.0); + pnh_.param("vpe/mag_tuning/adaptive_tuning/z", vpe_mag_adaptive_tuning_.c2, + 5.0); + pnh_.param("vpe/mag_tuning/adaptive_filtering/x", + vpe_mag_adaptive_filtering_.c0, 5.5); + pnh_.param("vpe/mag_tuning/adaptive_filtering/y", + vpe_mag_adaptive_filtering_.c1, 5.5); + pnh_.param("vpe/mag_tuning/adaptive_filtering/z", + vpe_mag_adaptive_filtering_.c2, 5.5); pnh_.param("vpe/accel_tuning/base_tuning/x", vpe_accel_base_tuning_.c0, 5.0); pnh_.param("vpe/accel_tuning/base_tuning/y", vpe_accel_base_tuning_.c1, 5.0); pnh_.param("vpe/accel_tuning/base_tuning/z", vpe_accel_base_tuning_.c2, 5.0); - pnh_.param("vpe/accel_tuning/adaptive_tuning/x", vpe_accel_adaptive_tuning_.c0, 3.0); - pnh_.param("vpe/accel_tuning/adaptive_tuning/y", vpe_accel_adaptive_tuning_.c1, 3.0); - pnh_.param("vpe/accel_tuning/adaptive_tuning/z", vpe_accel_adaptive_tuning_.c2, 3.0); - pnh_.param("vpe/accel_tuning/adaptive_filtering/x", vpe_accel_adaptive_filtering_.c0, 4.0); - pnh_.param("vpe/accel_tuning/adaptive_filtering/y", vpe_accel_adaptive_filtering_.c1, 4.0); - pnh_.param("vpe/accel_tuning/adaptive_filtering/z", vpe_accel_adaptive_filtering_.c2, 4.0); + pnh_.param("vpe/accel_tuning/adaptive_tuning/x", + vpe_accel_adaptive_tuning_.c0, 3.0); + pnh_.param("vpe/accel_tuning/adaptive_tuning/y", + vpe_accel_adaptive_tuning_.c1, 3.0); + pnh_.param("vpe/accel_tuning/adaptive_tuning/z", + vpe_accel_adaptive_tuning_.c2, 3.0); + pnh_.param("vpe/accel_tuning/adaptive_filtering/x", + vpe_accel_adaptive_filtering_.c0, 4.0); + pnh_.param("vpe/accel_tuning/adaptive_filtering/y", + vpe_accel_adaptive_filtering_.c1, 4.0); + pnh_.param("vpe/accel_tuning/adaptive_filtering/z", + vpe_accel_adaptive_filtering_.c2, 4.0); FixImuRate(); sync_info_.FixSyncRate(); } -void ImuVn100::CreateDiagnosedPublishers() { - imu_rate_double_ = imu_rate_; - pd_imu_.Create(pnh_, "imu", updater_, imu_rate_double_); - if (enable_mag_) { - pd_mag_.Create(pnh_, "magnetic_field", updater_, - imu_rate_double_); - } - if (enable_pres_) { - pd_pres_.Create(pnh_, "fluid_pressure", updater_, - imu_rate_double_); - } - if (enable_temp_) { - pd_temp_.Create(pnh_, "temperature", updater_, - imu_rate_double_); - } - if (enable_rpy_) { - pd_rpy_.Create(pnh_, "rpy", updater_, imu_rate_double_); +void ImuVn100::CreatePublishers() { + imu_rate_double_ = static_cast(imu_rate_); + + pub_dt_ = pnh_.advertise("dt", 1); + pub_dnow_ = pnh_.advertise("dnow", 1); + + pub_imu_.Advertise(pnh_, "imu", updater_, imu_rate_double_); + ROS_INFO("Publish imu to %s", pub_imu_.pub.getTopic().c_str()); + + if (pnh_.param("enable_mag_pres", true)) { + pub_mag_.Advertise(pnh_, "magnetic_field", updater_, + imu_rate_double_); + ROS_INFO("Publish magnetic filed to %s", pub_mag_.pub.getTopic().c_str()); + pub_pres_.Advertise(pnh_, "fluid_pressure", updater_, + imu_rate_double_); + ROS_INFO("Publish pressure to %s", pub_pres_.pub.getTopic().c_str()); + pub_temp_.Advertise(pnh_, "temperature", updater_, + imu_rate_double_); + ROS_INFO("Publish temperature to %s", pub_temp_.pub.getTopic().c_str()); } } @@ -227,9 +261,9 @@ void ImuVn100::Initialize() { if (!binary_output_) { ROS_INFO("Set Communication Protocol Control Register (id:30)."); VnEnsure(vn100_setCommunicationProtocolControl( - &imu_, SERIALCOUNT_SYNCOUT_COUNT, SERIALSTATUS_OFF, SPICOUNT_NONE, - SPISTATUS_OFF, SERIALCHECKSUM_8BIT, SPICHECKSUM_8BIT, ERRORMODE_SEND, - true)); + &imu_, SERIALCOUNT_SYNCOUT_COUNT, SERIALSTATUS_OFF, SPICOUNT_NONE, + SPISTATUS_OFF, SERIALCHECKSUM_8BIT, SPICHECKSUM_8BIT, ERRORMODE_SEND, + true)); } } @@ -238,82 +272,56 @@ void ImuVn100::Initialize() { uint8_t vpe_filtering_mode; uint8_t vpe_tuning_mode; VnEnsure(vn100_getVpeControl(&imu_, &vpe_enable, &vpe_heading_mode, - &vpe_filtering_mode, &vpe_tuning_mode)); - ROS_INFO("Default VPE enable: %hhu", vpe_enable); - ROS_INFO("Default VPE heading mode: %hhu", vpe_heading_mode); - ROS_INFO("Default VPE filtering mode: %hhu", vpe_filtering_mode); - ROS_INFO("Default VPE tuning mode: %hhu", vpe_tuning_mode); - if (vpe_enable != vpe_enable_ || - vpe_heading_mode != vpe_heading_mode_ || + &vpe_filtering_mode, &vpe_tuning_mode)); + ROS_INFO( + "Default VPE enable: %hhu, heading mode: %hhu, filtering mode: %hhu, " + "tuning mode: %hhu", + vpe_enable, vpe_heading_mode, vpe_filtering_mode, vpe_tuning_mode); + + if (vpe_enable != vpe_enable_ || vpe_heading_mode != vpe_heading_mode_ || vpe_filtering_mode != vpe_filtering_mode_ || vpe_tuning_mode != vpe_tuning_mode_) { - vpe_enable = vpe_enable_; - vpe_heading_mode = vpe_heading_mode_; - vpe_filtering_mode = vpe_filtering_mode_; - vpe_tuning_mode = vpe_tuning_mode_; - ROS_INFO("Setting VPE enable: %hhu", vpe_enable); - ROS_INFO("Setting VPE heading mode: %hhu", vpe_heading_mode); - ROS_INFO("Setting VPE filtering mode: %hhu", vpe_filtering_mode); - ROS_INFO("Setting VPE tuning mode: %hhu", vpe_tuning_mode); - VnEnsure(vn100_setVpeControl( - &imu_, - vpe_enable, - vpe_heading_mode, - vpe_filtering_mode, - vpe_tuning_mode, - true)); + vpe_enable = vpe_enable_; + vpe_heading_mode = vpe_heading_mode_; + vpe_filtering_mode = vpe_filtering_mode_; + vpe_tuning_mode = vpe_tuning_mode_; + ROS_INFO( + "Setting VPE enable: %hhu, heading mode: %hhu, filtering mode: %hhu, " + "tuning mode: %hhu", + vpe_enable, vpe_heading_mode, vpe_filtering_mode, vpe_tuning_mode); + VnEnsure(vn100_setVpeControl(&imu_, vpe_enable, vpe_heading_mode, + vpe_filtering_mode, vpe_tuning_mode, true)); } if (vpe_enable_) { - ROS_INFO( - "Setting VPE MagnetometerBasicTuning BaseTuning (%f, %f, %f)", - vpe_mag_base_tuning_.c0, - vpe_mag_base_tuning_.c1, - vpe_mag_base_tuning_.c2); - ROS_INFO( - "Setting VPE MagnetometerBasicTuning AdaptiveTuning (%f, %f, %f)", - vpe_mag_adaptive_tuning_.c0, - vpe_mag_adaptive_tuning_.c1, - vpe_mag_adaptive_tuning_.c2); - ROS_INFO( - "Setting VPE MagnetometerBasicTuning AdaptiveFiltering (%f, %f, %f)", - vpe_mag_adaptive_filtering_.c0, - vpe_mag_adaptive_filtering_.c1, - vpe_mag_adaptive_filtering_.c2); - VnEnsure(vn100_setVpeMagnetometerBasicTuning( - &imu_, - vpe_mag_base_tuning_, - vpe_mag_adaptive_tuning_, - vpe_mag_adaptive_filtering_, - true)); - - ROS_INFO( - "Setting VPE AccelerometerBasicTuning BaseTuning (%f, %f, %f)", - vpe_accel_base_tuning_.c0, - vpe_accel_base_tuning_.c1, - vpe_accel_base_tuning_.c2); - ROS_INFO( - "Setting VPE AccelerometerBasicTuning AdaptiveTuning (%f, %f, %f)", - vpe_accel_adaptive_tuning_.c0, - vpe_accel_adaptive_tuning_.c1, - vpe_accel_adaptive_tuning_.c2); - ROS_INFO( - "Setting VPE AccelerometerBasicTuning AdaptiveFiltering (%f, %f, %f)", - vpe_accel_adaptive_filtering_.c0, - vpe_accel_adaptive_filtering_.c1, - vpe_accel_adaptive_filtering_.c2); - VnEnsure(vn100_setVpeAccelerometerBasicTuning( - &imu_, - vpe_accel_base_tuning_, - vpe_accel_adaptive_tuning_, - vpe_accel_adaptive_filtering_, - true)); + ROS_INFO_STREAM("Setting VPE MagnetometerBasicTuning BaseTuning " + << vpe_mag_base_tuning_); + ROS_INFO_STREAM("Setting VPE MagnetometerBasicTuning AdaptiveTuning " + << vpe_mag_adaptive_tuning_); + ROS_INFO_STREAM("Setting VPE MagnetometerBasicTuning AdaptiveFiltering " + << vpe_mag_adaptive_filtering_); + + VnEnsure(vn100_setVpeMagnetometerBasicTuning( + &imu_, vpe_mag_base_tuning_, vpe_mag_adaptive_tuning_, + vpe_mag_adaptive_filtering_, true)); + + ROS_INFO_STREAM("Setting VPE AccelerometerBasicTuning BaseTuning " + << vpe_accel_base_tuning_); + ROS_INFO_STREAM("Setting VPE AccelerometerBasicTuning AdaptiveTuning " + << vpe_accel_adaptive_tuning_); + ROS_INFO_STREAM("Setting VPE AccelerometerBasicTuning AdaptiveFiltering " + << vpe_accel_adaptive_filtering_); + + VnEnsure(vn100_setVpeAccelerometerBasicTuning( + &imu_, vpe_accel_base_tuning_, vpe_accel_adaptive_tuning_, + vpe_accel_adaptive_filtering_, true)); } - CreateDiagnosedPublishers(); + CreatePublishers(); - auto hardware_id = std::string("vn100-") + std::string(model_number_buffer) + - std::string(serial_number_buffer); + const auto hardware_id = std::string("vn100-") + + std::string(model_number_buffer) + + std::string(serial_number_buffer); updater_.setHardwareID(hardware_id); } @@ -326,80 +334,30 @@ void ImuVn100::Stream(bool async) { if (binary_output_) { // Set the binary output data type and data rate - uint16_t grp1 = BG1_QTN | BG1_SYNC_IN_CNT; - std::list sgrp1 = {"BG1_QTN", "BG1_SYNC_IN_CNT"}; - if (enable_rpy_) { - grp1 |= BG1_YPR; - sgrp1.push_back("BG1_YPR"); - } - uint16_t grp3 = BG3_NONE; - std::list sgrp3; - uint16_t grp5 = BG5_NONE; - std::list sgrp5; - if (imu_compensated_) { - grp1 |= BG1_ACCEL | BG1_ANGULAR_RATE; + // Add BG1_TIME_STARTUP to have more accurate time stamp + uint16_t grp1 = BG1_QTN | BG1_SYNC_IN_CNT | BG1_TIME_STARTUP; + std::vector sgrp1 = {"BG1_QTN", "BG1_SYNC_IN_CNT", + "BG1_TIME_STARTUP"}; + + if (compensated_) { + grp1 |= BG1_ACCEL | BG1_ANGULAR_RATE; sgrp1.push_back("BG1_ACCEL"); sgrp1.push_back("BG1_ANGULAR_RATE"); - if (enable_mag_) { - grp3 |= BG3_MAG; - sgrp3.push_back("BG3_MAG"); - } } else { - grp1 |= BG1_IMU; + grp1 |= BG1_IMU; sgrp1.push_back("BG1_IMU"); - if (enable_mag_) { - grp3 |= BG3_UNCOMP_MAG; - sgrp3.push_back("BG3_UNCOMP_MAG"); - } - } - if (enable_temp_) { - grp3 |= BG3_TEMP; - sgrp3.push_back("BG3_TEMP"); } - if (enable_pres_) { - grp3 |= BG3_PRES; - sgrp3.push_back("BG3_PRES"); - } - if (!sgrp1.empty()) { - std::stringstream ss; - std::copy( - sgrp1.begin(), - sgrp1.end(), - std::ostream_iterator(ss, "|") - ); - std::string s = ss.str(); - s.pop_back(); - ROS_INFO("Streaming #1: %s", s.c_str()); - } - if (!sgrp3.empty()) { - std::stringstream ss; - std::copy( - sgrp3.begin(), - sgrp3.end(), - std::ostream_iterator(ss, "|") - ); - std::string s = ss.str(); - s.pop_back(); - ROS_INFO("Streaming #3: %s", s.c_str()); - } - if (!sgrp5.empty()) { - std::stringstream ss; - std::copy( - sgrp5.begin(), - sgrp5.end(), - std::ostream_iterator(ss, "|") - ); - std::string s = ss.str(); - s.pop_back(); - ROS_INFO("Streaming #5: %s", s.c_str()); + + if (pub_mag_) { + grp1 |= BG1_MAG_PRES; + sgrp1.push_back("BG1_MAG_PRES"); } + + ROS_INFO("Streaming #1: %s", VectorToString(sgrp1).c_str()); + VnEnsure(vn100_setBinaryOutput1Configuration( - &imu_, - binary_async_mode_, - kBaseImuRate / imu_rate_, - grp1, grp3, grp5, - true - )); + &imu_, binary_async_mode_, kBaseImuRate / imu_rate_, grp1, BG3_NONE, + BG5_NONE, true)); } else { // Set the ASCII output data type and data rate // ROS_INFO("Configure the output data type and frequency (id: 6 & 7)"); @@ -423,73 +381,99 @@ void ImuVn100::Stream(bool async) { VnEnsure(vn100_resumeAsyncOutputs(&imu_, true)); } -void ImuVn100::Resume(bool need_reply) { - vn100_resumeAsyncOutputs(&imu_, need_reply); -} - -void ImuVn100::Idle(bool need_reply) { - vn100_pauseAsyncOutputs(&imu_, need_reply); -} - void ImuVn100::Disconnect() { - // TODO: why reset the device? + ROS_INFO("Reset and disconnect from imu"); vn100_reset(&imu_); vn100_disconnect(&imu_); } void ImuVn100::PublishData(const VnDeviceCompositeData& data) { - sensor_msgs::Imu imu_msg; - imu_msg.header.stamp = ros::Time::now(); + // Handle timestamp + const auto ros_time_now = ros::Time::now(); + if (dev_time_last_ == 0) { + ros_time_last_ = ros_time_now; + stamp_last_ = ros_time_last_; + dev_time_last_ = data.timeStartup; + } + + // delta time from device + const int64_t dt_dev = data.timeStartup - dev_time_last_; + // delta time from ros + const int64_t dt_ros = (ros_time_now - ros_time_last_).toNSec(); + // filtered delta time + const int64_t dt_filtered = dt_ros * time_alpha_ + dt_dev * (1 - time_alpha_); + ROS_DEBUG_STREAM("dt_dev: " << dt_dev << ", dt_ros: " << dt_ros + << ", dt_filtered: " << dt_filtered); + + ros::Duration dt; + dt.fromNSec(dt_filtered); + const ros::Time stamp = stamp_last_ + dt; + + if (dt_dev < 0) { + // This should never happen, but we check for it anyway + ROS_WARN_STREAM("dt: " << dt_dev << " ns, startup: " << data.timeStartup + << ", zero: " << dev_time_last_); + } + + { + std_msgs::Float64 msg; + msg.data = dt.toSec(); + pub_dt_.publish(msg); + } + { + std_msgs::Float64 msg; + msg.data = (ros_time_now - stamp).toSec(); + pub_dnow_.publish(msg); + } + + ros_time_last_ = ros_time_now; + dev_time_last_ = data.timeStartup; + stamp_last_ = stamp; + + // Fill in data + Imu imu_msg; + imu_msg.header.stamp = stamp; imu_msg.header.frame_id = frame_id_; - if (imu_compensated_) { - RosVector3FromVnVector3(imu_msg.linear_acceleration, data.acceleration); - RosVector3FromVnVector3(imu_msg.angular_velocity, data.angularRate); + if (compensated_) { + imu_msg.linear_acceleration = RosVecFromVnVec(data.acceleration); + imu_msg.angular_velocity = RosVecFromVnVec(data.angularRate); } else { // NOTE: The IMU angular velocity and linear acceleration outputs are // swapped. And also why are they different? - RosVector3FromVnVector3(imu_msg.angular_velocity, - data.accelerationUncompensated); - RosVector3FromVnVector3(imu_msg.linear_acceleration, - data.angularRateUncompensated); - } - if (binary_output_) { - RosQuaternionFromVnQuaternion(imu_msg.orientation, data.quaternion); - } - pd_imu_.Publish(imu_msg); - - if (enable_rpy_) { - Vector3Stamped rpy_msg; - rpy_msg.header= imu_msg.header; - rpy_msg.vector.z = data.ypr.yaw * M_PI/180.0; - rpy_msg.vector.y = data.ypr.pitch * M_PI/180.0; - rpy_msg.vector.x = data.ypr.roll * M_PI/180.0; - pd_rpy_.Publish(rpy_msg); + imu_msg.angular_velocity = RosVecFromVnVec(data.accelerationUncompensated); + imu_msg.linear_acceleration = + RosVecFromVnVec(data.angularRateUncompensated); } - if (enable_mag_) { - sensor_msgs::MagneticField mag_msg; - mag_msg.header = imu_msg.header; - RosVector3FromVnVector3(mag_msg.magnetic_field, data.magnetic); - pd_mag_.Publish(mag_msg); + if (binary_output_) { + imu_msg.orientation = RosQuatFromVnQuat(data.quaternion); } - if (enable_pres_) { - sensor_msgs::FluidPressure pres_msg; - pres_msg.header = imu_msg.header; - pres_msg.fluid_pressure = data.pressure; - pd_pres_.Publish(pres_msg); - } + pub_imu_.Publish(imu_msg); - if (enable_temp_) { - sensor_msgs::Temperature temp_msg; - temp_msg.header = imu_msg.header; - temp_msg.temperature = data.temperature; - pd_temp_.Publish(temp_msg); + if (pub_mag_) { + { + MagneticField msg; + msg.header = imu_msg.header; + msg.magnetic_field = RosVecFromVnVec(data.magnetic); // compensated + pub_mag_.Publish(msg); + } + { + FluidPressure msg; + msg.header = imu_msg.header; + msg.fluid_pressure = data.pressure; + pub_pres_.Publish(msg); + } + { + Temperature msg; + msg.header = imu_msg.header; + msg.temperature = data.temperature; + pub_temp_.Publish(msg); + } } sync_info_.Update(data.syncInCnt, imu_msg.header.stamp); - updater_.update(); } @@ -522,19 +506,13 @@ void VnEnsure(const VnErrorCode& error_code) { } } -void RosVector3FromVnVector3(geometry_msgs::Vector3& ros_vec3, - const VnVector3& vn_vec3) { - ros_vec3.x = vn_vec3.c0; - ros_vec3.y = vn_vec3.c1; - ros_vec3.z = vn_vec3.c2; -} +} // namespace imu_vn_100 -void RosQuaternionFromVnQuaternion(geometry_msgs::Quaternion& ros_quat, - const VnQuaternion& vn_quat) { - ros_quat.x = vn_quat.x; - ros_quat.y = vn_quat.y; - ros_quat.z = vn_quat.z; - ros_quat.w = vn_quat.w; -} +int main(int argc, char** argv) { + ros::init(argc, argv, "imu_vn_100"); + ros::NodeHandle pnh("~"); -} // namespace imu_vn_100 + imu_vn_100::ImuVn100 imu(pnh); + imu.Stream(true); + ros::spin(); +} diff --git a/include/imu_vn_100/imu_vn_100.h b/src/imu_vn_100.h similarity index 58% rename from include/imu_vn_100/imu_vn_100.h rename to src/imu_vn_100.h index 5ad82a7..6dd87e8 100644 --- a/include/imu_vn_100/imu_vn_100.h +++ b/src/imu_vn_100.h @@ -13,59 +13,49 @@ * See the License for the specific language governing permissions and * limitations under the License. */ +#pragma once -#ifndef IMU_VN_100_ROS_H_ -#define IMU_VN_100_ROS_H_ - -#include #include #include -#include -#include -#include -#include +#include #include "vn100.h" namespace imu_vn_100 { namespace du = diagnostic_updater; -using TopicDiagnosticPtr = boost::shared_ptr; -// NOTE: there is a DiagnosedPublisher inside diagnostic_updater -// but it does not have a default constructor thus we use this simple one -// instead, which has the same functionality struct DiagnosedPublisher { - ros::Publisher pub; - TopicDiagnosticPtr diag; - - template - void Create(ros::NodeHandle& pnh, const std::string& topic, - du::Updater& updater, double& rate) { - pub = pnh.advertise(topic, 1); - du::FrequencyStatusParam freq_param(&rate, &rate, 0.01, 10); - du::TimeStampStatusParam time_param(0, 0.5 / rate); - diag = boost::make_shared(topic, updater, freq_param, - time_param); + template + void Advertise(ros::NodeHandle& nh, const std::string& topic, + du::Updater& updater, double& rate) { + pub = nh.advertise(topic, 1); + diag.reset(new du::TopicDiagnostic(topic, updater, {&rate, &rate, 0.01, 10}, + {-1 / rate, 1 / rate})); } - template - void Publish(const MessageT& message) { - diag->tick(message.header.stamp); + template + void Publish(const T& message) { + // diag->tick(message.header.stamp); pub.publish(message); } + + operator bool() const { return diag != nullptr; } + + ros::Publisher pub; + boost::shared_ptr diag; }; +static constexpr int kBaseImuRate = 800; +static constexpr int kDefaultImuRate = 100; +static constexpr int kDefaultSyncOutRate = 20; + /** * @brief ImuVn100 The class is a ros wrapper for the Imu class * @author Ke Sun */ class ImuVn100 { public: - static constexpr int kBaseImuRate = 800; - static constexpr int kDefaultImuRate = 100; - static constexpr int kDefaultSyncOutRate = 20; - explicit ImuVn100(const ros::NodeHandle& pnh); ImuVn100(const ImuVn100&) = delete; ImuVn100& operator=(const ImuVn100&) = delete; @@ -77,16 +67,8 @@ class ImuVn100 { void PublishData(const VnDeviceCompositeData& data); - void RequestOnce(); - - void Idle(bool need_reply = true); - - void Resume(bool need_reply = true); - void Disconnect(); - void Configure(); - struct SyncInfo { unsigned count = 0; ros::Time time; @@ -104,6 +86,10 @@ class ImuVn100 { const SyncInfo sync_info() const { return sync_info_; } private: + void FixImuRate(); + void LoadParameters(); + void CreatePublishers(); + ros::NodeHandle pnh_; Vn100 imu_; @@ -114,22 +100,20 @@ class ImuVn100 { double imu_rate_double_ = kDefaultImuRate; std::string frame_id_; - bool enable_mag_ = true; - bool enable_pres_ = true; - bool enable_temp_ = true; - bool enable_rpy_ = false; + ros::Time stamp_last_; ///< last used time, t0 + ros::Time ros_time_last_; ///< last ros time, t_ros + uint64_t dev_time_last_{0}; ///< last dev time, ns, t_dev + double time_alpha_{0.0}; ///< t1 = dt_ros * a + dt_dev * (1-a) + t0 bool binary_output_ = true; int binary_async_mode_ = BINARY_ASYNC_MODE_SERIAL_2; - - bool imu_compensated_ = false; - - bool tf_ned_to_enu_ = false; + bool compensated_ = false; bool vpe_enable_ = true; int vpe_heading_mode_ = 1; int vpe_filtering_mode_ = 1; int vpe_tuning_mode_ = 1; + VnVector3 vpe_mag_base_tuning_; VnVector3 vpe_mag_adaptive_tuning_; VnVector3 vpe_mag_adaptive_filtering_; @@ -139,18 +123,10 @@ class ImuVn100 { SyncInfo sync_info_; + ros::Publisher pub_dt_; ///< publish time between curr and last + ros::Publisher pub_dnow_; ///< publish deviate from realtime du::Updater updater_; - DiagnosedPublisher pd_imu_, pd_mag_, pd_pres_, pd_temp_, pd_rpy_; - - void FixImuRate(); - void LoadParameters(); - void CreateDiagnosedPublishers(); + DiagnosedPublisher pub_imu_, pub_mag_, pub_pres_, pub_temp_; }; -// Just don't like type that is ALL CAP -using VnErrorCode = VN_ERROR_CODE; -void VnEnsure(const VnErrorCode& error_code); - } // namespace imu_vn_100 - -#endif // IMU_VN_100_ROS_H_ diff --git a/src/imu_vn_100_node.cpp b/src/imu_vn_100_node.cpp deleted file mode 100644 index 8b2fc90..0000000 --- a/src/imu_vn_100_node.cpp +++ /dev/null @@ -1,33 +0,0 @@ -/* - * Copyright [2015] [Ke Sun] - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ - -#include -#include - -using namespace imu_vn_100; - -int main(int argc, char** argv) { - ros::init(argc, argv, "imu_vn_100"); - ros::NodeHandle pnh("~"); - - try { - ImuVn100 imu(pnh); - imu.Stream(true); - ros::spin(); - } catch (const std::exception& e) { - ROS_INFO("%s: %s", pnh.getNamespace().c_str(), e.what()); - } -}