Feature/dgnss updates (#91)

* Refactored gps_testing.py. Moved moving average to a class,
moved heading calculation to its own method

* build: Add ublox ROS driver to project.json dependencies

* toss some rtk configs together

* Add node for publishing ROS topic from RTK GNSS

* Updated gps_testing node to include new node that interfaces with ROS library for new gps

* Add launch documentation

* Add mapviz

* Rename rtk files

* updated README

* Update comments/document code

* Update wr_hsi_sensing readme

* Clarify README

* Update README

* rename nodes

* update launch files to launch dependent nodes

* delete old launch file

* remove extra debug logging

* revise dependency declarations

* remove build instructions in readme

* add images to readme

* Move README so images show up in doxygen

---------

Co-authored-by: Aiden Carney <[email protected]>
Co-authored-by: Colin Li <[email protected]>
Co-authored-by: LetianLi <[email protected]>

project.json

@@ -44,6 +44,20 @@
         "package": "ros-noetic-moveit-visual-tools",
         "provider": "apt"
       }
+    },
+    {
+      "name": "ros-ublox",
+      "build": {
+        "package": "ros-noetic-ublox",
+        "provider": "apt"
+      }
+    },
+    {
+      "name": "mapviz",
+      "build": {
+        "package": "ros-noetic-mapviz",
+        "provider": "apt"
+      }
     }
   ]
-}
+}

src/wr_hsi_sensing/CMakeLists.txt

@@ -7,7 +7,12 @@ project(wr_hsi_sensing)
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 ## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS message_generation std_msgs rospy)
+find_package(catkin REQUIRED COMPONENTS 
+    message_generation
+    std_msgs
+    rospy
+    ublox_gps
+)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)

src/wr_hsi_sensing/config/ardusimple_config.yaml

@@ -0,0 +1,17 @@
+debug: 1 # Range 0-4 (0 means no debug statements will print)
+
+device: /dev/ttyACM0
+frame_id: gps # TODO: change it in the future?
+
+config_on_startup: false
+uart1:
+  baudrate: 9600
+rate: 1
+nav_rate: 1
+publish:
+  all: true
+# fix_mode: auto
+# dgnss_mode: 3 # Fixed mode
+
+# inf: 
+#   all: true

src/wr_hsi_sensing/docs/README.md

@@ -0,0 +1,45 @@
+# wr_hsi_sensing
+
+@defgroup wr_hsi_sensing wr_hsi_sensing
+@brief Provides GPS data to the ROS network
+
+This package contains nodes for the ArduSimple RTK GNSS board, a mapping node that maps the ArduSimple /gps/fix topic to a message used in our ROS environment, and a node for mapviz, a visualization tool.
+
+There is also an old node "previous_gps_node.py" that was used when we were using an old GPS sensor to calculate IMU heading data as a temporary fix during a competition.
+It is no longer used.
+
+# Launching Nodes
+
+## Setup
+- Check which tty exists: `ls /dev/tty*`
+- We'll need access to a file so run: `sudo chmod a+rw /dev/ttyACM0`
+
+## Launching
+To launch the ArduSimple/Ublox GPS node: `roslaunch wr_hsi_sensing ardusimple_provided.launch`
+- To see all ros topics run: `rostopic list`
+- To get info of a topic run: `rostopic info /gps/fix`
+- To see all messages on a topic run: `rostopic echo /gps/fix`
+
+To launch the mapping node: `roslaunch wr_hsi_sensing gps_data_mapper.launch`
+- You'll need the ArduSimple/Ublox GPS node for this node to do anything
+
+To lanch mapviz: `roslaunch wr_hsi_sensing mapviz.launch`
+1. You'll need a BingMaps API key to get the satellite map layer 😭. Visit https://www.bingmapsportal.com/
+2. Set fixed Frame to map in the upper left corner, set Target Frame to "none", and check use latest transforms.
+3. Click Add at the bottom left to add tile_map. After that, set Source to BingMaps. Use the afforementioned API Key
+
+![Part1](./Part1.png)
+![Part2](./Part2.png)
+
+4. Click Add again and add navsat. After that, click Select on Topic to load the topic set in remap (/gps/fix). After setting the color you like, change the draw style to point.
+
+![Part3](./Part3.png)
+![Part4](./Part4.png)
+![Part5](./Part5.png)
+
+5. If you don't see the map, click the green arrow at the bottom right. Then, you can see a dot at your location.
+![Part6](docs/Part6.png)
+
+---
+
+OLD/DEPRECATED: The `hw_test.launch` file starts both the IMU and GPS sensing.  Currently, since the IMU logic was not run at competition, only the GPS node is started.

src/wr_hsi_sensing/launch/ardusimple_provided.launch

@@ -0,0 +1,16 @@
+<launch>
+    <arg name="node_name" default="gps"/>
+    <arg name="param_path" default="$(find wr_hsi_sensing)/config/ardusimple_config.yaml" />
+    <arg name="output" default="screen" />
+    <arg name="respawn" default="true" />
+    <arg name="respawn_delay" default="30" />
+    <arg name="clear_params" default="true" />
+
+    <node pkg="ublox_gps" type="ublox_gps" name="$(arg node_name)"
+        output="$(arg output)"
+        clear_params="$(arg clear_params)"
+        respawn="$(arg respawn)"
+        respawn_delay="$(arg respawn_delay)">
+    <rosparam command="load" file="$(arg param_path)" />
+    </node>
+</launch>

src/wr_hsi_sensing/launch/gps_data_mapper.launch

@@ -0,0 +1,6 @@
+<launch>
+    <!-- Launches GPS data mapping node that translate ardusimple GPS messages to CoordinateMsg -->
+    <!-- <arg name="debug" default="false"/> -->
+    <include file="$(find wr_hsi_sensing)/launch/ardusimple_provided.launch" />
+    <node name="gps_mapper" type="gps_forwarding_node.py" pkg="wr_hsi_sensing" output="screen"/>
+</launch>

src/wr_hsi_sensing/launch/mapviz.launch

@@ -0,0 +1,20 @@
+<launch>
+  <!-- Launches mapviz visualization. -->
+  <!-- Requires ardusimple_provided node to be running -->
+  <!-- <include file="$(find wr_hsi_sensing)/launch/ardusimple_provided.launch" /> -->
+
+  <node pkg="mapviz" type="mapviz" name="mapviz">
+    <!-- <param name="config" value=""/> -->
+  </node>
+
+  <node pkg="swri_transform_util" type="initialize_origin.py" name="initialize_origin" >
+    <param name="local_xy_frame" value="/map"/> <!-- Could be "/map"-->
+    <param name="local_xy_origin" value="auto"/>
+    <param name="local_xy_navsatfix_topic" value="/gps/fix"/>
+    <remap from="fix" to="/gps/fix"/>
+
+  </node>
+
+  <node pkg="tf" type="static_transform_publisher" name="swri_transform" args="0 0 0 0 0 0 /map /origin 100"  />
+
+</launch>

src/wr_hsi_sensing/nodes/deprecated_gps_node.py

@@ -0,0 +1,174 @@
+#!/usr/bin/env python
+
+## @file
+# @brief Node executing GPS and heading calculations
+# @ingroup wr_hsi_sensing
+# @defgroup wr_hsi_sensing_gps GPS
+# @brief Responsible for calculating GPS and heading data
+# @details Getting coordinate data from the GPS is a straightforward query to the sensor.  Calculating the heading from GPS data requires
+# reading multiple GPS positions and finding the heading of the line connecting the two GPS coordinates.  This method works well for systems that
+# have consistent motion patterns and can really only move in 1 direction (like cars/trucks).  This is not a great candidate for calculating heading on
+# the rover; it is a better idea to get the @ref wr_hsi_sensing_imu working.
+# @ingroup wr_hsi_sensing
+# @{
+
+import math
+import rospy
+from wr_hsi_sensing.msg import CoordinateMsg
+from std_msgs.msg import Float64
+from sensor_msgs.msg import NavSatFix
+
+
+# Constants to vary
+## Rate at which pose messages are published in hz
+f = 100
+
+## @cond
+rospy.init_node("gps_testing", anonymous=False)
+## @endcond
+## Rate controlling how frequently we publish data
+rate = rospy.Rate(f)
+## Publisher for GPS coordinate data
+gpsPub = rospy.Publisher("/gps_coord_data", CoordinateMsg, queue_size=1)
+## Publisher for heading data
+# TODO(@bennowotny): This should be migrated back to the IMU code
+headingPub = rospy.Publisher("/heading_data", Float64, queue_size=1)
+
+# ## Last latitude recorded, used to calculate heading
+# last_lat = None
+# ## Last longitude recorded, used to calculate heading
+# last_long = None
+# ## Last heading recorded, used to maintain publishing frequency once heading is locked
+# cached_heading = None
+
+## How much the GPS data has to deviate to reliably calculate heading
+GPS_TOLERANCE = 0.000001
+
+
+class MovingAverage:
+    """
+    Class for keeping track of moving average of data. Implemented
+    using a circular buffer
+    """
+    def __init__(self, windowSize: int):
+        self.windowSize = max(windowSize, 1)  # don't allow to be less than 1
+        self.data = []
+        self.avg = -1
+        self.index = 0
+
+    def add(self, data: float) -> None:
+        """
+        Adds data to the circular buffer. Simply appends if buffer not at max size, otherwise
+        it starts overwriting data
+        """
+        if len(self.data) < self.windowSize:  # buffer not at max size, so just increase size
+            self.data.append(data)
+            self.index += 1
+        else:
+            self.data[self.index] = data
+            self.index = (self.index + 1) % self.windowSize  # wrap around for next write
+        self.avg = sum(self.data) / len(self.data)
+
+    def get(self) -> float:
+        """
+        Returns average of items in circular buffer
+        """
+        return self.avg
+
+    def getLatest(self) -> float:
+        """
+        returns the last recorded data, not an average
+        """
+        return self.data[self.index]
+
+
+class GPSController:
+    """
+    Controls gps signals. Handles data updates and publishing data
+    """
+    def __init__(self, gpsPub: rospy.Publisher, headingPub: rospy.Publisher, movingAverageWindow: int, debug: bool = False):
+        self.headingPub = headingPub
+        self.gpsPub = gpsPub
+
+        self.averageHeading = True
+
+        self.latQueue = MovingAverage(movingAverageWindow)
+        self.longQueue = MovingAverage(movingAverageWindow)
+        self.prevLat = 0
+        self.prevLong = 0
+
+        self.heading = 0
+        self.publishHeading = True
+
+        self.debug = debug
+
+    def udpateHeading(self) -> None:
+        """
+        Updates heading according to formula explained here:
+        https://www.igismap.com/formula-to-find-bearing-or-heading-angle-between-two-points-latitude-longitude/
+        TODO: this function should not be used, switch back to IMU for heading
+        """
+        if (self.latQueue.get() is not None and self.longQueue.get() is not None and (
+            abs(self.latQueue.get() - self.prevLat) > GPS_TOLERANCE
+            or abs(self.longQueue.get() - self.lastLong))
+        ):
+            # Find heading from the change in latitude and longitude
+            X = math.cos(self.latQueue.get()) * math.sin(self.longQueue.get() - self.prevLong)
+            Y = math.cos(self.prevLat) * math.sin(self.latQueue.get()) - math.sin(
+                self.prevLat
+            ) * math.cos(self.latQueue.get()) * math.cos(self.longQueue.get() - self.prevLong)
+            bearing = math.atan2(X, Y)  # probably correct, not verified though
+            bearing = bearing if bearing > 0 else 2 * math.pi + bearing
+            bearing = math.degrees(bearing)
+
+            self.heading = bearing
+
+
+    def readCallback(self, msg):
+        self.prevLat = self.latQueue.get()
+        self.prevLong = self.longQueue.get()
+
+        self.latQueue.add(msg.latitude)
+        self.longQueue.add(msg.longitude)
+
+        if self.debug:
+            print("latitude  from sensor: {}".format(msg.latitude))
+            print("longitude from sensor: {}".format(msg.longitude))
+
+
+    def writeCallback(self) -> None:
+        """
+        Callback function called by ros. Updates headings and publishes data
+        """
+
+        if self.latQueue.get() is not None and self.longQueue.get() is not None:
+            self.updateHeading()
+
+            # publish lat and long
+            msg = CoordinateMsg(latitude=self.latQueue.get(), longitude=self.longQueue.get())
+            self.gpsPub.publish(msg)
+
+            # publish heading if desired
+            if self.publishHeading and self.heading is not None:
+                self.headingPub.publish(Float64(data=self.heading))
+
+            # debug message
+            if self.debug:
+                print(f"DATA PUBLISHED    lat: {self.latQueue.get()}  long: {self.longQueue.get()}  heading: {self.heading}")
+
+
+
+if __name__ == "__main__":
+    ## GPS used to get data, defined on the RPi I2C bus
+    controller = GPSController(gpsPub, headingPub, 1)
+
+    # set up subscriber to ros library for the gps sensor
+    rospy.init_node("gps_data_subscriber_node", anonymous = False)
+    rospy.Subscriber('/gps/fix', NavSatFix, controller.readCallback)
+
+    ## ROS timer to calculate and publish GPS data
+    timer = rospy.Timer(rate.sleep_dur, lambda _: controller.writeCallback()) 
+
+    rospy.spin()
+
+## @}

src/wr_hsi_sensing/nodes/gps_forwarding_node.py

@@ -0,0 +1,30 @@
+#!/usr/bin/env python
+
+## @file
+# @brief Node responsible for mapping Ardusimple GPS data to CoordinateMsg
+# @ingroup wr_hsi_sensing
+# @defgroup wr_hsi_sensing_gps GPS
+# @details Subscribes to the /gps/fix topic and publishes the latitude and longitude data to the /gps_coord_data topic
+# @{
+
+import rospy
+from sensor_msgs.msg import NavSatFix
+from wr_hsi_sensing.msg import CoordinateMsg
+
+
+def gps_callback(msg):
+    gps_coord_data = CoordinateMsg()
+    gps_coord_data.latitude = msg.latitude
+    gps_coord_data.longitude = msg.longitude
+    coord_data_publisher.publish(gps_coord_data)
+
+def gps_data_subscriber():
+    rospy.init_node("gps_data_subscriber_node", anonymous = False)
+    rospy.Subscriber('/gps/fix', NavSatFix, gps_callback)
+
+    global coord_data_publisher
+    coord_data_publisher = rospy.Publisher("/gps_coord_data", CoordinateMsg, queue_size=1)
+    rospy.spin()
+
+if(__name__ == "__main__"):
+    gps_data_subscriber()