Merge pull request #21 from ethz-asl/feature/reset-gps-ref-service

Feature/reset gps ref service

Author: raghavkhanna

CMakeLists.txt

@@ -13,8 +13,8 @@ cs_add_executable(set_gps_reference_node src/set_gps_reference_node.cpp)
 target_link_libraries(set_gps_reference_node  ${catkin_LIBRARIES})
 
 cs_add_executable(gps_to_pose_conversion_node src/gps_to_pose_conversion_node.cpp)
-target_link_libraries(gps_to_pose_conversion_node  ${catkin_LIBRARIES})
+#target_link_libraries(gps_to_pose_conversion_node  ${catkin_LIBRARIES})
 
 cs_install()
-cs_install_scripts(src/gps_spoofer.py src/euler_radians_to_degrees.py src/odometry_height_adjuster.py)
+cs_install_scripts(src/gps_spoofer.py src/euler_radians_to_degrees.py)
 cs_export()

package.xml

@@ -22,4 +22,6 @@
   <depend>geometry_msgs</depend>
   <!--cmake_modules dependency required for (find Eigen) command-->
   <depend>cmake_modules</depend>
+  <!--std_srvs provides the Empty service used to reset the gps reference ]-->
+  <depend>std_srvs</depend>
 </package>

src/set_gps_reference_node.cpp

@@ -2,12 +2,13 @@
 #include <sensor_msgs/NavSatFix.h>
 #include <string>
 #include <vector>
+#include <std_srvs/Empty.h>
 
 double g_lat_ref;
 double g_lon_ref;
 double g_alt_ref;
-
-std::vector<double> g_pressure_heights;
+int g_count = 1;
+bool gps_ref_is_init;
 int g_its;
 
 enum EMode
@@ -18,49 +19,62 @@ enum EMode
 // average over, or wait for, n GPS fixes
 EMode g_mode;
 
+bool reset_callback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+{
+  g_count = 1;
+  g_lat_ref = 0.0;
+  g_lon_ref = 0.0;
+  g_alt_ref = 0.0;
+  ros::NodeHandle nh;
+  nh.setParam("/gps_ref_is_init", false);
+  return true;
+}
 
 void gps_callback(const sensor_msgs::NavSatFixConstPtr & msg)
 {
+  ros::NodeHandle nh;
+  nh.getParam("/gps_ref_is_init", gps_ref_is_init);
 
-  static int count = 1;
+  if (!gps_ref_is_init){
 
-  if (msg->status.status < sensor_msgs::NavSatStatus::STATUS_FIX) {
-    ROS_WARN_STREAM_THROTTLE(1, "No GPS fix");
-    return;
-  }
 
-  g_lat_ref += msg->latitude;
-  g_lon_ref += msg->longitude;
-  g_alt_ref += msg->altitude;
+    if (msg->status.status < sensor_msgs::NavSatStatus::STATUS_FIX) {
+      ROS_WARN_STREAM_THROTTLE(1, "No GPS fix");
+      return;
+    }
 
-  ROS_INFO("Current measurement: %f, %f, %f", msg->latitude, msg->longitude, msg->altitude);
+    g_lat_ref += msg->latitude;
+    g_lon_ref += msg->longitude;
+    g_alt_ref += msg->altitude;
 
-  if (count == g_its) {
-    if (g_mode == MODE_AVERAGE) {
-      g_lat_ref /= g_its;
-      g_lon_ref /= g_its;
-      g_alt_ref /= g_its;
-    } else {
-      g_lat_ref = msg->latitude;
-      g_lon_ref = msg->longitude;
-      g_alt_ref = msg->altitude;
-    }
+    ROS_INFO("Current measurement: %f, %f, %f", msg->latitude, msg->longitude, msg->altitude);
 
-    ros::NodeHandle nh;
-    nh.setParam("/gps_ref_latitude", g_lat_ref);
-    nh.setParam("/gps_ref_longitude", g_lon_ref);
-    nh.setParam("/gps_ref_altitude", g_alt_ref);
-    nh.setParam("/gps_ref_is_init", true);
+    if (g_count == g_its) {
+      if (g_mode == MODE_AVERAGE) {
+        g_lat_ref /= g_its;
+        g_lon_ref /= g_its;
+        g_alt_ref /= g_its;
+      } else {
+        g_lat_ref = msg->latitude;
+        g_lon_ref = msg->longitude;
+        g_alt_ref = msg->altitude;
+      }
 
-    ROS_INFO("Final reference position: %f, %f, %f", g_lat_ref, g_lon_ref, g_alt_ref);
 
-    ros::shutdown();
-    return;
-  } else {
-    ROS_INFO("    Still waiting for %d measurements", g_its - count);
-  }
+      nh.setParam("/gps_ref_latitude", g_lat_ref);
+      nh.setParam("/gps_ref_longitude", g_lon_ref);
+      nh.setParam("/gps_ref_altitude", g_alt_ref);
+      nh.setParam("/gps_ref_is_init", true);
+
+      ROS_INFO("Final reference position: %f, %f, %f", g_lat_ref, g_lon_ref, g_alt_ref);
 
-  count++;
+      return;
+    } else {
+      ROS_INFO("    Still waiting for %d measurements", g_its - g_count);
+    }
+
+  g_count++;
+  }
 }
 
 int main(int argc, char** argv)
@@ -101,6 +115,7 @@ int main(int argc, char** argv)
       (g_mode == MODE_AVERAGE) ? "averaging to get the reference" : "taking the last as reference");
 
   ros::Subscriber gps_sub = nh.subscribe("gps", 1, &gps_callback);
+  ros::ServiceServer reset_srv = nh.advertiseService("reset_gps_reference", &reset_callback);
 
   ros::spin();
 }