def __init__(self, r5m_instance=0, device_ids=[1,2,3,4,5], heartbreat_frequency=20, request_timeout=0.1, **kwargs):
self.r5m_instance = r5m_instance
self.device_ids = device_ids
self.wait_for_heatbeat = False
self.heartbreat_frequency = heartbreat_frequency
self.request_timeout = request_timeout
for r5m_inst in Reach5MiniROSRequesterRS485.instances:
if r5m_inst.r5m_instance == self.r5m_instance:
raise Exception('Cannot use the same r5m_instance number as other Reach5MiniROSRequesterRS485 class instances!')
Reach5MiniROSRequesterRS485.instances.append(self)
# Init subscribers and publishers
# Request packets target topic
self.requests_pub = rospy.Publisher('r5m_' + str(self.r5m_instance) + '/requests', request_list, queue_size=10)
# Callbacks
self.position_sub = rospy.Subscriber('r5m_' + str(self.r5m_instance) + '/position', single_float, self.position_callback)
self.velocity_sub = rospy.Subscriber('r5m_' + str(self.r5m_instance) + '/velocity', single_float, self.velocity_callback)
self.current_sub = rospy.Subscriber('r5m_' + str(self.r5m_instance) + '/current', single_float, self.current_callback)
self.openloop_sub = rospy.Subscriber('r5m_' + str(self.r5m_instance) + '/openloop', single_float, self.openloop_callback)
self.mode_sub = rospy.Subscriber('r5m_' + str(self.r5m_instance) + '/mode', single_int, self.mode_callback)
self.other_callback = rospy.Subscriber('r5m_' + str(self.r5m_instance) + '/other', generic, self.other_callback)
# Define messages
self.position_msg = single_float()
self.velocity_msg = single_float()
self.position_msg = single_float()
self.current_msg = single_float()
self.mode_msg = single_int()
self.other_msg = generic()
self.requests_msg = request_list()
def __init__(self, r5m_instance=0, device_ids=[1, 2, 3, 4, 5], **kwargs):
self.r5m_instance = r5m_instance
self.device_ids = device_ids
for r5m_inst in Reach5Mini_Passthrough.instances:
if r5m_inst.r5m_instance == self.r5m_instance:
raise Exception(
'Cannot use the same r5m_instance number as other Reach5Mini_Passthrough class instances!'
)
Reach5Mini_Passthrough.instances.append(self)
self.commconnection = CommConnection()
self.serialconversion = SerialConversion()
# Initialise ROS publishers and subscribers
self.cmd_velocity_sub = rospy.Subscriber(
'r5m_' + str(self.r5m_instance) + '/cmd_velocity', single_float,
self.cmd_velocity_callback)
self.cmd_position_sub = rospy.Subscriber(
'r5m_' + str(self.r5m_instance) + '/cmd_position', single_float,
self.cmd_position_callback)
self.requests_sub = rospy.Subscriber(
'r5m_' + str(self.r5m_instance) + '/requests', request_list,
self.requests_callback)
self.position_pub = rospy.Publisher('r5m_' + str(self.r5m_instance) +
'/position',
single_float,
queue_size=10)
self.velocity_pub = rospy.Publisher('r5m_' + str(self.r5m_instance) +
'/velocity',
single_float,
queue_size=10)
self.current_pub = rospy.Publisher('r5m_' + str(self.r5m_instance) +
'/current',
single_float,
queue_size=10)
self.openloop_pub = rospy.Publisher('r5m_' + str(self.r5m_instance) +
'/openloop',
single_float,
queue_size=10)
self.mode_pub = rospy.Publisher('r5m_' + str(self.r5m_instance) +
'/mode',
single_int,
queue_size=10)
self.other_pub = rospy.Publisher('r5m_' + str(self.r5m_instance) +
'/other',
generic,
queue_size=10)
# Initialise ROS messages
self.position_message = single_float()
self.velocity_message = single_float()
self.current_message = single_float()
self.openloop_message = single_float()
self.mode_message = single_int()
self.other_message = generic()
#!/usr/bin/env python3
import os, sys, time
import rospy
import serial
import re
import struct
import threading
from blueprintlab_reachsystem_ros_messages.msg import single_float
from pynput.keyboard import Key
from pynput.keyboard import Listener
cmd_velocity_pub = rospy.Publisher('r5m_0/cmd_velocity',
single_float,
queue_size=10)
cmd_velocity_message = single_float()
def keyboard_press(key):
try:
key_pressed = key.char
except:
return
if key.char == 'w':
cmd_velocity_message.device_id = 3
cmd_velocity_message.value = 0.5
cmd_velocity_pub.publish(cmd_velocity_message)
print('w pressed')
elif key.char == 's':
cmd_velocity_message.device_id = 3
cmd_velocity_message.value = -0.5