servo_lidar_3d
読み取り中…
検索中…
一致する文字列を見つけられません
uno_servo_bridge.cpp
[詳解]
1#include <rclcpp/rclcpp.hpp>
2#include <sensor_msgs/msg/joint_state.hpp>
3#include <fcntl.h>
4#include <termios.h>
5#include <unistd.h>
6#include <cstdio>
7#include <cerrno>
8#include <cstring>
9#include <string>
10
11class UnoServoBridge : public rclcpp::Node {
12public:
13 UnoServoBridge() : Node("uno_servo_bridge") {
14 port_ = declare_parameter("serial_port", "/dev/ttyACM0");
15 state_topic_ = declare_parameter("state_topic", "/servo_state");
16 joint_name_ = declare_parameter("joint_name", "lidar_tilt_joint");
17 fd_ = open(port_.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);
18 if (fd_ >= 0) {
19 termios t{};
20 if (tcgetattr(fd_, &t) != 0) {
21 RCLCPP_ERROR(get_logger(), "Cannot read serial settings for %s: %s",
22 port_.c_str(), std::strerror(errno));
23 }
24 cfmakeraw(&t);
25 cfsetispeed(&t, B115200);
26 cfsetospeed(&t, B115200);
27 t.c_cflag |= CLOCAL | CREAD;
28 if (tcsetattr(fd_, TCSANOW, &t) != 0) {
29 RCLCPP_ERROR(get_logger(), "Cannot configure %s: %s",
30 port_.c_str(), std::strerror(errno));
31 }
32 RCLCPP_INFO(get_logger(), "Opened servo serial port %s at 115200 baud", port_.c_str());
33 } else {
34 RCLCPP_ERROR(get_logger(), "Cannot open %s: %s", port_.c_str(), std::strerror(errno));
35 }
36 pub_ = create_publisher<sensor_msgs::msg::JointState>(state_topic_, 10);
37 timer_ = create_wall_timer(std::chrono::milliseconds(10), [this] { read_serial(); });
38 }
39 ~UnoServoBridge() { if (fd_ >= 0) close(fd_); }
40private:
41 void read_serial() { if (fd_ < 0) return; char c; while (read(fd_, &c, 1) == 1) { if (c == '\n') { float a, v; if (sscanf(line_.c_str(), "S %f %f", &a, &v) == 2) { sensor_msgs::msg::JointState m; m.header.stamp = now(); m.name = {joint_name_}; m.position = {a}; m.velocity = {v}; pub_->publish(m); } line_.clear(); } else if (line_.size() < 63) line_ += c; } }
42 int fd_{-1}; std::string port_, state_topic_, joint_name_, line_;
43 rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr pub_;
44 rclcpp::TimerBase::SharedPtr timer_;
45};
46int main(int argc, char **argv) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared<UnoServoBridge>()); rclcpp::shutdown(); }
int main(int argc, char **argv)