Skip to content

WIP: Publish wheel effort #166

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
wants to merge 4 commits into
base: humble
Choose a base branch
from
Draft
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
39 changes: 39 additions & 0 deletions docs/driver_callbacks.md
Original file line number Diff line number Diff line change
@@ -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
37 changes: 37 additions & 0 deletions stretch_core/stretch_core/effort_plotter.py
Original file line number Diff line number Diff line change
@@ -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()
103 changes: 103 additions & 0 deletions stretch_core/stretch_core/multiprocessing_plotter.py
Original file line number Diff line number Diff line change
@@ -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()
17 changes: 14 additions & 3 deletions stretch_core/stretch_core/stretch_driver.py
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -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']
Expand Down Expand Up @@ -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()
Expand Down