servo_lidar_3d
読み取り中…
検索中…
一致する文字列を見つけられません
servo_sweep_controller.cpp
[詳解]
1#include <cmath>
2#include <chrono>
3
4#include "rclcpp/rclcpp.hpp"
5#include "std_msgs/msg/float32_multi_array.hpp"
6
7class ServoSweepController final : public rclcpp::Node {
8public:
9 ServoSweepController() : Node("servo_sweep_controller") {
10 command_topic_ = declare_parameter("command_topic", std::string("/servo_angle_cmd"));
11 target_angle_ = declare_parameter("target_angle", 3.141592653589793);
12 sweep_duration_ = declare_parameter("sweep_duration", 10.0);
13 start_delay_ = declare_parameter("start_delay", 1.0);
14
15 command_pub_ = create_publisher<std_msgs::msg::Float32MultiArray>(command_topic_, 10);
16 timer_ = create_wall_timer(
17 std::chrono::duration_cast<std::chrono::milliseconds>(
18 std::chrono::duration<double>(start_delay_)),
19 [this]() { send_command(); });
20
21 RCLCPP_INFO(get_logger(),
22 "Configured sweep: topic=%s target=%.3f rad duration=%.3f s start_delay=%.3f s",
23 command_topic_.c_str(), target_angle_, sweep_duration_, start_delay_);
24 }
25
26private:
27 void send_command() {
28 if (!std::isfinite(target_angle_) || !std::isfinite(sweep_duration_) ||
29 sweep_duration_ <= 0.0) {
30 RCLCPP_ERROR(get_logger(), "Invalid servo command parameters");
31 timer_->cancel();
32 return;
33 }
34 std_msgs::msg::Float32MultiArray command;
35 command.data = {static_cast<float>(target_angle_), static_cast<float>(sweep_duration_)};
36 command_pub_->publish(command);
37 RCLCPP_INFO(get_logger(), "Published servo command: %.3f rad over %.3f s",
38 target_angle_, sweep_duration_);
39 timer_->cancel();
40 }
41
42 std::string command_topic_;
43 double target_angle_{M_PI};
44 double sweep_duration_{10.0};
45 double start_delay_{1.0};
46 rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr command_pub_;
47 rclcpp::TimerBase::SharedPtr timer_;
48};
49
50int main(int argc, char ** argv) {
51 rclcpp::init(argc, argv);
52 rclcpp::spin(std::make_shared<ServoSweepController>());
53 rclcpp::shutdown();
54 return 0;
55}
int main(int argc, char **argv)