Merge pull request #201 from CMU-cabot/f-miyazaki/update-speed-debug

add speed graph associated with the separation of people speed limit

cabot_debug/src/check_speed_control.py

@@ -28,6 +28,7 @@
 from cabot_common.rosbag2 import BagReader
 import tkinter as tk
 from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg
+import subprocess
 
 parser = OptionParser(usage="""
 Example
@@ -58,10 +59,16 @@
     "/cabot/people_speed",
     "/cabot/tf_speed",
     "/cabot/map_speed",
-    "/cabot/user_speed",
     "/cmd_vel",
     "/cabot/activity_log",
     "/current_floor",
+    "/cabot/social_distance_speed",
+    "/cabot/pure_velocity_obstacle_speed",
+    "/cabot/combined_speed",
+    "/cabot/capacitive/touch",
+    "/cabot/capacitive/touch_raw",
+    "/cabot/tof/touch",
+    "/cabot/tof/touch_raw",
 ])
 reader.set_filter_by_options(options)  # filter by start and duration
 
@@ -76,6 +83,54 @@ def getIndex(name, increment=0):
         index += increment
     return indexes[name]
 
+def get_user_speed(start, duration, end_time):
+    process = subprocess.Popen(
+        ["ros2", "run", "cabot_debug", "print_topics.py", "-f", "ros2_topics", "-t", "/cabot/user_speed"],
+        stdout=subprocess.PIPE,
+        stderr=subprocess.PIPE,
+        text=True
+    )
+
+    st = []
+    data = []
+
+    for line in process.stdout:
+        line = line.strip()
+        st.append(float(line.split()[-2].split("(")[1].rstrip("):")))
+        data.append(float(line.split()[-1]))
+
+    process_st = []
+    process_data = []
+
+    tmp_st = start
+    tmp_data = 1.0
+
+    for a,b in zip(st, data):
+        if start > a:
+            tmp_data = b
+            continue
+
+        process_st.append(tmp_st)
+        process_data.append(tmp_data)
+
+        if start + duration < a:
+            break
+
+        tmp_st = a
+        process_st.append(tmp_st)
+        process_data.append(tmp_data)
+        tmp_data = b
+
+    process_st.append(tmp_st)
+    process_data.append(tmp_data)
+
+    last_time = min(float(start + duration), float(end_time))
+
+    process_st.append(last_time)
+    process_data.append(tmp_data)
+
+    return process_st, process_data
+
 while reader.has_next():
     (topic, msg, t, st) = reader.serialize_next()
     if not topic:
@@ -95,15 +150,20 @@ def getIndex(name, increment=0):
             "/cabot/people_speed",
             "/cabot/tf_speed",
             "/cabot/map_speed",
-            "/cabot/user_speed",
-            "/current_floor"]:
+            "/current_floor",
+            "/cabot/social_distance_speed",
+            "/cabot/pure_velocity_obstacle_speed",
+            "/cabot/combined_speed",
+            "/cabot/capacitive/touch",
+            "/cabot/capacitive/touch_raw",
+            "/cabot/tof/touch",
+            "/cabot/tof/touch_raw"]:
         i = getIndex(topic, 2)
         data[i].append([st, t])
         data[i+1].append(msg.data)
     elif topic in [
             "/cabot/activity_log"]:
         i = getIndex(topic, 2)
-        # print(msg.text)
         if msg.text in [
                 "navigation;event;navigation_start",
                 "navigation;event;elevator_door_may_be_ready"]:
@@ -114,6 +174,12 @@ def getIndex(name, increment=0):
                 "goal_completed"]:
             data[i].append(st)
             data[i+1].append(0)
+
+us_st, us_data = get_user_speed(options.start, options.duration, reader.bag_duration())
+i = getIndex("/cabot/user_speed", 2)
+data[i].extend(us_st)
+data[i+1].extend(us_data)
+
 
 # Create a Tkinter window
 root = tk.Tk()
@@ -134,7 +200,7 @@ def getIndex(name, increment=0):
 # Create a Matlotlib figure
 fig, ax1 = plt.subplots(figsize=(20, 10))
 line1, = ax1.plot([], [], 'red', linestyle='-', label='/cabot/cmd_vel.l')
-line2, = ax1.plot([], [], 'blue', linestyle='--', label='/cabot/touch')
+line2, = ax1.plot([], [], 'blue', linestyle='-', label='/cabot/touch')
 line3, = ax1.plot([], [], 'green', linestyle=':', label='/cabot/lidar_speed')
 line4, = ax1.plot([], [], 'orange', linestyle=':', label='/cabot/people_speed')
 line6, = ax1.plot([], [], 'brown', linestyle='-', label='/cabot/tf_speed')
@@ -143,11 +209,26 @@ def getIndex(name, increment=0):
 line9, = ax1.plot([], [], 'yellow', linestyle='-', label='/cmd_vel.l')
 line10, = ax1.plot([], [], 'teal', linestyle='-', label='/cabot/cmd_vel.r')
 line11, = ax1.plot([], [], 'magenta', linestyle='-', label='/cmd_vel.r')
-ax1.legend(bbox_to_anchor=(1.00, 1), loc='upper left')
+line12, = ax1.plot([], [], 'purple', linestyle='--', label='/cabot/social_distance_speed')
+line13, = ax1.plot([], [], 'lime', linestyle=':', label='/cabot/pure_velocity_obstacle_speed')
+line14, = ax1.plot([], [], 'gray', linestyle=':', label='/cabot/combined_speed')
+line15, = ax1.plot([], [], 'black', linestyle='-', label='/cabot/capacitive/touch')
+line17, = ax1.plot([], [], 'gold', linestyle='-', label='/cabot/tof/touch')
 
 ax2 = ax1.twinx()
-line5, = ax2.plot([], [], 'purple', linestyle='-', label='/cabot/touch_raw')
-ax2.legend(bbox_to_anchor=(1.00, 1), loc='center left')
+line5, = ax2.plot([], [], 'navy', linestyle='--', label='/cabot/touch_raw')
+line16, = ax2.plot([], [], 'navy', linestyle='--', label='/cabot/capacitive/touch_raw')
+ax2.tick_params(axis='y', colors='navy')
+
+ax3 = ax1.twinx()
+ax3.spines["right"].set_position(("axes", 1.03))
+line18, = ax3.plot([], [], 'maroon', linestyle='--', label='/cabot/tof/touch_raw')
+ax3.tick_params(axis='y', colors='maroon')
+
+lines1, labels1 = ax1.get_legend_handles_labels()
+lines2, labels2 = ax2.get_legend_handles_labels()
+lines3, labels3 = ax3.get_legend_handles_labels()
+ax1.legend(lines1 + lines2 + lines3, labels1 + labels2 + labels3, loc="upper right", bbox_to_anchor=(-0.02, 1.0))
 
 # Initially set to invisible
 line5.set_visible(False)
@@ -157,6 +238,13 @@ def getIndex(name, increment=0):
 line9.set_visible(False)
 line10.set_visible(False)
 line11.set_visible(False)
+line12.set_visible(False)
+line13.set_visible(False)
+line14.set_visible(False)
+line15.set_visible(False)
+line16.set_visible(False)
+line17.set_visible(False)
+line18.set_visible(False)
 
 # Embed the Matplotlib figure into the Tkinter window using FigureCanvasTkAgg
 canvas = FigureCanvasTkAgg(fig, master=root)
@@ -179,54 +267,54 @@ def toggle_category(var, checkboxes):
 
 # Plot data function
 def plot_data():
-    topic_index = []
-    topic_index.extend([
-        getIndex("/cabot/cmd_vel"),
-        getIndex("/cabot/touch"),
-        getIndex("/cabot/lidar_speed"),
-        getIndex("/cabot/people_speed"),
-        getIndex("/cabot/touch_raw"),
-        getIndex("/cabot/tf_speed"),
-        getIndex("/cabot/map_speed"),
-        getIndex("/cabot/user_speed"),
-        getIndex("/cmd_vel")
-    ])
-    line1.set_data([d[0] for d in data[topic_index[0]]], data[topic_index[0]+1])
-    line2.set_data([d[0] for d in data[topic_index[1]]], data[topic_index[1]+1])
-    line3.set_data([d[0] for d in data[topic_index[2]]], data[topic_index[2]+1])
-    line4.set_data([d[0] for d in data[topic_index[3]]], data[topic_index[3]+1])
-    line5.set_data([d[0] for d in data[topic_index[4]]], data[topic_index[4]+1])
-    line6.set_data([d[0] for d in data[topic_index[5]]], data[topic_index[5]+1])
-    line7.set_data([d[0] for d in data[topic_index[6]]], data[topic_index[6]+1])
-    line8.set_data([d[0] for d in data[topic_index[7]]], data[topic_index[7]+1])
-    line9.set_data([d[0] for d in data[topic_index[8]]], data[topic_index[8]+1])
-    line10.set_data([d[0] for d in data[topic_index[0]]], data[topic_index[0]+2])
-    line11.set_data([d[0] for d in data[topic_index[8]]], data[topic_index[8]+2])
+    line1.set_data([d[0] for d in data[getIndex("/cabot/cmd_vel")]], data[getIndex("/cabot/cmd_vel")+1])
+    line2.set_data([d[0] for d in data[getIndex("/cabot/touch")]], data[getIndex("/cabot/touch")+1])
+    line3.set_data([d[0] for d in data[getIndex("/cabot/lidar_speed")]], data[getIndex("/cabot/lidar_speed")+1])
+    line4.set_data([d[0] for d in data[getIndex("/cabot/people_speed")]], data[getIndex("/cabot/people_speed")+1])
+    line5.set_data([d[0] for d in data[getIndex("/cabot/touch_raw")]], data[getIndex("/cabot/touch_raw")+1])
+    line6.set_data([d[0] for d in data[getIndex("/cabot/tf_speed")]], data[getIndex("/cabot/tf_speed")+1])
+    line7.set_data([d[0] for d in data[getIndex("/cabot/map_speed")]], data[getIndex("/cabot/map_speed")+1])
+    line8.set_data([data[getIndex("/cabot/user_speed")]], data[getIndex("/cabot/user_speed")+1])
+    line9.set_data([d[0] for d in data[getIndex("/cmd_vel")]], data[getIndex("/cmd_vel")+1])
+    line10.set_data([d[0] for d in data[getIndex("/cabot/cmd_vel")]], data[getIndex("/cabot/cmd_vel")+2])
+    line11.set_data([d[0] for d in data[getIndex("/cmd_vel")]], data[getIndex("/cmd_vel")+2])
+    line12.set_data([d[0] for d in data[getIndex("/cabot/social_distance_speed")]], data[getIndex("/cabot/social_distance_speed")+1])
+    line13.set_data([d[0] for d in data[getIndex("/cabot/pure_velocity_obstacle_speed")]], data[getIndex("/cabot/pure_velocity_obstacle_speed")+1])
+    line14.set_data([d[0] for d in data[getIndex("/cabot/combined_speed")]], data[getIndex("/cabot/combined_speed")+1])
+    line15.set_data([d[0] for d in data[getIndex("/cabot/capacitive/touch")]], data[getIndex("/cabot/capacitive/touch")+1])
+    line16.set_data([d[0] for d in data[getIndex("/cabot/capacitive/touch_raw")]], data[getIndex("/cabot/capacitive/touch_raw")+1])
+    line17.set_data([d[0] for d in data[getIndex("/cabot/tof/touch")]], data[getIndex("/cabot/tof/touch")+1])
+    line18.set_data([d[0] for d in data[getIndex("/cabot/tof/touch_raw")]], data[getIndex("/cabot/tof/touch_raw")+1])
     ax1.relim()
     ax1.autoscale_view()
     ax2.relim()
     ax2.autoscale_view()
+    ax3.relim()
+    ax3.autoscale_view()
 
     current_ticks = ax1.get_xticks()
 
     custom_labels = []
     for tick in current_ticks:
-        closest_st = min([d[0] for d in data[topic_index[0]]], key=lambda x: abs(x - tick))
-        t_value = next(d[1] for d in data[topic_index[0]] if d[0] == closest_st)
+        closest_st = min([d[0] for d in data[getIndex("/cabot/cmd_vel")]], key=lambda x: abs(x - tick))
+        t_value = next(d[1] for d in data[getIndex("/cabot/cmd_vel")] if d[0] == closest_st)
         custom_labels.append(f'{int(tick)}\nt={t_value:.2f}\nst=({closest_st:.2f})')
 
     ax1.set_xticks(current_ticks)
     ax1.set_xticklabels(custom_labels, ha='center')
 
     y1_min, y1_max = ax1.get_ylim()
     y2_min, y2_max = ax2.get_ylim()
+    y3_min, y3_max = ax3.get_ylim()
     if y1_max > 2:
         y1_min = y1_min/(y1_max/2)
         ax1.set_ylim(bottom=y1_min)
         y1_max = 2
         ax1.set_ylim(top=y1_max)
-    min_lim = y1_min*(y2_max/y1_max)
-    ax2.set_ylim(bottom=min_lim)
+    y2_min_lim = y1_min*(y2_max/y1_max)
+    ax2.set_ylim(bottom=y2_min_lim)
+    y3_min_lim = y1_min*(y3_max/y1_max)
+    ax3.set_ylim(bottom=y3_min_lim)
 
     canvas.draw()
 
@@ -299,7 +387,14 @@ def toggle_vertical_lines(var):
 var9 = tk.BooleanVar(value=False)
 var10 = tk.BooleanVar(value=False)
 var11 = tk.BooleanVar(value=False)
-var12 = tk.BooleanVar(value=True)
+var12 = tk.BooleanVar(value=False)
+var13 = tk.BooleanVar(value=False)
+var14 = tk.BooleanVar(value=False)
+var15 = tk.BooleanVar(value=False)
+var16 = tk.BooleanVar(value=False)
+var17 = tk.BooleanVar(value=False)
+var18 = tk.BooleanVar(value=False)
+var30 = tk.BooleanVar(value=True)
 checkbox1 = tk.Checkbutton(cmd_vel_frame, text="Show /cabot/cmd_vel.l", variable=var1, command=lambda: toggle_line(line1, var1, ax1))
 checkbox2 = tk.Checkbutton(touch_frame, text="Show /cabot/touch", variable=var2, command=lambda: toggle_line(line2, var2, ax1))
 checkbox3 = tk.Checkbutton(speed_frame, text="Show /cabot/lidar_speed", variable=var3, command=lambda: toggle_line(line3, var3, ax1))
@@ -311,16 +406,23 @@ def toggle_vertical_lines(var):
 checkbox9 = tk.Checkbutton(cmd_vel_frame, text="Show /cmd_vel.l", variable=var9, command=lambda: toggle_line(line9, var9, ax1))
 checkbox10 = tk.Checkbutton(cmd_vel_frame, text="Show /cabot/cmd_vel.r", variable=var10, command=lambda: toggle_line(line10, var10, ax1))
 checkbox11 = tk.Checkbutton(cmd_vel_frame, text="Show /cmd_vel.r", variable=var11, command=lambda: toggle_line(line11, var11, ax1))
-checkbox12 = tk.Checkbutton(frame, text=f"Show /current_floor", variable=var12, command=lambda: toggle_vertical_lines(var12))
+checkbox12 = tk.Checkbutton(speed_frame, text="Show /cabot/social_distance_speed", variable=var12, command=lambda: toggle_line(line12, var12, ax1))
+checkbox13 = tk.Checkbutton(speed_frame, text="Show /cabot/pure_velocity_obstacle_speed", variable=var13, command=lambda: toggle_line(line13, var13, ax1))
+checkbox14 = tk.Checkbutton(speed_frame, text="Show /cabot/combined_speed", variable=var14, command=lambda: toggle_line(line14, var14, ax1))
+checkbox15 = tk.Checkbutton(touch_frame, text="Show /cabot/capacitive/touch", variable=var15, command=lambda: toggle_line(line15, var15, ax1))
+checkbox16 = tk.Checkbutton(touch_frame, text="Show /cabot/capacitive/touch_raw", variable=var16, command=lambda: toggle_line(line16, var16, ax2))
+checkbox17 = tk.Checkbutton(touch_frame, text="Show /cabot/tof/touch", variable=var17, command=lambda: toggle_line(line17, var17, ax1))
+checkbox18 = tk.Checkbutton(touch_frame, text="Show /cabot/tof/touch_raw", variable=var18, command=lambda: toggle_line(line18, var18, ax3))
+checkbox30 = tk.Checkbutton(frame, text=f"Show /current_floor", variable=var30, command=lambda: toggle_vertical_lines(var30))
 
 # Create category checkboxes
 cmd_vel_var = tk.BooleanVar(value=True)
 speed_var = tk.BooleanVar(value=True)
 touch_var = tk.BooleanVar(value=True)
 
 cmd_vel_checkbox = tk.Checkbutton(cmd_vel_frame, text="all", variable=cmd_vel_var, command=lambda: toggle_category(cmd_vel_var, [(var1, line1), (var9, line9), (var10, line10), (var11, line11)]))
-speed_checkbox = tk.Checkbutton(speed_frame, text="all", variable=speed_var, command=lambda: toggle_category(speed_var, [(var3, line3), (var4, line4), (var6, line6), (var7, line7), (var8, line8)]))
-touch_checkbox = tk.Checkbutton(touch_frame, text="all", variable=touch_var, command=lambda: toggle_category(touch_var, [(var2, line2), (var5, line5)]))
+speed_checkbox = tk.Checkbutton(speed_frame, text="all", variable=speed_var, command=lambda: toggle_category(speed_var, [(var3, line3), (var4, line4), (var6, line6), (var7, line7), (var8, line8), (var12, line12), (var13, line13), (var14, line14)]))
+touch_checkbox = tk.Checkbutton(touch_frame, text="all", variable=touch_var, command=lambda: toggle_category(touch_var, [(var2, line2), (var5, line5), (var15, line15), (var16, line16), (var17, line17), (var18, line18)]))
 
 # Arrange checkboxes in the frame
 cmd_vel_checkbox.pack(side=tk.TOP, anchor='w')
@@ -334,19 +436,26 @@ def toggle_vertical_lines(var):
 checkbox6.pack(side=tk.TOP, anchor='w')
 checkbox7.pack(side=tk.TOP, anchor='w')
 checkbox8.pack(side=tk.TOP, anchor='w')
+checkbox12.pack(side=tk.TOP, anchor='w')
+checkbox13.pack(side=tk.TOP, anchor='w')
+checkbox14.pack(side=tk.TOP, anchor='w')
 touch_checkbox.pack(side=tk.TOP, anchor='w')
 checkbox2.pack(side=tk.TOP, anchor='w')
 checkbox5.pack(side=tk.TOP, anchor='w')
+checkbox15.pack(side=tk.TOP, anchor='w')
+checkbox16.pack(side=tk.TOP, anchor='w')
+checkbox17.pack(side=tk.TOP, anchor='w')
+checkbox18.pack(side=tk.TOP, anchor='w')
 
-checkbox12.pack(side=tk.TOP, anchor='w')
+checkbox30.pack(side=tk.TOP, anchor='w')
 
 # Plot data
 plot_data()
 
 highlight_navigation_time(ax2, data, getIndex("/cabot/activity_log"), color=options.background_color, alpha=options.background_alpha)
 
 # Ensure vertical lines are displayed based on the initial checkbox state
-toggle_vertical_lines(var12)
+toggle_vertical_lines(var30)
 
 # Start the Tkinter main loop
 root.mainloop()