21 : Node(
"scan_safety_guard")
23 scan_topic_ = declare_parameter<std::string>(
"scan_topic",
"/scan");
24 input_topic_ = declare_parameter<std::string>(
"input_cmd_vel_topic",
"/cmd_vel_nav");
25 output_topic_ = declare_parameter<std::string>(
"output_cmd_vel_topic",
"/cmd_vel");
26 stop_distance_ = declare_parameter<double>(
"stop_distance", 0.35);
27 resume_distance_ = declare_parameter<double>(
"resume_distance", 0.50);
28 forward_angle_deg_ = declare_parameter<double>(
"forward_angle_deg", 30.0);
29 scan_timeout_sec_ = declare_parameter<double>(
"scan_timeout_sec", 0.5);
30 command_timeout_sec_ = declare_parameter<double>(
"command_timeout_sec", 0.5);
32 if (!(std::isfinite(stop_distance_) && std::isfinite(resume_distance_) &&
33 stop_distance_ > 0.0 && resume_distance_ > stop_distance_ &&
34 forward_angle_deg_ > 0.0 && forward_angle_deg_ <= 180.0 &&
35 scan_timeout_sec_ > 0.0 && command_timeout_sec_ > 0.0))
37 throw std::invalid_argument(
"invalid scan safety guard parameters");
40 auto qos = rclcpp::SensorDataQoS();
41 scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>(
42 scan_topic_, qos, std::bind(&ScanSafetyGuard::on_scan,
this, std::placeholders::_1));
43 cmd_sub_ = create_subscription<geometry_msgs::msg::Twist>(
44 input_topic_, rclcpp::QoS(10),
45 std::bind(&ScanSafetyGuard::on_command,
this, std::placeholders::_1));
46 cmd_pub_ = create_publisher<geometry_msgs::msg::Twist>(output_topic_, rclcpp::QoS(10));
47 timer_ = create_wall_timer(50ms, std::bind(&ScanSafetyGuard::publish_command,
this));
49 RCLCPP_WARN(get_logger(),
50 "Scan safety guard active: stop <= %.2fm, resume >= %.2fm, forward +/- %.1f deg; "
51 "input %s -> output %s. No fresh scan means stopped.",
52 stop_distance_, resume_distance_, forward_angle_deg_, input_topic_.c_str(),
53 output_topic_.c_str());
57 void on_scan(
const sensor_msgs::msg::LaserScan::SharedPtr scan)
59 last_scan_ = std::chrono::steady_clock::now();
61 const double half_angle = forward_angle_deg_ * M_PI / 180.0;
62 double min_range = std::numeric_limits<double>::infinity();
63 std::size_t samples = 0;
64 for (std::size_t i = 0; i < scan->ranges.size(); ++i) {
65 const double angle = scan->angle_min +
static_cast<double>(i) * scan->angle_increment;
66 if (!std::isfinite(angle) || std::abs(std::remainder(angle, 2.0 * M_PI)) > half_angle) {
69 const double range = scan->ranges[i];
70 if (std::isnan(range) || range < scan->range_min) {
75 if (std::isfinite(range) && range <= scan->range_max) {
76 min_range = std::min(min_range, range);
80 const bool should_stop = samples == 0 || min_range <= stop_distance_;
81 const bool should_resume = samples > 0 && min_range >= resume_distance_;
82 if (!stopped_ && should_stop) {
84 RCLCPP_ERROR(get_logger(),
"Obstacle/invalid forward scan: stopping robot");
85 }
else if (stopped_ && should_resume) {
87 RCLCPP_WARN(get_logger(),
"Forward clearance restored: releasing safety stop");
91 void on_command(
const geometry_msgs::msg::Twist::SharedPtr msg)
93 latest_command_ = *msg;
95 last_command_ = std::chrono::steady_clock::now();
98 void publish_command()
100 const auto current_time = std::chrono::steady_clock::now();
101 const bool scan_fresh = have_scan_ &&
102 std::chrono::duration<double>(current_time - last_scan_).count() <= scan_timeout_sec_;
103 const bool command_fresh = have_command_ &&
104 std::chrono::duration<double>(current_time - last_command_).count() <= command_timeout_sec_;
105 if (!scan_fresh && !stopped_) {
107 RCLCPP_ERROR(get_logger(),
"No fresh /scan: stopping robot");
109 if (!scan_fresh && have_scan_) {
110 RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 2000,
" /scan is stale; holding stop");
112 cmd_pub_->publish(scan_fresh && command_fresh && !stopped_ ?
113 latest_command_ : geometry_msgs::msg::Twist{});
116 std::string scan_topic_, input_topic_, output_topic_;
117 double stop_distance_{0.35}, resume_distance_{0.50}, forward_angle_deg_{30.0};
118 double scan_timeout_sec_{0.5}, command_timeout_sec_{0.5};
119 bool stopped_{
false}, have_scan_{
false}, have_command_{
false};
120 std::chrono::steady_clock::time_point last_scan_{};
121 std::chrono::steady_clock::time_point last_command_{};
122 geometry_msgs::msg::Twist latest_command_;
123 rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
124 rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_sub_;
125 rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_pub_;
126 rclcpp::TimerBase::SharedPtr timer_;