yikestone/ros_arduino_bridge
1
0
Fork 0
mirror of https://github.com/YikeStone/ros_arduino_bridge.git synced 2025-08-03 19:24:09 +05:30
Code Issues Projects Releases Wiki Activity

Conditionalized sensor publisher on rate != 0

Browse Source
This commit is contained in:
Patrick Goebel 2016-01-09 08:26:30 -08:00
parent fb24de4223
commit fc3a4509fe
1 changed files with 45 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
ros_arduino_python/src/ros_arduino_python/arduino_sensors.py
View File

@ -90,14 +90,15 @@ class AnalogSensor(Sensor):
self.msg.header.frame_id = self.frame_id self.msg.header.frame_id = self.frame_id
# Create the publisher # Create the publisher
self.pub = rospy.Publisher("~sensor/" + self.name, Analog, queue_size=5) if self.rate != 0:
self.pub = rospy.Publisher("~sensor/" + self.name, Analog, queue_size=5)
if self.direction == "output": if self.direction == "output":
self.device.pin_mode(self.pin, OUTPUT) self.device.analog_pin_mode(self.pin, OUTPUT)
# Create the write service # Create the write service
rospy.Service('~' + self.name + '/write', AnalogSensorWrite, self.sensor_write_handler) rospy.Service('~' + self.name + '/write', AnalogSensorWrite, self.sensor_write_handler)
else: else:
self.device.pin_mode(self.pin, INPUT) self.device.analog_pin_mode(self.pin, INPUT)
# Create the read service # Create the read service
rospy.Service('~' + self.name + '/read', AnalogSensorRead, self.sensor_read_handler) rospy.Service('~' + self.name + '/read', AnalogSensorRead, self.sensor_read_handler)
@ -110,11 +111,12 @@ class AnalogSensor(Sensor):
return self.device.analog_write(self.pin, value) return self.device.analog_write(self.pin, value)
def sensor_read_handler(self, req=None): def sensor_read_handler(self, req=None):
value = self.read_value() self.value = self.read_value()
return AnalogSensorReadResponse(value) return AnalogSensorReadResponse(self.value)
def sensor_write_handler(self, req): def sensor_write_handler(self, req):
self.write_value(req.value) self.write_value(req.value)
self.value = req.value
return AnalogSensorWriteResponse() return AnalogSensorWriteResponse()
class AnalogFloatSensor(Sensor): class AnalogFloatSensor(Sensor):
@ -127,14 +129,15 @@ class AnalogFloatSensor(Sensor):
self.msg.header.frame_id = self.frame_id self.msg.header.frame_id = self.frame_id
# Create the publisher # Create the publisher
self.pub = rospy.Publisher("~sensor/" + self.name, AnalogFloat, queue_size=5) if self.rate != 0:
self.pub = rospy.Publisher("~sensor/" + self.name, AnalogFloat, queue_size=5)
if self.direction == "output": if self.direction == "output":
self.device.pin_mode(self.pin, OUTPUT) self.device.analog_pin_mode(self.pin, OUTPUT)
# Create the write service # Create the write service
rospy.Service('~' + self.name + '/write', AnalogFloatSensorWrite, self.sensor_write_handler) rospy.Service('~' + self.name + '/write', AnalogFloatSensorWrite, self.sensor_write_handler)
else: else:
self.device.pin_mode(self.pin, INPUT) self.device.analog_pin_mode(self.pin, INPUT)
# Create the read service # Create the read service
rospy.Service('~' + self.name + '/read', AnalogFloatSensorRead, self.sensor_read_handler) rospy.Service('~' + self.name + '/read', AnalogFloatSensorRead, self.sensor_read_handler)
@ -147,11 +150,12 @@ class AnalogFloatSensor(Sensor):
return self.device.analog_write(self.pin, value) return self.device.analog_write(self.pin, value)
def sensor_read_handler(self, req=None): def sensor_read_handler(self, req=None):
value = self.read_value() self.value = self.read_value()
return AnalogFloatSensorReadResponse(value) return AnalogFloatSensorReadResponse(self.value)
def sensor_write_handler(self, req): def sensor_write_handler(self, req):
self.write_value(req.value) self.write_value(req.value)
self.value = req.value
return AnalogFloatSensorWriteResponse() return AnalogFloatSensorWriteResponse()
class DigitalSensor(Sensor): class DigitalSensor(Sensor):
@ -163,34 +167,44 @@ class DigitalSensor(Sensor):
self.msg = Digital() self.msg = Digital()
self.msg.header.frame_id = self.frame_id self.msg.header.frame_id = self.frame_id
# Create the publisher # Create the publisher
self.pub = rospy.Publisher("~sensor/" + self.name, Digital, queue_size=5) if self.rate != 0:
self.pub = rospy.Publisher("~sensor/" + self.name, Digital, queue_size=5)
# Get the initial state
self.value = self.read_value()
if self.direction == "output": if self.direction == "output":
self.device.pin_mode(self.pin, OUTPUT) self.device.digital_pin_mode(self.pin, OUTPUT)
# Create the write service # Create the write service
rospy.Service('~' + self.name + '/write', DigitalSensorWrite, self.sensor_write_handler) rospy.Service('~' + self.name + '/write', DigitalSensorWrite, self.sensor_write_handler)
else: else:
self.device.pin_mode(self.pin, INPUT) self.device.digital_pin_mode(self.pin, INPUT)
# Create the read service
rospy.Service('~' + self.name + '/read', DigitalSensorRead, self.sensor_read_handler) # Create the read service
rospy.Service('~' + self.name + '/read', DigitalSensorRead, self.sensor_read_handler)
self.value = LOW
def read_value(self): def read_value(self):
return self.device.digital_read(self.pin) return self.device.digital_read(self.pin)
def write_value(self): def write_value(self, value=None):
# Alternate HIGH/LOW when writing at a fixed rate # Alternate HIGH/LOW when publishing at a fixed rate
self.value = not self.value if self.rate != 0:
self.value = not self.value
else:
self.value = value
return self.device.digital_write(self.pin, self.value) return self.device.digital_write(self.pin, self.value)
def sensor_read_handler(self, req=None): def sensor_read_handler(self, req=None):
value = self.read_value() self.value = self.read_value()
return DigitalSensorReadResponse(value) return DigitalSensorReadResponse(self.value)
def sensor_write_handler(self, req): def sensor_write_handler(self, req):
self.write_value(req.value) self.write_value(req.value)
self.value = req.value
return DigitalSensorWriteResponse() return DigitalSensorWriteResponse()
class RangeSensor(Sensor): class RangeSensor(Sensor):
@ -202,11 +216,16 @@ class RangeSensor(Sensor):
self.msg = Range() self.msg = Range()
self.msg.header.frame_id = self.frame_id self.msg.header.frame_id = self.frame_id
# Create the publisher # Create the publisher
self.pub = rospy.Publisher("~sensor/" + self.name, Range, queue_size=5) if self.rate != 0:
self.pub = rospy.Publisher("~sensor/" + self.name, Range, queue_size=5)
def read_value(self): # Create the read service
self.msg.header.stamp = rospy.Time.now() rospy.Service('~' + self.name + '/read', AnalogFloatSensorRead, self.sensor_read_handler)
def sensor_read_handler(self, req=None):
self.value = self.read_value()
return AnalogFloatSensorReadResponse(self.value)
class SonarSensor(RangeSensor): class SonarSensor(RangeSensor):
def __init__(self, *args, **kwargs): def __init__(self, *args, **kwargs):