$search
00001 #include <cerrno> 00002 #include <cstdio> 00003 #include <cstring> 00004 #include <cstdlib> 00005 #include <unistd.h> 00006 00007 #include <net/if.h> 00008 #include <sys/ioctl.h> 00009 00010 #include <linux/can/raw.h> 00011 00012 #include <ros/console.h> 00013 00014 #include "can.h" 00015 00016 CAN::CAN() 00017 { 00018 sockaddr_can addr; 00019 ifreq ifr; 00020 char caninterface[] = "can0"; //TODO automatic searching for caninterface 00021 00022 cansocket_ = socket(PF_CAN, SOCK_RAW, CAN_RAW); 00023 if (cansocket_ < 0) { 00024 ROS_ERROR("can_init: Error opening socket (%s)", strerror(errno)); 00025 exit(1); 00026 } 00027 00028 addr.can_family = AF_CAN; 00029 00030 strcpy(ifr.ifr_name, caninterface); 00031 if (ioctl(cansocket_, SIOCGIFINDEX, &ifr) < 0) { 00032 ROS_ERROR("can_init: Error setting SIOCGIFINDEX for interace %s (%s)", caninterface, strerror(errno)); 00033 exit(1); 00034 } 00035 00036 addr.can_ifindex = ifr.ifr_ifindex; 00037 00038 if (bind(cansocket_, (sockaddr *)&addr, sizeof(addr)) < 0) { 00039 ROS_ERROR("can_init: Error binding socket (%s)", strerror(errno)); 00040 exit(1); 00041 } 00042 00043 ROS_INFO("CAN interface init done"); 00044 } 00045 00046 CAN::~CAN() 00047 { 00048 if (close(cansocket_) != 0) 00049 ROS_ERROR("can_close: Error closing can socket (%s)", strerror(errno)); 00050 } 00051 00052 bool CAN::send_frame(const can_frame *frame) 00053 { 00054 if (write(cansocket_, frame, sizeof(*frame)) != sizeof(*frame)) 00055 { 00056 ROS_ERROR("send_frame: Error writing socket (%s)", strerror(errno)); 00057 return false; 00058 } 00059 return true; 00060 } 00061 00062 bool CAN::receive_frame(can_frame *frame) 00063 { 00064 fd_set rfds; 00065 00066 FD_ZERO(&rfds); 00067 FD_SET(cansocket_, &rfds); 00068 00069 int rc = 1; 00070 timeval timeout; 00071 00072 timeout.tv_sec = 5; 00073 timeout.tv_usec = 0; 00074 00075 rc = select(cansocket_ + 1, &rfds, NULL, NULL, &timeout); 00076 00077 if (rc == 0) 00078 { 00079 ROS_ERROR("recive_frame: Receiving frame timed out (Kurt switched off?)"); 00080 return false; 00081 } 00082 else if (rc == -1) 00083 { 00084 ROS_WARN("recive_frame: Error receiving frame (%s)", strerror(errno)); 00085 return false; 00086 } 00087 00088 //TODO read time stamp 00089 if (read(cansocket_, frame, sizeof(*frame)) != sizeof(*frame)) 00090 { 00091 ROS_WARN("receive_frame: Error reading socket (%s)", strerror(errno)); 00092 return false; 00093 } 00094 return true; 00095 }