1  #!/usr/bin/env python3
  2  ################################################################################
  3  # Copyright 2022 FZI Research Center for Information Technology
  4  #
  5  # Redistribution and use in source and binary forms, with or without
  6  # modification, are permitted provided that the following conditions are met:
  7  #
  8  # 1. Redistributions of source code must retain the above copyright notice,
  9  # this list of conditions and the following disclaimer.
 10  #
 11  # 2. Redistributions in binary form must reproduce the above copyright notice,
 12  # this list of conditions and the following disclaimer in the documentation
 13  # and/or other materials provided with the distribution.
 14  #
 15  # 3. Neither the name of the copyright holder nor the names of its
 16  # contributors may be used to endorse or promote products derived from this
 17  # software without specific prior written permission.
 18  #
 19  # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 20  # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 21  # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 22  # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 23  # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 24  # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 25  # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 26  # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 27  # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 28  # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 29  # POSSIBILITY OF SUCH DAMAGE.
 30  ################################################################################
 31  
 32  # -----------------------------------------------------------------------------
 33  # \file    buttons.py
 34  #
 35  # \author  Stefan Scherzinger <scherzin@fzi.de>
 36  # \date    2022/11/09
 37  #
 38  # -----------------------------------------------------------------------------
 39  
 40  import rclpy
 41  from rclpy.node import Node
 42  from sensor_msgs.msg import Joy
 43  import subprocess
 44  import numpy as np
 45  import sys
 46  import time
 47  
 48  
 49  class buttons(Node):
 50      """React to button events"""
 51  
 52      def __init__(self):
 53          super().__init__("spacenav_buttons")
 54  
 55          self.repeat_same_button = self.declare_parameter(
 56              "repeat_same_button", False
 57          ).value
 58          self.button_sleep = self.declare_parameter("button_sleep", 0.1).value
 59          self.button_cmds = self.declare_parameter("button_cmds", [""]).value
 60          self.cmd_dirs = self.declare_parameter("cmd_dirs", [""]).value
 61          self.last_button_cmds = None
 62  
 63          self.joystick_topic = self.declare_parameter("joystick_topic", "").value
 64          self.sub = self.create_subscription(
 65              Joy, self.joystick_topic, self.event_callback, 1
 66          )
 67  
 68      def event_callback(self, data):
 69          # Have some buttons been repeatedly pressed?
 70          if self.last_button_cmds and any(
 71              np.bitwise_and(data.buttons, self.last_button_cmds)
 72          ):
 73              return
 74          for idx, val in enumerate(data.buttons):
 75              if val == 1:
 76                  exec_dir = self.cmd_dirs[idx]
 77                  if not exec_dir:  # Empty string
 78                      exec_dir = None
 79                  subprocess.Popen(
 80                      self.button_cmds[idx],
 81                      stdin=subprocess.PIPE,
 82                      cwd=exec_dir,
 83                      shell=True,
 84                  )
 85                  # Prevent pressing the same buttons in a row
 86                  if not self.repeat_same_button:
 87                      self.last_button_cmds = data.buttons
 88                  time.sleep(self.button_sleep)
 89  
 90  
 91  def main(args=None):
 92      rclpy.init(args=args)
 93  
 94      node = buttons()
 95      rclpy.spin(node)
 96      rclpy.shutdown()
 97  
 98  
 99  if __name__ == "__main__":
100      try:
101          main()
102      except KeyboardInterrupt:
103          sys.exit(0)
104      except Exception as e:
105          print(e)
106          sys.exit(1)