Skip to content
Permalink

Comparing changes

Choose two branches to see what’s changed or to start a new pull request. If you need to, you can also or learn more about diff comparisons.

Open a pull request

Create a new pull request by comparing changes across two branches. If you need to, you can also . Learn more about diff comparisons here.
base repository: ros-industrial/ros_canopen
Failed to load repositories. Confirm that selected base ref is valid, then try again.
Loading
base: kinetic-devel
Choose a base ref
...
head repository: MCFurry/ros_canopen
Failed to load repositories. Confirm that selected head ref is valid, then try again.
Loading
compare: kinetic-devel
Choose a head ref
Can’t automatically merge. Don’t worry, you can still create the pull request.
  • 3 commits
  • 5 files changed
  • 1 contributor

Commits on Feb 6, 2018

  1. Removed arbitration errors from socket error mask

    Since arbitration errors are not fatal, removed them from the mask
    MCFurry committed Feb 6, 2018
    Copy the full SHA
    cd89f5f View commit details
  2. Arbitration lost errors are now configurable: either report as error …

    …or ignore and throw warning
    MCFurry committed Feb 6, 2018
    Copy the full SHA
    b996bc9 View commit details
  3. Copy the full SHA
    6178d0a View commit details
Original file line number Diff line number Diff line change
@@ -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();

1 change: 1 addition & 0 deletions socketcan_bridge/src/socketcan_bridge_node.cpp
Original file line number Diff line number Diff line change
@@ -59,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();

46 changes: 45 additions & 1 deletion socketcan_bridge/src/socketcan_to_topic.cpp
Original file line number Diff line number Diff line change
@@ -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());
}
}
}

3 changes: 1 addition & 2 deletions socketcan_bridge/src/socketcan_to_topic_node.cpp
Original file line number Diff line number Diff line change
@@ -31,8 +31,6 @@
#include <socketcan_interface/string.h>
#include <string>



int main(int argc, char *argv[])
{
ros::init(argc, argv, "socketcan_to_topic_node");
@@ -56,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();

34 changes: 24 additions & 10 deletions socketcan_interface/include/socketcan_interface/socketcan.h
Original file line number Diff line number Diff line change
@@ -15,16 +15,24 @@

#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:
public:
ArbitLost_Level_t arbitrationLostIsError_;
SocketCANInterface()
: loopback_(false), sc_(-1)
: loopback_(false), sc_(-1), arbitrationLostIsError_(ARBITLOST_IS_ERROR)
{}

virtual bool doesLoopBack() const{
return loopback_;
}
@@ -160,7 +168,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));
@@ -196,15 +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;

LOG("error: " << input_.id);
setInternalError(input_.id);
setNotReady();

}else{
bool errorIsArbitLost = (frame_.can_id & CAN_ERR_LOSTARB);
if(!errorIsArbitLost ||
(errorIsArbitLost && (arbitrationLostIsError_ == ARBITLOST_IS_ERROR)))
{
LOG("error: " << input_.id);
setInternalError(input_.id);
setNotReady();
}
}
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;