diff --git a/sensors/temperature/launch/temperature_system_launch.py b/sensors/temperature/launch/temperature_system_launch.py new file mode 100644 index 00000000..524594ac --- /dev/null +++ b/sensors/temperature/launch/temperature_system_launch.py @@ -0,0 +1,10 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='temperature', + executable='temperature_publisher_node' + ) + ]) \ No newline at end of file diff --git a/sensors/temperature/package.xml b/sensors/temperature/package.xml new file mode 100644 index 00000000..5809885c --- /dev/null +++ b/sensors/temperature/package.xml @@ -0,0 +1,22 @@ + + + + temperature + 1.0.0 + TODO: Package description + vortex + MIT + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + std_msgs + freya_bms + ros2launch + + + ament_python + + diff --git a/sensors/temperature/resource/temperature b/sensors/temperature/resource/temperature new file mode 100644 index 00000000..e69de29b diff --git a/sensors/temperature/setup.cfg b/sensors/temperature/setup.cfg new file mode 100644 index 00000000..f79969f5 --- /dev/null +++ b/sensors/temperature/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/temperature +[install] +install_scripts=$base/lib/temperature diff --git a/sensors/temperature/setup.py b/sensors/temperature/setup.py new file mode 100644 index 00000000..8d1e3930 --- /dev/null +++ b/sensors/temperature/setup.py @@ -0,0 +1,29 @@ +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = 'temperature' + +setup( + name=package_name, + version='1.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name), glob('launch/*.py')), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='vortex', + maintainer_email='karate.martynas1@gmail.com', + description='Temperature sensor data gartehring from temperature sensor system', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + "temperature_publisher_node = temperature.temperature_publisher_node:main" + ], + }, +) diff --git a/sensors/temperature/temperature/__init__.py b/sensors/temperature/temperature/__init__.py new file mode 100755 index 00000000..e69de29b diff --git a/sensors/temperature/temperature/temperature_lib.py b/sensors/temperature/temperature/temperature_lib.py new file mode 100755 index 00000000..b1bd9632 --- /dev/null +++ b/sensors/temperature/temperature/temperature_lib.py @@ -0,0 +1,32 @@ +import smbus +import time +import struct + +class TemperatureModule: + def __init__(self): + # to read temperature from arduino through I2C + self.i2c_address = 0x22 + + # init of I2C bus communication + self.bus = smbus.SMBus(1) + time.sleep(1) + + 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 + + # Read a block of bytes from the I2C device + data = self.bus.read_i2c_block_data(self.i2c_address, 0, byte_length) + + # Convert the byte list to a list of floats + floats = struct.unpack('<' + 'f' * num_floats, bytes(data[:byte_length])) + + 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 diff --git a/sensors/temperature/test/test_copyright.py b/sensors/temperature/test/test_copyright.py new file mode 100644 index 00000000..97a39196 --- /dev/null +++ b/sensors/temperature/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/sensors/temperature/test/test_flake8.py b/sensors/temperature/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/sensors/temperature/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/sensors/temperature/test/test_pep257.py b/sensors/temperature/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/sensors/temperature/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings'