From c5279cc3cd5d1dbbf7dd3fd424478fe68501d285 Mon Sep 17 00:00:00 2001 From: jp-droid Date: Fri, 28 Jun 2024 13:21:44 +0200 Subject: [PATCH 01/48] Behaviour tree and State machine implementation --- Robile | 1 + executive_smach | 1 + kelo_tulip | 1 + py_trees_ros | 1 + robile_description | 1 + robile_gazebo | 1 + robile_interfaces | 1 + robile_navigation | 1 + safety_robile/LICENSE | 202 ++++++++++++++ safety_robile/package.xml | 18 ++ safety_robile/resource/safety_robile | 0 safety_robile/safety_robile/__init__.py | 0 safety_robile/safety_robile/behaviors.py | 264 ++++++++++++++++++ .../safety_robile/safety_robile_BT.py | 129 +++++++++ .../safety_robile/safety_robile_SMACH.py | 138 +++++++++ safety_robile/setup.cfg | 4 + safety_robile/setup.py | 29 ++ safety_robile/test/test_copyright.py | 25 ++ safety_robile/test/test_flake8.py | 25 ++ safety_robile/test/test_pep257.py | 23 ++ 20 files changed, 865 insertions(+) create mode 160000 Robile create mode 160000 executive_smach create mode 160000 kelo_tulip create mode 160000 py_trees_ros create mode 160000 robile_description create mode 160000 robile_gazebo create mode 160000 robile_interfaces create mode 160000 robile_navigation create mode 100644 safety_robile/LICENSE create mode 100644 safety_robile/package.xml create mode 100644 safety_robile/resource/safety_robile create mode 100644 safety_robile/safety_robile/__init__.py create mode 100644 safety_robile/safety_robile/behaviors.py create mode 100644 safety_robile/safety_robile/safety_robile_BT.py create mode 100644 safety_robile/safety_robile/safety_robile_SMACH.py create mode 100644 safety_robile/setup.cfg create mode 100644 safety_robile/setup.py create mode 100644 safety_robile/test/test_copyright.py create mode 100644 safety_robile/test/test_flake8.py create mode 100644 safety_robile/test/test_pep257.py 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/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..1042dbc --- /dev/null +++ b/safety_robile/safety_robile/behaviors.py @@ -0,0 +1,264 @@ +#!/usr/bin/env python3 + +import py_trees as pt +import py_trees_ros as ptr +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 + +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") + + try: + self.node = kwargs['node'] + except KeyError as e: + error_message = "didn't find 'node' in setup's kwargs [{}][{}]".format(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.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 = "didn't find 'node' in setup's kwargs [{}][{}]".format(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..858f9c3 --- /dev/null +++ b/safety_robile/safety_robile/safety_robile_BT.py @@ -0,0 +1,129 @@ +#!/usr/bin/env python3 + +import py_trees as pt +import py_trees_ros as ptr +import operator + +import py_trees.console as console +import rclpy +import sys +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 behvior 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..8fc98cb --- /dev/null +++ b/safety_robile/safety_robile/safety_robile_SMACH.py @@ -0,0 +1,138 @@ +#!/usr/bin/env python3 + +import rclpy +import smach + +from std_msgs.msg import String +from sensor_msgs.msg import LaserScan +from geometry_msgs.msg import Twist + +import time + +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() 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' From 49521d185e165fa66956ab7d469c43b33ec9a5e8 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Sun, 30 Jun 2024 01:02:26 +0200 Subject: [PATCH 02/48] tests --- tests/__init__.py | 0 .../test_behavior_tree.cpython-312.pyc | Bin 0 -> 2283 bytes tests/test_behavior_tree.py | 88 ++++++++++++++++++ tests/test_smach.py | 68 ++++++++++++++ 4 files changed, 156 insertions(+) create mode 100644 tests/__init__.py create mode 100644 tests/__pycache__/test_behavior_tree.cpython-312.pyc create mode 100644 tests/test_behavior_tree.py create mode 100644 tests/test_smach.py 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 0000000000000000000000000000000000000000..c4dfa973c5abcbb0f7b5adcae62e9a76e612b686 GIT binary patch literal 2283 zcmd^A&2Jk;6rb5$J7eej!}%adNkgG=Me70qi4qzpLNtej14gKps?o+fY1UcWVP;Hf zD~KQkksMG#9Fj;#xyA|-{2yHECXu5poH+FsB|Y`Tn_b&6#(dlv$#3V)y!p+W`TgGb zm-hBBQVoBxkk%3U-E%4?Q2CzSht3wVkcAzT@V_{rfv-7w+DI4}Y3KuF>FdZc7)tqw z^+;$RuvJS0S&J1|nm~y6Xb57v{PB9bUvV(X<2;TApD2GWirv;EXQpJ7XR|U{zXDgB zvrEv~0@wt$5G07#Q9`#gu!c%55wHxf!F(VZbi3kA$XjgD{LIesIA<(bMNwcH{8Y+k z<6yRstj$%ETSc@smPb)M7uC30;2ILJir6&-a~6b(8GH6dY%!Z=v4Uw^v3Ea;(-jdj zg``5fIvJCYhVU$%uVh0N#I@zT8xTx>yjckCA8r6Z6+N+_ZNW%cQ`xO+d_ED)dwC4My+8V;Z zweY`T$y#t;*Rg?HnE0)8|}6iRy_;KN@noOd%MTGh!wqq?t)F-U1V@wk+LH zg`^yJLlyNvx7AwqtRY;p(z{HE>lPZm;6qBbo}yb4i)w ztZ?ZsL^p{FDWp%ptf6DlRwkn*GP-T;+B^1-JwJ^UN6OLZQgr%|yyMLdmdN0C-|o=P z(C&qu3w!;CcxgNl^v;`7 zGM=YSc7?8(JY#3>#fyY@!(ul)@0iJ@`K-w;H+<{7uR}5V`UpuoxkR&|7dPw+BF=N5 zgnLXDHLiH$BVdG5+1Qw`gD#KZx_t{5n~R_>q(gzsfPun~3~b%~?ru49wG_E}5Q%Ms zk4evij?IpT126AchvYRN`$1?kR3^hEGJHrzmEXD9xgFl^*y(t9dFo*B?IUvYf6RGT zGV3_D$YyGU^I^DJKCUjgo~dq+DO*r)fd$eij5nvMKBn(KaTVR0^b7N!xO($o@Wv5& z%j0S;+Ts#Q)k{OE8>Td!wQ`Q?w^6EIA%BLeH<%A9^MW!jfhi&n!IK|X#p)T=5&dA+ z(W!<51I&qzF@B6XAEU@` x=*6c-FP{4LetBS`G%)cLfpc;N8EqSdNBYpQ(Y<#2>)9`7zxwb*N5*p=_8(f#$RGd! literal 0 HcmV?d00001 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..188e270 --- /dev/null +++ b/tests/test_smach.py @@ -0,0 +1,68 @@ +import unittest +import rclpy +import time +from std_msgs.msg import String +from sensor_msgs.msg import LaserScan +from geometry_msgs.msg import Twist +from safety_monitoring_SMACH import MonitorBatteryAndCollision, RotateBase, StopBase + +class TestStateMachine(unittest.TestCase): + # def __init__(self): + # self.setUp() + + 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=90, collision_threshold_distance=1, timeout=5) + + # Simulate low battery by publishing a message + low_battery_msg = String() + low_battery_msg.data = '15.0' # Set a value below the threshold + 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=90, collision_threshold_distance=1, timeout=5) + + # Simulate a possible collision by publishing a LaserScan message + collision_msg = LaserScan() + collision_msg.ranges = [0.4, 0.6, 0.7] # Set a value below the threshold + state.laser_scan_callback(collision_msg) + + # Run the state + 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() + # test_obj = TestStateMachine() + # test_obj.test_monitor_battery_and_collision_low_battery() From 4f127d377f26066824e55123ef35b5cae20f0abe Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 15:10:39 +0200 Subject: [PATCH 03/48] -- --- .codacy.yml | 13 ++++++++ .github/workflows/ci.yml | 47 +++++++++++++++++++++++++++ .github/workflows/codacy-analysis.yml | 20 ++++++++++++ README.md | 25 ++++++++++++-- requirements.txt | 9 +++++ tests/test_smach.py | 8 ++--- 6 files changed, 116 insertions(+), 6 deletions(-) create mode 100644 .codacy.yml create mode 100644 .github/workflows/ci.yml create mode 100644 .github/workflows/codacy-analysis.yml create mode 100644 requirements.txt 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..9831e89 --- /dev/null +++ b/.github/workflows/ci.yml @@ -0,0 +1,47 @@ +name: CI + +on: [push, pull_request] + +jobs: + build: + runs-on: ubuntu-latest + + steps: + - name: Checkout repository + uses: actions/checkout@v3 + + - 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 -y ros-humble-desktop + echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc + source ~/.bashrc + + - name: Set up Python environment + run: | + python3 -m venv venv + source venv/bin/activate + python -m pip install --upgrade pip + pip install pytest + + - 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 + source venv/bin/activate + pip install -r requirements.txt + + - 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..5b59a4f --- /dev/null +++ b/.github/workflows/codacy-analysis.yml @@ -0,0 +1,20 @@ +name: Codacy Analysis + +on: [pull_request] + +jobs: + codacy-analysis: + runs-on: ubuntu-latest + steps: + - name: Checkout code + uses: actions/checkout@v2 + + - name: Run Codacy Analysis + run: | + pip install codacy-coverage + coverage run -m pytest + coverage xml + 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/requirements.txt b/requirements.txt new file mode 100644 index 0000000..f0efae9 --- /dev/null +++ b/requirements.txt @@ -0,0 +1,9 @@ +pytest +py_trees +py_trees_ros +rclpy +geometry_msgs +std_msgs +sensor_msgs +smach + diff --git a/tests/test_smach.py b/tests/test_smach.py index 188e270..dd8c61d 100644 --- a/tests/test_smach.py +++ b/tests/test_smach.py @@ -4,7 +4,7 @@ from std_msgs.msg import String from sensor_msgs.msg import LaserScan from geometry_msgs.msg import Twist -from safety_monitoring_SMACH import MonitorBatteryAndCollision, RotateBase, StopBase +from safety_robile.safety_robile_SMACH import MonitorBatteryAndCollision, RotateBase, StopBase class TestStateMachine(unittest.TestCase): # def __init__(self): @@ -18,11 +18,11 @@ def tearDown(self): def test_monitor_battery_and_collision_low_battery(self): node = rclpy.create_node('test_node') - state = MonitorBatteryAndCollision(node, battery_threshold=90, collision_threshold_distance=1, timeout=5) + state = MonitorBatteryAndCollision(node, battery_threshold=50, collision_threshold_distance=1, timeout=5) # Simulate low battery by publishing a message low_battery_msg = String() - low_battery_msg.data = '15.0' # Set a value below the threshold + low_battery_msg.data = '80.0' # Set a value below the threshold state.battery_callback(low_battery_msg) # Run the state @@ -32,7 +32,7 @@ def test_monitor_battery_and_collision_low_battery(self): def test_monitor_battery_and_collision_possible_collision(self): node = rclpy.create_node('test_node') - state = MonitorBatteryAndCollision(node, battery_threshold=90, collision_threshold_distance=1, timeout=5) + state = MonitorBatteryAndCollision(node, battery_threshold=50, collision_threshold_distance=1, timeout=5) # Simulate a possible collision by publishing a LaserScan message collision_msg = LaserScan() From 2b38013cd8189cafde96ff4394f27921608f8943 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 15:17:46 +0200 Subject: [PATCH 04/48] ci.yml update --- .github/workflows/ci.yml | 74 +++++++++++++++++++--------------------- 1 file changed, 35 insertions(+), 39 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 9831e89..77a07ef 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -1,47 +1,43 @@ name: CI -on: [push, pull_request] +on: + push: + branches: + - main + pull_request: + branches: + - main jobs: build: runs-on: ubuntu-latest steps: - - name: Checkout repository - uses: actions/checkout@v3 - - - 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 -y ros-humble-desktop - echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc - source ~/.bashrc - - - name: Set up Python environment - run: | - python3 -m venv venv - source venv/bin/activate - python -m pip install --upgrade pip - pip install pytest - - - 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 - source venv/bin/activate - pip install -r requirements.txt - - - name: Run tests - run: | - source /opt/ros/humble/setup.bash - source venv/bin/activate - pytest tests/ - + - name: Checkout repository + uses: actions/checkout@v3 + + - name: Set up ROS 2 Humble + run: | + sudo apt update && sudo apt install -y curl + curl -s 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 -y ros-humble-desktop + + - name: Set up Python 3.10 + uses: actions/setup-python@v4 + with: + python-version: 3.10 + + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install -r requirements.txt + + - name: Run tests + run: | + source /opt/ros/humble/setup.bash + source ~/AST_ws/install/setup.bash + cd ~/AST_ws/src/ast_ss24/tests + python3 test_behavior_tree.py + python3 test_smach.py From 3fdf8666f0220c3dc1ec9453e820a0f76081322f Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 15:31:23 +0200 Subject: [PATCH 05/48] - --- requirements.txt | 3 +++ 1 file changed, 3 insertions(+) diff --git a/requirements.txt b/requirements.txt index f0efae9..e475db2 100644 --- a/requirements.txt +++ b/requirements.txt @@ -6,4 +6,7 @@ geometry_msgs std_msgs sensor_msgs smach +pytest +py_trees +# py_trees_ros (Commented out if not available) From aa2d47500658f63f072c0810e5fc4f0f6023122b Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 15:36:13 +0200 Subject: [PATCH 06/48] pp --- .github/workflows/codacy-analysis.yml | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/.github/workflows/codacy-analysis.yml b/.github/workflows/codacy-analysis.yml index 5b59a4f..66585b2 100644 --- a/.github/workflows/codacy-analysis.yml +++ b/.github/workflows/codacy-analysis.yml @@ -5,16 +5,29 @@ on: [pull_request] jobs: codacy-analysis: runs-on: ubuntu-latest + steps: - name: Checkout code - uses: actions/checkout@v2 + uses: actions/checkout@v3 + + - name: Set up Python 3.10 + uses: actions/setup-python@v4 + with: + python-version: 3.10 + + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install -r requirements.txt - - name: Run Codacy Analysis + - name: Run tests and coverage run: | pip install codacy-coverage coverage run -m pytest - coverage xml + coverage xml -o coverage.xml + + - name: Upload coverage to Codacy + run: | python-codacy-coverage -r coverage.xml env: CODACY_PROJECT_TOKEN: ${{ secrets.CODACY_PROJECT_TOKEN }} - From 9098736252e204a41442d6d050e3d441a38ce979 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 15:40:40 +0200 Subject: [PATCH 07/48] update ci.yml --- .github/workflows/ci.yml | 45 +++++++++++++++++++--------------------- 1 file changed, 21 insertions(+), 24 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 77a07ef..d330ad7 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -13,31 +13,28 @@ jobs: runs-on: ubuntu-latest steps: - - name: Checkout repository - uses: actions/checkout@v3 + - name: Checkout code + uses: actions/checkout@v3 - - name: Set up ROS 2 Humble - run: | - sudo apt update && sudo apt install -y curl - curl -s 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 -y ros-humble-desktop + - name: Set up Python 3.10 + uses: actions/setup-python@v4 + with: + python-version: '3.10' - - name: Set up Python 3.10 - uses: actions/setup-python@v4 - with: - python-version: 3.10 + - name: Install dependencies + run: | + python -m pip install --upgrade pip + pip install -r requirements.txt - - name: Install dependencies - run: | - python -m pip install --upgrade pip - pip install -r requirements.txt + - name: Run tests + run: | + pytest - - name: Run tests - run: | - source /opt/ros/humble/setup.bash - source ~/AST_ws/install/setup.bash - cd ~/AST_ws/src/ast_ss24/tests - python3 test_behavior_tree.py - python3 test_smach.py + - name: Run Codacy Analysis + run: | + pip install codacy-coverage + coverage run -m pytest + coverage xml + python-codacy-coverage -r coverage.xml + env: + CODACY_PROJECT_TOKEN: ${{ secrets.CODACY_PROJECT_TOKEN }} From 0d096be5ca7282334ba6575b33ae92084565348f Mon Sep 17 00:00:00 2001 From: jp-droid Date: Tue, 2 Jul 2024 15:42:23 +0200 Subject: [PATCH 08/48] Commented exception KeyError --- safety_robile/safety_robile/behaviors.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/safety_robile/safety_robile/behaviors.py b/safety_robile/safety_robile/behaviors.py index 1042dbc..84abc45 100644 --- a/safety_robile/safety_robile/behaviors.py +++ b/safety_robile/safety_robile/behaviors.py @@ -36,11 +36,11 @@ def setup(self, **kwargs): """ self.logger.info("[ROTATE] setting up rotate behavior") - try: - self.node = kwargs['node'] - except KeyError as e: - error_message = "didn't find 'node' in setup's kwargs [{}][{}]".format(self.qualified_name) - raise KeyError(error_message) from e # 'direct cause' traceability + # try: + self.node = kwargs['node'] + # except KeyError as e: + # error_message = "didn't find 'node' in setup's kwargs [{}][{}]".format(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( From df69fa8a388802e1fd5f7e90afe03ac0e022d235 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 15:57:38 +0200 Subject: [PATCH 09/48] - --- .github/workflows/ci.yml | 46 ++++++++++++++++++++++++++++------------ requirements.txt | 8 +++---- 2 files changed, 36 insertions(+), 18 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index d330ad7..5196f38 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -13,28 +13,46 @@ jobs: runs-on: ubuntu-latest steps: - - name: Checkout code + - name: Checkout repository uses: actions/checkout@v3 - - name: Set up Python 3.10 - uses: actions/setup-python@v4 - with: - python-version: '3.10' + - 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 -y ros-humble-desktop + echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc + source ~/.bashrc - - name: Install dependencies + - name: Set up Python environment run: | + python3 -m venv venv + source venv/bin/activate python -m pip install --upgrade pip + pip install pytest + + - 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 + source venv/bin/activate pip install -r requirements.txt - name: Run tests run: | - pytest + source /opt/ros/humble/setup.bash + source venv/bin/activate + pytest tests/ - - name: Run Codacy Analysis + - name: Debug information run: | - pip install codacy-coverage - coverage run -m pytest - coverage xml - python-codacy-coverage -r coverage.xml - env: - CODACY_PROJECT_TOKEN: ${{ secrets.CODACY_PROJECT_TOKEN }} + git config --global --add safe.directory /github/workspace + git status + ls -la diff --git a/requirements.txt b/requirements.txt index e475db2..0ac2a0a 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,12 +1,12 @@ +### Updated `requirements.txt` + +```plaintext pytest py_trees -py_trees_ros +# py_trees_ros (Commented out if not available) rclpy geometry_msgs std_msgs sensor_msgs smach -pytest -py_trees -# py_trees_ros (Commented out if not available) From 5b89b55314596c716ac3efd7c37bcded7c0d9e60 Mon Sep 17 00:00:00 2001 From: Mohammed Zaid Nidgundi <68942072+zaid3727@users.noreply.github.com> Date: Tue, 2 Jul 2024 16:07:27 +0200 Subject: [PATCH 10/48] Update ci.yml --- .github/workflows/ci.yml | 28 ++++++++-------------------- 1 file changed, 8 insertions(+), 20 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 5196f38..26d2e2a 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -1,12 +1,6 @@ name: CI -on: - push: - branches: - - main - pull_request: - branches: - - main +on: [push, pull_request] jobs: build: @@ -16,6 +10,11 @@ jobs: - name: Checkout repository uses: actions/checkout@v3 + - name: Set up Python 3.10 + uses: actions/setup-python@v2 + with: + python-version: 3.10 + - name: Set up ROS 2 Humble run: | sudo apt update @@ -27,13 +26,6 @@ jobs: echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc source ~/.bashrc - - name: Set up Python environment - run: | - python3 -m venv venv - source venv/bin/activate - python -m pip install --upgrade pip - pip install pytest - - name: Install ROS 2 dependencies run: | source /opt/ros/humble/setup.bash @@ -42,7 +34,9 @@ jobs: - 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 -r requirements.txt - name: Run tests @@ -50,9 +44,3 @@ jobs: source /opt/ros/humble/setup.bash source venv/bin/activate pytest tests/ - - - name: Debug information - run: | - git config --global --add safe.directory /github/workspace - git status - ls -la From c9d203cb56feb3209a1b9e8de0403b940cbc7973 Mon Sep 17 00:00:00 2001 From: Mohammed Zaid Nidgundi <68942072+zaid3727@users.noreply.github.com> Date: Tue, 2 Jul 2024 16:18:22 +0200 Subject: [PATCH 11/48] Update README.md --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 14116bc..4213516 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,7 @@ # 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 From 15821384880bc05a277be4707c40942a17019bb3 Mon Sep 17 00:00:00 2001 From: Mohammed Zaid Nidgundi <68942072+zaid3727@users.noreply.github.com> Date: Tue, 2 Jul 2024 16:18:43 +0200 Subject: [PATCH 12/48] Update README.md --- README.md | 1 - 1 file changed, 1 deletion(-) diff --git a/README.md b/README.md index 4213516..14116bc 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,6 @@ # 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 From 573e82285e08e42ff93e8ce92a975bacf685fa10 Mon Sep 17 00:00:00 2001 From: Mohammed Zaid Nidgundi <68942072+zaid3727@users.noreply.github.com> Date: Tue, 2 Jul 2024 16:36:29 +0200 Subject: [PATCH 13/48] Update ci.yml --- .github/workflows/ci.yml | 28 ++++++++++++++++++++-------- 1 file changed, 20 insertions(+), 8 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 26d2e2a..5196f38 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -1,6 +1,12 @@ name: CI -on: [push, pull_request] +on: + push: + branches: + - main + pull_request: + branches: + - main jobs: build: @@ -10,11 +16,6 @@ jobs: - name: Checkout repository uses: actions/checkout@v3 - - name: Set up Python 3.10 - uses: actions/setup-python@v2 - with: - python-version: 3.10 - - name: Set up ROS 2 Humble run: | sudo apt update @@ -26,6 +27,13 @@ jobs: echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc source ~/.bashrc + - name: Set up Python environment + run: | + python3 -m venv venv + source venv/bin/activate + python -m pip install --upgrade pip + pip install pytest + - name: Install ROS 2 dependencies run: | source /opt/ros/humble/setup.bash @@ -34,9 +42,7 @@ jobs: - 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 -r requirements.txt - name: Run tests @@ -44,3 +50,9 @@ jobs: source /opt/ros/humble/setup.bash source venv/bin/activate pytest tests/ + + - name: Debug information + run: | + git config --global --add safe.directory /github/workspace + git status + ls -la From 7edd83c98cd6fd196fd18a134d8ae5dcf4ff7219 Mon Sep 17 00:00:00 2001 From: Mohammed Zaid Nidgundi <68942072+zaid3727@users.noreply.github.com> Date: Tue, 2 Jul 2024 16:48:59 +0200 Subject: [PATCH 14/48] Update requirements.txt --- requirements.txt | 3 --- 1 file changed, 3 deletions(-) diff --git a/requirements.txt b/requirements.txt index 0ac2a0a..ae86f69 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,6 +1,3 @@ -### Updated `requirements.txt` - -```plaintext pytest py_trees # py_trees_ros (Commented out if not available) From 345278080fbb635faadb6765ceb114ed12def0b6 Mon Sep 17 00:00:00 2001 From: Mohammed Zaid Nidgundi <68942072+zaid3727@users.noreply.github.com> Date: Tue, 2 Jul 2024 16:56:05 +0200 Subject: [PATCH 15/48] Update ci.yml --- .github/workflows/ci.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 5196f38..1c02a71 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -44,6 +44,7 @@ jobs: source /opt/ros/humble/setup.bash source venv/bin/activate pip install -r requirements.txt + pip install pyyaml - name: Run tests run: | From 7e6507de7499b33fcb046dcf2472a54ac6fc72c5 Mon Sep 17 00:00:00 2001 From: Mohammed Zaid Nidgundi <68942072+zaid3727@users.noreply.github.com> Date: Tue, 2 Jul 2024 17:01:27 +0200 Subject: [PATCH 16/48] Update ci.yml --- .github/workflows/ci.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 1c02a71..e06f68f 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -45,6 +45,7 @@ jobs: source venv/bin/activate pip install -r requirements.txt pip install pyyaml + pip install lark-parser - name: Run tests run: | From 6f94025580c05837d848383f3dc39caab0937106 Mon Sep 17 00:00:00 2001 From: Mohammed Zaid Nidgundi <68942072+zaid3727@users.noreply.github.com> Date: Tue, 2 Jul 2024 17:10:09 +0200 Subject: [PATCH 17/48] Update ci.yml --- .github/workflows/ci.yml | 36 +++++++++++++++--------------------- 1 file changed, 15 insertions(+), 21 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index e06f68f..a4bc3db 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -1,12 +1,6 @@ name: CI -on: - push: - branches: - - main - pull_request: - branches: - - main +on: [push, pull_request] jobs: build: @@ -27,12 +21,10 @@ jobs: echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc source ~/.bashrc - - name: Set up Python environment - run: | - python3 -m venv venv - source venv/bin/activate - python -m pip install --upgrade pip - pip install pytest + - name: Install Python 3.10 + uses: actions/setup-python@v2 + with: + python-version: 3.10 - name: Install ROS 2 dependencies run: | @@ -42,19 +34,21 @@ jobs: - 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 -r requirements.txt - pip install pyyaml - pip install lark-parser + 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 + + - name: Build and source ROS 2 packages + run: | + source /opt/ros/humble/setup.bash + colcon build --symlink-install - name: Run tests run: | source /opt/ros/humble/setup.bash source venv/bin/activate pytest tests/ - - - name: Debug information - run: | - git config --global --add safe.directory /github/workspace - git status - ls -la From a80b342d0344aa5a4dc160a64c91513068d4f6a8 Mon Sep 17 00:00:00 2001 From: Mohammed Zaid Nidgundi <68942072+zaid3727@users.noreply.github.com> Date: Tue, 2 Jul 2024 17:16:16 +0200 Subject: [PATCH 18/48] Update ci.yml --- .github/workflows/ci.yml | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index a4bc3db..875bc5d 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -10,6 +10,11 @@ jobs: - name: Checkout repository uses: actions/checkout@v3 + - name: Set up Python 3.10 + uses: actions/setup-python@v2 + with: + python-version: 3.10 + - name: Set up ROS 2 Humble run: | sudo apt update @@ -21,11 +26,6 @@ jobs: echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc source ~/.bashrc - - name: Install Python 3.10 - uses: actions/setup-python@v2 - with: - python-version: 3.10 - - name: Install ROS 2 dependencies run: | source /opt/ros/humble/setup.bash @@ -42,11 +42,6 @@ jobs: pip install lark-parser # Install lark module for parsing pip install numpy # Install numpy for scientific computing - - name: Build and source ROS 2 packages - run: | - source /opt/ros/humble/setup.bash - colcon build --symlink-install - - name: Run tests run: | source /opt/ros/humble/setup.bash From 3b3e87d49c0d7d7ec2edb36e01a76f52eced2751 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 17:47:37 +0200 Subject: [PATCH 19/48] Commented Keyerror throw --- requirements.txt | 2 +- safety_robile/safety_robile/behaviors.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/requirements.txt b/requirements.txt index 0ac2a0a..9aa0b29 100644 --- a/requirements.txt +++ b/requirements.txt @@ -3,7 +3,7 @@ ```plaintext pytest py_trees -# py_trees_ros (Commented out if not available) +py_trees_ros (Commented out if not available) rclpy geometry_msgs std_msgs diff --git a/safety_robile/safety_robile/behaviors.py b/safety_robile/safety_robile/behaviors.py index 84abc45..8e5c792 100644 --- a/safety_robile/safety_robile/behaviors.py +++ b/safety_robile/safety_robile/behaviors.py @@ -106,7 +106,7 @@ def setup(self, **kwargs): try: self.node = kwargs['node'] except KeyError as e: - error_message = "didn't find 'node' in setup's kwargs [{}][{}]".format(self.qualified_name) + 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 From 9e6c42c2b6352432bf14cd87328f16f0caff4f10 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 18:07:48 +0200 Subject: [PATCH 20/48] PEP8 required changes --- safety_robile/safety_robile/behaviors.py | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/safety_robile/safety_robile/behaviors.py b/safety_robile/safety_robile/behaviors.py index 8e5c792..aa6da68 100644 --- a/safety_robile/safety_robile/behaviors.py +++ b/safety_robile/safety_robile/behaviors.py @@ -1,12 +1,12 @@ #!/usr/bin/env python3 -import py_trees as pt -import py_trees_ros as ptr 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): @@ -25,7 +25,7 @@ def __init__(self, name="rotate platform", topic_name="/cmd_vel", ang_vel=1.0): self.topic_name = topic_name # Set up Maximum allowable rotational velocity - self.ang_vel = ang_vel # units: rad/sec + self.ang_vel = ang_vel # units: rad/sec # Execution checker self.sent_goal = False @@ -36,11 +36,7 @@ def setup(self, **kwargs): """ self.logger.info("[ROTATE] setting up rotate behavior") - # try: self.node = kwargs['node'] - # except KeyError as e: - # error_message = "didn't find 'node' in setup's kwargs [{}][{}]".format(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( @@ -88,7 +84,7 @@ 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"): + 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 """ @@ -161,7 +157,7 @@ class battery_status2bb(ptr.subscribers.ToBlackboard): """ Checking battery status """ - def __init__(self, topic_name: str="/battery_voltage", name: str = 'Battery2BB', threshold: float=30.0): + 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 """ @@ -209,7 +205,7 @@ 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): + 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 """ From 121eb9b31df57fcde487430f95833a392f370a20 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 18:16:41 +0200 Subject: [PATCH 21/48] Remaining PEP8standard required changes --- safety_robile/safety_robile/safety_robile_BT.py | 16 +++++++--------- .../safety_robile/safety_robile_SMACH.py | 6 ++++-- tests/test_smach.py | 4 +++- 3 files changed, 14 insertions(+), 12 deletions(-) diff --git a/safety_robile/safety_robile/safety_robile_BT.py b/safety_robile/safety_robile/safety_robile_BT.py index 858f9c3..884a6e4 100644 --- a/safety_robile/safety_robile/safety_robile_BT.py +++ b/safety_robile/safety_robile/safety_robile_BT.py @@ -1,12 +1,13 @@ #!/usr/bin/env python3 -import py_trees as pt -import py_trees_ros as ptr +import sys import operator +import py_trees as pt import py_trees.console as console +import py_trees_ros as ptr import rclpy -import sys + from safety_robile.behaviors import * @@ -69,24 +70,20 @@ def create_root() -> pt.behaviour.Behaviour: """ Create a node called "Idle", which is a running node to keep the robot idle """ - idle = pt.behaviours.Running(name="Idle") """ - construct the behvior tree structure using the nodes and behaviors defined above + 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 @@ -125,5 +122,6 @@ def main(): 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 index 8fc98cb..00e105d 100644 --- a/safety_robile/safety_robile/safety_robile_SMACH.py +++ b/safety_robile/safety_robile/safety_robile_SMACH.py @@ -1,5 +1,8 @@ #!/usr/bin/env python3 + +import time + import rclpy import smach @@ -7,7 +10,6 @@ from sensor_msgs.msg import LaserScan from geometry_msgs.msg import Twist -import time class MonitorBatteryAndCollision(smach.State): """ @@ -134,5 +136,5 @@ def main(args=None): rclpy.shutdown() -if __name__ == "__main__": +if __name__ == '__main__': main() diff --git a/tests/test_smach.py b/tests/test_smach.py index dd8c61d..f015c05 100644 --- a/tests/test_smach.py +++ b/tests/test_smach.py @@ -1,6 +1,8 @@ + +import time import unittest import rclpy -import time + from std_msgs.msg import String from sensor_msgs.msg import LaserScan from geometry_msgs.msg import Twist From bd47c3b355b0e837bca1843fa1b5a2fab50eda1a Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 18:24:49 +0200 Subject: [PATCH 22/48] minor changes --- safety_robile/safety_robile/safety_robile_BT.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/safety_robile/safety_robile/safety_robile_BT.py b/safety_robile/safety_robile/safety_robile_BT.py index 884a6e4..0a437e3 100644 --- a/safety_robile/safety_robile/safety_robile_BT.py +++ b/safety_robile/safety_robile/safety_robile_BT.py @@ -3,10 +3,10 @@ import sys import operator +import rclpy import py_trees as pt import py_trees.console as console import py_trees_ros as ptr -import rclpy from safety_robile.behaviors import * From 0e502f85a1dce537fbf291e2b7d99af320daed24 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 18:30:45 +0200 Subject: [PATCH 23/48] minor changes PEP8 standards --- safety_robile/safety_robile/safety_robile_SMACH.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/safety_robile/safety_robile/safety_robile_SMACH.py b/safety_robile/safety_robile/safety_robile_SMACH.py index 00e105d..9f1979f 100644 --- a/safety_robile/safety_robile/safety_robile_SMACH.py +++ b/safety_robile/safety_robile/safety_robile_SMACH.py @@ -136,5 +136,6 @@ def main(args=None): rclpy.shutdown() + if __name__ == '__main__': - main() + main() \ No newline at end of file From df34a48af59837c75b549a48e2e52aa076732be3 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 18:36:13 +0200 Subject: [PATCH 24/48] yaml changes --- .github/workflows/codacy-analysis.yml | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/.github/workflows/codacy-analysis.yml b/.github/workflows/codacy-analysis.yml index 66585b2..11c89d3 100644 --- a/.github/workflows/codacy-analysis.yml +++ b/.github/workflows/codacy-analysis.yml @@ -10,10 +10,11 @@ jobs: - name: Checkout code uses: actions/checkout@v3 - - name: Set up Python 3.10 - uses: actions/setup-python@v4 + - name: Set up Python + uses: actions/setup-python@v3 with: - python-version: 3.10 + python-version: "3.x" + cache: "pip" - name: Install dependencies run: | From eac1e1cfda71a910bdb378900e6c971e3a69dede Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 18:37:37 +0200 Subject: [PATCH 25/48] requirementchanges --- requirements.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/requirements.txt b/requirements.txt index bf5bcf6..f0efae9 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,6 +1,6 @@ pytest py_trees -py_trees_ros (Commented out if not available) +py_trees_ros rclpy geometry_msgs std_msgs From 2419c232ea502f252d5470ab7071094249cc3cc1 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 18:41:59 +0200 Subject: [PATCH 26/48] codacy-analysis --- .github/workflows/codacy-analysis.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/codacy-analysis.yml b/.github/workflows/codacy-analysis.yml index 11c89d3..ab7dda6 100644 --- a/.github/workflows/codacy-analysis.yml +++ b/.github/workflows/codacy-analysis.yml @@ -20,6 +20,8 @@ jobs: run: | python -m pip install --upgrade pip pip install -r requirements.txt + 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 and coverage run: | From a07abce525b524ae6e00cad213beca2d59698652 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 18:44:01 +0200 Subject: [PATCH 27/48] trial --- .github/workflows/ci.yml | 4 +++- .github/workflows/codacy-analysis.yml | 3 +-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 875bc5d..622b020 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -37,10 +37,12 @@ jobs: python3 -m venv venv source venv/bin/activate python -m pip install --upgrade pip - pip install -r requirements.txt 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 git+https://github.com/splintered-reality/py_trees.git + pip install git+https://github.com/splintered-reality/py_trees_ros.git + pip install -r requirements.txt - name: Run tests run: | diff --git a/.github/workflows/codacy-analysis.yml b/.github/workflows/codacy-analysis.yml index ab7dda6..5ff6ece 100644 --- a/.github/workflows/codacy-analysis.yml +++ b/.github/workflows/codacy-analysis.yml @@ -20,8 +20,7 @@ jobs: run: | python -m pip install --upgrade pip pip install -r requirements.txt - 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 and coverage run: | From b29c8cd7be05ddaf35664ebc99f85271d5df53d7 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 18:44:56 +0200 Subject: [PATCH 28/48] changes --- requirements.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/requirements.txt b/requirements.txt index f0efae9..045c164 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,6 +1,6 @@ pytest py_trees -py_trees_ros +# py_trees_ros rclpy geometry_msgs std_msgs From 6581f4a9d591c35c14d975c5cacdfe3b71963f19 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 18:47:15 +0200 Subject: [PATCH 29/48] try --- .github/workflows/ci.yml | 3 ++- requirements.txt | 1 - 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 622b020..2f5cefc 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -40,9 +40,10 @@ jobs: 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 pip install git+https://github.com/splintered-reality/py_trees.git pip install git+https://github.com/splintered-reality/py_trees_ros.git - pip install -r requirements.txt + - name: Run tests run: | diff --git a/requirements.txt b/requirements.txt index 045c164..b7a26cd 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,6 +1,5 @@ pytest py_trees -# py_trees_ros rclpy geometry_msgs std_msgs From d0e41f5d344e32a0e227b10e12aad2892906043f Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 18:49:34 +0200 Subject: [PATCH 30/48] python version change --- .github/workflows/ci.yml | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 2f5cefc..3ac3787 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -10,10 +10,11 @@ jobs: - name: Checkout repository uses: actions/checkout@v3 - - name: Set up Python 3.10 - uses: actions/setup-python@v2 + - name: Set up Python + uses: actions/setup-python@v3 with: - python-version: 3.10 + python-version: "3.x" + cache: "pip - name: Set up ROS 2 Humble run: | From d978174b7db300d9c5ff6b6b909947ebbace2819 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 18:52:12 +0200 Subject: [PATCH 31/48] rclpy changes trial --- .github/workflows/ci.yml | 1 + requirements.txt | 1 - 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 3ac3787..a36d168 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -42,6 +42,7 @@ jobs: pip install lark-parser # Install lark module for parsing pip install numpy # Install numpy for scientific computing pip install -r requirements.txt + sudo apt install rclpy pip install git+https://github.com/splintered-reality/py_trees.git pip install git+https://github.com/splintered-reality/py_trees_ros.git diff --git a/requirements.txt b/requirements.txt index b7a26cd..35030f0 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,6 +1,5 @@ pytest py_trees -rclpy geometry_msgs std_msgs sensor_msgs From 611171d5485607ad2037d1aa37eb746f89834a70 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 18:53:55 +0200 Subject: [PATCH 32/48] yaml changes --- .github/workflows/ci.yml | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index a36d168..20f046d 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -41,8 +41,11 @@ jobs: 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 sudo apt install rclpy + sudo apt install geometry_msgs + sudo apt install std_msgs + sudo apt install sensor_msgs + 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 From 5ce137843a548a80fd9f6715574425577ea9c56e Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 18:55:31 +0200 Subject: [PATCH 33/48] changes --- .github/workflows/ci.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 20f046d..3dcc946 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -41,8 +41,9 @@ jobs: 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 sudo apt install rclpy - sudo apt install geometry_msgs + pip install geometry_msgs sudo apt install std_msgs sudo apt install sensor_msgs sudo apt install ros-humble-executive-smach From 65c6d94b27ba62d55ebb136b859976909d5e23f2 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 18:56:21 +0200 Subject: [PATCH 34/48] trial --- .github/workflows/ci.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 3dcc946..a189016 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -41,7 +41,6 @@ jobs: 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 sudo apt install rclpy pip install geometry_msgs sudo apt install std_msgs From a36ab8cdf11e3f0f7a74ecf02181113db8d9a7b1 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 19:00:08 +0200 Subject: [PATCH 35/48] humble yaml changes --- .github/workflows/ci.yml | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index a189016..b461f4b 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -23,9 +23,13 @@ jobs: 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 -y ros-humble-desktop + 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 ~/.bashrc + source /opt/ros/humble/setup.bash + sudo rosdep init + rosdep update - name: Install ROS 2 dependencies run: | From 1bc34f093ae1e8a754373febc22bf76c97f5fa84 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 19:02:20 +0200 Subject: [PATCH 36/48] other changes --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index b461f4b..4db4c2f 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -45,8 +45,8 @@ jobs: 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 - pip install geometry_msgs sudo apt install std_msgs sudo apt install sensor_msgs sudo apt install ros-humble-executive-smach From fb00004d272c6c4478d00eccf95f0d88698c6735 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 19:04:54 +0200 Subject: [PATCH 37/48] trial --- requirements.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/requirements.txt b/requirements.txt index 35030f0..a7f8239 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,6 +1,5 @@ pytest py_trees -geometry_msgs std_msgs sensor_msgs smach From 183458e649996e5b2abdd6e18854c29b3e841e40 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 19:08:40 +0200 Subject: [PATCH 38/48] std_msgs and geom msgs resolved --- .github/workflows/ci.yml | 2 -- requirements.txt | 2 -- 2 files changed, 4 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 4db4c2f..fc23fbe 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -47,8 +47,6 @@ jobs: pip install numpy # Install numpy for scientific computing pip install -r requirements.txt # Install other dependencies sudo apt install rclpy - sudo apt install std_msgs - sudo apt install sensor_msgs 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 diff --git a/requirements.txt b/requirements.txt index a7f8239..7f94eb4 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,6 +1,4 @@ pytest py_trees -std_msgs -sensor_msgs smach From 6d3741ff54ff2f6fd251e0cde66cdf46fa34bb70 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 19:09:37 +0200 Subject: [PATCH 39/48] smach changes --- requirements.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/requirements.txt b/requirements.txt index 7f94eb4..c1ed520 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,4 +1,3 @@ pytest py_trees -smach From f24338aa7a37664178a43baf0a69903c57565ff3 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 19:12:07 +0200 Subject: [PATCH 40/48] run change --- .github/workflows/ci.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index fc23fbe..537dda7 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -51,6 +51,8 @@ jobs: pip install git+https://github.com/splintered-reality/py_trees.git pip install git+https://github.com/splintered-reality/py_trees_ros.git + - name: Install coverage + run: pip install coverage - name: Run tests run: | From d7447515bfcc631ba83ef62bb2e1afe82802285d Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 19:14:10 +0200 Subject: [PATCH 41/48] debug --- .github/workflows/ci.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 537dda7..7dbfeca 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -54,6 +54,10 @@ jobs: - name: Install coverage run: pip install coverage + - name: Debug PATH + run: echo $PATH + + - name: Run tests run: | source /opt/ros/humble/setup.bash From 6f72ce5327743ed60892bd6ffe99d6d1057fbcdc Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 19:16:30 +0200 Subject: [PATCH 42/48] commit_pip --- .github/workflows/ci.yml | 5 ----- 1 file changed, 5 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 7dbfeca..c6cab08 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -51,11 +51,6 @@ jobs: pip install git+https://github.com/splintered-reality/py_trees.git pip install git+https://github.com/splintered-reality/py_trees_ros.git - - name: Install coverage - run: pip install coverage - - - name: Debug PATH - run: echo $PATH - name: Run tests From 30ee6cfa60495e96a8d2a9d07f823d0fafe49554 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 19:21:29 +0200 Subject: [PATCH 43/48] analysis-coverage --- .github/workflows/codacy-analysis.yml | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/.github/workflows/codacy-analysis.yml b/.github/workflows/codacy-analysis.yml index 5ff6ece..b47fbad 100644 --- a/.github/workflows/codacy-analysis.yml +++ b/.github/workflows/codacy-analysis.yml @@ -1,6 +1,7 @@ name: Codacy Analysis -on: [pull_request] +on: + pull_request: jobs: codacy-analysis: @@ -16,20 +17,23 @@ jobs: python-version: "3.x" cache: "pip" - - name: Install dependencies + - name: Install dependencies and coverage run: | python -m pip install --upgrade pip pip install -r requirements.txt + pip install coverage + - name: Debug PATH + run: echo $PATH - name: Run tests and coverage run: | - pip install codacy-coverage 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 + run: python-codacy-coverage -r coverage.xml env: CODACY_PROJECT_TOKEN: ${{ secrets.CODACY_PROJECT_TOKEN }} From 697108baee692fd970e1093f9beee704eafd2613 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 19:24:38 +0200 Subject: [PATCH 44/48] codacy_changes --- .github/workflows/codacy-analysis.yml | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/.github/workflows/codacy-analysis.yml b/.github/workflows/codacy-analysis.yml index b47fbad..9a81e9c 100644 --- a/.github/workflows/codacy-analysis.yml +++ b/.github/workflows/codacy-analysis.yml @@ -17,11 +17,21 @@ jobs: python-version: "3.x" cache: "pip" - - name: Install dependencies and coverage + - name: Install ROS dependencies + run: | + sudo apt-get update + sudo apt-get install -y python3-rosdep + rosdep update + 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 - pip install coverage - name: Debug PATH run: echo $PATH From 01b54d5cf0e81b736366e326b6ab107ae56ccfa0 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 19:28:34 +0200 Subject: [PATCH 45/48] humble changes --- .github/workflows/codacy-analysis.yml | 70 +++++++++++++++++++++++---- 1 file changed, 60 insertions(+), 10 deletions(-) diff --git a/.github/workflows/codacy-analysis.yml b/.github/workflows/codacy-analysis.yml index 9a81e9c..57c1729 100644 --- a/.github/workflows/codacy-analysis.yml +++ b/.github/workflows/codacy-analysis.yml @@ -1,3 +1,53 @@ +# 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: Install ROS dependencies +# run: | +# sudo apt-get update +# sudo apt-get install -y python3-rosdep +# rosdep update +# 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: Debug PATH +# run: echo $PATH + +# - 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: @@ -11,17 +61,20 @@ jobs: - name: Checkout code uses: actions/checkout@v3 - - name: Set up Python - uses: actions/setup-python@v3 - with: - python-version: "3.x" - cache: "pip" - - - name: Install ROS dependencies + - 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 @@ -33,9 +86,6 @@ jobs: python -m pip install --upgrade pip pip install -r requirements.txt - - name: Debug PATH - run: echo $PATH - - name: Run tests and coverage run: | coverage run -m pytest From bc2690b9b8a88e01c89feba04e460779a2e13a06 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 19:31:04 +0200 Subject: [PATCH 46/48] changes --- .github/workflows/codacy-analysis.yml | 103 +++++++++++++------------- 1 file changed, 52 insertions(+), 51 deletions(-) diff --git a/.github/workflows/codacy-analysis.yml b/.github/workflows/codacy-analysis.yml index 57c1729..ecba0d4 100644 --- a/.github/workflows/codacy-analysis.yml +++ b/.github/workflows/codacy-analysis.yml @@ -1,53 +1,3 @@ -# 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: Install ROS dependencies -# run: | -# sudo apt-get update -# sudo apt-get install -y python3-rosdep -# rosdep update -# 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: Debug PATH -# run: echo $PATH - -# - 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: @@ -75,7 +25,7 @@ jobs: - name: Install ROS 2 dependencies run: | - rosdep install --from-paths src --ignore-src -r -y + rosdep install --from-paths . --ignore-src -r -y - name: Install rclpy run: | @@ -97,3 +47,54 @@ jobs: 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 }} + From fec619d6383208c03e1eebca506359ad4c9dc1c9 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 19:33:02 +0200 Subject: [PATCH 47/48] rclpy --- .github/workflows/codacy-analysis.yml | 30 +++++++++++++-------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/.github/workflows/codacy-analysis.yml b/.github/workflows/codacy-analysis.yml index ecba0d4..f43f757 100644 --- a/.github/workflows/codacy-analysis.yml +++ b/.github/workflows/codacy-analysis.yml @@ -11,25 +11,24 @@ jobs: - 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: Set up Python + uses: actions/setup-python@v3 + with: + python-version: "3.x" + cache: "pip" - - name: Install rosdep + - name: Setup ROS 2 environment run: | - sudo apt-get install -y python3-rosdep + 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 - - - name: Install ROS 2 dependencies - run: | - rosdep install --from-paths . --ignore-src -r -y - - - name: Install rclpy - run: | - python -m pip install rclpy + source /opt/ros/foxy/setup.bash - name: Install dependencies run: | @@ -48,6 +47,7 @@ jobs: env: CODACY_PROJECT_TOKEN: ${{ secrets.CODACY_PROJECT_TOKEN }} + # name: Codacy Analysis # on: From eb8b35c537961651ca1a6a3011baf880af7ed526 Mon Sep 17 00:00:00 2001 From: zaid3727 Date: Tue, 2 Jul 2024 19:52:13 +0200 Subject: [PATCH 48/48] final changes --- tests/test_smach.py | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/tests/test_smach.py b/tests/test_smach.py index f015c05..68d681d 100644 --- a/tests/test_smach.py +++ b/tests/test_smach.py @@ -9,8 +9,6 @@ from safety_robile.safety_robile_SMACH import MonitorBatteryAndCollision, RotateBase, StopBase class TestStateMachine(unittest.TestCase): - # def __init__(self): - # self.setUp() def setUp(self): rclpy.init() @@ -22,9 +20,8 @@ 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) - # Simulate low battery by publishing a message low_battery_msg = String() - low_battery_msg.data = '80.0' # Set a value below the threshold + low_battery_msg.data = '20.0' # Battery level is 20% state.battery_callback(low_battery_msg) # Run the state @@ -36,12 +33,10 @@ 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) - # Simulate a possible collision by publishing a LaserScan message collision_msg = LaserScan() - collision_msg.ranges = [0.4, 0.6, 0.7] # Set a value below the threshold + collision_msg.ranges = [1.4, 1.6, 1.7] state.laser_scan_callback(collision_msg) - # Run the state outcome = state.execute(None) self.assertEqual(outcome, 'possible_collision') @@ -66,5 +61,3 @@ def test_stop_base(self): if __name__ == '__main__': unittest.main() - # test_obj = TestStateMachine() - # test_obj.test_monitor_battery_and_collision_low_battery()