nodding_lidar
読み取り中…
検索中…
一致する文字列を見つけられません
servo_node.cpp
[詳解]
1// 角度ノード。
2// - ±amplitude_deg の首振りの角度を生成し、PCA9685経由でサーボに指令する
3// - 推定した角度を sensor_msgs/JointState (/servo_state) で publish する
4// - servo_link -> lidar_link の動的TFを publish する
5//
6// 重要: エンコーダがないため、角度は「指令値ベースの推定」です。実際の角度ではありません。
7// - 推定角度 = angle_scale * 指令角度
8// - スタンプ = 指令時刻 + time_offset (time_offset = サーボの遅れ τ [秒]、正の値で遅らせる)
9//
10// 動作確認はしていません。use_hardware:=false で、サーボなしでも動作を確認できます。
11
12#include <chrono>
13#include <exception>
14#include <memory>
15#include <string>
16
17#include "geometry_msgs/msg/transform_stamped.hpp"
18#include "rclcpp/rclcpp.hpp"
19#include "sensor_msgs/msg/joint_state.hpp"
20#include "tf2_ros/transform_broadcaster.h"
21
24
25class ServoNode : public rclcpp::Node
26{
27public:
29 : Node("servo_node")
30 {
31 // --- 波形 ---
32 amplitude_deg_ = declare_parameter<double>("amplitude_deg", 15.0);
33 max_amplitude_deg_ = declare_parameter<double>("max_amplitude_deg", 30.0);
34 period_sec_ = declare_parameter<double>("period_sec", 3.0);
35 smoothness_ = declare_parameter<double>("smoothness", 0.3);
36 update_rate_hz_ = declare_parameter<double>("update_rate_hz", 100.0);
37
38 // --- 補正 ---
39 angle_scale_ = declare_parameter<double>("angle_scale", 1.0);
40 time_offset_ = declare_parameter<double>("time_offset", 0.0);
41
42 // --- 出力 ---
43 topic_ = declare_parameter<std::string>("topic", "/servo_state");
44 joint_name_ = declare_parameter<std::string>("joint_name", "nodding_joint");
45 parent_frame_ = declare_parameter<std::string>("parent_frame", "servo_link");
46 child_frame_ = declare_parameter<std::string>("child_frame", "lidar_link");
47 lidar_height_ = declare_parameter<double>("lidar_height", 0.0325);
48 publish_tf_ = declare_parameter<bool>("publish_tf", true);
49
50 // --- サーボ / PCA9685 ---
51 use_hardware_ = declare_parameter<bool>("use_hardware", true);
52 i2c_bus_ = declare_parameter<int>("i2c_bus", 1);
53 i2c_address_ = declare_parameter<int>("i2c_address", 0x40);
54 channel_ = declare_parameter<int>("channel", 0);
55 pwm_freq_hz_ = declare_parameter<double>("pwm_freq_hz", 50.0);
56 osc_hz_ = declare_parameter<double>("osc_hz", 25.0e6);
57 servo_center_us_ = declare_parameter<double>("servo_center_us", 1500.0);
58 servo_us_per_deg_ = declare_parameter<double>("servo_us_per_deg", 11.11);
59 servo_sign_ = declare_parameter<double>("servo_sign", 1.0);
60 min_pulse_us_ = declare_parameter<double>("min_pulse_us", 1000.0);
61 max_pulse_us_ = declare_parameter<double>("max_pulse_us", 2000.0);
62
63 if (amplitude_deg_ > max_amplitude_deg_) {
64 RCLCPP_WARN(
65 get_logger(), "amplitude_deg=%.1f が上限 max_amplitude_deg=%.1f を超えたため、制限します",
66 amplitude_deg_, max_amplitude_deg_);
67 amplitude_deg_ = max_amplitude_deg_;
68 }
69 if (update_rate_hz_ < 50.0) {
70 RCLCPP_WARN(get_logger(), "update_rate_hz=%.1f は50Hz未満です。TFの補間精度が下がります", update_rate_hz_);
71 }
72
73 if (use_hardware_) {
74 try {
75 pca_ = std::make_unique<nodding_lidar::Pca9685>(
76 i2c_bus_, i2c_address_, pwm_freq_hz_, osc_hz_);
77 pca_->set_pulse_us(channel_, servo_center_us_);
78 } catch (const std::exception & e) {
79 RCLCPP_FATAL(get_logger(), "PCA9685の初期化に失敗しました: %s", e.what());
80 throw;
81 }
82 } else {
83 RCLCPP_WARN(get_logger(), "use_hardware=false: サーボは動かさず、角度の生成と publish だけを行います");
84 }
85
86 pub_ = create_publisher<sensor_msgs::msg::JointState>(topic_, rclcpp::QoS(10));
87 if (publish_tf_) {
88 tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(this);
89 }
90
91 start_ = now();
92 const auto period = std::chrono::duration<double>(1.0 / update_rate_hz_);
93 timer_ = create_wall_timer(
94 std::chrono::duration_cast<std::chrono::nanoseconds>(period),
95 std::bind(&ServoNode::on_timer, this));
96
97 RCLCPP_INFO(
98 get_logger(),
99 "servo_node 開始: 振幅±%.1f度, 周期%.2f秒, %s -> %s, TF=%s。"
100 "角度はエンコーダなしの『指令値ベースの推定』です",
101 amplitude_deg_, period_sec_, parent_frame_.c_str(), child_frame_.c_str(),
102 publish_tf_ ? "on" : "off");
103 }
104
105 ~ServoNode() override
106 {
107 // 終了時は、サーボを中央に戻す (その位置を保持する)
108 if (pca_) {
109 try {
110 pca_->set_pulse_us(channel_, servo_center_us_);
111 } catch (const std::exception &) {
112 }
113 }
114 }
115
116private:
117 void on_timer()
118 {
119 const rclcpp::Time t_cmd = now();
120 const double t = (t_cmd - start_).seconds();
121
122 // 指令角度 [rad]
123 const double amp_rad = amplitude_deg_ * nodding_lidar::kPi / 180.0;
124 const double cmd = amp_rad * nodding_lidar::nodding_wave(t, period_sec_, smoothness_);
125
126 // サーボへ指令
127 if (pca_) {
128 try {
129 const double us = nodding_lidar::angle_to_pulse_us(
130 cmd, servo_center_us_, servo_us_per_deg_, servo_sign_, min_pulse_us_, max_pulse_us_);
131 pca_->set_pulse_us(channel_, us);
132 } catch (const std::exception & e) {
133 RCLCPP_ERROR_THROTTLE(
134 get_logger(), *get_clock(), 2000, "サーボへの指令に失敗しました: %s", e.what());
135 }
136 }
137
138 // 推定した姿勢 (指令値ベース)
139 const double est = angle_scale_ * cmd;
140 const rclcpp::Time stamp = t_cmd + rclcpp::Duration::from_seconds(time_offset_);
141
142 sensor_msgs::msg::JointState js;
143 js.header.stamp = stamp;
144 js.name = {joint_name_};
145 js.position = {est};
146 pub_->publish(js);
147
148 if (tf_broadcaster_) {
149 const nodding_lidar::Pose pose = nodding_lidar::servo_to_lidar_pose(est, lidar_height_);
150 geometry_msgs::msg::TransformStamped tf;
151 tf.header.stamp = stamp;
152 tf.header.frame_id = parent_frame_;
153 tf.child_frame_id = child_frame_;
154 tf.transform.translation.x = pose.t.x;
155 tf.transform.translation.y = pose.t.y;
156 tf.transform.translation.z = pose.t.z;
157 tf.transform.rotation.x = pose.qx;
158 tf.transform.rotation.y = pose.qy;
159 tf.transform.rotation.z = pose.qz;
160 tf.transform.rotation.w = pose.qw;
161 tf_broadcaster_->sendTransform(tf);
162 }
163 }
164
165 // パラメータ
166 double amplitude_deg_, max_amplitude_deg_, period_sec_, smoothness_, update_rate_hz_;
167 double angle_scale_, time_offset_;
168 std::string topic_, joint_name_, parent_frame_, child_frame_;
169 double lidar_height_;
170 bool publish_tf_, use_hardware_;
171 int i2c_bus_, i2c_address_, channel_;
172 double pwm_freq_hz_, osc_hz_;
173 double servo_center_us_, servo_us_per_deg_, servo_sign_, min_pulse_us_, max_pulse_us_;
174
175 // 状態
176 rclcpp::Time start_{0, 0, RCL_ROS_TIME};
177 std::unique_ptr<nodding_lidar::Pca9685> pca_;
178 rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr pub_;
179 std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
180 rclcpp::TimerBase::SharedPtr timer_;
181};
182
183int main(int argc, char ** argv)
184{
185 rclcpp::init(argc, argv);
186 int ret = 0;
187 try {
188 rclcpp::spin(std::make_shared<ServoNode>());
189 } catch (const std::exception & e) {
190 RCLCPP_FATAL(rclcpp::get_logger("servo_node"), "終了: %s", e.what());
191 ret = 1;
192 }
193 rclcpp::shutdown();
194 return ret;
195}
~ServoNode() override
首振り角度と取付寸法を使い、LiDAR点を別の座標系へ変換する。
double angle_to_pulse_us(double angle_rad, double center_us, double us_per_deg, double sign, double min_us, double max_us)
double nodding_wave(double t, double period, double smoothness)
constexpr double kPi
Pose servo_to_lidar_pose(double phi, double lidar_height)
I2C経由でPCA9685のPWM出力を設定し、サーボへパルスを送る。
int main(int argc, char **argv)