190 lines
6.0 KiB
Python
190 lines
6.0 KiB
Python
#!/usr/bin/env python
|
|
|
|
# -*- coding: utf-8 -*-
|
|
"""
|
|
Created on Mon Feb 10 17:09:57 2020
|
|
|
|
@author: Maxi
|
|
"""
|
|
|
|
import roslib
|
|
import rospy
|
|
import sys
|
|
|
|
import std_msgs.msg as msg
|
|
from dynamic_reconfigure.server import Server as DynamicReconfigureServer
|
|
|
|
# Import custom message data and dynamic reconfigure variables.
|
|
from Steve.msg import node_example
|
|
from Steve.cfg import node_example_paramsConfig as ConfigType
|
|
|
|
import RPi.GPIO as GPIO
|
|
import Pin
|
|
|
|
|
|
class Switch:
|
|
def __init__(self):
|
|
""" configure RPi, GPIO """
|
|
print(GPIO.getmode())
|
|
self.is_high = False
|
|
|
|
""" configure ROS """
|
|
self.node()
|
|
|
|
init_message = rospy.get_param('~message', 'hello')
|
|
rate = float(rospy.get_param('~rate', '1.0'))
|
|
topic = rospy.get_param('~topic', 'chatter')
|
|
rospy.loginfo('rate = %d', rate)
|
|
rospy.loginfo('topic = %s', topic)
|
|
|
|
# create a dynamic reconfigure server
|
|
self.server = DynamicReconfigureServer(ConfigType, self.reconfigure)
|
|
# create a publisher
|
|
self.pub = rospy.Publisher(topic, node_example, queue_size=10)
|
|
# set the message to publish
|
|
msg = node_example()
|
|
msg.a = 1
|
|
msg.b = 2
|
|
msg.message = init_message
|
|
|
|
|
|
""" start main processing loop """
|
|
while not rospy.is_shutdown():
|
|
# self.send_message()
|
|
self.detect_IO_change()
|
|
# if self.event_based:
|
|
# self.check_IO_value()
|
|
|
|
# sleep before next message
|
|
rospy.sleep(1/rate) # rate != 0
|
|
|
|
def reconfigure(self, config, level):
|
|
# fill in local variables from dynamic reconfigure items
|
|
self.message = config["message"]
|
|
self.a = config["a"]
|
|
self.b = config["b"]
|
|
|
|
rospy.loginfo('reconfiguring with %s' % config)
|
|
return config
|
|
|
|
""" trying to split stuff into functions """
|
|
|
|
def setup_pwr_low(self):
|
|
""" for switches connected to 3.3V --> will send 'LOW' when closing """
|
|
print("setting up GPIO")
|
|
print('setting PULL DOWN, expect to send LOW')
|
|
GPIO.setup(self.pin_out,
|
|
GPIO.OUT)
|
|
GPIO.output(self.pin_out,
|
|
1)
|
|
GPIO.setup(self.pin,
|
|
GPIO.IN,
|
|
pull_up_down=GPIO.PUD_DOWN)
|
|
GPIO.add_event_detect(self.pin,
|
|
GPIO.FALLING,
|
|
callback=self.send_if_low,
|
|
bouncetime=1000)
|
|
|
|
def setup_pwr_high(self):
|
|
""" for switches connected to 3.3V --> will send 'HIGH' when closing """
|
|
print("setting up GPIO")
|
|
print('setting PULL DOWN, expect to send LOW')
|
|
GPIO.setup(self.pin_out,
|
|
GPIO.OUT)
|
|
GPIO.output(self.pin_out,
|
|
1)
|
|
GPIO.setup(self.pin,
|
|
GPIO.IN,
|
|
pull_up_down=GPIO.PUD_UP)
|
|
GPIO.add_event_detect(self.pin,
|
|
GPIO.RISING,
|
|
callback=self.send_if_high,
|
|
bouncetime=1000)
|
|
def setup_gnd_low(self):
|
|
""" for switches connected to GND --> will send 'LOW' when triggered """
|
|
print("setting up GPIO")
|
|
print('setting PULL UP, expect to send HIGH')
|
|
GPIO.setup(self.pin_out,
|
|
GPIO.OUT)
|
|
GPIO.output(self.pin_out,
|
|
0)
|
|
GPIO.setup(self.pin,
|
|
GPIO.IN,
|
|
pull_up_down=GPIO.PUD_UP)
|
|
GPIO.add_event_detect(self.pin,
|
|
GPIO.FALLING,
|
|
callback=self.send_if_low,
|
|
bouncetime=1000)
|
|
def setup_gnd_high(self):
|
|
""" for switches connected to GND --> will send 'HIGH' when triggered """
|
|
print("setting up GPIO")
|
|
print('setting PULL UP, expect to send HIGH')
|
|
GPIO.setup(self.pin_out,
|
|
GPIO.OUT)
|
|
GPIO.output(self.pin_out,
|
|
0)
|
|
GPIO.setup(self.pin,
|
|
GPIO.IN,
|
|
pull_up_down=GPIO.PUD_UP)
|
|
GPIO.add_event_detect(self.pin,
|
|
GPIO.RISING,
|
|
callback=self.send_if_high,
|
|
bouncetime=1000)
|
|
|
|
def node(self):
|
|
# initialize the node
|
|
rospy.loginfo('launching node')
|
|
rospy.init_node('switch', anonymous=True)
|
|
|
|
def send_message(self):
|
|
# set the message to publish
|
|
msg = node_example()
|
|
|
|
msg.message = self.message
|
|
msg.a = self.a
|
|
msg.b = self.b
|
|
self.pub.publish(msg)
|
|
rospy.loginfo(rospy.get_caller_id() + ' sent message a: %s, b: %s' % (self.a, self.b))
|
|
|
|
def set_output(self):
|
|
GPIO.setup(self.output_pin,
|
|
GPIO.OUT)
|
|
|
|
def send_if_high(self, channel=None):
|
|
# if self.check_IO_value():
|
|
rospy.loginfo(rospy.get_caller_id() + 'Switch is HIGH == released')
|
|
my_msg = msg.String()
|
|
my_msg.data = 'HIGH'
|
|
self.SwitchPub.publish(my_msg)
|
|
|
|
def send_if_low(self, channel=None):
|
|
# if not self.check_IO_value():
|
|
rospy.loginfo(rospy.get_caller_id() + 'Switch is LOW == pressed')
|
|
my_msg = msg.String()
|
|
my_msg.data = 'LOW'
|
|
self.SwitchPub.publish(my_msg)
|
|
|
|
def check_IO_value(self):
|
|
if GPIO.input(self.pin) == GPIO.LOW:
|
|
rospy.loginfo('%s is LOW' % self.pin)
|
|
return False
|
|
elif GPIO.input(self.pin) == GPIO.HIGH:
|
|
rospy.loginfo('%s is HIGH' % self.pin)
|
|
return True
|
|
|
|
def detect_IO_change(self):
|
|
self.is_high_old = self.is_high
|
|
self.is_high = self.check_IO_value()
|
|
# if self.is_high_old and not self.is_high:
|
|
# self.send_if_low()
|
|
# elif not self.is_high_old and self.is_high:
|
|
# self.send_if_high()
|
|
# else:
|
|
# pass
|
|
if self.is_high:
|
|
self.send_if_high()
|
|
else:
|
|
self.send_if_low()
|
|
rospy.sleep(0.3)
|
|
|