mirs
読み取り中…
検索中…
一致する文字列を見つけられません
scan_safety_guard.cpp
[詳解]
1#include <algorithm>
2#include <chrono>
3#include <cmath>
4#include <functional>
5#include <limits>
6#include <memory>
7#include <stdexcept>
8#include <string>
9
10#include "geometry_msgs/msg/twist.hpp"
11#include "rclcpp/rclcpp.hpp"
12#include "sensor_msgs/msg/laser_scan.hpp"
13
14using namespace std::chrono_literals;
15
16// /scan の近距離障害物でNav2の速度指令を遮断する最終ゲート。
17class ScanSafetyGuard : public rclcpp::Node
18{
19public:
21 : Node("scan_safety_guard")
22 {
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);
31
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))
36 {
37 throw std::invalid_argument("invalid scan safety guard parameters");
38 }
39
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));
48
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());
54 }
55
56private:
57 void on_scan(const sensor_msgs::msg::LaserScan::SharedPtr scan)
58 {
59 last_scan_ = std::chrono::steady_clock::now();
60 have_scan_ = true;
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) {
67 continue;
68 }
69 const double range = scan->ranges[i];
70 if (std::isnan(range) || range < scan->range_min) {
71 continue;
72 }
73 ++samples;
74 // +inf や range_max 超過は「検出物なし」として停止判定から除外する。
75 if (std::isfinite(range) && range <= scan->range_max) {
76 min_range = std::min(min_range, range);
77 }
78 }
79
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) {
83 stopped_ = true;
84 RCLCPP_ERROR(get_logger(), "Obstacle/invalid forward scan: stopping robot");
85 } else if (stopped_ && should_resume) {
86 stopped_ = false;
87 RCLCPP_WARN(get_logger(), "Forward clearance restored: releasing safety stop");
88 }
89 }
90
91 void on_command(const geometry_msgs::msg::Twist::SharedPtr msg)
92 {
93 latest_command_ = *msg;
94 have_command_ = true;
95 last_command_ = std::chrono::steady_clock::now();
96 }
97
98 void publish_command()
99 {
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_) {
106 stopped_ = true;
107 RCLCPP_ERROR(get_logger(), "No fresh /scan: stopping robot");
108 }
109 if (!scan_fresh && have_scan_) {
110 RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 2000, " /scan is stale; holding stop");
111 }
112 cmd_pub_->publish(scan_fresh && command_fresh && !stopped_ ?
113 latest_command_ : geometry_msgs::msg::Twist{});
114 }
115
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_;
127};
128
129int main(int argc, char ** argv)
130{
131 rclcpp::init(argc, argv);
132 rclcpp::spin(std::make_shared<ScanSafetyGuard>());
133 rclcpp::shutdown();
134 return 0;
135}
int main(int argc, char **argv)