compass_odom_alg.cpp
Go to the documentation of this file.
00001 #include "compass_odom_alg.h"
00002 
00003 CompassOdomAlgorithm::CompassOdomAlgorithm()
00004 {
00005 }
00006 
00007 CompassOdomAlgorithm::~CompassOdomAlgorithm()
00008 {
00009 }
00010 
00011 void CompassOdomAlgorithm::config_update(const Config& new_cfg, uint32_t level)
00012 {
00013   this->lock();
00014 
00015   // save the current configuration
00016   this->config_=new_cfg;
00017 
00018   this->unlock();
00019 }
00020 
00021 // CompassOdomAlgorithm Public API
00022 
00023 //    CONVERSION FROM COMPASS MSG TO ODOMETRY MSG
00024 // -----------------------------------------------------------------------------
00025 
00026 void CompassOdomAlgorithm::getOdometry(nav_msgs::Odometry & o, geometry_msgs::TransformStamped & t)
00027 {
00028   // compass3axis [yaw,roll,-pitch]
00029   // odom [roll,pitch,yaw]
00030 
00031   double rc[3]; // last angle in radians
00032   double r[3];  // angles in radians
00033   double w[3];  // angular speed
00034 
00035   for(uint i=0;i<3;i++)
00036   {
00037     rc[i] = new_compass_.angles[i] * M_PI / 180;
00038     r[i] = ( new_compass_.angles[i] - old_compass_.angles[i] ) * M_PI / 180;
00039     w[i] = r[i] / ( new_compass_.header.stamp.toSec() - old_compass_.header.stamp.toSec() );
00040   }
00041 
00042   geometry_msgs::Quaternion or_q = tf::createQuaternionMsgFromRollPitchYaw( rc[2], -rc[1], -rc[0] );
00043 
00044   //   Header header
00045   //   uint32 seq
00046   //   time stamp
00047   //   string frame_id
00048   //   float64[3] angles
00049   //   float64[9] covariances
00050   //   bool[3] alarms
00051   // --------------------------
00052   //   Header header
00053   //    uint32 seq
00054   //    time stamp
00055   //    string frame_id
00056   o.header.stamp = new_compass_.header.stamp;
00057   o.header.frame_id = parent_id_;
00058   //   string child_frame_id
00059   o.child_frame_id  = new_compass_.header.frame_id;
00060   //   geometry_msgs/PoseWithCovariance pose
00061   //    geometry_msgs/Pose pose
00062   //      geometry_msgs/Point position
00063   //        float64 x
00064   //        float64 y
00065   //        float64 z
00066   //      geometry_msgs/Quaternion orientation
00067   //        float64 x
00068   //        float64 y
00069   //        float64 z
00070   //        float64 w
00071   //    float64[36] covariance
00072   o.pose.pose.position.x  = 0.0;
00073   o.pose.pose.position.y  = 0.0;
00074   o.pose.pose.position.z  = 0.0;
00075   o.pose.pose.orientation = or_q;
00076   for(uint i=0;i<36;i++)
00077     o.pose.covariance[i] = 999;
00078   o.pose.covariance[0] = new_compass_.covariances[0];
00079   o.pose.covariance[4] = new_compass_.covariances[4];
00080   o.pose.covariance[8] = 1;//new_compass_.covariances[8];
00081   o.twist.twist.linear.x  =  0.0;
00082   o.twist.twist.linear.y  =  0.0;
00083   o.twist.twist.linear.z  =  0.0;
00084   o.twist.twist.angular.x =  w[2];  // roll
00085   o.twist.twist.angular.y =  -w[1]; // pitch
00086   o.twist.twist.angular.z =  -w[0];  // yaw
00087   //o.twist.covariance = covs;
00088 
00089   t.header.stamp = new_compass_.header.stamp;
00090   t.header.frame_id = parent_id_;
00091   t.child_frame_id  = new_compass_.header.frame_id;
00092   t.transform.translation.x = 0.0;
00093   t.transform.translation.y = 0.0;
00094   t.transform.translation.z = 0.0;
00095   t.transform.rotation      = or_q;
00096 
00097 }
00098 
00099 void CompassOdomAlgorithm::getImuMessage(nav_msgs::Odometry & o, sensor_msgs::Imu & imu)
00100 {
00101   imu.header                = o.header;
00102   imu.orientation           = o.pose.pose.orientation;
00103   imu.orientation_covariance[0] = o.pose.covariance[0];
00104   imu.orientation_covariance[4] = o.pose.covariance[4];
00105   imu.orientation_covariance[8] = o.pose.covariance[8];
00106   imu.angular_velocity.x    = o.twist.twist.angular.x;
00107   imu.angular_velocity.y    = o.twist.twist.angular.x;
00108   imu.angular_velocity.z    = o.twist.twist.angular.x;
00109   imu.linear_acceleration.x = 0.0;
00110   imu.linear_acceleration.y = 0.0;
00111   imu.linear_acceleration.z = 0.0;
00112 }
00113 
00114 
00115 
00116 
00117 
00118 
00119 
00120 
00121 
00122 
00123 
00124 
00125 
00126 
00127 
00128 
00129 
00130 
00131 
00132 
00133 
00134 


iri_msg_to_odom
Author(s): Marti Morta, mmorta at iri.upc.edu
autogenerated on Fri Dec 6 2013 22:08:38