/* @file tools/sdk/cpp/Format.hpp @author Luke Tokheim, luke@motionnode.com @version 1.1 (C) Copyright GLI Interactive LLC 2008. All rights reserved. The coded instructions, statements, computer programs, and/or related material (collectively the "Data") in these files contain unpublished information proprietary to GLI Interactive LLC, which is protected by US federal copyright law and by international treaties. The Data may not be disclosed or distributed to third parties, in whole or in part, without the prior written consent of GLI Interactive LLC. The Data is provided "as is" without express or implied warranty, and with no claim as to its suitability for any purpose. */ #ifndef __MOTION_NODE_SDK_FORMAT_HPP_ #define __MOTION_NODE_SDK_FORMAT_HPP_ #include #include #include #include #include #include namespace MotionNode { namespace SDK { /** The Format class methods read a single binary message from a MotionNode service and returns an object representation of that message. This is layered (by default) on top of the @ref Client class which handles the socket level binary message protocol. Example usage, extends the @ref Client class example: @code try { using MotionNode::SDK::Client; using MotionNode::SDK::Format; // Connect to the Preview data service. Client client("", 32079); Client::data_type data; while (client.readData(data)) { // Create an object representation of the current binary message. Format::preview_service_type preview = Format::Preview(data.begin(), data.end()); // Iterate through the list of [id] => PreviewElement objects. for (Format::preview_service_type::iterator itr=preview.begin(); itr!=preview.end(); ++itr) { // Use the PreviewElement interface to access format specific data. const Format::PreviewElement & element = itr->second; } } } catch (std::runtime_error & e) { // The Client and Format class with throw std::runtime_error for // any unrecoverable conditions. } @endcode */ class Format { public: /** Defines the type of the integer map key for all Format types. */ typedef std::size_t id_type; /** Defines the type of the data array for all Format types. */ typedef std::vector data_type; /** MotionNode services send a list of data elements. The @ref Format functions create a map from integer id to array packed data for each service specific format. This is an abstract base class to implement a single format specific data element. The idea is that a child class implements a format specific interface (API) to access individual components of an array of packed data. The template parameter defines the type of the packed data elements. For example, the @ref PreviewElement class extends this class and provides a @ref PreviewElement#getEuler method to access an array of {x, y, z} Euler angles. */ template class Element { public: typedef std::vector data_type; protected: //typedef std::vector data_type; /** Constructor is protected. Only allow child classes to call this. @param data array of packed data values for this Format::Element @param length valid length of the data array @pre data.size() == length @throws std::runtime_error if the data array does not have element_length values */ Element(const data_type & data, const typename data_type::size_type & length) : m_data(data) { if (length != m_data.size()) { throw std::runtime_error("invalid input data for format element"); } } /** Utility function to copy portions of the packed data array into its component elements. @param base starting index to copy data from the internal data array @param length number of data values in this component element @pre base < m_data.size(), base + element_length < m_data.size() @return an array of element_length values, assigned to {m_data[i] ... m_data[i+element_length]} if there are valid values available or zeros otherwise */ data_type getData( const typename data_type::size_type & base, const typename data_type::size_type & length) const { data_type result(length); if (base + length <= m_data.size()) { std::copy( m_data.begin() + base, m_data.begin() + base + length, result.begin()); } return result; } private: /** Array of packed binary data for this element. If data.empty() == false then it contains a sample for each of the N channels. Define this as a private member. Only allow access through the getData method, and only allow it to child classes. */ data_type m_data; /** Provide direct access to the internal data buffer from client programs. @code class ElementAccess { public: template static const typename Format::Element::data_type & get(const Format::Element & element) { return element.m_data; } }; @endcode */ friend class ElementAccess; public: const data_type & access() const { return m_data; } }; // class Element /** The Preview service provides access to the current orientation output as a quaternion, a set of Euler angles, or a 4-by-4 rotation matrix. The Preview service sends a map of N Preview data elements. Use this class to wrap a single Preview data element such that we can access individual components through a simple API. Preview element format: id => [global quaternion, local quaternion, local euler, global acceleration] id => {Gqw, Gqx, Gqy, Gqz, Lqw, Lqx, Lqy, Lqz, rx, ry, rz, ax, ay, az} */ class PreviewElement : public Element { public: typedef Format::Element::data_type data_type; /** Two quaternion channels, two 3-axis channels. */ const static std::size_t Length = 2*4 + 2*3; static std::string Name; /** Initialize this container identifier with a packed data array in the Preview format. @param data is a packed array of global quaternion, local quaternion, local Euler angle, and local translation channel data @pre data.size() == Length */ PreviewElement(const data_type & data); /** Get a set of x, y, and z Euler angles that define the current orientation. Specified in radians assuming x-y-z rotation order. Not necessarily continuous over time, each angle lies on the domain [-pi, pi]. Euler angles are computed on the server side based on the current local quaternion orientation. @return a three element array {x, y, z} of Euler angles in radians or zeros if there is no available data */ data_type getEuler() const; /** Get a 4-by-4 rotation matrix from the current global or local quaternion orientation. Specified as a 16 element array in row-major order. @param local set local to true get the local orientation, set local to false to get the global orientation */ data_type getMatrix(bool local) const; /** Get the global or local unit quaternion that defines the current orientation. @param local set local to true get the local orientation, set local to false to get the global orientation @return a four element array {w, x, y, z} that defines a unit length quaternion q = w + x*i + y*j + z*k or zeros if there is no available data */ data_type getQuaternion(bool local) const; /** Get x, y, and z of the current estimate of linear acceleration. Specified in g. @return a three element array {x, y, z} of linear acceleration channels specified in g or zeros if there is no available data */ data_type getAccelerate() const; }; // class PreviewElement /** The Sensor service provides access to the current un-filtered sensor signals in real units. The Sensor service sends a map of N data elements. Use this class to wrap a single Sensor data element such that we can access individual components through a simple API. Sensor element format: id => [accelerometer, magnetometer, gyroscope] id => {ax, ay, az, mx, my, mz, gx, gy, gz} */ class SensorElement : public Element { public: typedef Format::Element::data_type data_type; /** Three 3-axis channels. */ const static std::size_t Length = 3*3; static std::string Name; /** Initialize this container identifier with a packed data array in the Sensor format. @param data is a packed array of accelerometer, magnetometer, and gyroscope un-filtered signal data. @pre data.size() == Length */ SensorElement(const data_type & data); /** Get a set of x, y, and z values of the current un-filtered accelerometer signal. Specified in g where 1 g = 9.80665 meter/second^2. Domain varies with configuration. Maximum is [-6, 6] g. @return a three element array {x, y, z} of acceleration in gs or zeros if there is no available data */ data_type getAccelerometer() const; /** Get a set of x, y, and z values of the current un-filtered gyroscope signal. Specified in degree/second. Valid domain of the sensor is [-500, 500] degree/second. Expect values outside of this domain as the system does not crop the sensor outputs. @return a three element array {x, y, z} of angular velocity in degree/second or zeros if there is no available data */ data_type getGyroscope() const; /** Get a set of x, y, and z values of the current un-filtered magnetometer signal. Specified in µT (microtesla). Domain varies with local magnetic field strength. Expect values on domain [-60, 60] µT (microtesla). @return a three element array {x, y, z} of magnetic field strength in µT (microtesla) or zeros if there is no available data */ data_type getMagnetometer() const; }; // class SensorElement /** The Raw service provides access to the current uncalibrated, unprocessed sensor signals in signed integer format. The Raw service sends a map of N data elements. Use this class to wrap a single Raw data element such that we can access individual components through a simple API. Raw element format: id => [accelerometer, magnetometer, gyroscope] id => {ax, ay, az, mx, my, mz, gx, gy, gz} All sensors output 12-bit integers. Process as 16-bit short integers on the server side. */ class RawElement : public Element { public: typedef Format::Element::data_type data_type; /** Three 3-axis channels. */ const static std::size_t Length = 3*3; static std::string Name; /** Initialize this container identifier with a packed data array in the Raw format. @param data is a packed array of accelerometer, magnetometer, and gyroscope unprocessed signal data @pre data.size() == Length */ RawElement(const data_type & data); /** Get a set of x, y, and z values of the current unprocessed accelerometer signal. Valid domain is [0, 4095]. @return a three element array {x, y, z} of raw accelerometer output or zeros if there is no available data */ data_type getAccelerometer() const; /** Get a set of x, y, and z values of the current unprocessed gyroscope signal. Valid domain is [0, 4095]. @return a three element array {x, y, z} of raw gyroscope output or zeros if there is no available data */ data_type getGyroscope() const; /** Get a set of x, y, and z values of the current unprocessed magnetometer signal. Valid domain is [0, 4095]. @return a three element array {x, y, z} of raw magnetometer output or zeros if there is no available data */ data_type getMagnetometer() const; }; // class RawElement /** Define the associative container type for PreviewElement entries. */ typedef std::map< id_type, PreviewElement > preview_service_type; /** Convert a range of binary data into an associative container (std::map) of PreviewElement entries. @pre [first, last) is a valid range @return an associative container PreviewElement entries */ template static inline preview_service_type Preview(InputIterator first, InputIterator last) { return Apply(first, last); } /** Define the associative container type for SensorElement entries. */ typedef std::map< id_type, SensorElement > sensor_service_type; /** Convert a range of binary data into an associative container (std::map) of SensorElement entries. @pre [first, last) is a valid range @return an associative container SensorElement entries */ template static inline sensor_service_type Sensor(InputIterator first, InputIterator last) { return Apply(first, last); } /** Define the associative container type for RawElement entries. */ typedef std::map< id_type, RawElement > raw_service_type; /** Convert a range of binary data into an associative container (std::map) of RawElement entries. @pre [first, last) is a valid range @return an associative container RawElement entries */ template static inline raw_service_type Raw(InputIterator first, InputIterator last) { return Apply(first, last); } private: /** Convert a binary packed data representation from a MotionNode service into a std::map. Use the IdToValueArray method to handle the low level message parsing. */ template static std::map Apply(InputIterator first, InputIterator last) { std::map result; { // Use this to do most of the dirty work. std::map map = IdToValueArray(first, last, T::Length); if (!map.empty()) { // Copy each [id] => vector entry into the [id] => PreviewMap result map. // Use the range insert operator since we can statically cast a vector // object directly into a PreviewElement. result.insert(map.begin(), map.end()); // Make sure that we were able to insert all of the elements. if (map.size() != result.size()) { result.clear(); } } } return result; } /** Convert a binary packed data representation from a MotionNode service into a std::map. @pre [first, last) is a valid range @pre type Key is an integral type @pre type Value is a model of a Sequence (STL) */ template static std::map IdToValueArray(InputIterator first, InputIterator last, const std::size_t & length) { typedef typename InputIterator::difference_type difference_type; typedef unsigned packed_key_type; std::map result; if ((length > 0) && (std::distance(first, last) > 0)) { // Compute the size in bytes of a single element. Conside the input // buffer to be a packed set of element entries. const std::size_t element_size = sizeof(packed_key_type) + sizeof(typename Value::value_type) * length; // while we have enough bytes to create a complete element. InputIterator itr = first; while ((itr != last) && (static_cast(element_size) <= std::distance(itr, last))) { std::pair value; // Read the integer id for this element. Unpack the unsigned 32-bit integer // into our host system key type. { packed_key_type key_value = *reinterpret_cast(&(*itr)); std::advance(itr, sizeof(packed_key_type)); value.first = static_cast(detail::little_endian_to_native(key_value)); } // Read the array of values for this element. value.second.resize(length); for (typename Value::iterator value_itr=value.second.begin(); value_itr!=value.second.end(); ++value_itr) { *value_itr = *reinterpret_cast(&(*itr)); std::advance(itr, sizeof(typename Value::value_type)); } // Big-endian systems need to implement byte swapping here. All // service data is little-endian. std::transform( value.second.begin(), value.second.end(), value.second.begin(), &detail::little_endian_to_native); result.insert(value); } // If we did not consume all of the input bytes this is an // invalid message. if (itr != last) { result.clear(); } } return result; } /** Hide the constructor. There is no need to instantiate the Format object. */ Format(); }; // class Format }} // namespace MotionNode::SDK #endif // __MOTION_NODE_SDK_FORMAT_HPP_