For reference, this code works even if the arduino IDE was never opened. It is for ROS but doesn't use rosserial, which I have found to be unreliable in the past.
The only time this code breaks is if the computer is restarted without unplugging the Teensy at some point. This could probably be solved but isn't crucial for my purposes.
Paul Stoffregen was right that I wasn't configuring the Teensy properly when I was opening it. Thanks for the help!
Code:
#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();
}
}
Reply With Quote
Originally Posted by LouisM
