fix: small adjustments

una-auxme · Dec 15, 2023 · 22b1a60 · 22b1a60
1 parent a3a3815
commit 22b1a60
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 15 deletions.
diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/behavior_speed.py b/code/planning/behavior_agent/src/behavior_agent/behaviours/behavior_speed.py
@@ -8,9 +8,7 @@ def convert_to_ms(speed):
 
 Behavior = namedtuple("Behavior", ("name", "speed"))
 
-# Change target_speed_pub to curr_behavior_pub
-
-# TODO: Cruise is in manuveurs -> ADD Publishers
+# Changed target_speed_pub to curr_behavior_pub
 
 # Intersection - Behaviors
 

diff --git a/code/planning/local_planner/src/behavior_speed.py b/code/planning/local_planner/src/behavior_speed.py
@@ -8,9 +8,7 @@ def convert_to_ms(speed):
 
 Behavior = namedtuple("Behavior", ("name", "speed"))
 
-# Change target_speed_pub to curr_behavior_pub
-
-# TODO: Cruise is in manuveurs -> ADD Publishers
+# Changed target_speed_pub to curr_behavior_pub
 
 # Intersection - Behaviors
 
@@ -19,7 +17,7 @@ def convert_to_ms(speed):
 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", -1)
+int_app_no_sign = Behavior("int_app_no_sign", -2)
 
 int_app_green = Behavior("int_app_green", convert_to_ms(30.0))
 
@@ -46,8 +44,9 @@ def convert_to_ms(speed):
 
 lc_app_init = Behavior("lc_app_blocked", convert_to_ms(30.0))
 
-# No Traffic Light or Sign -> stop dynamically at Stopline
-lc_app_blocked = Behavior("lc_app_blocked", -1)
+
+# TODO: Find out purpose of v_stop in lane_change (lines: 105 - 128)
+lc_app_blocked = Behavior("lc_app_blocked", 0.5)
 
 # Wait
 

diff --git a/code/planning/local_planner/src/motion_planning.py b/code/planning/local_planner/src/motion_planning.py
@@ -73,19 +73,17 @@ def __init__(self):
             f"/paf/{self.role_name}/target_velocity",
             qos_profile=1)
 
-    def update_target_speed(self):
-        be_speed = self.get_speed_by_behavior(self.__curr_behavior)
+    def update_target_speed(self, acc_speed, behavior):
+        be_speed = self.get_speed_by_behavior(behavior)
 
-        self.target_speed = min(be_speed, self.__acc_speed)
+        self.target_speed = min(be_speed, acc_speed)
         self.velocity_pub.publish(self.target_speed)
-        self.logerr(self.target_speed)
 
     def __set_acc_speed(self, data: Float32):
         self.__acc_speed = data.data
 
     def __set_curr_behavior(self, data: String):
         self.__curr_behavior = data.data
-        # self.logerr(self.__curr_behavior)
 
     def __set_stopline(self, data: Waypoint) -> float:
         if data is not None:
@@ -187,7 +185,7 @@ def run(self):
         """
 
         def loop(timer_event=None):
-            self.update_target_speed()
+            self.update_target_speed(self.__acc_speed, self.__curr_behavior)
 
         self.new_timer(self.control_loop_rate, loop)
         self.spin()