Fix auton latency (#507)

Author: CameronTressler

launch/basestation.launch

@@ -15,6 +15,9 @@
     <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true"/>
     <node name="topic_services" pkg="mrover" type="topic_services.py"/>
 
+    <!-- Run the auton enable forwarding program -->
+    <node name="auton_enable_forward" pkg="mrover" type="auton_enable_forward.py" output="screen"></node>
+
     <!-- network monitor node-->
     <node name="network_monitor" pkg="mrover" type="network_monitor.py"></node>
 </launch>

scripts/debug_service.py

@@ -6,16 +6,16 @@
 
 from typing import Any
 import rospy
-from std_srvs.srv import Trigger, TriggerResponse
+from mrover.srv import PublishEnableAuton, PublishEnableAutonResponse
 
 # Change these values for the service name and type definition to test different values
-SERVICE_NAME = "mcu_board_reset"
-SERVICE_TYPE = Trigger
+SERVICE_NAME = "enable_auton"
+SERVICE_TYPE = PublishEnableAuton
 
 
 def print_service_request(service_request: Any):
     rospy.loginfo(service_request)
-    return TriggerResponse(success=True, message="")
+    return PublishEnableAutonResponse(success=True)
 
 
 def main():

src/esw/network_monitor.py

@@ -8,6 +8,8 @@
 
 # Credit: https://stackoverflow.com/questions/15616378/python-network-bandwidth-monitor
 
+SLEEP_TIME_S = 3
+
 
 def get_bytes(t: str, interface: str) -> int:
     with open("/sys/class/net/" + interface + "/statistics/" + t + "_bytes", "r") as f:
@@ -27,21 +29,29 @@ def get_iface(default: str) -> Optional[str]:
     return eth_iface
 
 
-if __name__ == "__main__":
+def main():
     rospy.init_node("network_monitor")
     iface = get_iface(rospy.get_param("default_network_iface"))
 
     if iface is not None:
         pub = rospy.Publisher("network_bandwidth", NetworkBandwidth, queue_size=1)
-        while True:
+
+        while not rospy.is_shutdown():
             tx1 = get_bytes("tx", iface)
             rx1 = get_bytes("rx", iface)
-            time.sleep(1)
+            time.sleep(SLEEP_TIME_S)
             tx2 = get_bytes("tx", iface)
             rx2 = get_bytes("rx", iface)
-            tx_speed = (tx2 - tx1) * 8.0 / 1000000.0  # Mbps
-            rx_speed = (rx2 - rx1) * 8.0 / 1000000.0  # Mbps
+
+            # Convert to Mbps
+            tx_speed = (tx2 - tx1) * 8.0 / (SLEEP_TIME_S * 1000000.0)
+            rx_speed = (rx2 - rx1) * 8.0 / (SLEEP_TIME_S * 1000000.0)
+
             pub.publish(NetworkBandwidth(tx_speed, rx_speed))
 
     else:
         rospy.logerr(f"Node {rospy.get_name()} cannot locate valid network interface.")
+
+
+if __name__ == "__main__":
+    main()

src/navigation/navigation.py

@@ -18,6 +18,7 @@
 from post_backup import PostBackupState, PostBackupTransitions
 from smach.log import set_loggers
 from smach.log import loginfo, logwarn, logerr
+from std_msgs.msg import String
 
 
 class Navigation(threading.Thread):
@@ -34,6 +35,7 @@ def __init__(self, context: Context):
         self.context = context
         self.sis = smach_ros.IntrospectionServer("", self.state_machine, "/SM_ROOT")
         self.sis.start()
+        self.state_publisher = rospy.Publisher("/nav_state", String, queue_size=1)
         with self.state_machine:
             self.state_machine.add(
                 "OffState", OffState(self.context), transitions=self.get_transitions(OffStateTransitions)
@@ -70,12 +72,19 @@ def __init__(self, context: Context):
                 PostBackupState(self.context),
                 transitions=self.get_transitions(PostBackupTransitions),
             )
+            rospy.Timer(rospy.Duration(0.1), self.publish_state)
 
     def get_transitions(self, transitions_enum):
         transition_dict = {transition.name: transition.value for transition in transitions_enum}
         transition_dict["off"] = "OffState"  # logic for switching to offstate is built into OffState
         return transition_dict
 
+    def publish_state(self, event=None):
+        with self.state_machine:
+            active_states = self.state_machine.get_active_states()
+            if len(active_states) > 0:
+                self.state_publisher.publish(active_states[0])
+
     def run(self):
         self.state_machine.execute()
 

src/teleop/auton_enable_forward.py

@@ -0,0 +1,87 @@
+#!/usr/bin/env python3
+import rospy
+import threading
+
+from mrover.msg import EnableAuton
+from mrover.srv import PublishEnableAuton, PublishEnableAutonRequest
+
+from typing import Optional
+
+
+class AutonBridge:
+    """
+    A class that manages the state of auton enable. This is necessary because Vue does not have a way to act as a
+    persistent service client. We need persistence in order to quickly enable or disable autonomy, so this program
+    acts as a bridge to send the messages.
+    """
+
+    service_client: rospy.ServiceProxy
+    """
+    A persistent client to give navigation it's enable and course requests.
+    """
+
+    msg: Optional[EnableAuton]
+    """
+    The last received message from the GUI. None if not available.
+    """
+
+    msg_lock: threading.Lock
+    """
+    Mutex to access msg.
+    """
+
+    def __init__(self):
+        """
+        Construct bridge object.
+        """
+        self._connect_to_server()
+
+        self.msg = None
+        self.msg_lock = threading.Lock()
+
+    def _connect_to_server(self):
+        """
+        Create a service proxy, waiting as long as necessary for it to be advertised by auton.
+        """
+        rospy.loginfo("Waiting for navigation to launch...")
+        rospy.wait_for_service("enable_auton")
+
+        self.service_client = rospy.ServiceProxy("enable_auton", PublishEnableAuton, persistent=True)
+        rospy.loginfo("Navigation service found!")
+
+    def handle_message(self, msg: EnableAuton) -> None:
+        """
+        Receive an EnableAuton message from teleop and forward to navigation if it's updated.
+        """
+        with self.msg_lock:
+            # Guard against outdated messages.
+            if self.msg == msg:
+                return
+
+            # Attempt to make service request, updating msg state if successful.
+            try:
+                self.service_client(PublishEnableAutonRequest(msg))
+                self.msg = msg
+
+            # Reconnect service client upon failure.
+            except rospy.ServiceException as e:
+                rospy.logerr(f"Could not forward enable auton message: {e}")
+
+                self.service_client.close()
+                self._connect_to_server()
+
+
+def main():
+    rospy.init_node("auton_enable_forward")
+
+    # Construct the bridge.
+    bridge = AutonBridge()
+
+    # Configure subscriber.
+    rospy.Subscriber("intermediate_enable_auton", EnableAuton, bridge.handle_message)
+
+    rospy.spin()
+
+
+if __name__ == "__main__":
+    main()

src/teleop/gui/src/components/AutonTask.vue

@@ -230,8 +230,8 @@ export default {
   created: function () {
     this.nav_status_sub = new ROSLIB.Topic({
       ros: this.$ros,
-      name: "/smach/container_status",
-      messageType: "smach_msgs/SmachContainerStatus"
+      name: "/nav_state",
+      messageType: "std_msgs/String"
     });
 
     this.odom_sub = new ROSLIB.Topic({
@@ -268,7 +268,7 @@ export default {
 
     this.nav_status_sub.subscribe((msg) => {
       // Callback for nav_status
-      this.nav_status.nav_state_name = msg.active_states[0];
+      this.nav_status.nav_state_name = msg.data;
     });
 
     this.odom_sub.subscribe((msg) => {

src/teleop/gui/src/components/AutonWaypointEditor.vue

@@ -261,7 +261,7 @@ import _ from "lodash";
 import L from "leaflet";
 import ROSLIB from "roslib";
 
-let interval;
+let stuck_interval, intermediate_publish_interval;
 
 const WAYPOINT_TYPES = {
   NO_SEARCH: 0,
@@ -433,42 +433,53 @@ export default {
   },
 
   beforeDestroy: function () {
-    window.clearInterval(interval);
+    window.clearInterval(stuck_interval);
+    window.clearInterval(intermediate_publish_interval);
     this.autonEnabled = false;
     this.sendEnableAuton();
   },
 
   created: function () {
-    (this.course_pub = new ROSLIB.Service({
+    this.course_pub = new ROSLIB.Topic({
       ros: this.$ros,
-      name: "/enable_auton",
-      serviceType: "mrover/PublishEnableAuton",
-    })),
-      (this.nav_status_sub = new ROSLIB.Topic({
-        ros: this.$ros,
-        name: "/smach/container_status",
-        messageType: "smach_msgs/SmachContainerStatus",
-      })),
-      (this.rover_stuck_pub = new ROSLIB.Topic({
-        ros: this.$ros,
-        name: "/rover_stuck",
-        messageType: "std_msgs/Bool",
-      })),
-      // Make sure local odom format matches vuex odom format
-      (this.odom_format_in = this.odom_format);
+      name: "/intermediate_enable_auton",
+      messageType: "mrover/EnableAuton",
+    });
+
+    this.nav_status_sub = new ROSLIB.Topic({
+      ros: this.$ros,
+      name: "/nav_state",
+      messageType: "std_msgs/String",
+    });
+
+    this.rover_stuck_pub = new ROSLIB.Topic({
+      ros: this.$ros,
+      name: "/rover_stuck",
+      messageType: "std_msgs/Bool",
+    });
+
+    // Make sure local odom format matches vuex odom format
+    this.odom_format_in = this.odom_format;
 
     this.nav_status_sub.subscribe((msg) => {
-      if (msg.active_states[0] !== "OffState" && !this.autonEnabled) {
+      // If still waiting for nav...
+      if ((msg.data == "OffState" && this.autonEnabled) ||
+          (msg.data !== "OffState" && !this.autonEnabled)) {
         return;
       }
+
       this.waitingForNav = false;
       this.autonButtonColor = this.autonEnabled ? "green" : "red";
     });
 
     // Interval for publishing stuck status for training data
-    interval = window.setInterval(() => {
+    stuck_interval = window.setInterval(() => {
       this.rover_stuck_pub.publish({ data: this.roverStuck });
     }, 100);
+
+    intermediate_publish_interval = window.setInterval(() => {
+      this.sendEnableAuton();
+    }, 1000);
   },
 
   mounted: function () {
@@ -490,12 +501,9 @@ export default {
     }),
 
     sendEnableAuton() {
-      let course;
-
       // If Auton Enabled send course
       if (this.autonEnabled) {
-        course = {
-          enable: true,
+        this.course_pub.publish({
           // Map for every waypoint in the current route
           waypoints: _.map(this.route, (waypoint) => {
             const lat = waypoint.lat;
@@ -516,20 +524,12 @@ export default {
               id: parseInt(waypoint.id),
             };
           }),
-        };
+          enable: true
+        });
       } else {
         // Else send false and no array
-        course = {
-          enable: false,
-          waypoints: [],
-        };
+        this.course_pub.publish({waypoints: [], enable: false});
       }
-
-      const course_request = new ROSLIB.ServiceRequest({
-        enableMsg: course,
-      });
-
-      this.course_pub.callService(course_request, () => {});
     },
 
     openModal: function () {
@@ -677,8 +677,8 @@ export default {
 
     toggleAutonMode: function (val) {
       this.setAutonMode(val);
-      // This will trigger the yellow "waiting for nav" state of the checkbox only if we are enabling the button
-      this.autonButtonColor = val ? "yellow" : "red";
+      // This will trigger the yellow "waiting for nav" state of the checkbox
+      this.autonButtonColor = "yellow";
       this.waitingForNav = true;
       this.sendEnableAuton();
     },