serial_node.cpp
Go to the documentation of this file.
00001 
00026 #include "ros/ros.h"
00027 
00028 #include <unistd.h>
00029 #include <fcntl.h>
00030 #include <termios.h>
00031 
00032 #include "nmea_comms/rx.h"
00033 
00034 
00035 void manage_connection(const ros::TimerEvent& event, ros::NodeHandle& n, std::string port, int32_t baud, std::string frame_id)
00036 {
00037   if (rx_prune_threads() > 0)
00038   {
00039     // Serial thread already active. Nothing to do here.
00040     return;
00041   }
00042 
00043   // Only output error messages when things were previously peachy.
00044   static int previous_success = 1;
00045 
00046   while (ros::ok())
00047   {
00048     ROS_DEBUG_STREAM("Opening serial port: " << port);
00049     int serial_fd = open(port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY | O_NONBLOCK);
00050 
00051     if (serial_fd == -1)
00052     {
00053       if (previous_success)
00054       {
00055         ROS_ERROR_STREAM("Could not open " << port << ".");
00056       }
00057       goto retry_connection;
00058     }
00059 
00060     if (!isatty(serial_fd))
00061     {
00062       if (previous_success)
00063       {
00064         ROS_ERROR_STREAM("File " << port << " is not a tty.");
00065       }
00066       goto close_serial;
00067     }
00068 
00069     struct termios options;
00070     tcgetattr(serial_fd, &options);
00071     options.c_cflag = 0;
00072     options.c_cflag |= CS8;
00073 
00074     options.c_cflag |= (CLOCAL | CREAD);
00075 
00076     ROS_DEBUG_STREAM("Setting baud rate: " << baud);
00077     switch (baud)
00078     {
00079     case 9600:
00080       cfsetispeed(&options, B9600);
00081       cfsetospeed(&options, B9600);
00082       break;
00083     case 19200:
00084       cfsetispeed(&options, B19200);
00085       cfsetospeed(&options, B19200);
00086       break;
00087     case 38400:
00088       cfsetispeed(&options, B38400);
00089       cfsetospeed(&options, B38400);
00090       break;
00091     case 57600:
00092       cfsetispeed(&options, B57600);
00093       cfsetospeed(&options, B57600);
00094       break;
00095     case 115200:
00096       cfsetispeed(&options, B115200);
00097       cfsetospeed(&options, B115200);
00098       break;
00099     default:
00100       ROS_FATAL_STREAM("Unsupported baud rate: " << baud);
00101       ros::shutdown();
00102     }
00103     options.c_iflag = 0;
00104     options.c_oflag = 0;
00105     options.c_lflag = 0;
00106     options.c_cc[VMIN]  = 0;
00107     options.c_cc[VTIME] = 1;
00108     tcsetattr(serial_fd, TCSAFLUSH, &options);
00109 
00110     // Successful connection setup; kick off servicing thread.
00111     ROS_INFO_STREAM("Successfully connected on " << port << ".");
00112     previous_success = 1;
00113     rx_thread_start(n, serial_fd, frame_id, 1000000000 / baud * 10);
00114     break;
00115 
00116 close_serial:
00117     close(serial_fd);
00118 
00119 retry_connection:
00120     if (previous_success)
00121     {
00122       ROS_ERROR("Retrying connection every 1 second.");
00123       previous_success = 0;
00124     }
00125 
00126     ros::Duration(1.0).sleep();
00127   }
00128 }
00129 
00130 
00131 int main(int argc, char **argv)
00132 {
00133   ros::init(argc, argv, "nmea_serial_node");
00134   ros::NodeHandle n_local("~");
00135   ros::NodeHandle n;
00136 
00137   std::string port;
00138   int32_t baud;
00139   n_local.param<std::string>("port", port, "/dev/ttyUSB0");
00140   n_local.param("baud", baud, 115200);
00141 
00142   std::string frame_id;
00143   n_local.param<std::string>("frame_id", frame_id, "navsat");
00144 
00145   ros::Timer timer = n.createTimer(ros::Duration(1.0),
00146                                    boost::bind(manage_connection, _1, n, port, baud, frame_id));
00147   ros::spin();
00148 
00149   rx_stop_all();
00150 
00151   return 0;
00152 }


nmea_comms
Author(s): Mike Purvis
autogenerated on Thu Dec 22 2016 03:29:22