529 - Implement service for planning - acting communication

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(

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>

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()

code/planning/srv/RequestBehaviourChange.srv

@@ -0,0 +1,3 @@
+uint8 request
+---
+bool answer

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.