add nmea_msgs::Sentence nmea_gps_sensor_plugin::build_GPGGA_sentence(…

…) in nmea_gps_sensor_plugin
weilunwc · Dec 4, 2017 · f0cfde6 · f0cfde6
1 parent 83b2221
commit f0cfde6
Show file tree

Hide file tree

Showing 2 changed files with 61 additions and 1 deletion.
diff --git a/ros_ship_gazebo_plugins/include/nmea_gps_sensor_plugin.h b/ros_ship_gazebo_plugins/include/nmea_gps_sensor_plugin.h
@@ -39,6 +39,7 @@
 
 #include <dynamic_reconfigure/server.h>
 #include <hector_gazebo_plugins/GNSSConfig.h>
+#include <nmea_msgs/Sentence.h>
 
 namespace gazebo
 {
@@ -58,6 +59,9 @@ namespace gazebo
     void dynamicReconfigureCallback(GNSSConfig &config, uint32_t level);
 
   private:
+    //functions for building nmea sentence
+    nmea_msgs::Sentence build_GPGGA_sentence();
+    std::string get_nmea_checksum(std::string sentence);
     /// \brief The parent World
     physics::WorldPtr world;
 

diff --git a/ros_ship_gazebo_plugins/src/nmea_gps_sensor_plugin.cc b/ros_ship_gazebo_plugins/src/nmea_gps_sensor_plugin.cc
@@ -28,7 +28,10 @@
 
 #include <nmea_gps_sensor_plugin.h>
 #include <gazebo/physics/physics.hh>
-#include <nmea_msgs/Sentence.h>
+
+//headers in stl
+#include <sys/time.h>
+#include <vector>
 
 // WGS84 constants
 static const double equatorial_radius = 6378137.0;
@@ -245,6 +248,59 @@ namespace gazebo
     //velocity_publisher_.publish(velocity_);
   }
 
+  nmea_msgs::Sentence nmea_gps_sensor_plugin::build_GPGGA_sentence()
+  {
+    nmea_msgs::Sentence sentence;
+    sentence.header = fix_.header;
+    struct timeval time_value;
+    struct tm *time_st;
+    gettimeofday(&time_value, NULL);
+    sentence.sentence = std::to_string(time_st->tm_hour) + std::to_string(time_st->tm_min) + std::to_string(time_st->tm_sec);
+    sentence.sentence = sentence.sentence + "." + std::to_string(time_value.tv_usec) + ",";
+    sentence.sentence = sentence.sentence + "A,";
+    if(fix_.latitude < 0)
+    {
+      int digree = std::floor(std::fabs(fix_.latitude));
+      double min = (std::fabs(fix_.latitude)-std::floor(std::fabs(fix_.latitude)))/60;
+      sentence.sentence = sentence.sentence + std::to_string(digree) + std::to_string(min) + ",";
+      sentence.sentence = sentence.sentence + "S,";
+    }
+    else
+    {
+      int digree = std::floor(std::fabs(fix_.latitude));
+      double min = (std::fabs(fix_.latitude)-std::floor(std::fabs(fix_.latitude)))/60;
+      sentence.sentence = sentence.sentence + std::to_string(digree) + std::to_string(min) + ",";
+      sentence.sentence = sentence.sentence + "N,";
+    }
+    if(fix_.longitude < 0)
+    {
+      int digree = std::floor(std::fabs(fix_.longitude));
+      double min = (std::fabs(fix_.longitude)-std::floor(std::fabs(fix_.longitude)))/60;
+      sentence.sentence = sentence.sentence + std::to_string(digree) + std::to_string(min) + ",";
+      sentence.sentence = sentence.sentence + "W,";
+    }
+    else
+    {
+      int digree = std::floor(std::fabs(fix_.longitude));
+      double min = (std::fabs(fix_.longitude)-std::floor(std::fabs(fix_.longitude)))/60;
+      sentence.sentence = sentence.sentence + std::to_string(digree) + std::to_string(min) + ",";
+      sentence.sentence = sentence.sentence + "E,";
+    }
+    sentence.sentence = sentence.sentence + "1,08,1.0,";
+    sentence.sentence = sentence.sentence + std::to_string(fix_.altitude) + ",";
+    sentence.sentence = sentence.sentence + "M,";
+    sentence.sentence = sentence.sentence + std::to_string(fix_.altitude) + ",";
+    sentence.sentence = sentence.sentence + "M,,0000";
+    sentence.sentence = sentence.sentence + "*" + get_nmea_checksum(sentence.sentence);
+    sentence.sentence = "$GPGGA," + sentence.sentence;
+    return sentence;
+  }
+
+  std::string nmea_gps_sensor_plugin::get_nmea_checksum(std::string sentence)
+  {
+    return "";
+  }
+
   // Register this plugin with the simulator
   GZ_REGISTER_MODEL_PLUGIN(nmea_gps_sensor_plugin)