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 new file mode 100755 index 00000000..4eb9cf47 --- /dev/null +++ b/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) diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/intersection.py b/code/planning/behavior_agent/src/behavior_agent/behaviours/intersection.py index b8d7e8db..22697f27 100755 --- a/code/planning/behavior_agent/src/behavior_agent/behaviours/intersection.py +++ b/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 diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/lane_change.py b/code/planning/behavior_agent/src/behavior_agent/behaviours/lane_change.py index d4b84d19..4433cc2e 100644 --- a/code/planning/behavior_agent/src/behavior_agent/behaviours/lane_change.py +++ b/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): diff --git a/code/planning/behavior_agent/src/behavior_agent/behaviours/maneuvers.py b/code/planning/behavior_agent/src/behavior_agent/behaviours/maneuvers.py index ce672563..5a17c25a 100755 --- a/code/planning/behavior_agent/src/behavior_agent/behaviours/maneuvers.py +++ b/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): diff --git a/code/planning/global_planner/launch/global_planner.launch b/code/planning/global_planner/launch/global_planner.launch index d26b4873..6a13825f 100644 --- a/code/planning/global_planner/launch/global_planner.launch +++ b/code/planning/global_planner/launch/global_planner.launch @@ -1,12 +1,12 @@ - + --> diff --git a/code/planning/local_planner/launch/local_planner.launch b/code/planning/local_planner/launch/local_planner.launch index c469c05e..4819306e 100644 --- a/code/planning/local_planner/launch/local_planner.launch +++ b/code/planning/local_planner/launch/local_planner.launch @@ -3,4 +3,9 @@ + + + + + diff --git a/code/planning/local_planner/src/behavior_speed.py b/code/planning/local_planner/src/behavior_speed.py new file mode 100755 index 00000000..4eb9cf47 --- /dev/null +++ b/code/planning/local_planner/src/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) diff --git a/code/planning/local_planner/src/motion_planning.py b/code/planning/local_planner/src/motion_planning.py new file mode 100755 index 00000000..7b1e5982 --- /dev/null +++ b/code/planning/local_planner/src/motion_planning.py @@ -0,0 +1,207 @@ +#!/usr/bin/env python +# import rospy +# import tf.transformations +import ros_compatibility as roscomp +from ros_compatibility.node import CompatibleNode +from rospy import Publisher, Subscriber +from std_msgs.msg import String, Float32 +import numpy as np + +# from behavior_agent.msg import BehaviorSpeed +from perception.msg import Waypoint, LaneChange +import behavior_speed as bs + +# from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion +# from carla_msgs.msg import CarlaRoute # , CarlaWorldInfo +# from nav_msgs.msg import Path +# from std_msgs.msg import String +# from std_msgs.msg import Float32MultiArray + + +def convert_to_ms(speed): + return speed / 3.6 + + +class MotionPlanning(CompatibleNode): + """ + This node selects speeds according to the behavior in the Decision Tree + and the ACC. + Later this Node should compute a local Trajectory and forward + it to the Acting. + """ + + def __init__(self): + super(MotionPlanning, self).__init__('MotionPlanning') + self.role_name = self.get_param("role_name", "hero") + self.control_loop_rate = self.get_param("control_loop_rate", 0.5) + self.logdebug("MotionPlanning started") + + self.target_speed = 0.0 + self.__curr_behavior = None + self.__acc_speed = 0.0 + self.__stopline = None # (Distance, isStopline) + self.__change_point = None # (Distance, isLaneChange, roadOption) + + # Subscriber + self.curr_behavior_sub: Subscriber = self.new_subscription( + String, + f"/paf/{self.role_name}/curr_behavior", + self.__set_curr_behavior, + qos_profile=1) + + self.acc_sub: Subscriber = self.new_subscription( + Float32, + f"/paf/{self.role_name}/acc_velocity", + self.__set_acc_speed, + qos_profile=1) + + self.stopline_sub: Subscriber = self.new_subscription( + Waypoint, + f"/paf/{self.role_name}/waypoint_distance", + self.__set_stopline, + qos_profile=1) + + self.change_point_sub: Subscriber = self.new_subscription( + LaneChange, + f"/paf/{self.role_name}/lane_change_distance", + self.__set_change_point, + qos_profile=1) + + # Publisher + self.velocity_pub: Publisher = self.new_publisher( + Float32, + f"/paf/{self.role_name}/target_velocity", + qos_profile=1) + + def update_target_speed(self, acc_speed, behavior): + be_speed = self.get_speed_by_behavior(behavior) + + self.target_speed = min(be_speed, acc_speed) + self.velocity_pub.publish(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 + + def __set_stopline(self, data: Waypoint) -> float: + if data is not None: + self.__stopline = (data.distance, data.isStopLine) + + def __set_change_point(self, data: LaneChange): + if data is not None: + self.__change_point = \ + (data.distance, data.isLaneChange, data.roadOption) + + def get_speed_by_behavior(self, behavior: str) -> float: + speed = 0.0 + split = "_" + short_behavior = behavior.partition(split)[0] + if short_behavior == "int": + speed = self.__get_speed_intersection(behavior) + elif short_behavior == "lc": + speed = self.__get_speed_lanechange(behavior) + else: + speed = self.__get_speed_cruise() + + return speed + + def __get_speed_intersection(self, behavior: str) -> float: + speed = 0.0 + if behavior == bs.int_app_init.name: + speed = bs.int_app_init.speed + elif behavior == bs.int_app_green.name: + speed = bs.int_app_green.speed + elif behavior == bs.int_app_no_sign.name: + speed = self.__calc_speed_to_stop_intersection() + elif behavior == bs.int_wait.name: + speed == bs.int_wait.speed + elif behavior == bs.int_enter_no_light: + speed = bs.int_enter_no_light.speed + elif behavior == bs.int_enter_empty_str.name: + speed = bs.int_enter_empty_str.speed + elif behavior == bs.int_enter_light.name: + speed == bs.int_enter_light.speed + elif behavior == bs.int_exit: + speed = bs.int_exit.speed + + return speed + + def __get_speed_lanechange(self, behavior: str) -> float: + speed = 0.0 + if behavior == bs.lc_app_init.name: + speed = bs.lc_app_init.speed + elif behavior == bs.lc_app_blocked.name: + speed = bs.lc_app_blocked.speed # calc_speed_to_stop_lanechange() + elif behavior == bs.lc_enter_init.name: + speed = bs.lc_enter_init.speed + elif behavior == bs.lc_exit.name: + speed = bs.lc_exit.speed + + return speed + + def __get_speed_cruise(self) -> float: + return self.__acc_speed + + def __calc_speed_to_stop_intersection(self) -> float: + target_distance = 3.0 + virtual_stopline_distance = self.__calc_virtual_stopline() + # calculate speed needed for stopping + v_stop = max(convert_to_ms(10.), + convert_to_ms((virtual_stopline_distance / 30) + * 50)) + if v_stop > convert_to_ms(50.0): + v_stop = convert_to_ms(50.0) + if virtual_stopline_distance < target_distance: + v_stop = 0.0 + + # TODO: Find out purpose + def __calc_speed_to_stop_lanechange(self) -> float: + if self.__change_point[0] != np.inf and self.__change_point[1]: + stopline = self.__change_point[0] + else: + return 100 + + v_stop = max(convert_to_ms(5.), + convert_to_ms((stopline / 30) ** 1.5 + * 50)) + if v_stop > convert_to_ms(50.0): + v_stop = convert_to_ms(30.0) + return v_stop + + def __calc_virtual_stopline(self) -> float: + if self.__stopline[0] != np.inf and self.__stopline[1]: + return self.__stopline[0] + elif self.traffic_light_detected: + return self.traffic_light_distance + else: + return 0.0 + + def run(self): + """ + Control loop + :return: + """ + + def loop(timer_event=None): + self.update_target_speed(self.__acc_speed, self.__curr_behavior) + + self.new_timer(self.control_loop_rate, loop) + self.spin() + + +if __name__ == "__main__": + """ + main function starts the MotionPlanning node + :param args: + """ + roscomp.init('MotionPlanning') + + try: + node = MotionPlanning() + node.run() + except KeyboardInterrupt: + pass + finally: + roscomp.shutdown() diff --git a/doc/07_planning/motion_planning.md b/doc/07_planning/motion_planning.md new file mode 100644 index 00000000..175fbb25 --- /dev/null +++ b/doc/07_planning/motion_planning.md @@ -0,0 +1,56 @@ +# Motion Planning + +**Summary:** [motion_planning.py](.../code/planning/local_planner/src/motion_planning.py): +The motion planning is responsible for collecting all the speeds from the different components and choosing the optimal one to be fowarded into the acting. + +--- + +## Author + +Julius Miller + +## Date + +17.12.2023 + +## Prerequisite + +--- + +- [Motion Planning](#motion-planning) + - [Author](#author) + - [Date](#date) + - [Prerequisite](#prerequisite) + - [Description](#description) + - [Inputs](#inputs) + - [Outputs](#outputs) + + +--- + +## Description + +This node currently gathers the behavior speed and the acc_speed and chooses the lower one to be forwarded to the acting. +At the moment this is enough since the only present behaviors are Intersection, Lane Change and Cruise. + +When the Overtaking behavior will be added, choosing the minimum speed will not be sufficient. + +### Inputs + +This node subscribes to the following topics: + +- Current Behavior: + - `/paf/{role_name}/curr_behavior` (String) +- ACC Velocity: + - `/paf/{role_name}/acc_velocity` (Float32) +- Waypoint: + - `/paf/{role_name}/waypoint_distance` (Waypoint) +- Lane Change: + - `/paf/{role_name}/lane_change_distance` (LaneChange) + +### Outputs + +This node publishes the following topics: + +- Target Velocity + - `/paf/{role_name}/target_velocity` (Float32)