23 : Node(
"serial_angle_bridge")
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");
34 fd_ = open(serial_port_.c_str(), O_RDONLY | O_NOCTTY | O_NONBLOCK);
36 throw std::runtime_error(
37 "Cannot open servo serial port " + serial_port_ +
": " + std::strerror(errno));
41 if (tcgetattr(fd_, &settings) != 0) {
42 const std::string error = std::strerror(errno);
45 throw std::runtime_error(
"Cannot read serial settings: " + error);
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);
57 throw std::runtime_error(
"Cannot configure serial port: " + error);
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));
64 get_logger(),
"Reading estimated servo angle from %s at 115200 baud",
65 serial_port_.c_str());
76 void parse_line(
const std::string & line)
78 unsigned long device_time_ms = 0;
79 double angle_deg = 0.0;
80 double velocity_deg_s = 0.0;
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))
88 get_logger(), *get_clock(), 2000,
89 "Ignoring invalid servo CSV line (expected timestamp,angle,angular_velocity): '%s'",
95 sensor_msgs::msg::JointState state;
96 state.header.stamp = now();
97 state.name = {joint_name_};
99 angle_sign_ * (angle_deg - center_angle_deg_) *
103 publisher_->publish(state);
110 const ssize_t count = read(fd_, bytes,
sizeof(bytes));
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));
122 for (ssize_t i = 0; i < count; ++i) {
123 const char c = bytes[i];
125 if (!discard_line_ && !line_.empty()) {
129 discard_line_ =
false;
130 }
else if (!discard_line_ && c !=
'\r') {
131 if (line_.size() < 127) {
135 discard_line_ =
true;
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_;
150 rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr publisher_;
151 rclcpp::TimerBase::SharedPtr timer_;