main.cpp
Go to the documentation of this file.
00001 /* Copyright (c) Xsens Technologies B.V., 2006-2012. All rights reserved.
00002 
00003           This source code is provided under the MT SDK Software License Agreement
00004 and is intended for use only by Xsens Technologies BV and
00005            those that have explicit written permission to use it from
00006            Xsens Technologies BV.
00007 
00008           THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
00009            KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
00010            IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
00011            PARTICULAR PURPOSE.
00012  */
00013 
00014 //--------------------------------------------------------------------------------
00015 // ROS driver for Xsens MTi-10 and MTi-100 series
00016 //
00017 //--------------------------------------------------------------------------------
00018 #include <xsensdeviceapi.h> // The Xsens device API header
00019 #include "serialkey.h"
00020 
00021 #include <iostream>
00022 #include <list>
00023 #include <iomanip>
00024 #include <stdexcept>
00025 
00026 #include <xsens/xstime.h>
00027 
00028 #include "conio.h" // for non ANSI _kbhit() and _getch()
00029 
00030 #include <ros/ros.h>
00031 #include <sensor_msgs/Imu.h>
00032 
00033 class CallbackHandler : public XsCallback
00034 {
00035 public:
00036         CallbackHandler(size_t maxBufferSize = 5) : m_maxNumberOfPacketsInBuffer(maxBufferSize), m_numberOfPacketsInBuffer(0)
00037 #ifdef _MSC_VER
00038         {InitializeCriticalSection(&m_CriticalSection);}
00039         virtual ~CallbackHandler() throw() {DeleteCriticalSection(&m_CriticalSection);}
00040 #else
00041         {
00042           //create mutex attribute variable
00043           pthread_mutexattr_t mAttr;
00044 
00045           // setup recursive mutex for mutex attribute
00046           pthread_mutexattr_settype(&mAttr, PTHREAD_MUTEX_RECURSIVE_NP);
00047 
00048           // Use the mutex attribute to create the mutex
00049           pthread_mutex_init(&m_CriticalSection, &mAttr);
00050 
00051           // Mutex attribute can be destroy after initializing the mutex variable
00052           pthread_mutexattr_destroy(&mAttr);
00053 
00054         }
00055         virtual ~CallbackHandler() throw() {pthread_mutex_destroy(&m_CriticalSection);}
00056 #endif
00057 
00058         bool packetAvailable() const {Locker lock(*this); return m_numberOfPacketsInBuffer > 0;}
00059         XsDataPacket getNextPacket()
00060         {
00061                 assert(packetAvailable());
00062                 Locker lock(*this);
00063                 XsDataPacket oldestPacket(m_packetBuffer.front());
00064                 m_packetBuffer.pop_front();
00065                 --m_numberOfPacketsInBuffer;
00066                 return oldestPacket;
00067         }
00068 
00069 protected:
00070         virtual void onDataAvailable(XsDevice*, const XsDataPacket* packet)
00071         {
00072                 Locker lock(*this);
00073                 assert(packet != 0);
00074                 while (m_numberOfPacketsInBuffer >= m_maxNumberOfPacketsInBuffer)
00075                 {
00076                         (void)getNextPacket();
00077                 }
00078                 m_packetBuffer.push_back(*packet);
00079                 ++m_numberOfPacketsInBuffer;
00080                 assert(m_numberOfPacketsInBuffer <= m_maxNumberOfPacketsInBuffer);
00081         }
00082 private:
00083 #ifdef _MSC_VER
00084         mutable CRITICAL_SECTION m_CriticalSection;
00085 #else
00086         mutable pthread_mutex_t m_CriticalSection;
00087 #endif
00088         struct Locker
00089         {
00090 #ifdef _MSC_VER
00091                 Locker(CallbackHandler const & self) : m_self(self) {EnterCriticalSection(&m_self.m_CriticalSection);}
00092                 ~Locker() throw() {LeaveCriticalSection(&m_self.m_CriticalSection);}
00093 #else
00094                 Locker(CallbackHandler const & self) : m_self(self) {pthread_mutex_lock(&m_self.m_CriticalSection);}
00095                 ~Locker() throw() {pthread_mutex_unlock(&m_self.m_CriticalSection);}
00096 #endif
00097                 CallbackHandler const & m_self;
00098         };
00099         size_t m_maxNumberOfPacketsInBuffer;
00100         size_t m_numberOfPacketsInBuffer;
00101         std::list<XsDataPacket> m_packetBuffer;
00102 };
00103 
00104 //--------------------------------------------------------------------------------
00105 int main(int argc, char** argv)
00106 {
00107         ros::init(argc, argv, "receive_xsens");
00108         ros::NodeHandle nh;
00109 
00110         ros::Publisher publisher = nh.advertise<sensor_msgs::Imu> ("/imu/data", 1);
00111 
00112         if (!setSerialKey())
00113         {
00114                 std::cout << "Invalid serial key." << std::endl;
00115                 std::cout << "Press [ENTER] to continue." << std::endl; std::cin.get();
00116                 return 1;
00117         }
00118 
00119         // Create XsControl object
00120         XsControl* control = XsControl::construct();
00121         assert(control != 0);
00122 
00123         try
00124         {
00125                 // Scan for connected devices
00126                 XsPortInfoArray portInfoArray = XsScanner::scanPorts();
00127 
00128                 // Find an MTi / MTx / MTmk4 device
00129                 XsPortInfoArray::const_iterator mtPort = portInfoArray.begin();
00130                 while (mtPort != portInfoArray.end() && !mtPort->deviceId().isMtMk4()) {++mtPort;}
00131                 if (mtPort == portInfoArray.end())
00132                 {
00133                         throw std::runtime_error("No MTi / MTx / MTmk4 device found. Aborting.");
00134                 }
00135                 std::cout << "Found a device with id: " << mtPort->deviceId().toString().toStdString() << " @ port: " << mtPort->portName().toStdString() << ", baudrate: " << mtPort->baudrate() << std::endl;
00136 
00137                 // Open the port with the detected device
00138                 if (!control->openPort(mtPort->portName().toStdString(), mtPort->baudrate()))
00139                 {
00140                         throw std::runtime_error("Could not open port. Aborting.");
00141                 }
00142 
00143                 try
00144                 {
00145                         // Get the device object
00146                         XsDevice* device = control->device(mtPort->deviceId());
00147                         assert(device != 0);
00148 
00149                         // Print information about detected MTi / MTx / MTmk4 device
00150                         std::cout << "Device: " << device->productCode().toStdString() << " opened." << std::endl;
00151 
00152                         // Create and attach callback handler to device
00153                         CallbackHandler callback;
00154                         device->addCallbackHandler(&callback);
00155 
00156                         // Put the device in measurement mode
00157                         if (!device->gotoMeasurement())
00158                         {
00159                                 throw std::runtime_error("Could not put device into measurement mode. Aborting.");
00160                         }
00161 
00162                         //std::cout << "\nMain loop (press Ctrl+C to quit)" << std::endl;
00163                         //std::cout << std::string(79, '-') << std::endl;
00164                         while (ros::ok())
00165                         {
00166                                 if (callback.packetAvailable())
00167                                 {
00168                                         // Create ROS message
00169                                         sensor_msgs::Imu imuData;
00170                                         imuData.header.frame_id = "/imu";
00171                                         imuData.header.stamp = ros::Time::now();
00172 
00173                                         // Retrieve a packet
00174                                         XsDataPacket packet = callback.getNextPacket();
00175 
00176                                         // Get the orientation data
00177                                         if (packet.containsOrientation()) {
00178                                                 /*XsEuler euler = packet.orientationEuler();
00179                                                 std::cout << "\r"
00180                                                           << "Roll:" << std::setw(5) << std::fixed 
00181 << std::setprecision(2) << euler.m_roll
00182                                                           << ", Pitch:" << std::setw(5) << std::fixed << std::setprecision(2) << euler.m_pitch
00183                                                           << ", Yaw:" << std::setw(5) << std::fixed << std::setprecision(2) << euler.m_yaw;
00184                                                 std::cout << std::flush;*/
00185 
00186                                                 XsQuaternion quaternion = packet.orientationQuaternion();
00187 
00188                                                 imuData.orientation.x = quaternion.m_x;
00189                                                 imuData.orientation.y = quaternion.m_y;
00190                                                 imuData.orientation.z = quaternion.m_z;
00191                                                 imuData.orientation.w = quaternion.m_w;
00192                                         }
00193 
00194                                         // Get the gyroscope data
00195                                         if (packet.containsCalibratedGyroscopeData()) {
00196                                                 XsVector gyroscope = packet.calibratedGyroscopeData();
00197                                         
00198                                                 imuData.angular_velocity.x = gyroscope.at(0);
00199                                                 imuData.angular_velocity.y = gyroscope.at(1);
00200                                                 imuData.angular_velocity.z = gyroscope.at(2);
00201                                         }
00202 
00203                                         // Get the acceleration data
00204                                         if (packet.containsCalibratedAcceleration()) {
00205                                                 XsVector acceleration = packet.calibratedAcceleration();
00206 
00207                                                 imuData.linear_acceleration.x = acceleration.at(0);
00208                                                 imuData.linear_acceleration.y = acceleration.at(1);
00209                                                 imuData.linear_acceleration.z = acceleration.at(2);
00210                                         }
00211 
00212                                         // Get the magnetic field data
00213                                         /*if (packet.containsCalibratedMagneticField()) {
00214                                                 XsVector magneticField = packet.calibratedMagneticField();
00215                                         }*/
00216 
00217                                         // Publish ROS message
00218                                         publisher.publish(imuData);
00219                                 }
00220 
00221                                 ros::spinOnce();
00222                                 XsTime::msleep(1.0);
00223                         }
00224                         //std::cout << "\r" << std::string(79, '-') << "\n";
00225                         //std::cout << std::endl;
00226                 }
00227                 catch (std::runtime_error const & error)
00228                 {
00229                         std::cout << error.what() << std::endl;
00230                 }
00231                 catch (...)
00232                 {
00233                         std::cout << "An unknown fatal error has occured. Aborting." << std::endl;
00234                 }
00235 
00236                 // Close port
00237                 control->closePort(mtPort->portName().toStdString());
00238         }
00239         catch (std::runtime_error const & error)
00240         {
00241                 std::cout << error.what() << std::endl;
00242         }
00243         catch (...)
00244         {
00245                 std::cout << "An unknown fatal error has occured. Aborting." << std::endl;
00246         }
00247 
00248         // Free XsControl object
00249         control->destruct();
00250 
00251         std::cout << "Successful exit." << std::endl;
00252 
00253         return 0;
00254 }


receive_xsens
Author(s): Ji Zhang, Silvio Maeta
autogenerated on Sat Jun 8 2019 19:32:50