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);
39 angle_scale_ = declare_parameter<double>(
"angle_scale", 1.0);
40 time_offset_ = declare_parameter<double>(
"time_offset", 0.0);
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);
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);
63 if (amplitude_deg_ > max_amplitude_deg_) {
65 get_logger(),
"amplitude_deg=%.1f が上限 max_amplitude_deg=%.1f を超えたため、制限します",
66 amplitude_deg_, max_amplitude_deg_);
67 amplitude_deg_ = max_amplitude_deg_;
69 if (update_rate_hz_ < 50.0) {
70 RCLCPP_WARN(get_logger(),
"update_rate_hz=%.1f は50Hz未満です。TFの補間精度が下がります", update_rate_hz_);
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());
83 RCLCPP_WARN(get_logger(),
"use_hardware=false: サーボは動かさず、角度の生成と publish だけを行います");
86 pub_ = create_publisher<sensor_msgs::msg::JointState>(topic_, rclcpp::QoS(10));
88 tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(
this);
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));
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");
130 cmd, servo_center_us_, servo_us_per_deg_, servo_sign_, min_pulse_us_, max_pulse_us_);