<repo_name>sss22213/ds4_driver<file_sep>/CMakeLists.txt
cmake_minimum_required(VERSION 2.8.3)
project(ds4_driver)

find_package(catkin REQUIRED COMPONENTS
  message_generation
  std_msgs
  sensor_msgs
)

add_message_files(
  DIRECTORY msg
  FILES Feedback.msg Report.msg Status.msg Trackpad.msg
)

catkin_python_setup()

generate_messages(
  DEPENDENCIES
  std_msgs
  sensor_msgs
)

catkin_package()

# catkin_install_python(PROGRAMS
#   nodes/ds4_driver_node.py
#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
<file_sep>/nodes/ds4_driver_node.py
#!/usr/bin/env python

from ds4_driver.logger import Logger
from ds4_driver.controller_ros import ControllerRos

from ds4drv.backends import BluetoothBackend, HidrawBackend
from ds4drv.exceptions import BackendError

import rospy

import signal
import sys


class SignalHandler(object):
    def __init__(self, controller):
        self.controller = controller

    def __call__(self, signum, frame):
        rospy.loginfo('Shutting down...')
        self.controller.exit()
        sys.exit(0)


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)

if __name__ == '__main__':
    main()
<file_sep>/nodes/demo.py
#!/usr/bin/env python

import rospy
from ds4_driver.msg import Feedback, Status


class Handler(object):
    def __init__(self, status_topic='status', feedback_topic='set_feedback'):
        self._min_interval = 0.1
        self._last_pub_time = rospy.Time()
        self._prev = Status()
        self._led = {
            'r': 0,
            'g': 0,
            'b': 0,
            'flashing': False,
        }

        self._pub_feedback = rospy.Publisher(feedback_topic, Feedback, queue_size=1)
        rospy.Subscriber(status_topic, Status, self.cb_status, queue_size=1)

    def cb_status(self, msg):
        """
        :type msg: Status
        """
        now = rospy.Time.now()
        if (now - self._last_pub_time).to_sec() < self._min_interval:
            return

        feedback = Feedback()

        feedback.set_rumble = True
        feedback.rumble_small = abs(msg.axis_left_y)
        feedback.rumble_big = abs(msg.axis_right_y)

        # Set LED using the touchpad
        touch = msg.touch0
        if touch.active and msg.button_circle:
            feedback.set_led = True
            self._led['r'] = touch.x
        if touch.active and msg.button_triangle:
            feedback.set_led = True
            self._led['g'] = touch.x
        if touch.active and msg.button_cross:
            feedback.set_led = True
            self._led['b'] = touch.x
        feedback.led_r = self._led['r']
        feedback.led_g = self._led['g']
        feedback.led_b = self._led['b']

        # Turn on/off flash with PS button
        if not self._prev.button_ps and msg.button_ps:
            feedback.set_led_flash = True
            if self._led['flashing']:
                feedback.led_flash_off = 0
            else:
                feedback.led_flash_on = 0.2
                feedback.led_flash_off = 0.2
            self._led['flashing'] = not self._led['flashing']

        self._pub_feedback.publish(feedback)
        self._prev = msg
        self._last_pub_time = now


def main():
    rospy.init_node('sample')

    Handler()

    rospy.spin()


if __name__ == '__main__':
    main()
<file_sep>/nodes/ds4_twist_node.py
#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist, TwistStamped
from ds4_driver.msg import Status


class StatusToTwist(object):
    def __init__(self):
        self._stamped = rospy.get_param('~stamped', False)
        if self._stamped:
            self._cls = TwistStamped
            self._frame_id = rospy.get_param('~frame_id', 'base_link')
        else:
            self._cls = Twist
        self._inputs = rospy.get_param('~inputs')
        self._scales = rospy.get_param('~scales')

        self._attrs = []
        for attr in Status.__slots__:
            if attr.startswith('axis_') or attr.startswith('button_'):
                self._attrs.append(attr)

        self._pub = rospy.Publisher('cmd_vel', self._cls, queue_size=1)
        rospy.Subscriber('status', Status, self.cb_status, queue_size=1)

    def cb_status(self, msg):
        """
        :param msg:
        :type msg: Status
        :return:
        """
        input_vals = {}
        for attr in self._attrs:
            input_vals[attr] = getattr(msg, attr)

        to_pub = self._cls()
        if self._stamped:
            to_pub.header.stamp = rospy.Time.now()
            to_pub.header.frame_id = self._frame_id
            twist = to_pub.twist
        else:
            twist = to_pub

        for vel_type in self._inputs:
            vel_vec = getattr(twist, vel_type)
            for k, expr in self._inputs[vel_type].items():
                scale = self._scales[vel_type].get(k, 1.0)
                val = eval(expr, {}, input_vals)
                setattr(vel_vec, k, scale * val)

        self._pub.publish(to_pub)


def main():
    rospy.init_node('ds4_twist')

    StatusToTwist()

    rospy.spin()


if __name__ == '__main__':
    main()
