Changed boolean to enum to define log-level for arbitration errors

ros-industrial · Feb 5, 2018 · 7f63faf · 7f63faf
1 parent b811552
commit 7f63faf
Show file tree

Hide file tree

Showing 6 changed files with 68 additions and 22 deletions.
diff --git a/socketcan_bridge/include/socketcan_bridge/socketcan_to_topic.h b/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();
 

diff --git a/socketcan_bridge/src/socketcan_bridge_node.cpp b/socketcan_bridge/src/socketcan_bridge_node.cpp
@@ -38,13 +38,10 @@ 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.
   {
@@ -62,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();
 

diff --git a/socketcan_bridge/src/socketcan_to_topic.cpp b/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());
+          }
         }
       }
 

diff --git a/socketcan_bridge/src/socketcan_to_topic_node.cpp b/socketcan_bridge/src/socketcan_to_topic_node.cpp
@@ -31,21 +31,16 @@
 #include <socketcan_interface/string.h>
 #include <string>
 
-
-
 int main(int argc, char *argv[])
 {
   ros::init(argc, argv, "socketcan_to_topic_node");
 
   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.
   {
@@ -59,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();
 

diff --git a/socketcan_bridge/src/topic_to_socketcan_node.cpp b/socketcan_bridge/src/topic_to_socketcan_node.cpp
@@ -39,13 +39,10 @@ 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
@@ -15,15 +15,22 @@
 
 #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:
-    bool arbitrationLostIsError_;
+    ArbitLost_Level_t arbitrationLostIsError_;
     SocketCANInterface()
-    : loopback_(false), sc_(-1), arbitrationLostIsError_(true)
+    : loopback_(false), sc_(-1), arbitrationLostIsError_(ARBITLOST_IS_ERROR)
     {}
 
     virtual bool doesLoopBack() const{
@@ -196,19 +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;
 
                 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");
+                if(!errorIsArbitLost ||
+                    (errorIsArbitLost && (arbitrationLostIsError_ == ARBITLOST_IS_ERROR)))
+                {
+                  LOG("error: " << input_.id);
+                  setInternalError(input_.id);
+                  setNotReady();
                 }
-            }else{
+            }
+            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;