diff --git a/code/planning/src/behavior_agent/behavior_tree.py b/code/planning/src/behavior_agent/behavior_tree.py index 42ca1ea9..59adb810 100755 --- a/code/planning/src/behavior_agent/behavior_tree.py +++ b/code/planning/src/behavior_agent/behavior_tree.py @@ -96,7 +96,7 @@ def main(): rospy.logerr("Tree Setup failed") sys.exit(1) rospy.loginfo("tree setup worked") - r = rospy.Rate(5) + r = rospy.Rate(5.3) while not rospy.is_shutdown(): behaviour_tree.tick() try: diff --git a/code/planning/src/behavior_agent/behaviours/maneuvers.py b/code/planning/src/behavior_agent/behaviours/maneuvers.py index 8ba270a9..33e35946 100755 --- a/code/planning/src/behavior_agent/behaviours/maneuvers.py +++ b/code/planning/src/behavior_agent/behaviours/maneuvers.py @@ -59,7 +59,6 @@ def initialise(self): execution """ self.initPosition = self.blackboard.get("/paf/hero/current_pos") - rospy.loginfo("LEAVEPARKINGSPACE init") def update(self): """ @@ -99,7 +98,7 @@ def update(self): endPos = np.array([self.initPosition.pose.position.x, self.initPosition.pose.position.y]) distance = np.linalg.norm(startPos - endPos) - if distance < 2 or speed.speed < 2: + if distance < 1 or speed.speed < 2: self.curr_behavior_pub.publish(bs.parking.name) self.initPosition = position return py_trees.common.Status.RUNNING diff --git a/code/planning/src/local_planner/ACC.py b/code/planning/src/local_planner/ACC.py index 84853c54..a2b8182c 100755 --- a/code/planning/src/local_planner/ACC.py +++ b/code/planning/src/local_planner/ACC.py @@ -10,7 +10,6 @@ from std_msgs.msg import Float32MultiArray, Float32 from collision_check import CollisionCheck import time -from perception.msg import MinDistance class ACC(CompatibleNode): @@ -34,8 +33,8 @@ def __init__(self): # Subscriber for lidar distance # TODO: Change to real lidar distance self.lidar_dist = self.new_subscription( - MinDistance, - f"/paf/{self.role_name}/LIDAR_range", + Float32, + f"/carla/{self.role_name}/LIDAR_range", self._set_distance, qos_profile=1) # Get initial set of speed limits @@ -85,13 +84,13 @@ def __init__(self): self.logdebug("ACC initialized") - def _set_distance(self, data: MinDistance): + def _set_distance(self, data: Float32): """Get min distance to object in front from perception Args: data (MinDistance): Minimum Distance from LIDAR """ - self.obstacle_distance = data.distance + self.obstacle_distance = data.data def __approx_speed_callback(self, data: Float32): """Safe approximated speed form obstacle in front together with @@ -101,6 +100,7 @@ def __approx_speed_callback(self, data: Float32): Args: data (Float32): Speed from obstacle in front """ + self.logerr("ACC: Approx speed recieved: " + str(data.data)) self.obstacle_speed = (time.time(), data.data) def __get_current_velocity(self, data: CarlaSpeedometer): diff --git a/code/planning/src/local_planner/collision_check.py b/code/planning/src/local_planner/collision_check.py index e62d2ab9..ced02d54 100755 --- a/code/planning/src/local_planner/collision_check.py +++ b/code/planning/src/local_planner/collision_check.py @@ -9,7 +9,6 @@ # from std_msgs.msg import String from std_msgs.msg import Float32, Float32MultiArray from std_msgs.msg import Bool -from perception.msg import MinDistance import time @@ -23,7 +22,7 @@ def __init__(self): super(CollisionCheck, self).__init__('CollisionCheck') self.role_name = self.get_param("role_name", "hero") self.control_loop_rate = self.get_param("control_loop_rate", 1) - + self.lidar_position_offset = 2 # self.obstacle_sub: Subscriber = self.new_subscription( # ) # Subscriber for current speed @@ -35,14 +34,14 @@ def __init__(self): # Subscriber for lidar distance # TODO: Change to real lidar distance self.lidar_dist = self.new_subscription( - MinDistance, - f"/paf/{self.role_name}/LIDAR_range", + Float32, + f"/carla/{self.role_name}/LIDAR_range", self.__set_distance, qos_profile=1) # Publisher for emergency stop self.emergency_pub = self.new_publisher( Bool, - f"/paf/{self.role_name}/unchecked_emergency", + f"/paf/{self.role_name}/emergency", qos_profile=1) # Publisher for distance to collision self.collision_pub = self.new_publisher( @@ -60,24 +59,26 @@ def __init__(self): self.__object_last_position: tuple = None self.logdebug("CollisionCheck started") - def __set_distance(self, data: MinDistance): - """Saves the distance from the lidar - - Args: - data (MinDistance): Message from lidar with distance + def update_distance(self): + """Updates the distance to the obstacle in front """ - if np.isinf(data.distance): + if np.isinf(self.__object_last_position[1]): self.__object_last_position = None self.__object_first_position = None return if self.__object_first_position is None: - self.__object_first_position = (time.time(), data.distance) - return - if self.__object_last_position is None: - self.__object_last_position = (time.time(), data.distance) + self.__object_first_position = self.__object_last_position + self.__object_last_position = None return self.__object_first_position = self.__object_last_position - self.__object_last_position = (time.time(), data.distance) + + def __set_distance(self, data: Float32): + """Saves last distance from LIDAR + + Args: + data (Float32): Message from lidar with distance + """ + self.__object_last_position = (time.time(), data.data) def calculate_obstacle_speed(self): """Caluclate the speed of the obstacle in front of the ego vehicle @@ -91,8 +92,8 @@ def calculate_obstacle_speed(self): # If distance is np.inf no car is in front # Calculate time since last position update - current_time = time.time() - time_difference = current_time-self.__object_last_position[0] + time_difference = self.__object_last_position[0] - \ + self.__object_first_position[0] # Calculate distance (in m) distance = self.__object_last_position[1] -\ @@ -177,8 +178,8 @@ def check_crash(self, obstacle): if collision_time > 0: if distance < emergency_distance2: # Initiate emergency brake - self.emergency_pub.publish(True) self.logerr("Emergency Brake") + self.emergency_pub.publish(True) return # When no emergency brake is needed publish collision object data = Float32MultiArray(data=[distance, obstacle_speed]) @@ -198,6 +199,7 @@ def loop(timer_event=None): Checks if distance to a possible object is too small and publishes the desired speed to motion planning """ + self.update_distance() self.calculate_obstacle_speed() self.new_timer(self.control_loop_rate, loop) self.spin() diff --git a/code/planning/src/local_planner/motion_planning.py b/code/planning/src/local_planner/motion_planning.py index db0630c9..c0217b66 100755 --- a/code/planning/src/local_planner/motion_planning.py +++ b/code/planning/src/local_planner/motion_planning.py @@ -91,7 +91,9 @@ def __check_emergency(self, data: Bool): Args: data (Bool): True if emergency stop detected by collision check """ - if self.__curr_behavior is not bs.parking.name: + self.logerr("Emergency stop detected") + if not self.__curr_behavior == bs.parking.name: + self.logerr("Emergency stop detected and executed") self.emergency_pub.publish(data) def update_target_speed(self, acc_speed, behavior): @@ -100,6 +102,7 @@ def update_target_speed(self, acc_speed, behavior): self.target_speed = min(be_speed, acc_speed) else: self.target_speed = be_speed + # self.logerr("target speed: " + str(self.target_speed)) self.velocity_pub.publish(self.target_speed) def __set_acc_speed(self, data: Float32): @@ -131,7 +134,6 @@ def get_speed_by_behavior(self, behavior: str) -> float: speed = bs.parking.speed else: speed = self.__get_speed_cruise() - return speed def __get_speed_intersection(self, behavior: str) -> float: @@ -212,7 +214,6 @@ def run(self): """ def loop(timer_event=None): - self.loginfo("MotionPlanning loop") self.update_target_speed(self.__acc_speed, self.__curr_behavior) self.new_timer(self.control_loop_rate, loop)