mirs
読み取り中…
検索中…
一致する文字列を見つけられません
parameter_publisher.cpp
[詳解]
1// Copyright 2026 mirs260x
2//
3// Permission is hereby granted, free of charge, to any person obtaining a copy
4// of this software and associated documentation files (the "Software"), to deal
5// in the Software without restriction, including without limitation the rights
6// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7// copies of the Software, and to permit persons to whom the Software is
8// furnished to do so, subject to the following conditions:
9//
10// The above copyright notice and this permission notice shall be included in
11// all copies or substantial portions of the Software.
12//
13// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
16// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
19// THE SOFTWARE.
20
21
25#include <chrono>
26#include <memory>
27
28#include "rclcpp/rclcpp.hpp"
29#include "mirs_msgs/msg/basic_param.hpp"
30
32class ParameterPublisher : public rclcpp::Node
33{
34 static constexpr int PUBLISH_PERIOD_MS = 500; // ESP用パラメータ転送周期(2Hz)
35
36public:
38 : Node("parameter_publisher")
39 {
40 // パラメータを宣言(既定値はconfig.yamlと同一に保つこと。単一真実はyaml)
41 this->declare_parameter("wheel_radius", 0.0391);
42 this->declare_parameter("wheel_base", 0.39);
43 this->declare_parameter("rkp", 40.0);
44 this->declare_parameter("rki", 150.0);
45 this->declare_parameter("rkd", 0.4);
46 this->declare_parameter("lkp", 40.0);
47 this->declare_parameter("lki", 150.0);
48 this->declare_parameter("lkd", 0.4);
49
50 // YAMLファイルからパラメータを取得
51 wheel_radius = this->get_parameter("wheel_radius").as_double();
52 wheel_base = this->get_parameter("wheel_base").as_double();
53 rkp = this->get_parameter("rkp").as_double();
54 rki = this->get_parameter("rki").as_double();
55 rkd = this->get_parameter("rkd").as_double();
56 lkp = this->get_parameter("lkp").as_double();
57 lki = this->get_parameter("lki").as_double();
58 lkd = this->get_parameter("lkd").as_double();
59
60 // パブリッシャーの作成
61 param_pub_ = this->create_publisher<mirs_msgs::msg::BasicParam>("/params", 10);
62
63 // タイマーで定期的にオドメトリをパブリッシュ
64 timer_ = this->create_wall_timer(
65 std::chrono::milliseconds(PUBLISH_PERIOD_MS),
66 std::bind(&ParameterPublisher::publish_parameter, this));
67 }
68
69private:
70 void publish_parameter()
71 {
72 auto param_msg = mirs_msgs::msg::BasicParam();
73 param_msg.wheel_base = wheel_base;
74 param_msg.wheel_radius = wheel_radius;
75 param_msg.rkp = rkp;
76 param_msg.rki = rki;
77 param_msg.rkd = rkd;
78 param_msg.lkp = lkp;
79 param_msg.lki = lki;
80 param_msg.lkd = lkd;
81 param_pub_->publish(param_msg);
82 }
83
84 // パブリッシャー
85 rclcpp::Publisher<mirs_msgs::msg::BasicParam>::SharedPtr param_pub_;
86
87 // タイマー
88 rclcpp::TimerBase::SharedPtr timer_;
89
90 // エンコーダの変数
91 double wheel_radius, wheel_base, rkp, rki, rkd, lkp, lki, lkd;
92};
93
94int main(int argc, char *argv[])
95{
96 rclcpp::init(argc, argv);
97 rclcpp::spin(std::make_shared<ParameterPublisher>());
98 rclcpp::shutdown();
99 return 0;
100}
int main(int argc, char **argv)