From 9d504b8ee488388142e28ffc5d7b14779b0fe17a Mon Sep 17 00:00:00 2001 From: Shital Shah Date: Mon, 10 Jul 2017 20:14:04 -0700 Subject: [PATCH] fixed issue in simNotifyRender --- .../SimpleFlightDroneController.hpp | 21 ++++++++++--------- Examples/main.cpp | 2 +- 2 files changed, 12 insertions(+), 11 deletions(-) diff --git a/AirLib/include/controllers/simple_flight/SimpleFlightDroneController.hpp b/AirLib/include/controllers/simple_flight/SimpleFlightDroneController.hpp index 81008ba1..715624de 100644 --- a/AirLib/include/controllers/simple_flight/SimpleFlightDroneController.hpp +++ b/AirLib/include/controllers/simple_flight/SimpleFlightDroneController.hpp @@ -254,17 +254,18 @@ protected: void simNotifyRender() override { std::unique_lock render_wait_lock(render_mutex_); + if (!is_pose_update_done_) { + auto kinematics = physics_body_->getKinematics(); + if (! VectorMath::hasNan(pending_pose_.position)) + kinematics.pose.position = pending_pose_.position; + if (! VectorMath::hasNan(pending_pose_.orientation)) + kinematics.pose.orientation = pending_pose_.orientation; + physics_body_->setKinematics(kinematics); - auto kinematics = physics_body_->getKinematics(); - if (! VectorMath::hasNan(pending_pose_.position)) - kinematics.pose.position = pending_pose_.position; - if (! VectorMath::hasNan(pending_pose_.orientation)) - kinematics.pose.orientation = pending_pose_.orientation; - physics_body_->setKinematics(kinematics); - - is_pose_update_done_ = true; - render_wait_lock.unlock(); - render_cond_.notify_all(); + is_pose_update_done_ = true; + render_wait_lock.unlock(); + render_cond_.notify_all(); + } } //*** End: DroneControllerBase implementation ***// diff --git a/Examples/main.cpp b/Examples/main.cpp index 5ee399c1..725dc746 100644 --- a/Examples/main.cpp +++ b/Examples/main.cpp @@ -61,6 +61,6 @@ void runSteroImageGenerator(int num_samples, std::string storage_path) int main(int argc, const char *argv[]) { - runSteroImageGenerator(argc < 2 ? 20000 : std::stoi(argv[1]), argc < 3 ? "c:\\temp\\stg_blocks" : std::string(argv[2])); + runSteroImageGenerator(argc < 2 ? 20000 : std::stoi(argv[1]), argc < 3 ? "c:\\temp\\stig2" : std::string(argv[2])); }