ros2-robotics-navigation

ROS 2 Robotics & Nav2 Autonomous Navigation Guidelines

This skill details node design, DDS Quality of Service (QoS) tuning, coordinate transformation frame trees (tf2), Nav2 architecture configuration, and real-time execution safety for ROS 2 autonomous mobile robots (AMR).

1. ROS 2 Architecture & Node Design

1.1 Lifecycle Node Management (C++)

Managed Lifecycle Nodes transition explicitly through states (Unconfigured, Inactive, Active, Finalized), ensuring hardware drivers and controllers initialize predictably:

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "sensor_msgs/msg/laser_scan.hpp"
#include "nav_msgs/msg/odometry.hpp"

using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;

class AutonomousSafetyNode : public rclcpp_lifecycle::LifecycleNode {
public:
  explicit AutonomousSafetyNode(const rclcpp::NodeOptions & options)
  : rclcpp_lifecycle::LifecycleNode("safety_monitor_node", options) {}

  CallbackReturn on_configure(const rclcpp_lifecycle::State &) override {
    RCLCPP_INFO(get_logger(), "Configuring Safety Monitor Node...");
    
    // Configure QoS for High Frequency Sensor Data
    rclcpp::QoS sensor_qos(rclcpp::KeepLast(5));
    sensor_qos.reliability(RCLCPP_RELIABILITY_BEST_EFFORT);
    sensor_qos.durability(RCLCPP_DURABILITY_VOLATILE);

    scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>(
      "/scan", sensor_qos,
      std::bind(&AutonomousSafetyNode::scan_callback, this, std::placeholders::_1));

    cmd_vel_pub_ = create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", rclcpp::SystemDefaultsQoS());

    return CallbackReturn::SUCCESS;
  }

  CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override {
    LifecycleNode::on_activate(state);
    cmd_vel_pub_->on_activate();
    RCLCPP_INFO(get_logger(), "Safety Monitor Activated.");
    return CallbackReturn::SUCCESS;
  }

  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override {
    LifecycleNode::on_deactivate(state);
    cmd_vel_pub_->on_deactivate();
    RCLCPP_INFO(get_logger(), "Safety Monitor Deactivated.");
    return CallbackReturn::SUCCESS;
  }

private:
  void scan_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg) {
    if (!get_current_state().label().compare("active")) {
      // Emergency Brake logic if obstacle closer than threshold
      for (const auto & range : msg->ranges) {
        if (range < 0.35f && range > msg->range_min) {
          RCLCPP_WARN(get_logger(), "EMERGENCY STOP TRIGGERED! Obstacle detected at %.2fm", range);
          auto stop_msg = std::make_unique<geometry_msgs::msg::Twist>();
          stop_msg->linear.x = 0.0;
          stop_msg->angular.z = 0.0;
          cmd_vel_pub_->publish(std::move(stop_msg));
          break;
        }
      }
    }
  }

  rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
  rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
};