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);
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));
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));
32 RCLCPP_INFO(get_logger(),
"Opened servo serial port %s at 115200 baud", port_.c_str());
34 RCLCPP_ERROR(get_logger(),
"Cannot open %s: %s", port_.c_str(), std::strerror(errno));
36 pub_ = create_publisher<sensor_msgs::msg::JointState>(state_topic_, 10);
37 timer_ = create_wall_timer(std::chrono::milliseconds(10), [
this] { read_serial(); });
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_;