From 56de7014813eb388f67393fc036e224379c7f18b Mon Sep 17 00:00:00 2001 From: Cn-2000 Date: Mon, 25 Feb 2019 16:00:03 -0500 Subject: [PATCH 1/6] GUI monitoring 4 data real-time --- .../src/robobuggy/scripts/visualization.py | 58 +++++++++++++++++++ 1 file changed, 58 insertions(+) create mode 100755 Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/visualization.py diff --git a/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/visualization.py b/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/visualization.py new file mode 100755 index 00000000..f7a6510b --- /dev/null +++ b/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/visualization.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python +import rospy +from robobuggy.msg import Command +from robobuggy.msg import Diagnostics +from robobuggy.msg import Feedback +from robobuggy.msg import Pose +from Tkinter import * + +# Get brake state, commanded steering, feedback steering, current orientation, +# battery level and visualize them on a user interfate + +def commandUpdate(data): + steer_cmd_rad = "%10.4d" % data.steer_cmd_rad + canvas.itemconfigure(t1, text=steer_cmd_rad) + +def diagnosticsUpdate(data): + battery_level = "%d" % data.battery_level + canvas.itemconfigure(t2, text=battery_level) + +def feedbackUpdate(data): + if data.brake_state==True: + canvas.itemconfigure(t3, text="Deployed") + else: + canvas.itemconfigure(t3, text="Suspended") + +def poseUpdate(data): + heading_rad = "%10.4f" % data.heading_rad + canvas.itemconfigure(t4, text=heading_rad) + +def listener(): + + rospy.init_node('listener', anonymous=True) + + rospy.Subscriber("Command", Command, commandUpdate) + rospy.Subscriber("Diagnostics", Diagnostics, diagnosticsUpdate) + rospy.Subscriber("Feedback", Feedback, feedbackUpdate) + rospy.Subscriber("Pose", Pose, poseUpdate) + +if __name__ == '__main__': + + # Initialize GUI + root = Tk() + root.title("Robobuggy Status") + root.resizable(width=False, height=False) + canvas = Canvas(root, width=500, height=300) + canvas.configure(bd=0, highlightthickness=0) + canvas.pack() + canvas.create_text(20, 45, text="Commanded Steering:", anchor="w", fill="black", font="Times 20 bold") + canvas.create_text(20, 115, text="Battery Level:", anchor="w", fill="black", font="Times 20 bold") + canvas.create_text(20, 185, text="Brake State:", anchor="w", fill="black", font="Times 20 bold") + canvas.create_text(20, 255, text="Current Orientation:", anchor="w", fill="black", font="Times 20 bold") + t1 = canvas.create_text(250, 45, text="Fetching data...", anchor="w", fill="black", font="Times 16") + t2 = canvas.create_text(250, 115, text="Fetching data...", anchor="w", fill="black", font="Times 16") + t3 = canvas.create_text(250, 185, text="Fetching data...", anchor="w", fill="black", font="Times 16") + t4 = canvas.create_text(250, 255, text="Fetching data...", anchor="w", fill="black", font="Times 16") + + listener() + root.mainloop() From 46ed0b170a33cfe7601f504f8e665e76ffb8f11d Mon Sep 17 00:00:00 2001 From: Cn-2000 Date: Fri, 1 Mar 2019 18:25:32 -0500 Subject: [PATCH 2/6] small improvements --- .../src/robobuggy/scripts/visualization.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/visualization.py b/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/visualization.py index f7a6510b..d9256ff4 100755 --- a/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/visualization.py +++ b/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/visualization.py @@ -10,11 +10,17 @@ # battery level and visualize them on a user interfate def commandUpdate(data): - steer_cmd_rad = "%10.4d" % data.steer_cmd_rad + if data.steer_cmd_rad>0: + steer_cmd_rad = "%10.4f rad left" % data.steer_cmd_rad + elif data.steer_cmd_rad=0: + steer_cmd_rad = "Straight" + else: + steer_cmd_rad = "%10.4f rad right" % (data.steer_cmd_rad * -1) canvas.itemconfigure(t1, text=steer_cmd_rad) def diagnosticsUpdate(data): - battery_level = "%d" % data.battery_level + voltage = data.battery_level / 1000.00 + battery_level = "%10.3f V" % voltage canvas.itemconfigure(t2, text=battery_level) def feedbackUpdate(data): From 7ca712c1bd53e05442b7da02842915607325b28e Mon Sep 17 00:00:00 2001 From: Cn-2000 Date: Fri, 1 Mar 2019 18:49:43 -0500 Subject: [PATCH 3/6] bug fixes --- .../src/robobuggy/scripts/visualization.py | 27 +++++++++++++++---- 1 file changed, 22 insertions(+), 5 deletions(-) diff --git a/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/visualization.py b/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/visualization.py index d9256ff4..a1354a78 100755 --- a/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/visualization.py +++ b/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/visualization.py @@ -5,15 +5,16 @@ from robobuggy.msg import Feedback from robobuggy.msg import Pose from Tkinter import * +import math # Get brake state, commanded steering, feedback steering, current orientation, # battery level and visualize them on a user interfate def commandUpdate(data): - if data.steer_cmd_rad>0: - steer_cmd_rad = "%10.4f rad left" % data.steer_cmd_rad - elif data.steer_cmd_rad=0: + if abs(data.steer_cmd_rad) < 0.0001: steer_cmd_rad = "Straight" + elif data.steer_cmd_rad>0: + steer_cmd_rad = "%10.4f rad left" % data.steer_cmd_rad else: steer_cmd_rad = "%10.4f rad right" % (data.steer_cmd_rad * -1) canvas.itemconfigure(t1, text=steer_cmd_rad) @@ -30,7 +31,23 @@ def feedbackUpdate(data): canvas.itemconfigure(t3, text="Suspended") def poseUpdate(data): - heading_rad = "%10.4f" % data.heading_rad + heading_rad = "%10.4f heading " % data.heading_rad + if abs(data.heading_rad) < 0.0001: + heading_rad += "E" + elif abs(data.heading_rad - math.pi / 2) < 0.0001: + heading_rad += "N" + elif abs(data.heading_rad + math.pi / 2) < 0.0001: + heading_rad += "S" + elif abs(abs(data.heading_rad) - math.pi) < 0.0001: + heading_rad += "W" + elif 0 < data.heading_rad and data.heading_rad < math.pi / 2: + heading_rad += "NE" + elif math.pi / 2 < data.heading_rad and data.heading_rad < math.pi: + heading_rad += "NW" + elif -math.pi / 2 < data.heading_rad and data.heading_rad < 0: + heading_rad += "SE" + else: + heading_rad += "SW" canvas.itemconfigure(t4, text=heading_rad) def listener(): @@ -47,7 +64,7 @@ def listener(): # Initialize GUI root = Tk() root.title("Robobuggy Status") - root.resizable(width=False, height=False) + root.resizable(width=True, height=True) canvas = Canvas(root, width=500, height=300) canvas.configure(bd=0, highlightthickness=0) canvas.pack() From 34b4a1a26f60913d3d1b355482cff566c07012a5 Mon Sep 17 00:00:00 2001 From: Cn-2000 Date: Fri, 1 Mar 2019 18:52:33 -0500 Subject: [PATCH 4/6] UI changes --- .../ROS_RoboBuggy/src/robobuggy/scripts/visualization.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/visualization.py b/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/visualization.py index a1354a78..9404f7bb 100755 --- a/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/visualization.py +++ b/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/visualization.py @@ -72,10 +72,10 @@ def listener(): canvas.create_text(20, 115, text="Battery Level:", anchor="w", fill="black", font="Times 20 bold") canvas.create_text(20, 185, text="Brake State:", anchor="w", fill="black", font="Times 20 bold") canvas.create_text(20, 255, text="Current Orientation:", anchor="w", fill="black", font="Times 20 bold") - t1 = canvas.create_text(250, 45, text="Fetching data...", anchor="w", fill="black", font="Times 16") - t2 = canvas.create_text(250, 115, text="Fetching data...", anchor="w", fill="black", font="Times 16") - t3 = canvas.create_text(250, 185, text="Fetching data...", anchor="w", fill="black", font="Times 16") - t4 = canvas.create_text(250, 255, text="Fetching data...", anchor="w", fill="black", font="Times 16") + t1 = canvas.create_text(480, 45, text="Fetching data...", anchor="e", fill="black", font="Times 16") + t2 = canvas.create_text(480, 115, text="Fetching data...", anchor="e", fill="black", font="Times 16") + t3 = canvas.create_text(480, 185, text="Fetching data...", anchor="e", fill="black", font="Times 16") + t4 = canvas.create_text(480, 255, text="Fetching data...", anchor="e", fill="black", font="Times 16") listener() root.mainloop() From 186fef3de807e955dea2755cb672e2905bd03757 Mon Sep 17 00:00:00 2001 From: Cn-2000 Date: Sat, 2 Mar 2019 14:58:01 -0500 Subject: [PATCH 5/6] changed radians to degrees; GUI modifications --- .../src/robobuggy/scripts/visualization.py | 92 +++++++++++-------- 1 file changed, 54 insertions(+), 38 deletions(-) diff --git a/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/visualization.py b/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/visualization.py index 9404f7bb..25fa7d41 100755 --- a/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/visualization.py +++ b/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/visualization.py @@ -11,44 +11,46 @@ # battery level and visualize them on a user interfate def commandUpdate(data): - if abs(data.steer_cmd_rad) < 0.0001: - steer_cmd_rad = "Straight" - elif data.steer_cmd_rad>0: - steer_cmd_rad = "%10.4f rad left" % data.steer_cmd_rad + steer_cmd_deg = data.steer_cmd_rad * 180 / math.pi + if abs(steer_cmd_deg) < 0.1: + steer_cmd = "Straight" + elif steer_cmd_deg > 0: + steer_cmd = "%10.1f deg left" % steer_cmd_deg else: - steer_cmd_rad = "%10.4f rad right" % (data.steer_cmd_rad * -1) - canvas.itemconfigure(t1, text=steer_cmd_rad) + steer_cmd = "%10.1f deg right" % (steer_cmd_deg * -1) + t0.set(steer_cmd + " ") def diagnosticsUpdate(data): voltage = data.battery_level / 1000.00 battery_level = "%10.3f V" % voltage - canvas.itemconfigure(t2, text=battery_level) + t1.set(battery_level + " ") def feedbackUpdate(data): if data.brake_state==True: - canvas.itemconfigure(t3, text="Deployed") + t2.set("Deployed ") else: - canvas.itemconfigure(t3, text="Suspended") + t2.set("Suspended ") def poseUpdate(data): - heading_rad = "%10.4f heading " % data.heading_rad - if abs(data.heading_rad) < 0.0001: - heading_rad += "E" - elif abs(data.heading_rad - math.pi / 2) < 0.0001: - heading_rad += "N" - elif abs(data.heading_rad + math.pi / 2) < 0.0001: - heading_rad += "S" - elif abs(abs(data.heading_rad) - math.pi) < 0.0001: - heading_rad += "W" - elif 0 < data.heading_rad and data.heading_rad < math.pi / 2: - heading_rad += "NE" - elif math.pi / 2 < data.heading_rad and data.heading_rad < math.pi: - heading_rad += "NW" - elif -math.pi / 2 < data.heading_rad and data.heading_rad < 0: - heading_rad += "SE" + heading_deg = data.heading_rad * 180 / math.pi + heading = "%10.1f deg heading " % heading_deg + if abs(heading_deg) < 0.1: + heading += "E" + elif abs(heading_deg - 90) < 0.1: + heading += "N" + elif abs(heading_deg + 90) < 0.1: + heading += "S" + elif abs(abs(heading_deg) - 180) < 0.1: + heading += "W" + elif 0 < heading_deg and heading_deg < 90: + heading += "NE" + elif 90 < heading_deg and heading_deg < 180: + heading += "NW" + elif -90 < heading_deg and heading_deg < 0: + heading += "SE" else: - heading_rad += "SW" - canvas.itemconfigure(t4, text=heading_rad) + heading += "SW" + t3.set(heading + " ") def listener(): @@ -61,21 +63,35 @@ def listener(): if __name__ == '__main__': - # Initialize GUI root = Tk() root.title("Robobuggy Status") root.resizable(width=True, height=True) - canvas = Canvas(root, width=500, height=300) - canvas.configure(bd=0, highlightthickness=0) - canvas.pack() - canvas.create_text(20, 45, text="Commanded Steering:", anchor="w", fill="black", font="Times 20 bold") - canvas.create_text(20, 115, text="Battery Level:", anchor="w", fill="black", font="Times 20 bold") - canvas.create_text(20, 185, text="Brake State:", anchor="w", fill="black", font="Times 20 bold") - canvas.create_text(20, 255, text="Current Orientation:", anchor="w", fill="black", font="Times 20 bold") - t1 = canvas.create_text(480, 45, text="Fetching data...", anchor="e", fill="black", font="Times 16") - t2 = canvas.create_text(480, 115, text="Fetching data...", anchor="e", fill="black", font="Times 16") - t3 = canvas.create_text(480, 185, text="Fetching data...", anchor="e", fill="black", font="Times 16") - t4 = canvas.create_text(480, 255, text="Fetching data...", anchor="e", fill="black", font="Times 16") + + l0 = Label(root, text = " Commanded Steering: ", font = ("Times", 20, "bold")).grid(row = 0, column = 0, sticky = W) + l1 = Label(root, text = " Battery Level: ", font = ("Times", 20, "bold")).grid(row = 1, column = 0, sticky = W) + l2 = Label(root, text = " Brake State: ", font = ("Times", 20, "bold")).grid(row = 2, column = 0, sticky = W) + l3 = Label(root, text = " Current Orientation: ", font = ("Times", 20, "bold")).grid(row =3, column = 0, sticky = W) + + t0 = StringVar() + t1 = StringVar() + t2 = StringVar() + t3 = StringVar() + t0.set("Fetching data... ") + t1.set("Fetching data... ") + t2.set("Fetching data... ") + t3.set("Fetching data... ") + + w0 = Label(root, textvariable = t0, font = ("Times", 16)).grid(row = 0, column = 1, sticky = E) + w1 = Label(root, textvariable = t1, font = ("Times", 16)).grid(row = 1, column = 1, sticky = E) + w2 = Label(root, textvariable = t2, font = ("Times", 16)).grid(row = 2, column = 1, sticky = E) + w3 = Label(root, textvariable = t3, font = ("Times", 16)).grid(row = 3, column = 1, sticky = E) + + root.grid_columnconfigure(0, weight = 1) + root.grid_columnconfigure(1, weight = 1) + root.grid_rowconfigure(0, weight = 1) + root.grid_rowconfigure(1, weight = 1) + root.grid_rowconfigure(2, weight = 1) + root.grid_rowconfigure(3, weight = 1) listener() root.mainloop() From a827dc29df23c69ced9886633a595f91152ba621 Mon Sep 17 00:00:00 2001 From: Cn-2000 Date: Fri, 29 Mar 2019 18:17:18 -0400 Subject: [PATCH 6/6] minor changes --- .../ROS_RoboBuggy/src/robobuggy/scripts/visualization.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/visualization.py b/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/visualization.py index 25fa7d41..060a92e0 100755 --- a/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/visualization.py +++ b/Software/real_time/ROS_RoboBuggy/src/robobuggy/scripts/visualization.py @@ -11,7 +11,7 @@ # battery level and visualize them on a user interfate def commandUpdate(data): - steer_cmd_deg = data.steer_cmd_rad * 180 / math.pi + steer_cmd_deg = math.degrees(data.steer_cmd_rad) if abs(steer_cmd_deg) < 0.1: steer_cmd = "Straight" elif steer_cmd_deg > 0: @@ -32,7 +32,7 @@ def feedbackUpdate(data): t2.set("Suspended ") def poseUpdate(data): - heading_deg = data.heading_rad * 180 / math.pi + heading_deg = math.degrees(data.heading_rad) heading = "%10.1f deg heading " % heading_deg if abs(heading_deg) < 0.1: heading += "E"