diff --git a/src/planning/rtp/rtp/rtp_node.py b/src/planning/rtp/rtp/rtp_node.py index 64d6778a..0a22af42 100755 --- a/src/planning/rtp/rtp/rtp_node.py +++ b/src/planning/rtp/rtp/rtp_node.py @@ -536,7 +536,7 @@ def costMapCb(self, msg: OccupancyGrid): if np.sqrt(pose[0]**2 + pose[1]**2) > lookahead_distance: lookahead_pose = pose break - + if lookahead_pose is None: lookahead_pose = poses_np_bl[-1]