Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

To resolve the socketcan interface dying due to can errors and failing to recover #474

Open
wants to merge 2 commits into
base: melodic-devel
Choose a base branch
from
Open

To resolve the socketcan interface dying due to can errors and failing to recover #474

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
811d935
Added can device recovery code
akashmod Sep 1, 2022
23bcb45
Correction: recover is return type bool
akashmod Oct 29, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion socketcan_bridge/include/socketcan_bridge/socketcan_to_topic.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,10 +50,11 @@ class SocketCANToTopic

can::FrameListenerConstSharedPtr frame_listener_;
can::StateListenerConstSharedPtr state_listener_;

ros::Timer timer;

void frameCallback(const can::Frame& f);
void stateCallback(const can::State & s);
void timerCallback(const ros::TimerEvent & Event);
};

void convertSocketCANToMessage(const can::Frame& f, can_msgs::Frame& m)
Expand Down
2 changes: 2 additions & 0 deletions socketcan_bridge/include/socketcan_bridge/topic_to_socketcan.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,9 +45,11 @@ class TopicToSocketCAN
can::DriverInterfaceSharedPtr driver_;

can::StateListenerConstSharedPtr state_listener_;
ros::Timer timer;

void msgCallback(const can_msgs::Frame::ConstPtr& msg);
void stateCallback(const can::State & s);
void timerCallback(const ros::TimerEvent& Event );
};

void convertMessageToSocketCAN(const can_msgs::Frame& m, can::Frame& f)
Expand Down
9 changes: 8 additions & 1 deletion socketcan_bridge/src/socketcan_to_topic.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,7 @@ namespace socketcan_bridge
can_topic_ = nh->advertise<can_msgs::Frame>("received_messages",
nh_param->param("received_messages_queue_size", 10));
driver_ = driver;
timer = nh->createTimer(ros::Duration(0.1), std::bind(&SocketCANToTopic::timerCallback, this, std::placeholders::_1));
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is a polling approach. We should at least be able to configure the duration via for example a ROS parameter. Also there should be an option to disable this behaviour.

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Perhaps by treating a zero duration as disabled. Zero is a common exception used for this and save a new parameter.

};

void SocketCANToTopic::setup()
Expand Down Expand Up @@ -85,7 +86,13 @@ namespace socketcan_bridge
if (nh.getParam("can_ids", filters)) return setup(filters);
return setup();
}

void SocketCANToTopic::timerCallback(const ros::TimerEvent& Event )
{
if(!driver_->getState().isReady())
{
driver_->recover();
}
}

void SocketCANToTopic::frameCallback(const can::Frame& f)
{
Expand Down
9 changes: 8 additions & 1 deletion socketcan_bridge/src/topic_to_socketcan.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,14 +37,21 @@ namespace socketcan_bridge
can_topic_ = nh->subscribe<can_msgs::Frame>("sent_messages", nh_param->param("sent_messages_queue_size", 10),
std::bind(&TopicToSocketCAN::msgCallback, this, std::placeholders::_1));
driver_ = driver;
timer = nh->createTimer(ros::Duration(0.1), std::bind(&TopicToSocketCAN::timerCallback, this, std::placeholders::_1));
};

void TopicToSocketCAN::setup()
{
state_listener_ = driver_->createStateListener(
std::bind(&TopicToSocketCAN::stateCallback, this, std::placeholders::_1));
};

void TopicToSocketCAN::timerCallback(const ros::TimerEvent& Event )
{
if(!driver_->getState().isReady())
{
driver_->recover();
}
}
void TopicToSocketCAN::msgCallback(const can_msgs::Frame::ConstPtr& msg)
{
// ROS_DEBUG("Message came from sent_messages topic");
Expand Down
10 changes: 10 additions & 0 deletions socketcan_interface/include/socketcan_interface/threading.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,9 @@ class StateWaiter{

template<typename WrappedInterface> class ThreadedInterface : public WrappedInterface{
std::shared_ptr<boost::thread> thread_;
SettingsConstSharedPtr settings_;
std::string device_;
bool loopback_;
void run_thread(){
WrappedInterface::run();
}
Expand All @@ -61,6 +64,9 @@ template<typename WrappedInterface> class ThreadedInterface : public WrappedInte
#pragma GCC diagnostic pop
}
virtual bool init(const std::string &device, bool loopback, SettingsConstSharedPtr settings) override {
device_ = device;
loopback_ = loopback;
settings_ = settings;
if(!thread_ && WrappedInterface::init(device, loopback, settings)){
StateWaiter waiter(this);
thread_.reset(new boost::thread(&ThreadedInterface::run_thread, this));
Expand All @@ -78,6 +84,10 @@ template<typename WrappedInterface> class ThreadedInterface : public WrappedInte
thread_->join();
}
}
virtual bool recover(){
shutdown();
return init(device_,loopback_,settings_);
}
virtual ~ThreadedInterface() {
shutdown_internal();
}
Expand Down