-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathdectalk_ros.cpp
More file actions
45 lines (38 loc) · 1.45 KB
/
dectalk_ros.cpp
File metadata and controls
45 lines (38 loc) · 1.45 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
#include <stdio.h>
#include <string>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#define DECTALK_BINARY DECTALK_BINARY_DIR "/say -pre '[:phoneme on]' -"
class Dectalk_ROS : public rclcpp::Node {
public:
Dectalk_ROS() : Node("dectalk_ros") {
// start DECtalk
dectalk_write_pipe = popen(DECTALK_BINARY, "w");
if (!dectalk_write_pipe)
RCLCPP_ERROR(this->get_logger(), "popen failed!");
// turn off buffering - clashes with ROS2
setvbuf(dectalk_write_pipe, NULL, _IONBF, 0);
// create subscriber with class member function
subscriber_string_ = this->create_subscription<std_msgs::msg::String>(
"speak", 10,
std::bind(&Dectalk_ROS::string_callback, this, std::placeholders::_1));
}
~Dectalk_ROS() { pclose(dectalk_write_pipe); }
void string_callback(std_msgs::msg::String::UniquePtr msg) {
if (dectalk_write_pipe) {
// append a newline (\n) so DECtalk reads individual messages as sentences
if (fputs(msg->data.append("\n").c_str(), dectalk_write_pipe) == EOF)
RCLCPP_ERROR(this->get_logger(), "dectalk pipe write failed");
} else
RCLCPP_ERROR(this->get_logger(), "dectalk pipe closed unexpectedly");
}
private:
FILE *dectalk_write_pipe;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscriber_string_;
};
int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<Dectalk_ROS>());
rclcpp::shutdown();
return 0;
}