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 __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, 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()
def main():
rospy.init_node('ds4_driver_node')
device_addr = rospy.get_param('~device_addr', None)
backend_type = rospy.get_param('~backend', 'bluetooth')
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)
# rospy.loginfo(device_addr)
if len(device_addr) < 8:
# scan exist device address
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 :(")
else:
# connect exist device
device = None
while device == None:
device = backend.specify_addr_connect(device_addr)
controller.setup_device(device)
controller.start()
controller.loop.register_event('device-report', controller.cb_report)
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 :(")
class ds4_driver_BLE(ControllerRos):
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()
def ds4_connect(self):
try:
self.backend.setup()
except BackendError as err:
#rospy.logerr(err)
ros2_node.get_logger().info('Error')
sys.exit(1)
#
print('Wait....')
for device in self.backend.devices:
self.ros2_node.get_logger().info('I heard: "%s"' %
format(device.name))
#rospy.loginfo('Connected to {0}'.format(device.name))
if self.device_addr in (None, '', device.device_addr):
self.setup_device(device)
break
#rospy.loginfo('Waiting for specified controller')
self.ros2_node.get_logger().info(
'Waiting for specified controller')
# DS4 Connect Status
# Status = Int32()
self.Status.data = 1
self.DS4_Connect_Status.publish(self.Status)
# Status
# self.DS4_Connect_Status.publish(Status)
def ds4_Not_connect(self):
# DS4 Connect Status
# Status = Int32()
self.Status.data = 0
self.DS4_Connect_Status.publish(self.Status)
# Status
# self.DS4_Connect_Status.publish(Status)
#
self.ros2_node.get_logger().info('Disconnected...')
# self.exit()
self.loop.stop()
self.ds4_connect()
# Run
self.loop.register_event('device-report', self.cb_report)
self.loop.register_event('device-cleanup', self.ds4_Not_connect)
self.run()
'''
if self.is_alive():
self.join()
'''
def ds4_Start(self):
if not self.is_alive():
self.start()
self.loop.register_event('device-report', self.cb_report)
self.loop.register_event('device-cleanup', self.ds4_Not_connect)
def Send_Connect_Status(self):
self.DS4_Connect_Status.publish(self.Status)