diff --git a/project.json b/project.json
index 40c1c67b..9a744437 100644
--- a/project.json
+++ b/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"
+ }
}
]
-}
\ No newline at end of file
+}
diff --git a/src/wr_hsi_sensing/CMakeLists.txt b/src/wr_hsi_sensing/CMakeLists.txt
index dc4ff256..2cce2494 100644
--- a/src/wr_hsi_sensing/CMakeLists.txt
+++ b/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)
diff --git a/src/wr_hsi_sensing/README.md b/src/wr_hsi_sensing/README.md
deleted file mode 100644
index d47356ee..00000000
--- a/src/wr_hsi_sensing/README.md
+++ /dev/null
@@ -1,12 +0,0 @@
-# wr_hsi_sensing
-
-@defgroup wr_hsi_sensing wr_hsi_sensing
-@brief Provides GPS and IMU data to the ROS network
-
-This package contains nodes for the GPS sensor and IMU sensor on the WReaper board. These sensors communicate directly with the hardware over the I2C bus.
-
-The current IMU implementation doesn't actually use the IMU (due to a thermal failure at competition), but rather uses the movement of the GPS over time to derive a really coarse estimate of the heading of the rover.
-
-## Launching
-
-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.
diff --git a/src/wr_hsi_sensing/config/ardusimple_config.yaml b/src/wr_hsi_sensing/config/ardusimple_config.yaml
new file mode 100644
index 00000000..78798d71
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/src/wr_hsi_sensing/docs/Part1.png b/src/wr_hsi_sensing/docs/Part1.png
new file mode 100644
index 00000000..86c1e020
Binary files /dev/null and b/src/wr_hsi_sensing/docs/Part1.png differ
diff --git a/src/wr_hsi_sensing/docs/Part2.png b/src/wr_hsi_sensing/docs/Part2.png
new file mode 100644
index 00000000..5936d0e9
Binary files /dev/null and b/src/wr_hsi_sensing/docs/Part2.png differ
diff --git a/src/wr_hsi_sensing/docs/Part3.png b/src/wr_hsi_sensing/docs/Part3.png
new file mode 100644
index 00000000..beca1c03
Binary files /dev/null and b/src/wr_hsi_sensing/docs/Part3.png differ
diff --git a/src/wr_hsi_sensing/docs/Part4.png b/src/wr_hsi_sensing/docs/Part4.png
new file mode 100644
index 00000000..91c08d3a
Binary files /dev/null and b/src/wr_hsi_sensing/docs/Part4.png differ
diff --git a/src/wr_hsi_sensing/docs/Part5.png b/src/wr_hsi_sensing/docs/Part5.png
new file mode 100644
index 00000000..70768ed8
Binary files /dev/null and b/src/wr_hsi_sensing/docs/Part5.png differ
diff --git a/src/wr_hsi_sensing/docs/Part6.png b/src/wr_hsi_sensing/docs/Part6.png
new file mode 100644
index 00000000..1201561e
Binary files /dev/null and b/src/wr_hsi_sensing/docs/Part6.png differ
diff --git a/src/wr_hsi_sensing/docs/README.md b/src/wr_hsi_sensing/docs/README.md
new file mode 100644
index 00000000..8d03db6b
--- /dev/null
+++ b/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
+
+
+
+
+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.
+
+
+
+
+
+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.
+
+
+---
+
+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.
diff --git a/src/wr_hsi_sensing/launch/ardusimple_provided.launch b/src/wr_hsi_sensing/launch/ardusimple_provided.launch
new file mode 100644
index 00000000..cea99e55
--- /dev/null
+++ b/src/wr_hsi_sensing/launch/ardusimple_provided.launch
@@ -0,0 +1,16 @@
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/wr_hsi_sensing/launch/gps_data_mapper.launch b/src/wr_hsi_sensing/launch/gps_data_mapper.launch
new file mode 100644
index 00000000..afe08b2c
--- /dev/null
+++ b/src/wr_hsi_sensing/launch/gps_data_mapper.launch
@@ -0,0 +1,6 @@
+
+
+
+
+
+
diff --git a/src/wr_hsi_sensing/launch/hw_test.launch b/src/wr_hsi_sensing/launch/hw_test.launch
deleted file mode 100644
index b66d3a05..00000000
--- a/src/wr_hsi_sensing/launch/hw_test.launch
+++ /dev/null
@@ -1,7 +0,0 @@
-
-
-
-
-
-
-
diff --git a/src/wr_hsi_sensing/launch/mapviz.launch b/src/wr_hsi_sensing/launch/mapviz.launch
new file mode 100644
index 00000000..4f40db93
--- /dev/null
+++ b/src/wr_hsi_sensing/launch/mapviz.launch
@@ -0,0 +1,20 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/wr_hsi_sensing/nodes/deprecated_gps_node.py b/src/wr_hsi_sensing/nodes/deprecated_gps_node.py
new file mode 100755
index 00000000..33c3e5a5
--- /dev/null
+++ b/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()
+
+## @}
diff --git a/src/wr_hsi_sensing/nodes/imu_testing.py b/src/wr_hsi_sensing/nodes/deprecated_imu_from_gps_node.py
similarity index 100%
rename from src/wr_hsi_sensing/nodes/imu_testing.py
rename to src/wr_hsi_sensing/nodes/deprecated_imu_from_gps_node.py
diff --git a/src/wr_hsi_sensing/nodes/gps_forwarding_node.py b/src/wr_hsi_sensing/nodes/gps_forwarding_node.py
new file mode 100755
index 00000000..fb5c0f26
--- /dev/null
+++ b/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()
\ No newline at end of file
diff --git a/src/wr_hsi_sensing/nodes/gps_testing.py b/src/wr_hsi_sensing/nodes/gps_testing.py
deleted file mode 100755
index aa7acd71..00000000
--- a/src/wr_hsi_sensing/nodes/gps_testing.py
+++ /dev/null
@@ -1,121 +0,0 @@
-#!/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 adafruit_gps import I2C, GPS_GtopI2C as GPS
-from wr_hsi_sensing.msg import CoordinateMsg
-from std_msgs.msg import Float64
-
-# 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
-pub = rospy.Publisher("/gps_coord_data", CoordinateMsg, queue_size=1)
-## Publisher for heading data
-# TODO(@bennowotny): This should be migrated back to the IMU code
-pub_heading = 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
-
-"""
-Optional: average heading to get more stable value
-NUM_CACHED_HEADINGS = 3
-heading_ind = 0
-past_headings = np.zeros(NUM_CACHED_HEADINGS)
-"""
-
-
-def cb(gps: GPS) -> None:
- """ROS Timer callback to process data (coordinates and heading) from the GPS sensor
-
- @param gps The GPS sensor to talk to
- """
- try:
- gps.update()
- except:
- return
- print(f"lat: {gps.latitude} long: {gps.longitude}")
- if gps.latitude is not None and gps.longitude is not None:
- global last_lat
- global last_long
- global cached_heading
- """
- Optional: average heading to get more stable value
- global heading_ind
- global past_headings
- """
-
- msg = CoordinateMsg(latitude=gps.latitude, longitude=gps.longitude)
- pub.publish(msg)
-
- # TODO (@bennowotny): We probably want to keep this, but it should be in its own file, or at least an option we can shut off
- if (
- last_lat is not None
- and last_long is not None
- and (
- abs(gps.longitude - last_long) > GPS_TOLERANCE
- or abs(gps.latitude - last_lat) > GPS_TOLERANCE
- )
- ):
- # Find heading from the change in latitude and longitude
- X = math.cos(gps.latitude) * math.sin(gps.longitude - last_long)
- Y = math.cos(last_lat) * math.sin(gps.latitude) - math.sin(
- last_lat
- ) * math.cos(gps.latitude) * math.cos(gps.longitude - last_long)
- bearing = math.atan2(X, Y)
- bearing = bearing if bearing > 0 else 2 * math.pi + bearing
- bearing = math.degrees(bearing)
- cached_heading = bearing
- pub_heading.publish(Float64(data=bearing))
-
- """
- Optional: average heading to get more stable value
- past_headings[heading_ind] = bearing
- i += 1
- i %= NUM_CACHED_HEADINGS
- avg_heading = past_headings.sum() / NUM_CACHED_HEADINGS
- pub_heading.publish(Float64(data=avg_heading))
- """
-
- # Update last GPS latitude and longitude
- last_lat = gps.latitude
- last_long = gps.longitude
- if cached_heading is not None:
- pub_heading.publish(Float64(data=cached_heading))
-
-
-if __name__ == "__main__":
- ## GPS used to get data, defined on the RPi I2C bus
- gps = GPS(i2c_bus=I2C(sda=2, scl=3))
- gps.begin(0x10)
- ## ROS timer to calculate and publish GPS data
- timer = rospy.Timer(rate.sleep_dur, lambda _: cb(gps))
- rospy.spin()
-
-## @}
diff --git a/src/wr_hsi_sensing/package.xml b/src/wr_hsi_sensing/package.xml
index 7c0a865b..8aacb988 100644
--- a/src/wr_hsi_sensing/package.xml
+++ b/src/wr_hsi_sensing/package.xml
@@ -37,19 +37,27 @@
rospy
+ ublox_gps
+ mapviz
+
message_generation
+
message_generation
+
+ catkin
+
message_runtime
+
+
- catkin