#include <ros/ros.h>
#include <std_msgs/String.h>
#include <thread>
#include <chrono>
#include <stdio.h>
#include <string.h>
#include <signal.h>
#include <stdlib.h>
#include <stdio.h>
#include <fcntl.h> // Contains file controls like O_RDWR
#include <errno.h> // Error integer and strerror() function
#include <termios.h>
#include <unistd.h>
int serial_port = open("/dev/Ladybug_timestamping_teensy", O_RDWR);
void my_handler(sig_atomic_t s){ //this should close the USB port if you do ctrl+C
ROS_INFO("Caught signal %d, will close USB port in 1 seconds\n",s);
tcflush(serial_port,TCIOFLUSH);
sleep(1);
close(serial_port);
exit(1);
}
int main(int argc, char **argv){
ros::init(argc, argv, "Teensy_serial_node");
ros::NodeHandle nh;
ROS_INFO("Good evening.");
int serial_port = open("/dev/Ladybug_timestamping_teensy", O_RDWR);
usleep(10000);
if (serial_port < 0) {
ROS_INFO("Error %i from open: %s\n", errno, strerror(errno));
}
ros::Publisher from_Teensy_publisher = nh.advertise<std_msgs::String>("Timestamp_for_reconstruction", 10);
std::this_thread::sleep_for(std::chrono::milliseconds(1000)); //wait for node to initiate
struct termios tty; //we could call this anything, for convention we call it tty
if(tcgetattr(serial_port, &tty) != 0) {
ROS_INFO("Error %i from tcgetattr: %s\n", errno, strerror(errno));
}
tty.c_cflag &= ~PARENB; // Clear parity bit, disabling parity (most common)
tty.c_cflag &= ~CSTOPB; // Clear stop field, only one stop bit used in communication (most common)
tty.c_cflag &= ~CSIZE; // Clear all the size bits, then use one of the statements below
tty.c_cflag |= CS8; // 8 bits per byte (most common)
tty.c_cflag &= ~CRTSCTS; // Disable RTS/CTS hardware flow control (most common)
tty.c_cflag |= CREAD | CLOCAL; // Turn on READ & ignore ctrl lines (CLOCAL = 1)
tty.c_lflag &= ~ICANON; // THIS ~~~~~~~~~IMPORTANT~~~~~~~~~~~~~~~~ we want raw serial data, this is what the arduino IDE does
tty.c_lflag &= ~ECHO; // Disable echo WE PROBABLY DONT NEED THESE THREE LINES
tty.c_lflag &= ~ECHOE; // Disable erasure WE PROBABLY DONT NEED THESE THREE LINES
tty.c_lflag &= ~ECHONL; // Disable new-line echo WE PROBABLY DONT NEED THESE THREE LINES
tty.c_lflag &= ~ISIG; // Disable interpretation of INTR, QUIT and SUSP
tty.c_iflag &= ~(IXON | IXOFF | IXANY); // Turn off s/w flow ctrl
tty.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL); // ~~~~~~~~~~AGAIN WE WANT RAW~~~~~~~~Disable any special handling of received bytes
tty.c_oflag &= ~OPOST; // Prevent special interpretation of output bytes (e.g. newline chars)
tty.c_oflag &= ~ONLCR; // Prevent conversion of newline to carriage return/line feed
tty.c_cc[VTIME] = 10; //timeout value
tty.c_cc[VMIN] = 0;//returns data immediately, change if you need to wait for a certain number of characters
cfsetispeed(&tty, B9600);// set Baud rate
cfsetospeed(&tty, B9600);// set Baud rate
// Save tty settings, also checking for error
if (tcsetattr(serial_port, TCSANOW, &tty) != 0) {
ROS_INFO("Error %i from tcsetattr: %s\n", errno, strerror(errno));
}
unsigned char msg[] = { 'H'};
write(serial_port, "H", sizeof(msg)); // this tells the Teensy to start sending data
ros::Rate loop_rate(60);
ROS_INFO("declared the loop rate.");
signal (SIGINT,my_handler);
while(ros::ok()){
ros::spinOnce();
ROS_INFO_STREAM("Reading from serial port");
char read_buf [256];
memset(&read_buf, '\0', sizeof(read_buf));
int num_bytes = read(serial_port, &read_buf, sizeof(read_buf));
// num bytes is the number of bytes read. n may be 0 if no bytes were received, and can also be negative to signal an error.
if (num_bytes < 0) {//deal with the error
ROS_INFO("Error reading: %s", strerror(errno));
return 1;
}
// ROS_INFO("Read %i bytes. Received message: %s", num_bytes, read_buf);
std_msgs::String result;
result.data = read_buf;
ROS_INFO_STREAM("Read: " << result.data);
from_Teensy_publisher.publish(result);
loop_rate.sleep();
}
}