From bfc28889759097dfce5e7eb776bfd3fa1e5c7173 Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Tue, 27 Aug 2024 22:24:43 -0400 Subject: [PATCH 1/4] Create driver_callbacks.md --- docs/driver_callbacks.md | 39 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 39 insertions(+) create mode 100644 docs/driver_callbacks.md diff --git a/docs/driver_callbacks.md b/docs/driver_callbacks.md new file mode 100644 index 00000000..184f86de --- /dev/null +++ b/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 From 52e4ef4e46ad5e409130834c84ae15f2f023c7f2 Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Tue, 10 Sep 2024 15:20:03 -0400 Subject: [PATCH 2/4] Publish wheel effort --- stretch_core/stretch_core/stretch_driver.py | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/stretch_core/stretch_core/stretch_driver.py b/stretch_core/stretch_core/stretch_driver.py index baaa84b1..486f1fba 100755 --- a/stretch_core/stretch_core/stretch_driver.py +++ b/stretch_core/stretch_core/stretch_driver.py @@ -512,6 +512,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 +1011,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() From c8e05fe6c4365ad4241014c20848a059a2ea25e3 Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Tue, 10 Sep 2024 16:05:14 -0400 Subject: [PATCH 3/4] Add plotting --- stretch_core/stretch_core/effort_plotter.py | 37 +++++++ .../stretch_core/multiprocessing_plotter.py | 103 ++++++++++++++++++ 2 files changed, 140 insertions(+) create mode 100644 stretch_core/stretch_core/effort_plotter.py create mode 100644 stretch_core/stretch_core/multiprocessing_plotter.py diff --git a/stretch_core/stretch_core/effort_plotter.py b/stretch_core/stretch_core/effort_plotter.py new file mode 100644 index 00000000..ae3ca38b --- /dev/null +++ b/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() diff --git a/stretch_core/stretch_core/multiprocessing_plotter.py b/stretch_core/stretch_core/multiprocessing_plotter.py new file mode 100644 index 00000000..86c88404 --- /dev/null +++ b/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() \ No newline at end of file From 91583baf5da0679b58441fcfd4b7fd655956a43d Mon Sep 17 00:00:00 2001 From: Binit Shah Date: Wed, 24 Jul 2024 14:16:55 -0400 Subject: [PATCH 4/4] Fix PID whine noise from wheels during vel control Given a command, the wheels attempt to achieve it using PID controllers in the firmware. Ideally, the controller closes the error between the commanded position and the current one. But actually, this isn't always possible. For example, if the goal requires the robot to be within an obstacle, the controller will push against the obstacle, making no progress, and emitting a whining noise from the wheel motors. Another example, on carpet, the controller will find it difficult to achieve the goal exactly, due to the soft flooring. Unable to close the small error, but trying all the same, the wheel motors emit a noise. Tuning the PID parameters is no good because what works well on carpet will overshoot on hardwood, and vice versa. In this commit, we relax the controller 0.5 sec after the user stops sending velocity commands, eliminating the wheel noise. Note that this commit only address wheel noise in velocity mode and a future change will be needed to address it in position mode. --- stretch_core/stretch_core/stretch_driver.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/stretch_core/stretch_core/stretch_driver.py b/stretch_core/stretch_core/stretch_driver.py index 486f1fba..c2331ff8 100755 --- a/stretch_core/stretch_core/stretch_driver.py +++ b/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