Code Walkthrough

This launch file enables autonomous mapping of an indoor environment. It integrates:

• SLAM Toolbox: A 2D SLAM solution for mapping.
• RViz: Visualization of perception & map.
• obstacle_avoid.py: Safety node to prevent collisions.
• auto_save.py: Interactive map-saving utility.
• Keyboard Teleop: Manual driving via keyboard.

After exploration, pressing q or s triggers map saving via map_saver_cli.

Launch Snippet Explained:

Node(
    package='turtlebot4_navigation',
    executable='navigation_launch.py',
    output='screen'
),
Node(
    package='my_project',
    executable='obstacle_avoid.py',
    name='obstacle_avoid_node',
    output='screen'
),
Node(
    package='my_project',
    executable='auto_save.py',
    name='auto_save_node',
    output='screen'
)
        

• Navigation stack + SLAM.
• Custom obstacle avoidance.
• Keypress-triggered map save.

Runs after mapping is complete. Includes:

• AMCL: Localization in saved map.
• Nav2: Path planning & control.
• RViz: Live navigation status.
• esp32_goal.py: Connects to ESP32 goals.

Launch Snippet Explained:

Node(
    package='my_project',
    executable='esp32_goal.py',
    name='esp32_goal_node',
    output='screen'
)
        

• Subscribes to /esp32_07/goal & sends to Nav2.

Receives 3D goals and uses NavigateToPose action client:

Initialization:

self.goal_subscriber = this.create_subscription(
    Point,
    '/esp32_07/goal',
    this.goal_callback,
    10
)
this.marker_pub = this.create_publisher(Marker, '/goal_marker', 10)
this.action_client = ActionClient(this, NavigateToPose, '/rpi_07/navigate_to_pose')
        

Goal Callback:

def goal_callback(this, msg):
    this.get_logger().info(f"Received ESP32 goal: {msg.x}, {msg.y}, {msg.z}")
    goal_msg = NavigateToPose.Goal()
    goal_msg.pose.header.frame_id = "map"
    goal_msg.pose.header.stamp = this.get_clock().now().to_msg()
    goal_msg.pose.pose.position.x = msg.x
    goal_msg.pose.pose.position.y = msg.y
    goal_msg.pose.pose.orientation.w = 1.0
    this.publish_marker(msg.x, msg.y)
    while not this.action_client.wait_for_server(timeout_sec=1.0):
        this.get_logger().info('Waiting for NavigateToPose action server...')
    send_goal_future = this.action_client.send_goal_async(goal_msg)
    send_goal_future.add_done_callback(this.goal_response_callback)
        

Publish Marker:

def publish_marker(this, x, y):
    marker = Marker()
    marker.header.frame_id = "map"
    marker.header.stamp = this.get_clock().now().to_msg()
    marker.type = Marker.SPHERE
    marker.action = Marker.ADD
    marker.pose.position.x = x
    marker.pose.position.y = y
    marker.pose.position.z = 0.1
    marker.scale.x = 0.2
    marker.scale.y = 0.2
    marker.scale.z = 0.2
    marker.color.r = 1.0
    marker.color.g = 0.0
    marker.color.b = 0.0
    marker.color.a = 1.0
    this.marker_pub.publish(marker)
        

Monitors LiDAR and publishes cmd_vel to avoid collisions:

def scan_callback(this, msg):
    front_ranges = []
    angle_min = msg.angle_min
    angle_increment = msg.angle_increment
    for i, distance in enumerate(msg.ranges):
        angle = angle_min + i * angle_increment
        if -0.5 < angle < 0.5:
            front_ranges.append(distance)
    min_distance = min(front_ranges) if front_ranges else float('inf')
    cmd = Twist()
    if min_distance < 0.4:
        cmd.linear.x = 0.0
        cmd.angular.z = 0.5
    else:
        cmd.linear.x = 0.15
        cmd.angular.z = 0.0
    this.cmd_vel_pub.publish(cmd)
        

Init:

this.cmd_vel_pub = this.create_publisher(Twist, '/rpi_07/cmd_vel', 10)
this.scan_sub = this.create_subscription(
    LaserScan,
    '/rpi_07/scan',
    this.scan_callback,
    10
)
        

Reads serial from ESP32 and publishes as Point:

this.ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=1)
time.sleep(2)
this.publisher_ = this.create_publisher(Point, '/esp32_07/goal', 10)
        

line = this.ser.readline().decode('utf-8').strip()
match = re.search(r'X:\s*(-?\d+\.?\d*)\s*Y:\s*(-?\d+\.?\d*)\s*Z:\s*(-?\d+\.?\d*)', line)
if match:
    x, y, z = map(float, match.groups())
    point = Point()
    point.x = x
    point.y = y
    point.z = 0.0
    this.publisher_.publish(point)
        

Listens for q or s to save the map:

if key in ['q', 's']:
    save_cmd = f"ros2 run nav2_map_server map_saver_cli -f map_{timestamp}"
    os.system(save_cmd)