From 7d6055a16dbb7d1bae69c915d007943c9899add5 Mon Sep 17 00:00:00 2001 From: Chris Hold Date: Thu, 4 Apr 2019 00:52:50 +0200 Subject: [PATCH 1/8] Fix head rotation Fix 90 deg offset Fix rebase onto master and revert additional rotations Fix mouse handling listener Fix Head tracking rotation --- src/gui/qopenglplotter.cpp | 12 ++++++++---- src/gui/quserinterface.cpp | 13 +++++++------ src/trackerpolhemus.cpp | 2 +- src/trackerrazor.h | 2 +- src/trackervrpn.cpp | 2 +- 5 files changed, 18 insertions(+), 13 deletions(-) diff --git a/src/gui/qopenglplotter.cpp b/src/gui/qopenglplotter.cpp index 0e65e8f1..2c358b20 100644 --- a/src/gui/qopenglplotter.cpp +++ b/src/gui/qopenglplotter.cpp @@ -382,8 +382,10 @@ void ssr::QOpenGLPlotter::_draw_reference() glTranslatef(0.03f, -0.03f, 0.0f); - // rotate according to reference position - glRotatef(_scene.get_reference().orientation.azimuth, 0.0f, 0.0f, 1.0f); + // rotate according to reference offset position + glRotatef(_scene.get_reference().orientation.azimuth + + _scene.get_reference_offset().orientation.azimuth, + 0.0f, 0.0f, 1.0f); glBindTexture(GL_TEXTURE_2D, _listener_shadow_texture); @@ -396,8 +398,10 @@ void ssr::QOpenGLPlotter::_draw_reference() glPopMatrix(); - // rotate according to reference position - glRotatef(_scene.get_reference().orientation.azimuth, 0.0f, 0.0f, 1.0f); + // rotate according to reference offset position + glRotatef(_scene.get_reference().orientation.azimuth + + _scene.get_reference_offset().orientation.azimuth, + 0.0f, 0.0f, 1.0f); glBindTexture(GL_TEXTURE_2D, _listener_texture); diff --git a/src/gui/quserinterface.cpp b/src/gui/quserinterface.cpp index 782ff5b8..a701564c 100644 --- a/src/gui/quserinterface.cpp +++ b/src/gui/quserinterface.cpp @@ -980,8 +980,8 @@ void ssr::QUserInterface::mouseMoveEvent(QMouseEvent *event) // absolut mouse position in OpenGL coordinates Position mouse_pos(static_cast(pos_x), static_cast(pos_y)); - // position relative to reference position - mouse_pos -= _scene.get_reference().position; + // position relative to reference position offset + mouse_pos -= _scene.get_reference_offset().position; _get_openGL_pos(_previous_mouse_event.x(), _previous_mouse_event.y(), @@ -990,11 +990,12 @@ void ssr::QUserInterface::mouseMoveEvent(QMouseEvent *event) // previous absolut position in OpenGL coordinates Position prev_mouse_pos(static_cast(pos_x), static_cast(pos_y)); - // previous position relative to relative position - prev_mouse_pos -= _scene.get_reference().position; + // previous position relative to relative position offset + prev_mouse_pos -= _scene.get_reference_offset().position; - _controller.take_control()->reference_rotation(_scene.get_reference().orientation + - (mouse_pos.orientation() - prev_mouse_pos.orientation())); + _controller.take_control()->reference_rotation_offset(_scene.get_reference().orientation + + _scene.get_reference_offset().orientation + + (mouse_pos.orientation() - prev_mouse_pos.orientation())); } // else if diff --git a/src/trackerpolhemus.cpp b/src/trackerpolhemus.cpp index f2651ae8..fcfad114 100644 --- a/src/trackerpolhemus.cpp +++ b/src/trackerpolhemus.cpp @@ -192,7 +192,7 @@ ssr::TrackerPolhemus::_open_serial_port(const char *portname) void ssr::TrackerPolhemus::calibrate() { - _az_corr = _current_data.azimuth; + _az_corr = _current_data.azimuth + 90; } void diff --git a/src/trackerrazor.h b/src/trackerrazor.h index f0ab35e4..34fc9cdf 100644 --- a/src/trackerrazor.h +++ b/src/trackerrazor.h @@ -57,7 +57,7 @@ class TrackerRazor : public Tracker if (_tracker != nullptr) delete _tracker; } - virtual void calibrate() { _az_corr = _current_azimuth; } + virtual void calibrate() { _az_corr = _current_azimuth + 90; } private: /// constructor diff --git a/src/trackervrpn.cpp b/src/trackervrpn.cpp index 0f171602..934a1568 100644 --- a/src/trackervrpn.cpp +++ b/src/trackervrpn.cpp @@ -109,7 +109,7 @@ ssr::TrackerVrpn::vrpn_change_handler(const vrpn_TRACKERCB t) void ssr::TrackerVrpn::calibrate() { - _az_corr = _current_azimuth; + _az_corr = _current_azimuth + 90; } void From f359fe65ff979daea2fc6f0a5e1b1800c4085906 Mon Sep 17 00:00:00 2001 From: Chris Hold Date: Wed, 10 Jul 2019 16:32:49 +0300 Subject: [PATCH 2/8] Fix VRPN Tracker. Start to introduce augmented Tracker struct. --- src/tracker.h | 20 +++++++++++++++ src/trackervrpn.cpp | 61 +++++++++++++++++++++++++++++---------------- src/trackervrpn.h | 10 +++----- 3 files changed, 64 insertions(+), 27 deletions(-) diff --git a/src/tracker.h b/src/tracker.h index 51748fa2..f5bb6d82 100644 --- a/src/tracker.h +++ b/src/tracker.h @@ -30,6 +30,13 @@ #ifndef SSR_TRACKER_H #define SSR_TRACKER_H +#include + +#include "api.h" + +namespace ssr +{ + /// Class definition struct Tracker { @@ -37,6 +44,19 @@ struct Tracker /// calibrate tracker; set the instantaneous position to be the reference virtual void calibrate() = 0; + + // Azimuth value at calibration in degree + std::atomic azi_correction{0.0f}; + + // Current tracker data + struct tracker_data + { + // Sensor orientation in quaternions + struct Rot {std::atomic x{}, y{}, z{}, w{1}; } orientation; + }; + }; +} // namespace ssr + #endif diff --git a/src/trackervrpn.cpp b/src/trackervrpn.cpp index f760f0d9..9b6af241 100644 --- a/src/trackervrpn.cpp +++ b/src/trackervrpn.cpp @@ -35,12 +35,15 @@ #include "api.h" // for Publisher #include "legacy_orientation.h" // for Orientation #include "ssr_global.h" +#include "apf/math.h" // for rad2deg() + ssr::TrackerVrpn::TrackerVrpn(api::Publisher& controller , const std::string& address) : vrpn_Tracker_Remote(address.c_str()) , _controller(controller) - , _az_corr(0.0f) + //, _az_corr(0.0f) + , _tracker_orientation({}) , _stop_thread(false) { SSR_VERBOSE("Starting VRPN tracker \"" << address << "\""); @@ -48,8 +51,10 @@ ssr::TrackerVrpn::TrackerVrpn(api::Publisher& controller // TODO: what exactly is this supposed to do? //this->set_update_rate(120); - this->register_change_handler(this, _vrpn_change_handler); + // register vrpn callback + this->vrpn_Tracker_Remote::register_change_handler(&_tracker_orientation, this->handle_tracker); + // start thread _start(); // wait until tracker has started @@ -60,6 +65,8 @@ ssr::TrackerVrpn::TrackerVrpn(api::Publisher& controller ssr::TrackerVrpn::~TrackerVrpn() { + // Probably not absolutely necessary + this->vrpn_Tracker_Remote::unregister_change_handler(&_tracker_orientation, this->handle_tracker); // stop thread _stop(); // Release any ports? @@ -79,37 +86,49 @@ ssr::TrackerVrpn::create(api::Publisher& controller, const std::string& ports) } return temp; } + void VRPN_CALLBACK -ssr::TrackerVrpn::_vrpn_change_handler(void* arg, const vrpn_TRACKERCB t) +ssr::TrackerVrpn::handle_tracker(void *userdata, const vrpn_TRACKERCB t) { - return static_cast(arg)->vrpn_change_handler(t); + //this function gets called when the tracker's POSITION xform is updated + + ssr::Tracker::tracker_data *_data = reinterpret_cast(userdata); + + // https://github.com/vrpn/vrpn/wiki/Client-side-VRPN-Devices#type-definitions + double x = t.quat[0]; + double y = t.quat[1]; + double z = t.quat[2]; + double w = t.quat[3]; + + // write back to tracker_data + _data->orientation.x = x; + _data->orientation.y = y; + _data->orientation.z = z; + _data->orientation.w = w; } void -ssr::TrackerVrpn::vrpn_change_handler(const vrpn_TRACKERCB t) +ssr::TrackerVrpn::update(const tracker_data& _data) { - // TODO: check t.sensor for sensor number! - - // get quaternions information - double w = t.quat[0]; - double x = t.quat[1]; - double y = t.quat[2]; - double z = t.quat[3]; + double x = _data.orientation.x; + double y = _data.orientation.y; + double z = _data.orientation.z; + double w = _data.orientation.w; - // TODO: store _az_corr as quaternion and directly set 3D rotation + // yaw (z-axis rotation), from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles + double yaw = std::atan2(2.0f * (w * z + x * y), + 1.0f - 2.0f * (y * y + z * z)); + _current_azimuth = apf::math::rad2deg(yaw); - // calculate yaw (azimuth) (in radians) from quaternions - double azi = std::atan2(2*(w*x+y*z),1-2*(x*x+y*y)); - - _current_azimuth = azi; _controller.take_control()->reference_rotation_offset( - Orientation(-azi + _az_corr)); + Orientation(-_current_azimuth + Tracker::azi_correction)); } void ssr::TrackerVrpn::calibrate() { - _az_corr = _current_azimuth + 90; + VERBOSE2("Calibrate."); + Tracker::azi_correction = _current_azimuth + 90; } void @@ -136,8 +155,8 @@ ssr::TrackerVrpn::_thread() { while (!_stop_thread) { - this->mainloop(); - + this->vrpn_Tracker_Remote::mainloop(); + update(_tracker_orientation); // TODO: make this configurable: vrpn_SleepMsecs(10); }; diff --git a/src/trackervrpn.h b/src/trackervrpn.h index 8fa977ce..c3c8b9cc 100644 --- a/src/trackervrpn.h +++ b/src/trackervrpn.h @@ -45,7 +45,7 @@ namespace ssr namespace api { struct Publisher; } /// VRPN tracker -class TrackerVrpn : public vrpn_Tracker_Remote, public Tracker +class TrackerVrpn : public Tracker, public vrpn_Tracker_Remote { public: using ptr_t = std::unique_ptr; @@ -56,7 +56,6 @@ class TrackerVrpn : public vrpn_Tracker_Remote, public Tracker static ptr_t create(api::Publisher& controller, const std::string& ports); virtual void calibrate(); - void set_value(double azi); private: TrackerVrpn(api::Publisher& controller, const std::string& ports); @@ -67,12 +66,11 @@ class TrackerVrpn : public vrpn_Tracker_Remote, public Tracker double _current_azimuth; - float _az_corr; ///< correction of the azimuth due to calibration + Tracker::tracker_data _tracker_orientation; - static void VRPN_CALLBACK _vrpn_change_handler(void* arg - , const vrpn_TRACKERCB t); + static void VRPN_CALLBACK handle_tracker(void *userdata, const vrpn_TRACKERCB t); - void vrpn_change_handler(const vrpn_TRACKERCB t); + void update(const tracker_data& _data); // thread related stuff std::thread _tracker_thread; From edff317fa9ae9ed6770acd33f8b1abe39c34e343 Mon Sep 17 00:00:00 2001 From: Chris Hold Date: Thu, 11 Jul 2019 16:06:40 +0300 Subject: [PATCH 3/8] Use new tracker class --- src/razor-ahrs/RazorAHRS.h | 2 -- src/tracker.h | 43 +++++++++++++++++++++++---------- src/trackerpolhemus.cpp | 26 ++++++++++++-------- src/trackerpolhemus.h | 49 ++++++++++++++++---------------------- src/trackerrazor.cpp | 9 ++++--- src/trackerrazor.h | 40 ++++++++++++++++++------------- src/trackervrpn.cpp | 38 ++++++++++++----------------- src/trackervrpn.h | 27 +++++++++++---------- 8 files changed, 126 insertions(+), 108 deletions(-) diff --git a/src/razor-ahrs/RazorAHRS.h b/src/razor-ahrs/RazorAHRS.h index 5a59d021..6764dd33 100644 --- a/src/razor-ahrs/RazorAHRS.h +++ b/src/razor-ahrs/RazorAHRS.h @@ -131,11 +131,9 @@ class RazorAHRS // thread related stuff std::thread _tracker_thread; - std::atomic _stop_thread; // thread stop flag void _start(); ///< start the tracking thread void _stop(); ///< stop the tracking thread - void _thread(); // thread main function }; diff --git a/src/tracker.h b/src/tracker.h index f5bb6d82..d101d85c 100644 --- a/src/tracker.h +++ b/src/tracker.h @@ -31,30 +31,47 @@ #define SSR_TRACKER_H #include +#include -#include "api.h" namespace ssr { /// Class definition -struct Tracker +class Tracker { - virtual ~Tracker() = default; ///< destructor + public: + virtual ~Tracker() = default; ///< destructor - /// calibrate tracker; set the instantaneous position to be the reference - virtual void calibrate() = 0; + /// calibrate tracker; set the instantaneous position to be the reference + virtual void calibrate() = 0; - // Azimuth value at calibration in degree - std::atomic azi_correction{0.0f}; + // Azimuth value at calibration in degree + std::atomic azi_correction{0.0f}; - // Current tracker data - struct tracker_data - { - // Sensor orientation in quaternions - struct Rot {std::atomic x{}, y{}, z{}, w{1}; } orientation; - }; + struct Tracker_data + { + // Sensor orientation in quaternions + std::atomic x, y, z, w; + // Sensor orientation in axes rotation (in degree) + std::atomic yaw, pitch, roll; + + //constructor + Tracker_data() + : x(0.0), y(0.0), z(0.0), w(1.0), yaw(0.0), pitch(0.0), roll(0.0) + {} + }; + // Current tracker data + inline static Tracker_data current_data; + + // Update SSR + virtual void update(const Tracker::Tracker_data& _data) = 0; + + // thread related stuff + virtual void _start() = 0; ///< start the tracking thread + virtual void _stop() = 0; ///< stop the tracking thread + virtual void _thread() = 0; // thread main function }; } // namespace ssr diff --git a/src/trackerpolhemus.cpp b/src/trackerpolhemus.cpp index e4fe39e8..570697ef 100644 --- a/src/trackerpolhemus.cpp +++ b/src/trackerpolhemus.cpp @@ -38,7 +38,6 @@ #include "api.h" // for Publisher #include "legacy_orientation.h" // for Orientation #include "trackerpolhemus.h" -#include "ssr_global.h" #include "apf/stringtools.h" using apf::str::A2S; @@ -47,7 +46,7 @@ ssr::TrackerPolhemus::TrackerPolhemus(api::Publisher& controller , const std::string& type, const std::string& ports) : Tracker() , _controller(controller) - , _az_corr(0.0f) + , _current_azimuth(0.0) , _stop_thread(false) { if (ports == "") @@ -189,12 +188,6 @@ ssr::TrackerPolhemus::_open_serial_port(const char *portname) return -1; } -void -ssr::TrackerPolhemus::calibrate() -{ - _az_corr = _current_data.azimuth + 90; -} - void ssr::TrackerPolhemus::_start() { @@ -225,6 +218,17 @@ ssr::TrackerPolhemus::_thread() fds.fd = _tracker_port; fds.events = POLLRDNORM; + struct + { + float header{}; + float x{}; + float y{}; + float z{}; + float azimuth{}; + float elevation{}; + float roll{}; + } _current_data{} ; + while (!_stop_thread) { c = 0; @@ -291,7 +295,9 @@ ssr::TrackerPolhemus::_thread() >> _current_data.elevation >> _current_data.roll; - _controller.take_control()->reference_rotation_offset( - Orientation(-_current_data.azimuth + _az_corr)); + // Write back to tracker_data + Tracker::current_data.yaw = _current_data.azimuth; + Tracker::current_data.pitch = _current_data.elevation; + Tracker::current_data.roll = _current_data.roll; }; } diff --git a/src/trackerpolhemus.h b/src/trackerpolhemus.h index 507b6853..b98f28df 100644 --- a/src/trackerpolhemus.h +++ b/src/trackerpolhemus.h @@ -36,7 +36,9 @@ #include #include // for std::runtime_error -#include "tracker.h" +#include "ssr_global.h" // for ERROR, VERBOSE +#include "tracker.h" // base class + namespace ssr { @@ -55,49 +57,38 @@ class TrackerPolhemus : public Tracker static ptr_t create(api::Publisher& controller, const std::string& type , const std::string& ports); - virtual void calibrate(); + virtual void calibrate() override + { + VERBOSE2("Calibrate."); + Tracker::azi_correction = _current_azimuth + 90; + } private: /// constructor TrackerPolhemus(api::Publisher& controller, const std::string& type , const std::string& ports); - struct tracker_data_t - { - float header; - float x; - float y; - float z; - float azimuth; - float elevation; - float roll; - - // contructor - tracker_data_t() - : header(0.0f), x(0.0f), y(0.0f), z(0.0f) - , azimuth(0.0f), elevation(0.0f), roll(0.0f) - {} - }; - api::Publisher& _controller; - - tracker_data_t _current_data; + double _current_azimuth; int _tracker_port; - int _open_serial_port(const char *portname); - - float _az_corr; ///< correction of the azimuth due to calibration + int _open_serial_port(const char *portname); std::string::size_type _line_size; + void update(const Tracker::Tracker_data& _data) override + { + _current_azimuth = _data.yaw; + _controller.take_control()->reference_rotation_offset( + Orientation(-_current_azimuth + Tracker::azi_correction)); + } + // thread related stuff std::thread _tracker_thread; - std::atomic _stop_thread; // thread stop flag - void _start(); ///< start the tracking thread - void _stop(); ///< stop the tracking thread - - void _thread(); // thread main function + void _start() override; ///< start the tracking thread + void _stop() override; ///< stop the tracking thread + void _thread() override; // thread main function }; } // namespace ssr diff --git a/src/trackerrazor.cpp b/src/trackerrazor.cpp index 96b024de..4dd69284 100644 --- a/src/trackerrazor.cpp +++ b/src/trackerrazor.cpp @@ -34,9 +34,7 @@ ssr::TrackerRazor::TrackerRazor(api::Publisher& controller , const std::string& ports) : Tracker() , _controller(controller) - , _current_azimuth(0.0f) - , _az_corr(90.0f) - , _init_az_corr(true) + , _current_azimuth(0.0) , _tracker(nullptr) { if (ports == "") @@ -72,6 +70,11 @@ ssr::TrackerRazor::TrackerRazor(api::Publisher& controller { throw std::runtime_error("Could not open serial port!"); } + + // wait until tracker has started + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + this->calibrate(); } ssr::TrackerRazor::ptr_t diff --git a/src/trackerrazor.h b/src/trackerrazor.h index c841fff1..1fc21372 100644 --- a/src/trackerrazor.h +++ b/src/trackerrazor.h @@ -32,7 +32,7 @@ #define SSR_TRACKERRAZOR_H #include "legacy_orientation.h" // for Orientation -#include "ssr_global.h" // for ERROR +#include "ssr_global.h" // for ERROR, VERBOSE #include "tracker.h" // base class #include "razor-ahrs/RazorAHRS.h" @@ -57,33 +57,41 @@ class TrackerRazor : public Tracker if (_tracker != nullptr) delete _tracker; } - virtual void calibrate() { _az_corr = _current_azimuth + 90; } + virtual void calibrate() override + { + VERBOSE2("Calibrate."); + Tracker::azi_correction = _current_azimuth + 90; + } private: /// constructor TrackerRazor(api::Publisher& controller, const std::string& ports); + api::Publisher& _controller; + double _current_azimuth; + + RazorAHRS* _tracker; + /// Razor AHRS callback functions void on_data(const float ypr[]) { - // TODO: get 3D rotation - _current_azimuth = ypr[0]; - if (_init_az_corr) - { - calibrate(); - _init_az_corr = false; - } - _controller.take_control()->reference_rotation_offset( - Orientation(-_current_azimuth + _az_corr)); + Tracker::current_data.yaw = ypr[0]; + Tracker::current_data.pitch = ypr[1]; + Tracker::current_data.roll = ypr[2]; } void on_error(const std::string &msg) { SSR_ERROR("Razor AHRS: " << msg); } - api::Publisher& _controller; - volatile float _current_azimuth; - volatile float _az_corr; - volatile bool _init_az_corr; + void update(const Tracker::Tracker_data& _data) override + { + _current_azimuth = _data.yaw; + _controller.take_control()->reference_rotation_offset( + Orientation(-_current_azimuth + Tracker::azi_correction)); + }; - RazorAHRS* _tracker; + // thread related stuff + virtual void _start() override {}; // implemented in RazorAHRS + virtual void _stop() override {}; // implemented in RazorAHRS + virtual void _thread() override {}; // implemented in RazorAHRS }; } // namespace ssr diff --git a/src/trackervrpn.cpp b/src/trackervrpn.cpp index 9b6af241..b441ef18 100644 --- a/src/trackervrpn.cpp +++ b/src/trackervrpn.cpp @@ -32,9 +32,9 @@ #include // for runtime_error #include // for std::atan2() +#include "ssr_global.h" #include "api.h" // for Publisher #include "legacy_orientation.h" // for Orientation -#include "ssr_global.h" #include "apf/math.h" // for rad2deg() @@ -42,8 +42,7 @@ ssr::TrackerVrpn::TrackerVrpn(api::Publisher& controller , const std::string& address) : vrpn_Tracker_Remote(address.c_str()) , _controller(controller) - //, _az_corr(0.0f) - , _tracker_orientation({}) + , _current_azimuth(0.0) , _stop_thread(false) { SSR_VERBOSE("Starting VRPN tracker \"" << address << "\""); @@ -52,7 +51,7 @@ ssr::TrackerVrpn::TrackerVrpn(api::Publisher& controller //this->set_update_rate(120); // register vrpn callback - this->vrpn_Tracker_Remote::register_change_handler(&_tracker_orientation, this->handle_tracker); + this->vrpn_Tracker_Remote::register_change_handler(&Tracker::current_data, this->handle_tracker); // start thread _start(); @@ -66,7 +65,7 @@ ssr::TrackerVrpn::TrackerVrpn(api::Publisher& controller ssr::TrackerVrpn::~TrackerVrpn() { // Probably not absolutely necessary - this->vrpn_Tracker_Remote::unregister_change_handler(&_tracker_orientation, this->handle_tracker); + this->vrpn_Tracker_Remote::unregister_change_handler(&Tracker::current_data, this->handle_tracker); // stop thread _stop(); // Release any ports? @@ -92,7 +91,7 @@ ssr::TrackerVrpn::handle_tracker(void *userdata, const vrpn_TRACKERCB t) { //this function gets called when the tracker's POSITION xform is updated - ssr::Tracker::tracker_data *_data = reinterpret_cast(userdata); + ssr::Tracker::Tracker_data *_data = reinterpret_cast(userdata); // https://github.com/vrpn/vrpn/wiki/Client-side-VRPN-Devices#type-definitions double x = t.quat[0]; @@ -101,19 +100,19 @@ ssr::TrackerVrpn::handle_tracker(void *userdata, const vrpn_TRACKERCB t) double w = t.quat[3]; // write back to tracker_data - _data->orientation.x = x; - _data->orientation.y = y; - _data->orientation.z = z; - _data->orientation.w = w; + _data->x = x; + _data->y = y; + _data->z = z; + _data->w = w; } void -ssr::TrackerVrpn::update(const tracker_data& _data) +ssr::TrackerVrpn::update(const Tracker::Tracker_data& _data) { - double x = _data.orientation.x; - double y = _data.orientation.y; - double z = _data.orientation.z; - double w = _data.orientation.w; + double x = _data.x; + double y = _data.y; + double z = _data.z; + double w = _data.w; // yaw (z-axis rotation), from https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles double yaw = std::atan2(2.0f * (w * z + x * y), @@ -124,13 +123,6 @@ ssr::TrackerVrpn::update(const tracker_data& _data) Orientation(-_current_azimuth + Tracker::azi_correction)); } -void -ssr::TrackerVrpn::calibrate() -{ - VERBOSE2("Calibrate."); - Tracker::azi_correction = _current_azimuth + 90; -} - void ssr::TrackerVrpn::_start() { @@ -156,7 +148,7 @@ ssr::TrackerVrpn::_thread() while (!_stop_thread) { this->vrpn_Tracker_Remote::mainloop(); - update(_tracker_orientation); + update(Tracker::current_data); // TODO: make this configurable: vrpn_SleepMsecs(10); }; diff --git a/src/trackervrpn.h b/src/trackervrpn.h index c3c8b9cc..4bf94256 100644 --- a/src/trackervrpn.h +++ b/src/trackervrpn.h @@ -37,7 +37,9 @@ #include -#include "tracker.h" +#include "ssr_global.h" // for ERROR, VERBOSE +#include "tracker.h" // base class + namespace ssr { @@ -55,31 +57,32 @@ class TrackerVrpn : public Tracker, public vrpn_Tracker_Remote /// "named constructor" static ptr_t create(api::Publisher& controller, const std::string& ports); - virtual void calibrate(); + virtual void calibrate() override + { + VERBOSE2("Calibrate."); + Tracker::azi_correction = _current_azimuth + 90; + } private: + /// constructor TrackerVrpn(api::Publisher& controller, const std::string& ports); api::Publisher& _controller; - - std::string _address; - double _current_azimuth; - Tracker::tracker_data _tracker_orientation; + std::string _address; + /// VRPN callback function static void VRPN_CALLBACK handle_tracker(void *userdata, const vrpn_TRACKERCB t); - void update(const tracker_data& _data); + void update(const Tracker::Tracker_data& _data); // thread related stuff std::thread _tracker_thread; - std::atomic _stop_thread; // thread stop flag - void _start(); ///< start the tracking thread - void _stop(); ///< stop the tracking thread - - void _thread(); // thread main function + void _start() override; ///< start the tracking thread + void _stop() override; ///< stop the tracking thread + void _thread() override; // thread main function }; } // namespace ssr From f8041bba56eee5a99d76ab79432af0e4a497baaa Mon Sep 17 00:00:00 2001 From: Chris Hold Date: Fri, 12 Jul 2019 17:36:28 +0300 Subject: [PATCH 4/8] Fix update SSR with new values --- src/trackerpolhemus.cpp | 3 +++ src/trackerrazor.h | 2 ++ src/trackervrpn.cpp | 4 +++- 3 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/trackerpolhemus.cpp b/src/trackerpolhemus.cpp index 570697ef..8f4f14f2 100644 --- a/src/trackerpolhemus.cpp +++ b/src/trackerpolhemus.cpp @@ -299,5 +299,8 @@ ssr::TrackerPolhemus::_thread() Tracker::current_data.yaw = _current_data.azimuth; Tracker::current_data.pitch = _current_data.elevation; Tracker::current_data.roll = _current_data.roll; + + // Push updates to SSR + this->update(Tracker::current_data); }; } diff --git a/src/trackerrazor.h b/src/trackerrazor.h index 1fc21372..75b7fc8d 100644 --- a/src/trackerrazor.h +++ b/src/trackerrazor.h @@ -78,6 +78,8 @@ class TrackerRazor : public Tracker Tracker::current_data.yaw = ypr[0]; Tracker::current_data.pitch = ypr[1]; Tracker::current_data.roll = ypr[2]; + // Push updates to SSR + this->update(Tracker::current_data); } void on_error(const std::string &msg) { SSR_ERROR("Razor AHRS: " << msg); } diff --git a/src/trackervrpn.cpp b/src/trackervrpn.cpp index b441ef18..7763e4c4 100644 --- a/src/trackervrpn.cpp +++ b/src/trackervrpn.cpp @@ -147,8 +147,10 @@ ssr::TrackerVrpn::_thread() { while (!_stop_thread) { + // This calls the callback this->vrpn_Tracker_Remote::mainloop(); - update(Tracker::current_data); + // Push updates to SSR + this->update(Tracker::current_data); // TODO: make this configurable: vrpn_SleepMsecs(10); }; From 3d2fd032266942e447707fc7478c51c97416e930 Mon Sep 17 00:00:00 2001 From: Chris Hold Date: Mon, 15 Jul 2019 14:08:02 +0300 Subject: [PATCH 5/8] Further improvements on tracker handling, introduce data getter --- src/tracker.h | 15 ++++++++++----- src/trackerpolhemus.cpp | 9 ++++++++- src/trackerpolhemus.h | 7 +------ src/trackerrazor.cpp | 7 +++++++ src/trackerrazor.h | 9 ++------- src/trackervrpn.cpp | 2 +- src/trackervrpn.h | 2 +- 7 files changed, 30 insertions(+), 21 deletions(-) diff --git a/src/tracker.h b/src/tracker.h index d101d85c..a6963ccf 100644 --- a/src/tracker.h +++ b/src/tracker.h @@ -47,7 +47,7 @@ class Tracker virtual void calibrate() = 0; // Azimuth value at calibration in degree - std::atomic azi_correction{0.0f}; + std::atomic azi_correction{0.0}; struct Tracker_data { @@ -62,16 +62,21 @@ class Tracker : x(0.0), y(0.0), z(0.0), w(1.0), yaw(0.0), pitch(0.0), roll(0.0) {} }; - // Current tracker data - inline static Tracker_data current_data; + + // get current tracker data + const Tracker_data* const get_tracker_data() const { return &this->current_data; } // Update SSR - virtual void update(const Tracker::Tracker_data& _data) = 0; + virtual void update(const Tracker::Tracker_data &_data) = 0; // thread related stuff virtual void _start() = 0; ///< start the tracking thread virtual void _stop() = 0; ///< stop the tracking thread - virtual void _thread() = 0; // thread main function + virtual void _thread() = 0; ///< thread main function + + protected: + // Current tracker data + inline static Tracker_data current_data; }; } // namespace ssr diff --git a/src/trackerpolhemus.cpp b/src/trackerpolhemus.cpp index 8f4f14f2..fbab20fe 100644 --- a/src/trackerpolhemus.cpp +++ b/src/trackerpolhemus.cpp @@ -301,6 +301,13 @@ ssr::TrackerPolhemus::_thread() Tracker::current_data.roll = _current_data.roll; // Push updates to SSR - this->update(Tracker::current_data); + this->update(*Tracker::get_tracker_data()); }; } + +void ssr::TrackerPolhemus::update(const Tracker::Tracker_data &_data) +{ + _current_azimuth = _data.yaw; + _controller.take_control()->reference_rotation_offset( + Orientation(-_current_azimuth + Tracker::azi_correction)); +} diff --git a/src/trackerpolhemus.h b/src/trackerpolhemus.h index b98f28df..868fbde5 100644 --- a/src/trackerpolhemus.h +++ b/src/trackerpolhemus.h @@ -76,12 +76,7 @@ class TrackerPolhemus : public Tracker int _open_serial_port(const char *portname); std::string::size_type _line_size; - void update(const Tracker::Tracker_data& _data) override - { - _current_azimuth = _data.yaw; - _controller.take_control()->reference_rotation_offset( - Orientation(-_current_azimuth + Tracker::azi_correction)); - } + void update(const Tracker::Tracker_data &_data) override; // thread related stuff std::thread _tracker_thread; diff --git a/src/trackerrazor.cpp b/src/trackerrazor.cpp index 4dd69284..e0325cdf 100644 --- a/src/trackerrazor.cpp +++ b/src/trackerrazor.cpp @@ -91,3 +91,10 @@ ssr::TrackerRazor::create(api::Publisher& controller, const std::string& ports) } return temp; } + +void ssr::TrackerRazor::update(const Tracker::Tracker_data &_data) +{ + _current_azimuth = _data.yaw; + _controller.take_control()->reference_rotation_offset( + Orientation(-_current_azimuth + Tracker::azi_correction)); +} diff --git a/src/trackerrazor.h b/src/trackerrazor.h index 75b7fc8d..f71cc857 100644 --- a/src/trackerrazor.h +++ b/src/trackerrazor.h @@ -79,16 +79,11 @@ class TrackerRazor : public Tracker Tracker::current_data.pitch = ypr[1]; Tracker::current_data.roll = ypr[2]; // Push updates to SSR - this->update(Tracker::current_data); + this->update(*Tracker::get_tracker_data()); } void on_error(const std::string &msg) { SSR_ERROR("Razor AHRS: " << msg); } - void update(const Tracker::Tracker_data& _data) override - { - _current_azimuth = _data.yaw; - _controller.take_control()->reference_rotation_offset( - Orientation(-_current_azimuth + Tracker::azi_correction)); - }; + void update(const Tracker::Tracker_data &_data) override; // thread related stuff virtual void _start() override {}; // implemented in RazorAHRS diff --git a/src/trackervrpn.cpp b/src/trackervrpn.cpp index 7763e4c4..8030759b 100644 --- a/src/trackervrpn.cpp +++ b/src/trackervrpn.cpp @@ -150,7 +150,7 @@ ssr::TrackerVrpn::_thread() // This calls the callback this->vrpn_Tracker_Remote::mainloop(); // Push updates to SSR - this->update(Tracker::current_data); + this->update(*Tracker::get_tracker_data()); // TODO: make this configurable: vrpn_SleepMsecs(10); }; diff --git a/src/trackervrpn.h b/src/trackervrpn.h index 4bf94256..e1a47721 100644 --- a/src/trackervrpn.h +++ b/src/trackervrpn.h @@ -75,7 +75,7 @@ class TrackerVrpn : public Tracker, public vrpn_Tracker_Remote /// VRPN callback function static void VRPN_CALLBACK handle_tracker(void *userdata, const vrpn_TRACKERCB t); - void update(const Tracker::Tracker_data& _data); + void update(const Tracker::Tracker_data &_data); // thread related stuff std::thread _tracker_thread; From 2d78f520c05dfb7763553a91015827733303c768 Mon Sep 17 00:00:00 2001 From: Chris Hold Date: Mon, 15 Jul 2019 16:13:46 +0300 Subject: [PATCH 6/8] Fix Warning --- src/tracker.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tracker.h b/src/tracker.h index a6963ccf..351f09b2 100644 --- a/src/tracker.h +++ b/src/tracker.h @@ -64,7 +64,7 @@ class Tracker }; // get current tracker data - const Tracker_data* const get_tracker_data() const { return &this->current_data; } + const Tracker_data* get_tracker_data() const { return &this->current_data; } // Update SSR virtual void update(const Tracker::Tracker_data &_data) = 0; From fde501c3946b3abd2ea4cd390cda225e01dd4452 Mon Sep 17 00:00:00 2001 From: Chris Hold Date: Wed, 17 Jul 2019 13:36:50 +0300 Subject: [PATCH 7/8] Fix rebase onto upstream master --- src/trackerpolhemus.h | 2 +- src/trackerrazor.h | 2 +- src/trackervrpn.h | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/trackerpolhemus.h b/src/trackerpolhemus.h index 868fbde5..3e15fc13 100644 --- a/src/trackerpolhemus.h +++ b/src/trackerpolhemus.h @@ -59,7 +59,7 @@ class TrackerPolhemus : public Tracker virtual void calibrate() override { - VERBOSE2("Calibrate."); + SSR_VERBOSE2("Calibrate."); Tracker::azi_correction = _current_azimuth + 90; } diff --git a/src/trackerrazor.h b/src/trackerrazor.h index f71cc857..b34fd0e2 100644 --- a/src/trackerrazor.h +++ b/src/trackerrazor.h @@ -59,7 +59,7 @@ class TrackerRazor : public Tracker virtual void calibrate() override { - VERBOSE2("Calibrate."); + SSR_VERBOSE2("Calibrate."); Tracker::azi_correction = _current_azimuth + 90; } diff --git a/src/trackervrpn.h b/src/trackervrpn.h index e1a47721..17401baf 100644 --- a/src/trackervrpn.h +++ b/src/trackervrpn.h @@ -59,7 +59,7 @@ class TrackerVrpn : public Tracker, public vrpn_Tracker_Remote virtual void calibrate() override { - VERBOSE2("Calibrate."); + SSR_VERBOSE2("Calibrate."); Tracker::azi_correction = _current_azimuth + 90; } @@ -75,7 +75,7 @@ class TrackerVrpn : public Tracker, public vrpn_Tracker_Remote /// VRPN callback function static void VRPN_CALLBACK handle_tracker(void *userdata, const vrpn_TRACKERCB t); - void update(const Tracker::Tracker_data &_data); + void update(const Tracker::Tracker_data &_data) override; // thread related stuff std::thread _tracker_thread; From 5aca32381a06fa5975876955afca402ae3253f60 Mon Sep 17 00:00:00 2001 From: Chris Hold Date: Wed, 17 Jul 2019 18:12:24 +0300 Subject: [PATCH 8/8] Start to use quaternions, restructure --- src/controller.h | 2 +- src/geometry.h | 8 +++++++ src/tracker.h | 49 +++++++++++++++++------------------------ src/trackerpolhemus.cpp | 21 +++++------------- src/trackerpolhemus.h | 17 +++----------- src/trackerrazor.cpp | 13 ++--------- src/trackerrazor.h | 24 +++++--------------- 7 files changed, 45 insertions(+), 89 deletions(-) diff --git a/src/controller.h b/src/controller.h index 346b59d1..aaa31985 100644 --- a/src/controller.h +++ b/src/controller.h @@ -1924,7 +1924,7 @@ Controller::_calibrate_client() #if defined(ENABLE_INTERSENSE) || defined(ENABLE_POLHEMUS) || defined(ENABLE_VRPN) || defined(ENABLE_RAZOR) if (_tracker) { - _tracker->calibrate(); + _tracker->reset(); } else { diff --git a/src/geometry.h b/src/geometry.h index 0a60f391..d0e71cbf 100644 --- a/src/geometry.h +++ b/src/geometry.h @@ -66,6 +66,14 @@ struct quat : gml::quat } }; +// Converting from yaw (around Z), pitch (around Y), roll (around X) to quaternions. +// This a lot worse than the other way around and should be avoided. +template +ssr::quat ypr2quaternion(T yaw, T pitch, T roll) +{ + return gml::qrotate(gml::vec3{roll, pitch, yaw}); +} + /// Build a unit quaternion representing the rotation /// from u to v. The input vectors need not be normalised. /// From http://lolengine.net/blog/2014/02/24/quaternion-from-two-vectors-final diff --git a/src/tracker.h b/src/tracker.h index 351f09b2..1177da91 100644 --- a/src/tracker.h +++ b/src/tracker.h @@ -33,6 +33,8 @@ #include #include +#include "geometry.h" + namespace ssr { @@ -41,42 +43,31 @@ namespace ssr class Tracker { public: - virtual ~Tracker() = default; ///< destructor - - /// calibrate tracker; set the instantaneous position to be the reference - virtual void calibrate() = 0; + Tracker(api::Publisher& controller) : _controller(controller){}; ///< constructor - // Azimuth value at calibration in degree - std::atomic azi_correction{0.0}; + virtual ~Tracker() = default; ///< destructor - struct Tracker_data + /// reset tracker; set the instantaneous position to be the reference + virtual void reset() { - // Sensor orientation in quaternions - std::atomic x, y, z, w; - - // Sensor orientation in axes rotation (in degree) - std::atomic yaw, pitch, roll; - - //constructor - Tracker_data() - : x(0.0), y(0.0), z(0.0), w(1.0), yaw(0.0), pitch(0.0), roll(0.0) - {} - }; - - // get current tracker data - const Tracker_data* get_tracker_data() const { return &this->current_data; } + SSR_VERBOSE2("Tracker reset."); + this->_correction_rot = this->_current_rot; + } // Update SSR - virtual void update(const Tracker::Tracker_data &_data) = 0; - - // thread related stuff - virtual void _start() = 0; ///< start the tracking thread - virtual void _stop() = 0; ///< stop the tracking thread - virtual void _thread() = 0; ///< thread main function + virtual void update() + { + ssr::quat r = inverse(_correction_rot) * inverse(_current_rot); + _controller.take_control()->reference_rotation_offset(r); + } protected: - // Current tracker data - inline static Tracker_data current_data; + // Current tracker data (should be atomic) + ssr::quat _current_rot; + ssr::quat _correction_rot; + + private: + api::Publisher& _controller; }; } // namespace ssr diff --git a/src/trackerpolhemus.cpp b/src/trackerpolhemus.cpp index fbab20fe..4c232c49 100644 --- a/src/trackerpolhemus.cpp +++ b/src/trackerpolhemus.cpp @@ -44,9 +44,7 @@ using apf::str::A2S; ssr::TrackerPolhemus::TrackerPolhemus(api::Publisher& controller , const std::string& type, const std::string& ports) - : Tracker() - , _controller(controller) - , _current_azimuth(0.0) + : Tracker(controller) , _stop_thread(false) { if (ports == "") @@ -140,7 +138,7 @@ ssr::TrackerPolhemus::TrackerPolhemus(api::Publisher& controller // wait until tracker has started std::this_thread::sleep_for(std::chrono::milliseconds(50)); - this->calibrate(); + Tracker::reset(); } ssr::TrackerPolhemus::~TrackerPolhemus() @@ -296,18 +294,11 @@ ssr::TrackerPolhemus::_thread() >> _current_data.roll; // Write back to tracker_data - Tracker::current_data.yaw = _current_data.azimuth; - Tracker::current_data.pitch = _current_data.elevation; - Tracker::current_data.roll = _current_data.roll; + Tracker::_current_rot = ypr2quaternion(_current_data.azimuth, + _current_data.elevation, + _current_data.roll); // Push updates to SSR - this->update(*Tracker::get_tracker_data()); + Tracker::update(); }; } - -void ssr::TrackerPolhemus::update(const Tracker::Tracker_data &_data) -{ - _current_azimuth = _data.yaw; - _controller.take_control()->reference_rotation_offset( - Orientation(-_current_azimuth + Tracker::azi_correction)); -} diff --git a/src/trackerpolhemus.h b/src/trackerpolhemus.h index 3e15fc13..28b19162 100644 --- a/src/trackerpolhemus.h +++ b/src/trackerpolhemus.h @@ -57,33 +57,22 @@ class TrackerPolhemus : public Tracker static ptr_t create(api::Publisher& controller, const std::string& type , const std::string& ports); - virtual void calibrate() override - { - SSR_VERBOSE2("Calibrate."); - Tracker::azi_correction = _current_azimuth + 90; - } - private: /// constructor TrackerPolhemus(api::Publisher& controller, const std::string& type , const std::string& ports); - api::Publisher& _controller; - double _current_azimuth; - int _tracker_port; int _open_serial_port(const char *portname); std::string::size_type _line_size; - void update(const Tracker::Tracker_data &_data) override; - // thread related stuff std::thread _tracker_thread; std::atomic _stop_thread; // thread stop flag - void _start() override; ///< start the tracking thread - void _stop() override; ///< stop the tracking thread - void _thread() override; // thread main function + void _start(); ///< start the tracking thread + void _stop(); ///< stop the tracking thread + void _thread(); // thread main function }; } // namespace ssr diff --git a/src/trackerrazor.cpp b/src/trackerrazor.cpp index e0325cdf..fce55654 100644 --- a/src/trackerrazor.cpp +++ b/src/trackerrazor.cpp @@ -32,9 +32,7 @@ ssr::TrackerRazor::TrackerRazor(api::Publisher& controller , const std::string& ports) - : Tracker() - , _controller(controller) - , _current_azimuth(0.0) + : Tracker(controller) , _tracker(nullptr) { if (ports == "") @@ -74,7 +72,7 @@ ssr::TrackerRazor::TrackerRazor(api::Publisher& controller // wait until tracker has started std::this_thread::sleep_for(std::chrono::milliseconds(50)); - this->calibrate(); + Tracker::reset(); } ssr::TrackerRazor::ptr_t @@ -91,10 +89,3 @@ ssr::TrackerRazor::create(api::Publisher& controller, const std::string& ports) } return temp; } - -void ssr::TrackerRazor::update(const Tracker::Tracker_data &_data) -{ - _current_azimuth = _data.yaw; - _controller.take_control()->reference_rotation_offset( - Orientation(-_current_azimuth + Tracker::azi_correction)); -} diff --git a/src/trackerrazor.h b/src/trackerrazor.h index b34fd0e2..fba2e3ab 100644 --- a/src/trackerrazor.h +++ b/src/trackerrazor.h @@ -34,6 +34,7 @@ #include "legacy_orientation.h" // for Orientation #include "ssr_global.h" // for ERROR, VERBOSE #include "tracker.h" // base class +#include "apf/math.h" // for deg2rad() #include "razor-ahrs/RazorAHRS.h" @@ -57,38 +58,23 @@ class TrackerRazor : public Tracker if (_tracker != nullptr) delete _tracker; } - virtual void calibrate() override - { - SSR_VERBOSE2("Calibrate."); - Tracker::azi_correction = _current_azimuth + 90; - } - private: /// constructor TrackerRazor(api::Publisher& controller, const std::string& ports); - api::Publisher& _controller; - double _current_azimuth; - RazorAHRS* _tracker; /// Razor AHRS callback functions void on_data(const float ypr[]) { - Tracker::current_data.yaw = ypr[0]; - Tracker::current_data.pitch = ypr[1]; - Tracker::current_data.roll = ypr[2]; + Tracker::_current_rot = ypr2quaternion(apf::math::deg2rad(ypr[0]), + apf::math::deg2rad(ypr[1]), + apf::math::deg2rad(ypr[2])); // Push updates to SSR - this->update(*Tracker::get_tracker_data()); + Tracker::update(); } void on_error(const std::string &msg) { SSR_ERROR("Razor AHRS: " << msg); } - void update(const Tracker::Tracker_data &_data) override; - - // thread related stuff - virtual void _start() override {}; // implemented in RazorAHRS - virtual void _stop() override {}; // implemented in RazorAHRS - virtual void _thread() override {}; // implemented in RazorAHRS }; } // namespace ssr