Skip to content

Commit

Permalink
Implemented ROS param parsing for error ignoring in CAN, CAN_ERR_LOST…
Browse files Browse the repository at this point in the history 
…ARB is ignored by default, see also ros-industrial#362
  • Loading branch information
@yanick-douven
yanick-douven committed Apr 10, 2020
1 parent 860ebf5 commit fbe7bbf
Show file tree
Hide file tree
Showing 5 changed files with 153 additions and 19 deletions.
20 changes: 18 additions & 2 deletions socketcan_bridge/src/socketcan_to_topic_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,6 @@
#include <string>



int main(int argc, char *argv[])
{
ros::init(argc, argv, "socketcan_to_topic_node");
Expand All @@ -42,9 +41,26 @@ int main(int argc, char *argv[])
std::string can_device;
nh_param.param<std::string>("can_device", can_device, "can0");

// get ignored errors from ros parameters
can::ignored_errors_t ignored_errors;
nh_param.param<int>("ignore_errors/can_err_tx_timeout", ignored_errors["can_err_tx_timeout"], -1);
nh_param.param<int>("ignore_errors/can_err_lostarb", ignored_errors["can_err_lostarb"], -1);
nh_param.param<int>("ignore_errors/can_err_ctrl", ignored_errors["can_err_ctrl"], -1);
nh_param.param<int>("ignore_errors/can_err_prot", ignored_errors["can_err_prot"], -1);
nh_param.param<int>("ignore_errors/can_err_trx", ignored_errors["can_err_trx"], -1);
nh_param.param<int>("ignore_errors/can_err_ack", ignored_errors["can_err_ack"], -1);
nh_param.param<int>("ignore_errors/can_err_busoff", ignored_errors["can_err_busoff"], -1);
nh_param.param<int>("ignore_errors/can_err_buserror", ignored_errors["can_err_buserror"], -1);
nh_param.param<int>("ignore_errors/can_err_restarted", ignored_errors["can_err_restarted"], -1);

auto settings = can::SettingsMap();

settings.set("ignored_errors", ignored_errors);


can::ThreadedSocketCANInterfaceSharedPtr driver = std::make_shared<can::ThreadedSocketCANInterface> ();

if (!driver->init(can_device, 0, can::NoSettings())) // initialize device at can_device, 0 for no loopback.
if (!driver->init(can_device, 0, settings)) // initialize device at can_device, 0 for no loopback.
{
ROS_FATAL("Failed to initialize can_device at %s", can_device.c_str());
return 1;
Expand Down
19 changes: 18 additions & 1 deletion socketcan_bridge/src/topic_to_socketcan_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,9 +42,26 @@ int main(int argc, char *argv[])
std::string can_device;
nh_param.param<std::string>("can_device", can_device, "can0");

// get ignored errors from ros parameters
can::ignored_errors_t ignored_errors;
nh_param.param<int>("ignore_errors/can_err_tx_timeout", ignored_errors["can_err_tx_timeout"], -1);
nh_param.param<int>("ignore_errors/can_err_lostarb", ignored_errors["can_err_lostarb"], -1);
nh_param.param<int>("ignore_errors/can_err_ctrl", ignored_errors["can_err_ctrl"], -1);
nh_param.param<int>("ignore_errors/can_err_prot", ignored_errors["can_err_prot"], -1);
nh_param.param<int>("ignore_errors/can_err_trx", ignored_errors["can_err_trx"], -1);
nh_param.param<int>("ignore_errors/can_err_ack", ignored_errors["can_err_ack"], -1);
nh_param.param<int>("ignore_errors/can_err_busoff", ignored_errors["can_err_busoff"], -1);
nh_param.param<int>("ignore_errors/can_err_buserror", ignored_errors["can_err_buserror"], -1);
nh_param.param<int>("ignore_errors/can_err_restarted", ignored_errors["can_err_restarted"], -1);

auto settings = can::SettingsMap();

settings.set("ignored_errors", ignored_errors);


can::ThreadedSocketCANInterfaceSharedPtr driver = std::make_shared<can::ThreadedSocketCANInterface> ();

if (!driver->init(can_device, 0, can::NoSettings())) // initialize device at can_device, 0 for no loopback.
if (!driver->init(can_device, 0, settings)) // initialize device at can_device, 0 for no loopback.
{
ROS_FATAL("Failed to initialize can_device at %s", can_device.c_str());
return 1;
Expand Down
59 changes: 59 additions & 0 deletions socketcan_interface/include/socketcan_interface/serializable_map.h
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,59 @@
#ifndef SOCKETCAN_INTERFACE_SERIALIZABLE_MAP_H
#define SOCKETCAN_INTERFACE_SERIALIZABLE_MAP_H

#include <map>
#include <sstream>

template<class Key, class Value>
class serializable_map : public std::map<Key, Value> {
private:
size_t offset;

template<class T>
void write(std::stringstream &ss, T &t) const {
ss.write((char*)(&t), sizeof(t));
}

void write(std::stringstream &ss, std::string &str) const {
size_t size = str.size();
ss.write((char*)(&size), sizeof(size));
ss.write((char*)(str.data()), str.length());
}

template<class T>
void read(const std::string &s, T &t) {
t = (T)(*(s.data() + offset));
offset += sizeof(T);
}

void read(const std::string &s, std::string &str) {
size_t size = (int)(*(s.data() + offset));
offset += sizeof(size_t);
std::string str2(s.data() + offset, s.data() + offset + size);
str = str2;
offset += size;
}
public:
std::string serialize() const {
std::stringstream ss;
for (auto &i : (*this)) {
Key str = i.first;
Value value = i.second;
write(ss, str);
write(ss, value);
}
return ss.str();
}
void deserialize(const std::string &s) {
offset = 0;
while (offset < s.size()) {
Key key;
Value value;
read(s, key);
read(s, value);
(*this)[key] = value;
}
}
};

#endif /* SOCKETCAN_INTERFACE_SERIALIZABLE_MAP_H */
6 changes: 3 additions & 3 deletions socketcan_interface/include/socketcan_interface/settings.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ namespace can {
template <typename T> bool get(const std::string &n, T& val) const {
std::string repr;
if(!getRepr(n, repr)) return false;
val = boost::lexical_cast<T>(repr);
val = boost::lexical_cast<T>(repr);
return true;
}
virtual ~Settings() {}
Expand All @@ -29,7 +29,7 @@ namespace can {

class NoSettings : public Settings {
private:
virtual bool getRepr(const std::string &n, std::string & repr) const { return true; }
virtual bool getRepr(const std::string &n, std::string & repr) const { return false; }
};

class SettingsMap : public Settings {
Expand All @@ -41,7 +41,7 @@ namespace can {
return true;
}
public:
template <typename T> void set(const std::string &n, T& val) {
template <typename T> void set(const std::string &n, const T& val) {
settings_[n] = boost::lexical_cast<std::string>(val);
}
};
Expand Down
68 changes: 55 additions & 13 deletions socketcan_interface/include/socketcan_interface/socketcan.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,10 +14,49 @@
#include <linux/can/error.h>

#include <cstring>
#include <iostream>

#include <socketcan_interface/dispatcher.h>
#include <socketcan_interface/string.h>

#include "socketcan_interface/settings.h"
#include "socketcan_interface/serializable_map.h"

namespace can {

typedef serializable_map<std::string, int> ignored_errors_t;
typedef std::map<std::string, std::pair<can_err_mask_t, bool>> ignored_errors_defaults_t;

// default error settings
ignored_errors_defaults_t ignore_error_defaults {
{"can_err_tx_timeout", {CAN_ERR_TX_TIMEOUT, false}}, /* TX timeout (by netdevice driver) */
{"can_err_lostarb", {CAN_ERR_LOSTARB, true }}, /* lost arbitration / data[0] */
{"can_err_crtl", {CAN_ERR_CRTL, false}}, /* controller problems / data[1] */
{"can_err_prot", {CAN_ERR_PROT, false}}, /* protocol violations / data[2..3] */
{"can_err_trx", {CAN_ERR_TRX, false}}, /* transceiver status / data[4] */
{"can_err_ack", {CAN_ERR_ACK, false}}, /* received no ACK on transmission */
{"can_err_busoff", {CAN_ERR_BUSOFF, false}}, /* bus off */
{"can_err_buserror", {CAN_ERR_BUSERROR, false}}, /* bus error (may flood!) */
{"can_err_restarted", {CAN_ERR_RESTARTED, false}} /* controller restarted */
};

} // namespace can


namespace boost {
template<>
can::ignored_errors_t lexical_cast(const std::string & s) {
can::ignored_errors_t res;
res.deserialize(s);
return res;
}
template<>
std::string lexical_cast(const can::ignored_errors_t & i) {
return i.serialize();
}
} // namespace boost


namespace can {

class SocketCANInterface : public AsioDriver<boost::asio::posix::stream_descriptor> {
Expand All @@ -37,20 +76,10 @@ class SocketCANInterface : public AsioDriver<boost::asio::posix::stream_descript
return init(device, loopback, NoSettings());
}
virtual bool init(const std::string &device, bool loopback, const Settings &settings) override {
const can_err_mask_t all_errors = ( CAN_ERR_TX_TIMEOUT /* TX timeout (by netdevice driver) */
| 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] */
| CAN_ERR_ACK /* received no ACK on transmission */
| CAN_ERR_BUSOFF /* bus off */
| CAN_ERR_BUSERROR /* bus error (may flood!) */
| CAN_ERR_RESTARTED /* controller restarted */
);

State s = getState();
if(s.driver_state == State::closed){
can_err_mask_t ignored_errors = settings.get_optional("ignored_errors", 0);
ignored_errors_t ignored_errors;
settings.get("ignored_errors", ignored_errors);
sc_ = 0;
device_ = device;
loopback_ = loopback;
Expand All @@ -70,7 +99,20 @@ class SocketCANInterface : public AsioDriver<boost::asio::posix::stream_descript
close(sc);
return false;
}
can_err_mask_t err_mask = (all_errors & ~ignored_errors) | CAN_ERR_BUSOFF;

can_err_mask_t err_mask = 0;
ignored_errors_t::iterator ie_it;
for (ignored_errors_defaults_t::iterator it = ignore_error_defaults.begin(); it != ignore_error_defaults.end(); ++it) {
if ((ie_it = ignored_errors.find(it->first)) == ignored_errors.end() || ie_it->second == -1) {
// no user setting found, or user setting equals -1: use default
// std::cout << it->first << " -> default (" << it->second.first * !it->second.second << ")" << std::endl;
err_mask |= it->second.first * !it->second.second;
} else {
// use user setting
// std::cout << it->first << " -> user (" << it->second.first * !ie_it->second << ")" << std::endl;
err_mask |= it->second.first * !ie_it->second;
}
}

ret = setsockopt(sc, SOL_CAN_RAW, CAN_RAW_ERR_FILTER,
&err_mask, sizeof(err_mask));
Expand Down

0 comments on commit fbe7bbf

Please sign in to comment.