===============================================================================
                     BEETLEBOT AUTONOMOUS ROBOTICS MANUAL
           ROS 2 Jazzy / Lyra Platform — Experiments 1, 2, and 3
===============================================================================

This document is the dedicated operational guide for conducting the three
autonomous robotics experiments on the BeetleBot (Lyra mobile robot platform)
powered by ROS 2 Jazzy.

-------------------------------------------------------------------------------
TABLE OF CONTENTS
-------------------------------------------------------------------------------
1. EXPERIMENT 1: 2D LiDAR Autonomous Obstacle Avoidance (obstacle_avoidance.py)
2. EXPERIMENT 2: Vision-Based Autonomous Navigation (color_navigation.py & arrow_navigation.py)
3. EXPERIMENT 3: Autonomous 2D LiDAR SLAM, Frontier Exploration & Landmark Mapping (beetle_slam_mapper.py)
4. EMERGENCY STOP & DISARM PROCEDURES

===============================================================================
EXPERIMENT 1: 2D LiDAR AUTONOMOUS OBSTACLE AVOIDANCE
===============================================================================
File: termchat/codes/python/obstacle_avoidance.py

Objective:
  Implement autonomous reactive obstacle avoidance using a 360-degree 2D LiDAR
  sensor (RPLidar S1) on ROS 2 Jazzy. The robot navigates unknown environments,
  continuously monitors distances in left, front, and right sectors, and executes
  an intelligent "reverse-before-turn" maneuver when facing dead ends.

-------------------------------------------------------------------------------
TERMINAL 1: BRING UP THE ROBOT HARDWARE AND LIDAR
-------------------------------------------------------------------------------
1. Connect laptop and BeetleBot to the same WiFi network and verify:
     ping beetlebot-124.local (or ping 192.168.0.128)

2. Open Terminal 1 on your PC and SSH into the robot:
     ssh veerobot@beetlebot-124.local

3. Source environments and launch main robot driver and laser system:
     source /opt/ros/jazzy/setup.bash
     source ~/lyra_ws/install/setup.bash
     ros2 launch lyra_bringup robot.launch.py

   (Optional launch variants depending on your lab setup:)
     ros2 launch lyra_bringup robot.launch.py camera:=true
     ros2 launch lyra_bringup robot.launch.py mode:=slam camera:=true imu:=true

* Keep Terminal 1 running in the background to handle the robot hardware!

-------------------------------------------------------------------------------
TERMINAL 2: LAUNCH AUTONOMOUS DRIVING (CHOOSE METHOD A, B, OR C)
-------------------------------------------------------------------------------
Open a SECOND terminal window on your PC and SSH into the robot:
  ssh veerobot@beetlebot-124.local

Prerequisite:
  Ensure termchat-client is installed on the robot:
    pip install --upgrade termchat-client --break-system-packages

-------------------------------------------------------------------------------
METHOD A: THE 1-COMMAND ZERO-EFFORT METHOD (RECOMMENDED)
-------------------------------------------------------------------------------
No opening nano, no holding Ctrl+K, no pasting, no colcon build!
Just run:
  termchat run obstacle

Or if you prefer python module syntax:
  source /opt/ros/jazzy/setup.bash
  python3 -m termchat.codes run obstacle

What happens:
  - The exact Obstacle Avoidance node launches immediately.
  - Subscribes to /scan and publishes velocity commands to /cmd_vel_nav.
  - Implements front/left/right sector calculation, obstacle warnings,
    and automatic reverse-before-turn maneuvers.
  - Press [Ctrl + C] to safely halt all wheels and stop the node.

-------------------------------------------------------------------------------
METHOD B: 1-CLICK AUTOMATED WORKSPACE SETUP (FOR MADAM INSPECTION)
-------------------------------------------------------------------------------
If your lab instructor / madam requires the code to live specifically inside
`~/lyra_ws/src/lyra_control/lyra_control/obstacle_avoidance.py` and wants you
to run `ros2 run lyra_control obstacle_avoid`:

DO NOT spend time manually opening nano, deleting code, and editing setup.py!
Just run this one automated command:
  termchat setup-lyra

This automated command instantly:
  1. Copies obstacle_avoidance.py directly into ~/lyra_ws/src/lyra_control/lyra_control/
  2. Sets executable permissions (chmod +x)
  3. Registers the console_scripts entry point ('obstacle_avoid = ...') in setup.py
  4. Runs colcon build --packages-select lyra_control automatically!

Then you (or your madam) can simply execute:
  source ~/lyra_ws/install/setup.bash
  ros2 run lyra_control obstacle_avoid
  (or run: python3 ~/lyra_ws/src/lyra_control/lyra_control/obstacle_avoidance.py)

-------------------------------------------------------------------------------
METHOD C: MANUAL NANO PASTE & BUILD (MADAM'S MANUAL PROTOCOL)
-------------------------------------------------------------------------------
If you want to perform the manual steps manually:
  1. cd ~/lyra_ws/src/lyra_control/lyra_control/
  2. nano obstacle_avoidance.py
     (Hold Ctrl + K until completely empty, paste code from termchat/codes/python/obstacle_avoidance.py, Ctrl+O, Enter, Ctrl+X)
  3. chmod +x obstacle_avoidance.py
  4. nano ~/lyra_ws/src/lyra_control/setup.py
     Add under 'console_scripts':
       'obstacle_avoid = lyra_control.obstacle_avoidance:main',
  5. cd ~/lyra_ws
     colcon build --packages-select lyra_control
     source install/setup.bash
  6. ros2 run lyra_control obstacle_avoid

  Key Parameters Configured in the Script:
    - safe_distance  = 0.50 m  (General clearance boundary)
    - front_distance = 0.45 m  (Direct frontal collision threshold)
    - move_speed     = 0.20 m/s (Nominal cruising velocity)
    - reverse_speed  = 0.12 m/s (Back-up escape speed)
    - turn_speed     = 0.40 rad/s (Yaw rotation speed)
    - reverse_time   = 0.80 s  (Duration of backing up before turning)

State Machine Behavior:
  1. Front clear (> 0.45 m) and sides clear (> 0.50 m):
     -> Drive straight forward at move_speed.
  2. Front partially constrained:
     -> Compare left distance vs right distance. Turn toward the side with greater clearance.
  3. Front obstacle detected (<= 0.45 m):
     -> Reverse for 0.8 seconds at reverse_speed.
     -> Re-evaluate left vs right LiDAR ranges.
     -> Turn toward the clearer direction until front is clear.

Stopping & Disarming:
  Press Ctrl + C in the Python node terminal. The node automatically sends zero-velocity
  messages to /cmd_vel_nav before exiting.
  Disarm:
    ros2 service call /lyra/disarm std_srvs/srv/Trigger


===============================================================================
EXPERIMENT 2: VISION-BASED AUTONOMOUS NAVIGATION
===============================================================================
Files:
  - termchat/codes/python/color_navigation.py
  - termchat/codes/python/arrow_navigation.py

Objective:
  Drive the BeetleBot using real-time computer vision processing through the onboard
  monocular camera, responding dynamically to color signboards and directional arrows.

Step 1: Connect & Prepare System
  Verify connectivity:
    ping 192.168.0.122
    ssh veerobot@192.168.0.122

  Ensure dependencies are installed on BeetleBot / companion computer:
    sudo apt update
    sudo apt install -y python3-opencv python3-numpy ros-jazzy-geometry-msgs

Step 2: Launch Robot Base
  Terminal 1:
    source /opt/ros/jazzy/setup.bash
    source /home/veerobot/lyra_ws/install/setup.bash
    ros2 launch lyra_bringup robot.launch.py

  Terminal 2 (Arm Robot):
    source /opt/ros/jazzy/setup.bash
    source /home/veerobot/lyra_ws/install/setup.bash
    ros2 service call /lyra/arm std_srvs/srv/Trigger

-------------------------------------------------------------------------------
PART 2A: COLOR CARD NAVIGATION (color_navigation.py)
-------------------------------------------------------------------------------
Description:
  Uses HSV color segmentation and morphological filtering to detect Green, Yellow,
  and Red navigation cards held in front of the camera:
    - GREEN Card  -> Robot advances forward (linear: 0.20 m/s)
    - YELLOW Card -> Robot slows down to caution speed (linear: 0.08 m/s)
    - RED Card    -> Robot halts immediately (linear: 0.0 m/s, angular: 0.0 rad/s)
    - NO Card     -> Robot remains stopped for safety

Execution:
  python3 python/color_navigation.py

  An OpenCV visualization window displays the camera stream with segmented color contours
  and active navigation states. Press 'q' in the camera window to exit.

-------------------------------------------------------------------------------
PART 2B: ARROW DIRECTION NAVIGATION (arrow_navigation.py)
-------------------------------------------------------------------------------
Description:
  Detects directional arrows (black arrow on white background or colored arrows)
  using OpenCV contour approximation, vertex counting, and centroid-to-tip vector analysis:
    - LEFT Arrow Detected  -> Robot turns left (angular.z = +0.50 rad/s)
    - RIGHT Arrow Detected -> Robot turns right (angular.z = -0.50 rad/s)
    - VERTICAL / Ambiguous -> Robot halts (linear: 0.0 m/s, angular: 0.0 rad/s)

Execution:
  python3 python/arrow_navigation.py

  Press 'q' in the preview window to exit.

Monitoring Commands in Real Time:
  Open an additional terminal to observe velocity commands dispatched to the motors:
    source /opt/ros/jazzy/setup.bash
    ros2 topic echo /cmd_vel_nav


===============================================================================
EXPERIMENT 3: 2D LiDAR SLAM, FRONTIER EXPLORATION & LANDMARK MAPPING
===============================================================================
File: termchat/codes/python/beetle_slam_mapper.py

Objective:
  Complete end-to-end autonomous Simultaneous Localization and Mapping (SLAM)
  in ROS 2 Jazzy. The BeetleBot autonomously charts an unknown laboratory floorplan,
  utilizes frontier-based exploration to systematically visit unmapped boundaries,
  incorporates camera vision for floor obstacles and ArUco visual fiducial landmarks,
  and persists the resulting occupancy grid map to disk.

Architecture:
  - 2D LiDAR: 360-degree RPLidar scans published to /scan.
  - Odometry & TF: /odom frame to /base_link with wheel encoder feedback.
  - SLAM Engine: slam_toolbox / cartographer producing /map topic.
  - Autonomous Planner: Frontier detection (wavefront edge clustering) calculating
    optimal exploration waypoints and navigating safely without human intervention.
  - Camera Vision: Monocular camera (/camera/image_raw) segmenting floor navigable area
    and detecting ArUco landmark markers for pose graph enhancement.

Step 1: Network & Robot Bringup with SLAM Mode
  In Terminal 1 on BeetleBot:
    source /opt/ros/jazzy/setup.bash
    source /home/veerobot/lyra_ws/install/setup.bash
    ros2 launch lyra_bringup robot.launch.py mode:=slam camera:=true imu:=true

Step 2: Arm the Robot
  In Terminal 2:
    source /opt/ros/jazzy/setup.bash
    source /home/veerobot/lyra_ws/install/setup.bash
    ros2 service call /lyra/arm std_srvs/srv/Trigger

Step 3: Launch RViz2 Visualization (on Workstation / Laptop)
  Export ROS_DOMAIN_ID matching the robot (default 0):
    source /opt/ros/jazzy/setup.bash
    rviz2 -d /home/veerobot/lyra_ws/src/lyra_description/rviz/slam.rviz

  Displays:
    - Occupancy Grid Map (/map)
    - LaserScan point cloud (/scan)
    - Robot model & TF frames (map -> odom -> base_link -> laser)
    - Autonomous navigation goal poses

Step 4: Run the Complete SLAM & Frontier Explorer Node
  In Terminal 2 on the robot (or companion computer):
    python3 python/beetle_slam_mapper.py

Key Capabilities of beetle_slam_mapper.py:
  1. Automatic Start-Up Calibration:
     - Slowly creeps ~0.20 m and rotates ~40 degrees while comparing wheel odometry
       to scan correlation, verifying motor polarity and LiDAR coordinate frames.
  2. Dynamic Frontier Extraction:
     - Analyzes /map OccupancyGrid cells: finds frontiers between known free space (0)
       and unknown territory (-1). Clusters frontier points and selects highest-utility goal.
  3. Reactive Obstacle Fusion:
     - Merges real-time 2D laser scan readings with camera floor segmentation to prevent
       trapping near low-profile obstacles below LiDAR sight lines.
  4. ArUco Visual Landmark Integration:
     - Recognizes predefined ArUco dictionary markers placed in the room and logs
       precise coordinates relative to the /map frame.
  5. Continuous Map Serialization:
     - Regularly saves progress and writes final high-resolution map files.

Step 5: Export and Save the Final Map
  When exploration is complete, save the occupancy grid map:
    mkdir -p ~/maps
    ros2 run nav2_map_server map_saver_cli -f ~/maps/pab_map

  This creates:
    - ~/maps/pab_map.yaml (Map metadata, resolution, origin)
    - ~/maps/pab_map.pgm  (Greyscale 2D occupancy grid image)

Step 6: Shutdown & Disarm
  - Press Ctrl + C in the beetle_slam_mapper.py terminal.
  - Disarm motors:
      ros2 service call /lyra/disarm std_srvs/srv/Trigger


===============================================================================
EMERGENCY STOP & DISARM PROCEDURES
===============================================================================
In any unexpected robot motion, execute one of the following emergency commands:

1. Immediate Motor Zero-Command:
   ros2 topic pub --once /cmd_vel_nav geometry_msgs/msg/Twist "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"

2. Service Disarm:
   ros2 service call /lyra/disarm std_srvs/srv/Trigger

3. Emergency Flood Stop (20 Hz forced halt):
   ros2 topic pub -r 20 -t 40 /cmd_vel_nav geometry_msgs/msg/Twist "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"

4. Hardware E-Stop:
   Switch off the physical battery switch on the Lyra chassis or disconnect the motor battery connector.


===============================================================================
VEEROBOT BEETLE BOT (LYRA) - STEP-BY-STEP OPERATION & MANUAL TESTING
===============================================================================
Target Hardware IP: 192.168.0.128
SSH Account:        veerobot
ROS 2 Version:      Jazzy
Workspace:          /home/veerobot/lyra_ws

STEP 1: NETWORK SUBMAPPING AND VERIFICATION
  ping 192.168.0.128

STEP 2: LAUNCHING ROBOT HARDWARE BASE INTERFACES (TERMINAL 1)
  ssh veerobot@192.168.0.128
  source /opt/ros/jazzy/setup.bash
  source /home/veerobot/lyra_ws/install/setup.bash
  ros2 launch lyra_bringup robot.launch.py

STEP 3: OVERRIDING PROTECTION CIRCUITS & MANUAL MOTOR TESTING (TERMINAL 2)
  ssh veerobot@192.168.0.128
  source /opt/ros/jazzy/setup.bash
  source /home/veerobot/lyra_ws/install/setup.bash

  # Electronic arming call:
  ros2 service call /lyra/arm std_srvs/srv/Trigger

  # Forward hardware pulse test (0.3 m/s):
  ros2 topic pub --once /cmd_vel_nav geometry_msgs/msg/Twist "{linear: {x: 0.3, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"


===============================================================================
ROS 2 AUTOMATED ROBOT MOVEMENT COMMANDS
===============================================================================

1. MOVE BACKWARD (5 SECONDS + 1.5S FORCED STOP)
-------------------------------------------------------------------------------
ros2 topic pub -r 10 -t 50 /cmd_vel_nav geometry_msgs/msg/Twist "{linear: {x: -0.3, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" && ros2 topic pub -r 10 -t 15 /cmd_vel_nav geometry_msgs/msg/Twist "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"

(Shortcut: termchat run backward)


2. MOVE FORWARD (5 SECONDS + 1.5S FORCED STOP)
-------------------------------------------------------------------------------
ros2 topic pub -r 10 -t 50 /cmd_vel_nav geometry_msgs/msg/Twist "{linear: {x: 0.3, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" && ros2 topic pub -r 10 -t 15 /cmd_vel_nav geometry_msgs/msg/Twist "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"

(Shortcut: termchat run forward)


3. EMERGENCY FLOOD STOP COMMAND (FORCES HARDWARE TO HALT)
-------------------------------------------------------------------------------
ros2 topic pub -r 20 -t 40 /cmd_vel_nav geometry_msgs/msg/Twist "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"

(Shortcut: termchat run stop)


4. ADVANCED SEQUENCE (FORWARD 5S -> STOP -> BACKWARD 5S -> STOP)
-------------------------------------------------------------------------------
ros2 topic pub -r 10 -t 50 /cmd_vel_nav geometry_msgs/msg/Twist "{linear: {x: 0.3, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" && ros2 topic pub -r 10 -t 15 /cmd_vel_nav geometry_msgs/msg/Twist "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" && ros2 topic pub -r 10 -t 50 /cmd_vel_nav geometry_msgs/msg/Twist "{linear: {x: -0.3, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" && ros2 topic pub -r 10 -t 20 /cmd_vel_nav geometry_msgs/msg/Twist "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"

(Shortcut: termchat run seq)


5. SQUARE PATH SEQUENCE (4 SIDES: DRIVE -> STOP -> TURN -> STOP)
-------------------------------------------------------------------------------
ros2 topic pub -r 10 -t 30 /cmd_vel_nav geometry_msgs/msg/Twist "{linear: {x: 0.3, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" && ros2 topic pub -r 10 -t 10 /cmd_vel_nav geometry_msgs/msg/Twist "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" && ros2 topic pub -r 10 -t 20 /cmd_vel_nav geometry_msgs/msg/Twist "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.5}}" && ros2 topic pub -r 10 -t 10 /cmd_vel_nav geometry_msgs/msg/Twist "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" && ros2 topic pub -r 10 -t 30 /cmd_vel_nav geometry_msgs/msg/Twist "{linear: {x: 0.3, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" && ros2 topic pub -r 10 -t 10 /cmd_vel_nav geometry_msgs/msg/Twist "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" && ros2 topic pub -r 10 -t 20 /cmd_vel_nav geometry_msgs/msg/Twist "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.5}}" && ros2 topic pub -r 10 -t 10 /cmd_vel_nav geometry_msgs/msg/Twist "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" && ros2 topic pub -r 10 -t 30 /cmd_vel_nav geometry_msgs/msg/Twist "{linear: {x: 0.3, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" && ros2 topic pub -r 10 -t 10 /cmd_vel_nav geometry_msgs/msg/Twist "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" && ros2 topic pub -r 10 -t 20 /cmd_vel_nav geometry_msgs/msg/Twist "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.5}}" && ros2 topic pub -r 10 -t 10 /cmd_vel_nav geometry_msgs/msg/Twist "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" && ros2 topic pub -r 10 -t 30 /cmd_vel_nav geometry_msgs/msg/Twist "{linear: {x: 0.3, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}" && ros2 topic pub -r 10 -t 15 /cmd_vel_nav geometry_msgs/msg/Twist "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"

(Shortcut: termchat run square)


===============================================================================
ROS 2 JAZZY - KEYBOARD TELEOP STEP-BY-STEP GUIDE
===============================================================================

Target Topic: /cmd_vel_nav

STEP 1: SOURCE AND RUN THE NODE
-------------------------------------------------------------------------------
Source your ROS 2 Jazzy environment and launch the node. The default topic 
'cmd_vel' is explicitly remapped to match your robot's 'cmd_vel_nav' topic:

    source /opt/ros/jazzy/setup.bash
    ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r cmd_vel:=cmd_vel_nav

    (Shortcut: termchat run teleop)


STEP 2: KEYBOARD CONTROLS REFERENCE
-------------------------------------------------------------------------------
Keep the teleop terminal window selected and active. Use these keys to control:

    [ q ]  [ w ]  [ e ]      q/z : Increase/decrease max speeds by 10%
    [ a ]  [ s ]  [ d ]      w/x : Increase/decrease only linear speed by 10%
                             e/c : Increase/decrease only angular speed by 10%
    [ i ]  [ o ]  [ p ]      
    [ j ]  [ k ]  [ l ]      i   : Move Forward
    [ m ]  [ , ]  [ . ]      ,   : Move Backward
                             j   : Turn Left
                             l   : Turn Right
                             k   : Stop Completely (Zero velocities)

                             u/o/m/. : Diagonal moving combinations


STEP 3: VERIFICATION (OPTIONAL)
-------------------------------------------------------------------------------
To monitor the active velocity commands being sent out, open a separate terminal, 
source your environment, and echo the topic:

    source /opt/ros/jazzy/setup.bash
    ros2 topic echo /cmd_vel_nav

===============================================================================
EOF
