Votes Internally!
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

167 lines
4.1 KiB

6 years ago
6 years ago
6 years ago
6 years ago
6 years ago
6 years ago
6 years ago
6 years ago
6 years ago
  1. import socket
  2. import struct
  3. import time
  4. from cStringIO import StringIO
  5. import rospy
  6. from geometry_msgs.msg import Twist
  7. from kobuki_msgs.msg import BumperEvent, Sound
  8. ##### Configuration #####
  9. # Address to listen to (empty indicates all interfaces)
  10. LISTEN_ADDR=''
  11. LISTEN_PORT=13676
  12. # Cut power if we lose keepalives
  13. TERM_ON_KA=True
  14. KA_TIMEOUT = 3 # seconds
  15. # Rate with which to publish commands (also controls responsiveness)
  16. CMD_RATE = 10
  17. # Don't set linear velocity if the bumper is detected
  18. DONT_BUMP=True
  19. BUMP_REV_SPEED = 0.05 # This MUST be positive
  20. BUMP_REV_TIME = 1.5
  21. ##### Protocol #####
  22. class Packet(object):
  23. def __init__(self, ival = None):
  24. if ival is not None:
  25. self.buffer = StringIO(ival)
  26. else:
  27. self.buffer = StringIO()
  28. # Staticmethod
  29. def _reader(fmt):
  30. return lambda self: struct.unpack(fmt, self.buffer.read(struct.calcsize(fmt)))[0]
  31. # Staticmethod
  32. def _writer(fmt):
  33. return lambda self, obj: self.buffer.write(struct.pack(fmt, obj))
  34. read_byte = _reader('!b')
  35. read_ubyte = _reader('!B')
  36. read_short = _reader('!h')
  37. read_ushort = _reader('!H')
  38. read_int = _reader('!i')
  39. read_uint = _reader('!I')
  40. read_long = _reader('!l')
  41. read_ulong = _reader('!L')
  42. read_float = _reader('!f')
  43. read_double = _reader('!d')
  44. write_byte = _writer('!b')
  45. write_ubyte = _writer('!B')
  46. write_short = _writer('!h')
  47. write_ushort = _writer('!H')
  48. write_int = _writer('!i')
  49. write_uint = _writer('!I')
  50. write_long = _writer('!l')
  51. write_ulong = _writer('!L')
  52. write_float = _writer('!f')
  53. write_double = _writer('!d')
  54. def __str__(self):
  55. return self.buffer.getvalue()
  56. class CMD:
  57. KEEPALIVE = 0
  58. MOTION = 1
  59. SOUND = 2
  60. class Processor(object):
  61. dispatch = {}
  62. motion = Twist()
  63. lastka = None
  64. impact = [False]*3
  65. backingsince = None
  66. def __init__(self, addr):
  67. self.socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
  68. self.socket.bind(addr)
  69. self.socket.setblocking(False)
  70. print 'Listener ready on', addr
  71. def tick(self):
  72. try:
  73. while True:
  74. data, src = self.socket.recvfrom(4096)
  75. self.src = src
  76. self.process_pkt(Packet(data))
  77. except socket.error:
  78. pass
  79. if self.lastka is not None and time.time() > self.lastka + KA_TIMEOUT:
  80. print 'KA timeout'
  81. self.lastka = None
  82. if TERM_ON_KA:
  83. print '(killing motors)'
  84. self.motion.linear.x = 0
  85. self.motion.angular.z = 0
  86. def process_pkt(self, pkt):
  87. self.cmd = pkt.read_ubyte()
  88. self.dispatch.get(self.cmd, self.no_cmd)(self, pkt)
  89. def no_cmd(self, _, pkt):
  90. print 'Warning: unknown command:', self.cmd
  91. def on_bump(self, status):
  92. self.impact[status.bumper] = bool(status.state)
  93. def get_motion(self):
  94. t = Twist()
  95. buf = StringIO()
  96. self.motion.serialize(buf)
  97. t.deserialize(buf.getvalue())
  98. if DONT_BUMP:
  99. if any(self.impact):
  100. self.backingsince = time.time()
  101. self.motion.linear.x = 0
  102. if self.backingsince is not None:
  103. if time.time() < self.backingsince + BUMP_REV_TIME:
  104. t.linear.x = -BUMP_REV_SPEED
  105. else:
  106. self.backingsince = None
  107. return t
  108. @classmethod
  109. def _dispatch(cls, cmd):
  110. def _inner(f):
  111. cls.dispatch[cmd] = f
  112. return f
  113. return _inner
  114. @Processor._dispatch(CMD.KEEPALIVE)
  115. def proc_cmd_keepalive(self, pkt):
  116. if self.lastka is None:
  117. print 'New KA from', self.src
  118. self.lastka = time.time()
  119. @Processor._dispatch(CMD.MOTION)
  120. def proc_cmd_motion(self, pkt):
  121. self.motion.linear.x = pkt.read_double()
  122. self.motion.angular.z = pkt.read_double()
  123. print self.src, ':', 'motion', self.motion.linear.x, self.motion.angular.z
  124. sound_node = None
  125. @Processor._dispatch(CMD.SOUND)
  126. def proc_cmd_sound(self, pkt):
  127. snd = pkt.read_ubyte()
  128. print 'Sound:', snd
  129. sound_node.publish(Sound(snd))
  130. if __name__ == '__main__':
  131. proc = Processor((LISTEN_ADDR, LISTEN_PORT))
  132. rospy.init_node('tbot', anonymous=False)
  133. motion_node = rospy.Publisher('mobile_base/commands/velocity', Twist, queue_size=10)
  134. rospy.Subscriber('mobile_base/events/bumper', BumperEvent, proc.on_bump)
  135. sound_node = rospy.Publisher('mobile_base/commands/sound', Sound, queue_size=10)
  136. rate = rospy.Rate(CMD_RATE)
  137. rospy.on_shutdown(lambda: motion_node.publish(Twist()))
  138. while not rospy.is_shutdown():
  139. proc.tick()
  140. motion_node.publish(proc.get_motion())
  141. rate.sleep()