Removed arbitration errors from socket error mask

socketcan_bridge/include/socketcan_bridge/socketcan_to_topic.h

@@ -37,6 +37,7 @@ namespace socketcan_bridge
 class SocketCANToTopic
 {
   public:
+    ArbitLost_Level_t arbitrationLostIsError_;
     SocketCANToTopic(ros::NodeHandle* nh, ros::NodeHandle* nh_param, boost::shared_ptr<can::DriverInterface> driver);
     void setup();
 

socketcan_bridge/src/socketcan_bridge_node.cpp

@@ -59,6 +59,7 @@ int main(int argc, char *argv[])
 
   socketcan_bridge::SocketCANToTopic to_topic_bridge(&nh, &nh_param, driver);
   to_topic_bridge.setup();
+  driver->arbitrationLostIsError_ = to_topic_bridge.arbitrationLostIsError_;
 
   ros::spin();
 

socketcan_bridge/src/socketcan_to_topic.cpp

@@ -37,6 +37,31 @@ namespace socketcan_bridge
     {
       can_topic_ = nh->advertise<can_msgs::Frame>("received_messages", 10);
       driver_ = driver;
+
+      std::string lostArbLevel;
+      nh_param->param<std::string>("lost_arbitration_reporting_level", lostArbLevel,"ARBITLOST_IS_ERROR");
+      if (!lostArbLevel.compare("ARBITLOST_IS_DEBUG"))
+      {
+        this->arbitrationLostIsError_ = ARBITLOST_IS_DEBUG;
+      }
+      else if(!lostArbLevel.compare("ARBITLOST_IS_INFO"))
+      {
+        this->arbitrationLostIsError_ = ARBITLOST_IS_INFO;
+      }
+      else if(!lostArbLevel.compare("ARBITLOST_IS_WARN"))
+      {
+        this->arbitrationLostIsError_ = ARBITLOST_IS_WARN;
+      }
+      else if(!lostArbLevel.compare("ARBITLOST_IS_ERROR"))
+      {
+        this->arbitrationLostIsError_ = ARBITLOST_IS_ERROR;
+      }
+      else
+      {
+        ROS_ERROR("Unsupported value for 'lost_arbitration_reporting_level' parameter, using ARBITLOST_IS_ERROR as default");
+        this->arbitrationLostIsError_ = ARBITLOST_IS_ERROR;
+      }
+
     };
 
   void SocketCANToTopic::setup()
@@ -65,7 +90,26 @@ namespace socketcan_bridge
         {
           // can::tostring cannot be used for dlc > 8 frames. It causes an crash
           // due to usage of boost::array for the data array. The should always work.
-          ROS_WARN("Received frame is error: %s", can::tostring(f, true).c_str());
+          if (f.id & CAN_ERR_LOSTARB)
+          {
+            switch(arbitrationLostIsError_)
+            {
+            case ARBITLOST_IS_DEBUG:
+              ROS_DEBUG("Received frame indicates an arbitration loss");
+              break;
+            case ARBITLOST_IS_INFO:
+              ROS_INFO("Received frame indicates an arbitration loss");
+              break;
+            case ARBITLOST_IS_WARN:
+            case ARBITLOST_IS_ERROR:
+              ROS_WARN("Received frame indicates an arbitration loss");
+              break;
+            }
+          }
+          else
+          {
+            ROS_WARN("Received frame is error: %s", can::tostring(f, true).c_str());
+          }
         }
       }
 

socketcan_bridge/src/socketcan_to_topic_node.cpp

@@ -31,8 +31,6 @@
 #include <socketcan_interface/string.h>
 #include <string>
 
-
-
 int main(int argc, char *argv[])
 {
   ros::init(argc, argv, "socketcan_to_topic_node");
@@ -56,6 +54,7 @@ int main(int argc, char *argv[])
 
   socketcan_bridge::SocketCANToTopic to_topic_bridge(&nh, &nh_param, driver);
   to_topic_bridge.setup();
+  driver->arbitrationLostIsError_ = to_topic_bridge.arbitrationLostIsError_;
 
   ros::spin();
 

socketcan_interface/include/socketcan_interface/socketcan.h

@@ -15,16 +15,24 @@
 
 #include <socketcan_interface/dispatcher.h>
 
+typedef enum{
+    ARBITLOST_IS_DEBUG = 0,
+    ARBITLOST_IS_INFO,
+    ARBITLOST_IS_WARN,
+    ARBITLOST_IS_ERROR
+} ArbitLost_Level_t;
+
 namespace can {
 
 class SocketCANInterface : public AsioDriver<boost::asio::posix::stream_descriptor> {
     bool loopback_;
     int sc_;
-public:    
+public:
+    ArbitLost_Level_t arbitrationLostIsError_;
     SocketCANInterface()
-    : loopback_(false), sc_(-1)
+    : loopback_(false), sc_(-1), arbitrationLostIsError_(ARBITLOST_IS_ERROR)
     {}
-    
+
     virtual bool doesLoopBack() const{
         return loopback_;
     }
@@ -160,7 +168,6 @@ class SocketCANInterface : public AsioDriver<boost::asio::posix::stream_descript
 protected:
     std::string device_;
     can_frame frame_;
-
     virtual void triggerReadSome(){
         boost::mutex::scoped_lock lock(send_mutex_);
         socket_.async_read_some(boost::asio::buffer(&frame_, sizeof(frame_)), boost::bind( &SocketCANInterface::readFrame,this, boost::asio::placeholders::error));
@@ -196,15 +203,22 @@ class SocketCANInterface : public AsioDriver<boost::asio::posix::stream_descript
                 input_.data[i] = frame_.data[i];
             }
 
-            if(frame_.can_id & CAN_ERR_FLAG){ // error message
+            if(frame_.can_id & CAN_ERR_FLAG) // error message
+            {
                 input_.id = frame_.can_id & CAN_EFF_MASK;
                 input_.is_error = 1;
 
-                LOG("error: " << input_.id);
-                setInternalError(input_.id);
-                setNotReady();
-
-            }else{
+                bool errorIsArbitLost = (frame_.can_id & CAN_ERR_LOSTARB);
+                if(!errorIsArbitLost ||
+                    (errorIsArbitLost && (arbitrationLostIsError_ == ARBITLOST_IS_ERROR)))
+                {
+                  LOG("error: " << input_.id);
+                  setInternalError(input_.id);
+                  setNotReady();
+                }
+            }
+            else
+            {
                 input_.is_extended = (frame_.can_id & CAN_EFF_FLAG) ? 1 :0;
                 input_.id = frame_.can_id & (input_.is_extended ? CAN_EFF_MASK : CAN_SFF_MASK);
                 input_.is_error = 0;