From cfcb48671ec1e2f87147319e6fff859520cfa557 Mon Sep 17 00:00:00 2001 From: seefelke Date: Thu, 5 Dec 2024 02:37:30 +0100 Subject: [PATCH 1/5] implemented acting-planning service --- code/planning/CMakeLists.txt | 7 +- code/planning/launch/planning.launch | 7 +- .../RequestBehaviourChangeService.py | 83 +++++++++++++++++++ code/planning/srv/RequestBehaviourChange.srv | 3 + 4 files changed, 96 insertions(+), 4 deletions(-) create mode 100755 code/planning/src/behavior_agent/RequestBehaviourChangeService.py create mode 100644 code/planning/srv/RequestBehaviourChange.srv diff --git a/code/planning/CMakeLists.txt b/code/planning/CMakeLists.txt index c6288e03..d8cf2bff 100755 --- a/code/planning/CMakeLists.txt +++ b/code/planning/CMakeLists.txt @@ -58,11 +58,12 @@ catkin_python_setup() ) ## Generate services in the 'srv' folder -# add_service_files( -# FILES +add_service_files( + FILES # Service1.srv # Service2.srv -# ) + RequestBehaviourChange.srv +) ## Generate actions in the 'action' folder # add_action_files( diff --git a/code/planning/launch/planning.launch b/code/planning/launch/planning.launch index 6662d9a3..d5803ddd 100644 --- a/code/planning/launch/planning.launch +++ b/code/planning/launch/planning.launch @@ -1,7 +1,7 @@ - + @@ -35,4 +35,9 @@ + + + + + diff --git a/code/planning/src/behavior_agent/RequestBehaviourChangeService.py b/code/planning/src/behavior_agent/RequestBehaviourChangeService.py new file mode 100755 index 00000000..4cf3f376 --- /dev/null +++ b/code/planning/src/behavior_agent/RequestBehaviourChangeService.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python +# import BehaviourEnum +import ros_compatibility as roscomp +from ros_compatibility.node import CompatibleNode +from rospy import Subscriber, Publisher +import rospy +from planning.srv import RequestBehaviourChange, RequestBehaviourChangeResponse +from std_msgs.msg import String, Int8 + + +class RequestBehaviourChangeService(CompatibleNode): + def __init__(self): + super(RequestBehaviourChangeService, self).__init__( + "RequestBehaviourChangeService" + ) + self.role_name = self.get_param("role_name", "hero") + self.control_loop_rate = self.get_param("control_loop_rate", 1) + self.__curr_behavior = None + + self.service = rospy.Service( + "RequestBehaviourChange", + RequestBehaviourChange, + self.handle_request_behaviour_change, + ) + + self.behaviour_pub: Publisher = self.new_publisher( + Int8, + f"/paf/{self.role_name}/behaviour_request", + qos_profile=1, + ) + + self.curr_behavior_sub: Subscriber = self.new_subscription( + String, + f"/paf/{self.role_name}/curr_behavior", + self.__set_curr_behavior, + qos_profile=1, + ) + + self.behaviour_pub.publish(0) + rospy.spin() + + def __set_curr_behavior(self, data: String): + """ + Sets the received current behavior of the vehicle. + If the behavior is an overtake behavior, a trajectory change is triggered. + """ + self.__curr_behavior = data.data + + def handle_request_behaviour_change(self, req): + if ( + self.__curr_behavior == "us_unstuck" + or self.__curr_behavior == "us_stop" + or self.__curr_behavior == "us_overtake" + or self.__curr_behavior == "Cruise" + ): + self.behaviour_pub.publish(req.request) + return RequestBehaviourChangeResponse(True) + else: + return RequestBehaviourChangeResponse(False) + + def run(self): + """ + Control loop + + :return: + """ + + self.spin() + + +if __name__ == "__main__": + """ + main function starts the RequestBehaviourChangeService node + :param args: + """ + roscomp.init("RequestBehaviourChangeService") + try: + node = RequestBehaviourChangeService() + node.run() + except KeyboardInterrupt: + pass + finally: + roscomp.shutdown() diff --git a/code/planning/srv/RequestBehaviourChange.srv b/code/planning/srv/RequestBehaviourChange.srv new file mode 100644 index 00000000..d4da3b97 --- /dev/null +++ b/code/planning/srv/RequestBehaviourChange.srv @@ -0,0 +1,3 @@ +uint8 request +--- +bool answer \ No newline at end of file From ffbb8fdcd6d2cbec7ba470272de2ed92502b62d8 Mon Sep 17 00:00:00 2001 From: seefelke <33551476+seefelke@users.noreply.github.com> Date: Thu, 5 Dec 2024 17:29:38 +0100 Subject: [PATCH 2/5] Create RequestBehaviourChangeService.md --- doc/planning/RequestBehaviourChangeService.md | 1 + 1 file changed, 1 insertion(+) create mode 100644 doc/planning/RequestBehaviourChangeService.md diff --git a/doc/planning/RequestBehaviourChangeService.md b/doc/planning/RequestBehaviourChangeService.md new file mode 100644 index 00000000..8b137891 --- /dev/null +++ b/doc/planning/RequestBehaviourChangeService.md @@ -0,0 +1 @@ + From b807fea91b55fcf56ce0d85697638f047cbf9501 Mon Sep 17 00:00:00 2001 From: seefelke <33551476+seefelke@users.noreply.github.com> Date: Thu, 5 Dec 2024 17:38:19 +0100 Subject: [PATCH 3/5] Update RequestBehaviourChangeService.md --- doc/planning/RequestBehaviourChangeService.md | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/doc/planning/RequestBehaviourChangeService.md b/doc/planning/RequestBehaviourChangeService.md index 8b137891..ee5226a2 100644 --- a/doc/planning/RequestBehaviourChangeService.md +++ b/doc/planning/RequestBehaviourChangeService.md @@ -1 +1,24 @@ +This service is hosted in the node RequestBehaviourChangeService. +Calling it requires a behaviour ID integer (based on the corresponding enum) and returns a bool depending on whether the request is granted. + +To use it, import RequestBehaviourChange from planning.srv. + +To call it, create a callable instance with: + +``` +rospy.wait_for_service('RequestBehaviourChange') + +name = rospy.ServiceProxy('RequestBehaviourChange', RequestBehaviourChange) +``` + +Then, you can just use this instance in a try setup: + +``` +try: + response = name(input) +except rospy.ServiceException as e: + # handle exception +``` + +For communication with the behaviour tree this node publishes a granted request to a topic behaviour_request. From 391a113dbb2e661b4f63d7cee9c07f51ea5aca1a Mon Sep 17 00:00:00 2001 From: seefelke <33551476+seefelke@users.noreply.github.com> Date: Thu, 5 Dec 2024 17:51:44 +0100 Subject: [PATCH 4/5] Update RequestBehaviourChangeService.md --- doc/planning/RequestBehaviourChangeService.md | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/doc/planning/RequestBehaviourChangeService.md b/doc/planning/RequestBehaviourChangeService.md index ee5226a2..fe0a3751 100644 --- a/doc/planning/RequestBehaviourChangeService.md +++ b/doc/planning/RequestBehaviourChangeService.md @@ -1,3 +1,5 @@ +# Request Behaviour Change Service + This service is hosted in the node RequestBehaviourChangeService. Calling it requires a behaviour ID integer (based on the corresponding enum) and returns a bool depending on whether the request is granted. @@ -6,7 +8,7 @@ To use it, import RequestBehaviourChange from planning.srv. To call it, create a callable instance with: -``` +```python rospy.wait_for_service('RequestBehaviourChange') name = rospy.ServiceProxy('RequestBehaviourChange', RequestBehaviourChange) @@ -14,7 +16,7 @@ name = rospy.ServiceProxy('RequestBehaviourChange', RequestBehaviourChange) Then, you can just use this instance in a try setup: -``` +```python try: response = name(input) except rospy.ServiceException as e: From d6047751a773d586a2279e0b78c3de70a9e0d3f7 Mon Sep 17 00:00:00 2001 From: seefelke <33551476+seefelke@users.noreply.github.com> Date: Thu, 5 Dec 2024 17:55:48 +0100 Subject: [PATCH 5/5] Update RequestBehaviourChangeService.py small comment fix --- .../planning/src/behavior_agent/RequestBehaviourChangeService.py | 1 - 1 file changed, 1 deletion(-) diff --git a/code/planning/src/behavior_agent/RequestBehaviourChangeService.py b/code/planning/src/behavior_agent/RequestBehaviourChangeService.py index 4cf3f376..aa058ab3 100755 --- a/code/planning/src/behavior_agent/RequestBehaviourChangeService.py +++ b/code/planning/src/behavior_agent/RequestBehaviourChangeService.py @@ -42,7 +42,6 @@ def __init__(self): def __set_curr_behavior(self, data: String): """ Sets the received current behavior of the vehicle. - If the behavior is an overtake behavior, a trajectory change is triggered. """ self.__curr_behavior = data.data