imu_node.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, LABUST, UNIZG-FER
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the LABUST nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  *  Author : Dula Nad
00035  *  Created: 23.01.2013.
00036  *********************************************************************/
00037 #include <labust/tools/conversions.hpp>
00038 #include <labust/tools/MatrixLoader.hpp>
00039 #include <labust/math/NumberManipulation.hpp>
00040 //#include <cart2/ImuInfo.h>
00041 #include <std_msgs/Float32MultiArray.h>
00042 
00043 #include <std_msgs/String.h>
00044 #include <sensor_msgs/Imu.h>
00045 #include <sensor_msgs/NavSatFix.h>
00046 #include <tf/transform_broadcaster.h>
00047 #include <ros/ros.h>
00048 
00049 #include <boost/bind.hpp>
00050 #include <boost/asio.hpp>
00051 #include <boost/regex.hpp>
00052 #include <boost/thread.hpp>
00053 
00054 #include <iostream>
00055 #include <numeric>
00056 
00057 struct SharedData
00058 {
00059         enum {msg_size = 86,
00060                 data_offset=4,
00061                 checksum = 85,
00062                 float_offset=9};
00063         enum {lat=0, lon=1};
00064         ros::Publisher imuPub, gpsPub, imuinfo;
00065         tf::Transform imuPos, gpsPos, worldLatLon, world;
00066         tf::TransformBroadcaster broadcast;
00067         double magnetic_declination;
00068         unsigned char buffer[msg_size];
00069         std::vector<double> median[2];
00070         int gps_pub;
00071 };
00072 
00073 void start_receive(SharedData& shared,
00074                 boost::asio::serial_port& port, bool single=false);
00075 void handleIncoming(SharedData& shared,
00076                 boost::asio::serial_port& port,
00077                 const boost::system::error_code& error, const size_t& transferred);
00078 
00079 void sync(SharedData& shared,
00080                 boost::asio::serial_port& port,
00081                 const boost::system::error_code& error, const size_t& transferred)
00082 {
00083         if (!error)
00084         {
00085                 bool flag = true;
00086                 for(int i=0; i<SharedData::data_offset;++i) flag = flag && (shared.buffer[i] == 0xFF);
00087 
00088                 if (flag)
00089                 {
00090                         std::cout<<"Sync."<<std::endl;
00091                         boost::asio::async_read(port, boost::asio::buffer(&shared.buffer[SharedData::data_offset],
00092                                         SharedData::msg_size-SharedData::data_offset),
00093                                         boost::bind(&handleIncoming,
00094                                                         boost::ref(shared),
00095                                                         boost::ref(port),_1,_2));
00096                 }
00097                 else
00098                 {
00099                         //Rotate buffer to the left and read the next byte on the end.
00100                         for(int i=0; i<SharedData::data_offset-1;++i) shared.buffer[i] = shared.buffer[i+1];
00101                         std::cout<<"No sync."<<std::endl;
00102                         start_receive(shared,port,true);
00103                 }
00104         }
00105 }
00106 
00107 void handleIncoming(SharedData& shared,
00108                 boost::asio::serial_port& port,
00109                 const boost::system::error_code& error, const size_t& transferred)
00110 {
00111         std::cout<<"Got stuff."<<std::endl;
00112         if (!error && (transferred == (SharedData::msg_size-SharedData::data_offset)))
00113         {
00114                 std::cout<<"Processing."<<std::endl;
00115                 unsigned char calc = 0;
00116                 for (size_t i=SharedData::data_offset; i<SharedData::msg_size-1; ++i){calc^=shared.buffer[i];};
00117 
00118                 if (calc != shared.buffer[SharedData::checksum])
00119                 {
00120                         ROS_ERROR("Wrong checksum for imu data.");
00121                         start_receive(shared,port);
00122                         return;
00123                 }
00124 
00125                 float* data(reinterpret_cast<float*>(&shared.buffer[SharedData::data_offset + SharedData::float_offset]));
00126                 enum {sog=0, cog, declination,
00127                         accel_x, accel_y, accel_z,
00128                         gyro_x, gyro_y, gyro_z,
00129                         mag_x, mag_y, mag_z,
00130                         roll,pitch,yaw,ry,mmm,mm};
00131 
00132                 std_msgs::Float32MultiArray info;
00133                 info.data.resize(mm+6);
00134                 for (size_t i=0; i<mm+1; ++i) info.data[i+5] = data[i];
00135 
00136                 //std::cout<<"Euler:"<<data[roll]<<","<<data[pitch]<<","<<data[yaw]<<std::endl;
00137                 //std::cout<<"Magnetski:"<<data[mag_x]<<","<<data[mag_y]<<","<<data[mag_z]<<std::endl;
00138                 //std::cout<<"Test:"<<","<<data[ry]<<","<<data[mmm]<<","<<data[mm]<<std::endl;
00139 
00140                 //Send Imu stuff
00141                 sensor_msgs::Imu::Ptr imu(new sensor_msgs::Imu());
00142                 imu->header.stamp = ros::Time::now();
00143                 imu->header.frame_id = "imu_frame";
00144                 imu->linear_acceleration.x = data[accel_x];
00145                 imu->linear_acceleration.y = data[accel_y];
00146                 imu->linear_acceleration.z = data[accel_z];
00147                 imu->angular_velocity.x = data[gyro_x];
00148                 imu->angular_velocity.y = data[gyro_y];
00149                 imu->angular_velocity.z = data[gyro_z];
00150 
00151                 Eigen::Quaternion<float> quat;
00152                 labust::tools::quaternionFromEulerZYX(data[roll],
00153                                 data[pitch],
00154                                 labust::math::wrapRad(data[yaw] + shared.magnetic_declination), quat);
00155                 imu->orientation.x = quat.x();
00156                 imu->orientation.y = quat.y();
00157                 imu->orientation.z = quat.z();
00158                 imu->orientation.w = quat.w();
00159                 shared.broadcast.sendTransform(tf::StampedTransform(shared.imuPos, ros::Time::now(), "base_link", "imu_frame"));
00160                 shared.imuPub.publish(imu);
00161 
00162                 //Send GPS stuff
00163                 sensor_msgs::NavSatFix::Ptr gps(new sensor_msgs::NavSatFix());
00164 
00165                 int16_t* latlon(reinterpret_cast<int16_t*>(&shared.buffer[SharedData::data_offset]));
00166                 enum{lat=0, fraclat,lon,fraclon};
00167                 info.data[0] = latlon[lat];
00168                 info.data[1] = latlon[fraclat];
00169                 info.data[2] = latlon[lon];
00170                 info.data[3] = latlon[fraclon];
00171                 char status = info.data[4] = shared.buffer[SharedData::data_offset+SharedData::float_offset-1];
00172                 shared.imuinfo.publish(info);
00173                 gps->latitude = latlon[lat]/100 + (latlon[lat]%100 + latlon[fraclat]/10000.)/60. ;
00174                 gps->longitude = latlon[lon]/100 + (latlon[lon]%100 + latlon[fraclon]/10000.)/60.;
00175                 gps->position_covariance[0] = 9999;
00176                 gps->position_covariance[4] = 9999;
00177                 gps->position_covariance[8] = 9999;
00178                 gps->header.frame_id = "worldLatLon";
00179                 gps->header.stamp = ros::Time::now();
00180                 shared.broadcast.sendTransform(tf::StampedTransform(shared.gpsPos, ros::Time::now(), "base_link", "gps_frame"));
00181                 static int i=0;
00182                 ++i;
00183 //              if ((status == 'A') && ((i%shared.gps_pub)==0)) shared.gpsPub.publish(gps);
00184                 if (status == 'A')
00185                 {
00186                         shared.median[SharedData::lat].push_back(gps->latitude);
00187                         shared.median[SharedData::lon].push_back(gps->longitude);
00188                         if ((i%shared.gps_pub)==0)
00189                         {
00190                                 double med[2];
00191                                 for (int i=0; i<2; ++i)
00192                                 {
00193                                         int size = shared.median[i].size();
00194                                         //Median
00195 //                                      std::sort(shared.median[i].begin(),shared.median[i].end());
00196 //                                      if (size%2)
00197 //                                      {
00198 //                                              med[i] = shared.median[i][size/2-1];
00199 //                                      }
00200 //                                      else
00201 //                                      {
00202 //                                              med[i] = (shared.median[i][size/2] +
00203 //                                                              shared.median[i][size/2-1])/2;
00204 //                                      }
00205 //
00206                                         //Average
00207                                         med[i] = std::accumulate(shared.median[i].begin(), shared.median[i].end(), 0.0)/size;
00208                                         shared.median[i].clear();
00209                                 };
00210 
00211                                 gps->latitude = med[SharedData::lat];
00212                                 gps->longitude = med[SharedData::lon];
00213                                 shared.gpsPub.publish(gps);
00214                         }
00215                 }
00216 //
00217                 //Send the WorldLatLon frame update
00218                 //shared.broadcast.sendTransform(tf::StampedTransform(shared.worldLatLon, ros::Time::now(), "worldLatLon", "world"));
00219                 //shared.broadcast.sendTransform(tf::StampedTransform(shared.world, ros::Time::now(), "world", "local"));
00220         }
00221         start_receive(shared,port);
00222 }
00223 
00224 void start_receive(SharedData& shared,
00225                 boost::asio::serial_port& port, bool single)
00226 {
00227         if (single)
00228         {
00229                 boost::asio::async_read(port, boost::asio::buffer(&shared.buffer[SharedData::data_offset-1],1),
00230                                 boost::bind(&sync,
00231                                                 boost::ref(shared),
00232                                                 boost::ref(port),_1,_2));
00233         }
00234         else
00235         {
00236                 boost::asio::async_read(port, boost::asio::buffer(&shared.buffer[0],SharedData::data_offset),
00237                                 boost::bind(&sync,
00238                                                 boost::ref(shared),
00239                                                 boost::ref(port),_1,_2));
00240         }
00241 }
00242 
00243 int main(int argc, char* argv[])
00244 {
00245         ros::init(argc,argv,"imu_node");
00246         ros::NodeHandle nh,ph("~");
00247 
00248         std::string portName("/dev/ttyUSB0");
00249         int baud(115200);
00250 
00251         ph.param("PortName",portName,portName);
00252         ph.param("Baud",baud,baud);
00253 
00254         namespace ba=boost::asio;
00255         ba::io_service io;
00256         ba::serial_port port(io);
00257 
00258         port.open(portName);
00259         port.set_option(ba::serial_port::baud_rate(baud));
00260         port.set_option(ba::serial_port::flow_control(
00261                         ba::serial_port::flow_control::none));
00262 
00263         if (!port.is_open())
00264         {
00265                 std::cerr<<"Cannot open port."<<std::endl;
00266                 exit(-1);
00267         }
00268 
00269         SharedData shared;
00270         ph.param("magnetic_declination",shared.magnetic_declination,0.0);
00271         ph.param("gps_pub",shared.gps_pub,1);
00272         shared.imuPub = nh.advertise<sensor_msgs::Imu>("imu",1);
00273         shared.gpsPub = nh.advertise<sensor_msgs::NavSatFix>("fix",1);
00274         shared.imuinfo= nh.advertise<std_msgs::Float32MultiArray>("imu_info",1);
00275 
00276         //Configure Imu and GPS position relative to vehicle center of mass
00277         Eigen::Vector3d origin(Eigen::Vector3d::Zero()),
00278                         orientation(Eigen::Vector3d::Zero());
00279 
00280         labust::tools::getMatrixParam(nh, "imu_origin", origin);
00281         labust::tools::getMatrixParam(nh, "imu_orientation", orientation);
00282         shared.imuPos.setOrigin(tf::Vector3(origin(0),origin(1),origin(2)));
00283         shared.imuPos.setRotation(tf::createQuaternionFromRPY(orientation(0),
00284                         orientation(1),orientation(2)));
00285 
00286         origin = origin.Zero();
00287         orientation = orientation.Zero();
00288         labust::tools::getMatrixParam(nh, "gps_origin", origin);
00289         labust::tools::getMatrixParam(nh, "gps_orientation", orientation);
00290         shared.gpsPos.setOrigin(tf::Vector3(origin(0),origin(1),origin(2)));
00291         shared.gpsPos.setRotation(tf::createQuaternionFromRPY(orientation(0),
00292                         orientation(1),orientation(2)));
00293 
00294         //Setup the world coordinates
00295         double originLat(0), originLon(0);
00296         nh.param("LocalOriginLat",originLat,originLat);
00297         nh.param("LocalOriginLon",originLon,originLon);
00298         shared.worldLatLon.setOrigin(tf::Vector3(originLon, originLat, 0));
00299         shared.worldLatLon.setRotation(tf::createQuaternionFromRPY(0,0,0));
00300         Eigen::Quaternion<float> q;
00301         labust::tools::quaternionFromEulerZYX(M_PI,0,M_PI/2,q);
00302         shared.world.setOrigin(tf::Vector3(0,0,0));
00303         shared.world.setRotation(tf::Quaternion(q.x(),q.y(),q.z(),q.w()));
00304 
00305         start_receive(shared,port);
00306         boost::thread t(boost::bind(&ba::io_service::run,&io));
00307 
00308         ros::spin();
00309         io.stop();
00310         t.join();
00311 
00312         return 0;
00313 }
00314 
00315 


labust_imu
Author(s): Gyula Nagy
autogenerated on Fri Feb 7 2014 11:37:32