Add boats to path (#62)

* Add code to handle cases with invalid start states or not enough runtime

* Improve code readability, use inexact solution path if needed, show non-blocking pathfinding problem plot, and extend obstacles based on their distance to the boat

* Fix typo

* Improve log error message

* Fix bug, make the shrinkFactor a float

* Fix tests for extendObstaclesArray

* Save work for new working implementation

* Improve obstacleOnPath detection to use the boat's position and index

* Add unit tests for obstacleOnPath

* Fix visualizer to have equal aspect ratio and add optional plotting in createLocalPathSS

* Fix small bug

* Clean utilities.py code

* Fix typo

* Improve code readability

* Write tests for sailingUpwindOrDownwind and clean up code for that

* Make plot close-able

* modified MOCK_AIS to add and delete boats, addBoat node to accept addBoat msg, new msg type addBoat

* new class Ship, added roslaunch that includes visualizer, adding and deleting boats working

* accept speed parameters from ship part of msg

* added README, changed topic name, code cleanup

* fixed obstacles not showing when speed = 0

Co-authored-by: Tyler Lum <tylergwlum@gmail.com>

Author: Zephko

CMakeLists.txt

@@ -57,6 +57,7 @@ add_message_files(
   latlon.msg
   path.msg
   windSensor.msg
+  addBoat.msg
 )
 
 ## Generate services in the 'srv' folder

launch/all.launch

@@ -0,0 +1,10 @@
+<launch>
+    <node pkg="local_pathfinding" type="MOCK_wind_sensor.py" name="MOCK_wind_sensor" />
+    <node pkg="local_pathfinding" type="MOCK_controller_and_boat.py" name="MOCK_controller_and_boat" />
+    <node pkg="local_pathfinding" type="MOCK_AIS.py" name="MOCK_AIS" />
+    <node pkg="local_pathfinding" type="MOCK_global_planner.py" name="MOCK_global_planner" />
+    <node pkg="local_pathfinding" type="MOCK_UDP_bridge.py" name="MOCK_UDP_bridge" />
+    <node pkg="local_pathfinding" type="addBoat.py" name="addBoat" />
+    <node pkg="local_pathfinding" type="main_loop.py" name="main_loop" />
+    <node pkg="local_pathfinding" type="local_path_visualizer.py" name="local_path_visualizer" />
+</launch>

msg/addBoat.msg

@@ -0,0 +1,3 @@
+string addType
+AISShip ship
+int32 waypointIndex

python/MOCK_AIS.py

@@ -4,10 +4,11 @@
 import random
 from geopy.distance import distance
 from utilities import headingToBearingDegrees, PORT_RENFREW_LATLON
+from std_msgs.msg import Int32
 
-import local_pathfinding.msg as msg
+from local_pathfinding.msg import AISShip, AISMsg
 
-class Ship:
+class RandomShip:
     def __init__(self, id, sailbot_lat, sailbot_lon, publishPeriodSeconds):
         self.id = id
         self.headingDegrees = random.randint(0, 360)
@@ -28,42 +29,71 @@ def move(self):
         self.lat = boatLatlon.latitude
 
     def make_ros_message(self):
-        return msg.AISShip(
+        return AISShip(
                     self.id,
                     self.lat,
                     self.lon,
                     self.headingDegrees,
                     self.speedKmph)
 
+class Ship:
+    def __init__(self, id, boat_lat, boat_lon, heading, speed):
+        self.id = id
+        self.lat = boat_lat
+        self.lon = boat_lon
+        self.headingDegrees = heading
+        self.speedKmph = speed
+
+    def make_ros_message(self):
+        return AISShip(
+                    self.id,
+                    self.lat,
+                    self.lon,
+                    self.headingDegrees,
+                    self.speedKmph)
 
+        
 class MOCK_AISEnvironment: 
     # Just a class to keep track of the ships surrounding the sailbot
     def __init__(self, lat, lon):
         self.publishPeriodSeconds = 1.0
         self.numShips = 10
         self.ships = []
         for i in range(self.numShips):
-            self.ships.append(Ship(i, lat, lon, self.publishPeriodSeconds))
+            self.ships.append(RandomShip(i, lat, lon, self.publishPeriodSeconds))
+
 
         rospy.init_node('MOCK_AIS', anonymous=True)
-        self.publisher = rospy.Publisher("AIS", msg.AISMsg, queue_size=4)
+        self.publisher = rospy.Publisher("AIS", AISMsg, queue_size=4)
+        rospy.Subscriber('/new_boats', AISShip, self.new_boat_callback)
+        rospy.Subscriber('/delete_boats', Int32, self.remove_boat_callback)
 
     def move_ships(self):
         for i in range(self.numShips):
-            self.ships[i].move()
-
+            if isinstance(self.ships[i], RandomShip):
+                self.ships[i].move()
+      
     def make_ros_message(self):
+        rospy.loginfo([ship.id for ship in self.ships])
         ship_list = []
         for i in range(self.numShips):
             ship_list.append(self.ships[i].make_ros_message())
-        return msg.AISMsg(ship_list)
+        return AISMsg(ship_list)
+    
+    def new_boat_callback(self, msg):
+        self.ships.append(Ship(msg.ID, msg.lat, msg.lon, msg.headingDegrees, msg.speedKmph))
+        self.numShips += 1
+        
+    def remove_boat_callback(self, msg):
+        self.ships[:] = [ship for ship in self.ships if not ship.id == msg.data] 
+        self.numShips = len(self.ships)
 
 if __name__ == '__main__':
     ais_env = MOCK_AISEnvironment(PORT_RENFREW_LATLON.lat, PORT_RENFREW_LATLON.lon)
     r = rospy.Rate(1.0 / ais_env.publishPeriodSeconds) #hz
 
     while not rospy.is_shutdown():
-        ais_env.move_ships()
         data = ais_env.make_ros_message()
+        ais_env.move_ships()
         ais_env.publisher.publish(data)
         r.sleep()

python/README.md

@@ -0,0 +1,29 @@
+# How to add and delete boats
+
+Ensure all nodes are launched. Use `roslaunch local_pathfinding all.launch`
+
+## Add Boats
+
+Type `rostopic pub /new_boats ...` and press Tab twice to generate a message template and fill in the parameters for the ship you would like to add.
+
+## Delete Boats
+
+Type `rostopic pub /delete_boats` and press Tab twice. Fill in the ID of the boats you want to remove.
+
+## Add boats using other methods
+
+Type `rostopic pub /boat_on_path ...` and press Tab twice.
+
+specify the addType as one of the following options:
+
+
+1. `addType = latlon` will just publish a boat with the specified parameters in addBoat.ship
+
+2. `addType = nextWaypoint` will publish a boat at the next localWaypoint with the specified ID, heading, and speed in addBoat.ship (lat lon params are ignored)
+
+3. `addType = onBoat` will publish a boat at the current location of the Sailbot with the specified ID, heading, and speed in addBoat.ship (lat lon params are ignored)
+
+4. `addType = index` will publish a boat at localPath[addBoat.waypointIndex] with the specified ID, heading, and speed in addBoat.ship (lat lon params are ignored)
+
+
+

python/addBoat.py

@@ -0,0 +1,44 @@
+#!/usr/bin/env python
+import rospy
+from std_msgs.msg import Int32
+from local_pathfinding.msg import AISMsg, AISShip, latlon, addBoat, GPS, path
+
+localWaypoint = latlon()
+gps = latlon()
+localPath = []
+
+def command_callback(msg):
+    if msg.addType == "latlon":
+        add_pub.publish(msg.ship)
+    if msg.addType == "nextWaypoint":
+        add_pub.publish(AISShip(msg.ship.ID, localWaypoint.lat, localWaypoint.lon, msg.ship.headingDegrees, msg.ship.speedKmph))
+    if msg.addType == "onBoat":
+        add_pub.publish(AISShip(msg.ship.ID, gps.lat, gps.lon, msg.ship.headingDegrees, msg.ship.speedKmph))
+    if msg.addType == "index":
+        if msg.waypointIndex < len(localPath) and msg.waypointIndex >=0:
+            add_pub.publish(AISShip(msg.ship.ID, localPath[msg.waypointIndex].lat, localPath[msg.waypointIndex].lon, msg.ship.headingDegrees, msg.ship.speedKmph))
+            rospy.loginfo("new ship generated at index {} of local path".format(msg.waypointIndex))
+        else:
+            rospy.loginfo("invalid index passed")
+        
+def lwp_callback(coordinates):
+    global localWaypoint
+    localWaypoint = coordinates 
+
+def gps_callback(msg):
+    global gps
+    gps = latlon(msg.lat, msg.lon) 
+
+def localPath_callback(msg):
+    global localPath
+    localPath = msg.waypoints
+
+if __name__ == "__main__":
+    rospy.init_node('addBoatsToPath', anonymous=True)
+    rospy.Subscriber('/boat_on_path', addBoat, command_callback)
+    rospy.Subscriber('/nextLocalWaypoint', latlon, lwp_callback)
+    rospy.Subscriber('/GPS', GPS, gps_callback)
+    rospy.Subscriber('/localPath', path, localPath_callback)
+    add_pub = rospy.Publisher('/new_boats', AISShip, queue_size=4)
+    rospy.Rate(1)
+    rospy.spin()

python/utilities.py

@@ -295,6 +295,10 @@ def extendObstaclesArray(aisArray, sailbotPosition, sailbotSpeedKmph, referenceL
             timeToLocHours = distanceToBoatKm / sailbotSpeedKmph
         extendBoatLengthKm = aisData.speedKmph * timeToLocHours
 
+        if extendBoatLengthKm == 0:
+            obstacles.append(Obstacle(aisX, aisY, AIS_BOAT_RADIUS_KM))
+
+
         if aisData.headingDegrees == 90 or aisData.headingDegrees == 270:
             if aisData.headingDegrees == 90:
                 endY = aisY + extendBoatLengthKm