diff --git a/sensors/temperature/temperature/temperature.py b/sensors/temperature/temperature/temperature.py index e42ef363..271173da 100644 --- a/sensors/temperature/temperature/temperature.py +++ b/sensors/temperature/temperature/temperature.py @@ -1,3 +1,43 @@ -def main(): - print('Hi from temperature.') +import rclpy +from rclpy.node import Node +from std_msgs.msg import String + +import temperature.temperature_lib + + +class TemperaturePublisher(Node): + + def __init__(self): + super().__init__('temperature_publisher') + self.publisher_ = self.create_publisher(String, 'topic', 10) + timer_period = 0.5 # seconds + self.timer = self.create_timer(timer_period, self.timer_callback) + self.i = 0 + self.Temperature = temperature.temperature_lib.TemperatureModule() + + def timer_callback(self): + msg = String() + #msg.data = 'Hello World: %d' % self.i + msg.data = self.Temperature.get_temperature() + self.publisher_.publish(msg) + self.get_logger().info('Publishing: "%s"' % msg.data) + self.i += 1 + + +def main(args=None): + rclpy.init(args=args) + + temperature_publisher = TemperaturePublisher() + + rclpy.spin(temperature_publisher) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + temperature_publisher.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/sensors/temperature/temperature/temperature_lib.py b/sensors/temperature/temperature/temperature_lib.py new file mode 100644 index 00000000..642fa37e --- /dev/null +++ b/sensors/temperature/temperature/temperature_lib.py @@ -0,0 +1,39 @@ +import smbus +import time +#from MCP342x import MCP342x + +class TemperatureModule: + + def __init__(self): + + # Parameters + # to read temperature from arduino through I2C + self.i2c_adress = 0x08 + + # init of I2C bus communication + self.bus = smbus.SMBus(1) #was 1 before + + #time.sleep(1) + + + def get_temperature(self): + # Sometimes an I/O timeout or error happens, it will run again when the error disappears + try: + #system_temperature = self.bus.read_byte_data(self.i2c_adress, 1) + #print(str(self.bus.read_byte_data(self.i2c_adress, 1))) + #system_temperature = "here is the temperature" + #system_temperature = str(self.bus.read_byte_data(self.i2c_adress, 1)) + + data_received = self.bus.read_i2c_block_data(self.i2c_adress, 0, 18) # Read 16 bytes from address 0 + string_received = ''.join(chr(i) for i in data_received).strip('\x00') + + + + return string_received + + except IOError: + return + + + def shutdown(self): + self.bus.close() \ No newline at end of file