odometry.cc
Go to the documentation of this file.
00001 /*
00002  *  Copyright (C) 2006 Austin Robot Technology
00003  *  Copyright (C) 2009 Austin Robot Technology
00004  *
00005  *  License: Modified BSD Software License Agreement
00006  * 
00007  *  $Id: odometry.cc 697 2010-10-25 13:56:33Z jack.oquin $
00008  */
00009 
00043 #include <ros/ros.h>
00044 #include <angles/angles.h>
00045 
00046 #include <nav_msgs/Odometry.h>
00047 #include <sensor_msgs/Imu.h>
00048 #include <geometry_msgs/PoseStamped.h>
00049 #include <art_msgs/GpsInfo.h>
00050 #include <tf/transform_broadcaster.h>
00051 
00052 #include <art_msgs/Shifter.h>
00053 #include <art/frames.h>
00054 #include <art/UTM.h>
00055 
00056 #include "applanix.h"
00057 #include "position.h"
00058 
00059 #define NODE "applanix"
00060 
00061 namespace
00062 {
00063   // transmission gear information
00064   ros::Subscriber shifter_sub_;         // shifter position ROS topic
00065   uint8_t shifter_gear_ = art_msgs::Shifter::Drive;
00066 
00067   // command parameters
00068   int qDepth = 1;                       // ROS topic queue size
00069 
00070   // class for generating vehicle-relative frame IDs
00071   ArtFrames::VehicleRelative vr_;
00072 
00073   // GPS information
00074   applanix_data_t adata;                // saved applanix data packets
00075   DevApplanix *applanix_;               // Applanix device interface
00076 };
00077 
00078 
00086 bool GlobalToLocal(Position::Pose3D *current) 
00087 {
00088   // original pose when started (global coordinates)
00089   static Position::Pose3D map_origin;
00090   static bool first_pose_received = false;
00091 
00092   ROS_DEBUG("Global data (%.3f, %.3f, %.3f) (%.3f, %.3f, %.3f)",
00093             current->x, current->y, current->z,
00094             current->roll, current->pitch, current->yaw);
00095 
00096   bool initial_pose = !first_pose_received;
00097 
00098   if (initial_pose)
00099     {
00100       // Initial conditions. Compute map origin from starting point
00101       // using a UTM grid so we can offset future data points from
00102       // there. If the driver restarts within the same region, it will
00103       // pick the same origin.
00104 
00105       map_origin = *current;
00106       map_origin.x = rint(map_origin.x/UTM::grid_size) * UTM::grid_size;
00107       map_origin.y = rint(map_origin.y/UTM::grid_size) * UTM::grid_size;
00108       map_origin.z = 0.0;               // leave elevations alone
00109 
00110       first_pose_received = true;
00111 
00112       ROS_INFO("INITIAL data (%.3f, %.3f, %.3f), map origin (%.3f, %.3f, %.3f)",
00113                current->x, current->y, current->z,
00114                map_origin.x, map_origin.y, map_origin.z);
00115     }
00116 
00117 
00118   // General case.  See how far we have gone & the angle between
00119   // the coordinate systems and translate between global (X, Y)
00120   // and local (x, y)
00121 
00122   // We do not subtract one entire Pose3D from the other to avoid
00123   // changing the roll, pitch and yaw fields (and also to forgo
00124   // unnecessary arithmetic).
00125   current->x -= map_origin.x;
00126   current->y -= map_origin.y;
00127   current->z -= map_origin.z;
00128 
00129   ROS_DEBUG("Local data  (%.3f, %.3f, %.3f) (%.3f, %.3f, %.3f)",
00130             current->x, current->y, current->z,
00131             current->roll, current->pitch, current->yaw);
00132 
00133 
00134   return initial_pose;
00135 }
00136 
00143 bool getNewData(applanix_data_t *adata)
00144 {
00145   static ros::Time last_time;
00146 
00147   // read and unpack first packet
00148   int rc = applanix_->get_packet(adata);
00149   if (rc != 0)                          // none available?
00150     {
00151       ROS_DEBUG("no packet found");
00152       return false;
00153     }
00154 
00155   // Get any additional packets already queued.  It is OK if there are
00156   // none, but we want to return the latest available information.
00157   do
00158     {
00159       ROS_DEBUG_STREAM("got packet, time: " << adata->time);
00160       rc = applanix_->get_packet(adata);
00161     }
00162   while (rc == 0);
00163 
00164   // see if a new navigation solution is available
00165   if (adata->time == last_time)
00166     return false;
00167   // see if device is returning valid data yet
00168   if (adata->grp1.alignment == ApplStatusInvalid)
00169     {
00170       ROS_WARN("Applanix alignment invalid");
00171       return false;                     // no valid solution yet
00172     }
00173 
00174 #if 1
00175   if (adata->time == ros::Time())
00176     {
00177       ROS_WARN_STREAM("invalid packet time: " << adata->time);
00178       return false;
00179     }
00180 #endif
00181 
00182   last_time = adata->time;              // remember time of last update
00183   return true;
00184 }
00185 
00187 void publishGPS(const applanix_data_t &adata, double utm_e, double utm_n,
00188                 const char* zone, ros::Publisher *gps_pub)
00189 {
00190   art_msgs::GpsInfo gpsi;
00191 
00192   gpsi.header.stamp = adata.time;
00193   gpsi.header.frame_id = vr_.getFrame(ArtFrames::odom);
00194   gpsi.latitude   = adata.grp1.lat;
00195   gpsi.longitude  = adata.grp1.lon;
00196   gpsi.altitude   = adata.grp1.alt;
00197   gpsi.utm_e  = utm_e;
00198   gpsi.utm_n  = utm_n;
00199   gpsi.zone = std::string(zone);
00200 
00201   switch (adata.grp1.alignment)
00202     {
00203     case ApplStatusFull:
00204       gpsi.quality = art_msgs::GpsInfo::DGPS_FIX;
00205       break;
00206     case ApplStatusFine:
00207       gpsi.quality = art_msgs::GpsInfo::GPS_FIX;
00208       break;
00209     default:
00210       gpsi.quality = art_msgs::GpsInfo::INVALID_FIX;
00211     }
00212 
00214 
00215   gps_pub->publish(gpsi);
00216 }
00217 
00231 bool getOdom(Position::Position3D *odom_pos3d, ros::Time *odom_time,
00232              ros::Publisher *gps_pub)
00233 {
00234   if (!getNewData(&adata))
00235     {
00236       ROS_DEBUG("no data this cycle");
00237       return false;                     // nothing to publish
00238     }
00239 
00240   // remember when the new data arrived
00241   *odom_time = adata.time;
00242 
00243   // Convert latitude and longitude (spherical coordinates) to
00244   // Universal Transverse Mercator (Cartesian).
00245   double utm_e, utm_n;                  // easting, northing (in meters)
00246   char zone[20];
00247   UTM::LLtoUTM(adata.grp1.lat, adata.grp1.lon, utm_n, utm_e, zone);
00248 
00249   // publish GPS information topic
00250   publishGPS(adata, utm_e, utm_n, zone, gps_pub);
00251 
00252   using namespace angles;
00253     
00254   // fill in Position3D position
00255   odom_pos3d->pos.x = utm_e;
00256   odom_pos3d->pos.y = utm_n;
00257   odom_pos3d->pos.z = adata.grp1.alt;
00258 
00259   // Translate heading.  GPS heading is like a compass, zero degrees
00260   // is North, East is 90, West is 270. The robot heading is zero for
00261   // East (positive X direction), and pi/2 radians for North (positive
00262   // Y).
00263   odom_pos3d->pos.roll =  from_degrees(adata.grp1.roll);
00264   odom_pos3d->pos.pitch = from_degrees(-adata.grp1.pitch);
00265   odom_pos3d->pos.yaw = normalize_angle(from_degrees(90 - adata.grp1.heading));
00266 
00267   // Convert the current global coordinates to local values relative
00268   // to our initial position.
00269   if (GlobalToLocal(&odom_pos3d->pos))  // initial position?
00270     return false;                       // do not publish
00271 
00272   // Invert speed if the vehicle is going in reverse.
00273   double speed = adata.grp1.speed;      // in meters/second
00274   if (shifter_gear_ == art_msgs::Shifter::Reverse)
00275     speed = -speed;                     // handle reverse movement
00276 
00277   // Fill in Position3D velocity in /vehicle frame.  Y velocity should
00278   // normally be zero (unless skidding sideways).
00279   odom_pos3d->vel.x = speed;
00280   odom_pos3d->vel.y = 0.0;              // use adata.grp4.vel_y somehow?
00281   odom_pos3d->vel.z = -adata.grp1.vel_down;
00282 
00283   odom_pos3d->vel.roll =  from_degrees(adata.grp1.arate_lon);
00284   odom_pos3d->vel.pitch = from_degrees(-adata.grp1.arate_trans);
00285   odom_pos3d->vel.yaw =   from_degrees(-adata.grp1.arate_down);
00286 
00287   return true;                          // need to publish
00288 }  
00289 
00291 void getShifter(const art_msgs::Shifter::ConstPtr &shifterIn)
00292 {
00293   if (shifter_gear_ != shifterIn->gear)
00294     ROS_INFO("Gear changed from %u to %u",
00295              shifter_gear_, shifterIn->gear);
00296   shifter_gear_ = shifterIn->gear;
00297 }
00298 
00300 void putPose(const Position::Position3D *odom_pos3d,
00301              const ros::Time *odom_time,
00302              tf::TransformBroadcaster *odom_broad,
00303              ros::Publisher *odom_pub,
00304              ros::Publisher *imu_pub)
00305 {
00306   // translate roll, pitch and yaw into a Quaternion
00307   tf::Quaternion q;
00308   q.setRPY(odom_pos3d->pos.roll, odom_pos3d->pos.pitch, odom_pos3d->pos.yaw);
00309   geometry_msgs::Quaternion odom_quat;
00310   tf::quaternionTFToMsg(q, odom_quat);
00311 
00312   // broadcast Transform from /earth to /vehicle
00313   geometry_msgs::TransformStamped earth_tf;
00314   earth_tf.header.stamp = *odom_time;
00315   earth_tf.header.frame_id = vr_.getFrame(ArtFrames::earth);
00316   earth_tf.child_frame_id = vr_.getFrame(ArtFrames::vehicle);
00317   earth_tf.transform.translation.x = odom_pos3d->pos.x;
00318   earth_tf.transform.translation.y = odom_pos3d->pos.y;
00319   earth_tf.transform.translation.z = odom_pos3d->pos.z;
00320   earth_tf.transform.rotation = odom_quat;
00321   odom_broad->sendTransform(earth_tf);
00322 
00323   // broadcast Transform from /odom to /earth with same X, Y position
00324   // and orientation, but same Z coordinate as /vehicle
00325   geometry_msgs::TransformStamped odom_tf;
00326   odom_tf.header.stamp = *odom_time;
00327   odom_tf.header.frame_id = vr_.getFrame(ArtFrames::odom);
00328   odom_tf.child_frame_id = vr_.getFrame(ArtFrames::earth);
00329   odom_tf.transform.translation.z = -odom_pos3d->pos.z;
00330   odom_tf.transform.rotation.w = 1.0;
00331   odom_broad->sendTransform(odom_tf);
00332 
00333   // publish the Odometry message
00334   nav_msgs::Odometry odom_msg;
00335   odom_msg.header.stamp = *odom_time;
00336   odom_msg.header.frame_id = vr_.getFrame(ArtFrames::odom);
00337   odom_msg.pose.pose.position.x = odom_pos3d->pos.x;
00338   odom_msg.pose.pose.position.y = odom_pos3d->pos.y;
00339   odom_msg.pose.pose.position.z = odom_pos3d->pos.z;
00340   odom_msg.pose.pose.orientation = odom_quat;
00341   odom_msg.child_frame_id = vr_.getFrame(ArtFrames::vehicle);
00342 
00343   // Twist is relative to the /vehicle frame, not /odom
00344   odom_msg.twist.twist.linear.x = odom_pos3d->vel.x;
00345   odom_msg.twist.twist.linear.y = odom_pos3d->vel.y;
00346   odom_msg.twist.twist.linear.z = odom_pos3d->vel.z;
00347   odom_msg.twist.twist.angular.x = odom_pos3d->vel.roll;
00348   odom_msg.twist.twist.angular.y = odom_pos3d->vel.pitch;
00349   odom_msg.twist.twist.angular.z = odom_pos3d->vel.yaw;
00350 
00352 
00353   odom_pub->publish(odom_msg);
00354 
00355   // publish the Imu message
00356   sensor_msgs::Imu imu_msg;
00357   imu_msg.header.stamp = *odom_time;
00358   imu_msg.header.frame_id = vr_.getFrame(ArtFrames::vehicle);
00359   imu_msg.orientation = odom_quat;
00360   imu_msg.angular_velocity = odom_msg.twist.twist.angular;
00361   imu_msg.linear_acceleration.x = adata.grp1.accel_lon;
00362   imu_msg.linear_acceleration.y = adata.grp1.accel_trans;
00363   imu_msg.linear_acceleration.z = - adata.grp1.accel_down;
00364 
00366 
00367   imu_pub->publish(imu_msg);
00368 }
00369 
00370 void displayHelp() 
00371 {
00372   std::cerr << "ART Applanix odometry driver\n"
00373             << std::endl
00374             << "Usage: rosrun applanix odometry <options>\n"
00375             << std::endl
00376             << "Options:\n"
00377             << "\t -h, -?       print usage message\n"
00378             << "\t -f <file>    use PCAP dump from <file>\n"
00379             << "\t -q <integer> set ROS topic queue depth (default: 1)\n"
00380             << "\t -t <file>    run unit test with fake data from <file>\n"
00381             << std::endl
00382             << "Example:\n"
00383             << "  rosrun applanix odometry -q2\n"
00384             << std::endl;
00385 }
00386 
00387 
00392 int getParameters(int argc, char *argv[])
00393 {
00394   static std::string pcap_file = "";    // pcap dump file
00395   static std::string test_file = "";    // unit test input file
00396 
00397   // use getopt to parse the flags
00398   char ch;
00399   const char* optflags = "hf:q:t:?";
00400   while(-1 != (ch = getopt(argc, argv, optflags)))
00401     {
00402       switch(ch)
00403         {
00404         case 'f':
00405           pcap_file = optarg;
00406           break;
00407         case 'q':
00408           qDepth = atoi(optarg);
00409           if (qDepth < 1)
00410             qDepth = 1;
00411           break;
00412         case 't':
00413           test_file = optarg;
00414           break;
00415         default:                        // unknown
00416           ROS_WARN("unknown parameter: %c", ch);
00417           // fall through to display help...
00418         case 'h':                       // help
00419         case '?':
00420           displayHelp();
00421           return 1;
00422         }
00423     }
00424 
00425   ROS_INFO("topic queue depth = %d", qDepth);
00426   
00427   // Create Applanix odometry device interface
00428   if (pcap_file != "")
00429     applanix_ = new DevApplanixPCAP(pcap_file);
00430   else if (test_file != "")
00431     applanix_ = new DevApplanixTest(test_file);
00432   else
00433     applanix_ = new DevApplanix();
00434 
00435   vr_.getPrefixParam();                 // get vehicle-relative tf prefix
00436 
00437   return 0;
00438 }
00439 
00441 int main(int argc, char** argv)
00442 {
00443   ros::init(argc, argv, NODE);
00444   ros::NodeHandle node;
00445 
00446   if (0 != getParameters(argc, argv))
00447     return 9;
00448 
00449   // make this a class w/constructor to initialize?
00450   memset(&adata, 0, sizeof(adata));
00451   adata.grp1.alignment = ApplStatusInvalid; // no valid solution yet
00452 
00453   // initialize odometry state
00454   ros::Time odom_time;                  // time last full packet read
00455   Position::Position3D odom_pos3d;      // current 3D position
00456   
00457   // connect to some ROS topics
00458   // no delay: we always want the most recent data
00459   ros::TransportHints noDelay = ros::TransportHints().tcpNoDelay(true);
00460   ros::Publisher odom_pub =
00461     node.advertise<nav_msgs::Odometry>("odom", qDepth);
00462   ros::Publisher imu_pub =
00463     node.advertise<sensor_msgs::Imu>("imu", qDepth);
00464   ros::Publisher gps_pub =
00465     node.advertise<art_msgs::GpsInfo>("gps", qDepth);
00466   tf::TransformBroadcaster odom_broadcaster;
00467   shifter_sub_ = node.subscribe("shifter/state", qDepth, getShifter, noDelay);
00468 
00469   // connect to Applanix data socket
00470   int rc = applanix_->connect_socket();
00471   if (rc != 0)                          // device init failed?
00472     return 2;
00473 
00474   ros::Rate cycle(20);                  // set driver cycle rate
00475   
00476   ROS_INFO(NODE ": starting main loop");
00477 
00478   // main loop
00479   while(ros::ok())
00480     {
00481       if (getOdom(&odom_pos3d, &odom_time, &gps_pub))
00482         {
00483           // publish transform and odometry only when there are new
00484           // Applanix data
00485           putPose(&odom_pos3d, &odom_time, &odom_broadcaster,
00486                   &odom_pub, &imu_pub);
00487         }
00488 
00489       ros::spinOnce();                  // handle incoming messages
00490       cycle.sleep();                    // sleep until next cycle
00491     }
00492 
00493   ROS_INFO(NODE ": exiting main loop");
00494 
00495   // Stop the Applanix device, close its socket
00496 
00497   delete applanix_;
00498 
00499   return 0;
00500 }


applanix
Author(s): Jack O'Quin, Patrick Beeson, Alberto Alonso
autogenerated on Fri Jan 3 2014 11:09:02