From 06251ee04b4ef2b16e87fbcfb8867f0f9fcd2ac9 Mon Sep 17 00:00:00 2001 From: RoseKapps Date: Sun, 12 Nov 2023 20:35:27 +0100 Subject: [PATCH] added basic temperature node --- sensors/temperature/setup.py | 2 +- sensors/temperature/temperature/__init__.py | 0 .../temperature/temperature/temperature.py | 43 ------------ .../temperature/temperature_lib.py | 50 +++++--------- .../temperature/temperature_publisher_node.py | 65 +++++++++++++++++++ 5 files changed, 83 insertions(+), 77 deletions(-) mode change 100644 => 100755 sensors/temperature/temperature/__init__.py delete mode 100644 sensors/temperature/temperature/temperature.py mode change 100644 => 100755 sensors/temperature/temperature/temperature_lib.py create mode 100755 sensors/temperature/temperature/temperature_publisher_node.py diff --git a/sensors/temperature/setup.py b/sensors/temperature/setup.py index 6a4523e5..2338e8a3 100644 --- a/sensors/temperature/setup.py +++ b/sensors/temperature/setup.py @@ -20,7 +20,7 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - "temperature_publisher = temperature.temperature:main" + "temperature_publisher_node = temperature.temperature_publisher_node:main" ], }, ) diff --git a/sensors/temperature/temperature/__init__.py b/sensors/temperature/temperature/__init__.py old mode 100644 new mode 100755 diff --git a/sensors/temperature/temperature/temperature.py b/sensors/temperature/temperature/temperature.py deleted file mode 100644 index 271173da..00000000 --- a/sensors/temperature/temperature/temperature.py +++ /dev/null @@ -1,43 +0,0 @@ -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 old mode 100644 new mode 100755 index 103212c4..b1bd9632 --- a/sensors/temperature/temperature/temperature_lib.py +++ b/sensors/temperature/temperature/temperature_lib.py @@ -1,48 +1,32 @@ import smbus import time -#from MCP342x import MCP342x +import struct class TemperatureModule: - def __init__(self): - - # Parameters # to read temperature from arduino through I2C - self.i2c_address = 0x08 + self.i2c_address = 0x22 # 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)) - - - #for i in range(100): - # data_received = self.bus.read_i2c_block_data(self.i2c_address, i, 16) - # print(f"Data received from register {i}: {data_received}") - - - data_received = self.bus.read_i2c_block_data(self.i2c_address, 0, 11) # Read 16 bytes from address 0 - - #read_i2c_block_data(i2c_addr, register, length, force=None) --> what register?? + self.bus = smbus.SMBus(1) + time.sleep(1) - print(data_received) + def get_data(self): + try: + # Define the number of floats and the corresponding byte length + num_floats = 6 + byte_length = num_floats * 4 # Each float is 4 bytes - string_received = ''.join(chr(i) for i in data_received).strip('\x00') + # Read a block of bytes from the I2C device + data = self.bus.read_i2c_block_data(self.i2c_address, 0, byte_length) - return string_received + # Convert the byte list to a list of floats + floats = struct.unpack('<' + 'f' * num_floats, bytes(data[:byte_length])) - except IOError: - return - + return floats + except Exception as e: + print(f"Error: {e}") + return None def shutdown(self): self.bus.close() \ No newline at end of file diff --git a/sensors/temperature/temperature/temperature_publisher_node.py b/sensors/temperature/temperature/temperature_publisher_node.py new file mode 100755 index 00000000..ba4cfc6c --- /dev/null +++ b/sensors/temperature/temperature/temperature_publisher_node.py @@ -0,0 +1,65 @@ +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float32 # Import the Float32MultiArray message type + +import temperature.temperature_lib # Import your custom temperature library + +class TemperaturePublisher(Node): + def __init__(self): + super().__init__("temperature_publisher") + # Create a publisher that publishes Float32MultiArray messages on the 'temperature_data' topic + self.publisher_ESC1_ = self.create_publisher(Float32, "/asv/temperature/ESC1", 10) + self.publisher_ESC2_ = self.create_publisher(Float32, "/asv/temperature/ESC2", 10) + self.publisher_ESC3_ = self.create_publisher(Float32, "/asv/temperature/ESC3", 10) + self.publisher_ESC4_ = self.create_publisher(Float32, "/asv/temperature/ESC4", 10) + self.publisher_ambient1_ = self.create_publisher(Float32, "/asv/temperature/ambient1", 10) + self.publisher_ambient2_ = self.create_publisher(Float32, "/asv/temperature/ambient2", 10) + + # Create a timer that calls the timer_callback method every 1.0 seconds + timer_period = 1.0 + self.timer = self.create_timer(timer_period, self.timer_callback) + + # Initialize the TemperatureModule to interact with the temperature sensor + self.TemperatureObjcet = temperature.temperature_lib.TemperatureModule() + + def timer_callback(self): + # Get the temperature data from the sensor + temperature_data_array = self.TemperatureObjcet.get_data() + if temperature_data_array is not None: + # If data is received, create a Float32 messages + msg_ESC1 = Float32() + msg_ESC2 = Float32() + msg_ESC3 = Float32() + msg_ESC4 = Float32() + msg_ambient1 = Float32() + msg_ambient2 = Float32() + + # Assign the received temperature data to the correct message + msg_ESC1.data = temperature_data_array[0] + msg_ESC2.data = temperature_data_array[1] + msg_ESC3.data = temperature_data_array[2] + msg_ESC4.data = temperature_data_array[3] + msg_ambient1.data = temperature_data_array[5] # Inverse because of conections + msg_ambient2.data = temperature_data_array[4] # Inverse because of conections + + # Publish the messages + self.publisher_ESC1_.publish(msg_ESC1) + self.publisher_ESC2_.publish(msg_ESC2) + self.publisher_ESC3_.publish(msg_ESC3) + self.publisher_ESC4_.publish(msg_ESC4) + self.publisher_ambient1_.publish(msg_ambient1) + self.publisher_ambient2_.publish(msg_ambient2) + + # Log the published data for debugging purposes + self.get_logger().info(f"Temperature: {temperature_data_array}") + +def main(args=None): + rclpy.init(args=args) # Initialize the ROS2 Python client library + temperature_publisher = TemperaturePublisher() # Create the TemperaturePublisher node + rclpy.spin(temperature_publisher) # Spin the node so the callback function is called + # After shutdown, destroy the node and shutdown rclpy + temperature_publisher.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() # Run the main function if the script is executed