From 6178d0a0e40e812238e5a355cdc84de4ab6e8e6b Mon Sep 17 00:00:00 2001 From: MCFurry Date: Mon, 5 Feb 2018 16:28:57 +0100 Subject: [PATCH] Changed boolean to enum to define log-level for arbitration errors --- .../socketcan_bridge/socketcan_to_topic.h | 1 + .../src/socketcan_bridge_node.cpp | 4 +- socketcan_bridge/src/socketcan_to_topic.cpp | 46 ++++++++++++++++++- .../src/socketcan_to_topic_node.cpp | 6 +-- .../src/topic_to_socketcan_node.cpp | 3 -- .../include/socketcan_interface/socketcan.h | 30 ++++++++---- 6 files changed, 68 insertions(+), 22 deletions(-) diff --git a/socketcan_bridge/include/socketcan_bridge/socketcan_to_topic.h b/socketcan_bridge/include/socketcan_bridge/socketcan_to_topic.h index 1f25840e2..547c67e58 100644 --- 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 driver); void setup(); diff --git a/socketcan_bridge/src/socketcan_bridge_node.cpp b/socketcan_bridge/src/socketcan_bridge_node.cpp index c1efa6c6c..90b77c5a1 100644 --- 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("can_device", can_device, "can0"); - nh_param.param("lost_arbitration_is_error", lostArbIsError, true); boost::shared_ptr driver = boost::make_shared (); - 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 index 5f5aca6ac..b50d1a2a8 100644 --- 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("received_messages", 10); driver_ = driver; + + std::string lostArbLevel; + nh_param->param("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 index 117e194cf..fd68c38e5 100644 --- a/socketcan_bridge/src/socketcan_to_topic_node.cpp +++ b/socketcan_bridge/src/socketcan_to_topic_node.cpp @@ -31,21 +31,16 @@ #include #include - - 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("can_device", can_device, "can0"); - nh_param.param("lost_arbitration_is_error", lostArbIsError, true); boost::shared_ptr driver = boost::make_shared (); - 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 index 61b213f3c..2d6d37898 100644 --- 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("can_device", can_device, "can0"); - nh_param.param("lost_arbitration_is_error", lostArbIsError, true); boost::shared_ptr driver = boost::make_shared (); - 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 3c476add6..c384fb394 100644 --- a/socketcan_interface/include/socketcan_interface/socketcan.h +++ b/socketcan_interface/include/socketcan_interface/socketcan.h @@ -15,15 +15,22 @@ #include +typedef enum{ + ARBITLOST_IS_DEBUG = 0, + ARBITLOST_IS_INFO, + ARBITLOST_IS_WARN, + ARBITLOST_IS_ERROR +} ArbitLost_Level_t; + namespace can { class SocketCANInterface : public AsioDriver { 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