diff --git a/.codacy.yml b/.codacy.yml new file mode 100644 index 0000000..eabcd6d --- /dev/null +++ b/.codacy.yml @@ -0,0 +1,13 @@ +engines: + pylint: + enabled: true + flake8: + enabled: true + config: .flake8 + +patterns: + pylint: + - pylint + flake8: + - flake8 + diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 0000000..c6cab08 --- /dev/null +++ b/.github/workflows/ci.yml @@ -0,0 +1,60 @@ +name: CI + +on: [push, pull_request] + +jobs: + build: + runs-on: ubuntu-latest + + steps: + - name: Checkout repository + uses: actions/checkout@v3 + + - name: Set up Python + uses: actions/setup-python@v3 + with: + python-version: "3.x" + cache: "pip + + - name: Set up ROS 2 Humble + run: | + sudo apt update + sudo apt install -y curl gnupg2 lsb-release + sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add - + sudo sh -c 'echo "deb http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" > /etc/apt/sources.list.d/ros2-latest.list' + sudo apt update + sudo apt install ros-humble-desktop -y + sudo apt install python3-colcon-common-extensions -y + sudo apt install python3-rosdep -y + echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc + source /opt/ros/humble/setup.bash + sudo rosdep init + rosdep update + + - name: Install ROS 2 dependencies + run: | + source /opt/ros/humble/setup.bash + sudo apt install -y ros-humble-executive-smach ros-humble-rclpy ros-humble-geometry-msgs ros-humble-std-msgs ros-humble-sensor-msgs + + - name: Install Python dependencies + run: | + source /opt/ros/humble/setup.bash + python3 -m venv venv + source venv/bin/activate + python -m pip install --upgrade pip + pip install pyyaml # Install pyyaml to handle YAML files + pip install lark-parser # Install lark module for parsing + pip install numpy # Install numpy for scientific computing + pip install -r requirements.txt # Install other dependencies + sudo apt install rclpy + sudo apt install ros-humble-executive-smach + pip install git+https://github.com/splintered-reality/py_trees.git + pip install git+https://github.com/splintered-reality/py_trees_ros.git + + + + - name: Run tests + run: | + source /opt/ros/humble/setup.bash + source venv/bin/activate + pytest tests/ diff --git a/.github/workflows/codacy-analysis.yml b/.github/workflows/codacy-analysis.yml new file mode 100644 index 0000000..f43f757 --- /dev/null +++ b/.github/workflows/codacy-analysis.yml @@ -0,0 +1,100 @@ +name: Codacy Analysis + +on: + pull_request: + +jobs: + codacy-analysis: + runs-on: ubuntu-latest + + steps: + - name: Checkout code + uses: actions/checkout@v3 + + - name: Set up Python + uses: actions/setup-python@v3 + with: + python-version: "3.x" + cache: "pip" + + - name: Setup ROS 2 environment + run: | + sudo apt update + sudo apt install -y curl gnupg2 lsb-release + curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add - + 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' + sudo apt update + sudo apt install -y ros-foxy-desktop # Adjust distribution name as needed + sudo apt install -y python3-rosdep + sudo rosdep init + rosdep update + source /opt/ros/foxy/setup.bash + + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install -r requirements.txt + + - name: Run tests and coverage + run: | + coverage run -m pytest + coverage xml -o coverage.xml + env: + CODACY_PROJECT_TOKEN: ${{ secrets.CODACY_PROJECT_TOKEN }} + + - name: Upload coverage to Codacy + run: python-codacy-coverage -r coverage.xml + env: + CODACY_PROJECT_TOKEN: ${{ secrets.CODACY_PROJECT_TOKEN }} + + +# name: Codacy Analysis + +# on: +# pull_request: + +# jobs: +# codacy-analysis: +# runs-on: ubuntu-latest + +# steps: +# - name: Checkout code +# uses: actions/checkout@v3 + +# - name: Setup ROS 2 Sources List +# run: | +# sudo sh -c 'echo "deb [arch=amd64,arm64] http://packages.ros.org/ros2/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros2-latest.list' +# sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 +# sudo apt-get update + +# - name: Install rosdep +# run: | +# sudo apt-get install -y python3-rosdep +# sudo rosdep init +# rosdep update + +# - name: Install ROS 2 dependencies +# run: | +# rosdep install --from-paths src --ignore-src -r -y + +# - name: Install rclpy +# run: | +# python -m pip install rclpy + +# - name: Install dependencies +# run: | +# python -m pip install --upgrade pip +# pip install -r requirements.txt + +# - name: Run tests and coverage +# run: | +# coverage run -m pytest +# coverage xml -o coverage.xml +# env: +# CODACY_PROJECT_TOKEN: ${{ secrets.CODACY_PROJECT_TOKEN }} + +# - name: Upload coverage to Codacy +# run: python-codacy-coverage -r coverage.xml +# env: +# CODACY_PROJECT_TOKEN: ${{ secrets.CODACY_PROJECT_TOKEN }} + diff --git a/README.md b/README.md index c219347..14116bc 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,23 @@ -# ast_ss24 - repository for Advanced Software technologies, for summer semester 2024, HBRS +# AST_SS24 + +[![Codacy Badge](https://app.codacy.com/project/badge/Grade/456677987e644642b1ceac03558b15ee)](https://app.codacy.com?utm_source=gh&utm_medium=referral&utm_content=&utm_campaign=Badge_grade) +[![CI](https://github.com/zaid3727/ast_ss24/actions/workflows/ci.yml/badge.svg)](https://github.com/zaid3727/ast_ss24/actions/workflows/ci.yml) + +## Overview + +AST_SS24 is a project designed to implement and test behavior trees and state machines in ROS 2 using Python. This repository includes unit tests to ensure the functionality and correctness of the implementation. + +## Table of Contents + +- [Setup](#setup) +- [Continuous Integration](#continuous-integration) +- [Code Quality](#code-quality) +- [Running Tests](#running-tests) + +## Setup + +1. **Clone the Repository:** + + ```sh + git clone https://github.com/zaid3727/ast_ss24.git + cd ast_ss24 diff --git a/Robile b/Robile new file mode 160000 index 0000000..180fe69 --- /dev/null +++ b/Robile @@ -0,0 +1 @@ +Subproject commit 180fe69c50c2f8826010b76456bbb11d1671ae48 diff --git a/executive_smach b/executive_smach new file mode 160000 index 0000000..f14c51a --- /dev/null +++ b/executive_smach @@ -0,0 +1 @@ +Subproject commit f14c51a949519a6532125cc1fa5d026e07e210a0 diff --git a/kelo_tulip b/kelo_tulip new file mode 160000 index 0000000..319c515 --- /dev/null +++ b/kelo_tulip @@ -0,0 +1 @@ +Subproject commit 319c515290f6d5075b0db4dab5e63aedea1e7ed4 diff --git a/py_trees_ros b/py_trees_ros new file mode 160000 index 0000000..247cf47 --- /dev/null +++ b/py_trees_ros @@ -0,0 +1 @@ +Subproject commit 247cf47d509f827bb24464be376e8ed2ddefc0fb diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..c1ed520 --- /dev/null +++ b/requirements.txt @@ -0,0 +1,3 @@ +pytest +py_trees + diff --git a/robile_description b/robile_description new file mode 160000 index 0000000..c776e00 --- /dev/null +++ b/robile_description @@ -0,0 +1 @@ +Subproject commit c776e006d15dfbc8da776720b4624fe7e084cdae diff --git a/robile_gazebo b/robile_gazebo new file mode 160000 index 0000000..4428c17 --- /dev/null +++ b/robile_gazebo @@ -0,0 +1 @@ +Subproject commit 4428c17477195c5739436c0dc62a8bd78df4a07d diff --git a/robile_interfaces b/robile_interfaces new file mode 160000 index 0000000..89823dc --- /dev/null +++ b/robile_interfaces @@ -0,0 +1 @@ +Subproject commit 89823dcb4e454c9b260be213823b97efcec52075 diff --git a/robile_navigation b/robile_navigation new file mode 160000 index 0000000..8a88f53 --- /dev/null +++ b/robile_navigation @@ -0,0 +1 @@ +Subproject commit 8a88f531812a48d3b0da2ef7768a4dc535a5be3b diff --git a/safety_robile/LICENSE b/safety_robile/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/safety_robile/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/safety_robile/package.xml b/safety_robile/package.xml new file mode 100644 index 0000000..17c653a --- /dev/null +++ b/safety_robile/package.xml @@ -0,0 +1,18 @@ + + + + safety_robile + 0.0.0 + TODO: Package description + jay + Apache-2.0 + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/safety_robile/resource/safety_robile b/safety_robile/resource/safety_robile new file mode 100644 index 0000000..e69de29 diff --git a/safety_robile/safety_robile/__init__.py b/safety_robile/safety_robile/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/safety_robile/safety_robile/behaviors.py b/safety_robile/safety_robile/behaviors.py new file mode 100644 index 0000000..aa6da68 --- /dev/null +++ b/safety_robile/safety_robile/behaviors.py @@ -0,0 +1,260 @@ +#!/usr/bin/env python3 + +import rclpy +from geometry_msgs.msg import Twist +from std_msgs.msg import Float32 +from sensor_msgs.msg import LaserScan +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy +import py_trees as pt +import py_trees_ros as ptr + +class rotate(pt.behaviour.Behaviour): + + """ + Rotates the robot about z-axis + """ + + def __init__(self, name="rotate platform", topic_name="/cmd_vel", ang_vel=1.0): + """ + One time setup for the behavior which takes less time to execute, and inherited from the parent class + """ + # inherit all the class variables from the parent class and make it a behavior + super(rotate, self).__init__(name) + + # Set up topic name to publish rotation commands + self.topic_name = topic_name + + # Set up Maximum allowable rotational velocity + self.ang_vel = ang_vel # units: rad/sec + + # Execution checker + self.sent_goal = False + + def setup(self, **kwargs): + """ + Setting up things which generally might require time to prevent delay in tree initialisation + """ + self.logger.info("[ROTATE] setting up rotate behavior") + + self.node = kwargs['node'] + + # Create publisher to publish rotation commands + self.cmd_vel_pub = self.node.create_publisher( + msg_type=Twist, + topic=self.topic_name, + qos_profile=ptr.utilities.qos_profile_latched() + ) + + return True + + def update(self): + """ + Primary function of the behavior is implemented in this method + + Rotating the robot at maximum allowed angular velocity + """ + self.logger.info("[ROTATE] update: updating rotate behavior") + self.logger.debug("%s.update()" % self.__class__.__name__) + + # Send the rotation command to self.topic_name in this method using the message type Twist() + + cmd_vel_msg = Twist() + cmd_vel_msg.angular.z = self.ang_vel + self.cmd_vel_pub.publish(cmd_vel_msg) + + return pt.common.Status.RUNNING + + def terminate(self, new_status): + """ + terminate() is trigerred once the execution of the behavior finishes, + i.e. when the status changes from RUNNING to SUCCESS or FAILURE + """ + self.logger.info("[ROTATE] terminate: publishing zero angular velocity") + twist_msg = Twist() + twist_msg.linear.x = 0. + twist_msg.linear.y = 0. + twist_msg.angular.z = 0. + + self.cmd_vel_pub.publish(twist_msg) + self.sent_goal = False + return super().terminate(new_status) + +class stop_motion(pt.behaviour.Behaviour): + + """ + Stops the robot when it is controlled using joystick or by cmd_vel command + """ + def __init__(self, name: str = "stop platform", topic_name: str = "/cmd_vel"): + """ + One time setup for the behavior which takes less time to execute, and inherited from the parent class + """ + super(stop_motion, self).__init__(name) + + # Set up topic name to publish rotation commands + self.cmd_vel_topic = topic_name + + def setup(self, **kwargs): + """ + Setting up things which generally might require time to prevent delay in tree initialisation + """ + self.logger.info("[STOP MOTION] setting up stop motion behavior") + + try: + self.node = kwargs['node'] + except KeyError as e: + error_message = f"didn't find 'node' in setup's kwargs [{self.qualified_name}]" + raise KeyError(error_message) from e # 'direct cause' traceability + + # Create publisher to publish rotation commands + self.cmd_vel_pub = self.node.create_publisher( + msg_type=Twist, + topic=self.cmd_vel_topic, + qos_profile=ptr.utilities.qos_profile_latched() + ) + + return True + + def update(self): + """ + Primary function of the behavior is implemented in this method + + Stopping the robot by sending zero velocity command to self.cmd_vel_topic + """ + + self.logger.info("[STOP] update: updating stop behavior") + self.logger.debug("%s.update()" % self.__class__.__name__) + + """ + Send the zero rotation command to self.cmd_vel_topic + """ + + cmd_vel_msg = Twist() + cmd_vel_msg.linear.x = 0.0 + cmd_vel_msg.linear.y = 0.0 + cmd_vel_msg.angular.z = 0.0 + self.cmd_vel_pub.publish(cmd_vel_msg) + + return pt.common.Status.SUCCESS + + + def terminate(self, new_status): + """ + terminate() is trigerred once the execution of the behavior finishes, + i.e. when the status changes from RUNNING to SUCCESS or FAILURE + """ + self.logger.info("[ROTATE] terminate: publishing zero angular velocity") + twist_msg = Twist() + twist_msg.linear.x = 0. + twist_msg.linear.y = 0. + twist_msg.angular.z = 0. + + self.cmd_vel_pub.publish(twist_msg) + self.sent_goal = False + return super().terminate(new_status) + +class battery_status2bb(ptr.subscribers.ToBlackboard): + + """ + Checking battery status + """ + def __init__(self, topic_name: str = "/battery_voltage", name: str = 'Battery2BB', threshold: float = 30.0): + """ + One time setup for the behavior which takes less time to execute, and inherited from the parent class + """ + super().__init__(name=name, + topic_name=topic_name, + topic_type=Float32, + blackboard_variables={'battery': 'data'}, + initialise_variables={'battery': 100.0}, + clearing_policy=pt.common.ClearingPolicy.NEVER, # to decide when data should be cleared/reset. + qos_profile=ptr.utilities.qos_profile_unlatched() + ) + self.blackboard.register_key( + key='battery_low_warning', + access=pt.common.Access.WRITE + ) + self.blackboard.battery_low_warning = False # decision making + self.threshold = threshold + + def update(self): + """ + Primary function of the behavior is implemented in this method + + Call the parent to write the raw data to the blackboard and then check against the + threshold to determine if the low warning flag should also be updated. + """ + self.logger.info('[BATTERY] update: running battery_status2bb update') + self.logger.debug("%s.update()" % self.__class__.__name__) + status = super(battery_status2bb, self).update() + + """ + check battery voltage level stored in self.blackboard.battery. By comparing with threshold value, update the value of + self.blackboad.battery_low_warning + """ + if self.blackboard.exists('battery'): + if self.blackboard.battery < self.threshold: + self.blackboard.battery_low_warning = True + else: + self.blackboard.battery_low_warning = False + self.feedback_message = "Battery level is low" if self.blackboard.battery_low_warning else "Battery level is ok" + + return pt.common.Status.SUCCESS + +class laser_scan_2bb(ptr.subscribers.ToBlackboard): + + """ + Checking laser_scan to avoid possible collison + """ + def __init__(self, topic_name: str = "/scan", name: str = 'Scan2BB', safe_range: float = 0.25): + """ + One time setup for the behavior which takes less time to execute, and inherited from the parent class + """ + super().__init__(name=name, + topic_name=topic_name, + topic_type=LaserScan, + blackboard_variables={'laser_scan':'ranges'}, + clearing_policy=pt.common.ClearingPolicy.NEVER, # to decide when data should be cleared/reset. + qos_profile=QoSProfile( + reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT, + history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, + depth=10 + ) + ) + self.blackboard.register_key( + key='collison_warning', + access=pt.common.Access.WRITE + ) + self.blackboard.register_key( + key='point_at_min_dist', + access=pt.common.Access.WRITE + ) + self.blackboard.collison_warning = False # decision making + self.safe_min_range = safe_range + self.blackboard.point_at_min_dist = 0.0 + + def update(self): + """ + Primary function of the behavior is implemented in this method + + Call the parent to write the raw data to the blackboard and then check against the + threshold to set the warning if the robot is close to any obstacle. + """ + self.logger.info("[LASER SCAN] update: running laser_scan_2bb update") + self.logger.debug("%s.update()" % self.__class__.__name__) + status = super(laser_scan_2bb, self).update() + + """ + Based on the closeness of laser scan points (any of them) to robot, update the value of self.blackboard.collison_warning. + Assign the minimum value of laser scan to self.blackboard.point_at_min_dist. + """ + + if self.blackboard.exists('laser_scan'): + if min(self.blackboard.laser_scan) < self.safe_min_range: + self.blackboard.collison_warning = True + self.blackboard.point_at_min_dist = min(self.blackboard.laser_scan) + else: + self.blackboard.collison_warning = False + else: + return pt.common.Status.RUNNING + + return pt.common.Status.SUCCESS diff --git a/safety_robile/safety_robile/safety_robile_BT.py b/safety_robile/safety_robile/safety_robile_BT.py new file mode 100644 index 0000000..0a437e3 --- /dev/null +++ b/safety_robile/safety_robile/safety_robile_BT.py @@ -0,0 +1,127 @@ +#!/usr/bin/env python3 + +import sys +import operator + +import rclpy +import py_trees as pt +import py_trees.console as console +import py_trees_ros as ptr + +from safety_robile.behaviors import * + + +def create_root() -> pt.behaviour.Behaviour: + """ + Method to structure the behavior tree to monitor battery status and start to rotate if battery is low. + As well as, to stop if it detects an obstacle in front of it. + """ + + ## define nodes and behaviors ## + + # define root node + root = pt.composites.Parallel( + name="root", + policy=pt.common.ParallelPolicy.SuccessOnAll( + synchronise=False + ) + ) + + """ + Create a sequence node called "Topics2BB" and a selector node called "Priorities" + """ + topics2BB = pt.composites.Sequence("Topics2BB", memory=False) + priorities = pt.composites.Selector("Priorities", memory=False) + + """ + Using the battery_status2bb class, create a node which subscribes to the topic "/battery_voltage" + and using the laser_scan_2bb class, create a node which subscribes to the topic "/scan" + """ + battery2bb = battery_status2bb(topic_name='/battery_voltage', threshold=20.0) + LaserScan2BB = laser_scan_2bb(topic_name='/scan', safe_range=0.5) + + """ + Create two sequence nodes named "Battery Emergency" and "Collision Checking" + """ + battery_emergency = pt.composites.Sequence("Battery Emergency", memory=False) + collison_emergency = pt.composites.Sequence("Collision Checking", memory=False) + + """ + Using the check_blackboard_variable_value class, create a node called "Battery Low?" which checks if the battery is low, + as well as a node called "Is Colliding?" to check if the robot is about to collide with an obstacle + """ + battery_low = pt.behaviours.CheckBlackboardVariableValue(name="Battery Low?", + check=pt.common.ComparisonExpression(variable="battery_low_warning", + value=True, + operator=operator.eq)) + + + is_colliding = pt.behaviours.CheckBlackboardVariableValue(name="Is Colliding?", + check=pt.common.ComparisonExpression(variable="collison_warning", + value=True, + operator=operator.eq)) + + """ + Using the rotate class, create a node called "Rotate Platform", and using the stop_motion class, create a node called "Stop Platform" + """ + rotate_platform = rotate(name="Rotate Platform") + stop_platform = stop_motion(name="Stop Platform") + + """ + Create a node called "Idle", which is a running node to keep the robot idle + """ + idle = pt.behaviours.Running(name="Idle") + + """ + Construct the behavior tree structure using the nodes and behaviors defined above + """ + root.add_children([topics2BB, priorities]) + topics2BB.add_children([battery2bb, LaserScan2BB]) + priorities.add_children([collison_emergency, battery_emergency, idle]) + battery_emergency.add_children([battery_low, rotate_platform]) + collison_emergency.add_children([is_colliding, stop_platform]) + + return root + + +def main(): + """ + Main function initiates behavior tree construction + """ + rclpy.init(args=None) + # Initialising the node with name "behavior_tree" + root = create_root() + tree = ptr.trees.BehaviourTree( + root=root, + unicode_tree_debug=True + ) + + # setup the tree + try: + tree.setup(timeout=30.0) + except ptr.exceptions.TimedOutError as e: + console.logerror(console.red + "failed to setup the tree, aborting [{}]".format(str(e)) + console.reset) + tree.shutdown() + rclpy.try_shutdown() + sys.exit(1) + except KeyboardInterrupt: + # user-initiated shutdown + console.logerror("tree setup interrupted") + tree.shutdown() + rclpy.try_shutdown() + sys.exit(1) + + # frequency of ticks + tree.tick_tock(period_ms=100) + + try: + rclpy.spin(tree.node) + except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException): + pass + finally: + tree.shutdown() + rclpy.try_shutdown() + + +if __name__ == '__main__': + main() diff --git a/safety_robile/safety_robile/safety_robile_SMACH.py b/safety_robile/safety_robile/safety_robile_SMACH.py new file mode 100644 index 0000000..9f1979f --- /dev/null +++ b/safety_robile/safety_robile/safety_robile_SMACH.py @@ -0,0 +1,141 @@ +#!/usr/bin/env python3 + + +import time + +import rclpy +import smach + +from std_msgs.msg import String +from sensor_msgs.msg import LaserScan +from geometry_msgs.msg import Twist + + +class MonitorBatteryAndCollision(smach.State): + """ + Class to monitor battery level and possible collision + Args: + node: ROS node + battery_threshold (float): threshold for battery level + collision_threshold_distance (float): threshold for distance to obstacle + timeout (float): timeout in seconds + """ + def __init__(self, node, battery_threshold, collision_threshold_distance, timeout=5): + smach.State.__init__(self, outcomes=['low_battery_level', 'possible_collision', 'timeout']) + self.timeout = timeout + self.battery_threshold = battery_threshold + self.collision_threshold_distance = collision_threshold_distance + self.battery_subscriber = node.create_subscription(String, '/battery_voltage', self.battery_callback, 10) + self.laser_scan_subscriber = node.create_subscription(LaserScan, '/scan', self.laser_scan_callback, 10) + time.sleep(0.5) + self.start_time = time.time() + self.node = node + self.logger = self.node.get_logger() + self.battery_low = False + self.collision_possible = False + + def battery_callback(self, msg): + if float(msg.data) < self.battery_threshold: + self.logger.warning('Battery level is low') + self.battery_low = True + else: + self.battery_low = False + + def laser_scan_callback(self, msg): + if min(msg.ranges) < self.collision_threshold_distance: + self.logger.warning('Battery level is low') + self.collision_possible = True + else: + self.collision_possible = False + + def execute(self, ud): + self.logger.info('Executing state MONITOR_BATTERY_AND_COLLISION') + while (time.time() - self.start_time) < self.timeout: + time.sleep(0.1) + rclpy.spin_once(self.node) + + # priority: collision > battery + if self.collision_possible: + return 'possible_collision' + elif self.battery_low: + return 'low_battery_level' + + return 'timeout' + +class RotateBase(smach.State): + """ + Class to rotate the robot base + Args: + node: ROS node + """ + def __init__(self, node): + smach.State.__init__(self, outcomes=['succeeded']) + self.velocity_publisher = node.create_publisher(Twist, '/cmd_vel', 10) + time.sleep(0.2) + self.logger = node.get_logger() + + def execute(self, ud): + self.logger.info('Executing state ROTATE_BASE') + msg = Twist() + msg.angular.z = 0.5 + self.velocity_publisher.publish(msg) + + return 'succeeded' + +class StopBase(smach.State): + """ + Class to stop the robot base + Args: + node: ROS node + """ + def __init__(self, node): + smach.State.__init__(self, outcomes=['succeeded']) + self.velocity_publisher = node.create_publisher(Twist, '/cmd_vel', 10) + time.sleep(0.2) + self.logger = node.get_logger() + + def execute(self, ud): + self.logger.info('Executing state STOP_BASE') + msg = Twist() + msg.linear.x = 0.0 + msg.linear.y = 0.0 + msg.angular.z = 0.0 + self.velocity_publisher.publish(msg) + + return 'succeeded' + +def main(args=None): + """ + main function to execute the state machine + """ + rclpy.init(args=args) + node = rclpy.create_node('safety_features_node') + + sm = smach.StateMachine(['overall_success']) + battery_threshold = 20 + collision_threshold_distance = 0.3 + timeout = 1000 + + with sm: + smach.StateMachine.add('MONITOR_BATTERY_AND_COLLISION', + MonitorBatteryAndCollision(node, battery_threshold, collision_threshold_distance, timeout), + transitions={'low_battery_level':'ROTATE_BASE', 'possible_collision':'STOP_BASE', 'timeout':'overall_success'}) + + smach.StateMachine.add('ROTATE_BASE', + RotateBase(node), + transitions={'succeeded':'MONITOR_BATTERY_AND_COLLISION'}) + + smach.StateMachine.add('STOP_BASE', + StopBase(node), + transitions={'succeeded':'MONITOR_BATTERY_AND_COLLISION'}) + + outcome = sm.execute() + + if outcome == 'overall_success': + node.get_logger().info('State machine executed successfully') + + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/safety_robile/setup.cfg b/safety_robile/setup.cfg new file mode 100644 index 0000000..3cebafc --- /dev/null +++ b/safety_robile/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/safety_robile +[install] +install_scripts=$base/lib/safety_robile diff --git a/safety_robile/setup.py b/safety_robile/setup.py new file mode 100644 index 0000000..defc2f1 --- /dev/null +++ b/safety_robile/setup.py @@ -0,0 +1,29 @@ +from setuptools import find_packages, setup + +package_name = 'safety_robile' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='jay', + maintainer_email='jp52999@gmail.com', + description='TODO: Package description', + license='Apache-2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'safety_robile = safety_robile.safety_robile:main', + 'safety_robile_BT = safety_robile.safety_robile_BT:main', + 'safety_robile_SMACH = safety_robile.safety_robile_SMACH:main', + + ], + }, +) diff --git a/safety_robile/test/test_copyright.py b/safety_robile/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/safety_robile/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/safety_robile/test/test_flake8.py b/safety_robile/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/safety_robile/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/safety_robile/test/test_pep257.py b/safety_robile/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/safety_robile/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' diff --git a/tests/__init__.py b/tests/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/tests/__pycache__/test_behavior_tree.cpython-312.pyc b/tests/__pycache__/test_behavior_tree.cpython-312.pyc new file mode 100644 index 0000000..c4dfa97 Binary files /dev/null and b/tests/__pycache__/test_behavior_tree.cpython-312.pyc differ diff --git a/tests/test_behavior_tree.py b/tests/test_behavior_tree.py new file mode 100644 index 0000000..3ea7859 --- /dev/null +++ b/tests/test_behavior_tree.py @@ -0,0 +1,88 @@ +import unittest +import rclpy +import py_trees as pt +from geometry_msgs.msg import Twist +from std_msgs.msg import Float32 +from sensor_msgs.msg import LaserScan +from safety_robile.behaviors import rotate, stop_motion, battery_status2bb, laser_scan_2bb + +class TestBehaviors(unittest.TestCase): + + def setUp(self): + rclpy.init() + self.node = rclpy.create_node('test_node') + + def tearDown(self): + self.node.destroy_node() + rclpy.shutdown() + + def test_rotate_behavior(self): + rotate_behavior = rotate(topic_name='/cmd_vel', ang_vel=1.0) + rotate_behavior.setup(node=self.node) + self.assertIsInstance(rotate_behavior, rotate) + + def test_stop_motion_behavior(self): + stop_behavior = stop_motion(topic_name='/cmd_vel') + stop_behavior.setup(node=self.node) + self.assertIsInstance(stop_behavior, stop_motion) + + def test_battery_status2bb(self): + battery_behavior = battery_status2bb(topic_name='/battery_voltage', threshold=20.0) + battery_behavior.setup(node=self.node) + bb = pt.blackboard.Blackboard() + bb.set("battery_low_warning", False) + + # Simulate low battery voltage message + battery_msg = Float32() + battery_msg.data = 19.0 + self.node.get_logger().info("Simulating low battery voltage message") + + # Publish the simulated message + battery_publisher = self.node.create_publisher(Float32, '/battery_voltage', 10) + battery_publisher.publish(battery_msg) + + # Check if the battery_low_warning is set + def check_low_battery_warning(): + self.assertTrue(bb.get('battery_low_warning')) + self.assertEqual(bb.get('battery'), 19.0) + + self.node.create_timer(1.0, check_low_battery_warning) + rclpy.spin_once(self.node, timeout_sec=2.0) + + # Simulate normal battery voltage message + battery_msg.data = 21.0 + self.node.get_logger().info("Simulating normal battery voltage message") + battery_publisher.publish(battery_msg) + + # Check if the battery_low_warning is cleared + def check_normal_battery_warning(): + self.assertFalse(bb.get('battery_low_warning')) + self.assertEqual(bb.get('battery'), 21.0) + + self.node.create_timer(1.0, check_normal_battery_warning) + rclpy.spin_once(self.node, timeout_sec=2.0) + + + def test_laser_scan_2bb(self): + laser_behavior = laser_scan_2bb(topic_name='/scan', safe_range=0.5) + laser_behavior.setup(node=self.node) + bb = pt.blackboard.Blackboard() + bb.set("collison_warning", False) + + # Simulate LaserScan message + scan_msg = LaserScan() + scan_msg.ranges = [0.4] * 360 + self.node.get_logger().info("Simulating LaserScan message") + + # Simulate publishing a message + laser_publisher = self.node.create_publisher(LaserScan, '/scan', 10) + laser_publisher.publish(scan_msg) + + def check_msg_received(): + self.assertTrue(bb.get('collison_warning')) + + self.node.create_timer(1.0, check_msg_received) + rclpy.spin_once(self.node, timeout_sec=2.0) + +if __name__ == '__main__': + unittest.main() diff --git a/tests/test_smach.py b/tests/test_smach.py new file mode 100644 index 0000000..68d681d --- /dev/null +++ b/tests/test_smach.py @@ -0,0 +1,63 @@ + +import time +import unittest +import rclpy + +from std_msgs.msg import String +from sensor_msgs.msg import LaserScan +from geometry_msgs.msg import Twist +from safety_robile.safety_robile_SMACH import MonitorBatteryAndCollision, RotateBase, StopBase + +class TestStateMachine(unittest.TestCase): + + def setUp(self): + rclpy.init() + + def tearDown(self): + rclpy.shutdown() + + def test_monitor_battery_and_collision_low_battery(self): + node = rclpy.create_node('test_node') + state = MonitorBatteryAndCollision(node, battery_threshold=50, collision_threshold_distance=1, timeout=5) + + low_battery_msg = String() + low_battery_msg.data = '20.0' # Battery level is 20% + state.battery_callback(low_battery_msg) + + # Run the state + outcome = state.execute(None) + + self.assertEqual(outcome, 'low_battery_level') + + def test_monitor_battery_and_collision_possible_collision(self): + node = rclpy.create_node('test_node') + state = MonitorBatteryAndCollision(node, battery_threshold=50, collision_threshold_distance=1, timeout=5) + + collision_msg = LaserScan() + collision_msg.ranges = [1.4, 1.6, 1.7] + state.laser_scan_callback(collision_msg) + + outcome = state.execute(None) + + self.assertEqual(outcome, 'possible_collision') + + def test_rotate_base(self): + node = rclpy.create_node('test_node') + state = RotateBase(node) + + # Run the state + outcome = state.execute(None) + + self.assertEqual(outcome, 'succeeded') + + def test_stop_base(self): + node = rclpy.create_node('test_node') + state = StopBase(node) + + # Run the state + outcome = state.execute(None) + + self.assertEqual(outcome, 'succeeded') + +if __name__ == '__main__': + unittest.main()