Deployment Guide

#!/bin/bash
# File: ~/start_robot.sh

echo "=== Mecanum Robot Startup ==="
echo

# Navigate to workspace
cd ~/ros2_ws
source install/setup.bash

# Check battery voltage (optional: requires voltage sensor)
# ./scripts/check_battery.sh

echo "Starting robot system..."
echo "Press Ctrl+C to stop"
echo

# Launch complete system
ros2 launch mecanum_description robot_navigation.launch.py \
  map:=/home/ubuntu/ros2_ws/src/mecanum_description/maps/demo_map.yaml

chmod +x ~/start_robot.sh

./start_robot.sh

# File: /etc/systemd/system/mecanum-robot.service

[Unit]
Description=Mecanum Robot Navigation System
After=network.target

[Service]
Type=simple
User=ubuntu
WorkingDirectory=/home/ubuntu/ros2_ws
Environment="ROS_DOMAIN_ID=0"
ExecStart=/bin/bash -c 'source /home/ubuntu/ros2_ws/install/setup.bash && ros2 launch mecanum_description robot_navigation.launch.py map:=/home/ubuntu/ros2_ws/src/mecanum_description/maps/demo_map.yaml'
Restart=on-failure
RestartSec=10

[Install]
WantedBy=multi-user.target

sudo systemctl daemon-reload
sudo systemctl enable mecanum-robot.service
sudo systemctl start mecanum-robot.service

# Check status
sudo systemctl status mecanum-robot.service

sudo systemctl disable mecanum-robot.service

# Launch robot (if not already running)
ros2 launch mecanum_description robot_navigation.launch.py

# Define waypoints (x, y, yaw in map frame)
ros2 run mecanum_description waypoint_follower \
  --waypoints "[[2.0, 1.0, 0.0], [4.0, 2.0, 1.57], [3.0, 3.0, 3.14], [0.0, 0.0, 0.0]]" \
  --loop false

#!/usr/bin/env python3
# File: ~/missions/patrol_mission.py

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseStamped
from nav2_msgs.action import NavigateToPose
from rclpy.action import ActionClient
import math

class PatrolMission(Node):
    def __init__(self):
        super().__init__('patrol_mission')
        self.nav_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')

        # Define patrol waypoints (x, y, yaw)
        self.waypoints = [
            (2.0, 1.0, 0.0),
            (4.0, 2.0, math.pi/2),
            (3.0, 3.0, math.pi),
            (1.0, 2.5, -math.pi/2),
            (0.0, 0.0, 0.0)  # Return to start
        ]

    def run_patrol(self):
        for i, (x, y, yaw) in enumerate(self.waypoints):
            self.get_logger().info(f'Navigating to waypoint {i+1}/{len(self.waypoints)}: ({x}, {y}, {yaw})')
            success = self.navigate_to_pose(x, y, yaw)

            if not success:
                self.get_logger().error(f'Failed to reach waypoint {i+1}. Aborting mission.')
                return False

            self.get_logger().info(f'Reached waypoint {i+1}')

        self.get_logger().info('Patrol mission completed successfully!')
        return True

    def navigate_to_pose(self, x, y, yaw):
        goal_msg = NavigateToPose.Goal()
        goal_msg.pose.header.frame_id = 'map'
        goal_msg.pose.header.stamp = self.get_clock().now().to_msg()
        goal_msg.pose.pose.position.x = x
        goal_msg.pose.pose.position.y = y
        goal_msg.pose.pose.orientation.z = math.sin(yaw / 2.0)
        goal_msg.pose.pose.orientation.w = math.cos(yaw / 2.0)

        self.nav_client.wait_for_server()
        send_goal_future = self.nav_client.send_goal_async(goal_msg)
        rclpy.spin_until_future_complete(self, send_goal_future)

        goal_handle = send_goal_future.result()
        if not goal_handle.accepted:
            return False

        result_future = goal_handle.get_result_async()
        rclpy.spin_until_future_complete(self, result_future)

        return result_future.result().status == 4  # SUCCEEDED

def main():
    rclpy.init()
    mission = PatrolMission()
    mission.run_patrol()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

python3 ~/missions/patrol_mission.py

# Keyboard teleop
ros2 run teleop_twist_keyboard teleop_twist_keyboard \
  --ros-args -r /cmd_vel:=/mecanum_drive_controller/cmd_vel

# Or joystick (if available)
ros2 run joy joy_node
ros2 run teleop_twist_joy teleop_node

Press red emergency stop button
→ Motors power disconnected immediately
→ Robot stops instantly
→ Raspberry Pi remains on (safe shutdown)

# Stop all motion
ros2 topic pub --once /mecanum_drive_controller/cmd_vel \
  geometry_msgs/msg/Twist \
  "{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}"

# Cancel navigation goal
ros2 action send_goal /navigate_to_pose \
  nav2_msgs/action/NavigateToPose \
  "{}" --cancel

# If software stop doesn't work, kill navigation nodes
ros2 lifecycle set /planner_server shutdown
ros2 lifecycle set /controller_server shutdown
ros2 lifecycle set /bt_navigator shutdown

# Graceful shutdown
ros2 launch mecanum_description robot_navigation.launch.py --shutdown

# Or force kill
pkill -9 -f ros2

# Power off Raspberry Pi
sudo shutdown now

# config/nav2_params.yaml

controller_server:
  ros__parameters:
    # Velocity limits (conservative for safety)
    max_vel_x: 0.4        # Reduce from 0.5 m/s
    max_vel_y: 0.4
    max_vel_theta: 0.8    # Reduce from 1.0 rad/s

    # Acceleration limits
    acc_lim_x: 2.0        # Reduce from 2.5 m/s²
    acc_lim_y: 2.0
    acc_lim_theta: 2.5    # Reduce from 3.2 rad/s²

local_costmap:
  local_costmap:
    ros__parameters:
      # Larger safety buffer
      robot_radius: 0.25   # Increase from 0.22 m (inflate robot size)
      inflation_radius: 0.60  # Increase from 0.55 m

# Watch for unexpected forces (IMU acceleration spikes)
ros2 topic echo /imu/data --field linear_acceleration

# If accel > 20 m/s² → likely collision, stop robot

class CollisionDetector(Node):
    def __init__(self):
        super().__init__('collision_detector')
        self.subscription = self.create_subscription(
            Imu, '/imu/data', self.imu_callback, 10)
        self.cmd_pub = self.create_publisher(
            Twist, '/mecanum_drive_controller/cmd_vel', 10)

    def imu_callback(self, msg):
        accel_magnitude = math.sqrt(
            msg.linear_acceleration.x**2 +
            msg.linear_acceleration.y**2 +
            msg.linear_acceleration.z**2
        )

        if accel_magnitude > 20.0:  # Collision threshold
            self.get_logger().error('COLLISION DETECTED! Stopping robot.')
            # Stop robot
            stop_msg = Twist()
            self.cmd_pub.publish(stop_msg)

rviz2 -d ~/ros2_ws/src/mecanum_description/rviz/monitoring.rviz

#!/bin/bash
# File: ~/scripts/health_check.sh

echo "=== System Health Check ==="
echo

# Check nodes
echo "Checking critical nodes..."
NODES="controller_manager mecanum_drive_controller rplidar_node planner_server controller_server bt_navigator"
for node in $NODES; do
  if ros2 node list | grep -q $node; then
    echo "✓ $node running"
  else
    echo "✗ $node NOT running"
  fi
done

echo
echo "Checking sensor rates..."
# LiDAR
echo -n "LiDAR (/scan): "
timeout 5 ros2 topic hz /scan 2>&1 | grep "average rate" || echo "NOT publishing"

# Odometry
echo -n "Odometry (/odometry/filtered): "
timeout 5 ros2 topic hz /odometry/filtered 2>&1 | grep "average rate" || echo "NOT publishing"

echo
echo "Checking TF tree..."
ros2 run tf2_ros tf2_echo map base_link 2>&1 | head -n 5

echo
echo "=== Health Check Complete ==="

./scripts/health_check.sh

# Set log level
export RCUTILS_CONSOLE_OUTPUT_FORMAT="[{severity}] [{name}]: {message}"
export RCUTILS_COLORIZED_OUTPUT=1

# Launch with logging
ros2 launch mecanum_description robot_navigation.launch.py \
  --log-level DEBUG

# Or per-node logging
ros2 run --log-level INFO <package> <node>

# Record all topics
ros2 bag record -a -o robot_session_$(date +%Y%m%d_%H%M%S)

# Or specific topics
ros2 bag record /scan /odom/filtered /cmd_vel /plan