servo_lidar_3d
Toggle main menu visibility
読み取り中…
検索中…
一致する文字列を見つけられません
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
7
class
ServoSweepController
final :
public
rclcpp::Node {
8
public
:
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
26
private
:
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
50
int
main
(
int
argc,
char
** argv) {
51
rclcpp::init(argc, argv);
52
rclcpp::spin(std::make_shared<ServoSweepController>());
53
rclcpp::shutdown();
54
return
0;
55
}
ServoSweepController::ServoSweepController
ServoSweepController()
定義
servo_sweep_controller.cpp:9
main
int main(int argc, char **argv)
定義
pointcloud_to_pcd.cpp:57
src
servo_sweep_controller.cpp
構築:
1.18.0