HiepLM/AMR_T800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit for v2"

Browse Source
This commit is contained in:
HiepLM
2025-12-29 16:21:22 +07:00
commit aa3d832d5c
1807 changed files with 307078 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

128
Devices/Libraries/Ros/ros_canopen/socketcan_bridge/CHANGELOG.rst Executable file
View File

@@ -0,0 +1,128 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package socketcan_bridge
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.8.5 (2020-09-22)
------------------
0.8.4 (2020-08-22)
------------------
* pass settings from ROS node to SocketCANInterface
* Contributors: Mathias Lüdtke
0.8.3 (2020-05-07)
------------------
* add includes to <memory>
* Bump CMake version to avoid CMP0048 warning
Signed-off-by: ahcorde <ahcorde@gmail.com>
* Contributors: Mathias Lüdtke, ahcorde
0.8.2 (2019-11-04)
------------------
* fix roslint errors in socketcan_bridge
* run roslint as part of run_tests
* enable rosconsole_bridge bindings
* Contributors: Mathias Lüdtke
0.8.1 (2019-07-14)
------------------
* Added configurable queue sizes
* Set C++ standard to c++14
* implemented create\*ListenerM helpers
* Replacing FastDelegate with std::function and std::bind.
* Contributors: Harsh Deshpande, JeremyZoss, Joshua Whitley, Mathias Lüdtke, rchristopher
0.8.0 (2018-07-11)
------------------
* keep NodeHandle alive in socketcan_bridge tests
* migrated to std::function and std::bind
* migrated to std pointers
* compare can_msgs::Frame and can::Frame properly
* Contributors: Mathias Lüdtke
0.7.8 (2018-05-04)
------------------
* Revert "pull make_shared into namespaces"
This reverts commit 9b2cd05df76d223647ca81917d289ca6330cdee6.
* Contributors: Mathias Lüdtke
0.7.7 (2018-05-04)
------------------
* pull make_shared into namespaces
* added types for all shared_ptrs
* address catkin_lint errors/warnings
* protect tests from accessing front() or back() of empty list
* added checkMaskFilter for socketcan_bridge
* remove isValid work-around
* added unit test for can id pass filter
* add CAN filter to socketcan_bridge nodes
* Contributors: Benjamin Maidel, Mathias Lüdtke
0.7.6 (2017-08-30)
------------------
0.7.5 (2017-05-29)
------------------
0.7.4 (2017-04-25)
------------------
0.7.3 (2017-04-25)
------------------
0.7.2 (2017-03-28)
------------------
0.7.1 (2017-03-20)
------------------
0.7.0 (2016-12-13)
------------------
0.6.5 (2016-12-10)
------------------
* hamonized versions
* styled and sorted CMakeLists.txt
* removed boilerplate comments
* indention
* reviewed exported dependencies
* styled and sorted package.xml
* Removes gtest from test dependencies.
This dependency is covered by the rosunit dependency.
* Removes dependency on Boost, adds rosunit dependency.
The dependency on Boost was unnecessary, rosunit is required for gtest.
* Improves StateInterface implementation of the DummyInterface.
The doesLoopBack() method now returns the correct value. A state change is
correctly dispatched when the init() method is called.
* Changes the exit code of the nodes if device init fails.
Now exits with 1 if the initialization of the can device fails.
* Changes the frame field for the published messages.
An empty frame name is more commonly used to denote that there is no frame
associated with the message.
* Changes return type of setup() method.
Setup() calls the CreateMsgListener and CreateStateListener, it does not attempt
to verify if the interface is ready, which makes void more applicable.
* Cleanup, fixes and improvements in CmakeLists.
Adds the REQUIRED COMPONENTS packages to the CATKIN_DEPENDS.
Improves add_dependency on the messages to be built.
Removes unnecessary FILES_MATCHING.
Moves the roslint_cpp macro to the testing block.
* Finalizes work on the socketcan_bridge and can_msgs.
Readies the packages socketcan_bridge and can_msgs for the merge with ros_canopen.
Bumps the version for both packages to 0.1.0. Final cleanup in CMakeLists, added
comments to the shell script and launchfile used for testing.
* Adds tests for socketcan_bridge and bugfixes.
Uses rostests and the modified DummyInterface to test whether behaviour
is correct. Prevented possible crashes when can::tostring was called on
invalid frames.
* Adds conversion test between msg and SocketCAN Frame.
This test only covers the conversion between the can_msgs::Frame message and can::Frame from SocketCAN.
* Introduces topic_to_socketcan and the bridge.
Adds TopicToSocketCAN, the counterpart to the SocketCANToTopic class.
Also introduces a node to use this class and a node which combines the two
classes into a bidirectional bridge.
Contrary to the previous commit message the send() method appears to be
available from the ThreadedSocketCANInterface afterall.
* Introduces socketcan_to_topic and its node.
This is based on the ThreadedSocketCANInterface from the socketcan_interface package. Sending might become problematic with this class however, as the send() method is not exposed through the Threading wrappers.
* Contributors: Ivor Wanders, Mathias Lüdtke

144
Devices/Libraries/Ros/ros_canopen/socketcan_bridge/CMakeLists.txt Executable file
View File

@@ -0,0 +1,144 @@
cmake_minimum_required(VERSION 3.0.2)
project(socketcan_bridge)
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_STANDARD 14)
endif()
find_package(catkin REQUIRED
COMPONENTS
can_msgs
rosconsole_bridge
roscpp
socketcan_interface
)
catkin_package(
INCLUDE_DIRS
include
LIBRARIES
socketcan_to_topic
topic_to_socketcan
CATKIN_DEPENDS
can_msgs
rosconsole_bridge
roscpp
socketcan_interface
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
# socketcan_to_topic
add_library(socketcan_to_topic
src/rosconsole_bridge.cpp
src/socketcan_to_topic.cpp
)
target_link_libraries(socketcan_to_topic
${catkin_LIBRARIES}
)
add_dependencies(socketcan_to_topic
${catkin_EXPORTED_TARGETS}
)
# topic_to_socketcan
add_library(topic_to_socketcan
src/rosconsole_bridge.cpp
src/topic_to_socketcan.cpp
)
target_link_libraries(topic_to_socketcan
${catkin_LIBRARIES}
)
add_dependencies(topic_to_socketcan
${catkin_EXPORTED_TARGETS}
)
# socketcan_to_topic_node
add_executable(socketcan_to_topic_node
src/socketcan_to_topic_node.cpp
)
target_link_libraries(socketcan_to_topic_node
socketcan_to_topic
${catkin_LIBRARIES}
)
# topic_to_socketcan_node
add_executable(topic_to_socketcan_node
src/topic_to_socketcan_node.cpp
)
target_link_libraries(topic_to_socketcan_node
topic_to_socketcan
${catkin_LIBRARIES}
)
# socketcan_bridge_node
add_executable(${PROJECT_NAME}_node
src/${PROJECT_NAME}_node.cpp
)
target_link_libraries(${PROJECT_NAME}_node
topic_to_socketcan
socketcan_to_topic
${catkin_LIBRARIES}
)
install(
TARGETS
${PROJECT_NAME}_node
socketcan_to_topic
socketcan_to_topic_node
topic_to_socketcan
topic_to_socketcan_node
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(
DIRECTORY
include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
if(CATKIN_ENABLE_TESTING)
find_package(rostest REQUIRED)
find_package(roslint REQUIRED)
roslint_cpp()
roslint_add_test()
# unit test for the can_msgs::Frame and can::Frame types.
catkin_add_gtest(test_conversion
test/test_conversion.cpp
)
target_link_libraries(test_conversion
topic_to_socketcan
socketcan_to_topic
${catkin_LIBRARIES}
)
add_rostest_gtest(test_to_socketcan
test/to_socketcan.test
test/to_socketcan_test.cpp
)
target_link_libraries(test_to_socketcan
topic_to_socketcan
${catkin_LIBRARIES}
)
add_rostest_gtest(test_to_topic
test/to_topic.test
test/to_topic_test.cpp
)
target_link_libraries(test_to_topic
socketcan_to_topic
topic_to_socketcan
${catkin_LIBRARIES}
)
endif()

76
Devices/Libraries/Ros/ros_canopen/socketcan_bridge/include/socketcan_bridge/socketcan_to_topic.h Executable file
View File

@@ -0,0 +1,76 @@
/*
* 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.
*/
#ifndef SOCKETCAN_BRIDGE_SOCKETCAN_TO_TOPIC_H
#define SOCKETCAN_BRIDGE_SOCKETCAN_TO_TOPIC_H
#include <socketcan_interface/socketcan.h>
#include <socketcan_interface/filter.h>
#include <can_msgs/Frame.h>
#include <ros/ros.h>
namespace socketcan_bridge
{
class SocketCANToTopic
{
public:
SocketCANToTopic(ros::NodeHandle* nh, ros::NodeHandle* nh_param, can::DriverInterfaceSharedPtr driver);
void setup();
void setup(const can::FilteredFrameListener::FilterVector &filters);
void setup(XmlRpc::XmlRpcValue filters);
void setup(ros::NodeHandle nh);
private:
ros::Publisher can_topic_;
can::DriverInterfaceSharedPtr driver_;
can::FrameListenerConstSharedPtr frame_listener_;
can::StateListenerConstSharedPtr state_listener_;
void frameCallback(const can::Frame& f);
void stateCallback(const can::State & s);
};
void convertSocketCANToMessage(const can::Frame& f, can_msgs::Frame& m)
{
m.id = f.id;
m.dlc = f.dlc;
m.is_error = f.is_error;
m.is_rtr = f.is_rtr;
m.is_extended = f.is_extended;
for (int i = 0; i < 8; i++) // always copy all data, regardless of dlc.
{
m.data[i] = f.data[i];
}
};
}; // namespace socketcan_bridge
#endif // SOCKETCAN_BRIDGE_SOCKETCAN_TO_TOPIC_H

70
Devices/Libraries/Ros/ros_canopen/socketcan_bridge/include/socketcan_bridge/topic_to_socketcan.h Executable file
View File

@@ -0,0 +1,70 @@
/*
* 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.
*/
#ifndef SOCKETCAN_BRIDGE_TOPIC_TO_SOCKETCAN_H
#define SOCKETCAN_BRIDGE_TOPIC_TO_SOCKETCAN_H
#include <socketcan_interface/socketcan.h>
#include <can_msgs/Frame.h>
#include <ros/ros.h>
namespace socketcan_bridge
{
class TopicToSocketCAN
{
public:
TopicToSocketCAN(ros::NodeHandle* nh, ros::NodeHandle* nh_param, can::DriverInterfaceSharedPtr driver);
void setup();
private:
ros::Subscriber can_topic_;
can::DriverInterfaceSharedPtr driver_;
can::StateListenerConstSharedPtr state_listener_;
void msgCallback(const can_msgs::Frame::ConstPtr& msg);
void stateCallback(const can::State & s);
};
void convertMessageToSocketCAN(const can_msgs::Frame& m, can::Frame& f)
{
f.id = m.id;
f.dlc = m.dlc;
f.is_error = m.is_error;
f.is_rtr = m.is_rtr;
f.is_extended = m.is_extended;
for (int i = 0; i < 8; i++) // always copy all data, regardless of dlc.
{
f.data[i] = m.data[i];
}
};
}; // namespace socketcan_bridge
#endif // SOCKETCAN_BRIDGE_TOPIC_TO_SOCKETCAN_H

26
Devices/Libraries/Ros/ros_canopen/socketcan_bridge/package.xml Executable file
View File

@@ -0,0 +1,26 @@
<?xml version="1.0"?>
<package format="2">
<name>socketcan_bridge</name>
<version>0.8.5</version>
<description>Conversion nodes for messages from SocketCAN to a ROS Topic and vice versa.</description>
<maintainer email="mathias.luedtke@ipa.fraunhofer.de">Mathias Lüdtke</maintainer>
<author email="ivor@iwanders.net">Ivor Wanders</author>
<license>BSD</license>
<url type="website">http://wiki.ros.org/socketcan_bridge</url>
<url type="repository">https://github.com/ros-industrial/ros_canopen</url>
<url type="bugtracker">https://github.com/ros-industrial/ros_canopen/issues</url>
<buildtool_depend>catkin</buildtool_depend>
<depend>can_msgs</depend>
<depend>rosconsole_bridge</depend>
<depend>roscpp</depend>
<depend>socketcan_interface</depend>
<test_depend>roslint</test_depend>
<test_depend>rostest</test_depend>
<test_depend>rosunit</test_depend>
</package>

29
Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/rosconsole_bridge.cpp Executable file
View File

@@ -0,0 +1,29 @@
/*
* Copyright (c) 2019, Mathias Lüdtke
*
* 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 <rosconsole_bridge/bridge.h>
REGISTER_ROSCONSOLE_BRIDGE;

72
Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/socketcan_bridge_node.cpp Executable file
View File

@@ -0,0 +1,72 @@
/*
* 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 <ros/ros.h>
#include <socketcan_bridge/topic_to_socketcan.h>
#include <socketcan_bridge/socketcan_to_topic.h>
#include <socketcan_interface/threading.h>
#include <socketcan_interface/xmlrpc_settings.h>
#include <memory>
#include <string>
int main(int argc, char *argv[])
{
ros::init(argc, argv, "socketcan_bridge_node");
ros::NodeHandle nh(""), nh_param("~");
std::string can_device;
nh_param.param<std::string>("can_device", can_device, "can0");
can::ThreadedSocketCANInterfaceSharedPtr driver = std::make_shared<can::ThreadedSocketCANInterface> ();
// initialize device at can_device, 0 for no loopback.
if (!driver->init(can_device, 0, XmlRpcSettings::create(nh_param)))
{
ROS_FATAL("Failed to initialize can_device at %s", can_device.c_str());
return 1;
}
else
{
ROS_INFO("Successfully connected to %s.", can_device.c_str());
}
// initialize the bridge both ways.
socketcan_bridge::TopicToSocketCAN to_socketcan_bridge(&nh, &nh_param, driver);
to_socketcan_bridge.setup();
socketcan_bridge::SocketCANToTopic to_topic_bridge(&nh, &nh_param, driver);
to_topic_bridge.setup(nh_param);
ros::spin();
driver->shutdown();
driver.reset();
ros::waitForShutdown();
}

133
Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/socketcan_to_topic.cpp Executable file
View File

@@ -0,0 +1,133 @@
/*
* 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 <socketcan_interface/string.h>
#include <can_msgs/Frame.h>
#include <string>
namespace can
{
template<> can::FrameFilterSharedPtr tofilter(const XmlRpc::XmlRpcValue &ct)
{
XmlRpc::XmlRpcValue t(ct);
try // try to read as integer
{
uint32_t id = static_cast<int>(t);
return tofilter(id);
}
catch(...) // else read as string
{
return tofilter(static_cast<std::string>(t));
}
}
} // namespace can
namespace socketcan_bridge
{
SocketCANToTopic::SocketCANToTopic(ros::NodeHandle* nh, ros::NodeHandle* nh_param,
can::DriverInterfaceSharedPtr driver)
{
can_topic_ = nh->advertise<can_msgs::Frame>("received_messages",
nh_param->param("received_messages_queue_size", 10));
driver_ = driver;
};
void SocketCANToTopic::setup()
{
// register handler for frames and state changes.
frame_listener_ = driver_->createMsgListenerM(this, &SocketCANToTopic::frameCallback);
state_listener_ = driver_->createStateListenerM(this, &SocketCANToTopic::stateCallback);
};
void SocketCANToTopic::setup(const can::FilteredFrameListener::FilterVector &filters)
{
frame_listener_.reset(new can::FilteredFrameListener(driver_,
std::bind(&SocketCANToTopic::frameCallback,
this,
std::placeholders::_1),
filters));
state_listener_ = driver_->createStateListenerM(this, &SocketCANToTopic::stateCallback);
}
void SocketCANToTopic::setup(XmlRpc::XmlRpcValue filters)
{
setup(can::tofilters(filters));
}
void SocketCANToTopic::setup(ros::NodeHandle nh)
{
XmlRpc::XmlRpcValue filters;
if (nh.getParam("can_ids", filters)) return setup(filters);
return setup();
}
void SocketCANToTopic::frameCallback(const can::Frame& f)
{
// ROS_DEBUG("Message came in: %s", can::tostring(f, true).c_str());
if (!f.isValid())
{
ROS_ERROR("Invalid frame from SocketCAN: id: %#04x, length: %d, is_extended: %d, is_error: %d, is_rtr: %d",
f.id, f.dlc, f.is_extended, f.is_error, f.is_rtr);
return;
}
else
{
if (f.is_error)
{
// 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());
}
}
can_msgs::Frame msg;
// converts the can::Frame (socketcan.h) to can_msgs::Frame (ROS msg)
convertSocketCANToMessage(f, msg);
msg.header.frame_id = ""; // empty frame is the de-facto standard for no frame.
msg.header.stamp = ros::Time::now();
can_topic_.publish(msg);
};
void SocketCANToTopic::stateCallback(const can::State & s)
{
std::string err;
driver_->translateError(s.internal_error, err);
if (!s.internal_error)
{
ROS_INFO("State: %s, asio: %s", err.c_str(), s.error_code.message().c_str());
}
else
{
ROS_ERROR("Error: %s, asio: %s", err.c_str(), s.error_code.message().c_str());
}
};
}; // namespace socketcan_bridge

69
Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/socketcan_to_topic_node.cpp Executable file
View File

@@ -0,0 +1,69 @@
/*
* 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 <ros/ros.h>
#include <socketcan_bridge/socketcan_to_topic.h>
#include <socketcan_interface/threading.h>
#include <socketcan_interface/string.h>
#include <socketcan_interface/xmlrpc_settings.h>
#include <memory>
#include <string>
int main(int argc, char *argv[])
{
ros::init(argc, argv, "socketcan_to_topic_node");
ros::NodeHandle nh(""), nh_param("~");
std::string can_device;
nh_param.param<std::string>("can_device", can_device, "can0");
can::ThreadedSocketCANInterfaceSharedPtr driver = std::make_shared<can::ThreadedSocketCANInterface> ();
// initialize device at can_device, 0 for no loopback.
if (!driver->init(can_device, 0, XmlRpcSettings::create(nh_param)))
{
ROS_FATAL("Failed to initialize can_device at %s", can_device.c_str());
return 1;
}
else
{
ROS_INFO("Successfully connected to %s.", can_device.c_str());
}
socketcan_bridge::SocketCANToTopic to_topic_bridge(&nh, &nh_param, driver);
to_topic_bridge.setup(nh_param);
ros::spin();
driver->shutdown();
driver.reset();
ros::waitForShutdown();
}

91
Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/topic_to_socketcan.cpp Executable file
View File

@@ -0,0 +1,91 @@
/*
* 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_interface/string.h>
#include <string>
namespace socketcan_bridge
{
TopicToSocketCAN::TopicToSocketCAN(ros::NodeHandle* nh, ros::NodeHandle* nh_param,
can::DriverInterfaceSharedPtr driver)
{
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;
};
void TopicToSocketCAN::setup()
{
state_listener_ = driver_->createStateListener(
std::bind(&TopicToSocketCAN::stateCallback, this, std::placeholders::_1));
};
void TopicToSocketCAN::msgCallback(const can_msgs::Frame::ConstPtr& msg)
{
// ROS_DEBUG("Message came from sent_messages topic");
// translate it to the socketcan frame type.
can_msgs::Frame m = *msg.get(); // ROS message
can::Frame f; // socketcan type
// converts the can_msgs::Frame (ROS msg) to can::Frame (socketcan.h)
convertMessageToSocketCAN(m, f);
if (!f.isValid()) // check if the id and flags are appropriate.
{
// ROS_WARN("Refusing to send invalid frame: %s.", can::tostring(f, true).c_str());
// 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_ERROR("Invalid frame from topic: id: %#04x, length: %d, is_extended: %d", m.id, m.dlc, m.is_extended);
return;
}
bool res = driver_->send(f);
if (!res)
{
ROS_ERROR("Failed to send message: %s.", can::tostring(f, true).c_str());
}
};
void TopicToSocketCAN::stateCallback(const can::State & s)
{
std::string err;
driver_->translateError(s.internal_error, err);
if (!s.internal_error)
{
ROS_INFO("State: %s, asio: %s", err.c_str(), s.error_code.message().c_str());
}
else
{
ROS_ERROR("Error: %s, asio: %s", err.c_str(), s.error_code.message().c_str());
}
};
}; // namespace socketcan_bridge

69
Devices/Libraries/Ros/ros_canopen/socketcan_bridge/src/topic_to_socketcan_node.cpp Executable file
View File

@@ -0,0 +1,69 @@
/*
* 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 <ros/ros.h>
#include <socketcan_bridge/topic_to_socketcan.h>
#include <socketcan_interface/threading.h>
#include <socketcan_interface/string.h>
#include <socketcan_interface/xmlrpc_settings.h>
#include <memory>
#include <string>
int main(int argc, char *argv[])
{
ros::init(argc, argv, "topic_to_socketcan_node");
ros::NodeHandle nh(""), nh_param("~");
std::string can_device;
nh_param.param<std::string>("can_device", can_device, "can0");
can::ThreadedSocketCANInterfaceSharedPtr driver = std::make_shared<can::ThreadedSocketCANInterface> ();
// initialize device at can_device, 0 for no loopback.
if (!driver->init(can_device, 0, XmlRpcSettings::create(nh_param)))
{
ROS_FATAL("Failed to initialize can_device at %s", can_device.c_str());
return 1;
}
else
{
ROS_INFO("Successfully connected to %s.", can_device.c_str());
}
socketcan_bridge::TopicToSocketCAN to_socketcan_bridge(&nh, &nh_param, driver);
to_socketcan_bridge.setup();
ros::spin();
driver->shutdown();
driver.reset();
ros::waitForShutdown();
}

35
Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/initialize_vcan.sh Executable file
View File

@@ -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

140
Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/test_conversion.cpp Executable file
View File

@@ -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();
}

3
Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/to_socketcan.test Executable file
View File

@@ -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>

194
Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/to_socketcan_test.cpp Executable file
View File

@@ -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();
}

3
Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/to_topic.test Executable file
View File

@@ -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>

325
Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/to_topic_test.cpp Executable file
View File

@@ -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();
}

22
Devices/Libraries/Ros/ros_canopen/socketcan_bridge/test/vcan0_to_vcan1.launch Executable file
View File

@@ -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>