Skip to content

Commit 5a789dd

Browse files
author
Roman Kuzmenko
committed
added the first test case and made it work
1 parent f7e5741 commit 5a789dd

File tree

9 files changed

+375
-152
lines changed

9 files changed

+375
-152
lines changed

‎.gitignore

+2
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,2 @@
1+
__pycache__
2+
*.pyc

‎include/serial/port.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@ namespace serial {
2121
class PortSettings {
2222
public:
2323
rclcpp::Parameter dev_name;
24+
rclcpp::Parameter skip_init;
2425
rclcpp::Parameter baud_rate;
2526
rclcpp::Parameter data;
2627
rclcpp::Parameter parity;

‎include/serial/utils.hpp

+3-1
Original file line numberDiff line numberDiff line change
@@ -22,8 +22,10 @@ namespace utils {
2222
static inline std::string bin2hex(const std::string &bin) {
2323
std::stringstream ss;
2424

25+
ss << std::hex << std::setfill('0') << std::setw(2);
26+
2527
for (const auto &item : bin) {
26-
ss << std::hex << int(item);
28+
ss << int(item) << " ";
2729
}
2830
return ss.str();
2931
}

‎include/serial/worker.hpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -27,7 +27,7 @@ class Worker final : public InterfaceNative {
2727
public:
2828
Worker(std::shared_ptr<InterfaceRos> intf_ros,
2929
std::shared_ptr<PortSettings> settings);
30-
~Worker();
30+
virtual ~Worker();
3131

3232
void stop();
3333

‎src/interface_ros.cpp

+2-6
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@ void InterfaceRos::inject_input_handler_(
2424
std::shared_ptr<serial::srv::InjectInput::Response> response) {
2525
(void)response;
2626
RCLCPP_INFO(rclcpp::get_logger("rclcpp"),
27-
"Incoming request to inject input\ndata: %s",
27+
"Incoming request to inject input data: %s",
2828
utils::bin2hex(request->data).c_str());
2929

3030
node_->intf_native->inject_read(request->data);
@@ -39,14 +39,10 @@ void InterfaceRos::inject_output_handler_(
3939
std::shared_ptr<serial::srv::InjectOutput::Response> response) {
4040
(void)response;
4141
RCLCPP_INFO(rclcpp::get_logger("rclcpp"),
42-
"Incoming request to inject output\ndata: %s",
42+
"Incoming request to inject output data: %s",
4343
utils::bin2hex(request->data).c_str());
4444

4545
node_->intf_native->write(request->data);
46-
47-
auto message = std_msgs::msg::String();
48-
message.data = request->data;
49-
inspect_output->publish(message);
5046
}
5147

5248
InterfaceRos::InterfaceRos(Node *node, const std::string &interface_prefix)

‎src/node.cpp

+2
Original file line numberDiff line numberDiff line change
@@ -31,13 +31,15 @@ Node::Node()
3131
void Node::init_parameters_() {
3232
// port settings
3333
this->declare_parameter("dev_name", "/dev/ttyS0");
34+
this->declare_parameter("skip_init", false);
3435
this->declare_parameter("baud_rate", 115200);
3536
this->declare_parameter("data", 1);
3637
this->declare_parameter("parity", false);
3738
this->declare_parameter("stop", 1);
3839
this->declare_parameter("flow_control", false);
3940
this->declare_parameter("bs", 1024);
4041
this->get_parameter("dev_name", port_settings_->dev_name);
42+
this->get_parameter("skip_init", port_settings_->skip_init);
4143
this->get_parameter("baud_rate", port_settings_->baud_rate);
4244
this->get_parameter("data", port_settings_->data);
4345
this->get_parameter("stop", port_settings_->stop);

‎src/port.cpp

+136-133
Original file line numberDiff line numberDiff line change
@@ -23,155 +23,158 @@
2323
namespace serial {
2424

2525
int PortSettings::setup() {
26-
int fd = open(dev_name.as_string().data(), O_RDWR);
26+
int fd = ::open(dev_name.as_string().data(), O_RDWR);
2727
if (fd < 0) {
2828
throw std::invalid_argument("failed to open the device");
2929
}
3030

31-
// Get terminal attributes
32-
struct termios tty;
33-
memset(&tty, 0, sizeof(tty));
34-
if (tcgetattr(fd, &tty) != 0) {
35-
close(fd);
36-
throw std::invalid_argument("failed to get the device attributes");
37-
}
38-
39-
// Baud rate
40-
switch (baud_rate.as_int()) {
41-
case 0:
42-
cfsetispeed(&tty, B0);
43-
break;
44-
case 50:
45-
cfsetispeed(&tty, B50);
46-
break;
47-
case 75:
48-
cfsetispeed(&tty, B75);
49-
break;
50-
case 110:
51-
cfsetispeed(&tty, B110);
52-
break;
53-
case 134:
54-
cfsetispeed(&tty, B134);
55-
break;
56-
case 150:
57-
cfsetispeed(&tty, B150);
58-
break;
59-
case 200:
60-
cfsetispeed(&tty, B200);
61-
break;
62-
case 300:
63-
cfsetispeed(&tty, B300);
64-
break;
65-
case 600:
66-
cfsetispeed(&tty, B600);
67-
break;
68-
case 1200:
69-
cfsetispeed(&tty, B1200);
70-
break;
71-
case 1800:
72-
cfsetispeed(&tty, B1800);
73-
break;
74-
case 2400:
75-
cfsetispeed(&tty, B2400);
76-
break;
77-
case 4800:
78-
cfsetispeed(&tty, B4800);
79-
break;
80-
case 9600:
81-
cfsetispeed(&tty, B9600);
82-
break;
83-
case 19200:
84-
cfsetispeed(&tty, B19200);
85-
break;
86-
case 38400:
87-
cfsetispeed(&tty, B38400);
88-
break;
89-
case 57600:
90-
cfsetispeed(&tty, B57600);
91-
break;
92-
case 115200:
93-
cfsetispeed(&tty, B115200);
94-
break;
95-
case 230400:
96-
cfsetispeed(&tty, B230400);
97-
break;
98-
// case 460800:
99-
// cfsetispeed(&tty, B460800);
100-
// break;
101-
default:
102-
throw std::invalid_argument("unsupported baud rate");
103-
}
104-
105-
// Data bits
106-
tty.c_cflag &= ~CSIZE;
107-
switch (data.as_int()) {
108-
case 5:
109-
tty.c_cflag |= CS5;
110-
break;
111-
case 6:
112-
tty.c_cflag |= CS6;
113-
break;
114-
case 7:
115-
tty.c_cflag |= CS7;
116-
break;
117-
case 8:
118-
tty.c_cflag |= CS8;
119-
break;
120-
}
121-
122-
// Stop bits
123-
if (stop.as_int() == 1) {
124-
tty.c_cflag &= ~CSTOPB;
125-
} else {
126-
tty.c_cflag |= CSTOPB;
127-
}
128-
129-
// Parity
130-
// TODO(clairbee): handle this as a dictionary to support odd parity
131-
if (parity.as_bool()) {
132-
tty.c_cflag |= PARENB;
133-
tty.c_cflag &= ~PARODD;
134-
} else {
135-
tty.c_cflag &= ~PARENB;
136-
}
137-
138-
// Flow control
139-
if (flow_control.as_bool()) {
140-
tty.c_cflag |= CRTSCTS;
141-
} else {
142-
tty.c_cflag &= ~CRTSCTS;
143-
}
144-
145-
tty.c_cflag |= CREAD | CLOCAL;
146-
tty.c_iflag &= ~(IXON | IXOFF | IXANY); // Turn off s/w flow ctrl
147-
tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL);
148-
// Setting both to 0 will give a non-blocking read
149-
tty.c_cc[VTIME] = 0;
150-
tty.c_cc[VMIN] = 0;
151-
tty.c_lflag &= ~ICANON; // Turn off canonical input
152-
tty.c_lflag &= ~(ECHO); // Turn off echo
153-
tty.c_lflag &= ~ISIG; // Disables recognition of INTR (interrupt), QUIT and
31+
if (!skip_init.as_bool()) {
32+
// Get terminal attributes
33+
struct termios tty;
34+
::memset(&tty, 0, sizeof(tty));
35+
if (::tcgetattr(fd, &tty) != 0) {
36+
::close(fd);
37+
throw std::invalid_argument("failed to get the device attributes");
38+
}
39+
40+
// Baud rate
41+
switch (baud_rate.as_int()) {
42+
case 0:
43+
cfsetispeed(&tty, B0);
44+
break;
45+
case 50:
46+
cfsetispeed(&tty, B50);
47+
break;
48+
case 75:
49+
cfsetispeed(&tty, B75);
50+
break;
51+
case 110:
52+
cfsetispeed(&tty, B110);
53+
break;
54+
case 134:
55+
cfsetispeed(&tty, B134);
56+
break;
57+
case 150:
58+
cfsetispeed(&tty, B150);
59+
break;
60+
case 200:
61+
cfsetispeed(&tty, B200);
62+
break;
63+
case 300:
64+
cfsetispeed(&tty, B300);
65+
break;
66+
case 600:
67+
cfsetispeed(&tty, B600);
68+
break;
69+
case 1200:
70+
cfsetispeed(&tty, B1200);
71+
break;
72+
case 1800:
73+
cfsetispeed(&tty, B1800);
74+
break;
75+
case 2400:
76+
cfsetispeed(&tty, B2400);
77+
break;
78+
case 4800:
79+
cfsetispeed(&tty, B4800);
80+
break;
81+
case 9600:
82+
cfsetispeed(&tty, B9600);
83+
break;
84+
case 19200:
85+
cfsetispeed(&tty, B19200);
86+
break;
87+
case 38400:
88+
cfsetispeed(&tty, B38400);
89+
break;
90+
case 57600:
91+
cfsetispeed(&tty, B57600);
92+
break;
93+
case 115200:
94+
cfsetispeed(&tty, B115200);
95+
break;
96+
case 230400:
97+
cfsetispeed(&tty, B230400);
98+
break;
99+
// case 460800:
100+
// cfsetispeed(&tty, B460800);
101+
// break;
102+
default:
103+
throw std::invalid_argument("unsupported baud rate");
104+
}
105+
106+
// Data bits
107+
tty.c_cflag &= ~CSIZE;
108+
switch (data.as_int()) {
109+
case 5:
110+
tty.c_cflag |= CS5;
111+
break;
112+
case 6:
113+
tty.c_cflag |= CS6;
114+
break;
115+
case 7:
116+
tty.c_cflag |= CS7;
117+
break;
118+
case 8:
119+
tty.c_cflag |= CS8;
120+
break;
121+
}
122+
123+
// Stop bits
124+
if (stop.as_int() == 1) {
125+
tty.c_cflag &= ~CSTOPB;
126+
} else {
127+
tty.c_cflag |= CSTOPB;
128+
}
129+
130+
// Parity
131+
// TODO(clairbee): handle this as a dictionary to support odd parity
132+
if (parity.as_bool()) {
133+
tty.c_cflag |= PARENB;
134+
tty.c_cflag &= ~PARODD;
135+
} else {
136+
tty.c_cflag &= ~PARENB;
137+
}
138+
139+
// Flow control
140+
if (flow_control.as_bool()) {
141+
tty.c_cflag |= CRTSCTS;
142+
} else {
143+
tty.c_cflag &= ~CRTSCTS;
144+
}
145+
146+
tty.c_cflag |= CREAD | CLOCAL;
147+
tty.c_iflag &= ~(IXON | IXOFF | IXANY); // Turn off s/w flow ctrl
148+
tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL);
149+
// Setting both to 0 will give a non-blocking read
150+
tty.c_cc[VTIME] = 0;
151+
tty.c_cc[VMIN] = 0;
152+
tty.c_lflag &= ~ICANON; // Turn off canonical input
153+
tty.c_lflag &= ~(ECHO); // Turn off echo
154+
tty.c_lflag &= ~ISIG; // Disables recognition of INTR (interrupt), QUIT and
154155
// SUSP (suspend) characters
155156

156-
// Set the terminal attributes
157-
tcflush(fd, TCIFLUSH);
158-
if (tcsetattr(fd, TCSANOW, &tty) != 0) {
159-
close(fd);
160-
throw std::invalid_argument("failed to set the device attributes");
157+
// Set the terminal attributes
158+
::tcflush(fd, TCIFLUSH);
159+
if (::tcsetattr(fd, TCSANOW, &tty) != 0) {
160+
::close(fd);
161+
throw std::invalid_argument("failed to set the device attributes");
162+
}
161163
}
162164

163165
// Get the file descriptor attributes
164-
int flags = fcntl(fd, F_GETFL, 0);
166+
::fcntl(fd, F_NOCACHE, 1);
167+
int flags = ::fcntl(fd, F_GETFL, 0);
165168
if (flags == -1) {
166-
close(fd);
169+
::close(fd);
167170
throw std::invalid_argument("failed to get the file descriptor attributes");
168171
}
169172

170173
flags |= O_NONBLOCK;
171174

172175
// Set the file attributes
173-
if (fcntl(fd, F_SETFL, flags)) {
174-
close(fd);
176+
if (::fcntl(fd, F_SETFL, flags)) {
177+
::close(fd);
175178
throw std::invalid_argument("failed to set the file descriptor attributes");
176179
}
177180

0 commit comments

Comments
 (0)