STEVE_Cammast/Steve/Scripts/Switch.py
2021-04-08 13:04:58 +02:00

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)