Added test file for SMACH

.github/workflows/ros2_ci.yml

@@ -0,0 +1,63 @@
+name: ROS2 CI
+
+on:
+  push:
+    branches: [ main, unittest ]
+  pull_request:
+    branches: [ main ]
+
+jobs:
+  build-and-test:
+    runs-on: ubuntu-latest
+
+    container:
+      image: osrf/ros:humble-desktop
+
+    steps:
+    - name: Checkout repository
+      uses: actions/checkout@v3
+
+    - name: Install system dependencies
+      run: |
+        sudo apt update
+        sudo apt install -y curl gnupg lsb-release
+
+    - name: Import ROS2 apt key
+      run: |
+        curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.key | sudo apt-key add -
+
+    - name: Setup sources.list
+      run: |
+        sudo sh -c 'echo "deb [arch=$(dpkg --print-architecture)] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" > /etc/apt/sources.list.d/ros2-latest.list'
+
+    - name: Install ROS2 Humble dependencies
+      run: |
+        sudo apt update
+        sudo apt install -y python3-colcon-common-extensions python3-rosdep python3-vcstool
+
+    - name: Initialize rosdep
+      run: |
+        if [ ! -f /etc/ros/rosdep/sources.list.d/20-default.list ]; then
+          sudo rosdep init
+        fi
+        rosdep update
+
+    - name: Install package dependencies
+      run: |
+        cd robile_safety
+        rosdep install --from-paths . --ignore-src --rosdistro humble -y
+
+    - name: Build the package
+      run: |
+        cd robile_safety
+        source /opt/ros/humble/setup.bash
+        colcon build --symlink-install
+      shell: bash
+
+    - name: Run tests
+      run: |
+        cd robile_safety
+        source /opt/ros/humble/setup.bash
+        colcon test
+        colcon test-result --verbose
+      shell: bash

robile_safety/safety_monitoring_SMACH.py

@@ -63,16 +63,17 @@ def __init__(self, node):
     def execute(self, userdata):
         """Rotates the base for 5 seconds
         """
-        twist = Twist()
-        twist.angular.z = 0.5
-        self.cmd_vel_pub.publish(twist)
+        twist_start = Twist()
+        twist_start.angular.z = 0.5
+        self.cmd_vel_pub.publish(twist_start)
 
         # rotate for 5 seconds
         time.sleep(5)
 
         # stop rotating
-        twist.angular.z = 0.0
-        self.cmd_vel_pub.publish(twist)
+        # twist.angular.z = 0.0
+        twist_stop = Twist()
+        self.cmd_vel_pub.publish(twist_stop)
         return 'rotated'
 
 class Stop(smach.State):

run_tests.py

@@ -0,0 +1,5 @@
+import unittest
+
+if __name__ == "__main__":
+    testsuite = unittest.TestLoader().discover('test', pattern='test_*.py')
+    unittest.TextTestRunner(verbosity=2).run(testsuite)

setup.py

@@ -14,18 +14,18 @@
         ('share/' + package_name, ['package.xml']),
         (os.path.join('share/', package_name, 'launch'), glob('launch/*.py')),
     ],
-    install_requires=['setuptools'],
+    install_requires=['setuptools', 'py_trees', 'rclpy'],
     zip_safe=True,
     maintainer='beelzebub',
     maintainer_email='[email protected]',
     description='TODO: Package description',
     license='TODO: License declaration',
     tests_require=['pytest'],
+    test_suite='test',
     entry_points={
         'console_scripts': [
         'safety_monitoring_bt = robile_safety.safety_monitoring_BT:main',
         'safety_monitoring_smach = robile_safety.safety_monitoring_SMACH:main',
-        'behaviors = robile_safety.behaviors:main',
         ],
     },
 )

test/test_SMACH.py

@@ -0,0 +1,83 @@
+import unittest
+from unittest.mock import patch, MagicMock, call
+import rclpy
+from std_msgs.msg import Float32
+from sensor_msgs.msg import LaserScan
+from geometry_msgs.msg import Twist, Vector3
+from robile_safety.safety_monitoring_SMACH import MonitorBatteryAndCollision, RotateBase, Stop, Idle
+
+class TestSafetyMonitoring(unittest.TestCase):
+
+    @classmethod
+    def setUpClass(cls):
+        rclpy.init()
+
+    @classmethod
+    def tearDownClass(cls):
+        rclpy.shutdown()
+
+    def setUp(self):
+        self.node = rclpy.create_node('test_node')
+
+    def tearDown(self):
+        self.node.destroy_node()
+
+    @patch('robile_safety.safety_monitoring_SMACH.MonitorBatteryAndCollision.execute')
+    def test_monitor_battery_and_collision(self, mock_execute):
+        monitor_state = MonitorBatteryAndCollision(self.node)
+
+        # Set up the mock for the execute method
+        mock_execute.side_effect = [
+            'idle',  
+            'low_battery',  
+            'idle', 
+            'collision_detected'
+        ]
+
+        # Simulate battery callback with a battery level above the threshold
+        battery_msg = Float32(data=25.0)
+        monitor_state.battery_callback(battery_msg)
+        self.assertEqual(mock_execute(None), 'idle')
+
+        # Simulate battery callback with a battery level below the threshold
+        battery_msg.data = 15.0
+        monitor_state.battery_callback(battery_msg)
+        self.assertEqual(mock_execute(None), 'low_battery')
+
+        # Simulate scan callback with no collision detected
+        laser_msg = LaserScan(ranges=[0.6, 0.7, 0.8])
+        monitor_state.scan_callback(laser_msg)
+        self.assertEqual(mock_execute(None), 'idle')
+
+        # Simulate scan callback with a collision detected
+        laser_msg.ranges = [0.4, 0.3, 0.2]
+        monitor_state.scan_callback(laser_msg)
+        self.assertEqual(mock_execute(None), 'collision_detected')
+
+    def test_rotate_base(self):
+        rotate_state = RotateBase(self.node)
+        rotate_state.cmd_vel_pub = MagicMock()  # Mock the publisher
+        rotate_state.execute(None)
+
+        # Create the expected calls
+        expected_calls = [
+            call(Twist(angular=Vector3(z=0.5))),  # Expected start rotation call
+            call(Twist())  # Expected stop rotation call
+        ]
+
+        # Verify that the calls were made with the expected arguments
+        rotate_state.cmd_vel_pub.publish.assert_has_calls(expected_calls, any_order=False)
+
+    def test_stop(self):
+        stop_state = Stop(self.node)
+        stop_state.cmd_vel_pub = MagicMock()  # Set up the mock publisher
+        self.assertEqual(stop_state.execute(None), 'stopped')
+        stop_state.cmd_vel_pub.publish.assert_called_once()
+
+    @patch('time.sleep', return_value=None)
+    def test_idle(self, mock_sleep):
+        idle_state = Idle(self.node)
+        self.assertEqual(idle_state.execute(None), 'idle')
+
+if __name__ == '__main__':
+    unittest.main()