tx.cpp
Go to the documentation of this file.
00001 
00026 #include "nmea_comms/rx.h"
00027 #include "nmea_comms/checksum.h"
00028 
00029 #include <stdio.h>
00030 #include <poll.h>
00031 #include <boost/algorithm/string.hpp>
00032 
00033 #include "ros/ros.h"
00034 #include "nmea_msgs/Sentence.h"
00035 
00036 
00037 void tx_msg_callback(const nmea_msgs::SentenceConstPtr sentence_msg_ptr, int fd)
00038 {
00039   static int consecutive_errors = 0;
00040 
00041   char buffer[256];
00042   int buffer_length = snprintf(buffer, 256, "%s\r\n", sentence_msg_ptr->sentence.c_str());
00043 
00044   // No guarantee that write() will write everything, so we use poll() to block
00045   // on the availability of the fd for writing until the whole message has been
00046   // written out.
00047   const char* buffer_write = buffer;
00048   struct pollfd pollfds[] = { { fd, POLLOUT, 0 } };
00049   while (ros::ok())
00050   {
00051     int retval = poll(pollfds, 1, 1000);
00052 
00053     if (pollfds[0].revents & POLLHUP)
00054     {
00055       ROS_INFO("Device hangup occurred on attempted write.");
00056       return;
00057       //ros::shutdown();
00058     }
00059 
00060     if (pollfds[0].revents & POLLERR)
00061     {
00062       ROS_FATAL("Killing node due to device error.");
00063       ros::shutdown();
00064     }
00065 
00066     retval = write(fd, buffer_write, buffer_length - (buffer_write - buffer));
00067     if (retval > 0)
00068     {
00069       buffer_write += retval;
00070     }
00071     else
00072     {
00073       ROS_WARN("Device write error; abandoning message (%s).",
00074                sentence_msg_ptr->sentence.c_str());
00075       if (++consecutive_errors >= 10)
00076       {
00077         ROS_FATAL("Killing node due to %d consecutive write errors.", consecutive_errors);
00078         ros::shutdown();
00079       }
00080       break;
00081     }
00082     if (buffer_write - buffer >= buffer_length)
00083     {
00084       consecutive_errors = 0;
00085       break;
00086     }
00087   }
00088 }


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