🏁 Project: Robot That Maps and Navigates

# 1) find the details of the adapter connected now
udevadm info -a -n /dev/ttyUSB0 | grep -m3 -E "idVendor|idProduct|serial"

# 2) create the rule (one line per device) and reload it
sudo nano /etc/udev/rules.d/99-robo.rules
#  SUBSYSTEM=="tty", ATTRS{idVendor}=="<idVendor>", ATTRS{idProduct}=="<idProduct>", ATTRS{serial}=="<serial>", SYMLINK+="lidar"
#  SUBSYSTEM=="tty", ATTRS{idVendor}=="<idVendor>", ATTRS{idProduct}=="<idProduct>", ATTRS{serial}=="<serial>", SYMLINK+="esp32"
sudo udevadm control --reload-rules && sudo udevadm trigger

# 3) same ROS 2 channel on the Pi and PC (choose a number from 0 to 101)
echo "export ROS_DOMAIN_ID=42" >> ~/.bashrc && source ~/.bashrc
ls -l /dev/lidar /dev/esp32

# Brings up the entire real robot: TF, ESP32 (micro-ROS), LiDAR, and Nav2 with SLAM or a map
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command, LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue


def generate_launch_description():
    pkg = get_package_share_directory('meu_robo')
    nav2 = get_package_share_directory('nav2_bringup')
    desc = get_package_share_directory('meu_robo_description')   # package from module 4.2
    xacro_arq = os.path.join(desc, 'urdf', 'robo.urdf.xacro')
    descricao = ParameterValue(Command(['xacro ', xacro_arq]), value_type=str)

    return LaunchDescription([
        DeclareLaunchArgument('slam', default_value='True',
                              description='True = map; False = navigate using the saved map'),
        DeclareLaunchArgument('mapa', default_value=os.path.expanduser('~/mapas/casa.yaml')),
        DeclareLaunchArgument('params', default_value=os.path.join(pkg, 'config', 'nav2_robo.yaml')),

        # 1) URDF -> fixed TFs (base_link -> laser, base_link -> imu)
        Node(package='robot_state_publisher', executable='robot_state_publisher',
             parameters=[{'robot_description': descricao}]),

        # 2) bridge to the ESP32: /ticks and /cmd_vel pass through here (module 4.3)
        Node(package='micro_ros_agent', executable='micro_ros_agent',
             arguments=['serial', '--dev', '/dev/esp32', '-b', '115200']),

        # 3) LiDAR (RPLIDAR; for the LD19, replace the package, executable, and parameters)
        Node(package='rplidar_ros', executable='rplidar_node',
             parameters=[{'serial_port': '/dev/lidar', 'serial_baudrate': 460800,
                          'frame_id': 'laser'}]),

        # 3b) odometry node from module 4.3: /ticks -> /odom and TF odom -> base_link
        Node(package='meu_robo', executable='odometria'),

        # 4) Nav2: with slam:=True, it also brings up slam_toolbox
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(os.path.join(nav2, 'launch', 'bringup_launch.py')),
            launch_arguments={'slam': LaunchConfiguration('slam'),
                              'map': LaunchConfiguration('mapa'),
                              'params_file': LaunchConfiguration('params'),
                              'use_sim_time': 'False'}.items()),
    ])

# in meu_robo's setup.py (ament_python package), add this inside data_files:
#   (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
#   (os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
# (and "import os" + "from glob import glob" at the top of setup.py)
mkdir -p ~/ros2_ws/src/meu_robo/config
cp ~/meu_nav2.yaml ~/ros2_ws/src/meu_robo/config/nav2_robo.yaml   # from module 4.5
cd ~/ros2_ws && colcon build --symlink-install && source install/setup.bash
ros2 launch meu_robo robo_completo.launch.py slam:=True

# 1) rates: /scan close to the LiDAR rate, /odom stable
ros2 topic hz /scan
ros2 topic hz /odom

# 2) does the odometry move with the robot? (push it 1 m by hand and compare)
ros2 run tf2_ros tf2_echo odom base_link

# 3) does the command reach the motors? (wheels off the ground; Ctrl+C to stop)
ros2 topic pub -r 10 /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.1}}"

# Pi 5 (SSH): complete robot in mapping mode
ros2 launch meu_robo robo_completo.launch.py slam:=True

# PC: view and drive (same ROS_DOMAIN_ID)
rviz2 &
ros2 run teleop_twist_keyboard teleop_twist_keyboard

# Pi 5 (another SSH), with the good map on screen: save
mkdir -p ~/mapas && ros2 run nav2_map_server map_saver_cli -f ~/mapas/casa

#!/usr/bin/env python3
# Usage: python3 vai_para.py cozinha   (only accepts names from the LUGARES list)
import math
import sys
import rclpy
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult

LUGARES = {                       # name: (x, y, angle in degrees) on the casa.yaml map
    'base': (0.0, 0.0, 0.0),
    'sala': (1.2, 0.8, 0.0),
    'cozinha': (3.0, 1.5, 90.0),
    'quarto': (0.5, -1.5, 180.0),
}


def main():
    nome = sys.argv[1].lower() if len(sys.argv) > 1 else ''
    if nome not in LUGARES:
        print('Unknown place. Use one of these:', ', '.join(LUGARES))
        return
    x, y, graus = LUGARES[nome]

    rclpy.init()
    nav = BasicNavigator()
    nav.waitUntilNav2Active()
    alvo = PoseStamped()
    alvo.header.frame_id = 'map'
    alvo.header.stamp = nav.get_clock().now().to_msg()
    alvo.pose.position.x, alvo.pose.position.y = x, y
    alvo.pose.orientation.z = math.sin(math.radians(graus) / 2)
    alvo.pose.orientation.w = math.cos(math.radians(graus) / 2)

    print(f'Going to {nome}...')
    nav.goToPose(alvo)
    while not nav.isTaskComplete():
        pass
    ok = nav.getResult() == TaskResult.SUCCEEDED
    print('Cheguei.' if ok else 'I could not get there.')
    rclpy.shutdown()


if __name__ == '__main__':
    main()