GPS sensor in Gazebo

.gitignore

@@ -12,3 +12,4 @@ build_*
 
 .ycm_extra_conf.py
 *.orig
+.vscode

CMakeLists.txt

@@ -104,6 +104,7 @@ ign_find_package(ignition-sensors4 REQUIRED
     altimeter
     camera
     gpu_lidar
+    gps
     imu
     logical_camera
     magnetometer

examples/worlds/sensors.sdf

@@ -27,6 +27,10 @@
       filename="ignition-gazebo-altimeter-system"
       name="ignition::gazebo::systems::Altimeter">
     </plugin>
+    <plugin
+      filename="ignition-gazebo-gps-system"
+      name="ignition::gazebo::systems::Gps">
+    </plugin>
     <plugin
       filename="ignition-gazebo-imu-system"
       name="ignition::gazebo::systems::Imu">
@@ -146,12 +150,20 @@
           </air_pressure>
         </sensor>
 
+        <sensor name="gps" type="gps">
+          <always_on>1</always_on>
+          <update_rate>1</update_rate>
+          <visualize>true</visualize>
+          <topic>gps</topic>
+        </sensor>
+
         <sensor name="imu" type="imu">
           <always_on>1</always_on>
           <update_rate>100</update_rate>
           <visualize>true</visualize>
           <topic>imu</topic>
         </sensor>
+
         <sensor name="magnetometer" type="magnetometer">
           <always_on>1</always_on>
           <update_rate>100</update_rate>
@@ -169,7 +181,7 @@
                 <mean>0.0</mean>
                 <stddev>0.1</stddev>
               </noise>
-            </y>
+	    </y>
             <z>
               <noise type="gaussian">
                 <mean>0.0</mean>

include/ignition/gazebo/components/Gps.hh

@@ -0,0 +1,46 @@
+/*
+ * Copyright (C) 2019 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifndef IGNITION_GAZEBO_COMPONENTS_GPS_HH_
+#define IGNITION_GAZEBO_COMPONENTS_GPS_HH_
+
+#include <sdf/Sensor.hh>
+
+#include <ignition/gazebo/config.hh>
+#include <ignition/gazebo/Export.hh>
+
+#include <ignition/gazebo/components/Factory.hh>
+#include <ignition/gazebo/components/Component.hh>
+#include <ignition/gazebo/components/Serialization.hh>
+
+namespace ignition
+{
+namespace gazebo
+{
+// Inline bracket to help doxygen filtering.
+inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
+namespace components
+{
+  /// \brief A component type that contains an GPS sensor,
+  /// sdf::GPS, information.
+  using Gps = Component<sdf::Sensor, class GpsTag,
+      serializers::SensorSerializer>;
+  IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Gps", Gps)
+}
+}
+}
+}
+#endif

src/Conversions.cc

@@ -22,6 +22,7 @@
 #include <ignition/msgs/entity.pb.h>
 #include <ignition/msgs/geometry.pb.h>
 #include <ignition/msgs/gui.pb.h>
+#include <ignition/msgs/gps_sensor.pb.h>
 #include <ignition/msgs/imu_sensor.pb.h>
 #include <ignition/msgs/lidar_sensor.pb.h>
 #include <ignition/msgs/actor.pb.h>
@@ -49,6 +50,7 @@
 #include <sdf/Geometry.hh>
 #include <sdf/Gui.hh>
 #include <sdf/Imu.hh>
+#include <sdf/Gps.hh>
 #include <sdf/Lidar.hh>
 #include <sdf/Light.hh>
 #include <sdf/Magnetometer.hh>
@@ -849,6 +851,34 @@ msgs::Sensor ignition::gazebo::convert(const sdf::Sensor &_in)
         << "sensor pointer is null.\n";
     }
   }
+  else if (_in.Type() == sdf::SensorType::GPS)
+  {
+    if (_in.GpsSensor())
+    {
+      msgs::GPSSensor *sensor = out.mutable_gps();
+
+      if (_in.GpsSensor()->PositionNoise().Type() != sdf::NoiseType::NONE)
+      {
+        ignition::gazebo::set(sensor->mutable_position()->mutable_horizontal_noise(),
+            _in.GpsSensor()->PositionNoise());
+        ignition::gazebo::set(sensor->mutable_position()->mutable_vertical_noise(),
+            _in.GpsSensor()->PositionNoise());
+
+      }
+      if (_in.GpsSensor()->VelocityNoise().Type() != sdf::NoiseType::NONE)
+      {
+        ignition::gazebo::set(sensor->mutable_velocity()->mutable_horizontal_noise(),
+            _in.GpsSensor()->VelocityNoise());
+        ignition::gazebo::set(sensor->mutable_velocity()->mutable_vertical_noise(),
+            _in.GpsSensor()->VelocityNoise());
+      }
+    }
+    else
+    {
+      ignerr << "Attempting to convert a GPS SDF sensor, but the "
+        << "sensor pointer is null.\n";
+    }
+  }
   else if (_in.Type() == sdf::SensorType::ALTIMETER)
   {
     if (_in.AltimeterSensor())

src/SdfEntityCreator.cc

@@ -37,6 +37,7 @@
 #include "ignition/gazebo/components/GpuLidar.hh"
 #include "ignition/gazebo/components/Gravity.hh"
 #include "ignition/gazebo/components/Imu.hh"
+#include "ignition/gazebo/components/Gps.hh"
 #include "ignition/gazebo/components/Inertial.hh"
 #include "ignition/gazebo/components/Joint.hh"
 #include "ignition/gazebo/components/JointAxis.hh"
@@ -627,6 +628,18 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Sensor *_sensor)
     this->dataPtr->ecm->CreateComponent(sensorEntity,
         components::WorldLinearVelocity(math::Vector3d::Zero));
   }
+  else if (_sensor->Type() == sdf::SensorType::GPS)
+  {
+    this->dataPtr->ecm->CreateComponent(sensorEntity,
+            components::Gps(*_sensor));
+
+    // create components to be filled by physics
+    this->dataPtr->ecm->CreateComponent(sensorEntity,
+            components::WorldPose(math::Pose3d::Zero));
+
+    this->dataPtr->ecm->CreateComponent(sensorEntity,
+            components::WorldLinearVelocity(math::Vector3d::Zero));
+  }
   else if (_sensor->Type() == sdf::SensorType::IMU)
   {
     this->dataPtr->ecm->CreateComponent(sensorEntity,

src/systems/CMakeLists.txt

@@ -82,6 +82,7 @@ add_subdirectory(detachable_joint)
 add_subdirectory(diff_drive)
 add_subdirectory(follow_actor)
 add_subdirectory(imu)
+add_subdirectory(gps)
 add_subdirectory(joint_controller)
 add_subdirectory(joint_position_controller)
 add_subdirectory(joint_state_publisher)

src/systems/gps/CMakeLists.txt

@@ -0,0 +1,9 @@
+gz_add_system(gps
+  SOURCES
+    Gps.cc
+  PUBLIC_LINK_LIBS
+    ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER}
+  PRIVATE_LINK_LIBS
+    ignition-sensors${IGN_SENSORS_VER}::gps
+)
+