Merge pull request #149 from una-auxme/143-feature-create-a-simple-mo…

…tion-planning-node

143 feature create a simple motion planning node

code/planning/behavior_agent/src/behavior_agent/behaviours/behavior_speed.py

@@ -0,0 +1,66 @@
+
+from collections import namedtuple
+
+
+def convert_to_ms(speed):
+    return speed / 3.6
+
+
+Behavior = namedtuple("Behavior", ("name", "speed"))
+
+# Changed target_speed_pub to curr_behavior_pub
+
+# Intersection - Behaviors
+
+# Approach
+
+int_app_init = Behavior("int_app_init", convert_to_ms(30.0))
+
+# No Traffic Light or Sign -> stop dynamically at Stopline
+int_app_no_sign = Behavior("int_app_no_sign", -2)
+
+int_app_green = Behavior("int_app_green", convert_to_ms(30.0))
+
+# Wait
+
+int_wait = Behavior("int_wait", 0)
+
+# Enter
+
+int_enter_no_light = Behavior("int_enter_no_light", convert_to_ms(50.0))
+
+int_enter_empty_str = Behavior("int_enter_empty_string", convert_to_ms(18.0))
+
+int_enter_light = Behavior("int_enter_light", convert_to_ms(50.0))
+
+# Exit
+
+int_exit = Behavior("int_exit", -1)  # Get SpeedLimit dynamically
+
+
+# Lane Change
+
+# Approach
+
+lc_app_init = Behavior("lc_app_blocked", convert_to_ms(30.0))
+
+
+# TODO: Find out purpose of v_stop in lane_change (lines: 105 - 128)
+lc_app_blocked = Behavior("lc_app_blocked", 0.5)
+
+# Wait
+
+# Has a publisher but doesnt publish anything ??
+
+# Enter
+
+lc_enter_init = Behavior("lc_enter_init", convert_to_ms(20.0))
+
+# Exit
+
+lc_exit = Behavior("lc_exit", -1)  # Get SpeedLimit dynamically
+
+
+# Cruise
+
+cruise = Behavior("Cruise", -1)

code/planning/behavior_agent/src/behavior_agent/behaviours/intersection.py

@@ -1,9 +1,11 @@
 import py_trees
 import numpy as np
-from std_msgs.msg import Float32
+from std_msgs.msg import String
 
 import rospy
 
+from .import behavior_speed as bs
+
 """
 Source: https://github.com/ll7/psaf2
 """
@@ -41,9 +43,9 @@ def setup(self, timeout):
         successful
         :return: True, as the set up is successful.
         """
-        self.target_speed_pub = rospy.Publisher("/paf/hero/"
-                                                "max_tree_velocity",
-                                                Float32, queue_size=1)
+        self.curr_behavior_pub = rospy.Publisher("/paf/hero/"
+                                                 "curr_behavior",
+                                                 String, queue_size=1)
         self.blackboard = py_trees.blackboard.Blackboard()
         return True
 
@@ -66,7 +68,7 @@ def initialise(self):
         self.traffic_light_distance = np.inf
         self.traffic_light_status = ''
         self.virtual_stopline_distance = np.inf
-        self.target_speed_pub.publish(convert_to_ms(30.0))
+        self.curr_behavior_pub.publish(bs.int_app_init.name)
         self.last_virtual_distance = np.inf
 
     def update(self):
@@ -117,16 +119,7 @@ def update(self):
             self.virtual_stopline_distance = 0.0
 
         rospy.loginfo(f"Stopline distance: {self.virtual_stopline_distance}")
-
         target_distance = 3.0
-        # calculate speed needed for stopping
-        v_stop = max(convert_to_ms(10.),
-                     convert_to_ms((self.virtual_stopline_distance / 30)
-                                   * 50))
-        if v_stop > convert_to_ms(50.0):
-            v_stop = convert_to_ms(50.0)
-        if self.virtual_stopline_distance < target_distance:
-            v_stop = 0.0
         # stop when there is no or red/yellow traffic light or a stop sign is
         # detected
         if self.traffic_light_status == '' \
@@ -135,13 +128,13 @@ def update(self):
                 or (self.stop_sign_detected and
                     not self.traffic_light_detected):
 
-            rospy.loginfo(f"slowing down: {v_stop}")
-            self.target_speed_pub.publish(v_stop)
+            rospy.loginfo("slowing down!")
+            self.curr_behavior_pub.publish(bs.int_app_no_sign.name)
 
         # approach slowly when traffic light is green as traffic lights are
         # higher priority than traffic signs this behavior is desired
         if self.traffic_light_status == 'green':
-            self.target_speed_pub.publish(convert_to_ms(30))
+            self.curr_behavior_pub.publish(bs.int_app_green.name)
 
         # get speed
         speedometer = self.blackboard.get("/carla/hero/Speed")
@@ -224,9 +217,9 @@ def setup(self, timeout):
         successful
         :return: True, as the set up is successful.
         """
-        self.target_speed_pub = rospy.Publisher("/paf/hero/"
-                                                "max_tree_velocity", Float32,
-                                                queue_size=1)
+        self.curr_behavior_pub = rospy.Publisher("/paf/hero/"
+                                                 "curr_behavior", String,
+                                                 queue_size=1)
         self.blackboard = py_trees.blackboard.Blackboard()
         return True
 
@@ -279,7 +272,7 @@ def update(self):
             if traffic_light_status == "red" or \
                     traffic_light_status == "yellow":
                 rospy.loginfo(f"Light Status: {traffic_light_status}")
-                self.target_speed_pub.publish(0)
+                self.curr_behavior_pub.publish(bs.int_wait.name)
                 return py_trees.common.Status.RUNNING
             else:
                 rospy.loginfo(f"Light Status: {traffic_light_status}")
@@ -333,9 +326,9 @@ def setup(self, timeout):
         successful
         :return: True, as the set up is successful.
         """
-        self.target_speed_pub = rospy.Publisher("/paf/hero/"
-                                                "max_tree_velocity", Float32,
-                                                queue_size=1)
+        self.curr_behavior_pub = rospy.Publisher("/paf/hero/"
+                                                 "curr_behavior", String,
+                                                 queue_size=1)
         self.blackboard = py_trees.blackboard.Blackboard()
         return True
 
@@ -352,16 +345,16 @@ def initialise(self):
         rospy.loginfo("Enter Intersection")
         light_status_msg = self.blackboard.get("/paf/hero/traffic_light")
         if light_status_msg is None:
-            self.target_speed_pub.publish(convert_to_ms(50.0))
+            self.curr_behavior_pub.publish(bs.int_enter_no_light.name)
             return True
         else:
             traffic_light_status = light_status_msg.color
 
         if traffic_light_status == "":
-            self.target_speed_pub.publish(convert_to_ms(18.0))
+            self.curr_behavior_pub.publish(bs.int_enter_empty_str.name)
         else:
             rospy.loginfo(f"Light Status: {traffic_light_status}")
-            self.target_speed_pub.publish(convert_to_ms(50.0))
+            self.curr_behavior_pub.publish(bs.int_enter_light.name)
 
     def update(self):
         """
@@ -433,9 +426,9 @@ def setup(self, timeout):
         successful
         :return: True, as the set up is successful.
         """
-        self.target_speed_pub = rospy.Publisher("/paf/hero/"
-                                                "max_tree_velocity", Float32,
-                                                queue_size=1)
+        self.curr_behavior_pub = rospy.Publisher("/paf/hero/"
+                                                 "curr_behavior", String,
+                                                 queue_size=1)
         self.blackboard = py_trees.blackboard.Blackboard()
         return True
 
@@ -453,16 +446,15 @@ def initialise(self):
         rospy.loginfo("Leave Intersection")
         street_speed_msg = self.blackboard.get("/paf/hero/speed_limit")
         if street_speed_msg is not None:
-            self.target_speed_pub.publish(street_speed_msg.data)
+            # self.curr_behavior_pub.publish(street_speed_msg.data)
+            self.curr_behavior_pub.publish(bs.int_exit.name)
         return True
 
     def update(self):
         """
         When is this called?
             Every time your behaviour is ticked.
-        What to do here?
-           - Triggering, checking, monitoring. Anything...but do not block!
-           - Set a feedback message
+        What to do here?exit_int
            - return a py_trees.common.Status.[RUNNING, SUCCESS, FAILURE]
         Abort this subtree
         :return: py_trees.common.Status.FAILURE, to exit this subtree

code/planning/behavior_agent/src/behavior_agent/behaviours/lane_change.py

@@ -1,11 +1,13 @@
 import py_trees
 import numpy as np
-from std_msgs.msg import Float32
+from std_msgs.msg import String
 # from nav_msgs.msg import Odometry
 # from custom_carla_msgs.srv import UpdateLocalPath
 
 import rospy
 
+from . import behavior_speed as bs
+
 """
 Source: https://github.com/ll7/psaf2
 """
@@ -43,9 +45,9 @@ def setup(self, timeout):
         successful
         :return: True, as the set up is successful.
         """
-        self.target_speed_pub = rospy.Publisher("/paf/hero/"
-                                                "max_tree_velocity",
-                                                Float32, queue_size=1)
+        self.curr_behavior_pub = rospy.Publisher("/paf/hero/"
+                                                 "curr_behavior",
+                                                 String, queue_size=1)
         # rospy.wait_for_service('update_local_path') # TODO is this necessary?
         # self.update_local_path =
         # rospy.ServiceProxy("update_local_path", UpdateLocalPath)
@@ -68,7 +70,7 @@ def initialise(self):
         self.change_detected = False
         self.change_distance = np.inf
         self.virtual_change_distance = np.inf
-        self.target_speed_pub.publish(convert_to_ms(30.0))
+        self.curr_behavior_pub.publish(bs.lc_init.name)
 
     def update(self):
         """
@@ -98,12 +100,6 @@ def update(self):
         if self.change_distance != np.inf and self.change_detected:
             self.virtual_change_distance = self.change_distance
 
-        # calculate speed needed for stopping
-        v_stop = max(convert_to_ms(5.),
-                     convert_to_ms((self.virtual_change_distance / 30) ** 1.5
-                                   * 50))
-        if v_stop > convert_to_ms(50.0):
-            v_stop = convert_to_ms(30.0)
         # slow down before lane change
         if self.virtual_change_distance < 15.0:
             if self.change_option == 5:
@@ -120,9 +116,8 @@ def update(self):
                 # self.update_local_path(leave_intersection=True)
                 return py_trees.common.Status.SUCCESS
             else:
-                v_stop = 0.5
-                rospy.loginfo(f"Change blocked slowing down: {v_stop}")
-                self.target_speed_pub.publish(v_stop)
+                rospy.loginfo("Change blocked slowing down")
+                self.curr_behavior_pub.publish(bs.lc_app_blocked.name)
 
         # get speed
         speedometer = self.blackboard.get("/carla/hero/Speed")
@@ -194,9 +189,9 @@ def setup(self, timeout):
         successful
         :return: True, as the set up is successful.
         """
-        self.target_speed_pub = rospy.Publisher("/paf/hero/"
-                                                "max_tree_velocity", Float32,
-                                                queue_size=1)
+        self.curr_behavior_pub = rospy.Publisher("/paf/hero/"
+                                                 "curr_behavior", String,
+                                                 queue_size=1)
         self.blackboard = py_trees.blackboard.Blackboard()
         return True
 
@@ -303,9 +298,9 @@ def setup(self, timeout):
         successful
         :return: True, as the set up is successful.
         """
-        self.target_speed_pub = rospy.Publisher("/paf/hero/"
-                                                "max_tree_velocity", Float32,
-                                                queue_size=1)
+        self.curr_behavior_pub = rospy.Publisher("/paf/hero/"
+                                                 "curr_behavior", String,
+                                                 queue_size=1)
         # rospy.wait_for_service('update_local_path')
         # self.update_local_path = rospy.ServiceProxy("update_local_path",
         # UpdateLocalPath)
@@ -323,7 +318,7 @@ def initialise(self):
         the intersection.
         """
         rospy.loginfo("Enter next Lane")
-        self.target_speed_pub.publish(convert_to_ms(20.0))
+        self.curr_behavior_pub.publish(bs.lc_enter_init.name)
 
     def update(self):
         """
@@ -396,9 +391,9 @@ def setup(self, timeout):
         successful
         :return: True, as the set up is successful.
         """
-        self.target_speed_pub = rospy.Publisher("/paf/hero/"
-                                                "max_tree_velocity", Float32,
-                                                queue_size=1)
+        self.curr_behavior_pub = rospy.Publisher("/paf/hero/"
+                                                 "curr_behavior", String,
+                                                 queue_size=1)
         self.blackboard = py_trees.blackboard.Blackboard()
         return True
 
@@ -415,7 +410,8 @@ def initialise(self):
         rospy.loginfo("Leave Change")
         street_speed_msg = self.blackboard.get("/paf/hero/speed_limit")
         if street_speed_msg is not None:
-            self.target_speed_pub.publish(street_speed_msg.data)
+            # self.curr_behavior_pub.publish(street_speed_msg.data)
+            self.curr_behavior_pub.publish(bs.lc_exit.name)
         return True
 
     def update(self):

code/planning/behavior_agent/src/behavior_agent/behaviours/maneuvers.py

@@ -1,4 +1,9 @@
 import py_trees
+import rospy
+from std_msgs.msg import String
+
+from . import behavior_speed as bs
+# from behavior_agent.msg import BehaviorSpeed
 
 """
 Source: https://github.com/ll7/psaf2
@@ -264,6 +269,7 @@ def __init__(self, name):
          :param name: name of the behaviour
         """
         super(Cruise, self).__init__(name)
+        rospy.loginfo("Cruise started")
 
     def setup(self, timeout):
         """
@@ -277,6 +283,11 @@ def setup(self, timeout):
         successful
         :return: True, as there is nothing to set up.
         """
+
+        self.curr_behavior_pub = rospy.Publisher("/paf/hero/"
+                                                 "curr_behavior",
+                                                 String, queue_size=1)
+
         self.blackboard = py_trees.blackboard.Blackboard()
         return True
 
@@ -290,6 +301,7 @@ def initialise(self):
             Any initialisation you need before putting your behaviour to work.
         :return: True
         """
+        rospy.loginfo("Starting Cruise")
         return True
 
     def update(self):
@@ -308,6 +320,7 @@ def update(self):
         :return: py_trees.common.Status.RUNNING, keeps the decision tree from
         finishing
         """
+        self.curr_behavior_pub.publish(bs.cruise.name)
         return py_trees.common.Status.RUNNING
 
     def terminate(self, new_status):