⭐ 주행 경로 계획 예시

[Jinsun-Lee]

공동

MOVE   # ros2 service call /go std_srvs/srv/SetBool "{data: true}" 
STOP   # ros2 service call /go std_srvs/srv/SetBool "{data: false}"  

launch를 실행해도 바로 동작하지 않음
MOVE는 터미널에서 ros2 service call /go std_srvs/srv/SetBool "{data: true}"을 실행한 것과 같음
STOP도 동일

기본 구조

class MotionPlanningNode(Node):
    def __init__(self):

        # 토픽 설정: 차선, 신호등, 검출, 라이다, 장애물
        self.sub_lane_topic = self.declare_parameter('sub_lane_topic', SUB_LANE_TOPIC_NAME).value
        self.sub_traffic_light_topic = self.declare_parameter('sub_traffic_light_topic', SUB_TRAFFIC_LIGHT_TOPIC_NAME).value
        self.sub_detection_topic = self.declare_parameter('sub_detection_topic', SUB_DETECTION_TOPIC_NAME).value
        self.sub_lidar_obstacle_topic = self.declare_parameter('sub_lidar_obstacle_topic', SUB_LIDAR_OBSTACLE_TOPIC_NAME).value
        
        self.pub_topic = self.declare_parameter('pub_topic', PUB_TOPIC_NAME).value
        self.timer_period = self.declare_parameter('timer', TIMER).value
      
        # MOVE / STOP을 위함 
        self.should_stop = True
        self.srv = self.create_service(SetBool, 'go', self.set_service)
        
        # 타이머(2초 이후에 실행)
        self.delay_timer = None
        self.delay_duration = 2 
  
        self.steering_command = 0     # 좌회전, 직진, 우회전
        self.left_speed_command = 0   # 좌측뒷바퀴 모터의 속도 (-255) ~ (+255) 후진, 전진
        self.right_speed_command = 0  # 우측뒷바퀴 모터의 속도
        
        self.lane_data = None
        self.traffic_light_data = None
        self.detection_data = None
        self.lidar_data = None
        ...

    def set_service(self, request, response):
        ...
    
    def stop(self):
        ...
           
    
    #--------------- TODO 아래만 수정 --------------
         
    def lane_callback(self, msg: LaneInfo):
        self.lane_data = msg

    def traffic_light_callback(self, msg: String):
        self.traffic_light_data = msg
    
    def detection_callback(self, msg: DetectionArray):
        self.detection_data = msg
        
    def lidar_callback(self, msg: Bool):
        self.lidar_data = msg
                
    # 모션 명령 메시지 전송 퍼블리시
    def timer_callback(self):        
        

    #------------------- 위에만 수정 ---------------
    
        motion_command_msg = MotionCommand()
        
        if self.should_stop:
            self.steering_command = 0
            self.left_speed_command = 0
            self.right_speed_command = 0
            
        motion_command_msg.steering = self.steering_command
        motion_command_msg.left_speed = self.left_speed_command
        motion_command_msg.right_speed = self.right_speed_command
        self.publisher.publish(motion_command_msg)
    
def main(args=None):
    ...

예시1: timer_callback()에서 통합 수행

class MotionPlanningNode(Node):
    def __init__(self):
        ...
    
    def set_service(self, request, response):
        ...
    
    def stop(self):
        ...

    def detection_callback(self, msg: DetectionArray):
        self.detection_data = msg

    def lane_callback(self, msg: LaneInfo):
        self.lane_data = msg

    def traffic_light_callback(self, msg: String):
        self.traffic_light_data = msg

    def lidar_callback(self, msg: Bool):
        self.lidar_data = msg
        

    def timer_callback(self):

        if self.lidar_data is not None and self.lidar_data.data is True:
            # 라이다가 장애물을 감지한 경우
            self.steering_command = 0 
            self.left_speed_command = 0 
            self.right_speed_command = 0 

        elif self.traffic_light_data is not None and self.traffic_light_data.data == 'Red':
            # 빨간색 신호등을 감지한 경우
            for detection in self.detection_data.detections:
                if detection.class_name=='traffic_light':
                    x_min = int(detection.bbox.center.position.x - detection.bbox.size.x / 2) # bbox의 좌측상단 꼭짓점 x좌표
                    x_max = int(detection.bbox.center.position.x + detection.bbox.size.x / 2) # bbox의 우측하단 꼭짓점 x좌표
                    y_min = int(detection.bbox.center.position.y - detection.bbox.size.y / 2) # bbox의 좌측상단 꼭짓점 y좌표
                    y_max = int(detection.bbox.center.position.y + detection.bbox.size.y / 2) # bbox의 우측하단 꼭짓점 y좌표

                    if y_max < 150:
                        # 신호등 위치에 따른 정지명령 결정
                        self.steering_command = 0 
                        self.left_speed_command = 0 
                        self.right_speed_command = 0
        else:
            if self.lane_data is None:
                self.steering_command = 0

            else:    
                target_point = (self.lane_data.target_x, self.lane_data.target_y)
                car_center_point = (320, 477)

                target_slope = DMFL.calculate_slope_between_points(target_point, car_center_point)
        
                if target_slope > 0:
                    self.steering_command =  7 # 예시 (7이 최대 조향) 
                elif target_slope < 0:
                    self.steering_command =  -7
                else:
                    self.steering_command = 0

                #self.get_logger().info("target_slope: %s \n" % target_slope)
        
                self.left_speed_command = 100  # 예시 속도 값 (255가 최대 속도)
                self.right_speed_command = 100  # 예시 속도 값 (255가 최대 속도) 
        
        
        #------------------- 위에만 수정 ---------------
   
        # 모션 명령 메시지 생성 및 퍼블리시
        motion_command_msg = MotionCommand()

        if self.should_stop:
            self.steering_command = 0
            self.left_speed_command = 0
            self.right_speed_command = 0
            
        motion_command_msg.steering = self.steering_command
        motion_command_msg.left_speed = self.left_speed_command
        motion_command_msg.right_speed = self.right_speed_command
        self.publisher.publish(motion_command_msg)

def main(args=None):
        ...

예시2: timer_callback()는 publish만 수행

class MotionPlanningNode(Node):
    def __init__(self):
        ...
    
    def set_service(self, request, response):
        ...
    
    def stop(self):
        ...
                
    def lane_callback(self, msg: LaneInfo):
        self.lane_data = msg
        
        # 2차선에 대한 정보가 없으면 정지
        if self.lane_data == None:
            self.steering_command = 0
        
        # 2차선에 대한 정보가 있으면 조향값 계산
        else:
            # 기울기를 통해 차선을 벗어나지 않도록 조향값 계산
            self.steering_command = CONTROL.convert_steeringangle2command(52, self.lane_data.slope ) # 변수
            
            # 라이다가 장애물을 감지한 경우  
            
            # 구동 모터의 좌우 속도를 달리하고, 속도를 부드럽게 변화 (주차에는 없애도 무관)
            self.left_speed_command = self.left_smooth_speed_controller.update_actual_speed(self.left_speed_command)
            self.right_speed_command = self.right_smooth_speed_controller.update_actual_speed(self.right_speed_command)

    def traffic_light_callback(self, msg: String):
        self.traffic_light_data = msg
        
        # 빨간색 신호등을 감지한 경우 
        if (self.traffic_light_data.data == 'Red'): # 변수
            
            # 일정 거리보다 가까워지면
            if self.traffic_flag:
                self.stop()
                
        else:
            # 빨간 신호등이 아닌 경우 flag를 false로 재설정
            self.traffic_flag = False
    
    def detection_callback(self, msg: DetectionArray):
        self.detection_data = msg
        traffic_light_detected = False
        
        for detection in self.detection_data.detections:
            if detection.class_name== DebugYOLO.TRAFFIC_CLASS_NAME:
                x_min = int(detection.bbox.center.position.x - detection.bbox.size.x / 2) # bbox의 좌측상단 꼭짓점 x좌표
                x_max = int(detection.bbox.center.position.x + detection.bbox.size.x / 2) # bbox의 우측하단 꼭짓점 x좌표
                y_min = int(detection.bbox.center.position.y - detection.bbox.size.y / 2) # bbox의 좌측상단 꼭짓점 y좌표
                y_max = int(detection.bbox.center.position.y + detection.bbox.size.y / 2) # bbox의 우측하단 꼭짓점 y좌표
                
                # 신호등 위치에 따른 정지명령 결정 # 변수
                if y_max < 150:
                    self.traffic_flag = True
                    
        # 신호등을 감지하지 못하거나 y_max가 조건에 맞지 않으면 flag를 false로 재설정
        if not traffic_light_detected:
            self.traffic_flag = False
        
    def lidar_callback(self, msg: Bool):
        self.lidar_data = msg
        
        if self.lidar_data != None:
            left_avg_dist = int(self.lidar_data.data)
            
            # 라이다가 장애물을 감지한 경우  
            if left_avg_dist < 150:  # 변수
                self.lidar_flag = True
                self.stop()
            
            else:
                # 장애물이 없으면 flag를 false로 재설정
                self.lidar_flag = False


    # 모션 명령 메시지 생성 및 퍼블리시
    def timer_callback(self):        
        motion_command_msg = MotionCommand()
        
        if self.should_stop:
            self.steering_command = 0
            self.left_speed_command = 0
            self.right_speed_command = 0
            
        motion_command_msg.steering = self.steering_command
        motion_command_msg.left_speed = self.left_speed_command
        motion_command_msg.right_speed = self.right_speed_command
        self.publisher.publish(motion_command_msg)

def main(args=None):
    ...

예시3: 주차 로직

    def lidar_callback(self, msg: Bool):
        self.lidar_data = msg.data
        
        # 첫 번째 장애물 차량 인식
        if self.lidar_data and (not self.lidar_flag):  # 처음일 때만 반응
            #self.get_logger().info("첫 번째 장애물 차량 인식")
            self.lidar_flag = True
            
        # 첫 번째 장애물 차량이 인식 + 빈자리를 인식
        if self.lidar_flag and (not self.lidar_data) and (not self.lidar_bet_flag):
            #self.get_logger().info("빈 자리 인식")
            self.lidar_bet_flag = True
        
        # 첫 번째 차량 + 빈 자리 + 다음 차량 인식 시
        if self.lidar_flag and self.lidar_bet_flag and self.lidar_data and (not self.lidar_flag2):
            self.get_logger().info('2초 후 후진 전환...')
            self.lidar_flag2 = True  
            self.delay_timer = self.create_timer(self.delay_duration, self.start_reverse)
            
    def start_reverse(self):
        self.get_logger().info('후진 모드')
        self.left_speed_command *= -1
        self.right_speed_command *= -1
        
        # 타이머가 유효한 경우에만 cancel() 호출
        if self.delay_timer is not None:
            self.delay_timer.cancel()
            self.delay_timer = None