From 727f1d5dad110ae194d40ca7f3549943be9e4840 Mon Sep 17 00:00:00 2001
From: seefelke <33551476+seefelke@users.noreply.github.com>
Date: Thu, 12 Dec 2024 21:53:40 +0100
Subject: [PATCH] 529 - Implement service for planning - acting communication
 (#549)

* implemented acting-planning service

* Create RequestBehaviourChangeService.md

* Update RequestBehaviourChangeService.md

* Update RequestBehaviourChangeService.md

* Update RequestBehaviourChangeService.py

small comment fix
---
 code/planning/CMakeLists.txt                  |  7 +-
 code/planning/launch/planning.launch          |  7 +-
 .../RequestBehaviourChangeService.py          | 82 +++++++++++++++++++
 code/planning/srv/RequestBehaviourChange.srv  |  3 +
 doc/planning/RequestBehaviourChangeService.md | 26 ++++++
 5 files changed, 121 insertions(+), 4 deletions(-)
 create mode 100755 code/planning/src/behavior_agent/RequestBehaviourChangeService.py
 create mode 100644 code/planning/srv/RequestBehaviourChange.srv
 create mode 100644 doc/planning/RequestBehaviourChangeService.md

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 @@
 <launch>
     <arg name="role_name" default="hero" />
     <arg name="control_loop_rate" default="0.05" /> 
-    
+
     <node pkg="planning" type="collision_check.py" name="CollisionCheck" output="screen">
            <param name="role_name" value="hero" />
            <param name="control_loop_rate" value="$(arg control_loop_rate)" />
@@ -35,4 +35,9 @@
     <node pkg="planning" type="behavior_tree.py" name="behavior_agent" output="screen"/>
     <node pkg="rqt_gui" type="rqt_gui" name="rqt_gui"/>
 
+    <node pkg="planning" type="RequestBehaviourChangeService.py" name="RequestBehaviourChangeService" output="screen">
+           <param name="role_name" value="hero" />
+           <param name="control_loop_rate" value="$(arg control_loop_rate)" />
+    </node>
+    
 </launch>
diff --git a/code/planning/src/behavior_agent/RequestBehaviourChangeService.py b/code/planning/src/behavior_agent/RequestBehaviourChangeService.py
new file mode 100755
index 00000000..aa058ab3
--- /dev/null
+++ b/code/planning/src/behavior_agent/RequestBehaviourChangeService.py
@@ -0,0 +1,82 @@
+#!/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.
+        """
+        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
diff --git a/doc/planning/RequestBehaviourChangeService.md b/doc/planning/RequestBehaviourChangeService.md
new file mode 100644
index 00000000..fe0a3751
--- /dev/null
+++ b/doc/planning/RequestBehaviourChangeService.md
@@ -0,0 +1,26 @@
+# 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.
+
+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)
+```
+
+Then, you can just use this instance in a try setup:
+
+```python
+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.