From f81c500ce9cd831a0de4d75d21c45561f3c35b66 Mon Sep 17 00:00:00 2001 From: Emily Date: Wed, 1 Sep 2021 05:20:20 -0700 Subject: [PATCH 1/3] watched workshop --- tutorials/heartbeat_subscriber.py | 107 ++++++++++++++++++++++++++++++ tutorials/turtle_pub.py | 31 +++++++++ tutorials/turtle_sub.py | 21 ++++++ 3 files changed, 159 insertions(+) create mode 100755 tutorials/heartbeat_subscriber.py create mode 100755 tutorials/turtle_pub.py create mode 100755 tutorials/turtle_sub.py diff --git a/tutorials/heartbeat_subscriber.py b/tutorials/heartbeat_subscriber.py new file mode 100755 index 0000000..7fd1839 --- /dev/null +++ b/tutorials/heartbeat_subscriber.py @@ -0,0 +1,107 @@ +#!/usr/bin/env python3 + +""" +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +Monash Nova Rover Team + +This python script sends out the beep boops from base station. +Hopefully rovey picks them up and don't die + +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +NODE: heartbeat_subscriber +TOPICS: + - /tutorials/heartbeat [Empty] +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +PACKAGE: tutorials +AUTHOR(S): Emily Kuo +CREATION: 01/09/2021 +EDITED: 01/09/2021 +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ +TODO: + - Have a Sunny day outside +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + + + + + + + + +Not done yet!! + + +""" + + + + + + + + +import rclpy +from rclpy.node import Node +from std_msgs.msg import Empty + +class HeartbeatSub: + # Stores whether or not a beat has been found + beat = False + beat_count = 0 + + # The time out and loop counts + LOOP_HERTZ = 50 + TIME_OUT = 25 + + # The initialisation function + def __init__ (self): + print("Initialising Heartbeat Subscriber class.") + + # Set up the ROS params + rospy.init_node('heartbeat_sub', anonymous=True) + rospy.Subscriber("/base/heartbeat", Empty, self.received_cb) + + # Run the check node for seeing if heartbeat exists + self.check_heartbeat() + + # Called when received heartbeat information + def received_cb (self, data): + self.heartbeat() + self.beat_count = 0 + + # Checks the heartbeat on the loop + def check_heartbeat (self): + rate = rospy.Rate(self.LOOP_HERTZ) # 10hz + + # Call the first heartbeat for the LED controller + self.heartbeat() + + # While still running ROS + while not rospy.is_shutdown(): + # If the beat count exceeds the time out + if self.beat_count > self.TIME_OUT: + self.no_heartbeat() + + # Otherwise increase beat count + else: + self.beat_count += 1 + + # Set the ROS param for the heartbeat + rospy.set_param("/rover/heartbeat", self.beat_count <= self.TIME_OUT) + + rate.sleep() + + # When heartbeat times out + def no_heartbeat (self): + self.beat = False + print("\033[1;31;48mNo Heartbeart Detected\033[0;0m") + + # When heartbeat returns + def heartbeat (self): + if not self.beat: + print("\033[1;31;44mHeartbeat Returned\033[0;0m") + self.beat = True + +# The main function when run +if __name__ == "__main__": + sub = HeartbeatSub() diff --git a/tutorials/turtle_pub.py b/tutorials/turtle_pub.py new file mode 100755 index 0000000..30c19c2 --- /dev/null +++ b/tutorials/turtle_pub.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from std_msgs.msg import Empty + +class Publisher(Node): + def __init__(self): + super().__init__("node_pub") + + self.publisher = self.create_publisher(Empty,"/tutorials/turtle", 10) + self.timer = self.create_timer(1, self.callback_func) + + def callback_func(self): + msg = Empty() + self.publisher.publish(msg) + +def main(): + rclpy.init() + publisher = Publisher() + + rclpy.spin(publisher) + +if __name__ == "__main__": + main() + + + + + + diff --git a/tutorials/turtle_sub.py b/tutorials/turtle_sub.py new file mode 100755 index 0000000..a845086 --- /dev/null +++ b/tutorials/turtle_sub.py @@ -0,0 +1,21 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from std_msgs.msg import Empty + +class Subscriber(Node): + def __init__(self): + super().__init__("node_sub") + self.subscription = self.create_subscription(Empty,"/tutorials/turtle", self.callback_func, 10) + + def callback_func(self, msg): + print("Got data yo") + +def main(): + rclpy.init() + subscriber = Subscriber() + rclpy.spin(subscriber) + +if __name__ == "__main__": + main() From 92331433d406e457ef63f4804525d66e59b86e86 Mon Sep 17 00:00:00 2001 From: Emily Date: Thu, 2 Sep 2021 02:42:43 -0700 Subject: [PATCH 2/3] tested hb_sub and it works --- tutorials/heartbeat_subscriber.py | 123 +++++++++++++----------------- 1 file changed, 51 insertions(+), 72 deletions(-) diff --git a/tutorials/heartbeat_subscriber.py b/tutorials/heartbeat_subscriber.py index 7fd1839..75de9bb 100755 --- a/tutorials/heartbeat_subscriber.py +++ b/tutorials/heartbeat_subscriber.py @@ -21,87 +21,66 @@ - Have a Sunny day outside ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ - - - - - - - -Not done yet!! - - """ - - - - - - - import rclpy from rclpy.node import Node from std_msgs.msg import Empty -class HeartbeatSub: - # Stores whether or not a beat has been found - beat = False - beat_count = 0 - - # The time out and loop counts - LOOP_HERTZ = 50 - TIME_OUT = 25 - - # The initialisation function - def __init__ (self): +class HeartbeatSubscriber(Node): + def __init__(self): + super().__init__("heartbeat_sub") print("Initialising Heartbeat Subscriber class.") - - # Set up the ROS params - rospy.init_node('heartbeat_sub', anonymous=True) - rospy.Subscriber("/base/heartbeat", Empty, self.received_cb) - - # Run the check node for seeing if heartbeat exists - self.check_heartbeat() - - # Called when received heartbeat information - def received_cb (self, data): - self.heartbeat() - self.beat_count = 0 - - # Checks the heartbeat on the loop - def check_heartbeat (self): - rate = rospy.Rate(self.LOOP_HERTZ) # 10hz - - # Call the first heartbeat for the LED controller + self.subscription = self.create_subscription(Empty, "/tutorials/heartbeat", self.heartbeat_cb, 10) + # Stores whether or not a beat has been found + self.hb_detected = False + self.timeout_count = 0 + + # The time out and loop count constants + self.LOOP_HERTZ = 50 + self.TIMEOUT = 25 + self.timer = self.create_timer(1/self.LOOP_HERTZ, self.increment_count) + + def heartbeat_cb(self, msg): + """ + Called when receives a message from the /tutorials/heartbeat topic + """ self.heartbeat() - - # While still running ROS - while not rospy.is_shutdown(): - # If the beat count exceeds the time out - if self.beat_count > self.TIME_OUT: - self.no_heartbeat() - - # Otherwise increase beat count - else: - self.beat_count += 1 - - # Set the ROS param for the heartbeat - rospy.set_param("/rover/heartbeat", self.beat_count <= self.TIME_OUT) - - rate.sleep() - - # When heartbeat times out - def no_heartbeat (self): - self.beat = False + self.timeout_count = 0 + + def increment_count(self): + """ + Called based on the timer, LOOP_HERTZ times a second + Increments the timeout counter + """ + if self.timeout_count == self.TIMEOUT: + #only prints once when heartbeat is lost + #can change if you prefer the spam + self.no_heartbeat() + self.timeout_count +=1 + + def no_heartbeat(self): + """ + Called when the timeout has been exceeded without receiving any heartbeat messages + """ + self.hb_detected = False print("\033[1;31;48mNo Heartbeart Detected\033[0;0m") - # When heartbeat returns - def heartbeat (self): - if not self.beat: - print("\033[1;31;44mHeartbeat Returned\033[0;0m") - self.beat = True + def heartbeat(self): + """ + Called when a heartbeat message is received + """ + if self.hb_detected == False: + #if heartbeat was lost previously but is found again + print("\033[1;31;44mHeartbeat Returned\033[0;0m") + self.hb_detected = True + self.timeout_count = 0 + +def main(): + rclpy.init() + hb_subscriber = HeartbeatSubscriber() + + rclpy.spin(hb_subscriber) -# The main function when run if __name__ == "__main__": - sub = HeartbeatSub() + main() From b547a31ccad760bbd6c04e5d1b17ae89572c811b Mon Sep 17 00:00:00 2001 From: Emily Date: Thu, 2 Sep 2021 02:55:49 -0700 Subject: [PATCH 3/3] bug fix --- tutorials/heartbeat_subscriber.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tutorials/heartbeat_subscriber.py b/tutorials/heartbeat_subscriber.py index 75de9bb..a405c68 100755 --- a/tutorials/heartbeat_subscriber.py +++ b/tutorials/heartbeat_subscriber.py @@ -64,7 +64,7 @@ def no_heartbeat(self): Called when the timeout has been exceeded without receiving any heartbeat messages """ self.hb_detected = False - print("\033[1;31;48mNo Heartbeart Detected\033[0;0m") + print("\033[1;31;48mBoop Brrtz :(\033[0;0m") def heartbeat(self): """ @@ -72,7 +72,7 @@ def heartbeat(self): """ if self.hb_detected == False: #if heartbeat was lost previously but is found again - print("\033[1;31;44mHeartbeat Returned\033[0;0m") + print("\033[1;31;44mBeep Boop\033[0;0m") self.hb_detected = True self.timeout_count = 0