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 index 24a35eb1..4eb9cf47 100755 --- 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 index 5af18e4a..4eb9cf47 100755 --- 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 index dde1e9b0..7b1e5982 100755 --- 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()