From a3a38151aa5870e1dffeae88453555784044ccc6 Mon Sep 17 00:00:00 2001 From: JuliusMiller Date: Thu, 14 Dec 2023 18:26:53 +0100 Subject: [PATCH] feat: Add Node for Motion Planning --- .../launch/global_planner.launch | 6 +- .../local_planner/launch/local_planner.launch | 5 + .../local_planner/src/motion_planning.py | 209 ++++++++++++++++++ 3 files changed, 217 insertions(+), 3 deletions(-) create mode 100755 code/planning/local_planner/src/motion_planning.py 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/motion_planning.py b/code/planning/local_planner/src/motion_planning.py new file mode 100755 index 00000000..dde1e9b0 --- /dev/null +++ b/code/planning/local_planner/src/motion_planning.py @@ -0,0 +1,209 @@ +#!/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): + be_speed = self.get_speed_by_behavior(self.__curr_behavior) + + self.target_speed = min(be_speed, self.__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: + 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.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()