From a00ddd24debcaf057b323ba1ec6d77f85b6f7743 Mon Sep 17 00:00:00 2001 From: Raghav Khanna Date: Wed, 17 May 2017 23:28:11 +0200 Subject: [PATCH 1/3] working version with 2-3 ms correction --- dji_sdk/src/modules/dji_sdk_node_main.cpp | 157 +++++++++++----------- dji_sdk_demo/script/client.py | 2 +- 2 files changed, 83 insertions(+), 76 deletions(-) diff --git a/dji_sdk/src/modules/dji_sdk_node_main.cpp b/dji_sdk/src/modules/dji_sdk_node_main.cpp index 6a4aa2cf..11190562 100644 --- a/dji_sdk/src/modules/dji_sdk_node_main.cpp +++ b/dji_sdk/src/modules/dji_sdk_node_main.cpp @@ -25,13 +25,7 @@ //---------------------------------------------------------- using namespace std; -// TODO: Fix temporary hack for getting hardware timestamps using nanoTime -// should not be defined globally -auto g_counter = 0; -const auto g_WRAP_TIME = 4.295; //seconds -uint32_t g_prevNanoTime = 0; -long double g_hardwareStamp; - + void DJISDKNode::transparent_transmission_callback(uint8_t *buf, uint8_t len) { dji_sdk::TransparentTransmissionData transparent_transmission_data; @@ -43,39 +37,93 @@ void DJISDKNode::transparent_transmission_callback(uint8_t *buf, uint8_t len) void DJISDKNode::broadcast_callback() { DJI::onboardSDK::BroadcastData bc_data = rosAdapter->coreAPI->getBroadcastData(); - DJI::onboardSDK::Version version = rosAdapter->coreAPI->getSDKVersion(); unsigned short msg_flags = bc_data.dataFlag; static int frame_id = 0; frame_id ++; - - auto current_time = ros::Time::now(); - + auto current_time = ros::Time::now(); - if(msg_flags & HAS_TIME){ + if (msg_flags & HAS_TIME) { + + // Only this message containes the received time in the header and the + // hardware time within the message + // All following messages should have translated hardware times in the + // header. + + time_stamp.header.frame_id = "/time"; + time_stamp.header.stamp = current_time; + time_stamp.time = bc_data.timeStamp.time; // This is now in units of 1/400 + // s but resolution is only of + // the order of 1s!!! + time_stamp.time_ns = + bc_data.timeStamp.nanoTime; // This is perhaps more accurate??? But + // requires handling the wrapping after + // every 4.295 secs. DO NOT SUM both as it + // leads to double counting. + + time_stamp.sync_flag = bc_data.timeStamp.syncFlag; + time_stamp_publisher.publish(time_stamp); + + + // Use translated hardware time + if (device_time_translator_) { + current_time = device_time_translator_->update(bc_data.timeStamp.nanoTime, current_time,0.0); + } + /* + if (device_time_translator_->isReadyToTranslate()) { + current_time = + device_time_translator_->translate(bc_data.timeStamp.nanoTime); + }*/ else { + ROS_WARN( + "device_time_translator is not ready yet, using ros::Time::now()"); + } + + } - // Only this message containes the received time in the header and the hardware time within the message - // All following messages should have translated hardware times in the header. - time_stamp.header.frame_id = "/time"; - time_stamp.header.stamp = current_time; + // update Imu message + if ((msg_flags & HAS_Q) && (msg_flags & HAS_W) && (msg_flags & HAS_A)) { + + ROS_DEBUG_STREAM("Frame " << frame_id << " " << "Hardware timeStamp.nanoTime "<< std::setprecision(15) << double(bc_data.timeStamp.nanoTime)*1e-9); + imu_msg.header.frame_id = "/world"; + + imu_msg.header.stamp = current_time; + + // Conversion from NED to NWU (XYZ--robot body frame) + /* + tf::Quaternion qNED2ENU(); + qNED2ENU.setEulerZYX(pi/2,0, -pi/2); + tf::Quaternion qIMU(bc_data.q.q1, bc_data.q.q2, bc_data.q.q3, + bc_data.q.q0); + tf::Quaternion qIMUENU; + qIMUENU = qNED2ENU*qIMU; + imu_msg.orientation = quaternionTFToMsg(qIMUENU); + */ + // Convert from NED to ENU frame // This is conversion to NWU frame not + // ENU + imu_msg.orientation.w = bc_data.q.q0; + imu_msg.orientation.x = bc_data.q.q1; + imu_msg.orientation.y = -bc_data.q.q2; + imu_msg.orientation.z = -bc_data.q.q3; + imu_msg.angular_velocity.x = bc_data.w.x; + imu_msg.angular_velocity.y = -bc_data.w.y; + imu_msg.angular_velocity.z = -bc_data.w.z; + + imu_msg.linear_acceleration.x = bc_data.a.x * 9.807; + imu_msg.linear_acceleration.y = -bc_data.a.y * 9.807; + imu_msg.linear_acceleration.z = -bc_data.a.z * 9.807; + + imu_msg.orientation_covariance = {-1.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0}; + imu_msg.angular_velocity_covariance = {0, 0.0, 0.0, 0.0, 0, + 0.0, 0.0, 0.0, 0}; + imu_msg.linear_acceleration_covariance = {0, 0.0, 0.0, 0.0, 0, + 0.0, 0.0, 0.0, 0}; + imu_msg_publisher.publish(imu_msg); + } + - if(device_time_translator_){ - current_time = device_time_translator_->update(bc_data.timeStamp.time, current_time, 0.0); // estimated offset is 0.0 for now observed to usuall be < 2 ms can be upto ~ 20 ms!!! - //cout << setprecision(15) < Date: Thu, 7 Sep 2017 22:23:35 +0200 Subject: [PATCH 2/3] save current state --- ...h_zr300_rw_outdoor_integration_week.launch | 85 ++++++++++++++++++ ...300_rw_outdoor_integration_week_new.launch | 86 +++++++++++++++++++ dji_sdk/src/modules/dji_sdk_node_main.cpp | 2 +- 3 files changed, 172 insertions(+), 1 deletion(-) create mode 100644 dji_sdk/asl_launch/Onboard_exp_with_zr300_rw_outdoor_integration_week.launch create mode 100644 dji_sdk/asl_launch/Onboard_exp_with_zr300_rw_outdoor_integration_week_new.launch diff --git a/dji_sdk/asl_launch/Onboard_exp_with_zr300_rw_outdoor_integration_week.launch b/dji_sdk/asl_launch/Onboard_exp_with_zr300_rw_outdoor_integration_week.launch new file mode 100644 index 00000000..cd563d22 --- /dev/null +++ b/dji_sdk/asl_launch/Onboard_exp_with_zr300_rw_outdoor_integration_week.launch @@ -0,0 +1,85 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/dji_sdk/asl_launch/Onboard_exp_with_zr300_rw_outdoor_integration_week_new.launch b/dji_sdk/asl_launch/Onboard_exp_with_zr300_rw_outdoor_integration_week_new.launch new file mode 100644 index 00000000..07c9ff6d --- /dev/null +++ b/dji_sdk/asl_launch/Onboard_exp_with_zr300_rw_outdoor_integration_week_new.launch @@ -0,0 +1,86 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/dji_sdk/src/modules/dji_sdk_node_main.cpp b/dji_sdk/src/modules/dji_sdk_node_main.cpp index 11190562..8b77ef2d 100644 --- a/dji_sdk/src/modules/dji_sdk_node_main.cpp +++ b/dji_sdk/src/modules/dji_sdk_node_main.cpp @@ -89,7 +89,7 @@ void DJISDKNode::broadcast_callback() imu_msg.header.frame_id = "/world"; imu_msg.header.stamp = current_time; - + //TODO: Raghav - Update with nearest neighbour messages to increase frequency // Conversion from NED to NWU (XYZ--robot body frame) /* tf::Quaternion qNED2ENU(); From 64717ce71a31bb6ba4495241ebd872037823ab09 Mon Sep 17 00:00:00 2001 From: Raghav Khanna Date: Thu, 30 Mar 2017 21:30:06 +0200 Subject: [PATCH 3/3] improved navigation without overshooting