WIP: Publish wheel effort

docs/driver_callbacks.md

@@ -0,0 +1,39 @@
+# Executors and Callback Groups in Stretch's ROS2 Driver
+
+When writing a ROS2 program, you can control execution flow by using executors and callback groups. In a ROS2 node with lots of callbacks, such as Stretch's ROS2 driver, it's important to carefully select executors and callback groups to ensure callbacks are running in the most efficient way possible, and that no deadlocks are possible. In this document, we capture how the driver's execution and callback groups are currently set up, and provide insight into the design decision behind why it's set up this way.
+
+## Background
+
+We'll need to know about:
+
+ 1. ROS2 callbacks, executors, and callback groups
+ 2. The driver's callbacks 
+
+### ROS2 Callbacks, Executors, and Callback Groups
+
+Callbacks are functions that get called when an event happens. For example, when subscribing to a ROS2 topic, we write a callback function that gets called when the topic gets a message. There's a few types of callbacks:
+
+ - subscription callbacks (for handling the message received from a topic)
+ - service callbacks (for when our ROS2 service receives a request)
+ - timer callbacks (called at a regular rate)
+ - action server callbacks (for starting/cancelling/etc. a "goal" send by a client)
+ - action client callbacks (for results from the action server for our starting/cancelling/etc. the goal)
+ - futures (for other times the client has to wait for a result)
+
+Rclpy offers two different executors:
+
+ - SingleThreadedExecutor (default)
+ - MultiThreadedExecutor
+
+The single threaded executor takes every callback in the program, and executes it serially in a single thread. For the driver, which has a lot of callbacks, this type of executor will cause lag if the driver receives a lot of requests. The multi-threaded executor uses multiple threads to execute callbacks in parallel.
+
+Rclpy offers two different kinds of callback groups:
+
+ - MutuallyExclusiveCallbackGroup (default)
+ - ReentrantCallbackGroup
+
+TODO
+
+### The Driver's Callbacks
+
+TODO

stretch_core/stretch_core/effort_plotter.py

@@ -0,0 +1,37 @@
+# Required Hack whenever importing cv2 (stretch_body does it implicitly). Details: https://forum.hello-robot.com/t/1047
+import os; os.environ['QT_QPA_PLATFORM_PLUGIN_PATH'] = '/usr/lib/x86_64-linux-gnu/qt5/plugins/platforms/libqxcb.so'
+from multiprocessing_plotter import NBPlot
+
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+
+class PlotterNode(Node):
+    def __init__(self):
+        super().__init__('plotter_node')
+        self.node_name = self.get_name()
+        self.get_logger().info("{0} started".format(self.node_name))
+
+        self.topic_name = '/wheel_effort'
+        qos_policy = rclpy.qos.QoSProfile(reliability=rclpy.qos.ReliabilityPolicy.BEST_EFFORT,
+                                          history=rclpy.qos.HistoryPolicy.KEEP_LAST,
+                                          depth=1)
+        self.accel_subscriber = self.create_subscription(JointState, self.topic_name, self.plot_callback, qos_policy)
+        self.pl=NBPlot()
+
+    def plot_callback(self, wheel_effort):
+        pl.plot(wheel_effort.effort[0], wheel_effort.effort[1], wheel_effort.effort[2], wheel_effort.effort[3])
+
+def main():
+    rclpy.init()
+    node = PlotterNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        print('interrupt received, so shutting down')
+
+
+if __name__ == '__main__':
+    main()

stretch_core/stretch_core/multiprocessing_plotter.py

@@ -0,0 +1,103 @@
+# Based on the multiprocessing tutorial here:
+# https://matplotlib.org/stable/gallery/misc/multiprocess_sgskip.html
+
+import threading
+import multiprocessing as mp
+import time
+import random
+
+import matplotlib.pyplot as plt
+import numpy as np
+
+
+class ProcessPlotter:
+    def __init__(self):
+        self.time = []
+        self.lift = []
+        self.arm = []
+        self.wrist_roll = []
+        self.head_pan = []
+
+    def terminate(self):
+        plt.close('all')
+
+    def call_back(self):
+        while self.pipe.poll():
+            command = self.pipe.recv()
+            if command is None:
+                self.terminate()
+                return False
+            else:
+                self.time.append(command[0])
+                self.lift.append(command[1])
+                self.arm.append(command[2])
+                self.wrist_roll.append(command[3])
+                self.head_pan.append(command[4])
+                self.ax[0].plot(self.time, self.lift, color='red')
+                self.ax[1].plot(self.time, self.arm, color='green')
+                self.ax[2].plot(self.time, self.wrist_roll, color='blue')
+                self.ax[3].plot(self.time, self.head_pan, color='purple')
+        self.fig.canvas.draw()
+        return True
+
+    def __call__(self, pipe):
+        self.pipe = pipe
+        self.fig, self.ax = plt.subplots(4)
+        self.ax[0].set_ylim([0.0, 1.1])
+        self.ax[1].set_ylim([0.0, 0.5])
+        self.ax[2].set_ylim([-1.57, 1.57])
+        self.ax[3].set_ylim([-4.04, 1.73])
+        timer = self.fig.canvas.new_timer(interval=250)
+        timer.add_callback(self.call_back)
+        timer.start()
+
+        plt.show()
+
+
+class NBPlot:
+    def __init__(self):
+        self.plot_pipe, plotter_pipe = mp.Pipe()
+        self.plotter = ProcessPlotter()
+        self.plot_process = mp.Process(
+            target=self.plotter, args=(plotter_pipe,), daemon=True)
+        self.plot_process.start()
+        self.start_time = time.time()
+
+    def plot(self, lift_pos, arm_pos, roll_pos, pan_pos, finished=False):
+        send = self.plot_pipe.send
+        if finished:
+            send(None)
+        else:
+            data = np.array([time.time() - self.start_time, lift_pos, arm_pos, roll_pos, pan_pos])
+            send(data)
+
+
+def main(flag):
+    pl=NBPlot()
+    while not flag.is_set():
+        pl.plot(random.uniform(0.0, 1.1), random.uniform(0.0, 0.5), random.uniform(-1.57, 1.57), random.uniform(-4.04, 1.5))
+        time.sleep(0.25)
+    pl.plot(None, None, None, None, finished=True)
+
+
+# if __name__ == '__main__':
+#     shutdown_flag = threading.Event()
+#     threading.Thread(target=main, args=(shutdown_flag,)).start()
+#     threading.Thread(target=main, args=(shutdown_flag,)).start()
+#     print('Press Ctrl-C to exit')
+#     try:
+#         while True:
+#             pass
+#     except:
+#         pass
+#     finally:
+#         shutdown_flag.set()
+#         time.sleep(1)
+
+
+if __name__ == '__main__':
+    shutdown_flag = threading.Event()
+    threading.Thread(target=main, args=(shutdown_flag,)).start()
+    threading.Thread(target=main, args=(shutdown_flag,)).start()
+    time.sleep(5)
+    shutdown_flag.set()

stretch_core/stretch_core/stretch_driver.py

@@ -189,15 +189,16 @@ def command_mobile_base_velocity_and_publish_state(self):
         else:
             self.gamepad_teleop.update_gamepad_state(self.robot) # Update gamepad input readings within gamepad_teleop instance
 
-        # set new mobile base velocities, if appropriate
-        # check on thread safety for this with callback that sets velocity command values
+        # Set new mobile base velocities
         if self.robot_mode == 'navigation':
             time_since_last_twist = self.get_clock().now() - self.last_twist_time
             if time_since_last_twist < self.timeout:
                 self.robot.base.set_velocity(self.linear_velocity_mps, self.angular_velocity_radps)
                 # self.robot.push_command() #Moved to main
+            elif time_since_last_twist < Duration(seconds=self.timeout_s+1.0):
+                self.robot.base.translate_by(0)
+                # self.robot.push_command() #Moved to main
             else:
-                # Too much information in general, although it could be blocked, since it's just INFO.
                 self.robot.base.set_velocity(0.0, 0.0)
                 # self.robot.push_command() #Moved to main
 
@@ -512,6 +513,15 @@ def command_mobile_base_velocity_and_publish_state(self):
         joint_state.effort = efforts
         self.joint_state_pub.publish(joint_state)
 
+        # publish wheel effort
+        wheel_effort = JointState()
+        wheel_effort.header.stamp = current_time
+        wheel_effort.name = ['joint_left_wheel_current', 'joint_right_wheel_current', 'joint_left_wheel_effortpct', 'joint_right_wheel_effortpct']
+        wheel_effort.position = [0.0, 0.0]
+        wheel_effort.velocity = [0.0, 0.0]
+        wheel_effort.effort = [base_status['left_wheel']['current'], base_status['right_wheel']['current'], base_status['left_wheel']['effort_pct'], base_status['right_wheel']['effort_pct']]
+        self.wheel_effort_pub.publish(wheel_effort)
+
         ##################################################
         # publish IMU sensor data
         imu_status = robot_status['pimu']['imu']
@@ -1002,6 +1012,7 @@ def ros_setup(self):
 
         self.joint_state_pub = self.create_publisher(JointState, 'joint_states', 1)
         self.joint_limits_pub = self.create_publisher(JointState, 'joint_limits', 1)
+        self.wheel_effort_pub = self.create_publisher(JointState, 'wheel_effort', 1)
 
         self.last_twist_time = self.get_clock().now()
         self.last_gamepad_joy_time = self.get_clock().now()