diff --git a/CMakeLists.txt b/CMakeLists.txt index 66369ae..d964bf5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,16 +7,19 @@ find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs diagnostic_updater + trigger_msgs ) + find_package(Boost REQUIRED) catkin_package( INCLUDE_DIRS include vncpplib/include LIBRARIES imu_vn_100 - CATKIN_DEPENDS diagnostic_updater roscpp sensor_msgs + CATKIN_DEPENDS diagnostic_updater roscpp sensor_msgs DEPENDS Boost ) + include_directories( include vncpplib/include diff --git a/README.md b/README.md index 4ff95b4..59ef048 100644 --- a/README.md +++ b/README.md @@ -19,6 +19,8 @@ Note that the official SDK is modified within this package due to some bugs or u This is a Catkin package. Make sure the package is on `ROS_PACKAGE_PATH` after cloning the package to your workspace. And the normal procedure for compiling a catkin package will work. +You also need to download the [trigger_msgs](https://gitlab.com/neufieldrobotics/trigger_msgs) package to compile it. Clone them in the same workspace. + ``` cd your_work_space catkin_make --pkg imu_vn_100 --cmake-args -DCMAKE_BUILD_TYPE=Release diff --git a/include/imu_vn_100/imu_vn_100.h b/include/imu_vn_100/imu_vn_100.h index 5ad82a7..a9c4c32 100644 --- a/include/imu_vn_100/imu_vn_100.h +++ b/include/imu_vn_100/imu_vn_100.h @@ -20,10 +20,12 @@ #include #include #include +#include #include #include #include #include +#include #include "vn100.h" @@ -54,6 +56,7 @@ struct DiagnosedPublisher { diag->tick(message.header.stamp); pub.publish(message); } + }; /** @@ -122,14 +125,21 @@ class ImuVn100 { bool binary_output_ = true; int binary_async_mode_ = BINARY_ASYNC_MODE_SERIAL_2; - bool imu_compensated_ = false; + int imu_compensated_ = 0; bool tf_ned_to_enu_ = false; bool vpe_enable_ = true; + bool use_imu_clock = true; // condition to set variables if we are using IMU clock + ros::Time start_of_node_ros_time; // this is the offset time added to the IMU time since startup when + bool is_first_data_point = true; // this condition used to check whether we have first binary message + uint64_t nanoseconds_till_first_data_point = 0; // the variable stores the time since startup that we get when we get our first binary message int vpe_heading_mode_ = 1; int vpe_filtering_mode_ = 1; int vpe_tuning_mode_ = 1; + int syncOutCnt_old=0; + int syncOutCnt=0; + VnVector3 vpe_mag_base_tuning_; VnVector3 vpe_mag_adaptive_tuning_; VnVector3 vpe_mag_adaptive_filtering_; @@ -140,7 +150,8 @@ class ImuVn100 { SyncInfo sync_info_; du::Updater updater_; - DiagnosedPublisher pd_imu_, pd_mag_, pd_pres_, pd_temp_, pd_rpy_; + DiagnosedPublisher pd_imu_,pd_imu_comp_,pd_imu_uncomp_, pd_mag_,pd_mag_comp_; + DiagnosedPublisher pd_mag_uncomp_, pd_pres_, pd_temp_, pd_rpy_, pd_sync_trigger; void FixImuRate(); void LoadParameters(); diff --git a/launch/vn_100_cont.launch b/launch/vn_100_cont.launch index f7c00a1..1846adb 100644 --- a/launch/vn_100_cont.launch +++ b/launch/vn_100_cont.launch @@ -4,19 +4,23 @@ - - + + - + + + + - + - + + @@ -25,19 +29,20 @@ - + + - + diff --git a/launch/vn_100_cont_ext_trigger.launch b/launch/vn_100_cont_ext_trigger.launch new file mode 100755 index 0000000..bb65f97 --- /dev/null +++ b/launch/vn_100_cont_ext_trigger.launch @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/vn_100_cont_rpy_ex_trig.launch b/launch/vn_100_cont_rpy_ex_trig.launch new file mode 100755 index 0000000..19db4b7 --- /dev/null +++ b/launch/vn_100_cont_rpy_ex_trig.launch @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/package.xml b/package.xml index 70e03f4..b51bbc6 100644 --- a/package.xml +++ b/package.xml @@ -10,10 +10,10 @@ Ke Sun catkin - diagnostic_updater roscpp sensor_msgs + trigger_msgs diff --git a/src/imu_vn_100.cpp b/src/imu_vn_100.cpp index a23ccea..4686a3a 100644 --- a/src/imu_vn_100.cpp +++ b/src/imu_vn_100.cpp @@ -15,9 +15,8 @@ */ #include - #include - +# define NANOSEC_TO_SEC 1000000000 namespace imu_vn_100 { // LESS HACK IS STILL HACK @@ -29,7 +28,11 @@ using sensor_msgs::Imu; using sensor_msgs::MagneticField; using sensor_msgs::FluidPressure; using sensor_msgs::Temperature; - +enum class IMU_data_opts{ + uncompensated = 0, + compensated = 1, + both = 2 +}; void RosVector3FromVnVector3(geometry_msgs::Vector3& ros_vec3, const VnVector3& vn_vec3); void RosQuaternionFromVnQuaternion(geometry_msgs::Quaternion& ros_quat, @@ -115,15 +118,13 @@ void ImuVn100::LoadParameters() { pnh_.param("sync_rate", sync_info_.rate, kDefaultSyncOutRate); pnh_.param("sync_pulse_width_us", sync_info_.pulse_width_us, 1000); + pnh_.param("use_imu_clock",use_imu_clock,true); pnh_.param("binary_output", binary_output_, true); pnh_.param("binary_async_mode", binary_async_mode_, BINARY_ASYNC_MODE_SERIAL_2); - - pnh_.param("imu_compensated", imu_compensated_, false); - + pnh_.param("imu_compensated", imu_compensated_, 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); @@ -147,18 +148,41 @@ void ImuVn100::LoadParameters() { 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); - + ROS_INFO("Use IMU clock %d",use_imu_clock); 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_, + if (imu_compensated_ == 0){ + pd_imu_uncomp_.Create(pnh_,"imu_uncompensated",updater_,imu_rate_double_); + if (enable_mag_) { + pd_mag_uncomp_.Create(pnh_, "magnetic_field_uncomp", updater_, + imu_rate_double_); + } + } + else if (imu_compensated_== 1){ + pd_imu_comp_.Create(pnh_,"imu_compensated",updater_,imu_rate_double_); + if (enable_mag_) { + pd_mag_comp_.Create(pnh_, "magnetic_field_comp", updater_, imu_rate_double_); + } } + else if (imu_compensated_== 2){ + pd_imu_uncomp_.Create(pnh_,"imu_uncompensated",updater_,imu_rate_double_); + pd_imu_comp_.Create(pnh_,"imu_compensated",updater_,imu_rate_double_); + if (enable_mag_) { + pd_mag_comp_.Create(pnh_, "magnetic_field_compensated", updater_, + imu_rate_double_); + } + } + + //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_); @@ -170,11 +194,12 @@ void ImuVn100::CreateDiagnosedPublishers() { if (enable_rpy_) { pd_rpy_.Create(pnh_, "rpy", updater_, imu_rate_double_); } + if(sync_info_.SyncEnabled()) { + pd_sync_trigger.Create(pnh_,"sync_trigger",updater_,imu_rate_double_); + } } - void ImuVn100::Initialize() { LoadParameters(); - ROS_DEBUG("Connecting to device"); VnEnsure(vn100_connect(&imu_, port_.c_str(), 115200)); ros::Duration(0.5).sleep(); @@ -320,7 +345,6 @@ void ImuVn100::Initialize() { void ImuVn100::Stream(bool async) { // Pause the device first VnEnsure(vn100_pauseAsyncOutputs(&imu_, true)); - if (async) { VnEnsure(vn100_setAsynchronousDataOutputType(&imu_, VNASYNC_OFF, true)); @@ -332,11 +356,30 @@ void ImuVn100::Stream(bool async) { grp1 |= BG1_YPR; sgrp1.push_back("BG1_YPR"); } + // Set the binary output data type for group 2 + uint16_t grp2 = BG2_NONE; + + if (sync_info_.SyncEnabled()) + { + grp2 |= BG2_SYNC_OUT_CNT; + } + if (use_imu_clock) + { + grp2 |= BG2_TIME_STARTUP; + } uint16_t grp3 = BG3_NONE; std::list sgrp3; uint16_t grp5 = BG5_NONE; std::list sgrp5; - if (imu_compensated_) { + if (imu_compensated_== 0){ + grp1 |= BG1_IMU; + sgrp1.push_back("BG1_IMU"); + if (enable_mag_) { + grp3 |= BG3_UNCOMP_MAG; + sgrp3.push_back("BG3_UNCOMP_MAG"); + } + } + else if(imu_compensated_== 1){ grp1 |= BG1_ACCEL | BG1_ANGULAR_RATE; sgrp1.push_back("BG1_ACCEL"); sgrp1.push_back("BG1_ANGULAR_RATE"); @@ -344,12 +387,15 @@ void ImuVn100::Stream(bool async) { grp3 |= BG3_MAG; sgrp3.push_back("BG3_MAG"); } - } else { - grp1 |= BG1_IMU; + } + else if(imu_compensated_== 2){ + grp1 |= BG1_IMU | BG1_ACCEL | BG1_ANGULAR_RATE; sgrp1.push_back("BG1_IMU"); + sgrp1.push_back("BG1_ACCEL"); + sgrp1.push_back("BG1_ANGULAR_RATE"); if (enable_mag_) { - grp3 |= BG3_UNCOMP_MAG; - sgrp3.push_back("BG3_UNCOMP_MAG"); + grp3 |= BG3_MAG; // |BG3_UNCOMP_MAG; + sgrp3.push_back("BG3_COMP_MAG"); } } if (enable_temp_) { @@ -371,6 +417,10 @@ void ImuVn100::Stream(bool async) { s.pop_back(); ROS_INFO("Streaming #1: %s", s.c_str()); } + if(sync_info_.SyncEnabled()) + { + ROS_INFO("Streaming #2: BG2_SYNC_OUT_CNT"); + } if (!sgrp3.empty()) { std::stringstream ss; std::copy( @@ -393,11 +443,11 @@ void ImuVn100::Stream(bool async) { s.pop_back(); ROS_INFO("Streaming #5: %s", s.c_str()); } - VnEnsure(vn100_setBinaryOutput1Configuration( + VnEnsure(vn100_setBinaryOutput1Configuration_withgrp2( &imu_, binary_async_mode_, kBaseImuRate / imu_rate_, - grp1, grp3, grp5, + grp1, grp2, grp3, grp5, true )); } else { @@ -437,30 +487,96 @@ void ImuVn100::Disconnect() { vn100_disconnect(&imu_); } -void ImuVn100::PublishData(const VnDeviceCompositeData& data) { - sensor_msgs::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); - } 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); +void ImuVn100::PublishData(const VnDeviceCompositeData& data) { + std_msgs::Header header; + ros::Duration duration; + ROS_INFO("publishing"); + if (use_imu_clock) + { + if(is_first_data_point) + { + start_of_node_ros_time = ros::Time::now(); + nanoseconds_till_first_data_point = data.timeStartup; // in uint64_t + is_first_data_point = false; + } + + //convert time to int32 seconds and int32 nanoseconds to prevent overflow of buffer + int32_t seconds = (data.timeStartup - nanoseconds_till_first_data_point)/NANOSEC_TO_SEC; + int32_t nanoseconds = (data.timeStartup-nanoseconds_till_first_data_point)%NANOSEC_TO_SEC; + ros::Duration duration1(seconds,nanoseconds); + duration = duration1; + //ROS_INFO_STREAM(" the duration is = "<isConnected) + return VNERR_NOT_CONNECTED; + + return vndevice_setBinaryOutputConfiguration( + &vn100->vndevice, + binaryOutputRegisterId, + asyncMode, + rateDivisor, + outputGroup1Selections, + outputGroup2Selections, + outputGroup3Selections, + 0, + outputGroup5Selections, + 0, + waitForResponse); + +} + VN_ERROR_CODE vn100_setBinaryOutputConfiguration( Vn100* vn100, uint8_t binaryOutputRegisterId, @@ -204,6 +233,28 @@ VN_ERROR_CODE vn100_setBinaryOutputConfiguration( } +VN_ERROR_CODE vn100_setBinaryOutput1Configuration_withgrp2( + Vn100* vn100, + uint16_t asyncMode, + uint16_t rateDivisor, + uint16_t outputGroup1Selections, + uint16_t outputGroup2Selections, + uint16_t outputGroup3Selections, + uint16_t outputGroup5Selections, + bool waitForResponse) +{ + return vn100_setBinaryOutputConfiguration_withgrp2( + vn100, + 1, + asyncMode, + rateDivisor, + outputGroup1Selections, + outputGroup2Selections, + outputGroup3Selections, + outputGroup5Selections, + waitForResponse); +} + VN_ERROR_CODE vn100_setBinaryOutput1Configuration( Vn100* vn100, uint16_t asyncMode, diff --git a/vncpplib/src/vndevice.c b/vncpplib/src/vndevice.c index 607246b..ca07922 100644 --- a/vncpplib/src/vndevice.c +++ b/vncpplib/src/vndevice.c @@ -39,7 +39,7 @@ #define ASCII_RECEIVE_BUFFER_SIZE 256 #define BINARY_RECEIVE_BUFFER_SIZE 256 #define READ_BUFFER_SIZE 256 -#define NUMBER_OF_MILLISECONDS_TO_SLEEP_AFTER_RECEIVING_NO_BYTES_ON_COM_PORT_READ 5 +#define NUMBER_OF_MILLISECONDS_TO_SLEEP_AFTER_RECEIVING_NO_BYTES_ON_COM_PORT_READ .1 #define DEFAULT_TIMEOUT_IN_MS 5000 #define ASCII_START_CHAR '$' #define ASCII_FIRST_END_CHAR '\r' @@ -50,7 +50,7 @@ VN-200 user manual. */ const unsigned char BinaryPacketGroupLengths[6][16] = { {8, 8, 8, 12, 16, 12, 24, 12, 12, 24, 20, 28, 2, 4, 8, 0}, /* Group 1 */ - {8, 8, 8, 2, 8, 8, 8, 4, 0, 0, 0, 0, 0, 0, 0, 0}, /* Group 2 */ + {8, 8, 8, 2, 8, 8, 8, 4, 4, 0, 0, 0, 0, 0, 0, 0}, /* Group 2 */ {2, 12, 12, 12, 4, 4, 16, 12, 12, 12, 12, 2, 40, 0, 0, 0}, /* Group 3 */ {8, 8, 2, 1, 1, 24, 24, 12, 12, 12, 4, 4, 32, 0, 0, 0}, /* Group 4 */ {2, 12, 16, 36, 12, 12, 12, 12, 12, 12, 28, 24, 0, 0, 0, 0}, /* Group 5 */ @@ -208,7 +208,6 @@ int vndevice_computeLengthOfBinaryGroupPayload( runningLength += BinaryPacketGroupLengths[groupIndex][i]; } } - return runningLength; } @@ -392,7 +391,7 @@ void vndevice_processReceivedBinaryPacket( curGroupFieldPointer = buffer + 1 + 1; curDataPointer = buffer + 1 + 1 + numberOfTotalGroupFieldBytes; - + if (groups & 0x01) { numberOfDataBytesProcessed = vndevice_processGroup1Data( *((uint16_t*) curGroupFieldPointer), curDataPointer, &data); @@ -769,6 +768,14 @@ uint16_t vndevice_processGroup2Data( groupDataPtr += sizeof (uint32_t); } + + if (groupField & 0x0100) + { + data->syncOutCnt = *((uint32_t*) groupDataPtr); + groupDataPtr += sizeof(uint32_t); + } + + /*implement here for 0x0100 - synchronisation counter output*/ return (uint16_t) (groupDataPtr - originalGroupDataPtr); } @@ -1629,15 +1636,15 @@ void* vndevice_communicationHandler( unsigned int curResponsePos = 0; vndevice_readData_threadSafe(vndevice, readBuffer, READ_BUFFER_SIZE, &numOfBytesRead); - + //printf("No. of bytes read %d \r \n",numOfBytesRead); if (numOfBytesRead == 0) { /* There was no data. Sleep for a short amount of time before continuing. */ vncp_sleepInMs(NUMBER_OF_MILLISECONDS_TO_SLEEP_AFTER_RECEIVING_NO_BYTES_ON_COM_PORT_READ); continue; } - + int loop_counter = 0; for ( ; curResponsePos < numOfBytesRead; curResponsePos++) { - + char curChar = readBuffer[curResponsePos]; /* Make sure we are not overrunning our ASCII buffer. */ @@ -1745,6 +1752,7 @@ void* vndevice_communicationHandler( binaryGroupsFieldFound = true; binaryReceiveBuffer[binaryBufferIndex] = curChar; + binaryBufferIndex++; expectedNumberOfBinaryGroupFieldBytes = vndevice_numberOfSetBits(binaryGroups) * 2; @@ -1770,9 +1778,9 @@ void* vndevice_communicationHandler( int payloadLength; payloadLength = vndevice_computeLengthOfExpectedBinaryPayload(binaryReceiveBuffer); - + expectedBinaryPacketLength = 2 + expectedNumberOfBinaryGroupFieldBytes + payloadLength + 2; - + if (expectedBinaryPacketLength > BINARY_RECEIVE_BUFFER_SIZE) { /* This packet will be too large for our buffer @@ -1785,7 +1793,6 @@ void* vndevice_communicationHandler( } else { - /* Add this byte to the binary buffer. */ binaryReceiveBuffer[binaryBufferIndex] = curChar; binaryBufferIndex++; @@ -1794,14 +1801,13 @@ void* vndevice_communicationHandler( if (binaryBufferIndex == expectedBinaryPacketLength) { /* We might have a packet if we reach here. */ - uint16_t calculatedCrc; + uint16_t calculatedCrc = 0; /* Verify the packet is valid. */ calculatedCrc = vndevice_checksum_computeCrc16(binaryReceiveBuffer + 1, expectedBinaryPacketLength - 1); if (calculatedCrc == 0) { /* We have a valid binary packet. */ - vndevice_processReceivedBinaryPacket(vndevice, binaryReceiveBuffer); } @@ -1841,7 +1847,6 @@ int vndevice_computeLengthOfExpectedBinaryPayload( /* We have group 1 present. */ runningPayloadLength += vndevice_computeLengthOfBinaryGroupPayload(0, *((uint16_t*) ptrToCurrentGroupField)); - ptrToCurrentGroupField += 2; } @@ -1849,7 +1854,6 @@ int vndevice_computeLengthOfExpectedBinaryPayload( /* We have group 2 present. */ runningPayloadLength += vndevice_computeLengthOfBinaryGroupPayload(1, *((uint16_t*) ptrToCurrentGroupField)); - ptrToCurrentGroupField += 2; } @@ -1857,7 +1861,6 @@ int vndevice_computeLengthOfExpectedBinaryPayload( /* We have group 3 present. */ runningPayloadLength += vndevice_computeLengthOfBinaryGroupPayload(2, *((uint16_t*) ptrToCurrentGroupField)); - ptrToCurrentGroupField += 2; } @@ -1865,7 +1868,6 @@ int vndevice_computeLengthOfExpectedBinaryPayload( /* We have group 4 present. */ runningPayloadLength += vndevice_computeLengthOfBinaryGroupPayload(3, *((uint16_t*) ptrToCurrentGroupField)); - ptrToCurrentGroupField += 2; } @@ -1873,7 +1875,6 @@ int vndevice_computeLengthOfExpectedBinaryPayload( /* We have group 5 present. */ runningPayloadLength += vndevice_computeLengthOfBinaryGroupPayload(4, *((uint16_t*) ptrToCurrentGroupField)); - ptrToCurrentGroupField += 2; } @@ -1881,7 +1882,6 @@ int vndevice_computeLengthOfExpectedBinaryPayload( /* We have group 6 present. */ runningPayloadLength += vndevice_computeLengthOfBinaryGroupPayload(5, *((uint16_t*) ptrToCurrentGroupField)); - ptrToCurrentGroupField += 2; } @@ -3403,7 +3403,6 @@ DLL_EXPORT VN_ERROR_CODE vndevice_setBinaryOutputConfiguration( if (outputGroup6Selections != 0) { curBufLoc += sprintf(cmdToSendBuilder + curBufLoc, ",%X", outputGroup6Selections); } - if (waitForResponse) errorCode = vndevice_transaction(vndevice, cmdToSendBuilder, responseMatch); else