candump.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 #include <boost/unordered_set.hpp>
00003 #include <boost/exception/diagnostic_information.hpp>
00004 #include <boost/make_shared.hpp>
00005 #include <class_loader/class_loader.h>
00006 #include <socketcan_interface/socketcan.h>
00007 
00008 using namespace can;
00009 
00010 #include <iostream>
00011 
00012 void print_error(const State & s);
00013 
00014 void print_frame(const Frame &f){
00015     
00016     if(f.is_error){
00017         std::cout << "E " << std::hex << f.id << std::dec;
00018     }else if(f.is_extended){
00019         std::cout << "e " << std::hex << f.id << std::dec;
00020     }else{
00021         std::cout << "s " << std::hex << f.id << std::dec;
00022     }
00023     
00024     std::cout << "\t";
00025     
00026     if(f.is_rtr){
00027         std::cout << "r";
00028     }else{
00029         std::cout << (int) f.dlc << std::hex;
00030         
00031         for(int i=0; i < f.dlc; ++i){
00032             std::cout << std::hex << " " << (int) f.data[i];
00033         }
00034     }
00035     
00036     std::cout << std::dec << std::endl;
00037 }
00038 
00039 
00040 boost::shared_ptr<class_loader::ClassLoader> g_loader;
00041 boost::shared_ptr<DriverInterface> g_driver;
00042 
00043 void print_error(const State & s){
00044     std::string err;
00045     g_driver->translateError(s.internal_error,err);
00046     std::cout << "ERROR: state=" << s.driver_state << " internal_error=" << s.internal_error << "('" << err << "') asio: " << s.error_code << std::endl;
00047 }
00048 
00049 
00050 int main(int argc, char *argv[]){
00051     
00052     if(argc != 2 && argc != 4){
00053         std::cout << "usage: "<< argv[0] << " DEVICE [PLUGIN_PATH PLUGIN_NAME]" << std::endl;
00054         return 1;
00055     }
00056 
00057     if(argc == 4 ){
00058         try
00059         {
00060             g_loader = boost::make_shared<class_loader::ClassLoader>(argv[2]);
00061             g_driver = g_loader->createInstance<DriverInterface>(argv[3]);
00062         }
00063         
00064         catch(std::exception& ex)
00065         {
00066             std::cerr << boost::diagnostic_information(ex) << std::endl;;
00067             return 1;
00068         }
00069     }else{
00070         g_driver = boost::make_shared<SocketCANInterface>();
00071     }
00072     
00073 
00074 
00075     CommInterface::FrameListener::Ptr frame_printer = g_driver->createMsgListener(print_frame);
00076     StateInterface::StateListener::Ptr error_printer = g_driver->createStateListener(print_error);
00077     
00078     if(!g_driver->init(argv[1], false)){
00079         print_error(g_driver->getState());
00080         return 1;
00081     }
00082 
00083     g_driver->run();
00084     
00085     g_driver->shutdown();
00086     g_driver.reset();
00087     g_loader.reset();
00088     
00089     return 0;
00090     
00091 }


socketcan_interface
Author(s): Mathias Lüdtke
autogenerated on Sun Sep 3 2017 03:10:37