git commit -m "first commit for v2"

Devices/Libraries/Ros/ros_canopen/socketcan_interface/src/canbcm.cpp

@@ -0,0 +1,39 @@
+#include <socketcan_interface/bcm.h>
+#include <socketcan_interface/string.h>
+
+using namespace can;
+
+#include <iostream>
+
+int main(int argc, char *argv[]){
+    BCMsocket bcm;
+
+    int extra_frames = argc - 4; 
+
+    if(extra_frames < 0){
+        std::cout << "usage: "<< argv[0] << " DEVICE PERIOD HEADER#DATA [DATA*]" << std::endl;
+        return 1;
+    }
+
+    if(!bcm.init(argv[1])){
+        return 2;
+    }
+
+    int num_frames = extra_frames+1;
+    Frame *frames = new Frame[num_frames];
+    Header header = frames[0] = toframe(argv[3]);
+
+    if(extra_frames > 0){
+        for(int i=0; i< extra_frames; ++i){
+            frames[1+i] = toframe(tostring(header,true)+"#"+argv[4+i]);
+        }
+    }
+    for(int i = 0; i < num_frames; ++i){
+        std::cout << frames[i] << std::endl;
+    }
+    if(bcm.startTX(boost::chrono::duration<double>(atof(argv[2])), header, num_frames, frames)){
+        pause();
+        return 0;
+    }
+    return 4;
+}

Devices/Libraries/Ros/ros_canopen/socketcan_interface/src/candump.cpp

@@ -0,0 +1,90 @@
+#include <iostream>
+#include <memory>
+#include <unordered_set>
+
+#include <boost/exception/diagnostic_information.hpp>
+#include <class_loader/class_loader.hpp>
+#include <socketcan_interface/socketcan.h>
+
+using namespace can;
+
+#include <iostream>
+
+void print_frame(const Frame &f){
+
+    if(f.is_error){
+        std::cout << "E " << std::hex << f.id << std::dec;
+    }else if(f.is_extended){
+        std::cout << "e " << std::hex << f.id << std::dec;
+    }else{
+        std::cout << "s " << std::hex << f.id << std::dec;
+    }
+
+    std::cout << "\t";
+
+    if(f.is_rtr){
+        std::cout << "r";
+    }else{
+        std::cout << (int) f.dlc << std::hex;
+
+        for(int i=0; i < f.dlc; ++i){
+            std::cout << std::hex << " " << (int) f.data[i];
+        }
+    }
+
+    std::cout << std::dec << std::endl;
+}
+
+
+std::shared_ptr<class_loader::ClassLoader> g_loader;
+DriverInterfaceSharedPtr g_driver;
+
+void print_state(const State & s){
+    std::string err;
+    g_driver->translateError(s.internal_error,err);
+    std::cout << "STATE: driver_state=" << s.driver_state << " internal_error=" << s.internal_error << "('" << err << "') asio: " << s.error_code << std::endl;
+}
+
+
+int main(int argc, char *argv[]){
+
+    if(argc != 2 && argc != 4){
+        std::cout << "usage: "<< argv[0] << " DEVICE [PLUGIN_PATH PLUGIN_NAME]" << std::endl;
+        return 1;
+    }
+
+    if(argc == 4 ){
+        try
+        {
+            g_loader = std::make_shared<class_loader::ClassLoader>(argv[2]);
+            g_driver = g_loader->createUniqueInstance<DriverInterface>(argv[3]);
+        }
+
+        catch(std::exception& ex)
+        {
+            std::cerr << boost::diagnostic_information(ex) << std::endl;;
+            return 1;
+        }
+    }else{
+        g_driver = std::make_shared<SocketCANInterface>();
+    }
+
+
+
+    FrameListenerConstSharedPtr frame_printer = g_driver->createMsgListener(print_frame);
+    StateListenerConstSharedPtr error_printer = g_driver->createStateListener(print_state);
+
+    if(!g_driver->init(argv[1], false, can::NoSettings::create())){
+        print_state(g_driver->getState());
+        return 1;
+    }
+
+    g_driver->run();
+
+    g_driver->shutdown();
+    g_driver.reset();
+    g_loader.reset();
+
+    return 0;
+
+}

Devices/Libraries/Ros/ros_canopen/socketcan_interface/src/socketcan_interface_plugin.cpp

@@ -0,0 +1,4 @@
+#include <class_loader/class_loader.hpp> 
+#include <socketcan_interface/socketcan.h>
+
+CLASS_LOADER_REGISTER_CLASS(can::SocketCANInterface, can::DriverInterface);

Devices/Libraries/Ros/ros_canopen/socketcan_interface/src/string.cpp

@@ -0,0 +1,182 @@
+#include <socketcan_interface/string.h>
+#include <iomanip>
+
+namespace can {
+
+bool hex2dec(uint8_t& d, const char& h) {
+	if ('0' <= h && h <= '9')
+		d = h - '0';
+	else if ('a' <= h && h <= 'f')
+		d = h - 'a' + 10;
+	else if ('A' <= h && h <= 'F')
+		d = h - 'A' + 10;
+	else
+		return false;
+
+	return true;
+}
+
+bool hex2buffer(std::string& out, const std::string& in_raw, bool pad) {
+	std::string in(in_raw);
+	if ((in.size() % 2) != 0) {
+		if (pad)
+			in.insert(0, "0");
+		else
+			return false;
+	}
+	out.resize(in.size() >> 1);
+	for (size_t i = 0; i < out.size(); ++i) {
+		uint8_t hi, lo;
+		if (!hex2dec(hi, in[i << 1]) || !hex2dec(lo, in[(i << 1) + 1]))
+			return false;
+
+		out[i] = (hi << 4) | lo;
+	}
+	return true;
+}
+
+bool dec2hex(char& h, const uint8_t& d, bool lc) {
+	if (d < 10) {
+		h = '0' + d;
+	} else if (d < 16 && lc) {
+		h = 'a' + (d - 10);
+	} else if (d < 16 && !lc) {
+		h = 'A' + (d - 10);
+	} else {
+		h='?';
+		return false;
+	}
+	return true;
+}
+
+std::string byte2hex(const uint8_t& d, bool pad, bool lc) {
+	uint8_t hi = d >> 4;
+	char c=0;
+	std::string s;
+	if (hi || pad) {
+		dec2hex(c, hi, lc);
+		s += c;
+	}
+	dec2hex(c, d & 0xf, lc);
+	s += c;
+	return s;
+}
+
+std::string buffer2hex(const std::string& in, bool lc) {
+	std::string s;
+	s.reserve(in.size() * 2);
+	for (size_t i = 0; i < in.size(); ++i) {
+		std::string b = byte2hex(in[i], true, lc);
+		if (b.empty())
+			return b;
+
+		s += b;
+	}
+	return s;
+}
+
+std::string tostring(const Header& h, bool lc) {
+	std::string s;
+	s.reserve(8);
+	std::stringstream buf;
+	buf << std::hex;
+	if (lc)
+		buf << std::nouppercase;
+	else
+		buf << std::uppercase;
+
+	if (h.is_extended)
+		buf << std::setfill('0') << std::setw(8);
+
+	buf << (h.fullid() & ~Header::EXTENDED_MASK);
+	return buf.str();
+}
+
+uint32_t tohex(const std::string& s) {
+		unsigned int h = 0;
+		std::stringstream stream;
+		stream << std::hex << s;
+		stream >> h;
+		return h;
+}
+
+Header toheader(const std::string& s) {
+	unsigned int h = tohex(s);
+	unsigned int id = h & Header::ID_MASK;
+	return Header(id, h & Header::EXTENDED_MASK || (s.size() == 8 && id >= (1<<11)),
+			h & Header::RTR_MASK, h & Header::ERROR_MASK);
+}
+
+std::string tostring(const Frame& f, bool lc) {
+	std::string s;
+	s.resize(f.dlc);
+	for (uint8_t i = 0; i < f.dlc; ++i) {
+		s[i] = f.data[i];
+	}
+	return tostring((const Header&) (f), lc) + '#' + buffer2hex(s, lc);
+}
+
+Frame toframe(const std::string& s) {
+	size_t sep = s.find('#');
+	if (sep == std::string::npos)
+		return MsgHeader(0xfff);
+
+	Header header = toheader(s.substr(0, sep));
+	Frame frame(header);
+	std::string buffer;
+	if (header.isValid() && hex2buffer(buffer, s.substr(sep + 1), false)) {
+		if (buffer.size() > 8)
+			return MsgHeader(0xfff);
+
+		for (size_t i = 0; i < buffer.size(); ++i) {
+			frame.data[i] = buffer[i];
+		}
+		frame.dlc = buffer.size();
+	}
+	return frame;
+}
+
+template<> FrameFilterSharedPtr tofilter(const std::string  &s){
+	  FrameFilter * filter = 0;
+		size_t delim = s.find_first_of(":~-_");
+
+		uint32_t second = FrameMaskFilter::MASK_RELAXED;
+		bool invert = false;
+		char type = ':';
+
+		if(delim != std::string::npos) {
+			type = s.at(delim);
+			second = tohex(s.substr(delim +1));
+		}
+		uint32_t first = toheader(s.substr(0, delim)).fullid();
+		switch (type) {
+			case '~':
+				invert = true;
+			case ':':
+				filter = new FrameMaskFilter(first, second, invert);
+				break;
+			case '_':
+				invert = true;
+			case '-':
+				filter = new FrameRangeFilter(first, second, invert);
+				break;
+		}
+		return FrameFilterSharedPtr(filter);
+}
+template<> FrameFilterSharedPtr tofilter(const uint32_t &id){
+		return FrameFilterSharedPtr(new FrameMaskFilter(id));
+}
+
+FrameFilterSharedPtr tofilter(const char* s){
+		return tofilter<std::string>(s);
+}
+
+std::ostream& operator <<(std::ostream& stream, const Header& h) {
+	return stream << can::tostring(h, true);
+}
+
+std::ostream& operator <<(std::ostream& stream, const Frame& f) {
+	return stream << can::tostring(f, true);
+}
+
+}