🏁 Projeto: robô que mapeia e navega

# 1) descubra os dados do adaptador ligado agora
udevadm info -a -n /dev/ttyUSB0 | grep -m3 -E "idVendor|idProduct|serial"

# 2) crie a regra (uma linha por dispositivo) e recarregue
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) mesmo canal ROS 2 no Pi e no PC (escolha um número de 0 a 101)
echo "export ROS_DOMAIN_ID=42" >> ~/.bashrc && source ~/.bashrc
ls -l /dev/lidar /dev/esp32

# Sobe o robô real inteiro: TF, ESP32 (micro-ROS), LiDAR e Nav2 com SLAM ou mapa
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')   # pacote do módulo 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 = mapear; False = navegar no mapa salvo'),
        DeclareLaunchArgument('mapa', default_value=os.path.expanduser('~/mapas/casa.yaml')),
        DeclareLaunchArgument('params', default_value=os.path.join(pkg, 'config', 'nav2_robo.yaml')),

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

        # 2) ponte com o ESP32: /ticks e /cmd_vel passam por aqui (módulo 4.3)
        Node(package='micro_ros_agent', executable='micro_ros_agent',
             arguments=['serial', '--dev', '/dev/esp32', '-b', '115200']),

        # 3) LiDAR (RPLIDAR; para o LD19 troque pacote, executável e parâmetros)
        Node(package='rplidar_ros', executable='rplidar_node',
             parameters=[{'serial_port': '/dev/lidar', 'serial_baudrate': 460800,
                          'frame_id': 'laser'}]),

        # 3b) nó odometria do módulo 4.3: /ticks -> /odom e TF odom -> base_link
        Node(package='meu_robo', executable='odometria'),

        # 4) Nav2: com slam:=True ele também sobe o 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()),
    ])

# no setup.py do meu_robo (pacote ament_python), dentro de data_files, acrescente:
#   (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
#   (os.path.join('share', package_name, 'config'), glob('config/*.yaml')),
# (e "import os" + "from glob import glob" no topo do setup.py)
mkdir -p ~/ros2_ws/src/meu_robo/config
cp ~/meu_nav2.yaml ~/ros2_ws/src/meu_robo/config/nav2_robo.yaml   # do módulo 4.5
cd ~/ros2_ws && colcon build --symlink-install && source install/setup.bash
ros2 launch meu_robo robo_completo.launch.py slam:=True

# 1) taxas: /scan perto da taxa do LiDAR, /odom estável
ros2 topic hz /scan
ros2 topic hz /odom

# 2) a odometria anda junto com o robô? (empurre 1 m com a mão e compare)
ros2 run tf2_ros tf2_echo odom base_link

# 3) o comando chega aos motores? (rodas no ar; Ctrl+C para parar)
ros2 topic pub -r 10 /cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.1}}"

# Pi 5 (SSH): robô completo em modo mapeamento
ros2 launch meu_robo robo_completo.launch.py slam:=True

# PC: ver e dirigir (mesmo ROS_DOMAIN_ID)
rviz2 &
ros2 run teleop_twist_keyboard teleop_twist_keyboard

# Pi 5 (outro SSH), com o mapa bom na tela: salvar
mkdir -p ~/mapas && ros2 run nav2_map_server map_saver_cli -f ~/mapas/casa

#!/usr/bin/env python3
# Uso: python3 vai_para.py cozinha   (só aceita nomes da lista LUGARES)
import math
import sys
import rclpy
from geometry_msgs.msg import PoseStamped
from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult

LUGARES = {                       # nome: (x, y, ângulo em graus) no mapa casa.yaml
    '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('Lugar desconhecido. Use um destes:', ', '.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'Indo para {nome}...')
    nav.goToPose(alvo)
    while not nav.isTaskComplete():
        pass
    ok = nav.getResult() == TaskResult.SUCCEEDED
    print('Cheguei.' if ok else 'Não consegui chegar.')
    rclpy.shutdown()


if __name__ == '__main__':
    main()