git commit -m "first commit for v2"

Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/initialize_vcan.sh

@@ -0,0 +1,35 @@
+#!/bin/bash
+
+# sets up two virtual can interfaces, vcan0 and vcan1
+
+lsmod | grep -q "vcan"
+VCAN_NOT_LOADED=$?
+
+if [ $VCAN_NOT_LOADED -eq 1 ]; then
+    echo "vcan kernel module is not available..."
+    echo "loading it;"
+    sudo modprobe -a vcan
+fi
+
+ifconfig vcan0 > /dev/null
+VCAN_NOT_EXIST=$?
+
+if [ $VCAN_NOT_EXIST -eq 1 ]; then
+  echo "vcan0 does not exist, creating it."
+  sudo ip link add dev vcan0 type vcan
+  sudo ip link set vcan0 up
+else
+  echo "vcan0 already exists."
+fi
+
+
+ifconfig vcan1 > /dev/null
+VCAN_NOT_EXIST=$?
+if [ $VCAN_NOT_EXIST -eq 1 ]; then
+  echo "vcan0 does not exist, creating it."
+  sudo ip link add dev vcan1 type vcan
+  sudo ip link set vcan1 up
+else
+  echo "vcan0 already exists."
+fi
+

Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/test_conversion.cpp

@@ -0,0 +1,140 @@
+/*
+ * Copyright (c) 2016, Ivor Wanders
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *   * Redistributions of source code must retain the above copyright notice,
+ *     this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in the
+ *     documentation and/or other materials provided with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived from
+ *     this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <socketcan_bridge/topic_to_socketcan.h>
+#include <socketcan_bridge/socketcan_to_topic.h>
+
+#include <can_msgs/Frame.h>
+#include <socketcan_interface/socketcan.h>
+
+// Bring in gtest
+#include <gtest/gtest.h>
+
+// test whether the content of a conversion from a SocketCAN frame
+// to a ROS message correctly maintains the data.
+TEST(ConversionTest, socketCANToTopicStandard)
+{
+  can::Frame f;
+  can_msgs::Frame m;
+  f.id = 127;
+  f.dlc = 8;
+  f.is_error = false;
+  f.is_rtr = false;
+  f.is_extended = false;
+  for (uint8_t i = 0; i < f.dlc; ++i)
+  {
+    f.data[i] = i;
+  }
+  socketcan_bridge::convertSocketCANToMessage(f, m);
+  EXPECT_EQ(127, m.id);
+  EXPECT_EQ(8, m.dlc);
+  EXPECT_EQ(false, m.is_error);
+  EXPECT_EQ(false, m.is_rtr);
+  EXPECT_EQ(false, m.is_extended);
+
+  for (uint8_t i=0; i < 8; i++)
+  {
+    EXPECT_EQ(i, m.data[i]);
+  }
+}
+
+// test all three flags seperately.
+TEST(ConversionTest, socketCANToTopicFlags)
+{
+  can::Frame f;
+  can_msgs::Frame m;
+
+  f.is_error = true;
+  socketcan_bridge::convertSocketCANToMessage(f, m);
+  EXPECT_EQ(true, m.is_error);
+  f.is_error = false;
+
+  f.is_rtr = true;
+  socketcan_bridge::convertSocketCANToMessage(f, m);
+  EXPECT_EQ(true, m.is_rtr);
+  f.is_rtr = false;
+
+  f.is_extended = true;
+  socketcan_bridge::convertSocketCANToMessage(f, m);
+  EXPECT_EQ(true, m.is_extended);
+  f.is_extended = false;
+}
+
+// idem, but the other way around.
+TEST(ConversionTest, topicToSocketCANStandard)
+{
+  can::Frame f;
+  can_msgs::Frame m;
+  m.id = 127;
+  m.dlc = 8;
+  m.is_error = false;
+  m.is_rtr = false;
+  m.is_extended = false;
+  for (uint8_t i = 0; i < m.dlc; ++i)
+  {
+    m.data[i] = i;
+  }
+  socketcan_bridge::convertMessageToSocketCAN(m, f);
+  EXPECT_EQ(127, f.id);
+  EXPECT_EQ(8, f.dlc);
+  EXPECT_EQ(false, f.is_error);
+  EXPECT_EQ(false, f.is_rtr);
+  EXPECT_EQ(false, f.is_extended);
+
+
+  for (uint8_t i=0; i < 8; i++)
+  {
+    EXPECT_EQ(i, f.data[i]);
+  }
+}
+
+TEST(ConversionTest, topicToSocketCANFlags)
+{
+  can::Frame f;
+  can_msgs::Frame m;
+
+  m.is_error = true;
+  socketcan_bridge::convertMessageToSocketCAN(m, f);
+  EXPECT_EQ(true, f.is_error);
+  m.is_error = false;
+
+  m.is_rtr = true;
+  socketcan_bridge::convertMessageToSocketCAN(m, f);
+  EXPECT_EQ(true, f.is_rtr);
+  m.is_rtr = false;
+
+  m.is_extended = true;
+  socketcan_bridge::convertMessageToSocketCAN(m, f);
+  EXPECT_EQ(true, f.is_extended);
+  m.is_extended = false;
+}
+
+// Run all the tests that were declared with TEST()
+int main(int argc, char **argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/to_socketcan.test

@@ -0,0 +1,3 @@
+<launch>
+    <test test-name="test_to_socketcan" pkg="socketcan_bridge" type="test_to_socketcan" clear_params="true" time-limit="10.0" />
+</launch>

Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/to_socketcan_test.cpp

@@ -0,0 +1,194 @@
+/*
+ * Copyright (c) 2016, Ivor Wanders
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *   * Redistributions of source code must retain the above copyright notice,
+ *     this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in the
+ *     documentation and/or other materials provided with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived from
+ *     this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <socketcan_bridge/topic_to_socketcan.h>
+#include <socketcan_bridge/socketcan_to_topic.h>
+
+#include <can_msgs/Frame.h>
+#include <socketcan_interface/socketcan.h>
+#include <socketcan_interface/dummy.h>
+
+#include <gtest/gtest.h>
+#include <ros/ros.h>
+#include <list>
+#include <memory>
+
+class frameCollector
+{
+  public:
+    std::list<can::Frame> frames;
+
+    frameCollector() {}
+
+    void frameCallback(const can::Frame& f)
+    {
+      frames.push_back(f);
+    }
+};
+
+TEST(TopicToSocketCANTest, checkCorrectData)
+{
+  ros::NodeHandle nh(""), nh_param("~");
+
+  can::DummyBus bus("checkCorrectData");
+  // create the dummy interface
+  can::ThreadedDummyInterfaceSharedPtr dummy = std::make_shared<can::ThreadedDummyInterface>();
+
+  // start the to topic bridge.
+  socketcan_bridge::TopicToSocketCAN to_socketcan_bridge(&nh, &nh_param, dummy);
+  to_socketcan_bridge.setup();
+
+  // init the driver to test stateListener (not checked automatically).
+  dummy->init(bus.name, true, can::NoSettings::create());
+
+  // register for messages on received_messages.
+  ros::Publisher publisher_ = nh.advertise<can_msgs::Frame>("sent_messages", 10);
+
+  // create a frame collector.
+  frameCollector frame_collector_;
+
+  //  driver->createMsgListener(&frameCallback);
+  can::FrameListenerConstSharedPtr frame_listener_ = dummy->createMsgListener(
+
+            std::bind(&frameCollector::frameCallback, &frame_collector_, std::placeholders::_1));
+
+  // create a message
+  can_msgs::Frame msg;
+  msg.is_extended = true;
+  msg.is_rtr = false;
+  msg.is_error = false;
+  msg.id = 0x1337;
+  msg.dlc = 8;
+  for (uint8_t i=0; i < msg.dlc; i++)
+  {
+    msg.data[i] = i;
+  }
+
+  msg.header.frame_id = "0";  // "0" for no frame.
+  msg.header.stamp = ros::Time::now();
+
+  // send the can_frame::Frame message to the sent_messages topic.
+  publisher_.publish(msg);
+
+  // give some time for the interface some time to process the message
+  ros::WallDuration(1.0).sleep();
+  ros::spinOnce();
+
+  dummy->flush();
+
+  can_msgs::Frame received;
+  can::Frame f = frame_collector_.frames.back();
+  socketcan_bridge::convertSocketCANToMessage(f, received);
+
+  EXPECT_EQ(received.id, msg.id);
+  EXPECT_EQ(received.dlc, msg.dlc);
+  EXPECT_EQ(received.is_extended, msg.is_extended);
+  EXPECT_EQ(received.is_rtr, msg.is_rtr);
+  EXPECT_EQ(received.is_error, msg.is_error);
+  EXPECT_EQ(received.data, msg.data);
+}
+
+TEST(TopicToSocketCANTest, checkInvalidFrameHandling)
+{
+  // - tries to send a non-extended frame with an id larger than 11 bits.
+  //   that should not be sent.
+  // - verifies that sending one larger than 11 bits actually works.
+  // - tries sending a message with a dlc > 8 bytes, this should not be sent.
+  //   sending with 8 bytes is verified by the checkCorrectData testcase.
+
+  ros::NodeHandle nh(""), nh_param("~");
+
+
+  can::DummyBus bus("checkInvalidFrameHandling");
+
+  // create the dummy interface
+  can::ThreadedDummyInterfaceSharedPtr dummy = std::make_shared<can::ThreadedDummyInterface>();
+
+  // start the to topic bridge.
+  socketcan_bridge::TopicToSocketCAN to_socketcan_bridge(&nh, &nh_param, dummy);
+  to_socketcan_bridge.setup();
+
+  dummy->init(bus.name, true, can::NoSettings::create());
+
+  // register for messages on received_messages.
+  ros::Publisher publisher_ = nh.advertise<can_msgs::Frame>("sent_messages", 10);
+
+  // create a frame collector.
+  frameCollector frame_collector_;
+
+  //  add callback to the dummy interface.
+  can::FrameListenerConstSharedPtr frame_listener_ = dummy->createMsgListener(
+          std::bind(&frameCollector::frameCallback, &frame_collector_, std::placeholders::_1));
+
+  // create a message
+  can_msgs::Frame msg;
+  msg.is_extended = false;
+  msg.id = (1<<11)+1;  // this is an illegal CAN packet... should not be sent.
+  msg.header.frame_id = "0";  // "0" for no frame.
+  msg.header.stamp = ros::Time::now();
+
+  // send the can_frame::Frame message to the sent_messages topic.
+  publisher_.publish(msg);
+
+  // give some time for the interface some time to process the message
+  ros::WallDuration(1.0).sleep();
+  ros::spinOnce();
+  dummy->flush();
+
+  EXPECT_EQ(frame_collector_.frames.size(), 0);
+
+  msg.is_extended = true;
+  msg.id = (1<<11)+1;  // now it should be alright.
+  // send the can_frame::Frame message to the sent_messages topic.
+  publisher_.publish(msg);
+  ros::WallDuration(1.0).sleep();
+  ros::spinOnce();
+  dummy->flush();
+
+  EXPECT_EQ(frame_collector_.frames.size(), 1);
+
+  // wipe the frame queue.
+  frame_collector_.frames.clear();
+
+
+  // finally, check if frames with a dlc > 8 are discarded.
+  msg.dlc = 10;
+  publisher_.publish(msg);
+  ros::WallDuration(1.0).sleep();
+  ros::spinOnce();
+  dummy->flush();
+
+  EXPECT_EQ(frame_collector_.frames.size(), 0);
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "test_to_topic");
+  ros::NodeHandle nh;
+  ros::WallDuration(1.0).sleep();
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/to_topic.test

@@ -0,0 +1,3 @@
+<launch>
+    <test test-name="test_to_topic" pkg="socketcan_bridge" type="test_to_topic" clear_params="true" time-limit="10.0" />
+</launch>

Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/to_topic_test.cpp

@@ -0,0 +1,325 @@
+/*
+ * Copyright (c) 2016, Ivor Wanders
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *   * Redistributions of source code must retain the above copyright notice,
+ *     this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above copyright
+ *     notice, this list of conditions and the following disclaimer in the
+ *     documentation and/or other materials provided with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived from
+ *     this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <socketcan_bridge/socketcan_to_topic.h>
+
+#include <can_msgs/Frame.h>
+#include <socketcan_interface/socketcan.h>
+#include <socketcan_interface/dummy.h>
+#include <socketcan_bridge/topic_to_socketcan.h>
+
+#include <gtest/gtest.h>
+#include <ros/ros.h>
+#include <list>
+#include <memory>
+#include <string>
+#include <vector>
+
+class msgCollector
+{
+  public:
+    std::list<can_msgs::Frame> messages;
+
+    msgCollector() {}
+
+    void msgCallback(const can_msgs::Frame& f)
+    {
+      messages.push_back(f);
+    }
+};
+
+std::string convertMessageToString(const can_msgs::Frame &msg, bool lc = true)
+{
+  can::Frame f;
+  socketcan_bridge::convertMessageToSocketCAN(msg, f);
+  return can::tostring(f, lc);
+}
+
+TEST(SocketCANToTopicTest, checkCorrectData)
+{
+  ros::NodeHandle nh(""), nh_param("~");
+
+  can::DummyBus bus("checkCorrectData");
+
+  // create the dummy interface
+  can::ThreadedDummyInterfaceSharedPtr dummy = std::make_shared<can::ThreadedDummyInterface>();
+
+  // start the to topic bridge.
+  socketcan_bridge::SocketCANToTopic to_topic_bridge(&nh, &nh_param, dummy);
+  to_topic_bridge.setup();  // initiate the message callbacks
+
+  // init the driver to test stateListener (not checked automatically).
+  dummy->init(bus.name, true, can::NoSettings::create());
+
+  // create a frame collector.
+  msgCollector message_collector_;
+
+  // register for messages on received_messages.
+  ros::Subscriber subscriber_ = nh.subscribe("received_messages", 1, &msgCollector::msgCallback, &message_collector_);
+
+  // create a can frame
+  can::Frame f;
+  f.is_extended = true;
+  f.is_rtr = false;
+  f.is_error = false;
+  f.id = 0x1337;
+  f.dlc = 8;
+  for (uint8_t i=0; i < f.dlc; i++)
+  {
+    f.data[i] = i;
+  }
+
+  // send the can frame to the driver
+  dummy->send(f);
+
+  // give some time for the interface some time to process the message
+  ros::WallDuration(1.0).sleep();
+  ros::spinOnce();
+
+  ASSERT_EQ(1, message_collector_.messages.size());
+
+  // compare the received can_msgs::Frame message to the sent can::Frame.
+  can::Frame received;
+  can_msgs::Frame msg = message_collector_.messages.back();
+  socketcan_bridge::convertMessageToSocketCAN(msg, received);
+
+  EXPECT_EQ(received.id, f.id);
+  EXPECT_EQ(received.dlc, f.dlc);
+  EXPECT_EQ(received.is_extended, f.is_extended);
+  EXPECT_EQ(received.is_rtr, f.is_rtr);
+  EXPECT_EQ(received.is_error, f.is_error);
+  EXPECT_EQ(received.data, f.data);
+}
+
+TEST(SocketCANToTopicTest, checkInvalidFrameHandling)
+{
+  // - tries to send a non-extended frame with an id larger than 11 bits.
+  //   that should not be sent.
+  // - verifies that sending one larger than 11 bits actually works.
+
+  // sending a message with a dlc > 8 is not possible as the DummyInterface
+  // causes a crash then.
+
+  ros::NodeHandle nh(""), nh_param("~");
+
+  can::DummyBus bus("checkInvalidFrameHandling");
+
+  // create the dummy interface
+  can::ThreadedDummyInterfaceSharedPtr dummy = std::make_shared<can::ThreadedDummyInterface>();
+
+  // start the to topic bridge.
+  socketcan_bridge::SocketCANToTopic to_topic_bridge(&nh, &nh_param, dummy);
+  to_topic_bridge.setup();  // initiate the message callbacks
+
+  dummy->init(bus.name, true, can::NoSettings::create());
+
+  // create a frame collector.
+  msgCollector message_collector_;
+
+  // register for messages on received_messages.
+  ros::Subscriber subscriber_ = nh.subscribe("received_messages", 1, &msgCollector::msgCallback, &message_collector_);
+
+  // create a message
+  can::Frame f;
+  f.is_extended = false;
+  f.id = (1<<11)+1;  // this is an illegal CAN packet... should not be sent.
+
+  // send the can::Frame over the driver.
+  // dummy->send(f);
+
+  // give some time for the interface some time to process the message
+  ros::WallDuration(1.0).sleep();
+  ros::spinOnce();
+  EXPECT_EQ(message_collector_.messages.size(), 0);
+
+  f.is_extended = true;
+  f.id = (1<<11)+1;  // now it should be alright.
+
+  dummy->send(f);
+  ros::WallDuration(1.0).sleep();
+  ros::spinOnce();
+  EXPECT_EQ(message_collector_.messages.size(), 1);
+}
+
+TEST(SocketCANToTopicTest, checkCorrectCanIdFilter)
+{
+  ros::NodeHandle nh(""), nh_param("~");
+
+  can::DummyBus bus("checkCorrectCanIdFilter");
+
+  // create the dummy interface
+  can::ThreadedDummyInterfaceSharedPtr dummy = std::make_shared<can::ThreadedDummyInterface>();
+
+  // create can_id vector with id that should be passed and published to ros
+  std::vector<unsigned int> pass_can_ids;
+  pass_can_ids.push_back(0x1337);
+
+  // start the to topic bridge.
+  socketcan_bridge::SocketCANToTopic to_topic_bridge(&nh, &nh_param, dummy);
+  to_topic_bridge.setup(can::tofilters(pass_can_ids));  // initiate the message callbacks
+
+  // init the driver to test stateListener (not checked automatically).
+  dummy->init(bus.name, true, can::NoSettings::create());
+
+  // create a frame collector.
+  msgCollector message_collector_;
+
+  // register for messages on received_messages.
+  ros::Subscriber subscriber_ = nh.subscribe("received_messages", 1, &msgCollector::msgCallback, &message_collector_);
+
+  // create a can frame
+  can::Frame f;
+  f.is_extended = true;
+  f.is_rtr = false;
+  f.is_error = false;
+  f.id = 0x1337;
+  f.dlc = 8;
+  for (uint8_t i=0; i < f.dlc; i++)
+  {
+    f.data[i] = i;
+  }
+
+  // send the can frame to the driver
+  dummy->send(f);
+
+  // give some time for the interface some time to process the message
+  ros::WallDuration(1.0).sleep();
+  ros::spinOnce();
+
+  ASSERT_EQ(1, message_collector_.messages.size());
+
+  // compare the received can_msgs::Frame message to the sent can::Frame.
+  can::Frame received;
+  can_msgs::Frame msg = message_collector_.messages.back();
+  socketcan_bridge::convertMessageToSocketCAN(msg, received);
+
+  EXPECT_EQ(received.id, f.id);
+  EXPECT_EQ(received.dlc, f.dlc);
+  EXPECT_EQ(received.is_extended, f.is_extended);
+  EXPECT_EQ(received.is_rtr, f.is_rtr);
+  EXPECT_EQ(received.is_error, f.is_error);
+  EXPECT_EQ(received.data, f.data);
+}
+
+TEST(SocketCANToTopicTest, checkInvalidCanIdFilter)
+{
+  ros::NodeHandle nh(""), nh_param("~");
+
+  can::DummyBus bus("checkInvalidCanIdFilter");
+
+  // create the dummy interface
+  can::ThreadedDummyInterfaceSharedPtr dummy = std::make_shared<can::ThreadedDummyInterface>();
+
+  // create can_id vector with id that should not be received on can bus
+  std::vector<unsigned int> pass_can_ids;
+  pass_can_ids.push_back(0x300);
+
+  // start the to topic bridge.
+  socketcan_bridge::SocketCANToTopic to_topic_bridge(&nh, &nh_param, dummy);
+  to_topic_bridge.setup(can::tofilters(pass_can_ids));  // initiate the message callbacks
+
+  // init the driver to test stateListener (not checked automatically).
+  dummy->init(bus.name, true, can::NoSettings::create());
+
+  // create a frame collector.
+  msgCollector message_collector_;
+
+  // register for messages on received_messages.
+  ros::Subscriber subscriber_ = nh.subscribe("received_messages", 1, &msgCollector::msgCallback, &message_collector_);
+
+  // create a can frame
+  can::Frame f;
+  f.is_extended = true;
+  f.is_rtr = false;
+  f.is_error = false;
+  f.id = 0x1337;
+  f.dlc = 8;
+  for (uint8_t i=0; i < f.dlc; i++)
+  {
+    f.data[i] = i;
+  }
+
+  // send the can frame to the driver
+  dummy->send(f);
+
+  // give some time for the interface some time to process the message
+  ros::WallDuration(1.0).sleep();
+  ros::spinOnce();
+
+  EXPECT_EQ(0, message_collector_.messages.size());
+}
+
+TEST(SocketCANToTopicTest, checkMaskFilter)
+{
+  ros::NodeHandle nh(""), nh_param("~");
+
+  can::DummyBus bus("checkMaskFilter");
+
+  // create the dummy interface
+  can::ThreadedDummyInterfaceSharedPtr dummy = std::make_shared<can::ThreadedDummyInterface>();
+
+  // setup filter
+  can::FilteredFrameListener::FilterVector filters;
+  filters.push_back(can::tofilter("300:ffe"));
+
+  // start the to topic bridge.
+  socketcan_bridge::SocketCANToTopic to_topic_bridge(&nh, &nh_param, dummy);
+  to_topic_bridge.setup(filters);  // initiate the message callbacks
+
+  // init the driver to test stateListener (not checked automatically).
+  dummy->init(bus.name, true, can::NoSettings::create());
+
+  // create a frame collector.
+  msgCollector message_collector_;
+
+  // register for messages on received_messages.
+  ros::Subscriber subscriber_ = nh.subscribe("received_messages", 10, &msgCollector::msgCallback, &message_collector_);
+
+  const std::string pass1("300#1234"), nopass1("302#9999"), pass2("301#5678");
+
+  // send the can framew to the driver
+  dummy->send(can::toframe(pass1));
+  dummy->send(can::toframe(nopass1));
+  dummy->send(can::toframe(pass2));
+
+  // give some time for the interface some time to process the message
+  ros::WallDuration(1.0).sleep();
+  ros::spinOnce();
+
+  // compare the received can_msgs::Frame message to the sent can::Frame.
+  ASSERT_EQ(2, message_collector_.messages.size());
+  EXPECT_EQ(pass1, convertMessageToString(message_collector_.messages.front()));
+  EXPECT_EQ(pass2, convertMessageToString(message_collector_.messages.back()));
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "test_to_topic");
+  ros::NodeHandle nh;
+  ros::WallDuration(1.0).sleep();
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/vcan0_to_vcan1.launch

@@ -0,0 +1,22 @@
+<launch>
+    <!-- 
+        This launchfile expects two virtual can interfaces, vcan0 and vcan1. One node sends messages received
+        on vcan0 to the ros topic received_messages, another node passes the messages received here to vcan1.
+
+        With the can-utils tools you can then use:
+            cangen vcan0
+
+        to create random frames on vcan0 and:
+            candump vcan0 vcan1
+
+        to show data from both busses to see that the messages end up on vcan1 as well.
+        
+    -->
+    <node pkg="socketcan_bridge" type="socketcan_to_topic_node" name="socketcan_to_topic_node" output="screen">
+        <param name="can_device" value="vcan0" />
+    </node>
+    <node pkg="socketcan_bridge" type="topic_to_socketcan_node" name="topic_to_socketcan_node" output="screen">
+          <param name="can_device" value="vcan1" />
+          <remap from="sent_messages" to="received_messages" />
+    </node>
+</launch>