nodding_lidar
読み取り中…
検索中…
一致する文字列を見つけられません
serial_angle_bridge.cpp
[詳解]
1#include <cerrno>
2#include <chrono>
3#include <cmath>
4#include <cstdio>
5#include <cstring>
6#include <functional>
7#include <stdexcept>
8#include <string>
9
10#include <fcntl.h>
11#include <termios.h>
12#include <unistd.h>
13
14#include "rclcpp/rclcpp.hpp"
15#include "sensor_msgs/msg/joint_state.hpp"
16
18
19class SerialAngleBridge : public rclcpp::Node
20{
21public:
23 : Node("serial_angle_bridge")
24 {
25 serial_port_ = declare_parameter<std::string>("serial_port", "/dev/ttyACM0");
26 state_topic_ = declare_parameter<std::string>("state_topic", "/servo_state");
27 joint_name_ = declare_parameter<std::string>("joint_name", "nodding_joint");
28 center_angle_deg_ = declare_parameter<double>("center_angle_deg", 90.0);
29 angle_sign_ = declare_parameter<double>("angle_sign", 1.0);
30 if (!std::isfinite(center_angle_deg_) || !std::isfinite(angle_sign_) || angle_sign_ == 0.0) {
31 throw std::invalid_argument("center_angle_deg must be finite and angle_sign must be nonzero");
32 }
33
34 fd_ = open(serial_port_.c_str(), O_RDONLY | O_NOCTTY | O_NONBLOCK);
35 if (fd_ < 0) {
36 throw std::runtime_error(
37 "Cannot open servo serial port " + serial_port_ + ": " + std::strerror(errno));
38 }
39
40 termios settings{};
41 if (tcgetattr(fd_, &settings) != 0) {
42 const std::string error = std::strerror(errno);
43 close(fd_);
44 fd_ = -1;
45 throw std::runtime_error("Cannot read serial settings: " + error);
46 }
47 cfmakeraw(&settings);
48 cfsetispeed(&settings, B115200);
49 cfsetospeed(&settings, B115200);
50 settings.c_cflag |= CLOCAL | CREAD;
51 settings.c_cc[VMIN] = 0;
52 settings.c_cc[VTIME] = 0;
53 if (tcsetattr(fd_, TCSANOW, &settings) != 0) {
54 const std::string error = std::strerror(errno);
55 close(fd_);
56 fd_ = -1;
57 throw std::runtime_error("Cannot configure serial port: " + error);
58 }
59
60 publisher_ = create_publisher<sensor_msgs::msg::JointState>(state_topic_, 10);
61 timer_ = create_wall_timer(
62 std::chrono::milliseconds(5), std::bind(&SerialAngleBridge::read_serial, this));
63 RCLCPP_INFO(
64 get_logger(), "Reading estimated servo angle from %s at 115200 baud",
65 serial_port_.c_str());
66 }
67
69 {
70 if (fd_ >= 0) {
71 close(fd_);
72 }
73 }
74
75private:
76 void parse_line(const std::string & line)
77 {
78 unsigned long device_time_ms = 0;
79 double angle_deg = 0.0;
80 double velocity_deg_s = 0.0;
81 char trailing = '\0';
82 if (std::sscanf(
83 line.c_str(), "%lu,%lf,%lf%c", &device_time_ms, &angle_deg, &velocity_deg_s,
84 &trailing) != 3 || !std::isfinite(angle_deg) || angle_deg < 0.0 || angle_deg > 180.0 ||
85 !std::isfinite(velocity_deg_s))
86 {
87 RCLCPP_WARN_THROTTLE(
88 get_logger(), *get_clock(), 2000,
89 "Ignoring invalid servo CSV line (expected timestamp,angle,angular_velocity): '%s'",
90 line.c_str());
91 return;
92 }
93 (void)device_time_ms;
94
95 sensor_msgs::msg::JointState state;
96 state.header.stamp = now();
97 state.name = {joint_name_};
98 state.position = {
99 angle_sign_ * (angle_deg - center_angle_deg_) *
100 nodding_lidar::kPi / 180.0};
101 state.velocity = {
102 angle_sign_ * velocity_deg_s * nodding_lidar::kPi / 180.0};
103 publisher_->publish(state);
104 }
105
106 void read_serial()
107 {
108 char bytes[128];
109 while (true) {
110 const ssize_t count = read(fd_, bytes, sizeof(bytes));
111 if (count < 0) {
112 if (errno != EAGAIN && errno != EWOULDBLOCK && errno != EINTR) {
113 RCLCPP_ERROR_THROTTLE(
114 get_logger(), *get_clock(), 2000, "Serial read failed: %s", std::strerror(errno));
115 }
116 return;
117 }
118 if (count == 0) {
119 return;
120 }
121
122 for (ssize_t i = 0; i < count; ++i) {
123 const char c = bytes[i];
124 if (c == '\n') {
125 if (!discard_line_ && !line_.empty()) {
126 parse_line(line_);
127 }
128 line_.clear();
129 discard_line_ = false;
130 } else if (!discard_line_ && c != '\r') {
131 if (line_.size() < 127) {
132 line_ += c;
133 } else {
134 line_.clear();
135 discard_line_ = true;
136 }
137 }
138 }
139 }
140 }
141
142 int fd_{-1};
143 bool discard_line_{false};
144 double center_angle_deg_{90.0};
145 double angle_sign_{1.0};
146 std::string serial_port_;
147 std::string state_topic_;
148 std::string joint_name_;
149 std::string line_;
150 rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr publisher_;
151 rclcpp::TimerBase::SharedPtr timer_;
152};
153
154int main(int argc, char ** argv)
155{
156 rclcpp::init(argc, argv);
157 int result = 0;
158 try {
159 rclcpp::spin(std::make_shared<SerialAngleBridge>());
160 } catch (const std::exception & e) {
161 RCLCPP_FATAL(rclcpp::get_logger("serial_angle_bridge"), "%s", e.what());
162 result = 1;
163 }
164 rclcpp::shutdown();
165 return result;
166}
首振り角度と取付寸法を使い、LiDAR点を別の座標系へ変換する。
constexpr double kPi
int main(int argc, char **argv)