serial_node.cpp
Go to the documentation of this file.
00001 
00034 #include <iostream>
00035 #include <boost/bind.hpp>
00036 #include <boost/asio.hpp>
00037 
00038 #include <ros/ros.h>
00039 
00040 #include "Session.h"
00041 #include "AsyncOkPoll.h"
00042 
00043 
00044 class SerialSession : public Session<boost::asio::serial_port>
00045 {
00046 public:
00047   SerialSession(boost::asio::io_service& io_service, std::string port, int baud)
00048     : Session(io_service), port_(port), baud_(baud), // io_service_(io_service),
00049       timer_(io_service), interval_(boost::posix_time::milliseconds(1000))
00050   {
00051     connect_with_reconnection();
00052   }
00053 
00054 private:
00055   ~SerialSession() {
00056     if (ros::ok()) {
00057       new SerialSession(socket().get_io_service(), port_, baud_);
00058     }
00059   }
00060 
00061   bool attempt_connection(bool log_errors = true)
00062   {
00063     if (log_errors) ROS_INFO("Opening serial port.");
00064     boost::system::error_code ec;
00065     socket().open(port_, ec);
00066     if (ec) {
00067       if (log_errors) ROS_ERROR_STREAM("Unable to open port " << port_);
00068       return false;
00069     }
00070 
00071     typedef boost::asio::serial_port_base serial;
00072     socket().set_option(serial::baud_rate(baud_));
00073     socket().set_option(serial::character_size(8));
00074     socket().set_option(serial::stop_bits(serial::stop_bits::one));
00075     socket().set_option(serial::parity(serial::parity::none));
00076     socket().set_option(serial::flow_control(serial::flow_control::none));
00077 
00078     // Kick off the session.
00079     start();
00080     return true;
00081   }
00082 
00083   void connect_with_reconnection(bool log_errors = true) {
00084     if (!attempt_connection(log_errors)) {  
00085       if (log_errors) {
00086         ROS_INFO_STREAM("Attempting reconnection every " << interval_.total_milliseconds() << " ms.");
00087       }
00088       timer_.expires_from_now(interval_);
00089       timer_.async_wait(boost::bind(&SerialSession::connect_with_reconnection, this, false));
00090     } else {
00091     }
00092   }
00093 
00094   std::string port_;
00095   int baud_;
00096   boost::posix_time::time_duration interval_;
00097   boost::asio::deadline_timer timer_;
00098 };
00099 
00100 
00101 int main(int argc, char* argv[])
00102 {
00103   boost::asio::io_service io_service;
00104 
00105   // Initialize ROS.
00106   ros::init(argc, argv, "rosserial_server_serial_node");
00107   ros::NodeHandle nh("~");
00108   std::string port; nh.param<std::string>("port", port, "/dev/ttyACM0");
00109   int baud; nh.param<int>("baud", baud, 57600);
00110 
00111   // ROS background thread.
00112   ros::AsyncSpinner ros_spinner(1);
00113   ros_spinner.start();
00114 
00115   // Monitor ROS for shutdown, and stop the io_service accordingly.
00116   AsyncOkPoll ok_poll(io_service, boost::posix_time::milliseconds(500), ros::ok);
00117 
00118   new SerialSession(io_service, port, baud);
00119   io_service.run();
00120 
00121   return 0;
00122 }


rosserial_server
Author(s): Mike Purvis
autogenerated on Fri Aug 28 2015 12:44:56