add gps_device (#208)

* add gps_device

* fix check speed subscriber also and publish

webots_ros2_core/webots_ros2_core/devices/device_manager.py

@@ -24,6 +24,7 @@
 from .light_sensor_device import LightSensorDevice
 from .robot_device import RobotDevice
 from .imu_device import ImuDevice
+from .gps_device import GpsDevice
 from webots_ros2_core.webots.controller import Node
 
 
@@ -56,6 +57,8 @@ def __init__(self, node, config=None):
                 device = DistanceSensorDevice(node, device_key, wb_device, self.__config.get(device_key, None))
             elif wb_device.getNodeType() == Node.LIGHT_SENSOR:
                 device = LightSensorDevice(node, device_key, wb_device, self.__config.get(device_key, None))
+            elif wb_device.getNodeType() == Node.GPS:
+                device = GpsDevice(node, device_key, wb_device, self.__config.get(device_key, None))
 
             # Add device to the list
             self.__wb_devices[device_key] = wb_device

webots_ros2_core/webots_ros2_core/devices/gps_device.py

@@ -0,0 +1,105 @@
+# Copyright 1996-2021 Cyberbotics Ltd.
+#
+# 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.
+
+"""Webots GPS device wrapper for ROS2."""
+
+from rclpy.qos import qos_profile_sensor_data
+from std_msgs.msg import Float32
+from sensor_msgs.msg import NavSatFix, NavSatStatus
+from geometry_msgs.msg import PointStamped
+from .sensor_device import SensorDevice
+from controller import GPS
+
+
+class GpsDevice(SensorDevice):
+    """
+    ROS2 wrapper for Webots GPS node.
+
+    Creates suitable ROS2 interface based on Webots
+    [GPS](https://cyberbotics.com/doc/reference/gps) node instance:
+
+    It allows the following functinalities:
+    - Publishes position measurements of type `sensor_msgs::NavSatFix` if WGS84
+    - Publishes position measurements of type `geometry_msgs::PointStamped` if LOCAL
+
+    Args:
+        node (WebotsNode): The ROS2 node.
+        device_key (str): Unique identifier of the device used for configuration.
+        wb_device (Gps): Webots node of type GPS.
+
+    Kwargs:
+        params (dict): Inherited from `SensorDevice` + the following::
+
+            dict: {
+                'timestep': int,              # Publish period in ms (default 128ms)
+            }
+
+    """
+
+    def __init__(self, node, device_key, wb_device, params=None):
+        super().__init__(node, device_key, wb_device, params)
+        self.__speed_publisher = None
+        self.__gps_publisher = None
+        self.__coordinate_system = self._wb_device.getCoordinateSystem()
+
+        # Exit if disabled
+        if self._disable:
+            return
+
+        # Change default timestep
+        self._timestep = 128
+
+        # Create topics
+        self.__speed_publisher = node.create_publisher(
+            Float32, self._topic_name + '/speed', qos_profile_sensor_data)
+
+        if self.__coordinate_system == GPS.WGS84:
+            self.__gps_publisher = node.create_publisher(
+                NavSatFix, self._topic_name + '/gps', qos_profile_sensor_data)
+        else:
+            self.__gps_publisher = node.create_publisher(
+                PointStamped, self._topic_name + '/gps', qos_profile_sensor_data)
+
+    def step(self):
+        stamp = super().step()
+        if not stamp:
+            return
+
+        if self.__gps_publisher.get_subscription_count() > 0 or \
+            self.__speed_publisher.get_subscription_count() > 0 or \
+                self._always_publish:
+            self._wb_device.enable(self._timestep)
+            msg = Float32()
+            msg.data = self._wb_device.getSpeed()
+            self.__speed_publisher.publish(msg)
+            if self.__coordinate_system == GPS.WGS84:
+                msg = NavSatFix()
+                msg.header.stamp = stamp
+                msg.header.frame_id = self._frame_id
+                msg.latitude = self._wb_device.getValues()[0]
+                msg.longitude = self._wb_device.getValues()[1]
+                msg.altitude = self._wb_device.getValues()[2]
+                msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN
+                msg.status.service = NavSatStatus.SERVICE_GPS
+                self.__gps_publisher.publish(msg)
+            else:
+                msg = PointStamped()
+                msg.header.stamp = stamp
+                msg.header.frame_id = self._frame_id
+                msg.point.x = self._wb_device.getValues()[0]
+                msg.point.y = self._wb_device.getValues()[1]
+                msg.point.z = self._wb_device.getValues()[2]
+                self.__gps_publisher.publish(msg)
+        else:
+            self._wb_device.disable()