can.cpp
Go to the documentation of this file.
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");
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 }


uos_rotunit_driver
Author(s):
autogenerated on Fri Aug 28 2015 13:31:03