SensorHandlers.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 #include <labust/navigation/SensorHandlers.hpp>
00035 #include <labust/math/NumberManipulation.hpp>
00036 #include <labust/tools/GeoUtilities.hpp>
00037 
00038 #include <kdl/frames.hpp>
00039 
00040 using namespace labust::navigation;
00041 
00042 void GPSHandler::configure(ros::NodeHandle& nh)
00043 {
00044         gps = nh.subscribe<sensor_msgs::NavSatFix>("gps", 1,
00045                         &GPSHandler::onGps, this);
00046 }
00047 
00048 void GPSHandler::onGps(const sensor_msgs::NavSatFix::ConstPtr& data)
00049 {
00050         //Calculate to X-Y tangent plane
00051         tf::StampedTransform transformDeg, transformLocal;
00052         try
00053         {
00054                 listener.lookupTransform("local", "gps_frame", ros::Time(0), transformLocal);
00055                 listener.lookupTransform("/worldLatLon", "local", ros::Time(0), transformDeg);
00056                 posxy = labust::tools::deg2meter(data->latitude - transformDeg.getOrigin().y(),
00057                                         data->longitude - transformDeg.getOrigin().x(),
00058                                         transformDeg.getOrigin().y());
00059 
00060                 originLL.first = transformDeg.getOrigin().y();
00061                 originLL.second = transformDeg.getOrigin().x();
00062 
00063                 posLL.first = data->latitude;
00064                 posLL.second = data->longitude;
00065 
00066                 isNew = true;
00067         }
00068         catch(tf::TransformException& ex)
00069         {
00070                 ROS_WARN("Unable to decode GPS measurement. Missing frame : %s",ex.what());
00071         }
00072 };
00073 
00074 void ImuHandler::configure(ros::NodeHandle& nh)
00075 {
00076         imu = nh.subscribe<sensor_msgs::Imu>("imu", 1,
00077                         &ImuHandler::onImu, this);
00078 }
00079 
00080 void ImuHandler::onImu(const sensor_msgs::Imu::ConstPtr& data)
00081 {
00082         tf::StampedTransform transform;
00083         try
00084         {
00085                 listener.lookupTransform("base_link", "imu_frame", ros::Time(0), transform);
00086                 tf::Quaternion meas(data->orientation.x,data->orientation.y,
00087                                 data->orientation.z,data->orientation.w);
00088                 tf::Quaternion result = meas*transform.getRotation();
00089                 KDL::Rotation::Quaternion(result.x(),result.y(),result.z(),result.w()).GetEulerZYX
00090                                 (rpy[yaw],rpy[pitch],rpy[roll]);
00091                 rpy[yaw] = unwrap(rpy[yaw]);
00092 
00093                 //Transform angular velocities
00094 
00095                 pqr[p] = data->angular_velocity.x;
00096                 pqr[q] = data->angular_velocity.y;
00097                 pqr[r] = data->angular_velocity.z;
00098 
00099                 //Transform the accelerations
00100 
00101                 axyz[ax] = data->linear_acceleration.x;
00102                 axyz[ay] = data->linear_acceleration.y;
00103                 axyz[az] = data->linear_acceleration.z;
00104 
00105                 isNew = true;
00106         }
00107         catch (tf::TransformException& ex)
00108         {
00109                 ROS_WARN("Failed converting the IMU data: %s",ex.what());
00110         }
00111 };
00112 
00113 void DvlHandler::configure(ros::NodeHandle& nh)
00114 {
00115         nu_dvl = nh.subscribe<geometry_msgs::TwistStamped>("dvl", 1,
00116                         &DvlHandler::onDvl, this);
00117 
00118         uvw[u] = uvw[v] = uvw[w] = 0;
00119 }
00120 
00121 void DvlHandler::onDvl(const geometry_msgs::TwistStamped::ConstPtr& data)
00122 {
00123         if (data->header.frame_id == "dvl_frame")
00124         {
00125                 try
00126                 {
00127                         tf::StampedTransform transform;
00128                         listener.lookupTransform("base_link", "dvl_frame", ros::Time(0), transform);
00129 
00130                         tf::Vector3 speed(data->twist.linear.x, data->twist.linear.y, data->twist.linear.z);
00131                         tf::Vector3 body_speed = transform.getBasis()*speed;
00132 
00133                         //Add compensation for excentralized DVL
00134                         tf::Vector3 origin=transform.getOrigin();
00135                         if (origin.x() != 0 || origin.y() != 0)
00136                         {
00137                                 double ang = atan2(origin.y(), origin.x());
00138                                 double vm = r*sqrt(origin.x()*origin.x() + origin.y()*origin.y());
00139                                 body_speed -= tf::Vector3(vm*sin(ang),vm*cos(ang),0);
00140                         }
00141 
00142                         uvw[u] = body_speed.x();
00143                         uvw[v] = body_speed.y();
00144                         uvw[w] = body_speed.z();
00145                 }
00146                 catch (std::exception& ex)
00147                 {
00148                         ROS_WARN("DVL measurement failure:%s",ex.what());
00149                         isNew = false;
00150                         return;
00151                 }
00152         }
00153         else if (data->header.frame_id == "base_link")
00154         {
00155                 uvw[u] = data->twist.linear.x;
00156                 uvw[v] = data->twist.linear.y;
00157                 uvw[w] = data->twist.linear.z;
00158         }
00159         else if (data->header.frame_id == "local")
00160         {
00161                 tf::StampedTransform transform;
00162                 tf::Vector3 meas(data->twist.linear.x,
00163                                 data->twist.linear.y,
00164                                 data->twist.linear.z);
00165                 listener.lookupTransform("local", "base_link", ros::Time(0), transform);
00166                 tf::Vector3 result = transform.getBasis()*meas;
00167                 uvw[u] = result.x();
00168                 uvw[v] = result.y();
00169                 uvw[w] = result.z();
00170         }
00171         else
00172         {
00173                 isNew = false;
00174                 return;
00175         }
00176 
00177         isNew = true;
00178 }


labust_navigation
Author(s): Gyula Nagy
autogenerated on Fri Feb 7 2014 11:36:19