comm.h
Go to the documentation of this file.
00001 /*
00002 
00003 Copyright (c) 2011, Markus Achtelik, ASL, ETH Zurich, Switzerland
00004 You can contact the author at <markus dot achtelik at mavt dot ethz dot ch>
00005 
00006 All rights reserved.
00007 
00008 Redistribution and use in source and binary forms, with or without
00009 modification, are permitted provided that the following conditions are met:
00010 * Redistributions of source code must retain the above copyright
00011 notice, this list of conditions and the following disclaimer.
00012 * Redistributions in binary form must reproduce the above copyright
00013 notice, this list of conditions and the following disclaimer in the
00014 documentation and/or other materials provided with the distribution.
00015 * Neither the name of ETHZ-ASL nor the
00016 names of its contributors may be used to endorse or promote products
00017 derived from this software without specific prior written permission.
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00020 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022 DISCLAIMED. IN NO EVENT SHALL ETHZ-ASL BE LIABLE FOR ANY
00023 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029 
00030 */
00031 
00032 #ifndef __COMM_H__
00033 #define __COMM_H__
00034 
00035 #include <iostream>
00036 #include <map>
00037 #include <vector>
00038 #include <boost/bind.hpp>
00039 #include <boost/function.hpp>
00040 #include <boost/shared_ptr.hpp>
00041 #include <boost/thread.hpp>
00042 #include <boost/asio.hpp>
00043 
00044 #include <ros/ros.h>
00045 
00046 // communication definitions
00047 #include <asctec_hl_comm/HL_interface.h>
00048 
00049 typedef boost::function<void(uint8_t*, int32_t)> CommCallbackType;
00050 typedef std::map<uint8_t, CommCallbackType> CommCallbackMap;
00051 
00052 class Comm;
00053 typedef boost::shared_ptr<Comm> CommPtr;
00054 
00056 
00062 class Comm
00063 {
00064   typedef boost::asio::serial_port SerialPort;
00065   typedef boost::shared_ptr<SerialPort> SerialPortPtr;
00066 
00067 private:
00068 
00069   SerialPortPtr port_rx_;
00070   SerialPortPtr port_tx_;
00071   boost::asio::io_service uart_service_;
00072   boost::asio::deadline_timer rx_timeout_;
00073   bool rx_timeout_occurred_;
00074   boost::thread uart_thread_[2];
00075 
00076   std::string port_rx_name_;
00077   std::string port_tx_name_;
00078 
00079   uint32_t rx_packet_cnt_;
00080   uint32_t rx_packet_good_cnt_;
00081 
00082   uint8_t tx_buffer_[256];
00083   uint8_t rx_buffer_[256];
00084   uint32_t tx_bytes2send_;
00085   int rx_state_;
00086   std::vector<uint8_t> packet_acks_;
00087   boost::mutex tx_mutex_;
00088   const int TX_HEADER_SIZE;
00089 
00090   CommCallbackMap callbacks_;
00091 
00092   void rxReadStart(uint32_t length, uint32_t timeout = 0);
00093   void rxReadCallback(const boost::system::error_code& error, size_t bytes_transferred);
00094   void rxTimeoutCallback(const boost::system::error_code & error);
00095   bool configurePort(SerialPortPtr & serial_port, uint32_t *baudrate);
00096   bool connect(SerialPortPtr & serial_port, const std::string & port, uint32_t baudrate);
00097 
00098   void processPacketAck(uint8_t * buf, uint32_t length);
00099   bool validateChecksum(uint8_t * buf, uint32_t length, uint16_t checksum);
00100 
00101   bool waitForPacketAck(uint8_t ack_id, const double & timeout = 1.0);
00102 
00103   inline uint16_t crc_update(uint16_t crc, uint8_t data);
00104   uint16_t crc16(void* data, uint16_t cnt, uint16_t crc = 0xff);
00105 
00106   void serializePacket(uint8_t packet_id, const void * data, const int32_t & packet_size, uint8_t flag = 0);
00107 
00108 public:
00109   Comm();
00110 //  Comm(const std::string & port, uint32_t baudrate);
00111   ~Comm();
00112 
00114 
00125   bool connect(const std::string & port_rx, const std::string & port_tx, uint32_t baudrate);
00126 
00128   void close();
00129 
00130   uint32_t getRxPackets()
00131   {
00132     return rx_packet_cnt_;
00133   }
00134 
00135   uint32_t getRxPacketsGood()
00136   {
00137     return rx_packet_good_cnt_;
00138   }
00139 
00141   template<typename T>
00142     bool sendPacket(uint8_t packet_id, const T & data)
00143     {
00144       serializePacket(packet_id, &data, sizeof(T));
00145 //      return port_tx_->write_block(tx_buffer_, tx_bytes2send_);
00146       return tx_bytes2send_ == boost::asio::write(*port_tx_, boost::asio::buffer(tx_buffer_, tx_bytes2send_));
00147     }
00148 
00150   template<typename T>
00151     bool sendPacketAck(uint8_t packet_id, const T & data, const double & timeout = 1.0)
00152     {
00153       // generate some "random" value to determine acknowledged packet afterwards. This is only meant for some configuration packets, so it should be random enough ;-)
00154       ros::Time time_start = ros::Time::now();
00155       uint8_t ack_id = (uint8_t)(time_start.toNSec() & 0xff) | HLI_COMM_ACK;
00156 //      static uint8_t cnt = 1;
00157 //      uint8_t ack_id = cnt | HLI_COMM_ACK;
00158 //      cnt++;
00159 
00160       serializePacket(packet_id, &data, sizeof(T), ack_id);
00161       if (tx_bytes2send_ != boost::asio::write(*port_tx_, boost::asio::buffer(tx_buffer_, tx_bytes2send_)))
00162         return false;
00163 
00164       return waitForPacketAck(ack_id, timeout);
00165     }
00166 
00168 
00175   template<class T>
00176     void registerCallback(uint8_t id, void(T::*cb_func)(uint8_t*, uint32_t), T* p_obj)
00177     {
00178       if (callbacks_.find(id) != callbacks_.end())
00179         std::cerr<<"message "<< id << "is already registered"<<std::endl;
00180       else
00181         callbacks_[id] = boost::bind(cb_func, p_obj, _1, _2);
00182     }
00183 void loopOnce(){uart_service_.run_one();};
00184 };
00185 
00186 #endif /* ASCTECCOMM_H_ */


asctec_hl_interface
Author(s): Markus Achtelik, Michael Achtelik, Stephan Weiss, Laurent Kneip
autogenerated on Thu Aug 27 2015 12:26:52