From 2fcefc396fdfbefee5c0ec3bf1dbec8fca87704d Mon Sep 17 00:00:00 2001 From: Chao Qu Date: Thu, 22 Aug 2019 13:21:42 -0400 Subject: [PATCH 1/9] various cleanup --- CMakeLists.txt | 93 +++----- launch/vn_100_cont.launch | 2 +- package.xml | 4 +- src/imu_vn_100.cpp | 291 +++++++++++------------ {include/imu_vn_100 => src}/imu_vn_100.h | 31 +-- src/imu_vn_100_node.cpp | 33 --- 6 files changed, 177 insertions(+), 277 deletions(-) rename {include/imu_vn_100 => src}/imu_vn_100.h (91%) delete mode 100644 src/imu_vn_100_node.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 66369ae..d3ba40f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,73 +1,40 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.10) project(imu_vn_100) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -Wall") +set(CMAKE_CXX_STANDARD 11) -find_package(catkin REQUIRED COMPONENTS - roscpp - sensor_msgs - diagnostic_updater -) -find_package(Boost REQUIRED) +find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs diagnostic_updater) -catkin_package( - INCLUDE_DIRS include vncpplib/include - LIBRARIES imu_vn_100 - CATKIN_DEPENDS diagnostic_updater roscpp sensor_msgs - DEPENDS Boost -) +catkin_package(CATKIN_DEPENDS + diagnostic_updater + roscpp + sensor_msgs) -include_directories( - include - vncpplib/include - ${catkin_INCLUDE_DIRS} -) +add_executable(${PROJECT_NAME} + 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} + PUBLIC src vncpplib/include ${catkin_INCLUDE_DIRS}) +target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES}) -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(TARGETS ${PROJECT_NAME} + 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**" -) + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING + PATTERN "*.h**") 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/launch/vn_100_cont.launch b/launch/vn_100_cont.launch index f7c00a1..7de5806 100644 --- a/launch/vn_100_cont.launch +++ b/launch/vn_100_cont.launch @@ -30,7 +30,7 @@ - + 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..b574078 100644 --- a/src/imu_vn_100.cpp +++ b/src/imu_vn_100.cpp @@ -14,35 +14,46 @@ * limitations under the License. */ -#include +#include "imu_vn_100.h" #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; +} + +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 +69,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"); @@ -123,7 +132,6 @@ void ImuVn100::LoadParameters() { pnh_.param("imu_compensated", imu_compensated_, false); 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,29 +139,41 @@ 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_; + imu_rate_double_ = static_cast(imu_rate_); pd_imu_.Create(pnh_, "imu", updater_, imu_rate_double_); if (enable_mag_) { pd_mag_.Create(pnh_, "magnetic_field", updater_, @@ -168,7 +188,7 @@ void ImuVn100::CreateDiagnosedPublishers() { imu_rate_double_); } if (enable_rpy_) { - pd_rpy_.Create(pnh_, "rpy", updater_, imu_rate_double_); + pd_rpy_.Create(pnh_, "rpy", updater_, imu_rate_double_); } } @@ -227,9 +247,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,76 +258,49 @@ 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)); + &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_ || + + 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", 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)); } 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(); @@ -337,7 +330,7 @@ void ImuVn100::Stream(bool async) { uint16_t grp5 = BG5_NONE; std::list sgrp5; if (imu_compensated_) { - grp1 |= BG1_ACCEL | BG1_ANGULAR_RATE; + grp1 |= BG1_ACCEL | BG1_ANGULAR_RATE; sgrp1.push_back("BG1_ACCEL"); sgrp1.push_back("BG1_ANGULAR_RATE"); if (enable_mag_) { @@ -345,7 +338,7 @@ void ImuVn100::Stream(bool async) { sgrp3.push_back("BG3_MAG"); } } else { - grp1 |= BG1_IMU; + grp1 |= BG1_IMU; sgrp1.push_back("BG1_IMU"); if (enable_mag_) { grp3 |= BG3_UNCOMP_MAG; @@ -353,53 +346,40 @@ void ImuVn100::Stream(bool async) { } } if (enable_temp_) { - grp3 |= BG3_TEMP; - sgrp3.push_back("BG3_TEMP"); + grp3 |= BG3_TEMP; + sgrp3.push_back("BG3_TEMP"); } if (enable_pres_) { - grp3 |= BG3_PRES; - sgrp3.push_back("BG3_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::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::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::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()); } - VnEnsure(vn100_setBinaryOutput1Configuration( - &imu_, - binary_async_mode_, - kBaseImuRate / imu_rate_, - grp1, grp3, grp5, - true - )); + VnEnsure(vn100_setBinaryOutput1Configuration(&imu_, binary_async_mode_, + kBaseImuRate / imu_rate_, + grp1, grp3, grp5, true)); } else { // Set the ASCII output data type and data rate // ROS_INFO("Configure the output data type and frequency (id: 6 & 7)"); @@ -432,57 +412,58 @@ void ImuVn100::Idle(bool 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 imu_msg; imu_msg.header.stamp = ros::Time::now(); imu_msg.header.frame_id = frame_id_; if (imu_compensated_) { - RosVector3FromVnVector3(imu_msg.linear_acceleration, data.acceleration); - RosVector3FromVnVector3(imu_msg.angular_velocity, data.angularRate); + 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); + imu_msg.angular_velocity = RosVecFromVnVec(data.accelerationUncompensated); + imu_msg.linear_acceleration = + RosVecFromVnVec(data.angularRateUncompensated); } + if (binary_output_) { - RosQuaternionFromVnQuaternion(imu_msg.orientation, data.quaternion); + imu_msg.orientation = RosQuatFromVnQuat(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; + 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); } if (enable_mag_) { - sensor_msgs::MagneticField mag_msg; + MagneticField mag_msg; mag_msg.header = imu_msg.header; - RosVector3FromVnVector3(mag_msg.magnetic_field, data.magnetic); + mag_msg.magnetic_field = RosVecFromVnVec(data.magnetic); pd_mag_.Publish(mag_msg); } if (enable_pres_) { - sensor_msgs::FluidPressure pres_msg; + FluidPressure pres_msg; pres_msg.header = imu_msg.header; pres_msg.fluid_pressure = data.pressure; pd_pres_.Publish(pres_msg); } if (enable_temp_) { - sensor_msgs::Temperature temp_msg; + Temperature temp_msg; temp_msg.header = imu_msg.header; temp_msg.temperature = data.temperature; pd_temp_.Publish(temp_msg); @@ -522,19 +503,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 91% rename from include/imu_vn_100/imu_vn_100.h rename to src/imu_vn_100.h index 5ad82a7..c3e6eb9 100644 --- a/include/imu_vn_100/imu_vn_100.h +++ b/src/imu_vn_100.h @@ -13,16 +13,14 @@ * 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 #include "vn100.h" @@ -56,16 +54,16 @@ struct DiagnosedPublisher { } }; +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; @@ -104,6 +102,10 @@ class ImuVn100 { const SyncInfo sync_info() const { return sync_info_; } private: + void FixImuRate(); + void LoadParameters(); + void CreateDiagnosedPublishers(); + ros::NodeHandle pnh_; Vn100 imu_; @@ -130,6 +132,7 @@ class ImuVn100 { 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_; @@ -141,16 +144,6 @@ class ImuVn100 { du::Updater updater_; DiagnosedPublisher pd_imu_, pd_mag_, pd_pres_, pd_temp_, pd_rpy_; - - void FixImuRate(); - void LoadParameters(); - void CreateDiagnosedPublishers(); }; -// 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()); - } -} From 5cd1079da0955bebd7ed3d24cbbfe4bb81cd6f93 Mon Sep 17 00:00:00 2001 From: Chao Qu Date: Thu, 22 Aug 2019 13:34:17 -0400 Subject: [PATCH 2/9] add a publisher for dt --- src/imu_vn_100.cpp | 30 ++++++++++++++++++++---------- src/imu_vn_100.h | 20 +++++--------------- 2 files changed, 25 insertions(+), 25 deletions(-) diff --git a/src/imu_vn_100.cpp b/src/imu_vn_100.cpp index b574078..d01ac21 100644 --- a/src/imu_vn_100.cpp +++ b/src/imu_vn_100.cpp @@ -17,6 +17,11 @@ #include "imu_vn_100.h" #include +#include +#include +#include +#include +#include namespace imu_vn_100 { @@ -172,9 +177,13 @@ void ImuVn100::LoadParameters() { sync_info_.FixSyncRate(); } -void ImuVn100::CreateDiagnosedPublishers() { +void ImuVn100::CreatePublishers() { imu_rate_double_ = static_cast(imu_rate_); + + pub_dt_ = pnh_.advertise("dt", 1); + pd_imu_.Create(pnh_, "imu", updater_, imu_rate_double_); + if (enable_mag_) { pd_mag_.Create(pnh_, "magnetic_field", updater_, imu_rate_double_); @@ -303,7 +312,7 @@ void ImuVn100::Initialize() { vpe_accel_adaptive_filtering_, true)); } - CreateDiagnosedPublishers(); + CreatePublishers(); auto hardware_id = std::string("vn100-") + std::string(model_number_buffer) + std::string(serial_number_buffer); @@ -403,14 +412,6 @@ 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() { ROS_INFO("Reset and disconnect from imu"); vn100_reset(&imu_); @@ -422,6 +423,15 @@ void ImuVn100::PublishData(const VnDeviceCompositeData& data) { imu_msg.header.stamp = ros::Time::now(); imu_msg.header.frame_id = frame_id_; + if (last_time_ != ros::Time()) { + const auto dt = imu_msg.header.stamp - last_time_; + std_msgs::Float64 dt_msg; + dt_msg.data = dt.toSec(); + pub_dt_.publish(dt_msg); + } + + last_time_ = imu_msg.header.stamp; + if (imu_compensated_) { imu_msg.linear_acceleration = RosVecFromVnVec(data.acceleration); imu_msg.angular_velocity = RosVecFromVnVec(data.angularRate); diff --git a/src/imu_vn_100.h b/src/imu_vn_100.h index c3e6eb9..cb843b0 100644 --- a/src/imu_vn_100.h +++ b/src/imu_vn_100.h @@ -18,10 +18,6 @@ #include #include #include -#include -#include -#include -#include #include "vn100.h" @@ -75,16 +71,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,7 +92,7 @@ class ImuVn100 { private: void FixImuRate(); void LoadParameters(); - void CreateDiagnosedPublishers(); + void CreatePublishers(); ros::NodeHandle pnh_; Vn100 imu_; @@ -116,6 +104,8 @@ class ImuVn100 { double imu_rate_double_ = kDefaultImuRate; std::string frame_id_; + ros::Time last_time_; + bool enable_mag_ = true; bool enable_pres_ = true; bool enable_temp_ = true; @@ -126,8 +116,6 @@ class ImuVn100 { bool imu_compensated_ = false; - bool tf_ned_to_enu_ = false; - bool vpe_enable_ = true; int vpe_heading_mode_ = 1; int vpe_filtering_mode_ = 1; @@ -142,6 +130,8 @@ class ImuVn100 { SyncInfo sync_info_; + ros::Publisher pub_dt_; + du::Updater updater_; DiagnosedPublisher pd_imu_, pd_mag_, pd_pres_, pd_temp_, pd_rpy_; }; From 0f06438fcf5dd152cc438bc6611f59be7ca6495d Mon Sep 17 00:00:00 2001 From: Chao Qu Date: Thu, 22 Aug 2019 14:10:33 -0400 Subject: [PATCH 3/9] fix for timestamp --- src/imu_vn_100.cpp | 40 +++++++++++++++++++++++++++++----------- src/imu_vn_100.h | 5 +++-- 2 files changed, 32 insertions(+), 13 deletions(-) diff --git a/src/imu_vn_100.cpp b/src/imu_vn_100.cpp index d01ac21..9571976 100644 --- a/src/imu_vn_100.cpp +++ b/src/imu_vn_100.cpp @@ -328,16 +328,18 @@ 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"}; + // 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 (enable_rpy_) { grp1 |= BG1_YPR; sgrp1.push_back("BG1_YPR"); } uint16_t grp3 = BG3_NONE; - std::list sgrp3; + std::vector sgrp3; uint16_t grp5 = BG5_NONE; - std::list sgrp5; + std::vector sgrp5; if (imu_compensated_) { grp1 |= BG1_ACCEL | BG1_ANGULAR_RATE; sgrp1.push_back("BG1_ACCEL"); @@ -419,18 +421,34 @@ void ImuVn100::Disconnect() { } void ImuVn100::PublishData(const VnDeviceCompositeData& data) { - Imu imu_msg; - imu_msg.header.stamp = ros::Time::now(); - imu_msg.header.frame_id = frame_id_; + // Handle timestamp + if (device_time_zero_ == 0) { + ros_time_zero_ = ros::Time::now(); + device_time_zero_ = data.timeStartup; + ROS_INFO_STREAM("Set device time zero to " << device_time_zero_); + } - if (last_time_ != ros::Time()) { - const auto dt = imu_msg.header.stamp - last_time_; + ros::Duration dt; + dt.fromNSec(data.timeStartup - device_time_zero_); + const ros::Time stamp = ros_time_zero_ + dt; + + if (dt.toSec() < 0) { + // This should never happen, but we check for it anyway + ROS_WARN_STREAM("dt: " << dt.toSec() << ", startup: " << data.timeStartup + << ", zero: " << device_time_zero_); + } + + if (ros_time_last_ != ros::Time()) { std_msgs::Float64 dt_msg; - dt_msg.data = dt.toSec(); + dt_msg.data = (stamp - ros_time_last_).toSec(); pub_dt_.publish(dt_msg); } - last_time_ = imu_msg.header.stamp; + ros_time_last_ = stamp; + + Imu imu_msg; + imu_msg.header.stamp = stamp; + imu_msg.header.frame_id = frame_id_; if (imu_compensated_) { imu_msg.linear_acceleration = RosVecFromVnVec(data.acceleration); diff --git a/src/imu_vn_100.h b/src/imu_vn_100.h index cb843b0..3b2bb0b 100644 --- a/src/imu_vn_100.h +++ b/src/imu_vn_100.h @@ -104,7 +104,9 @@ class ImuVn100 { double imu_rate_double_ = kDefaultImuRate; std::string frame_id_; - ros::Time last_time_; + ros::Time ros_time_last_; ///< previous time stamp + ros::Time ros_time_zero_; ///< ros time of first data + uint64_t device_time_zero_{0}; ///< device time of first data, ns bool enable_mag_ = true; bool enable_pres_ = true; @@ -113,7 +115,6 @@ class ImuVn100 { bool binary_output_ = true; int binary_async_mode_ = BINARY_ASYNC_MODE_SERIAL_2; - bool imu_compensated_ = false; bool vpe_enable_ = true; From 59b2f90e42d8e88988d9d214afae529be4824b36 Mon Sep 17 00:00:00 2001 From: Chao Qu Date: Thu, 22 Aug 2019 17:21:25 -0400 Subject: [PATCH 4/9] more clean up --- launch/vn_100_cont.launch | 7 +- src/imu_vn_100.cpp | 157 +++++++++++++++++++------------------- src/imu_vn_100.h | 44 +++++------ 3 files changed, 102 insertions(+), 106 deletions(-) diff --git a/launch/vn_100_cont.launch b/launch/vn_100_cont.launch index 7de5806..913ae2a 100644 --- a/launch/vn_100_cont.launch +++ b/launch/vn_100_cont.launch @@ -16,7 +16,7 @@ - + @@ -37,13 +37,16 @@ - + + + + diff --git a/src/imu_vn_100.cpp b/src/imu_vn_100.cpp index 9571976..a5e9aa0 100644 --- a/src/imu_vn_100.cpp +++ b/src/imu_vn_100.cpp @@ -38,6 +38,14 @@ std::ostream& operator<<(std::ostream& os, const VnVector3& v) { 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; @@ -98,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(); } @@ -122,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); @@ -134,7 +138,7 @@ 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("vpe/enable", vpe_enable_, true); pnh_.param("vpe/heading_mode", vpe_heading_mode_, 1); @@ -182,22 +186,28 @@ void ImuVn100::CreatePublishers() { pub_dt_ = pnh_.advertise("dt", 1); - pd_imu_.Create(pnh_, "imu", updater_, imu_rate_double_); + pub_imu_.Advertise(pnh_, "imu", updater_, imu_rate_double_); + ROS_INFO("Publish imu to %s", pub_imu_.pub.getTopic().c_str()); - if (enable_mag_) { - pd_mag_.Create(pnh_, "magnetic_field", updater_, - imu_rate_double_); + if (pnh_.param("enable_mag", true)) { + pub_mag_.Advertise(pnh_, "magnetic_field", updater_, + imu_rate_double_); + ROS_INFO("Publish magnetic filed to %s", pub_mag_.pub.getTopic().c_str()); } - if (enable_pres_) { - pd_pres_.Create(pnh_, "fluid_pressure", updater_, - imu_rate_double_); + + if (pnh_.param("enable_pres", true)) { + pub_pres_.Advertise(pnh_, "fluid_pressure", updater_, + imu_rate_double_); + ROS_INFO("Publish pressure to %s", pub_pres_.pub.getTopic().c_str()); } - if (enable_temp_) { - pd_temp_.Create(pnh_, "temperature", updater_, - imu_rate_double_); + if (pnh_.param("enable_temp", true)) { + pub_temp_.Advertise(pnh_, "temperature", updater_, + imu_rate_double_); + ROS_INFO("Publish temperature to %s", pub_temp_.pub.getTopic().c_str()); } - if (enable_rpy_) { - pd_rpy_.Create(pnh_, "rpy", updater_, imu_rate_double_); + if (pnh_.param("enable_rpy", false)) { + pub_rpy_.Advertise(pnh_, "rpy", updater_, imu_rate_double_); + ROS_INFO("Publish rpy to %s", pub_rpy_.pub.getTopic().c_str()); } } @@ -314,8 +324,9 @@ void ImuVn100::Initialize() { 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); } @@ -332,62 +343,48 @@ void ImuVn100::Stream(bool async) { 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 (enable_rpy_) { + if (pub_imu_) { grp1 |= BG1_YPR; sgrp1.push_back("BG1_YPR"); } + uint16_t grp3 = BG3_NONE; std::vector sgrp3; + uint16_t grp5 = BG5_NONE; std::vector sgrp5; - if (imu_compensated_) { + + if (compensated_) { grp1 |= BG1_ACCEL | BG1_ANGULAR_RATE; sgrp1.push_back("BG1_ACCEL"); sgrp1.push_back("BG1_ANGULAR_RATE"); - if (enable_mag_) { + if (pub_mag_) { grp3 |= BG3_MAG; sgrp3.push_back("BG3_MAG"); } } else { grp1 |= BG1_IMU; sgrp1.push_back("BG1_IMU"); - if (enable_mag_) { + if (pub_mag_) { grp3 |= BG3_UNCOMP_MAG; sgrp3.push_back("BG3_UNCOMP_MAG"); } } - if (enable_temp_) { + + if (pub_temp_) { grp3 |= BG3_TEMP; sgrp3.push_back("BG3_TEMP"); } - if (enable_pres_) { + + if (pub_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()); - } + + ROS_INFO("Streaming #1: %s", VectorToString(sgrp1).c_str()); + ROS_INFO("Streaming #3: %s", VectorToString(sgrp3).c_str()); + ROS_INFO("Streaming #5: %s", VectorToString(sgrp5).c_str()); + VnEnsure(vn100_setBinaryOutput1Configuration(&imu_, binary_async_mode_, kBaseImuRate / imu_rate_, grp1, grp3, grp5, true)); @@ -426,15 +423,17 @@ void ImuVn100::PublishData(const VnDeviceCompositeData& data) { ros_time_zero_ = ros::Time::now(); device_time_zero_ = data.timeStartup; ROS_INFO_STREAM("Set device time zero to " << device_time_zero_); + ROS_INFO_STREAM("Set ros time zero to " << ros_time_zero_.toSec()); } ros::Duration dt; dt.fromNSec(data.timeStartup - device_time_zero_); const ros::Time stamp = ros_time_zero_ + dt; - if (dt.toSec() < 0) { + if (dt.toNSec() < 0) { // This should never happen, but we check for it anyway - ROS_WARN_STREAM("dt: " << dt.toSec() << ", startup: " << data.timeStartup + ROS_WARN_STREAM("dt: " << dt.toNSec() + << " ns, startup: " << data.timeStartup << ", zero: " << device_time_zero_); } @@ -446,11 +445,12 @@ void ImuVn100::PublishData(const VnDeviceCompositeData& data) { ros_time_last_ = stamp; + // Fill in data Imu imu_msg; imu_msg.header.stamp = stamp; imu_msg.header.frame_id = frame_id_; - if (imu_compensated_) { + if (compensated_) { imu_msg.linear_acceleration = RosVecFromVnVec(data.acceleration); imu_msg.angular_velocity = RosVecFromVnVec(data.angularRate); } else { @@ -465,40 +465,43 @@ void ImuVn100::PublishData(const VnDeviceCompositeData& data) { imu_msg.orientation = RosQuatFromVnQuat(data.quaternion); } - pd_imu_.Publish(imu_msg); + pub_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); + if (pub_rpy_) { + Vector3Stamped msg; + msg.header = imu_msg.header; + msg.vector.z = data.ypr.yaw * M_PI / 180.0; + msg.vector.y = data.ypr.pitch * M_PI / 180.0; + msg.vector.x = data.ypr.roll * M_PI / 180.0; + pub_rpy_.Publish(msg); } - if (enable_mag_) { - MagneticField mag_msg; - mag_msg.header = imu_msg.header; - mag_msg.magnetic_field = RosVecFromVnVec(data.magnetic); - pd_mag_.Publish(mag_msg); + if (pub_mag_) { + MagneticField msg; + msg.header = imu_msg.header; + if (compensated_) { + msg.magnetic_field = RosVecFromVnVec(data.magnetic); + } else { + msg.magnetic_field = RosVecFromVnVec(data.magneticUncompensated); + } + pub_mag_.Publish(msg); } - if (enable_pres_) { - FluidPressure pres_msg; - pres_msg.header = imu_msg.header; - pres_msg.fluid_pressure = data.pressure; - pd_pres_.Publish(pres_msg); + if (pub_pres_) { + FluidPressure msg; + msg.header = imu_msg.header; + msg.fluid_pressure = data.pressure; + pub_pres_.Publish(msg); } - if (enable_temp_) { - Temperature temp_msg; - temp_msg.header = imu_msg.header; - temp_msg.temperature = data.temperature; - pd_temp_.Publish(temp_msg); + if (pub_temp_) { + 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(); } diff --git a/src/imu_vn_100.h b/src/imu_vn_100.h index 3b2bb0b..d14f2e3 100644 --- a/src/imu_vn_100.h +++ b/src/imu_vn_100.h @@ -24,30 +24,26 @@ 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}, + {0, 0.5 / rate})); } - template - void Publish(const MessageT& message) { + 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; @@ -104,18 +100,13 @@ class ImuVn100 { double imu_rate_double_ = kDefaultImuRate; std::string frame_id_; - ros::Time ros_time_last_; ///< previous time stamp - ros::Time ros_time_zero_; ///< ros time of first data + ros::Time ros_time_last_; ///< previous time stamp + ros::Time ros_time_zero_; ///< ros time of first data uint64_t device_time_zero_{0}; ///< device time of first data, ns - bool enable_mag_ = true; - bool enable_pres_ = true; - bool enable_temp_ = true; - bool enable_rpy_ = false; - bool binary_output_ = true; int binary_async_mode_ = BINARY_ASYNC_MODE_SERIAL_2; - bool imu_compensated_ = false; + bool compensated_ = false; bool vpe_enable_ = true; int vpe_heading_mode_ = 1; @@ -132,9 +123,8 @@ class ImuVn100 { SyncInfo sync_info_; ros::Publisher pub_dt_; - du::Updater updater_; - DiagnosedPublisher pd_imu_, pd_mag_, pd_pres_, pd_temp_, pd_rpy_; + DiagnosedPublisher pub_imu_, pub_mag_, pub_pres_, pub_temp_, pub_rpy_; }; } // namespace imu_vn_100 From 3786c398260d0a04d7067997b50ac92ad596569e Mon Sep 17 00:00:00 2001 From: Chao Qu Date: Thu, 22 Aug 2019 17:54:45 -0400 Subject: [PATCH 5/9] also publishes deviates from realtime --- src/imu_vn_100.cpp | 17 +++++++++++++---- src/imu_vn_100.h | 3 ++- 2 files changed, 15 insertions(+), 5 deletions(-) diff --git a/src/imu_vn_100.cpp b/src/imu_vn_100.cpp index a5e9aa0..ce5c4b3 100644 --- a/src/imu_vn_100.cpp +++ b/src/imu_vn_100.cpp @@ -22,6 +22,7 @@ #include #include #include +#include namespace imu_vn_100 { @@ -185,6 +186,7 @@ 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()); @@ -421,6 +423,7 @@ void ImuVn100::PublishData(const VnDeviceCompositeData& data) { // Handle timestamp if (device_time_zero_ == 0) { ros_time_zero_ = ros::Time::now(); + ros_time_last_ = ros_time_zero_; device_time_zero_ = data.timeStartup; ROS_INFO_STREAM("Set device time zero to " << device_time_zero_); ROS_INFO_STREAM("Set ros time zero to " << ros_time_zero_.toSec()); @@ -429,6 +432,7 @@ void ImuVn100::PublishData(const VnDeviceCompositeData& data) { ros::Duration dt; dt.fromNSec(data.timeStartup - device_time_zero_); const ros::Time stamp = ros_time_zero_ + dt; + const ros::Duration dnow = ros::Time::now() - stamp; if (dt.toNSec() < 0) { // This should never happen, but we check for it anyway @@ -437,10 +441,15 @@ void ImuVn100::PublishData(const VnDeviceCompositeData& data) { << ", zero: " << device_time_zero_); } - if (ros_time_last_ != ros::Time()) { - std_msgs::Float64 dt_msg; - dt_msg.data = (stamp - ros_time_last_).toSec(); - pub_dt_.publish(dt_msg); + { + std_msgs::Float64 msg; + msg.data = (stamp - ros_time_last_).toSec(); + pub_dt_.publish(msg); + } + { + std_msgs::Int64 msg; + msg.data = dnow.toNSec(); + pub_dnow_.publish(msg); } ros_time_last_ = stamp; diff --git a/src/imu_vn_100.h b/src/imu_vn_100.h index d14f2e3..bbb4c31 100644 --- a/src/imu_vn_100.h +++ b/src/imu_vn_100.h @@ -122,7 +122,8 @@ class ImuVn100 { SyncInfo sync_info_; - ros::Publisher pub_dt_; + ros::Publisher pub_dt_; ///< publish time between curr and last + ros::Publisher pub_dnow_; ///< publish time between curr and now du::Updater updater_; DiagnosedPublisher pub_imu_, pub_mag_, pub_pres_, pub_temp_, pub_rpy_; }; From ae5231a88129158fed6d145a7235427cd783b3d0 Mon Sep 17 00:00:00 2001 From: Chao Qu Date: Thu, 22 Aug 2019 18:16:37 -0400 Subject: [PATCH 6/9] follow manual suggestiong using only binary group 1 --- launch/{vn_100_cont.launch => imu.launch} | 14 +-- src/imu_vn_100.cpp | 107 ++++++++-------------- src/imu_vn_100.h | 5 +- 3 files changed, 47 insertions(+), 79 deletions(-) rename launch/{vn_100_cont.launch => imu.launch} (78%) diff --git a/launch/vn_100_cont.launch b/launch/imu.launch similarity index 78% rename from launch/vn_100_cont.launch rename to launch/imu.launch index 913ae2a..87a57e1 100644 --- a/launch/vn_100_cont.launch +++ b/launch/imu.launch @@ -9,7 +9,7 @@ - + @@ -25,10 +25,8 @@ - - - - + + @@ -39,10 +37,8 @@ - - - - + + diff --git a/src/imu_vn_100.cpp b/src/imu_vn_100.cpp index ce5c4b3..88a2a34 100644 --- a/src/imu_vn_100.cpp +++ b/src/imu_vn_100.cpp @@ -191,25 +191,21 @@ void ImuVn100::CreatePublishers() { 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", true)) { + 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()); - } - - if (pnh_.param("enable_pres", true)) { pub_pres_.Advertise(pnh_, "fluid_pressure", updater_, imu_rate_double_); ROS_INFO("Publish pressure to %s", pub_pres_.pub.getTopic().c_str()); - } - if (pnh_.param("enable_temp", true)) { pub_temp_.Advertise(pnh_, "temperature", updater_, imu_rate_double_); ROS_INFO("Publish temperature to %s", pub_temp_.pub.getTopic().c_str()); } - if (pnh_.param("enable_rpy", false)) { - pub_rpy_.Advertise(pnh_, "rpy", updater_, imu_rate_double_); - ROS_INFO("Publish rpy to %s", pub_rpy_.pub.getTopic().c_str()); + + if (pnh_.param("enable_ypr", false)) { + pub_ypr_.Advertise(pnh_, "ypr", updater_, imu_rate_double_); + ROS_INFO("Publish rpy to %s", pub_ypr_.pub.getTopic().c_str()); } } @@ -280,10 +276,10 @@ void ImuVn100::Initialize() { 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); + 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_ || @@ -292,10 +288,10 @@ void ImuVn100::Initialize() { 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); + 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)); } @@ -345,51 +341,30 @@ void ImuVn100::Stream(bool async) { 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 (pub_imu_) { + if (pub_ypr_) { grp1 |= BG1_YPR; sgrp1.push_back("BG1_YPR"); } - uint16_t grp3 = BG3_NONE; - std::vector sgrp3; - - uint16_t grp5 = BG5_NONE; - std::vector sgrp5; - if (compensated_) { grp1 |= BG1_ACCEL | BG1_ANGULAR_RATE; sgrp1.push_back("BG1_ACCEL"); sgrp1.push_back("BG1_ANGULAR_RATE"); - if (pub_mag_) { - grp3 |= BG3_MAG; - sgrp3.push_back("BG3_MAG"); - } } else { grp1 |= BG1_IMU; sgrp1.push_back("BG1_IMU"); - if (pub_mag_) { - grp3 |= BG3_UNCOMP_MAG; - sgrp3.push_back("BG3_UNCOMP_MAG"); - } - } - - if (pub_temp_) { - grp3 |= BG3_TEMP; - sgrp3.push_back("BG3_TEMP"); } - if (pub_pres_) { - grp3 |= BG3_PRES; - sgrp3.push_back("BG3_PRES"); + if (pub_mag_) { + grp1 |= BG1_MAG_PRES; + sgrp1.push_back("BG1_MAG_PRES"); } ROS_INFO("Streaming #1: %s", VectorToString(sgrp1).c_str()); - ROS_INFO("Streaming #3: %s", VectorToString(sgrp3).c_str()); - ROS_INFO("Streaming #5: %s", VectorToString(sgrp5).c_str()); - VnEnsure(vn100_setBinaryOutput1Configuration(&imu_, binary_async_mode_, - kBaseImuRate / imu_rate_, - grp1, grp3, grp5, true)); + VnEnsure(vn100_setBinaryOutput1Configuration( + &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)"); @@ -476,38 +451,34 @@ void ImuVn100::PublishData(const VnDeviceCompositeData& data) { pub_imu_.Publish(imu_msg); - if (pub_rpy_) { + if (pub_ypr_) { Vector3Stamped msg; msg.header = imu_msg.header; msg.vector.z = data.ypr.yaw * M_PI / 180.0; msg.vector.y = data.ypr.pitch * M_PI / 180.0; msg.vector.x = data.ypr.roll * M_PI / 180.0; - pub_rpy_.Publish(msg); + pub_ypr_.Publish(msg); } if (pub_mag_) { - MagneticField msg; - msg.header = imu_msg.header; - if (compensated_) { - msg.magnetic_field = RosVecFromVnVec(data.magnetic); - } else { - msg.magnetic_field = RosVecFromVnVec(data.magneticUncompensated); + { + 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); } - pub_mag_.Publish(msg); - } - - if (pub_pres_) { - FluidPressure msg; - msg.header = imu_msg.header; - msg.fluid_pressure = data.pressure; - pub_pres_.Publish(msg); - } - - if (pub_temp_) { - Temperature msg; - msg.header = imu_msg.header; - msg.temperature = data.temperature; - pub_temp_.Publish(msg); } sync_info_.Update(data.syncInCnt, imu_msg.header.stamp); diff --git a/src/imu_vn_100.h b/src/imu_vn_100.h index bbb4c31..fdc0d93 100644 --- a/src/imu_vn_100.h +++ b/src/imu_vn_100.h @@ -31,7 +31,7 @@ struct DiagnosedPublisher { du::Updater& updater, double& rate) { pub = nh.advertise(topic, 1); diag.reset(new du::TopicDiagnostic(topic, updater, {&rate, &rate, 0.01, 10}, - {0, 0.5 / rate})); + {-1 / rate, 1 / rate})); } template @@ -103,6 +103,7 @@ class ImuVn100 { ros::Time ros_time_last_; ///< previous time stamp ros::Time ros_time_zero_; ///< ros time of first data uint64_t device_time_zero_{0}; ///< device time of first data, ns + double time_alpha_{0.0}; ///< t1 = dnow * a + ddev * (1-a) + t0 bool binary_output_ = true; int binary_async_mode_ = BINARY_ASYNC_MODE_SERIAL_2; @@ -125,7 +126,7 @@ class ImuVn100 { ros::Publisher pub_dt_; ///< publish time between curr and last ros::Publisher pub_dnow_; ///< publish time between curr and now du::Updater updater_; - DiagnosedPublisher pub_imu_, pub_mag_, pub_pres_, pub_temp_, pub_rpy_; + DiagnosedPublisher pub_imu_, pub_mag_, pub_pres_, pub_temp_, pub_ypr_; }; } // namespace imu_vn_100 From ebdd887e4f3ff4fd03d463ae558511c7d1c74062 Mon Sep 17 00:00:00 2001 From: Chao Qu Date: Thu, 22 Aug 2019 18:55:05 -0400 Subject: [PATCH 7/9] use a weighted average of dev time and ros time to compute message time --- launch/imu.launch | 2 ++ src/imu_vn_100.cpp | 52 ++++++++++++++++++++++++++++------------------ src/imu_vn_100.h | 12 +++++------ 3 files changed, 40 insertions(+), 26 deletions(-) diff --git a/launch/imu.launch b/launch/imu.launch index 87a57e1..17602db 100644 --- a/launch/imu.launch +++ b/launch/imu.launch @@ -17,6 +17,7 @@ + @@ -36,6 +37,7 @@ + diff --git a/src/imu_vn_100.cpp b/src/imu_vn_100.cpp index 88a2a34..c4b44c3 100644 --- a/src/imu_vn_100.cpp +++ b/src/imu_vn_100.cpp @@ -22,7 +22,6 @@ #include #include #include -#include namespace imu_vn_100 { @@ -140,6 +139,9 @@ void ImuVn100::LoadParameters() { BINARY_ASYNC_MODE_SERIAL_2); 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); @@ -186,7 +188,7 @@ void ImuVn100::CreatePublishers() { imu_rate_double_ = static_cast(imu_rate_); pub_dt_ = pnh_.advertise("dt", 1); - pub_dnow_ = pnh_.advertise("dnow", 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()); @@ -396,38 +398,48 @@ void ImuVn100::Disconnect() { void ImuVn100::PublishData(const VnDeviceCompositeData& data) { // Handle timestamp - if (device_time_zero_ == 0) { - ros_time_zero_ = ros::Time::now(); - ros_time_last_ = ros_time_zero_; - device_time_zero_ = data.timeStartup; - ROS_INFO_STREAM("Set device time zero to " << device_time_zero_); - ROS_INFO_STREAM("Set ros time zero to " << ros_time_zero_.toSec()); + 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; + // ROS_INFO_STREAM("Set device time zero to " << dev_time_last_); + // ROS_INFO_STREAM("Set ros time zero to " << ros_time_zero_.toSec()); } + // 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(data.timeStartup - device_time_zero_); - const ros::Time stamp = ros_time_zero_ + dt; - const ros::Duration dnow = ros::Time::now() - stamp; + dt.fromNSec(dt_filtered); + const ros::Time stamp = stamp_last_ + dt; - if (dt.toNSec() < 0) { + if (dt_dev < 0) { // This should never happen, but we check for it anyway - ROS_WARN_STREAM("dt: " << dt.toNSec() - << " ns, startup: " << data.timeStartup - << ", zero: " << device_time_zero_); + ROS_WARN_STREAM("dt: " << dt_dev << " ns, startup: " << data.timeStartup + << ", zero: " << dev_time_last_); } { std_msgs::Float64 msg; - msg.data = (stamp - ros_time_last_).toSec(); + msg.data = dt.toSec(); pub_dt_.publish(msg); } { - std_msgs::Int64 msg; - msg.data = dnow.toNSec(); + std_msgs::Float64 msg; + msg.data = (ros_time_now - stamp).toSec(); pub_dnow_.publish(msg); } - ros_time_last_ = stamp; + ros_time_last_ = ros_time_now; + dev_time_last_ = data.timeStartup; + stamp_last_ = stamp; // Fill in data Imu imu_msg; @@ -482,7 +494,7 @@ void ImuVn100::PublishData(const VnDeviceCompositeData& data) { } sync_info_.Update(data.syncInCnt, imu_msg.header.stamp); - updater_.update(); + // updater_.update(); } void VnEnsure(const VnErrorCode& error_code) { diff --git a/src/imu_vn_100.h b/src/imu_vn_100.h index fdc0d93..94d57b4 100644 --- a/src/imu_vn_100.h +++ b/src/imu_vn_100.h @@ -36,7 +36,7 @@ struct DiagnosedPublisher { template void Publish(const T& message) { - diag->tick(message.header.stamp); + // diag->tick(message.header.stamp); pub.publish(message); } @@ -100,10 +100,10 @@ class ImuVn100 { double imu_rate_double_ = kDefaultImuRate; std::string frame_id_; - ros::Time ros_time_last_; ///< previous time stamp - ros::Time ros_time_zero_; ///< ros time of first data - uint64_t device_time_zero_{0}; ///< device time of first data, ns - double time_alpha_{0.0}; ///< t1 = dnow * a + ddev * (1-a) + t0 + 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; @@ -124,7 +124,7 @@ class ImuVn100 { SyncInfo sync_info_; ros::Publisher pub_dt_; ///< publish time between curr and last - ros::Publisher pub_dnow_; ///< publish time between curr and now + ros::Publisher pub_dnow_; ///< publish deviate from realtime du::Updater updater_; DiagnosedPublisher pub_imu_, pub_mag_, pub_pres_, pub_temp_, pub_ypr_; }; From 3b1f7d5654b7ef569e9fe8acd54605ff209e18e1 Mon Sep 17 00:00:00 2001 From: Chao Qu Date: Thu, 22 Aug 2019 19:09:39 -0400 Subject: [PATCH 8/9] update README and kill ypr --- README.md | 25 +++++++++++-------------- launch/imu.launch | 2 -- src/imu_vn_100.cpp | 18 ------------------ src/imu_vn_100.h | 2 +- 4 files changed, 12 insertions(+), 35 deletions(-) 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/imu.launch b/launch/imu.launch index 17602db..9935387 100644 --- a/launch/imu.launch +++ b/launch/imu.launch @@ -27,7 +27,6 @@ - @@ -40,7 +39,6 @@ - diff --git a/src/imu_vn_100.cpp b/src/imu_vn_100.cpp index c4b44c3..fcfe36a 100644 --- a/src/imu_vn_100.cpp +++ b/src/imu_vn_100.cpp @@ -204,11 +204,6 @@ void ImuVn100::CreatePublishers() { imu_rate_double_); ROS_INFO("Publish temperature to %s", pub_temp_.pub.getTopic().c_str()); } - - if (pnh_.param("enable_ypr", false)) { - pub_ypr_.Advertise(pnh_, "ypr", updater_, imu_rate_double_); - ROS_INFO("Publish rpy to %s", pub_ypr_.pub.getTopic().c_str()); - } } void ImuVn100::Initialize() { @@ -343,10 +338,6 @@ void ImuVn100::Stream(bool async) { 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 (pub_ypr_) { - grp1 |= BG1_YPR; - sgrp1.push_back("BG1_YPR"); - } if (compensated_) { grp1 |= BG1_ACCEL | BG1_ANGULAR_RATE; @@ -463,15 +454,6 @@ void ImuVn100::PublishData(const VnDeviceCompositeData& data) { pub_imu_.Publish(imu_msg); - if (pub_ypr_) { - Vector3Stamped msg; - msg.header = imu_msg.header; - msg.vector.z = data.ypr.yaw * M_PI / 180.0; - msg.vector.y = data.ypr.pitch * M_PI / 180.0; - msg.vector.x = data.ypr.roll * M_PI / 180.0; - pub_ypr_.Publish(msg); - } - if (pub_mag_) { { MagneticField msg; diff --git a/src/imu_vn_100.h b/src/imu_vn_100.h index 94d57b4..6dd87e8 100644 --- a/src/imu_vn_100.h +++ b/src/imu_vn_100.h @@ -126,7 +126,7 @@ class ImuVn100 { ros::Publisher pub_dt_; ///< publish time between curr and last ros::Publisher pub_dnow_; ///< publish deviate from realtime du::Updater updater_; - DiagnosedPublisher pub_imu_, pub_mag_, pub_pres_, pub_temp_, pub_ypr_; + DiagnosedPublisher pub_imu_, pub_mag_, pub_pres_, pub_temp_; }; } // namespace imu_vn_100 From bfdd5b33b01198329b7cef880e2c9e8eafa269a3 Mon Sep 17 00:00:00 2001 From: Chao Qu Date: Mon, 26 Aug 2019 17:06:29 -0400 Subject: [PATCH 9/9] address part of the review --- CMakeLists.txt | 19 ++++++++----------- launch/imu.launch | 5 +++-- src/imu_vn_100.cpp | 4 +--- 3 files changed, 12 insertions(+), 16 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d3ba40f..1b6474d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,7 +1,9 @@ -cmake_minimum_required(VERSION 3.10) +cmake_minimum_required(VERSION 2.8.3) project(imu_vn_100) -set(CMAKE_CXX_STANDARD 11) +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 11) +endif() find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs diagnostic_updater) @@ -10,25 +12,20 @@ catkin_package(CATKIN_DEPENDS roscpp sensor_msgs) -add_executable(${PROJECT_NAME} +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} +target_include_directories(${PROJECT_NAME}_node PUBLIC src vncpplib/include ${catkin_INCLUDE_DIRS}) -target_link_libraries(${PROJECT_NAME} PUBLIC ${catkin_LIBRARIES}) +target_link_libraries(${PROJECT_NAME}_node PUBLIC ${catkin_LIBRARIES}) -install(TARGETS ${PROJECT_NAME} +install(TARGETS ${PROJECT_NAME}_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**") - install(FILES vncpplib/include/vn100.h vncpplib/include/vncp_services.h vncpplib/include/vndevice.h diff --git a/launch/imu.launch b/launch/imu.launch index 9935387..29fe39e 100644 --- a/launch/imu.launch +++ b/launch/imu.launch @@ -26,9 +26,10 @@ - + + - + diff --git a/src/imu_vn_100.cpp b/src/imu_vn_100.cpp index fcfe36a..446dfef 100644 --- a/src/imu_vn_100.cpp +++ b/src/imu_vn_100.cpp @@ -394,8 +394,6 @@ void ImuVn100::PublishData(const VnDeviceCompositeData& data) { ros_time_last_ = ros_time_now; stamp_last_ = ros_time_last_; dev_time_last_ = data.timeStartup; - // ROS_INFO_STREAM("Set device time zero to " << dev_time_last_); - // ROS_INFO_STREAM("Set ros time zero to " << ros_time_zero_.toSec()); } // delta time from device @@ -476,7 +474,7 @@ void ImuVn100::PublishData(const VnDeviceCompositeData& data) { } sync_info_.Update(data.syncInCnt, imu_msg.header.stamp); - // updater_.update(); + updater_.update(); } void VnEnsure(const VnErrorCode& error_code) {