diff --git a/launch/basestation.launch b/launch/basestation.launch
index b486d7783..53277bc44 100644
--- a/launch/basestation.launch
+++ b/launch/basestation.launch
@@ -15,6 +15,9 @@
+
+
+
\ No newline at end of file
diff --git a/scripts/debug_service.py b/scripts/debug_service.py
index 9b60afc91..082a85981 100755
--- a/scripts/debug_service.py
+++ b/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():
diff --git a/src/esw/network_monitor.py b/src/esw/network_monitor.py
index b3485753e..d17b34c27 100755
--- a/src/esw/network_monitor.py
+++ b/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()
diff --git a/src/navigation/navigation.py b/src/navigation/navigation.py
index a2112b308..aba60a399 100755
--- a/src/navigation/navigation.py
+++ b/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()
diff --git a/src/teleop/auton_enable_forward.py b/src/teleop/auton_enable_forward.py
new file mode 100755
index 000000000..a2072dd83
--- /dev/null
+++ b/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()
diff --git a/src/teleop/gui/src/components/AutonTask.vue b/src/teleop/gui/src/components/AutonTask.vue
index 9ffae25fd..678444f90 100644
--- a/src/teleop/gui/src/components/AutonTask.vue
+++ b/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) => {
diff --git a/src/teleop/gui/src/components/AutonWaypointEditor.vue b/src/teleop/gui/src/components/AutonWaypointEditor.vue
index d6e17582c..1922ea15b 100644
--- a/src/teleop/gui/src/components/AutonWaypointEditor.vue
+++ b/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();
},