set_gps_reference.cpp
Go to the documentation of this file.
00001 /*
00002  * set_gps_reference.cpp
00003  *
00004  *  Created on: Jun 29, 2011
00005  *      Author: acmarkus
00006  */
00007 
00008 #include <ros/ros.h>
00009 #include <sensor_msgs/NavSatFix.h>
00010 
00011 double g_lat_ref;
00012 double g_lon_ref;
00013 double g_alt_ref;
00014 int g_its;
00015 
00016 
00017 void gpsCb(const sensor_msgs::NavSatFixConstPtr & msg){
00018 
00019   static int count = 1;
00020 
00021   if (msg->status.status != sensor_msgs::NavSatStatus::STATUS_FIX)
00022   {
00023     ROS_WARN_STREAM_THROTTLE(1, "No GPS fix");
00024     return;
00025   }
00026 
00027   g_lat_ref += msg->latitude;
00028   g_lon_ref += msg->longitude;
00029   g_alt_ref += msg->altitude;
00030 
00031   ROS_INFO("Current measurement: %f %f %f", msg->latitude, msg->longitude, msg->altitude);
00032 
00033   if(count == g_its){
00034     g_lat_ref /= g_its;
00035     g_lon_ref /= g_its;
00036     g_alt_ref /= g_its;
00037 
00038     ros::NodeHandle nh;
00039     nh.setParam("/gps_ref_latitude", g_lat_ref);
00040     nh.setParam("/gps_ref_longitude", g_lon_ref);
00041     nh.setParam("/gps_ref_altitude", g_alt_ref);
00042 
00043     ROS_INFO("final reference position: %f %f %f", g_lat_ref, g_lon_ref, g_alt_ref);
00044 
00045     ros::shutdown();
00046     return;
00047   }
00048 
00049   count ++;
00050 }
00051 
00052 int main(int argc, char** argv){
00053   ros::init(argc, argv, "set_gps_reference");
00054   ros::NodeHandle nh;
00055 
00056   g_lat_ref = 0;
00057   g_lon_ref = 0;
00058   g_alt_ref = 0;
00059 
00060   ros::V_string args;
00061   ros::removeROSArgs(argc, argv, args);
00062 
00063   if(args.size() > 1)
00064     g_its = atoi(args[1].c_str());
00065   else
00066     g_its = 50;
00067 
00068   ROS_INFO("taking %d measurements\n",g_its);
00069 
00070   ros::Subscriber gps_sub = nh.subscribe("gps", 1, &gpsCb);
00071 
00072   ros::spin();
00073 }


asctec_hl_gps
Author(s): Markus Achtelik
autogenerated on Tue Jan 7 2014 11:05:15