44 static int previous_success = 1;
49 int serial_fd = open(port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
57 goto retry_connection;
60 if (!isatty(serial_fd))
69 struct termios options;
70 tcgetattr(serial_fd, &options);
72 options.c_cflag |= CS8;
74 options.c_cflag |= (CLOCAL | CREAD);
80 cfsetispeed(&options, B9600);
81 cfsetospeed(&options, B9600);
84 cfsetispeed(&options, B19200);
85 cfsetospeed(&options, B19200);
88 cfsetispeed(&options, B38400);
89 cfsetospeed(&options, B38400);
92 cfsetispeed(&options, B57600);
93 cfsetospeed(&options, B57600);
96 cfsetispeed(&options, B115200);
97 cfsetospeed(&options, B115200);
106 options.c_cc[VMIN] = 0;
107 options.c_cc[VTIME] = 1;
108 tcsetattr(serial_fd, TCSAFLUSH, &options);
112 previous_success = 1;
120 if (previous_success)
122 ROS_ERROR(
"Retrying connection every 1 second.");
123 previous_success = 0;
131 int main(
int argc,
char **argv)
133 ros::init(argc, argv,
"nmea_serial_node");
139 n_local.
param<std::string>(
"port", port,
"/dev/ttyUSB0");
140 n_local.
param(
"baud", baud, 115200);
142 std::string frame_id;
143 n_local.
param<std::string>(
"frame_id", frame_id,
"navsat");
void rx_thread_start(ros::NodeHandle &n, int fd, std::string frame_id, uint32_t byte_time_ns=0)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define ROS_FATAL_STREAM(args)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
int main(int argc, char **argv)
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
#define ROS_DEBUG_STREAM(args)
void manage_connection(const ros::TimerEvent &event, ros::NodeHandle &n, std::string port, int32_t baud, std::string frame_id)
#define ROS_INFO_STREAM(args)
ROSCPP_DECL void shutdown()
#define ROS_ERROR_STREAM(args)