# เจาะลึกซอร์สโค้ด (Source Code Breakdown) ในบทนี้ เราจะมาดูเบื้องหลังการทำงานของหุ่นยนต์ Batly R1 ผ่านซอร์สโค้ดหลัก 3 ส่วน ได้แก่ Firmware ของไมโครคอนโทรลเลอร์, โปรแกรมควบคุมด้วยคีย์บอร์ด และไฟล์ตั้งค่าระบบนำทาง --- ## Firmware: micro-ROS บน RP2040 (`batlyr1.ino`) โค้ดนี้ทำงานบนบอร์ด Raspberry Pi Pico (RP2040) ทำหน้าที่เป็น Low-level Controller เชื่อมต่อกับเซนเซอร์และ Actuator ต่างๆ แล้วสื่อสารกับ ROS 2 Agent ผ่านโปรโตคอล micro-ROS ### ไฟล์: `batlyr1.ino` ```{code-block} cpp /***************************************************************************************** RP2040 ALL-IN-ONE – ROS 2 VOICE ALERT, STATUS LIGHTS & SERVO SWING *****************************************************************************************/ #include #include #include #include #include #include #include #include #include #include #include // ======================= SERVO (RP2040_ISR) ======================== #if ( defined(ARDUINO_ARCH_RP2040) || defined(ARDUINO_RASPBERRY_PI_PICO) || defined(ARDUINO_ADAFRUIT_FEATHER_RP2040) || \ defined(ARDUINO_GENERIC_RP2040) ) && !defined(ARDUINO_ARCH_MBED) #if !defined(RP2040_ISR_SERVO_USING_MBED) #define RP2040_ISR_SERVO_USING_MBED false #endif #elif ( defined(ARDUINO_NANO_RP2040_CONNECT) || defined(ARDUINO_RASPBERRY_PI_PICO) || \ defined(ARDUINO_ADAFRUIT_FEATHER_RP2040) || \ defined(ARDUINO_GENERIC_RP2040) ) && defined(ARDUINO_ARCH_MBED) #if !defined(RP2040_ISR_SERVO_USING_MBED) #define RP2040_ISR_SERVO_USING_MBED true #endif #endif #include "RP2040_ISR_Servo.h" #define SERVO_PIN_A0 A0 #define MIN_MICROS 800 #define MAX_MICROS 2450 #define MIN_POS 60 #define MAX_POS 120 #define CENTER_POS 90 int servoIndex = -1; int current_servo_pos = CENTER_POS; bool servo_direction_up = true; unsigned long last_servo_ms = 0; const int servo_speed_ms = 8; // ======================= FASTLED ======================== #define DATA_PIN 17 #define NUM_LEDS 6 #define BRIGHTNESS 150 CRGB leds[NUM_LEDS]; // ======================= ULTRASONIC ===================== #define TRIG_PIN 10 #define ECHO_PIN 9 unsigned long last_ultrasonic_ms = 0; // ======================= SYSTEM STATE =================== enum State { WAITING_AGENT, AGENT_CONNECTED }; State current_state = WAITING_AGENT; bool alert_mode = false; bool is_moving = false; // ======================= IMU & TIMING =================== Madgwick filter; float g_ax, g_ay, g_az; float g_gx, g_gy, g_gz; unsigned long last_imu_ms = 0; const unsigned long imu_interval = 20; // 50Hz // ======================= micro-ROS ====================== rcl_allocator_t allocator; rclc_support_t support; rcl_node_t node; rcl_publisher_t pub_imu; rcl_publisher_t pub_alert; rcl_subscription_t sub_cmd_vel; geometry_msgs__msg__Twist msg_cmd_vel; std_msgs__msg__Bool msg_alert; rclc_executor_t executor; sensor_msgs__msg__Imu msg_imu; // CALLBACK: รับค่าความเร็วจาก ROS 2 (Teleop/Navigation) void cmd_vel_callback(const void * msgin) { const geometry_msgs__msg__Twist * msg = (const geometry_msgs__msg__Twist *)msgin; is_moving = (abs(msg->linear.x) > 0.01 || abs(msg->angular.z) > 0.01); } // ======================================================= // SERVO TASK // ======================================================= void updateServoTask() { if (servoIndex == -1) return; if (alert_mode) { // โหมดแจ้งเตือน: สวิงซ้าย-ขวาต่อเนื่องรวดเร็ว if (millis() - last_servo_ms >= servo_speed_ms) { last_servo_ms = millis(); if (servo_direction_up) { current_servo_pos += 2; if (current_servo_pos >= MAX_POS) { current_servo_pos = MAX_POS; servo_direction_up = false; } } else { current_servo_pos -= 2; if (current_servo_pos <= MIN_POS) { current_servo_pos = MIN_POS; servo_direction_up = true; } } RP2040_ISR_Servos.setPosition(servoIndex, current_servo_pos); } } else { // โหมดปกติ: กลับไปหยุดนิ่งที่ 90 องศา if (current_servo_pos != CENTER_POS) { if (millis() - last_servo_ms >= 15) { last_servo_ms = millis(); if (current_servo_pos < CENTER_POS) current_servo_pos++; else if (current_servo_pos > CENTER_POS) current_servo_pos--; RP2040_ISR_Servos.setPosition(servoIndex, current_servo_pos); } } } } // ======================================================= // 🔥 STARTUP ANIMATION (ORIGINAL) // ======================================================= void startupAnimation() { int left_out[] = {2, 1, 0}; int right_out[] = {3, 4, 5}; int left_in[] = {0, 1, 2}; int right_in[] = {5, 4, 3}; FastLED.clear(); for (int i = 0; i < 3; i++) { leds[left_out[i]] = CHSV(150, 80, 90); leds[right_out[i]] = CHSV(150, 80, 90); FastLED.show(); delay(120); } FastLED.clear(); for (int i = 0; i < 3; i++) { leds[left_in[i]] = CHSV(150, 80, 90); leds[right_in[i]] = CHSV(150, 80, 90); FastLED.show(); delay(120); } FastLED.clear(); int speed = 160; for (int i = 0; i < 3; i++) { leds[left_in[i]] = CHSV(150, 100, 120); leds[right_in[i]] = CHSV(150, 100, 120); FastLED.show(); delay(speed); speed = max(speed - 35, 45); } FastLED.clear(true); FastLED.show(); } // ======================================================= // LED TASK // ======================================================= void updateLEDTask() { static uint8_t brightness = 0; if (current_state == WAITING_AGENT) { if (brightness < 200) brightness++; fill_solid(leds, NUM_LEDS, CHSV(150, 70, brightness)); } else { if (alert_mode) { static bool blink = false; static unsigned long last_blink = 0; if(millis() - last_blink > 300) { last_blink = millis(); blink = !blink; } fill_solid(leds, NUM_LEDS, blink ? CRGB::Red : CRGB::Black); } else if (is_moving) { fill_solid(leds, NUM_LEDS, CHSV(150, 180, 200)); } else { fill_solid(leds, NUM_LEDS, CHSV(110, 140, 200)); } } FastLED.show(); } void updateIMUTask() { if (current_state != AGENT_CONNECTED) return; if (millis() - last_imu_ms < imu_interval) return; last_imu_ms = millis(); if (!IMU.accelerationAvailable() || !IMU.gyroscopeAvailable()) return; IMU.readAcceleration(g_ax, g_ay, g_az); IMU.readGyroscope(g_gx, g_gy, g_gz); msg_imu.header.stamp.sec = millis() / 1000; msg_imu.header.stamp.nanosec = (millis() % 1000) * 1000000; msg_imu.linear_acceleration.x = g_ax * 9.81; msg_imu.linear_acceleration.y = g_ay * 9.81; msg_imu.linear_acceleration.z = g_az * 9.81; rcl_publish(&pub_imu, &msg_imu, NULL); } void updateUltrasonicTask() { if (current_state != AGENT_CONNECTED) return; if (millis() - last_ultrasonic_ms < 80) return; last_ultrasonic_ms = millis(); digitalWrite(TRIG_PIN, LOW); delayMicroseconds(2); digitalWrite(TRIG_PIN, HIGH); delayMicroseconds(10); digitalWrite(TRIG_PIN, LOW); long d = pulseIn(ECHO_PIN, HIGH, 15000); int cm = (d > 0) ? (d * 0.034 / 2) : 999; bool prev_alert = alert_mode; alert_mode = (cm > 0 && cm < 45); if (alert_mode != prev_alert) { msg_alert.data = alert_mode; rcl_publish(&pub_alert, &msg_alert, NULL); } } // ======================================================= // micro-ROS RECONNECTION & ENTITIES // ======================================================= bool create_entities() { allocator = rcl_get_default_allocator(); if (rclc_support_init(&support, 0, NULL, &allocator) != RCL_RET_OK) return false; if (rclc_node_init_default(&node, "rp2040_robot_node", "", &support) != RCL_RET_OK) return false; rclc_publisher_init_default(&pub_imu, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Imu), "imu/data"); rclc_publisher_init_default(&pub_alert, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool), "ultrasonic_alert"); rclc_subscription_init_default(&sub_cmd_vel, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist), "cmd_vel"); executor = rclc_executor_get_zero_initialized_executor(); rclc_executor_init(&executor, &support.context, 1, &allocator); rclc_executor_add_subscription(&executor, &sub_cmd_vel, &msg_cmd_vel, &cmd_vel_callback, ON_NEW_DATA); return true; } void destroy_entities() { rcl_publisher_fini(&pub_imu, &node); rcl_publisher_fini(&pub_alert, &node); rcl_subscription_fini(&sub_cmd_vel, &node); rclc_executor_fini(&executor); rcl_node_fini(&node); rclc_support_fini(&support); } void updateMicroROSTask() { static unsigned long last_check = 0; if (millis() - last_check < 500) return; last_check = millis(); if (rmw_uros_ping_agent(50, 1) == RMW_RET_OK) { if (current_state == WAITING_AGENT) { if (create_entities()) current_state = AGENT_CONNECTED; else destroy_entities(); } } else if (current_state == AGENT_CONNECTED) { watchdog_reboot(0, 0, 0); } } // ======================================================= // SETUP / LOOP // ======================================================= void setup() { watchdog_enable(3000, 1); FastLED.addLeds(leds, NUM_LEDS); FastLED.setBrightness(BRIGHTNESS); pinMode(TRIG_PIN, OUTPUT); pinMode(ECHO_PIN, INPUT); // Setup Servo servoIndex = RP2040_ISR_Servos.setupServo(SERVO_PIN_A0, MIN_MICROS, MAX_MICROS); if (servoIndex != -1) { RP2040_ISR_Servos.setPosition(servoIndex, CENTER_POS); } IMU.begin(); filter.begin(50); set_microros_transports(); startupAnimation(); } void loop() { watchdog_update(); updateMicroROSTask(); if (current_state == AGENT_CONNECTED) { rclc_executor_spin_some(&executor, RCL_MS_TO_NS(5)); updateIMUTask(); updateUltrasonicTask(); } updateServoTask(); updateLEDTask(); } ``` ### อธิบายการทำงานของโค้ด (Firmware) 1. **การเชื่อมต่อ micro-ROS:** * ใช้ฟังก์ชัน `updateMicroROSTask()` เพื่อตรวจสอบสถานะการเชื่อมต่อกับ ROS 2 Agent ตลอดเวลา (Ping Agent) * ถ้า Agent หายไป (`rmw_uros_ping_agent` ไม่ตอบกลับ) บอร์ดจะสั่ง Reboot ตัวเองเพื่อพยายามเชื่อมต่อใหม่ (`watchdog_reboot`) * มีการสร้าง Entities (Publisher/Subscriber) ผ่านฟังก์ชัน `create_entities()` 2. **IMU Sensor (LSM6DSOX):** * อ่านค่า Acceleration และ Gyroscope ส่งไปที่หัวข้อ `imu/data` * ทำงานที่ความถี่ 50Hz 3. **Ultrasonic Sensor:** * วัดระยะทางเพื่อตรวจจับสิ่งกีดขวาง * หากระยะน้อยกว่า 45 cm จะเปลี่ยนตัวแปร `alert_mode` เป็น True และส่งข้อความแจ้งเตือนไปที่หัวข้อ `ultrasonic_alert` 4. **Servo Motor:** * **โหมดปกติ:** เซอร์โวจะหยุดนิ่งที่ตรงกลาง (90 องศา) * **โหมดแจ้งเตือน (Alert Mode):** เซอร์โวจะสวิงซ้าย-ขวาอย่างรวดเร็ว (เหมือนอาการตกใจ) 5. **LED Status (FastLED):** * **สีฟ้า (Breathing):** รอการเชื่อมต่อ Agent * **สีเขียวสว่าง:** เชื่อมต่อแล้วและหุ่นยนต์หยุดนิ่ง * **สีน้ำเงิน:** หุ่นยนต์กำลังเคลื่อนที่ (`is_moving` เป็น True โดยเช็คจาก `cmd_vel`) * **สีแดงกระพริบ:** แจ้งเตือนสิ่งกีดขวาง (Alert Mode) --- ## Teleoperation: การควบคุมด้วยคีย์บอร์ด (`teleop_keyboard.py`) Node นี้เขียนด้วย Python ทำหน้าที่รับค่าการกดปุ่มจากผู้ใช้ แล้วแปลงเป็นข้อความ `geometry_msgs/Twist` เพื่อสั่งความเร็วหุ่นยนต์ ### ไฟล์: `teleop_keyboard.py` ```{code-block} python #!/usr/bin/env python3 import threading import rclpy from rclpy.node import Node from geometry_msgs.msg import Twist, TwistStamped import sys from select import select if sys.platform == 'win32': import msvcrt else: import termios import tty msg = """ Reading from the keyboard and Publishing to Twist! --------------------------- Moving around: 7 8 9 4 5 6 1 2 3 q/w : increase/decrease max speeds by 0.04 and 0.2 a/s : increase/decrease only linear speed by 0.04 z/x : increase/decrease only angular speed by 0.2 CTRL-C to quit """ moveBindings = { '2': (-1, 0, 0, 0), '1': (-1, 0, 0, -1), '4': (0, 0, 0, 1), '6': (0, 0, 0, -1), '3': (-1, 0, 0, 1), '8': (1, 0, 0, 0), '7': (1, 0, 0, 1), '9': (1, 0, 0, -1), } speedBindings = { 'q': (0.04, 0.2), 'w': (-0.04, -0.2), 'a': (0.04, 0), 's': (-0.04, 0), 'z': (0, 0.2), 'x': (0, -0.2), } class TeleopTwistKeyboard(Node): def __init__(self): super().__init__('teleop_twist_keyboard') # Parameters self.speed = self.declare_parameter("speed", 0.04).value self.turn = self.declare_parameter("turn", 0.2).value self.speed_limit = self.declare_parameter("speed_limit", 2.04).value self.turn_limit = self.declare_parameter("turn_limit", 10.0).value self.key_timeout = self.declare_parameter("key_timeout", 0.5).value self.stamped = self.declare_parameter("stamped", False).value self.twist_frame = self.declare_parameter("frame_id", "").value # Publisher self.publisher = self.create_publisher(TwistStamped if self.stamped else Twist, 'cmd_vel', 10) # Initialize variables self.x = 0 self.y = 0 self.z = 0 self.th = 0 # Start publishing thread self.publish_thread = PublishThread(self.publisher, self.stamped, self.twist_frame) self.publish_thread.start() def run(self): settings = save_terminal_settings() try: print(msg) print(self.vels(self.speed, self.turn)) while True: key = get_key(settings, self.key_timeout) if key in moveBindings: self.x, self.y, self.z, self.th = moveBindings[key] elif key in speedBindings: self.speed = min(self.speed_limit, max(self.speed + speedBindings[key][0], 0.0)) self.turn = min(self.turn_limit, max(self.turn + speedBindings[key][1], 0.0)) print(self.vels(self.speed, self.turn)) else: if key == '' and self.x == 0 and self.y == 0 and self.z == 0 and self.th == 0: continue self.x, self.y, self.z, self.th = 0, 0, 0, 0 if key == '\x03': # CTRL-C break self.publish_thread.update(self.x, self.y, self.z, self.th, self.speed, self.turn) except Exception as e: self.get_logger().error(str(e)) finally: self.publish_thread.stop() restore_terminal_settings(settings) def vels(self, speed, turn): return f"currently:\tLinear {speed:.2f} m/s\tAngular {turn:.2f} rad/s" class PublishThread(threading.Thread): def __init__(self, publisher, stamped, twist_frame): super().__init__() self.publisher = publisher self.stamped = stamped self.twist_frame = twist_frame self.x = 0.0 self.y = 0.0 self.z = 0.0 self.th = 0.0 self.speed = 0.0 self.turn = 0.0 self.done = False self.condition = threading.Condition() def update(self, x, y, z, th, speed, turn): with self.condition: self.x = float(x) self.y = float(y) self.z = float(z) self.th = float(th) self.speed = float(speed) self.turn = float(turn) self.condition.notify() def run(self): while not self.done: with self.condition: self.condition.wait() msg = TwistStamped() if self.stamped else Twist() if self.stamped: msg.header.stamp = rclpy.time.Time().to_msg() msg.header.frame_id = self.twist_frame msg.linear.x = self.x * self.speed msg.linear.y = self.y * self.speed msg.linear.z = self.z msg.angular.z = self.th * self.turn self.publisher.publish(msg) def stop(self): self.done = True self.update(0, 0, 0, 0, 0, 0) def get_key(settings, timeout): if sys.platform == 'win32': return msvcrt.getwch() else: tty.setraw(sys.stdin.fileno()) rlist, _, _ = select([sys.stdin], [], [], timeout) key = sys.stdin.read(1) if rlist else '' termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) return key def save_terminal_settings(): if sys.platform == 'win32': return None return termios.tcgetattr(sys.stdin) def restore_terminal_settings(settings): if sys.platform == 'win32': return termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings) def main(): rclpy.init() node = TeleopTwistKeyboard() try: node.run() except KeyboardInterrupt: pass finally: rclpy.shutdown() if __name__ == '__main__': main() ``` ### อธิบายการทำงานของโค้ด (Teleop) 1. **การอ่านค่าคีย์บอร์ด:** ใช้ไลบรารี `termios` และ `tty` (บน Linux) เพื่ออ่านค่าจาก Keyboard แบบ Real-time โดยไม่ต้องกด Enter 2. **Mapping ปุ่มกด:** * ใช้ Numpad 8, 2, 4, 6 เพื่อเคลื่อนที่ (หน้า, หลัง, ซ้าย, ขวา) * ใช้ปุ่ม q, z เพื่อเพิ่มความเร็ว 3. **Publish Thread:** แยก Thread สำหรับการส่งค่า (`publisher.publish`) ออกจาก Thread ที่รับค่าคีย์บอร์ด เพื่อให้การส่งข้อมูลมีความต่อเนื่องแม้ผู้ใช้จะไม่ได้กดปุ่มรัวๆ --- ## Navigation Configuration (`navigation.yaml`) ไฟล์นี้เป็นหัวใจสำคัญของ Nav2 Stack ทำหน้าที่กำหนดพารามิเตอร์ของระบบนำทางทั้งหมด ทั้งเรื่องการระบุตำแหน่ง (AMCL), การวางแผนเส้นทาง (Planner), และการหลบหลีกสิ่งกีดขวาง (Costmap) ### ไฟล์: `navigation.yaml` ```{code-block} yaml amcl: ros__parameters: alpha1: 0.2 alpha2: 0.2 alpha3: 0.2 alpha4: 0.2 base_frame_id: "base_footprint" beam_skip_distance: 0.5 do_beamskip: false global_frame_id: "map" laser_likelihood_max_dist: 2.0 laser_max_range: 12.0 laser_min_range: 0.05 laser_model_type: "likelihood_field" max_particles: 2000 min_particles: 500 odom_frame_id: "odom" resample_interval: 1 robot_model_type: "nav2_amcl::DifferentialMotionModel" scan_topic: scan_filtered bt_navigator: ros__parameters: global_frame: map robot_base_frame: base_link odom_topic: /odom navigators: ["navigate_to_pose", "navigate_through_poses"] navigate_to_pose: plugin: "nav2_bt_navigator::NavigateToPoseNavigator" navigate_through_poses: plugin: "nav2_bt_navigator::NavigateThroughPosesNavigator" controller_server: ros__parameters: controller_frequency: 20.0 min_x_velocity_threshold: 0.001 min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 failure_tolerance: 0.3 progress_checker_plugins: ["progress_checker"] goal_checker_plugins: ["general_goal_checker"] controller_plugins: ["FollowPath"] general_goal_checker: stateful: True plugin: "nav2_controller::SimpleGoalChecker" xy_goal_tolerance: 0.25 yaw_goal_tolerance: 0.25 FollowPath: plugin: "nav2_rotation_shim_controller::RotationShimController" primary_controller: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" desired_linear_vel: 0.4 lookahead_dist: 0.6 rotate_to_heading_angular_vel: 1.8 max_angular_accel: 3.2 use_rotate_to_heading: true allow_reversing: false local_costmap: local_costmap: ros__parameters: update_frequency: 5.0 publish_frequency: 2.0 global_frame: odom robot_base_frame: base_link rolling_window: true width: 3 height: 3 resolution: 0.05 footprint: "[[0.225, 0.175], [0.225, -0.175], [-0.225, -0.175], [-0.225, 0.175]]" plugins: ["voxel_layer", "inflation_layer"] inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 2.5 inflation_radius: 0.65 voxel_layer: plugin: "nav2_costmap_2d::VoxelLayer" enabled: True scan: topic: /scan_filtered clearing: True marking: True global_costmap: global_costmap: ros__parameters: update_frequency: 1.0 publish_frequency: 1.0 global_frame: map robot_base_frame: base_link footprint: "[[0.225, 0.175], [0.225, -0.175], [-0.225, -0.175], [-0.225, 0.175]]" plugins: ["static_layer", "obstacle_layer", "inflation_layer"] obstacle_layer: plugin: "nav2_costmap_2d::ObstacleLayer" scan: topic: /scan_filtered clearing: True marking: True static_layer: plugin: "nav2_costmap_2d::StaticLayer" inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" cost_scaling_factor: 2.5 inflation_radius: 0.65 planner_server: ros__parameters: expected_planner_frequency: 20.0 planner_plugins: ["GridBased"] GridBased: plugin: "nav2_navfn_planner::NavfnPlanner" use_astar: false allow_unknown: true ``` ### อธิบายการตั้งค่า (Navigation Parameters) เบื้องต้น 1. **AMCL (Adaptive Monte Carlo Localization):** * ใช้โมเดล `DifferentialMotionModel` เหมาะสำหรับหุ่นยนต์ล้อคู่ * รับค่า Lidar จาก Topic `scan_filtered` เพื่อเทียบกับแผนที่และหาตำแหน่งตัวเอง 2. **Controller Server (Path Following):** * ใช้ Plugin หลักเป็น `RegulatedPurePursuitController` ซึ่งเหมาะกับหุ่นยนต์ขนาดเล็ก-กลาง * มีการครอบด้วย `RotationShimController` เพื่อให้หุ่นยนต์หมุนตัวไปหาทิศทางเป้าหมายก่อนเริ่มออกเดิน (`rotate_to_heading`) 3. **Costmaps (แผนที่ค่าใช้จ่าย):** * **Local Costmap:** แผนที่ระยะสั้นรอบตัวหุ่นยนต์ (Rolling Window 3x3 เมตร) ใช้อัปเดตสิ่งกีดขวางแบบ Real-time * **Global Costmap:** แผนที่รวมทั้งหมด ใช้สำหรับการวางแผนเส้นทางระยะไกล * มีการกำหนด `footprint` เป็นสี่เหลี่ยมผืนผ้าขนาด 0.45 x 0.35 เมตร เพื่อให้หุ่นยนต์รู้ขนาดของตัวเองเวลาลอดช่องแคบๆ ### อธิบายการตั้งค่า (Navigation Parameters) แบบละเอียด ไฟล์ `navigation.yaml` คือหัวใจสำคัญของระบบ Nav2 (Navigation 2) ทำหน้าที่กำหนดพฤติกรรมของหุ่นยนต์ Batly R1 ทั้งหมด ตั้งแต่การระบุตำแหน่ง, การวางแผนเส้นทาง, การขับเคลื่อน, ไปจนถึงความปลอดภัย ไฟล์นี้แบ่งการตั้งค่าออกเป็น Node ย่อยๆ ดังนี้: --- #### AMCL (Adaptive Monte Carlo Localization) ทำหน้าที่ระบุตำแหน่งของหุ่นยนต์ในแผนที่ (Localization) โดยใช้หลักการความน่าจะเป็น ```{code-block} yaml amcl: ros__parameters: robot_model_type: "nav2_amcl::DifferentialMotionModel" min_particles: 500 max_particles: 2000 scan_topic: scan_filtered laser_max_range: 12.0 ``` * **`robot_model_type`**: กำหนดเป็น `"nav2_amcl::DifferentialMotionModel"` สำหรับหุ่นยนต์ขับเคลื่อน 2 ล้อ (Differential Drive) * **`min_particles` / `max_particles`**: จำนวนจุดสมมติ (Particles) ที่ใช้คาดเดาตำแหน่ง * **500 - 2000**: เป็นค่าที่เหมาะสมสำหรับบอร์ดระดับ Raspberry Pi หรือ RP2040 หากค่าน้อยกว่านี้ตำแหน่งอาจเพี้ยนง่าย หากมากกว่านี้จะกิน CPU สูง * **`scan_topic`**: ใช้ข้อมูลจากหัวข้อ `scan_filtered` ซึ่งมักจะเป็นข้อมูล Lidar ที่ตัดส่วนตัวถังหุ่นยนต์ออกแล้ว * **`laser_max_range`**: ระยะไกลสุดที่เซนเซอร์เชื่อถือได้ (12 เมตร) --- #### Controller Server (Path Following) ทำหน้าที่ "ขับ" หุ่นยนต์ให้เกาะตามเส้นทางที่วางไว้ (Local Planner) ในไฟล์นี้มีการใช้เทคนิค **Controller Shim** (ตัวสวม) ซ้อนกัน 2 ชั้น: ```{code-block} yaml controller_server: ros__parameters: controller_plugins: ["FollowPath"] FollowPath: plugin: "nav2_rotation_shim_controller::RotationShimController" primary_controller: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" # การตั้งค่า Rotation Shim rotate_to_heading_angular_vel: 1.8 use_rotate_to_heading: true # การตั้งค่า Regulated Pure Pursuit desired_linear_vel: 0.4 lookahead_dist: 0.6 ``` * **Rotation Shim Controller (ตัวช่วยหมุน)** * `use_rotate_to_heading: true`: สั่งให้หุ่นยนต์ **หมุนตัวอยู่กับที่** ให้หน้าหันไปทางเป้าหมายก่อน แล้วค่อยออกตัวเดินหน้า * **`rotate_to_heading_angular_vel`**: ความเร็วในการหมุนตัว **1.8 rad/s** (ประมาณ 100 องศา/วินาที) * ประโยชน์: ช่วยให้หุ่นยนต์เดินในที่แคบได้ดีมาก ป้องกันการเอาด้านข้างไปชนขอบประตูขณะเลี้ยว * **Regulated Pure Pursuit Controller (ตัวขับหลัก)** * **`desired_linear_vel`**: ความเร็วเดินหน้าเป้าหมายที่ **0.4 m/s** * **`lookahead_dist`**: ระยะมองล่วงหน้า **0.6 เมตร** เพื่อคำนวณความโค้งในการเลี้ยว (ถ้าน้อยไปหุ่นจะส่าย ถ้ามากไปจะตัดโค้ง) --- #### Planner Server (Path Planning) ทำหน้าที่คำนวณหาเส้นทางจากจุด A ไปจุด B (Global Planner) ```{code-block} yaml planner_server: ros__parameters: planner_plugins: ["GridBased"] GridBased: plugin: "nav2_navfn_planner::NavfnPlanner" allow_unknown: true ``` * **`plugin`**: ใช้ `NavfnPlanner` (ใช้อัลกอริทึม A* หรือ Dijkstra) ซึ่งเป็นมาตรฐานที่เสถียรที่สุด * **`allow_unknown: true`**: ยอมให้วางแผนเส้นทางผ่านพื้นที่สีเทา (Unknown space) ได้ ซึ่งจำเป็นมากหากแผนที่ยังสร้างไม่สมบูรณ์ 100% --- #### Costmaps Nav2 ใช้แผนที่ 2 ชั้นเพื่อความปลอดภัยและการคำนวณที่มีประสิทธิภาพ * **Global Costmap (สำหรับวางแผนระยะไกล)** ```{code-block} yaml global_costmap: global_costmap: ros__parameters: update_frequency: 1.0 global_frame: map footprint: "[[0.225, 0.175], [0.225, -0.175], [-0.225, -0.175], [-0.225, 0.175]]" ``` * **`global_frame: map`**: อ้างอิงกับแผนที่ใหญ่ * **`update_frequency: 1.0`**: อัปเดตแค่ 1 ครั้ง/วินาที ก็พอ เพราะกำแพงไม่ขยับ * **`footprint`**: กำหนดขนาดตัวหุ่นยนต์เป็นสี่เหลี่ยมผืนผ้า (กว้าง 35cm x ยาว 45cm) เพื่อกันไม่ให้ Planner พาไปติดช่องแคบ * **Local Costmap (สำหรับหลบหลีกระยะประชิด)** ```{code-block} yaml local_costmap: local_costmap: ros__parameters: update_frequency: 5.0 global_frame: odom rolling_window: true width: 3 height: 3 inflation_layer: inflation_radius: 0.65 cost_scaling_factor: 2.5 ``` * **`global_frame: odom`**: อ้างอิงกับจุดที่หุ่นยนต์เริ่มเดิน * **`rolling_window: true`**: แผนที่ขนาด 3x3 เมตรนี้จะ **เลื่อนตามตัวหุ่นยนต์ไปเรื่อยๆ** * **`update_frequency: 5.0`**: อัปเดตเร็ว (5 ครั้ง/วินาที) เพื่อตอบสนองต่อสิ่งกีดขวางที่เคลื่อนที่ได้ เช่น คนเดินตัดหน้า * **`inflation_radius`**: รัศมี "ไข่ดาว" **0.65 เมตร** รอบสิ่งกีดขวาง เพื่อให้หุ่นยนต์พยายามเดินห่างออกมา --- #### Velocity Smoother ทำหน้าที่ปรับความเร็วให้นุ่มนวล ลดการกระชาก ```{code-block} yaml velocity_smoother: ros__parameters: max_velocity: [0.25, 0.0, 2.5] max_accel: [0.8, 0.0, 1.0] ``` * ควบคุมความเร็วสูงสุดไม่ให้เกิน **0.25 m/s** และความเร่งไม่เกิน **0.8 m/s²** เพื่อความปลอดภัยและการเคลื่อนที่ที่สมูท --- #### Collision Monitor ด่านป้องกันสุดท้าย (Emergency Stop) หากระบบอื่นล้มเหลว ```yaml collision_monitor: ros__parameters: polygons: ["FootprintApproach"] FootprintApproach: type: "polygon" action_type: "approach" time_before_collision: 1.2 ``` * **`action_type: "approach"`**: ระบบจะคำนวณล่วงหน้า * **`time_before_collision: 1.2`**: หากคำนวณแล้วว่าจะชนภายใน **1.2 วินาที** ระบบจะสั่งลดความเร็วหรือหยุดทันที โดยไม่สนคำสั่งจาก Navigation