def __init__(self, cfg_path=None): # Adapted from ds4drv __main__:main()
self.threads = []
self.sigint_handler = SigintHandler(self.threads)
signal.signal(signal.SIGINT, self.sigint_handler)
try:
if cfg_path:
sys.argv.extend(['--config', cfg_path])
aux = sys.argv
sys.argv = [a for a in sys.argv if (not a.startswith('_') and ":=" not in a)]
self.options = load_options()
sys.argv = aux # Return original argv
except ValueError as err:
Daemon.exit("Failed to parse options: {0}", err)
if self.options.hidraw:
self.backend = HidrawBackend(Daemon.logger)
else:
self.backend = BluetoothBackend(Daemon.logger)
try:
self.backend.setup()
except BackendError as err:
Daemon.exit(err)
if len(self.options.controllers) > 1:
rospy.logwarn(
'[ROSDS4Controller] Only one controller is processed. Multiple controller options will be ignored')
self.thread = None
self.disconnect_called = True
def __init__(self):
self.thread = None
options = load_options()
if options.hidraw:
raise ValueError("HID mode not supported")
backend = HidrawBackend(Daemon.logger)
else:
subprocess.run(["hciconfig", "hciX", "up"])
backend = BluetoothBackend(Daemon.logger)
backend.setup()
self.thread = create_controller_thread(1, options.controllers[0])
self.thread.controller.setup_device(next(backend.devices))
self.shim = ActionShim(self.thread.controller)
self.thread.controller.actions.append(self.shim)
self.shim.enable()
self._color = (None, None, None)
self._rumble = (None, None)
self._flash = (None, None)
# ensure we get a value before returning
while self.shim.values is None:
pass
def main():
rclpy.init()
node = rclpy.create_node("ds4_driver_node")
node.declare_parameter("device_addr", None)
node.declare_parameter("backend", "hidraw")
device_addr = node.get_parameter("device_addr").value
backend_type = node.get_parameter("backend").value
logger = node.get_logger()
controller = ControllerRos(node)
sigint_handler = SignalHandler(controller, logger)
# Since backend.devices is a non-ROS iterator that doesn't consider
# rclpy.is_shutdown(), the program freezes upon receiving SIGINT when
# using rclpy.on_shutdown. Thus, we need to define our shutdown sequence
# using signal.signal as is done in the original ds4drv script.
signal.signal(signal.SIGINT, sigint_handler)
if backend_type == "bluetooth":
backend = BluetoothBackend(Logger("backend"))
else:
backend = HidrawBackend(Logger("backend"))
try:
backend.setup()
except BackendError as err:
logger.error(err)
rclpy.signal_shutdown(str(err))
sys.exit(1)
spin_thread = Thread(target=rclpy.spin, args=(node,))
spin_thread.start()
for device in backend.devices:
logger.info("Connected to {0}".format(device.name))
if device_addr in (None, "", device.device_addr):
controller.setup_device(device)
if not controller.is_alive():
controller.start()
controller.loop.register_event("device-report", controller.cb_report)
else:
logger.error("...but it's not the one we're looking for :(")
def main():
rospy.init_node('ds4_driver_node')
device_addr = rospy.get_param('~device_addr', None)
backend_type = rospy.get_param('~backend', 'hidraw')
controller = ControllerRos()
sigint_handler = SignalHandler(controller)
# Since backend.devices is a non-ROS iterator that doesn't consider
# rospy.is_shutdown(), the program freezes upon receiving SIGINT when
# using rospy.on_shutdown. Thus, we need to define our shutdown sequence
# using signal.signal as is done in the original ds4drv script.
signal.signal(signal.SIGINT, sigint_handler)
if backend_type == 'bluetooth':
backend = BluetoothBackend(Logger('backend'))
else:
backend = HidrawBackend(Logger('backend'))
try:
backend.setup()
except BackendError as err:
rospy.logerr(err)
rospy.signal_shutdown(str(err))
sys.exit(1)
for device in backend.devices:
rospy.loginfo('Connected to {0}'.format(device.name))
if device_addr in (None, '', device.device_addr):
controller.setup_device(device)
if not controller.is_alive():
controller.start()
controller.loop.register_event('device-report',
controller.cb_report)
else:
rospy.loginfo("...but it's not the one we're looking for :(")
def __init__(self, ros2_node):
super(ds4_driver_BLE, self).__init__(ros2_node)
self.ros2_node = ros2_node
# Publish Connect Status
self.DS4_Connect_Status = self.ros2_node.create_publisher(
Int32, "ds4_connect_status")
#
# self.device_addr = rospy.get_param('~device_addr', None)
self.device_addr = None
backend_type = 'bluetooth'
#backend_type = rospy.get_param('~backend', 'hidraw')
# Connect Status
self.Status = Int32()
self.Status.data = 0
# backend_type
if backend_type == 'bluetooth':
self.backend = BluetoothBackend(Logger('backend'))
else:
self.backend = HidrawBackend(Logger('backend'))
self.ds4_connect()
class ROSDS4Controller:
def __init__(self, cfg_path=None): # Adapted from ds4drv __main__:main()
self.threads = []
self.sigint_handler = SigintHandler(self.threads)
signal.signal(signal.SIGINT, self.sigint_handler)
try:
if cfg_path:
sys.argv.extend(['--config', cfg_path])
aux = sys.argv
sys.argv = [a for a in sys.argv if (not a.startswith('_') and ":=" not in a)]
self.options = load_options()
sys.argv = aux # Return original argv
except ValueError as err:
Daemon.exit("Failed to parse options: {0}", err)
if self.options.hidraw:
self.backend = HidrawBackend(Daemon.logger)
else:
self.backend = BluetoothBackend(Daemon.logger)
try:
self.backend.setup()
except BackendError as err:
Daemon.exit(err)
if len(self.options.controllers) > 1:
rospy.logwarn(
'[ROSDS4Controller] Only one controller is processed. Multiple controller options will be ignored')
self.thread = None
self.disconnect_called = True
def connect(self):
if not self.thread or not self.thread.is_alive():
self.thread = create_controller_thread(1, self.options.controllers[0])
if self.thread not in self.threads:
self.threads.append(self.thread)
self.device = next(self.backend.devices) # Gets device
self.thread.controller.setup_device(self.device)
self.disconnect_called = False
return self.device
def disconnect(self):
self.thread.controller.cleanup_device()
self.disconnect_called = True
def is_connected(self):
return self.thread.controller.device is not None
def get_jsdev(self):
i = ActionRegistry.actions.index(ds4drv.actions.input.ReportActionInput)
return self.thread.controller.actions[i].joystick.joystick_dev
def get_battery(self, _):
if not self.is_connected():
raise Exception('Controller is not connected!')
i = ActionRegistry.actions.index(ds4drv.actions.status.ReportActionStatus)
lr = self.thread.controller.actions[i]._last_report
max_value = lr.plug_usb and ds4drv.actions.status.BATTERY_MAX_CHARGING or ds4drv.actions.status.BATTERY_MAX
battery = 100 * lr.battery / float(max_value)
res = BatteryStatusResponse()
res.charge_percent = battery
res.charging = lr.plug_usb
return res
def set_color(self, r, g, b):
self.device.set_led(r, g, b)
def srv_set_color(self, req):
if not self.is_connected():
return []
self.set_color(int(req.color.r), int(req.color.g), int(req.color.b))
return []
def start_led_flash(self, req):
if not self.is_connected():
return []
self.stop_led_flash()
self.device.start_led_flash(req.on_time, req.off_time)
return ()
def stop_led_flash(self, _=''):
if not self.is_connected():
return []
self.device.stop_led_flash()
return ()
def rumble(self, req):
if not self.is_connected():
return []
s = rospy.get_time()
d = rospy.get_time() - s
while d < req.duration and not rospy.is_shutdown():
if d % 4 < 0.01: # Every 4 seconds we reactivate rumble so it doesn't stop...
self.device.rumble(req.small, req.big)
d = rospy.get_time() - s
self.stop_rumble()
return ()
def stop_rumble(self, _=None):
if not self.is_connected():
return []
self.device.rumble(0, 0)
return ()
def srv_connect(self, _):
if not self.is_connected():
self.connect()
else:
raise Exception('Controller is already connected!')
return ()
def srv_disconnect(self, _):
if self.is_connected():
self.disconnect()
else:
raise Exception('Controller is not connected!')
return ()
def srv_is_connected(self, _):
res = IsConnectedResponse()
res.is_connected = self.is_connected()
return res
def start_ros_services(self):
self.connectsrv = rospy.Service('~connect', Empty, self.srv_connect)
self.disconnectsrv = rospy.Service('~disconnect', Empty, self.srv_disconnect)
self.color = rospy.Service('~set_color', SetColor, self.srv_set_color)
self.flash = rospy.Service('~flash', StartFlash, self.start_led_flash)
self.stopflash = rospy.Service('~stop_flash', Empty, self.stop_led_flash)
self.rumble = rospy.Service('~rumble', Rumble, self.rumble)
self.stoprumble = rospy.Service('~stop_rumble', Empty, self.stop_rumble)
self.battery = rospy.Service('~battery_status', BatteryStatus, self.get_battery)
self.isconnected = rospy.Service('~is_connected', IsConnected, self.srv_is_connected)