From b996bc99e57b38200ff86eafb3dad6e64cec9d3b Mon Sep 17 00:00:00 2001
From: MCFurry <f.b.f.schoenmakers@gmail.com>
Date: Fri, 2 Feb 2018 11:07:20 +0100
Subject: [PATCH] Arbitration lost errors are now configurable: either report
 as error or ignore and throw warning

---
 .../src/socketcan_bridge_node.cpp             |  3 +++
 .../src/socketcan_to_topic_node.cpp           |  3 +++
 .../src/topic_to_socketcan_node.cpp           |  3 +++
 .../include/socketcan_interface/socketcan.h   | 22 +++++++++++--------
 4 files changed, 22 insertions(+), 9 deletions(-)

diff --git a/socketcan_bridge/src/socketcan_bridge_node.cpp b/socketcan_bridge/src/socketcan_bridge_node.cpp
index 09afbd58d..c1efa6c6c 100644
--- a/socketcan_bridge/src/socketcan_bridge_node.cpp
+++ b/socketcan_bridge/src/socketcan_bridge_node.cpp
@@ -38,10 +38,13 @@ int main(int argc, char *argv[])
 
   ros::NodeHandle nh(""), nh_param("~");
 
+  bool lostArbIsError;
   std::string can_device;
   nh_param.param<std::string>("can_device", can_device, "can0");
+  nh_param.param<bool>("lost_arbitration_is_error", lostArbIsError, true);
 
   boost::shared_ptr<can::ThreadedSocketCANInterface> driver = boost::make_shared<can::ThreadedSocketCANInterface> ();
+  driver->arbitrationLostIsError_ = lostArbIsError;
 
   if (!driver->init(can_device, 0))  // initialize device at can_device, 0 for no loopback.
   {
diff --git a/socketcan_bridge/src/socketcan_to_topic_node.cpp b/socketcan_bridge/src/socketcan_to_topic_node.cpp
index 35b9c22d3..117e194cf 100644
--- a/socketcan_bridge/src/socketcan_to_topic_node.cpp
+++ b/socketcan_bridge/src/socketcan_to_topic_node.cpp
@@ -39,10 +39,13 @@ int main(int argc, char *argv[])
 
   ros::NodeHandle nh(""), nh_param("~");
 
+  bool lostArbIsError;
   std::string can_device;
   nh_param.param<std::string>("can_device", can_device, "can0");
+  nh_param.param<bool>("lost_arbitration_is_error", lostArbIsError, true);
 
   boost::shared_ptr<can::ThreadedSocketCANInterface> driver = boost::make_shared<can::ThreadedSocketCANInterface> ();
+  driver->arbitrationLostIsError_ = lostArbIsError;
 
   if (!driver->init(can_device, 0))  // initialize device at can_device, 0 for no loopback.
   {
diff --git a/socketcan_bridge/src/topic_to_socketcan_node.cpp b/socketcan_bridge/src/topic_to_socketcan_node.cpp
index 2d6d37898..61b213f3c 100644
--- a/socketcan_bridge/src/topic_to_socketcan_node.cpp
+++ b/socketcan_bridge/src/topic_to_socketcan_node.cpp
@@ -39,10 +39,13 @@ int main(int argc, char *argv[])
 
   ros::NodeHandle nh(""), nh_param("~");
 
+  bool lostArbIsError;
   std::string can_device;
   nh_param.param<std::string>("can_device", can_device, "can0");
+  nh_param.param<bool>("lost_arbitration_is_error", lostArbIsError, true);
 
   boost::shared_ptr<can::ThreadedSocketCANInterface> driver = boost::make_shared<can::ThreadedSocketCANInterface> ();
+  driver->arbitrationLostIsError_ = lostArbIsError;
 
   if (!driver->init(can_device, 0))  // initialize device at can_device, 0 for no loopback.
   {
diff --git a/socketcan_interface/include/socketcan_interface/socketcan.h b/socketcan_interface/include/socketcan_interface/socketcan.h
index 797fc8711..3c476add6 100644
--- a/socketcan_interface/include/socketcan_interface/socketcan.h
+++ b/socketcan_interface/include/socketcan_interface/socketcan.h
@@ -20,11 +20,12 @@ namespace can {
 class SocketCANInterface : public AsioDriver<boost::asio::posix::stream_descriptor> {
     bool loopback_;
     int sc_;
-public:    
+public:
+    bool arbitrationLostIsError_;
     SocketCANInterface()
-    : loopback_(false), sc_(-1)
+    : loopback_(false), sc_(-1), arbitrationLostIsError_(true)
     {}
-    
+
     virtual bool doesLoopBack() const{
         return loopback_;
     }
@@ -53,7 +54,7 @@ class SocketCANInterface : public AsioDriver<boost::asio::posix::stream_descript
             }
             can_err_mask_t err_mask =
                 ( CAN_ERR_TX_TIMEOUT   /* TX timeout (by netdevice driver) */
-                //CAN_ERR_LOSTARB      /* lost arbitration    / data[0]  (Not a fatal error)  */
+                | CAN_ERR_LOSTARB      /* lost arbitration    / data[0]    */
                 | CAN_ERR_CRTL         /* controller problems / data[1]    */
                 | CAN_ERR_PROT         /* protocol violations / data[2..3] */
                 | CAN_ERR_TRX          /* transceiver status  / data[4]    */
@@ -160,7 +161,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));
@@ -200,10 +200,14 @@ class SocketCANInterface : public AsioDriver<boost::asio::posix::stream_descript
                 input_.id = frame_.can_id & CAN_EFF_MASK;
                 input_.is_error = 1;
 
-                LOG("error: " << input_.id);
-                setInternalError(input_.id);
-                setNotReady();
-
+                bool errorIsArbitLost = (frame_.can_id & CAN_ERR_LOSTARB);
+                if(!errorIsArbitLost || (errorIsArbitLost && arbitrationLostIsError_)){
+                    LOG("error: " << input_.id);
+                    setInternalError(input_.id);
+                    setNotReady();
+                }else{
+                    LOG("warning: " << input_.id << ", arbitration lost but ignored");
+                }
             }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);