rtswitches.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 import sys
3 import rospy
4 from raspimouse_ros.msg import Switches
5 
6 devfile = '/dev/rtswitch'
7 
8 rospy.init_node('rtswitches')
9 pub = rospy.Publisher('switches', Switches, queue_size=10)
10 rate = rospy.Rate(10)
11 
12 d = Switches()
13 
14 d.state = 'neutral'
15 state_change_counter = 0
16 
17 while not rospy.is_shutdown():
18  try:
19  with open(devfile + '0','r') as f:
20  d.front = True if '0' in f.readline() else False
21  with open(devfile + '1','r') as f:
22  d.center = True if '0' in f.readline() else False
23  with open(devfile + '2','r') as f:
24  d.rear = True if '0' in f.readline() else False
25  except:
26  rospy.logerr("cannot open " + devfile + "[0,1,2]")
27 
28  if d.front: state_change_counter += 1
29 
30  if state_change_counter >= 5 and not d.front:
31  state_change_counter = 0
32  if d.state == 'neutral': d.state = 'ready'
33  elif d.state == 'ready': d.state = 'run'
34  else: d.state = 'neutral'
35 
36  pub.publish(d)
37  rate.sleep()
38 
msg

raspimouse_ros
Author(s):
autogenerated on Mon Jun 10 2019 14:27:02