├── canopen_chain_node ├── src │ ├── rosconsole_bridge.cpp │ ├── chain_node.cpp │ └── sync_node.cpp ├── srv │ ├── GetObject.srv │ └── SetObject.srv ├── launch │ └── chain.launch ├── package.xml └── CMakeLists.txt ├── can_msgs ├── msg │ └── Frame.msg ├── CMakeLists.txt ├── package.xml └── CHANGELOG.rst ├── ros_canopen ├── CMakeLists.txt ├── package.xml └── CHANGELOG.rst ├── socketcan_bridge ├── test │ ├── to_topic.test │ ├── to_socketcan.test │ ├── initialize_vcan.sh │ ├── vcan0_to_vcan1.launch │ ├── test_conversion.cpp │ └── to_socketcan_test.cpp ├── package.xml ├── src │ ├── rosconsole_bridge.cpp │ ├── socketcan_to_topic_node.cpp │ ├── topic_to_socketcan_node.cpp │ ├── socketcan_bridge_node.cpp │ ├── topic_to_socketcan.cpp │ └── socketcan_to_topic.cpp ├── include │ └── socketcan_bridge │ │ ├── topic_to_socketcan.h │ │ └── socketcan_to_topic.h ├── CMakeLists.txt └── CHANGELOG.rst ├── socketcan_interface ├── src │ ├── socketcan_interface_plugin.cpp │ ├── canbcm.cpp │ ├── candump.cpp │ └── string.cpp ├── socketcan_interface_plugin.xml ├── include │ └── socketcan_interface │ │ ├── make_shared.h │ │ ├── delegates.h │ │ ├── logging.h │ │ ├── string.h │ │ ├── xmlrpc_settings.h │ │ ├── settings.h │ │ ├── filter.h │ │ ├── threading.h │ │ ├── bcm.h │ │ ├── reader.h │ │ ├── asio_base.h │ │ └── dispatcher.h ├── test │ ├── test_dummy_interface.cpp │ ├── test_string.cpp │ ├── test_dispatcher.cpp │ ├── test_filter.cpp │ ├── test_delegates.cpp │ └── test_settings.cpp ├── package.xml ├── CMakeLists.txt └── CHANGELOG.rst ├── canopen_402 ├── canopen_402_plugin.xml ├── src │ └── plugin.cpp ├── package.xml ├── include │ └── canopen_402 │ │ └── base.h ├── CMakeLists.txt └── test │ └── clamping.cpp ├── .gitignore ├── canopen_master ├── master_plugin.xml ├── include │ └── canopen_master │ │ ├── exceptions.h │ │ ├── timer.h │ │ ├── can_layer.h │ │ └── bcm_sync.h ├── package.xml ├── CMakeLists.txt ├── src │ ├── bcm_sync.cpp │ ├── emcy.cpp │ ├── master_plugin.cpp │ └── node.cpp └── test │ └── test_node.cpp ├── .github └── workflows │ ├── prerelease.yml │ └── main.yml ├── canopen_motor_node ├── src │ ├── canopen_motor_chain_node.cpp │ ├── controller_manager_layer.cpp │ ├── interface_mapping.h │ └── motor_chain.cpp ├── include │ └── canopen_motor_node │ │ ├── motor_chain.h │ │ ├── controller_manager_layer.h │ │ ├── handle_layer_base.h │ │ ├── robot_layer.h │ │ └── unit_converter.h ├── test │ └── test_muparser.cpp ├── package.xml └── CMakeLists.txt ├── README.md ├── .travis.yml └── CONTRIBUTING.md /canopen_chain_node/src/rosconsole_bridge.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | REGISTER_ROSCONSOLE_BRIDGE; 3 | -------------------------------------------------------------------------------- /can_msgs/msg/Frame.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint32 id 3 | bool is_rtr 4 | bool is_extended 5 | bool is_error 6 | uint8 dlc 7 | uint8[8] data -------------------------------------------------------------------------------- /canopen_chain_node/srv/GetObject.srv: -------------------------------------------------------------------------------- 1 | string node 2 | string object 3 | bool cached 4 | --- 5 | bool success 6 | string message 7 | string value 8 | 9 | -------------------------------------------------------------------------------- /canopen_chain_node/srv/SetObject.srv: -------------------------------------------------------------------------------- 1 | string node 2 | string object 3 | string value 4 | bool cached 5 | --- 6 | bool success 7 | string message 8 | 9 | -------------------------------------------------------------------------------- /ros_canopen/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(ros_canopen) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /socketcan_bridge/test/to_topic.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /socketcan_bridge/test/to_socketcan.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /socketcan_interface/src/socketcan_interface_plugin.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | CLASS_LOADER_REGISTER_CLASS(can::SocketCANInterface, can::DriverInterface); 5 | -------------------------------------------------------------------------------- /canopen_402/canopen_402_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Allocator for Motor402 instances 4 | 5 | -------------------------------------------------------------------------------- /socketcan_interface/socketcan_interface_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | SocketCAN inteface plugin. 4 | 5 | -------------------------------------------------------------------------------- /canopen_chain_node/launch/chain.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /socketcan_interface/include/socketcan_interface/make_shared.h: -------------------------------------------------------------------------------- 1 | #ifndef SOCKETCAN_INTERFACE_MAKE_SHARED_H 2 | #define SOCKETCAN_INTERFACE_MAKE_SHARED_H 3 | 4 | #include 5 | #define ROSCANOPEN_MAKE_SHARED std::make_shared 6 | 7 | #endif // ! SOCKETCAN_INTERFACE_MAKE_SHARED_H 8 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # temporary files 2 | *~ 3 | 4 | # autogenerated folders 5 | bin 6 | build 7 | lib 8 | __pycache__ 9 | msg_gen 10 | srv_gen 11 | 12 | # autogenerated msgs 13 | *Action.msg 14 | *Action.py 15 | *ActionFeedback.msg 16 | *ActionFeedback.py 17 | *ActionGoal.msg 18 | *ActionGoal.py 19 | *Feedback.msg 20 | *Feedback.py 21 | *Goal.msg 22 | *Goal.py 23 | *Result.msg 24 | *Result.py 25 | *.pyc 26 | -------------------------------------------------------------------------------- /can_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(can_msgs) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | message_generation 7 | std_msgs 8 | ) 9 | 10 | add_message_files(DIRECTORY msg 11 | FILES 12 | Frame.msg 13 | ) 14 | 15 | generate_messages( 16 | DEPENDENCIES 17 | std_msgs 18 | ) 19 | 20 | catkin_package( 21 | CATKIN_DEPENDS 22 | message_runtime 23 | std_msgs 24 | ) 25 | -------------------------------------------------------------------------------- /canopen_402/src/plugin.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | canopen::MotorBaseSharedPtr canopen::Motor402::Allocator::allocate(const std::string &name, canopen::ObjectStorageSharedPtr storage, const canopen::Settings &settings) { 5 | return std::make_shared(name, storage, settings); 6 | } 7 | 8 | CLASS_LOADER_REGISTER_CLASS(canopen::Motor402::Allocator, canopen::MotorBase::Allocator); 9 | -------------------------------------------------------------------------------- /canopen_master/master_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Allocator for SimpleMaster instances 4 | 5 | 6 | Allocator for ExternalMaster instances 7 | 8 | 9 | -------------------------------------------------------------------------------- /.github/workflows/prerelease.yml: -------------------------------------------------------------------------------- 1 | name: Pre-release 2 | 3 | on: [workflow_dispatch] 4 | 5 | jobs: 6 | default: 7 | strategy: 8 | matrix: 9 | distro: [melodic, noetic] 10 | 11 | env: 12 | BUILDER: catkin_make_isolated 13 | ROS_DISTRO: ${{ matrix.distro }} 14 | PRERELEASE: true 15 | 16 | name: "${{ matrix.distro }}" 17 | runs-on: ubuntu-latest 18 | steps: 19 | - uses: actions/checkout@v2 20 | - uses: ros-industrial/industrial_ci@master 21 | -------------------------------------------------------------------------------- /canopen_motor_node/src/canopen_motor_chain_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | using namespace canopen; 4 | 5 | 6 | int main(int argc, char** argv){ 7 | ros::init(argc, argv, "canopen_motor_chain_node"); 8 | ros::AsyncSpinner spinner(0); 9 | spinner.start(); 10 | 11 | ros::NodeHandle nh; 12 | ros::NodeHandle nh_priv("~"); 13 | 14 | MotorChain chain(nh, nh_priv); 15 | 16 | if(!chain.setup()){ 17 | return 1; 18 | } 19 | 20 | ros::waitForShutdown(); 21 | return 0; 22 | } 23 | -------------------------------------------------------------------------------- /canopen_chain_node/src/chain_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | using namespace can; 6 | using namespace canopen; 7 | 8 | int main(int argc, char** argv){ 9 | ros::init(argc, argv, "canopen_chain_node_node"); 10 | ros::NodeHandle nh; 11 | ros::NodeHandle nh_priv("~"); 12 | 13 | RosChain chain(nh, nh_priv); 14 | 15 | if(!chain.setup()){ 16 | return 1; 17 | } 18 | 19 | ros::spin(); 20 | return 0; 21 | } 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ros_canopen 2 | =========== 3 | 4 | Canopen implementation for ROS. 5 | 6 | [![Build Status](https://travis-ci.com/ros-industrial/ros_canopen.svg?branch=melodic-devel)](https://travis-ci.com/ros-industrial/ros_canopen) 7 | [![License: LGPL v3](https://img.shields.io/badge/License-LGPL%20v3-blue.svg)](https://www.gnu.org/licenses/lgpl-3.0) 8 | ([![License: BSD 3-Clause](https://img.shields.io/badge/License-BSD%203--Clause-blue.svg)](https://opensource.org/licenses/BSD-3-Clause) for `can_msgs` and `socketcan_bridge`) 9 | 10 | The current develop branch is `melodic-devel`, it targets ROS `melodic`. Needs C++14 compiler. 11 | The released version gets synced over to the distro branch for each release. 12 | -------------------------------------------------------------------------------- /socketcan_interface/include/socketcan_interface/delegates.h: -------------------------------------------------------------------------------- 1 | #ifndef SOCKETCAN_INTERFACE_DELEGATES_H_ 2 | #define SOCKETCAN_INTERFACE_DELEGATES_H_ 3 | 4 | #include 5 | 6 | namespace can 7 | { 8 | 9 | template class DelegateHelper : public T { 10 | public: 11 | template 12 | DelegateHelper(Object &&o, typename T::result_type (Instance::*member)(Args... args)) : 13 | T([o, member](Args... args) -> typename T::result_type { return ((*o).*member)(args...); }) 14 | { 15 | } 16 | template 17 | DelegateHelper(Callable &&c) : T(c) 18 | { 19 | } 20 | }; 21 | 22 | } // namespace can 23 | 24 | #endif // SOCKETCAN_INTERFACE_DELEGATES_H_ 25 | -------------------------------------------------------------------------------- /canopen_master/include/canopen_master/exceptions.h: -------------------------------------------------------------------------------- 1 | #ifndef H_EXCEPTIONS 2 | #define H_EXCEPTIONS 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | namespace canopen{ 9 | 10 | class Exception : public std::runtime_error { 11 | public: 12 | Exception(const std::string &w) : std::runtime_error(w) {} 13 | }; 14 | 15 | class PointerInvalid : public Exception{ 16 | public: 17 | PointerInvalid(const std::string &w) : Exception("Pointer invalid") {} 18 | }; 19 | 20 | class ParseException : public Exception{ 21 | public: 22 | ParseException(const std::string &w) : Exception(w) {} 23 | }; 24 | 25 | class TimeoutException : public Exception{ 26 | public: 27 | TimeoutException(const std::string &w) : Exception(w) {} 28 | }; 29 | 30 | 31 | } // canopen 32 | 33 | #endif // !H_EXCEPTIONS 34 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: generic 2 | services: 3 | - docker 4 | 5 | cache: 6 | directories: 7 | - $HOME/.ccache 8 | 9 | env: 10 | global: 11 | - CCACHE_DIR=$HOME/.ccache 12 | matrix: 13 | - ROS_DISTRO="melodic" ROS_REPO=ros-shadow-fixed 14 | - ROS_DISTRO="melodic" ROS_REPO=ros CATKIN_LINT=pedantic 15 | - ROS_DISTRO="melodic" ROSDEP_SKIP_KEYS=muparser EXPECT_EXIT_CODE=1 16 | - ROS_DISTRO="melodic" ABICHECK_URL='github:ros-industrial/ros_canopen#melodic' 17 | - ROS_DISTRO="noetic" 18 | - ROS_DISTRO="noetic" ROS_REPO=ros OS_NAME=debian OS_CODE_NAME=buster 19 | 20 | matrix: 21 | allow_failures: 22 | - env: ROS_DISTRO="melodic" ABICHECK_URL='github:ros-industrial/ros_canopen#melodic' 23 | 24 | install: 25 | - git clone --quiet --depth=1 https://github.com/ros-industrial/industrial_ci.git .industrial_ci -b master 26 | script: 27 | - .industrial_ci/travis.sh 28 | -------------------------------------------------------------------------------- /can_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | . 3 | can_msgs 4 | 0.8.5 5 | CAN related message types. 6 | 7 | Mathias Lüdtke 8 | Ivor Wanders 9 | 10 | BSD 11 | 12 | http://wiki.ros.org/can_msgs 13 | https://github.com/ros-industrial/ros_canopen 14 | https://github.com/ros-industrial/ros_canopen/issues 15 | 16 | catkin 17 | 18 | std_msgs 19 | 20 | message_generation 21 | message_runtime 22 | message_runtime 23 | 24 | 25 | -------------------------------------------------------------------------------- /socketcan_bridge/test/initialize_vcan.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # sets up two virtual can interfaces, vcan0 and vcan1 4 | 5 | lsmod | grep -q "vcan" 6 | VCAN_NOT_LOADED=$? 7 | 8 | if [ $VCAN_NOT_LOADED -eq 1 ]; then 9 | echo "vcan kernel module is not available..." 10 | echo "loading it;" 11 | sudo modprobe -a vcan 12 | fi 13 | 14 | ifconfig vcan0 > /dev/null 15 | VCAN_NOT_EXIST=$? 16 | 17 | if [ $VCAN_NOT_EXIST -eq 1 ]; then 18 | echo "vcan0 does not exist, creating it." 19 | sudo ip link add dev vcan0 type vcan 20 | sudo ip link set vcan0 up 21 | else 22 | echo "vcan0 already exists." 23 | fi 24 | 25 | 26 | ifconfig vcan1 > /dev/null 27 | VCAN_NOT_EXIST=$? 28 | if [ $VCAN_NOT_EXIST -eq 1 ]; then 29 | echo "vcan0 does not exist, creating it." 30 | sudo ip link add dev vcan1 type vcan 31 | sudo ip link set vcan1 up 32 | else 33 | echo "vcan0 already exists." 34 | fi 35 | 36 | -------------------------------------------------------------------------------- /socketcan_bridge/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | socketcan_bridge 4 | 0.8.5 5 | Conversion nodes for messages from SocketCAN to a ROS Topic and vice versa. 6 | 7 | Mathias Lüdtke 8 | Ivor Wanders 9 | 10 | BSD 11 | 12 | http://wiki.ros.org/socketcan_bridge 13 | https://github.com/ros-industrial/ros_canopen 14 | https://github.com/ros-industrial/ros_canopen/issues 15 | 16 | catkin 17 | 18 | can_msgs 19 | rosconsole_bridge 20 | roscpp 21 | socketcan_interface 22 | 23 | roslint 24 | rostest 25 | rosunit 26 | 27 | -------------------------------------------------------------------------------- /socketcan_bridge/test/vcan0_to_vcan1.launch: -------------------------------------------------------------------------------- 1 | 2 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /canopen_motor_node/include/canopen_motor_node/motor_chain.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef CANOPEN_MOTOR_NODE_MOTOR_CHAIN_H_ 3 | #define CANOPEN_MOTOR_NODE_MOTOR_CHAIN_H_ 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | 14 | namespace canopen { 15 | 16 | class MotorChain : public canopen::RosChain { 17 | ClassAllocator motor_allocator_; 18 | std::shared_ptr< canopen::LayerGroupNoDiag > motors_; 19 | RobotLayerSharedPtr robot_layer_; 20 | 21 | std::shared_ptr cm_; 22 | 23 | virtual bool nodeAdded(XmlRpc::XmlRpcValue ¶ms, const canopen::NodeSharedPtr &node, const LoggerSharedPtr &logger); 24 | 25 | public: 26 | MotorChain(const ros::NodeHandle &nh, const ros::NodeHandle &nh_priv); 27 | 28 | virtual bool setup_chain(); 29 | }; 30 | 31 | } // namespace canopen 32 | 33 | #endif /* INCLUDE_CANOPEN_MOTOR_NODE_MOTOR_CHAIN_H_ */ 34 | -------------------------------------------------------------------------------- /canopen_402/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | canopen_402 4 | 0.8.5 5 | This implements the CANopen device profile for drives and motion control. CiA(r) 402 6 | 7 | Mathias Lüdtke 8 | Thiago de Freitas 9 | Mathias Lüdtke 10 | 11 | LGPLv3 12 | 13 | http://wiki.ros.org/canopen_402 14 | https://github.com/ros-industrial/ros_canopen 15 | https://github.com/ros-industrial/ros_canopen/issues 16 | 17 | catkin 18 | 19 | libboost-dev 20 | canopen_master 21 | class_loader 22 | 23 | rosunit 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /canopen_motor_node/test/test_muparser.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | using namespace canopen; 7 | 8 | double * mapVariable(const std::string &, double *p) { 9 | return p; 10 | } 11 | 12 | TEST(TestMuparser, CheckNorm){ 13 | double input = 0; 14 | UnitConverter uc("norm(in,-1000,1000)", std::bind(HandleLayer::assignVariable, std::placeholders::_1, &input, "in")); 15 | input = 0; EXPECT_EQ(0, uc.evaluate()); 16 | input = 10; EXPECT_EQ(10, uc.evaluate()); 17 | input = -10; EXPECT_EQ(-10, uc.evaluate()); 18 | input = 1000; EXPECT_EQ(-1000, uc.evaluate()); 19 | input = 1001; EXPECT_EQ(-999, uc.evaluate()); 20 | input = 2000; EXPECT_EQ(0, uc.evaluate()); 21 | input = 2001; EXPECT_EQ(1, uc.evaluate()); 22 | input = -1000; EXPECT_EQ(-1000, uc.evaluate()); 23 | input = 999; EXPECT_EQ(999, uc.evaluate()); 24 | } 25 | 26 | int main(int argc, char **argv){ 27 | testing::InitGoogleTest(&argc, argv); 28 | return RUN_ALL_TESTS(); 29 | } 30 | -------------------------------------------------------------------------------- /socketcan_interface/src/canbcm.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using namespace can; 5 | 6 | #include 7 | 8 | int main(int argc, char *argv[]){ 9 | BCMsocket bcm; 10 | 11 | int extra_frames = argc - 4; 12 | 13 | if(extra_frames < 0){ 14 | std::cout << "usage: "<< argv[0] << " DEVICE PERIOD HEADER#DATA [DATA*]" << std::endl; 15 | return 1; 16 | } 17 | 18 | if(!bcm.init(argv[1])){ 19 | return 2; 20 | } 21 | 22 | int num_frames = extra_frames+1; 23 | Frame *frames = new Frame[num_frames]; 24 | Header header = frames[0] = toframe(argv[3]); 25 | 26 | if(extra_frames > 0){ 27 | for(int i=0; i< extra_frames; ++i){ 28 | frames[1+i] = toframe(tostring(header,true)+"#"+argv[4+i]); 29 | } 30 | } 31 | for(int i = 0; i < num_frames; ++i){ 32 | std::cout << frames[i] << std::endl; 33 | } 34 | if(bcm.startTX(boost::chrono::duration(atof(argv[2])), header, num_frames, frames)){ 35 | pause(); 36 | return 0; 37 | } 38 | return 4; 39 | } -------------------------------------------------------------------------------- /ros_canopen/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ros_canopen 4 | 0.8.5 5 | A generic canopen implementation for ROS 6 | 7 | LGPL 8 | 9 | http://ros.org/wiki/ros_canopen 10 | https://github.com/ros-industrial/ros_canopen 11 | https://github.com/ros-industrial/ros_canopen/issues 12 | 13 | Mathias Lüdtke 14 | Florian Weisshardt 15 | 16 | catkin 17 | 18 | can_msgs 19 | canopen_402 20 | canopen_chain_node 21 | canopen_master 22 | canopen_motor_node 23 | 24 | socketcan_bridge 25 | socketcan_interface 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /canopen_master/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | canopen_master 4 | 0.8.5 5 | CiA(r) CANopen 301 master implementation with support for interprocess master synchronisation. 6 | 7 | Mathias Lüdtke 8 | Mathias Lüdtke 9 | 10 | LGPLv3 11 | 12 | http://wiki.ros.org/canopen_master 13 | https://github.com/ros-industrial/ros_canopen 14 | https://github.com/ros-industrial/ros_canopen/issues 15 | 16 | catkin 17 | 18 | libboost-dev 19 | libboost-atomic-dev 20 | libboost-chrono-dev 21 | libboost-thread-dev 22 | class_loader 23 | socketcan_interface 24 | 25 | rosunit 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /socketcan_interface/test/test_dummy_interface.cpp: -------------------------------------------------------------------------------- 1 | // Bring in my package's API, which is what I'm testing 2 | #include 3 | #include 4 | 5 | // Bring in gtest 6 | #include 7 | 8 | // Declare a test 9 | TEST(DummyInterfaceTest, testCase1) 10 | { 11 | can::DummyBus bus("testCase1"); 12 | can::ThreadedDummyInterface dummy; 13 | dummy.init(bus.name, true, can::NoSettings::create()); 14 | 15 | can::DummyReplay replay; 16 | replay.add("0#8200", {"701#00", "701#04"}); 17 | replay.init(bus); 18 | 19 | std::list expected{"0#8200", "701#00", "701#04"}; 20 | std::list responses; 21 | 22 | auto listener = dummy.createMsgListener([&responses](auto& f) { 23 | responses.push_back(can::tostring(f, true)); 24 | }); 25 | 26 | EXPECT_FALSE(replay.done()); 27 | 28 | dummy.send(can::toframe("0#8200")); 29 | 30 | replay.flush(); 31 | dummy.flush(); 32 | 33 | EXPECT_EQ(expected, responses); 34 | EXPECT_TRUE(replay.done()); 35 | } 36 | 37 | 38 | // Run all the tests that were declared with TEST() 39 | int main(int argc, char **argv){ 40 | testing::InitGoogleTest(&argc, argv); 41 | return RUN_ALL_TESTS(); 42 | } 43 | -------------------------------------------------------------------------------- /socketcan_interface/include/socketcan_interface/logging.h: -------------------------------------------------------------------------------- 1 | #ifndef SOCKETCAN_INTERFACE_LOGGING_H 2 | #define SOCKETCAN_INTERFACE_LOGGING_H 3 | 4 | #include 5 | #include 6 | 7 | #define ROSCANOPEN_LOG(name, file, line, level, args) { std::stringstream sstr; sstr << name << ": " << args; console_bridge::getOutputHandler()->log(sstr.str(), level, file, line); } 8 | 9 | #define ROSCANOPEN_ERROR(name, args) ROSCANOPEN_LOG(name, __FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_ERROR, args) 10 | #define ROSCANOPEN_INFO(name, args) ROSCANOPEN_LOG(name, __FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_INFO, args) 11 | #define ROSCANOPEN_WARN(name, args) ROSCANOPEN_LOG(name, __FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_WARN, args) 12 | #define ROSCANOPEN_DEBUG(name, args) ROSCANOPEN_LOG(name, __FILE__, __LINE__,console_bridge::CONSOLE_BRIDGE_LOG_DEBUG, args) 13 | 14 | // extra function to mark it as deprecated 15 | inline __attribute__ ((deprecated("please use ROSCANOPEN_* macros"))) void roscanopen_log_deprecated(const std::string s, const char* f, int l) { console_bridge::getOutputHandler()->log(s, console_bridge::CONSOLE_BRIDGE_LOG_ERROR, f, l); } 16 | #define LOG(args) { std::stringstream sstr; sstr << "LOG: " << args; roscanopen_log_deprecated(sstr.str(), __FILE__, __LINE__); } 17 | #endif 18 | -------------------------------------------------------------------------------- /socketcan_interface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | socketcan_interface 4 | 0.8.5 5 | Generic CAN interface description with helpers for filtering and driver implementation. Further a socketcan implementation based on boost::asio is included. 6 | 7 | Mathias Lüdtke 8 | Mathias Lüdtke 9 | 10 | LGPLv3 11 | 12 | http://wiki.ros.org/socketcan_interface 13 | https://github.com/ros-industrial/ros_canopen 14 | https://github.com/ros-industrial/ros_canopen/issues 15 | 16 | catkin 17 | 18 | libboost-dev 19 | libboost-chrono-dev 20 | libboost-system-dev 21 | libboost-thread-dev 22 | class_loader 23 | libconsole-bridge-dev 24 | linux-kernel-headers 25 | 26 | rosunit 27 | xmlrpcpp 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /canopen_402/include/canopen_402/base.h: -------------------------------------------------------------------------------- 1 | #ifndef CANOPEN_402_BASE_H 2 | #define CANOPEN_402_BASE_H 3 | 4 | #include 5 | 6 | namespace canopen 7 | { 8 | 9 | class MotorBase : public canopen::Layer { 10 | protected: 11 | MotorBase(const std::string &name) : Layer(name) {} 12 | public: 13 | enum OperationMode 14 | { 15 | No_Mode = 0, 16 | Profiled_Position = 1, 17 | Velocity = 2, 18 | Profiled_Velocity = 3, 19 | Profiled_Torque = 4, 20 | Reserved = 5, 21 | Homing = 6, 22 | Interpolated_Position = 7, 23 | Cyclic_Synchronous_Position = 8, 24 | Cyclic_Synchronous_Velocity = 9, 25 | Cyclic_Synchronous_Torque = 10, 26 | }; 27 | virtual bool setTarget(double val) = 0; 28 | virtual bool enterModeAndWait(uint16_t mode) = 0; 29 | virtual bool isModeSupported(uint16_t mode) = 0; 30 | virtual uint16_t getMode() = 0; 31 | virtual void registerDefaultModes(ObjectStorageSharedPtr storage) {} 32 | 33 | typedef std::shared_ptr MotorBaseSharedPtr; 34 | 35 | class Allocator { 36 | public: 37 | virtual MotorBaseSharedPtr allocate(const std::string &name, ObjectStorageSharedPtr storage, const canopen::Settings &settings) = 0; 38 | virtual ~Allocator() {} 39 | }; 40 | }; 41 | typedef MotorBase::MotorBaseSharedPtr MotorBaseSharedPtr; 42 | 43 | } 44 | 45 | #endif 46 | -------------------------------------------------------------------------------- /canopen_motor_node/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | canopen_motor_node 4 | 0.8.5 5 | canopen_chain_node specialization for handling of canopen_402 motor devices. It facilitates interface abstraction with ros_control. 6 | 7 | Mathias Lüdtke 8 | Mathias Lüdtke 9 | 10 | LGPLv3 11 | 12 | http://wiki.ros.org/canopen_motor_node 13 | https://github.com/ros-industrial/ros_canopen 14 | https://github.com/ros-industrial/ros_canopen/issues 15 | 16 | catkin 17 | 18 | libboost-dev 19 | libboost-thread-dev 20 | canopen_402 21 | canopen_chain_node 22 | canopen_master 23 | controller_manager 24 | filters 25 | hardware_interface 26 | joint_limits_interface 27 | muparser 28 | roscpp 29 | urdf 30 | 31 | controller_manager_msgs 32 | controller_manager_msgs 33 | 34 | rosunit 35 | 36 | 37 | -------------------------------------------------------------------------------- /.github/workflows/main.yml: -------------------------------------------------------------------------------- 1 | # This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). 2 | # For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) 3 | 4 | name: CI 5 | 6 | on: 7 | push: 8 | pull_request: 9 | schedule: 10 | - cron: '0 4 * * *' # every day at 4 AM (UTC) 11 | workflow_dispatch: # allow manually starting this workflow 12 | 13 | jobs: 14 | industrial_ci: 15 | name: ROS ${{ matrix.ROS_DISTRO }} (${{ matrix.ROS_REPO }}) 16 | runs-on: ubuntu-latest 17 | strategy: 18 | matrix: # matrix is the product of entries 19 | ROS_DISTRO: [melodic, noetic] 20 | ROS_REPO: [testing, main] 21 | env: 22 | CCACHE_DIR: "${{ github.workspace }}/.ccache" # directory for ccache (and how we enable ccache in industrial_ci) 23 | steps: 24 | - uses: actions/checkout@v2 # clone target repository 25 | - uses: actions/cache@v4 # fetch/store the directory used by ccache before/after the ci run 26 | with: 27 | path: ${{ env.CCACHE_DIR }} 28 | key: ccache-${{ matrix.ROS_DISTRO }}-${{ matrix.ROS_REPO }}-${{github.run_id}} 29 | restore-keys: | 30 | ccache-${{ matrix.ROS_DISTRO }}-${{ matrix.ROS_REPO }}- 31 | - uses: 'ros-industrial/industrial_ci@master' # run industrial_ci 32 | env: 33 | ROS_DISTRO: ${{ matrix.ROS_DISTRO }} 34 | ROS_REPO: ${{ matrix.ROS_REPO }} 35 | -------------------------------------------------------------------------------- /canopen_chain_node/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | canopen_chain_node 4 | 0.8.5 5 | Base implementation for CANopen chains node with support for management services and diagnostics 6 | 7 | Mathias Lüdtke 8 | Mathias Lüdtke 9 | 10 | LGPLv3 11 | 12 | http://wiki.ros.org/canopen_chain_node 13 | https://github.com/ros-industrial/ros_canopen 14 | https://github.com/ros-industrial/ros_canopen/issues 15 | 16 | catkin 17 | 18 | message_generation 19 | message_runtime 20 | message_runtime 21 | 22 | libboost-dev 23 | libboost-filesystem-dev 24 | canopen_master 25 | diagnostic_updater 26 | pluginlib 27 | rosconsole_bridge 28 | roscpp 29 | roslib 30 | socketcan_interface 31 | std_srvs 32 | 33 | std_msgs 34 | std_msgs 35 | 36 | 37 | -------------------------------------------------------------------------------- /socketcan_interface/include/socketcan_interface/string.h: -------------------------------------------------------------------------------- 1 | #ifndef SOCKETCAN_INTERFACE_STRING_H 2 | #define SOCKETCAN_INTERFACE_STRING_H 3 | 4 | #include "interface.h" 5 | #include "filter.h" 6 | #include 7 | 8 | namespace can { 9 | 10 | bool hex2dec(uint8_t& d, const char& h); 11 | 12 | bool hex2buffer(std::string& out, const std::string& in_raw, bool pad); 13 | 14 | bool dec2hex(char& h, const uint8_t& d, bool lc); 15 | 16 | std::string byte2hex(const uint8_t& d, bool pad, bool lc); 17 | 18 | 19 | std::string buffer2hex(const std::string& in, bool lc); 20 | 21 | std::string tostring(const Header& h, bool lc); 22 | 23 | Header toheader(const std::string& s); 24 | 25 | std::string tostring(const Frame& f, bool lc); 26 | 27 | Frame toframe(const std::string& s); 28 | 29 | template FrameFilterSharedPtr tofilter(const T &ct); 30 | template<> FrameFilterSharedPtr tofilter(const std::string &s); 31 | template<> FrameFilterSharedPtr tofilter(const uint32_t &id); 32 | 33 | FrameFilterSharedPtr tofilter(const char* s); 34 | 35 | template FilteredFrameListener::FilterVector tofilters(const T& v) { 36 | FilteredFrameListener::FilterVector filters; 37 | for(size_t i = 0; i < static_cast(v.size()); ++i){ 38 | filters.push_back(tofilter(v[i])); 39 | } 40 | return filters; 41 | } 42 | 43 | std::ostream& operator <<(std::ostream& stream, const Header& h); 44 | std::ostream& operator <<(std::ostream& stream, const Frame& f); 45 | 46 | } 47 | #endif 48 | -------------------------------------------------------------------------------- /ros_canopen/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package ros_canopen 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.8.5 (2020-09-22) 6 | ------------------ 7 | 8 | 0.8.4 (2020-08-22) 9 | ------------------ 10 | 11 | 0.8.3 (2020-05-07) 12 | ------------------ 13 | * Bump CMake version to avoid CMP0048 warning 14 | Signed-off-by: ahcorde 15 | * Contributors: ahcorde 16 | 17 | 0.8.2 (2019-11-04) 18 | ------------------ 19 | 20 | 0.8.1 (2019-07-14) 21 | ------------------ 22 | 23 | 0.8.0 (2018-07-11) 24 | ------------------ 25 | 26 | 0.7.8 (2018-05-04) 27 | ------------------ 28 | 29 | 0.7.7 (2018-05-04) 30 | ------------------ 31 | 32 | 0.7.6 (2017-08-30) 33 | ------------------ 34 | 35 | 0.7.5 (2017-05-29) 36 | ------------------ 37 | 38 | 0.7.4 (2017-04-25) 39 | ------------------ 40 | 41 | 0.7.3 (2017-04-25) 42 | ------------------ 43 | 44 | 0.7.2 (2017-03-28) 45 | ------------------ 46 | 47 | 0.7.1 (2017-03-20) 48 | ------------------ 49 | 50 | 0.7.0 (2016-12-13) 51 | ------------------ 52 | 53 | 0.6.5 (2016-12-10) 54 | ------------------ 55 | * updated metapackage 56 | * format 2 57 | * updated maintaner 58 | * added new packages 59 | * update package URLs 60 | * Contributors: Mathias Lüdtke 61 | 62 | 0.6.4 (2015-07-03) 63 | ------------------ 64 | 65 | 0.6.3 (2015-06-30) 66 | ------------------ 67 | 68 | 0.6.2 (2014-12-18) 69 | ------------------ 70 | * remove canopen_test_utils from metapackage 71 | * Contributors: Florian Weisshardt 72 | 73 | 0.6.1 (2014-12-15) 74 | ------------------ 75 | * add metapackage 76 | * Contributors: Florian Weisshardt 77 | -------------------------------------------------------------------------------- /canopen_402/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(canopen_402) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | canopen_master 7 | class_loader 8 | ) 9 | 10 | find_package(Boost REQUIRED 11 | ) 12 | 13 | catkin_package( 14 | INCLUDE_DIRS 15 | include 16 | LIBRARIES 17 | ${PROJECT_NAME} 18 | CATKIN_DEPENDS 19 | canopen_master 20 | DEPENDS 21 | Boost 22 | ) 23 | 24 | include_directories( 25 | include 26 | ${Boost_INCLUDE_DIRS} 27 | ${catkin_INCLUDE_DIRS} 28 | ) 29 | 30 | # canopen_402 31 | add_library(${PROJECT_NAME} 32 | src/motor.cpp 33 | ) 34 | target_link_libraries(${PROJECT_NAME} 35 | ${catkin_LIBRARIES} 36 | ) 37 | 38 | # canopen_402_plugin 39 | add_library(${PROJECT_NAME}_plugin 40 | src/plugin.cpp 41 | ) 42 | target_link_libraries(${PROJECT_NAME}_plugin 43 | ${catkin_LIBRARIES} 44 | ${PROJECT_NAME} 45 | ) 46 | 47 | install( 48 | TARGETS 49 | ${PROJECT_NAME} 50 | ${PROJECT_NAME}_plugin 51 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 52 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 53 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 54 | ) 55 | 56 | install(DIRECTORY include/${PROJECT_NAME}/ 57 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 58 | FILES_MATCHING PATTERN "*.h" 59 | ) 60 | install( 61 | FILES 62 | ${PROJECT_NAME}_plugin.xml 63 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 64 | ) 65 | 66 | if(CATKIN_ENABLE_TESTING) 67 | catkin_add_gtest(${PROJECT_NAME}-test_clamping 68 | test/clamping.cpp 69 | ) 70 | target_link_libraries(${PROJECT_NAME}-test_clamping 71 | ${catkin_LIBRARIES} 72 | ) 73 | endif() 74 | -------------------------------------------------------------------------------- /socketcan_interface/include/socketcan_interface/xmlrpc_settings.h: -------------------------------------------------------------------------------- 1 | #ifndef SOCKETCAN_INTERFACE_XMLRPC_SETTINGS_H 2 | #define SOCKETCAN_INTERFACE_XMLRPC_SETTINGS_H 3 | #include 4 | 5 | #include 6 | #include "xmlrpcpp/XmlRpcValue.h" 7 | #include 8 | #include 9 | 10 | class XmlRpcSettings : public can::Settings { 11 | public: 12 | XmlRpcSettings() {} 13 | XmlRpcSettings(const XmlRpc::XmlRpcValue &v) : value_(v) {} 14 | XmlRpcSettings& operator=(const XmlRpc::XmlRpcValue &v) { value_ = v; return *this; } 15 | template static can::SettingsConstSharedPtr create(T nh, const std::string &ns="/") { 16 | std::shared_ptr settings(new XmlRpcSettings); 17 | nh.getParam(ns, settings->value_); 18 | return settings; 19 | } 20 | private: 21 | virtual bool getRepr(const std::string &name, std::string & repr) const { 22 | const XmlRpc::XmlRpcValue *value = &value_; 23 | 24 | std::string n = name; 25 | size_t delim_pos; 26 | while (value->getType() == XmlRpc::XmlRpcValue::TypeStruct && (delim_pos = n.find('/')) != std::string::npos){ 27 | std::string segment = n.substr(0, delim_pos); 28 | if (!value->hasMember(segment)) return false; 29 | value = &((*value)[segment]); 30 | n.erase(0, delim_pos+1); 31 | } 32 | if(value->hasMember(n)){ 33 | std::stringstream sstr; 34 | sstr << (*value)[n]; 35 | repr = sstr.str(); 36 | return true; 37 | } 38 | return false; 39 | } 40 | XmlRpc::XmlRpcValue value_; 41 | 42 | }; 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /canopen_motor_node/include/canopen_motor_node/controller_manager_layer.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef CANOPEN_MOTOR_NODE_CONTROLLER_MANAGER_LAYER_H_ 3 | #define CANOPEN_MOTOR_NODE_CONTROLLER_MANAGER_LAYER_H_ 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | // forward declarations 13 | namespace controller_manager { 14 | class ControllerManager; 15 | } 16 | 17 | namespace canopen { 18 | 19 | class ControllerManagerLayer : public canopen::Layer { 20 | std::shared_ptr cm_; 21 | canopen::RobotLayerSharedPtr robot_; 22 | ros::NodeHandle nh_; 23 | 24 | canopen::time_point last_time_; 25 | std::atomic recover_; 26 | const ros::Duration fixed_period_; 27 | 28 | public: 29 | ControllerManagerLayer(const canopen::RobotLayerSharedPtr robot, const ros::NodeHandle &nh, const ros::Duration &fixed_period) 30 | :Layer("ControllerManager"), robot_(robot), nh_(nh), fixed_period_(fixed_period) { 31 | } 32 | 33 | virtual void handleRead(canopen::LayerStatus &status, const LayerState ¤t_state); 34 | virtual void handleWrite(canopen::LayerStatus &status, const LayerState ¤t_state); 35 | virtual void handleDiag(canopen::LayerReport &report) { /* nothing to do */ } 36 | virtual void handleHalt(canopen::LayerStatus &status) { /* nothing to do (?) */ } 37 | virtual void handleInit(canopen::LayerStatus &status); 38 | virtual void handleRecover(canopen::LayerStatus &status); 39 | virtual void handleShutdown(canopen::LayerStatus &status); 40 | }; 41 | 42 | } // namespace canopen 43 | 44 | #endif /* CANOPEN_MOTOR_NODE_CONTROLLER_MANAGER_LAYER_H_ */ 45 | -------------------------------------------------------------------------------- /socketcan_bridge/src/rosconsole_bridge.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2019, Mathias Lüdtke 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are met: 6 | * * Redistributions of source code must retain the above copyright notice, 7 | * this list of conditions and the following disclaimer. 8 | * * Redistributions in binary form must reproduce the above copyright 9 | * notice, this list of conditions and the following disclaimer in the 10 | * documentation and/or other materials provided with the distribution. 11 | * * Neither the name of the copyright holder nor the names of its 12 | * contributors may be used to endorse or promote products derived from 13 | * this software without specific prior written permission. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | * POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include 29 | REGISTER_ROSCONSOLE_BRIDGE; 30 | -------------------------------------------------------------------------------- /socketcan_interface/test/test_string.cpp: -------------------------------------------------------------------------------- 1 | // Bring in my package's API, which is what I'm testing 2 | #include 3 | 4 | // Bring in gtest 5 | #include 6 | 7 | 8 | TEST(StringTest, stringconversion) 9 | { 10 | const std::string s1("123#1234567812345678"); 11 | can::Frame f1 = can::toframe(s1); 12 | EXPECT_EQ(s1, can::tostring(f1, true)); 13 | 14 | const std::string s2("1337#1234567812345678"); 15 | can::Frame f2 = can::toframe(s2); 16 | EXPECT_FALSE(f2.isValid()); 17 | 18 | const std::string s3("80001337#1234567812345678"); 19 | const std::string s4("00001337#1234567812345678"); 20 | 21 | can::Frame f3 = can::toframe(s3); 22 | EXPECT_EQ(f3.fullid(), 0x80001337); 23 | EXPECT_TRUE(f3.isValid()); 24 | EXPECT_TRUE(f3.is_extended); 25 | EXPECT_EQ(s4, can::tostring(f3, true)); // 8000 is converted to 0000 26 | 27 | can::Frame f4 = can::toframe(s4); 28 | EXPECT_EQ(f4.fullid(), 0x80001337); 29 | EXPECT_TRUE(f4.isValid()); 30 | EXPECT_TRUE(f4.is_extended); 31 | EXPECT_EQ(s4, can::tostring(f4, true)); 32 | 33 | const std::string s5("20001337#1234567812345678"); 34 | can::Frame f5 = can::toframe(s5); 35 | EXPECT_EQ(f5.fullid(), 0xA0001337); 36 | EXPECT_TRUE(f5.isValid()); 37 | EXPECT_TRUE(f5.is_error); 38 | EXPECT_EQ(s5, can::tostring(f5, true)); 39 | 40 | const std::string s6("40001337#1234567812345678"); 41 | can::Frame f6 = can::toframe(s6); 42 | EXPECT_EQ(f6.fullid(), 0xC0001337); 43 | EXPECT_TRUE(f6.isValid()); 44 | EXPECT_TRUE(f6.is_rtr); 45 | EXPECT_EQ(s6, can::tostring(f6, true)); 46 | 47 | } 48 | 49 | // Run all the tests that were declared with TEST() 50 | int main(int argc, char **argv){ 51 | testing::InitGoogleTest(&argc, argv); 52 | return RUN_ALL_TESTS(); 53 | } 54 | -------------------------------------------------------------------------------- /canopen_402/test/clamping.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | 5 | template class ModeTargetHelperTest : public canopen::ModeTargetHelper, public ::testing::Test{ 6 | public: 7 | ModeTargetHelperTest() : canopen::ModeTargetHelper(0) {} 8 | virtual bool read(const uint16_t &sw) { return false; } 9 | virtual bool write(canopen::Mode::OpModeAccesser& cw) { return false; } 10 | }; 11 | 12 | typedef ::testing::Types MyTypes; 13 | 14 | TYPED_TEST_CASE(ModeTargetHelperTest, MyTypes); 15 | 16 | TYPED_TEST(ModeTargetHelperTest, CheckNaN){ 17 | ASSERT_FALSE(this->setTarget(std::numeric_limits::quiet_NaN())); 18 | } 19 | 20 | TYPED_TEST(ModeTargetHelperTest, CheckZero){ 21 | ASSERT_TRUE(this->setTarget(0.0)); 22 | } 23 | 24 | TYPED_TEST(ModeTargetHelperTest, CheckOne){ 25 | ASSERT_TRUE(this->setTarget(1.0)); 26 | } 27 | 28 | TYPED_TEST(ModeTargetHelperTest, CheckMax){ 29 | double max = static_cast(std::numeric_limits::max()); 30 | 31 | ASSERT_TRUE(this->setTarget(max)); 32 | ASSERT_EQ(max, this->getTarget()); 33 | 34 | ASSERT_TRUE(this->setTarget(max-1)); 35 | ASSERT_EQ(max-1,this->getTarget()); 36 | 37 | ASSERT_TRUE(this->setTarget(max+1)); 38 | ASSERT_EQ(max, this->getTarget()); 39 | } 40 | 41 | TYPED_TEST(ModeTargetHelperTest, CheckMin){ 42 | double min = static_cast(std::numeric_limits::min()); 43 | 44 | ASSERT_TRUE(this->setTarget(min)); 45 | ASSERT_EQ(min, this->getTarget()); 46 | 47 | ASSERT_TRUE(this->setTarget(min-1)); 48 | ASSERT_EQ(min, this->getTarget()); 49 | 50 | ASSERT_TRUE(this->setTarget(min+1)); 51 | ASSERT_EQ(min+1,this->getTarget()); 52 | } 53 | 54 | int main(int argc, char **argv){ 55 | testing::InitGoogleTest(&argc, argv); 56 | return RUN_ALL_TESTS(); 57 | } 58 | -------------------------------------------------------------------------------- /canopen_motor_node/src/controller_manager_layer.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | 5 | using namespace canopen; 6 | 7 | void ControllerManagerLayer::handleRead(canopen::LayerStatus &status, const LayerState ¤t_state) { 8 | if(current_state > Shutdown){ 9 | if(!cm_) status.error("controller_manager is not intialized"); 10 | } 11 | } 12 | 13 | void ControllerManagerLayer::handleWrite(canopen::LayerStatus &status, const LayerState ¤t_state) { 14 | if(current_state > Shutdown){ 15 | if(!cm_){ 16 | status.error("controller_manager is not intialized"); 17 | }else{ 18 | time_point abs_now = canopen::get_abs_time(); 19 | ros::Time now = ros::Time::now(); 20 | 21 | ros::Duration period = fixed_period_; 22 | 23 | if(period.isZero()) { 24 | period.fromSec(boost::chrono::duration(abs_now -last_time_).count()); 25 | } 26 | 27 | last_time_ = abs_now; 28 | 29 | bool recover = recover_.exchange(false); 30 | cm_->update(now, period, recover); 31 | robot_->enforce(period, recover); 32 | } 33 | } 34 | } 35 | 36 | void ControllerManagerLayer::handleInit(canopen::LayerStatus &status) { 37 | if(cm_){ 38 | status.warn("controller_manager is already intialized"); 39 | }else{ 40 | recover_ = true; 41 | last_time_ = canopen::get_abs_time(); 42 | cm_.reset(new controller_manager::ControllerManager(robot_.get(), nh_)); 43 | } 44 | } 45 | 46 | void ControllerManagerLayer::handleRecover(canopen::LayerStatus &status) { 47 | if(!cm_) status.error("controller_manager is not intialized"); 48 | else recover_ = true; 49 | } 50 | 51 | void ControllerManagerLayer::handleShutdown(canopen::LayerStatus &status) { 52 | cm_.reset(); 53 | } 54 | 55 | -------------------------------------------------------------------------------- /can_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package can_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.8.5 (2020-09-22) 6 | ------------------ 7 | 8 | 0.8.4 (2020-08-22) 9 | ------------------ 10 | 11 | 0.8.3 (2020-05-07) 12 | ------------------ 13 | * Bump CMake version to avoid CMP0048 warning 14 | Signed-off-by: ahcorde 15 | * Contributors: ahcorde 16 | 17 | 0.8.2 (2019-11-04) 18 | ------------------ 19 | 20 | 0.8.1 (2019-07-14) 21 | ------------------ 22 | * Set C++ standard to c++14 23 | * Contributors: Harsh Deshpande 24 | 25 | 0.8.0 (2018-07-11) 26 | ------------------ 27 | 28 | 0.7.8 (2018-05-04) 29 | ------------------ 30 | 31 | 0.7.7 (2018-05-04) 32 | ------------------ 33 | 34 | 0.7.6 (2017-08-30) 35 | ------------------ 36 | 37 | 0.7.5 (2017-05-29) 38 | ------------------ 39 | 40 | 0.7.4 (2017-04-25) 41 | ------------------ 42 | 43 | 0.7.3 (2017-04-25) 44 | ------------------ 45 | 46 | 0.7.2 (2017-03-28) 47 | ------------------ 48 | 49 | 0.7.1 (2017-03-20) 50 | ------------------ 51 | 52 | 0.7.0 (2016-12-13) 53 | ------------------ 54 | 55 | 0.6.5 (2016-12-10) 56 | ------------------ 57 | * hamonized versions 58 | * styled and sorted CMakeLists.txt 59 | * removed boilerplate comments 60 | * indention 61 | * reviewed exported dependencies 62 | * styled and sorted package.xml 63 | * Adds message_runtime to can_msgs dependencies. 64 | Added the missing dependency, also changes message_generation to a build_depend. 65 | * Finalizes work on the socketcan_bridge and can_msgs. 66 | Readies the packages socketcan_bridge and can_msgs for the merge with ros_canopen. 67 | Bumps the version for both packages to 0.1.0. Final cleanup in CMakeLists, added 68 | comments to the shell script and launchfile used for testing. 69 | * Introduces the can_msgs package for message types. 70 | Package to hold CAN message types, the Frame message type can contain the data 71 | as returned by SocketCAN. 72 | * Contributors: Ivor Wanders, Mathias Lüdtke 73 | -------------------------------------------------------------------------------- /socketcan_interface/include/socketcan_interface/settings.h: -------------------------------------------------------------------------------- 1 | #ifndef SOCKETCAN_INTERFACE_SETTINGS_H 2 | #define SOCKETCAN_INTERFACE_SETTINGS_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | 10 | namespace can { 11 | class Settings 12 | { 13 | public: 14 | template T get_optional(const std::string &n, const T& def) const { 15 | std::string repr; 16 | if(!getRepr(n, repr)){ 17 | return def; 18 | } 19 | return boost::lexical_cast(repr); 20 | } 21 | template bool get(const std::string &n, T& val) const { 22 | std::string repr; 23 | if(!getRepr(n, repr)) return false; 24 | val = boost::lexical_cast(repr); 25 | return true; 26 | } 27 | virtual ~Settings() {} 28 | private: 29 | virtual bool getRepr(const std::string &n, std::string & repr) const = 0; 30 | }; 31 | using SettingsConstSharedPtr = std::shared_ptr; 32 | using SettingsSharedPtr = std::shared_ptr; 33 | 34 | class NoSettings : public Settings { 35 | public: 36 | static SettingsConstSharedPtr create() { return SettingsConstSharedPtr(new NoSettings); } 37 | private: 38 | virtual bool getRepr(const std::string &n, std::string & repr) const { return false; } 39 | }; 40 | 41 | class SettingsMap : public Settings { 42 | std::map settings_; 43 | virtual bool getRepr(const std::string &n, std::string & repr) const { 44 | std::map::const_iterator it = settings_.find(n); 45 | if (it == settings_.cend()) return false; 46 | repr = it->second; 47 | return true; 48 | } 49 | public: 50 | template void set(const std::string &n, const T& val) { 51 | settings_[n] = boost::lexical_cast(val); 52 | } 53 | static std::shared_ptr create() { return std::shared_ptr(new SettingsMap); } 54 | }; 55 | 56 | 57 | } // can 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /canopen_master/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(canopen_master) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | class_loader 7 | socketcan_interface 8 | ) 9 | 10 | find_package(Boost REQUIRED 11 | COMPONENTS 12 | atomic 13 | chrono 14 | thread 15 | ) 16 | 17 | catkin_package( 18 | INCLUDE_DIRS 19 | include 20 | LIBRARIES 21 | ${PROJECT_NAME} 22 | CATKIN_DEPENDS 23 | socketcan_interface 24 | DEPENDS 25 | Boost 26 | ) 27 | include_directories(include ${catkin_INCLUDE_DIRS}) 28 | 29 | add_library(${PROJECT_NAME} 30 | src/emcy.cpp 31 | src/node.cpp 32 | src/objdict.cpp 33 | src/pdo.cpp 34 | src/sdo.cpp 35 | ) 36 | target_link_libraries(${PROJECT_NAME} 37 | ${catkin_LIBRARIES} 38 | ${Boost_LIBRARIES} 39 | ) 40 | 41 | add_library(${PROJECT_NAME}_plugin 42 | src/master_plugin.cpp 43 | ) 44 | 45 | target_link_libraries(${PROJECT_NAME}_plugin 46 | ${catkin_LIBRARIES} 47 | ${Boost_LIBRARIES} 48 | ${PROJECT_NAME} 49 | ) 50 | 51 | # canopen_bcm_sync 52 | add_executable(canopen_bcm_sync 53 | src/bcm_sync.cpp 54 | ) 55 | target_link_libraries(canopen_bcm_sync 56 | ${Boost_LIBRARIES} 57 | ${catkin_LIBRARIES} 58 | ) 59 | 60 | install( 61 | TARGETS 62 | canopen_bcm_sync 63 | ${PROJECT_NAME} 64 | ${PROJECT_NAME}_plugin 65 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 66 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 67 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 68 | ) 69 | 70 | install( 71 | DIRECTORY 72 | include/${PROJECT_NAME}/ 73 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 74 | FILES_MATCHING PATTERN "*.h" 75 | ) 76 | 77 | install( 78 | FILES 79 | master_plugin.xml 80 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 81 | ) 82 | 83 | if(CATKIN_ENABLE_TESTING) 84 | catkin_add_gtest(${PROJECT_NAME}-test_parser 85 | test/test_parser.cpp 86 | ) 87 | target_link_libraries(${PROJECT_NAME}-test_parser 88 | ${PROJECT_NAME} 89 | ) 90 | 91 | catkin_add_gtest(${PROJECT_NAME}-test_node 92 | test/test_node.cpp 93 | ) 94 | target_link_libraries(${PROJECT_NAME}-test_node 95 | ${PROJECT_NAME} 96 | ) 97 | endif() 98 | -------------------------------------------------------------------------------- /canopen_master/src/bcm_sync.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | int main(int argc, char** argv){ 6 | 7 | if(argc < 4){ 8 | std::cout << "Usage: " << argv[0] << " DEVICE PERIOD_MS HEADER [OVERFLOW] [+ID*] [-ID*] [--]" << std::endl; 9 | return 1; 10 | } 11 | 12 | std::string can_device = argv[1]; 13 | int sync_ms = atoi(argv[2]); 14 | can::Header header = can::toheader(argv[3]); 15 | 16 | if(!header.isValid()){ 17 | std::cout << "header is invalid" << std::endl; 18 | return 1; 19 | } 20 | int sync_overflow = 0; 21 | 22 | int start = 4; 23 | if(argc > start && argv[start][0] != '-' && argv[start][0] != '+'){ 24 | sync_overflow = atoi(argv[4]); 25 | if(sync_overflow == 1 || sync_overflow < 0 || sync_overflow > 240){ 26 | std::cout << "sync overflow is invalid" << std::endl; 27 | return 1; 28 | } 29 | ++start; 30 | } 31 | 32 | std::set monitored, ignored; 33 | 34 | for(; argc > start; ++start){ 35 | if(strncmp("--", argv[start], 2) == 0) break; 36 | int id = atoi(argv[start]); 37 | 38 | if(id > 0 && id < 128) monitored.insert(id); 39 | else if (id < 0 && id > -128) ignored.insert(-id); 40 | else{ 41 | std::cout << "ID is invalid: " << id << std::endl; 42 | return 1; 43 | } 44 | } 45 | 46 | can::SocketCANDriverSharedPtr driver = std::make_shared(); 47 | if(!driver->init(can_device, false, can::NoSettings::create())){ 48 | std::cout << "Could not initialize CAN" << std::endl; 49 | return 1; 50 | } 51 | 52 | canopen::SyncProperties sync_properties(header, sync_ms, sync_overflow); 53 | canopen::BCMsync sync(can_device, driver, sync_properties); 54 | 55 | sync.setMonitored(monitored); 56 | sync.setIgnored(ignored); 57 | 58 | canopen::LayerStatus status; 59 | sync.init(status); 60 | if(!status.bounded()){ 61 | std::cout << "Could not initialize sync" << std::endl; 62 | return 1; 63 | } 64 | 65 | driver->run(); 66 | 67 | return 0; 68 | } 69 | -------------------------------------------------------------------------------- /canopen_motor_node/include/canopen_motor_node/handle_layer_base.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef CANOPEN_MOTOR_NODE_HANDLE_LAYER_BASE_H_ 3 | #define CANOPEN_MOTOR_NODE_HANDLE_LAYER_BASE_H_ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | namespace canopen { 10 | 11 | class HandleLayerBase: public canopen::Layer{ 12 | public: 13 | HandleLayerBase(const std::string &name) : Layer(name) {} 14 | 15 | enum CanSwitchResult{ 16 | NotSupported, 17 | NotReadyToSwitch, 18 | ReadyToSwitch, 19 | NoNeedToSwitch 20 | }; 21 | 22 | virtual CanSwitchResult canSwitch(const canopen::MotorBase::OperationMode &m) = 0; 23 | virtual bool switchMode(const canopen::MotorBase::OperationMode &m) = 0; 24 | 25 | virtual bool forwardForMode(const canopen::MotorBase::OperationMode &m) = 0; 26 | 27 | virtual void registerHandle(hardware_interface::JointStateInterface &iface) = 0; 28 | virtual hardware_interface::JointHandle* registerHandle(hardware_interface::PositionJointInterface &iface, 29 | const joint_limits_interface::JointLimits &limits, 30 | const joint_limits_interface::SoftJointLimits *soft_limits = 0) = 0; 31 | virtual hardware_interface::JointHandle* registerHandle(hardware_interface::VelocityJointInterface &iface, 32 | const joint_limits_interface::JointLimits &limits, 33 | const joint_limits_interface::SoftJointLimits *soft_limits = 0) = 0; 34 | virtual hardware_interface::JointHandle* registerHandle(hardware_interface::EffortJointInterface &iface, 35 | const joint_limits_interface::JointLimits &limits, 36 | const joint_limits_interface::SoftJointLimits *soft_limits = 0) = 0; 37 | 38 | virtual void enforceLimits(const ros::Duration &period, bool reset) = 0; 39 | virtual void enableLimits(bool enable) = 0; 40 | }; 41 | typedef std::shared_ptr HandleLayerBaseSharedPtr; 42 | 43 | } // namespace canopen 44 | 45 | #endif /* CANOPEN_MOTOR_NODE_HANDLE_LAYER_BASE_H_ */ 46 | -------------------------------------------------------------------------------- /socketcan_interface/include/socketcan_interface/filter.h: -------------------------------------------------------------------------------- 1 | #ifndef SOCKETCAN_INTERFACE_FILTER_H 2 | #define SOCKETCAN_INTERFACE_FILTER_H 3 | 4 | #include 5 | 6 | #include "interface.h" 7 | 8 | namespace can { 9 | 10 | class FrameFilter { 11 | public: 12 | virtual bool pass(const can::Frame &frame) const = 0; 13 | virtual ~FrameFilter() {} 14 | }; 15 | using FrameFilterSharedPtr = std::shared_ptr; 16 | 17 | class FrameMaskFilter : public FrameFilter { 18 | public: 19 | static const uint32_t MASK_ALL = 0xffffffff; 20 | static const uint32_t MASK_RELAXED = ~Frame::EXTENDED_MASK; 21 | FrameMaskFilter(uint32_t can_id, uint32_t mask = MASK_RELAXED, bool invert = false) 22 | : mask_(mask), masked_id_(can_id & mask), invert_(invert) 23 | {} 24 | virtual bool pass(const can::Frame &frame) const{ 25 | const uint32_t k = frame.key(); 26 | return ((mask_ & k) == masked_id_) != invert_; 27 | } 28 | private: 29 | const uint32_t mask_; 30 | const uint32_t masked_id_; 31 | const bool invert_; 32 | }; 33 | 34 | class FrameRangeFilter : public FrameFilter { 35 | public: 36 | FrameRangeFilter(uint32_t min_id, uint32_t max_id, bool invert = false) 37 | : min_id_(min_id), max_id_(max_id), invert_(invert) 38 | {} 39 | virtual bool pass(const can::Frame &frame) const{ 40 | const uint32_t k = frame.key(); 41 | return (min_id_ <= k && k <= max_id_) != invert_; 42 | } 43 | private: 44 | const uint32_t min_id_; 45 | const uint32_t max_id_; 46 | const bool invert_; 47 | }; 48 | 49 | class FilteredFrameListener : public CommInterface::FrameListener { 50 | public: 51 | using FilterVector = std::vector; 52 | FilteredFrameListener(CommInterfaceSharedPtr comm, const Callable &callable, const FilterVector &filters) 53 | : CommInterface::FrameListener(callable), 54 | filters_(filters), 55 | listener_(comm->createMsgListener([this](const Frame &frame) { 56 | for(FilterVector::const_iterator it=this->filters_.begin(); it != this->filters_.end(); ++it) { 57 | if((*it)->pass(frame)){ 58 | (*this)(frame); 59 | break; 60 | } 61 | } 62 | })) 63 | {} 64 | const std::vector filters_; 65 | CommInterface::FrameListenerConstSharedPtr listener_; 66 | }; 67 | 68 | } // namespace can 69 | 70 | #endif /*SOCKETCAN_INTERFACE_FILTER_H*/ 71 | -------------------------------------------------------------------------------- /canopen_motor_node/src/interface_mapping.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef INTERFACE_MAPPING_H_ 3 | #define INTERFACE_MAPPING_H_ 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | class InterfaceMapping { 14 | typedef boost::bimap, boost::bimaps::set_of > bimap_type; 15 | bimap_type mapping_; 16 | public: 17 | InterfaceMapping(){ 18 | mapping_.insert(bimap_type::value_type("hardware_interface::PositionJointInterface" ,canopen::MotorBase::Profiled_Position)); 19 | mapping_.insert(bimap_type::value_type("hardware_interface::PositionJointInterface" ,canopen::MotorBase::Interpolated_Position)); 20 | mapping_.insert(bimap_type::value_type("hardware_interface::PositionJointInterface" ,canopen::MotorBase::Cyclic_Synchronous_Position)); 21 | 22 | mapping_.insert(bimap_type::value_type("hardware_interface::VelocityJointInterface" ,canopen::MotorBase::Velocity)); 23 | mapping_.insert(bimap_type::value_type("hardware_interface::VelocityJointInterface" ,canopen::MotorBase::Profiled_Velocity)); 24 | mapping_.insert(bimap_type::value_type("hardware_interface::VelocityJointInterface" ,canopen::MotorBase::Cyclic_Synchronous_Velocity)); 25 | 26 | mapping_.insert(bimap_type::value_type("hardware_interface::EffortJointInterface" ,canopen::MotorBase::Profiled_Torque)); 27 | mapping_.insert(bimap_type::value_type("hardware_interface::EffortJointInterface" ,canopen::MotorBase::Cyclic_Synchronous_Torque)); 28 | } 29 | std::vector getInterfaceModes(const std::string &interface){ 30 | std::vector modes; 31 | BOOST_FOREACH(bimap_type::left_reference i, mapping_.left.equal_range(interface)){ 32 | modes.push_back(i.second); 33 | } 34 | return modes; 35 | } 36 | bool hasConflict(const std::string &interface, canopen::MotorBase::OperationMode mode){ 37 | bimap_type::right_const_iterator it; 38 | if((it = mapping_.right.find(mode)) != mapping_.right.end()){ 39 | return it->second != interface; 40 | } 41 | return false; 42 | } 43 | 44 | }; 45 | 46 | extern InterfaceMapping g_interface_mapping; 47 | 48 | #endif /* INTERFACE_MAPPING_H_ */ 49 | -------------------------------------------------------------------------------- /canopen_master/include/canopen_master/timer.h: -------------------------------------------------------------------------------- 1 | #ifndef H_CANOPEN_TIMER 2 | #define H_CANOPEN_TIMER 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | 13 | namespace canopen{ 14 | 15 | class Timer{ 16 | public: 17 | using TimerFunc = std::function; 18 | using TimerDelegate [[deprecated("use TimerFunc instead")]] = can::DelegateHelper; 19 | 20 | Timer():work(io), timer(io),thread(std::bind( 21 | static_cast(&boost::asio::io_service::run), &io)) 22 | { 23 | } 24 | 25 | void stop(){ 26 | boost::mutex::scoped_lock lock(mutex); 27 | timer.cancel(); 28 | } 29 | template void start(const TimerFunc &del, const T &dur, bool start_now = true){ 30 | boost::mutex::scoped_lock lock(mutex); 31 | delegate = del; 32 | period = boost::chrono::duration_cast(dur); 33 | if(start_now){ 34 | timer.expires_from_now(period); 35 | timer.async_wait(std::bind(&Timer::handler, this, std::placeholders::_1)); 36 | } 37 | } 38 | void restart(){ 39 | boost::mutex::scoped_lock lock(mutex); 40 | timer.expires_from_now(period); 41 | timer.async_wait(std::bind(&Timer::handler, this, std::placeholders::_1)); 42 | } 43 | const boost::chrono::high_resolution_clock::duration & getPeriod(){ 44 | boost::mutex::scoped_lock lock(mutex); 45 | return period; 46 | } 47 | ~Timer(){ 48 | io.stop(); 49 | thread.join(); 50 | } 51 | 52 | private: 53 | boost::asio::io_service io; 54 | boost::asio::io_service::work work; 55 | boost::asio::basic_waitable_timer timer; 56 | boost::chrono::high_resolution_clock::duration period; 57 | boost::mutex mutex; 58 | boost::thread thread; 59 | 60 | TimerFunc delegate; 61 | void handler(const boost::system::error_code& ec){ 62 | if(!ec){ 63 | boost::mutex::scoped_lock lock(mutex); 64 | if(delegate && delegate()){ 65 | timer.expires_at(timer.expires_at() + period); 66 | timer.async_wait(std::bind(&Timer::handler, this, std::placeholders::_1)); 67 | } 68 | 69 | } 70 | } 71 | }; 72 | 73 | } 74 | 75 | #endif 76 | -------------------------------------------------------------------------------- /canopen_chain_node/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(canopen_chain_node) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | canopen_master 7 | diagnostic_updater 8 | message_generation 9 | pluginlib 10 | rosconsole_bridge 11 | roscpp 12 | roslib 13 | socketcan_interface 14 | std_msgs 15 | std_srvs 16 | ) 17 | 18 | find_package(Boost REQUIRED 19 | COMPONENTS 20 | filesystem 21 | ) 22 | 23 | add_service_files(DIRECTORY srv 24 | FILES 25 | GetObject.srv 26 | SetObject.srv) 27 | 28 | generate_messages(DEPENDENCIES) 29 | 30 | catkin_package( 31 | INCLUDE_DIRS 32 | include 33 | LIBRARIES 34 | canopen_ros_chain 35 | CATKIN_DEPENDS 36 | canopen_master 37 | diagnostic_updater 38 | message_runtime 39 | pluginlib 40 | rosconsole_bridge 41 | roscpp 42 | socketcan_interface 43 | std_srvs 44 | DEPENDS 45 | Boost 46 | ) 47 | 48 | include_directories( 49 | include 50 | ${Boost_INCLUDE_DIRS} 51 | ${catkin_INCLUDE_DIRS} 52 | ) 53 | 54 | # canopen_ros_chain 55 | add_library(canopen_ros_chain 56 | src/ros_chain.cpp 57 | src/rosconsole_bridge.cpp 58 | ) 59 | target_link_libraries(canopen_ros_chain 60 | ${Boost_LIBRARIES} 61 | ${catkin_LIBRARIES} 62 | ) 63 | add_dependencies(canopen_ros_chain 64 | ${catkin_EXPORTED_TARGETS} 65 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 66 | ) 67 | 68 | # canopen_chain_node 69 | add_executable(${PROJECT_NAME} 70 | src/chain_node.cpp 71 | ) 72 | target_link_libraries(${PROJECT_NAME} 73 | canopen_ros_chain 74 | ${Boost_LIBRARIES} 75 | ${catkin_LIBRARIES} 76 | ) 77 | 78 | # canopen_sync_node 79 | add_executable(canopen_sync_node 80 | src/rosconsole_bridge.cpp 81 | src/sync_node.cpp 82 | ) 83 | target_link_libraries(canopen_sync_node 84 | ${Boost_LIBRARIES} 85 | ${catkin_LIBRARIES} 86 | ) 87 | 88 | install( 89 | TARGETS 90 | ${PROJECT_NAME} 91 | canopen_ros_chain 92 | canopen_sync_node 93 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 94 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 95 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 96 | ) 97 | 98 | install( 99 | DIRECTORY 100 | include/${PROJECT_NAME}/ 101 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 102 | FILES_MATCHING PATTERN "*.h" 103 | ) 104 | 105 | install( 106 | DIRECTORY 107 | launch 108 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 109 | ) 110 | -------------------------------------------------------------------------------- /socketcan_interface/src/candump.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | using namespace can; 10 | 11 | #include 12 | 13 | void print_frame(const Frame &f){ 14 | 15 | if(f.is_error){ 16 | std::cout << "E " << std::hex << f.id << std::dec; 17 | }else if(f.is_extended){ 18 | std::cout << "e " << std::hex << f.id << std::dec; 19 | }else{ 20 | std::cout << "s " << std::hex << f.id << std::dec; 21 | } 22 | 23 | std::cout << "\t"; 24 | 25 | if(f.is_rtr){ 26 | std::cout << "r"; 27 | }else{ 28 | std::cout << (int) f.dlc << std::hex; 29 | 30 | for(int i=0; i < f.dlc; ++i){ 31 | std::cout << std::hex << " " << (int) f.data[i]; 32 | } 33 | } 34 | 35 | std::cout << std::dec << std::endl; 36 | } 37 | 38 | 39 | std::shared_ptr g_loader; 40 | DriverInterfaceSharedPtr g_driver; 41 | 42 | void print_state(const State & s){ 43 | std::string err; 44 | g_driver->translateError(s.internal_error,err); 45 | std::cout << "STATE: driver_state=" << s.driver_state << " internal_error=" << s.internal_error << "('" << err << "') asio: " << s.error_code << std::endl; 46 | } 47 | 48 | 49 | int main(int argc, char *argv[]){ 50 | 51 | if(argc != 2 && argc != 4){ 52 | std::cout << "usage: "<< argv[0] << " DEVICE [PLUGIN_PATH PLUGIN_NAME]" << std::endl; 53 | return 1; 54 | } 55 | 56 | if(argc == 4 ){ 57 | try 58 | { 59 | g_loader = std::make_shared(argv[2]); 60 | g_driver = g_loader->createUniqueInstance(argv[3]); 61 | } 62 | 63 | catch(std::exception& ex) 64 | { 65 | std::cerr << boost::diagnostic_information(ex) << std::endl;; 66 | return 1; 67 | } 68 | }else{ 69 | g_driver = std::make_shared(); 70 | } 71 | 72 | 73 | 74 | FrameListenerConstSharedPtr frame_printer = g_driver->createMsgListener(print_frame); 75 | StateListenerConstSharedPtr error_printer = g_driver->createStateListener(print_state); 76 | 77 | if(!g_driver->init(argv[1], false, can::NoSettings::create())){ 78 | print_state(g_driver->getState()); 79 | return 1; 80 | } 81 | 82 | g_driver->run(); 83 | 84 | g_driver->shutdown(); 85 | g_driver.reset(); 86 | g_loader.reset(); 87 | 88 | return 0; 89 | 90 | } 91 | -------------------------------------------------------------------------------- /canopen_master/test/test_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | // Bring in gtest 5 | #include 6 | 7 | canopen::ObjectDictSharedPtr make_dict(){ 8 | canopen::DeviceInfo info; 9 | info.nr_of_rx_pdo = 0; 10 | info.nr_of_tx_pdo = 0; 11 | 12 | canopen::ObjectDictSharedPtr dict = std::make_shared(info); 13 | dict->insert(false, std::make_shared(canopen::ObjectDict::VAR, 14 | 0x1017, 15 | canopen::ObjectDict::DEFTYPE_UNSIGNED16, 16 | "producer heartbeat", 17 | true, true, false, 18 | canopen::HoldAny((uint16_t)0), 19 | canopen::HoldAny((uint16_t)100) 20 | )); 21 | return dict; 22 | } 23 | TEST(TestNode, testInitandShutdown){ 24 | 25 | can::DummyBus bus("testInitandShutdown"); 26 | 27 | can::ThreadedDummyInterfaceSharedPtr driver = std::make_shared(); 28 | 29 | can::DummyReplay replay; 30 | 31 | replay.add("0#8201", "701#00"); 32 | replay.add("601#2b17100064000000", "581#6017100000000000"); 33 | replay.add("0#0101", "701#05"); 34 | replay.add("601#2b17100000000000", "581#6017100000000000"); 35 | replay.init(bus); 36 | 37 | EXPECT_FALSE(replay.done()); 38 | 39 | auto settings = can::SettingsMap::create(); 40 | settings->set("trace", true); 41 | 42 | driver->init(bus.name, false, settings); 43 | 44 | canopen::NodeSharedPtr node = std::make_shared(driver, make_dict(), 1); 45 | 46 | { 47 | canopen::LayerStatus status; 48 | node->init(status); 49 | ASSERT_TRUE(status.bounded()); 50 | ASSERT_EQ(canopen::Node::Operational, node->getState()); 51 | } 52 | 53 | { 54 | canopen::LayerStatus status; 55 | node->shutdown(status); 56 | ASSERT_TRUE(status.bounded()); 57 | } 58 | EXPECT_TRUE(replay.done()); 59 | } 60 | 61 | // Run all the tests that were declared with TEST() 62 | int main(int argc, char **argv){ 63 | testing::InitGoogleTest(&argc, argv); 64 | return RUN_ALL_TESTS(); 65 | } 66 | -------------------------------------------------------------------------------- /canopen_motor_node/src/motor_chain.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | using namespace canopen; 6 | 7 | MotorChain::MotorChain(const ros::NodeHandle &nh, const ros::NodeHandle &nh_priv) : 8 | RosChain(nh, nh_priv), motor_allocator_("canopen_402", "canopen::MotorBase::Allocator") {} 9 | 10 | bool MotorChain::nodeAdded(XmlRpc::XmlRpcValue ¶ms, const canopen::NodeSharedPtr &node, const LoggerSharedPtr &logger) 11 | { 12 | std::string name = params["name"]; 13 | std::string &joint = name; 14 | if(params.hasMember("joint")) joint.assign(params["joint"]); 15 | 16 | if(!robot_layer_->getJoint(joint)){ 17 | ROS_ERROR_STREAM("joint " + joint + " was not found in URDF"); 18 | return false; 19 | } 20 | 21 | std::string alloc_name = "canopen::Motor402::Allocator"; 22 | if(params.hasMember("motor_allocator")) alloc_name.assign(params["motor_allocator"]); 23 | 24 | XmlRpcSettings settings; 25 | if(params.hasMember("motor_layer")) settings = params["motor_layer"]; 26 | 27 | MotorBaseSharedPtr motor; 28 | 29 | try{ 30 | motor = motor_allocator_.allocateInstance(alloc_name, name + "_motor", node->getStorage(), settings); 31 | } 32 | catch( const std::exception &e){ 33 | std::string info = boost::diagnostic_information(e); 34 | ROS_ERROR_STREAM(info); 35 | return false; 36 | } 37 | 38 | if(!motor){ 39 | ROS_ERROR_STREAM("Could not allocate motor."); 40 | return false; 41 | } 42 | 43 | motor->registerDefaultModes(node->getStorage()); 44 | motors_->add(motor); 45 | logger->add(motor); 46 | 47 | HandleLayerSharedPtr handle = std::make_shared(joint, motor, node->getStorage(), params); 48 | 49 | canopen::LayerStatus s; 50 | if(!handle->prepareFilters(s)){ 51 | ROS_ERROR_STREAM(s.reason()); 52 | return false; 53 | } 54 | 55 | robot_layer_->add(joint, handle); 56 | logger->add(handle); 57 | 58 | return true; 59 | } 60 | 61 | bool MotorChain::setup_chain() { 62 | motors_.reset(new LayerGroupNoDiag("402 Layer")); 63 | robot_layer_.reset(new RobotLayer(nh_)); 64 | 65 | ros::Duration dur(0.0) ; 66 | 67 | if(RosChain::setup_chain()){ 68 | add(motors_); 69 | add(robot_layer_); 70 | 71 | if(!nh_.param("use_realtime_period", false)){ 72 | dur.fromSec(boost::chrono::duration(update_duration_).count()); 73 | ROS_INFO_STREAM("Using fixed control period: " << dur); 74 | }else{ 75 | ROS_INFO("Using real-time control period"); 76 | } 77 | cm_.reset(new ControllerManagerLayer(robot_layer_, nh_, dur)); 78 | add(cm_); 79 | 80 | return true; 81 | } 82 | 83 | return false; 84 | } 85 | -------------------------------------------------------------------------------- /socketcan_bridge/include/socketcan_bridge/topic_to_socketcan.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016, Ivor Wanders 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are met: 6 | * * Redistributions of source code must retain the above copyright notice, 7 | * this list of conditions and the following disclaimer. 8 | * * Redistributions in binary form must reproduce the above copyright 9 | * notice, this list of conditions and the following disclaimer in the 10 | * documentation and/or other materials provided with the distribution. 11 | * * Neither the name of the copyright holder nor the names of its 12 | * contributors may be used to endorse or promote products derived from 13 | * this software without specific prior written permission. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | * POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #ifndef SOCKETCAN_BRIDGE_TOPIC_TO_SOCKETCAN_H 29 | #define SOCKETCAN_BRIDGE_TOPIC_TO_SOCKETCAN_H 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | namespace socketcan_bridge 36 | { 37 | class TopicToSocketCAN 38 | { 39 | public: 40 | TopicToSocketCAN(ros::NodeHandle* nh, ros::NodeHandle* nh_param, can::DriverInterfaceSharedPtr driver); 41 | void setup(); 42 | 43 | private: 44 | ros::Subscriber can_topic_; 45 | can::DriverInterfaceSharedPtr driver_; 46 | 47 | can::StateListenerConstSharedPtr state_listener_; 48 | 49 | void msgCallback(const can_msgs::Frame::ConstPtr& msg); 50 | void stateCallback(const can::State & s); 51 | }; 52 | 53 | void convertMessageToSocketCAN(const can_msgs::Frame& m, can::Frame& f) 54 | { 55 | f.id = m.id; 56 | f.dlc = m.dlc; 57 | f.is_error = m.is_error; 58 | f.is_rtr = m.is_rtr; 59 | f.is_extended = m.is_extended; 60 | 61 | for (int i = 0; i < 8; i++) // always copy all data, regardless of dlc. 62 | { 63 | f.data[i] = m.data[i]; 64 | } 65 | }; 66 | 67 | }; // namespace socketcan_bridge 68 | 69 | 70 | #endif // SOCKETCAN_BRIDGE_TOPIC_TO_SOCKETCAN_H 71 | -------------------------------------------------------------------------------- /socketcan_bridge/src/socketcan_to_topic_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016, Ivor Wanders 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are met: 6 | * * Redistributions of source code must retain the above copyright notice, 7 | * this list of conditions and the following disclaimer. 8 | * * Redistributions in binary form must reproduce the above copyright 9 | * notice, this list of conditions and the following disclaimer in the 10 | * documentation and/or other materials provided with the distribution. 11 | * * Neither the name of the copyright holder nor the names of its 12 | * contributors may be used to endorse or promote products derived from 13 | * this software without specific prior written permission. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | * POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | 37 | 38 | int main(int argc, char *argv[]) 39 | { 40 | ros::init(argc, argv, "socketcan_to_topic_node"); 41 | 42 | ros::NodeHandle nh(""), nh_param("~"); 43 | 44 | std::string can_device; 45 | nh_param.param("can_device", can_device, "can0"); 46 | 47 | can::ThreadedSocketCANInterfaceSharedPtr driver = std::make_shared (); 48 | 49 | // initialize device at can_device, 0 for no loopback. 50 | if (!driver->init(can_device, 0, XmlRpcSettings::create(nh_param))) 51 | { 52 | ROS_FATAL("Failed to initialize can_device at %s", can_device.c_str()); 53 | return 1; 54 | } 55 | else 56 | { 57 | ROS_INFO("Successfully connected to %s.", can_device.c_str()); 58 | } 59 | 60 | socketcan_bridge::SocketCANToTopic to_topic_bridge(&nh, &nh_param, driver); 61 | to_topic_bridge.setup(nh_param); 62 | 63 | ros::spin(); 64 | 65 | driver->shutdown(); 66 | driver.reset(); 67 | 68 | ros::waitForShutdown(); 69 | } 70 | -------------------------------------------------------------------------------- /socketcan_bridge/src/topic_to_socketcan_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016, Ivor Wanders 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are met: 6 | * * Redistributions of source code must retain the above copyright notice, 7 | * this list of conditions and the following disclaimer. 8 | * * Redistributions in binary form must reproduce the above copyright 9 | * notice, this list of conditions and the following disclaimer in the 10 | * documentation and/or other materials provided with the distribution. 11 | * * Neither the name of the copyright holder nor the names of its 12 | * contributors may be used to endorse or promote products derived from 13 | * this software without specific prior written permission. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | * POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | 37 | 38 | int main(int argc, char *argv[]) 39 | { 40 | ros::init(argc, argv, "topic_to_socketcan_node"); 41 | 42 | ros::NodeHandle nh(""), nh_param("~"); 43 | 44 | std::string can_device; 45 | nh_param.param("can_device", can_device, "can0"); 46 | 47 | can::ThreadedSocketCANInterfaceSharedPtr driver = std::make_shared (); 48 | 49 | // initialize device at can_device, 0 for no loopback. 50 | if (!driver->init(can_device, 0, XmlRpcSettings::create(nh_param))) 51 | { 52 | ROS_FATAL("Failed to initialize can_device at %s", can_device.c_str()); 53 | return 1; 54 | } 55 | else 56 | { 57 | ROS_INFO("Successfully connected to %s.", can_device.c_str()); 58 | } 59 | 60 | socketcan_bridge::TopicToSocketCAN to_socketcan_bridge(&nh, &nh_param, driver); 61 | to_socketcan_bridge.setup(); 62 | 63 | ros::spin(); 64 | 65 | driver->shutdown(); 66 | driver.reset(); 67 | 68 | ros::waitForShutdown(); 69 | } 70 | -------------------------------------------------------------------------------- /canopen_motor_node/include/canopen_motor_node/robot_layer.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef CANOPEN_MOTOR_NODE_ROBOT_LAYER_H_ 3 | #define CANOPEN_MOTOR_NODE_ROBOT_LAYER_H_ 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | 17 | namespace canopen { 18 | 19 | class RobotLayer : public LayerGroupNoDiag, public hardware_interface::RobotHW{ 20 | hardware_interface::JointStateInterface state_interface_; 21 | hardware_interface::PositionJointInterface pos_interface_; 22 | hardware_interface::VelocityJointInterface vel_interface_; 23 | hardware_interface::EffortJointInterface eff_interface_; 24 | 25 | joint_limits_interface::PositionJointSoftLimitsInterface pos_soft_limits_interface_; 26 | joint_limits_interface::PositionJointSaturationInterface pos_saturation_interface_; 27 | joint_limits_interface::VelocityJointSoftLimitsInterface vel_soft_limits_interface_; 28 | joint_limits_interface::VelocityJointSaturationInterface vel_saturation_interface_; 29 | joint_limits_interface::EffortJointSoftLimitsInterface eff_soft_limits_interface_; 30 | joint_limits_interface::EffortJointSaturationInterface eff_saturation_interface_; 31 | 32 | ros::NodeHandle nh_; 33 | urdf::Model urdf_; 34 | 35 | typedef std::unordered_map< std::string, HandleLayerBaseSharedPtr > HandleMap; 36 | HandleMap handles_; 37 | struct SwitchData { 38 | HandleLayerBaseSharedPtr handle; 39 | canopen::MotorBase::OperationMode mode; 40 | bool enforce_limits; 41 | }; 42 | typedef std::vector SwitchContainer; 43 | typedef std::unordered_map SwitchMap; 44 | SwitchMap switch_map_; 45 | 46 | std::atomic first_init_; 47 | 48 | void stopControllers(const std::vector controllers); 49 | public: 50 | void add(const std::string &name, HandleLayerBaseSharedPtr handle); 51 | RobotLayer(ros::NodeHandle nh); 52 | urdf::JointConstSharedPtr getJoint(const std::string &n) const { return urdf_.getJoint(n); } 53 | 54 | virtual void handleInit(canopen::LayerStatus &status); 55 | void enforce(const ros::Duration &period, bool reset); 56 | virtual bool prepareSwitch(const std::list &start_list, const std::list &stop_list); 57 | virtual void doSwitch(const std::list &start_list, const std::list &stop_list); 58 | }; 59 | 60 | typedef std::shared_ptr RobotLayerSharedPtr; 61 | 62 | } // namespace canopen 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /canopen_motor_node/include/canopen_motor_node/unit_converter.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef CANOPEN_MOTOR_NODE_UNIT_CONVERTER_H_ 3 | #define CANOPEN_MOTOR_NODE_UNIT_CONVERTER_H_ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include "muParser.h" 11 | 12 | namespace canopen { 13 | class UnitConverter{ 14 | public: 15 | typedef std::function GetVarFuncType; 16 | 17 | UnitConverter(const std::string &expression, GetVarFuncType var_func) 18 | : var_func_(var_func) 19 | { 20 | parser_.SetVarFactory(UnitConverter::createVariable, this); 21 | 22 | parser_.DefineConst("pi", M_PI); 23 | parser_.DefineConst("nan", std::numeric_limits::quiet_NaN()); 24 | 25 | parser_.DefineFun("rad2deg", UnitConverter::rad2deg); 26 | parser_.DefineFun("deg2rad", UnitConverter::deg2rad); 27 | parser_.DefineFun("norm", UnitConverter::norm); 28 | parser_.DefineFun("smooth", UnitConverter::smooth); 29 | parser_.DefineFun("avg", UnitConverter::avg); 30 | 31 | parser_.SetExpr(expression); 32 | } 33 | 34 | void reset(){ 35 | for(variable_ptr_list::iterator it = var_list_.begin(); it != var_list_.end(); ++it){ 36 | **it = std::numeric_limits::quiet_NaN(); 37 | } 38 | } 39 | double evaluate() { int num; return parser_.Eval(num)[0]; } 40 | private: 41 | typedef std::shared_ptr variable_ptr; 42 | typedef std::list variable_ptr_list; 43 | 44 | static double* createVariable(const char *name, void * userdata) { 45 | UnitConverter * uc = static_cast(userdata); 46 | double *p = uc->var_func_ ? uc->var_func_(name) : 0; 47 | if(!p){ 48 | p = new double(std::numeric_limits::quiet_NaN()); 49 | uc->var_list_.push_back(variable_ptr(p)); 50 | } 51 | return p; 52 | } 53 | variable_ptr_list var_list_; 54 | GetVarFuncType var_func_; 55 | 56 | mu::Parser parser_; 57 | 58 | static double rad2deg(double r){ 59 | return r*180.0/M_PI; 60 | } 61 | static double deg2rad(double d){ 62 | return d*M_PI/180.0; 63 | } 64 | static double norm(double val, double min, double max){ 65 | while(val >= max) val -= (max-min); 66 | while(val < min) val += (max-min); 67 | return val; 68 | } 69 | static double smooth(double val, double old_val, double alpha){ 70 | if(std::isnan(val)) return 0; 71 | if(std::isnan(old_val)) return val; 72 | return alpha*val + (1.0-alpha)*old_val; 73 | } 74 | static double avg(const double *vals, int num) 75 | { 76 | double s = 0.0; 77 | int i=0; 78 | for (; i 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | 37 | int main(int argc, char *argv[]) 38 | { 39 | ros::init(argc, argv, "socketcan_bridge_node"); 40 | 41 | ros::NodeHandle nh(""), nh_param("~"); 42 | 43 | std::string can_device; 44 | nh_param.param("can_device", can_device, "can0"); 45 | 46 | can::ThreadedSocketCANInterfaceSharedPtr driver = std::make_shared (); 47 | 48 | // initialize device at can_device, 0 for no loopback. 49 | if (!driver->init(can_device, 0, XmlRpcSettings::create(nh_param))) 50 | { 51 | ROS_FATAL("Failed to initialize can_device at %s", can_device.c_str()); 52 | return 1; 53 | } 54 | else 55 | { 56 | ROS_INFO("Successfully connected to %s.", can_device.c_str()); 57 | } 58 | 59 | // initialize the bridge both ways. 60 | socketcan_bridge::TopicToSocketCAN to_socketcan_bridge(&nh, &nh_param, driver); 61 | to_socketcan_bridge.setup(); 62 | 63 | socketcan_bridge::SocketCANToTopic to_topic_bridge(&nh, &nh_param, driver); 64 | to_topic_bridge.setup(nh_param); 65 | 66 | ros::spin(); 67 | 68 | driver->shutdown(); 69 | driver.reset(); 70 | 71 | ros::waitForShutdown(); 72 | } 73 | -------------------------------------------------------------------------------- /socketcan_bridge/include/socketcan_bridge/socketcan_to_topic.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016, Ivor Wanders 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are met: 6 | * * Redistributions of source code must retain the above copyright notice, 7 | * this list of conditions and the following disclaimer. 8 | * * Redistributions in binary form must reproduce the above copyright 9 | * notice, this list of conditions and the following disclaimer in the 10 | * documentation and/or other materials provided with the distribution. 11 | * * Neither the name of the copyright holder nor the names of its 12 | * contributors may be used to endorse or promote products derived from 13 | * this software without specific prior written permission. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | * POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #ifndef SOCKETCAN_BRIDGE_SOCKETCAN_TO_TOPIC_H 29 | #define SOCKETCAN_BRIDGE_SOCKETCAN_TO_TOPIC_H 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | namespace socketcan_bridge 37 | { 38 | class SocketCANToTopic 39 | { 40 | public: 41 | SocketCANToTopic(ros::NodeHandle* nh, ros::NodeHandle* nh_param, can::DriverInterfaceSharedPtr driver); 42 | void setup(); 43 | void setup(const can::FilteredFrameListener::FilterVector &filters); 44 | void setup(XmlRpc::XmlRpcValue filters); 45 | void setup(ros::NodeHandle nh); 46 | 47 | private: 48 | ros::Publisher can_topic_; 49 | can::DriverInterfaceSharedPtr driver_; 50 | 51 | can::FrameListenerConstSharedPtr frame_listener_; 52 | can::StateListenerConstSharedPtr state_listener_; 53 | 54 | 55 | void frameCallback(const can::Frame& f); 56 | void stateCallback(const can::State & s); 57 | }; 58 | 59 | void convertSocketCANToMessage(const can::Frame& f, can_msgs::Frame& m) 60 | { 61 | m.id = f.id; 62 | m.dlc = f.dlc; 63 | m.is_error = f.is_error; 64 | m.is_rtr = f.is_rtr; 65 | m.is_extended = f.is_extended; 66 | 67 | for (int i = 0; i < 8; i++) // always copy all data, regardless of dlc. 68 | { 69 | m.data[i] = f.data[i]; 70 | } 71 | }; 72 | 73 | }; // namespace socketcan_bridge 74 | 75 | 76 | #endif // SOCKETCAN_BRIDGE_SOCKETCAN_TO_TOPIC_H 77 | -------------------------------------------------------------------------------- /canopen_motor_node/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(canopen_motor_node) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | canopen_402 7 | canopen_chain_node 8 | canopen_master 9 | controller_manager 10 | controller_manager_msgs 11 | filters 12 | hardware_interface 13 | joint_limits_interface 14 | roscpp 15 | urdf 16 | ) 17 | 18 | find_package(Boost REQUIRED 19 | COMPONENTS 20 | thread 21 | ) 22 | 23 | find_package(PkgConfig) 24 | pkg_check_modules(PC_MUPARSER QUIET muparser) 25 | set(MUPARSER_DEFINITIONS ${PC_MUPARSER_CFLAGS_OTHER}) 26 | 27 | find_path(MUPARSER_INCLUDE_DIR muParser.h 28 | HINTS ${PC_MUPARSER_INCLUDEDIR} ${PC_MUPARSER_INCLUDE_DIRS} 29 | ) 30 | 31 | find_library(MUPARSER_LIBRARY NAMES muparser libmurser 32 | HINTS ${PC_MUPARSER_LIBDIR} ${PC_MUPARSER_LIBRARY_DIRS} ) 33 | 34 | include(FindPackageHandleStandardArgs) 35 | # handle the QUIETLY and REQUIRED arguments and set MUPARSER_FOUND to TRUE 36 | # if all listed variables are TRUE 37 | find_package_handle_standard_args(MUPARSER DEFAULT_MSG 38 | MUPARSER_LIBRARY MUPARSER_INCLUDE_DIR) 39 | mark_as_advanced(MUPARSER_INCLUDE_DIR MUPARSER_LIBRARY ) 40 | 41 | if(NOT ${MUPARSER_FOUND}) 42 | message(FATAL_ERROR "muparser library not found") 43 | endif() 44 | 45 | set(MUPARSER_LIBRARIES ${MUPARSER_LIBRARY} ) 46 | set(MUPARSER_INCLUDE_DIRS ${MUPARSER_INCLUDE_DIR} ) 47 | 48 | catkin_package( 49 | INCLUDE_DIRS 50 | include 51 | LIBRARIES 52 | canopen_motor 53 | CATKIN_DEPENDS 54 | canopen_402 55 | canopen_chain_node 56 | canopen_master 57 | controller_manager 58 | hardware_interface 59 | joint_limits_interface 60 | roscpp 61 | urdf 62 | DEPENDS 63 | Boost 64 | MUPARSER 65 | ) 66 | 67 | include_directories( 68 | include 69 | ${catkin_INCLUDE_DIRS} 70 | ${MUPARSER_INCLUDE_DIRS} 71 | ) 72 | 73 | # canopen_motor 74 | add_library(canopen_motor 75 | src/controller_manager_layer.cpp 76 | src/handle_layer.cpp 77 | src/motor_chain.cpp 78 | src/robot_layer.cpp 79 | ) 80 | target_link_libraries(canopen_motor 81 | ${catkin_LIBRARIES} 82 | ${MUPARSER_LIBRARIES} 83 | ) 84 | add_dependencies(canopen_motor 85 | ${catkin_EXPORTED_TARGETS} 86 | ) 87 | 88 | # canopen_motor_node 89 | add_executable(${PROJECT_NAME} 90 | src/canopen_motor_chain_node.cpp 91 | ) 92 | target_link_libraries(${PROJECT_NAME} 93 | canopen_motor 94 | ${catkin_LIBRARIES} 95 | ) 96 | add_dependencies(${PROJECT_NAME} 97 | ${catkin_EXPORTED_TARGETS} 98 | ) 99 | 100 | install( 101 | TARGETS 102 | canopen_motor 103 | ${PROJECT_NAME} 104 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 105 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 106 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 107 | ) 108 | 109 | install(DIRECTORY include/${PROJECT_NAME}/ 110 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 111 | FILES_MATCHING PATTERN "*.h" 112 | ) 113 | 114 | if(CATKIN_ENABLE_TESTING) 115 | catkin_add_gtest(${PROJECT_NAME}-test_muparser 116 | test/test_muparser.cpp 117 | ) 118 | target_link_libraries(${PROJECT_NAME}-test_muparser 119 | canopen_motor 120 | ${catkin_LIBRARIES} 121 | ${MUPARSER_LIBRARIES} 122 | ) 123 | endif() 124 | -------------------------------------------------------------------------------- /socketcan_bridge/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(socketcan_bridge) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | can_msgs 7 | rosconsole_bridge 8 | roscpp 9 | socketcan_interface 10 | ) 11 | 12 | catkin_package( 13 | INCLUDE_DIRS 14 | include 15 | LIBRARIES 16 | socketcan_to_topic 17 | topic_to_socketcan 18 | CATKIN_DEPENDS 19 | can_msgs 20 | rosconsole_bridge 21 | roscpp 22 | socketcan_interface 23 | ) 24 | 25 | include_directories( 26 | include 27 | ${catkin_INCLUDE_DIRS} 28 | ) 29 | 30 | 31 | # socketcan_to_topic 32 | add_library(socketcan_to_topic 33 | src/rosconsole_bridge.cpp 34 | src/socketcan_to_topic.cpp 35 | ) 36 | target_link_libraries(socketcan_to_topic 37 | ${catkin_LIBRARIES} 38 | ) 39 | add_dependencies(socketcan_to_topic 40 | ${catkin_EXPORTED_TARGETS} 41 | ) 42 | 43 | # topic_to_socketcan 44 | add_library(topic_to_socketcan 45 | src/rosconsole_bridge.cpp 46 | src/topic_to_socketcan.cpp 47 | ) 48 | target_link_libraries(topic_to_socketcan 49 | ${catkin_LIBRARIES} 50 | ) 51 | add_dependencies(topic_to_socketcan 52 | ${catkin_EXPORTED_TARGETS} 53 | ) 54 | 55 | # socketcan_to_topic_node 56 | add_executable(socketcan_to_topic_node 57 | src/socketcan_to_topic_node.cpp 58 | ) 59 | target_link_libraries(socketcan_to_topic_node 60 | socketcan_to_topic 61 | ${catkin_LIBRARIES} 62 | ) 63 | 64 | # topic_to_socketcan_node 65 | add_executable(topic_to_socketcan_node 66 | src/topic_to_socketcan_node.cpp 67 | 68 | ) 69 | target_link_libraries(topic_to_socketcan_node 70 | topic_to_socketcan 71 | ${catkin_LIBRARIES} 72 | ) 73 | 74 | # socketcan_bridge_node 75 | add_executable(${PROJECT_NAME}_node 76 | src/${PROJECT_NAME}_node.cpp 77 | ) 78 | target_link_libraries(${PROJECT_NAME}_node 79 | topic_to_socketcan 80 | socketcan_to_topic 81 | ${catkin_LIBRARIES} 82 | ) 83 | 84 | install( 85 | TARGETS 86 | ${PROJECT_NAME}_node 87 | socketcan_to_topic 88 | socketcan_to_topic_node 89 | topic_to_socketcan 90 | topic_to_socketcan_node 91 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 92 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 93 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 94 | ) 95 | 96 | install( 97 | DIRECTORY 98 | include/${PROJECT_NAME}/ 99 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 100 | ) 101 | 102 | if(CATKIN_ENABLE_TESTING) 103 | find_package(rostest REQUIRED) 104 | find_package(roslint REQUIRED) 105 | 106 | roslint_cpp() 107 | roslint_add_test() 108 | 109 | # unit test for the can_msgs::Frame and can::Frame types. 110 | catkin_add_gtest(test_conversion 111 | test/test_conversion.cpp 112 | ) 113 | target_link_libraries(test_conversion 114 | topic_to_socketcan 115 | socketcan_to_topic 116 | ${catkin_LIBRARIES} 117 | ) 118 | 119 | 120 | add_rostest_gtest(test_to_socketcan 121 | test/to_socketcan.test 122 | test/to_socketcan_test.cpp 123 | ) 124 | target_link_libraries(test_to_socketcan 125 | topic_to_socketcan 126 | ${catkin_LIBRARIES} 127 | ) 128 | 129 | add_rostest_gtest(test_to_topic 130 | test/to_topic.test 131 | test/to_topic_test.cpp 132 | ) 133 | target_link_libraries(test_to_topic 134 | socketcan_to_topic 135 | topic_to_socketcan 136 | ${catkin_LIBRARIES} 137 | ) 138 | 139 | endif() 140 | -------------------------------------------------------------------------------- /socketcan_interface/include/socketcan_interface/threading.h: -------------------------------------------------------------------------------- 1 | #ifndef H_CAN_THREADING_BASE 2 | #define H_CAN_THREADING_BASE 3 | 4 | #include 5 | #include 6 | 7 | namespace can{ 8 | 9 | class StateWaiter{ 10 | boost::mutex mutex_; 11 | boost::condition_variable cond_; 12 | can::StateInterface::StateListenerConstSharedPtr state_listener_; 13 | can::State state_; 14 | void updateState(const can::State &s){ 15 | boost::mutex::scoped_lock lock(mutex_); 16 | state_ = s; 17 | lock.unlock(); 18 | cond_.notify_all(); 19 | } 20 | public: 21 | template StateWaiter(InterfaceType *interface){ 22 | state_ = interface->getState(); 23 | state_listener_ = interface->createStateListener(std::bind(&StateWaiter::updateState, this, std::placeholders::_1)); 24 | } 25 | template bool wait(const can::State::DriverState &s, const DurationType &duration){ 26 | boost::mutex::scoped_lock cond_lock(mutex_); 27 | boost::system_time abs_time = boost::get_system_time() + duration; 28 | while(s != state_.driver_state) 29 | { 30 | if(!cond_.timed_wait(cond_lock,abs_time)) 31 | { 32 | return false; 33 | } 34 | } 35 | return true; 36 | } 37 | }; 38 | 39 | template class ThreadedInterface : public WrappedInterface{ 40 | std::shared_ptr thread_; 41 | void run_thread(){ 42 | WrappedInterface::run(); 43 | } 44 | void shutdown_internal(){ 45 | if(thread_){ 46 | thread_->interrupt(); 47 | thread_->join(); 48 | thread_.reset(); 49 | } 50 | } 51 | public: 52 | [[deprecated("provide settings explicitly")]] bool init(const std::string &device, bool loopback) override { 53 | #pragma GCC diagnostic push 54 | #pragma GCC diagnostic ignored "-Wdeprecated-declarations" 55 | if(!thread_ && WrappedInterface::init(device, loopback)){ 56 | StateWaiter waiter(this); 57 | thread_.reset(new boost::thread(&ThreadedInterface::run_thread, this)); 58 | return waiter.wait(can::State::ready, boost::posix_time::seconds(1)); 59 | } 60 | return WrappedInterface::getState().isReady(); 61 | #pragma GCC diagnostic pop 62 | } 63 | bool init(const std::string &device, bool loopback, SettingsConstSharedPtr settings) override { 64 | if(!thread_ && WrappedInterface::init(device, loopback, settings)){ 65 | StateWaiter waiter(this); 66 | thread_.reset(new boost::thread(&ThreadedInterface::run_thread, this)); 67 | return waiter.wait(can::State::ready, boost::posix_time::seconds(1)); 68 | } 69 | return WrappedInterface::getState().isReady(); 70 | } 71 | 72 | void shutdown() override{ 73 | WrappedInterface::shutdown(); 74 | shutdown_internal(); 75 | } 76 | void join(){ 77 | if(thread_){ 78 | thread_->join(); 79 | } 80 | } 81 | virtual ~ThreadedInterface() { 82 | shutdown_internal(); 83 | } 84 | ThreadedInterface(): WrappedInterface() {} 85 | template ThreadedInterface(const T1 &t1): WrappedInterface(t1) {} 86 | template ThreadedInterface(const T1 &t1, const T2 &t2): WrappedInterface(t1, t2) {} 87 | 88 | }; 89 | 90 | 91 | } // namespace can 92 | #endif 93 | -------------------------------------------------------------------------------- /socketcan_interface/test/test_dispatcher.cpp: -------------------------------------------------------------------------------- 1 | // Bring in my package's API, which is what I'm testing 2 | #include 3 | 4 | // Bring in gtest 5 | #include 6 | 7 | class Counter { 8 | public: 9 | size_t counter_; 10 | Counter() : counter_(0) {} 11 | void count(const can::Frame &msg) { 12 | ++counter_; 13 | } 14 | }; 15 | 16 | TEST(DispatcherTest, testFilteredDispatcher) 17 | { 18 | can::FilteredDispatcher dispatcher; 19 | Counter counter1; 20 | Counter counter2; 21 | std::vector listeners; 22 | const size_t max_id = (1<<11); 23 | for(size_t i=0; i < max_id; i+=2) { 24 | listeners.push_back(dispatcher.createListener(can::MsgHeader(i), can::CommInterface::FrameDelegate(&counter1, &Counter::count))); 25 | listeners.push_back(dispatcher.createListener(can::MsgHeader(i+1), can::CommInterface::FrameDelegate(&counter2, &Counter::count))); 26 | } 27 | 28 | boost::chrono::steady_clock::time_point start = boost::chrono::steady_clock::now(); 29 | const size_t num = 1000 * max_id; 30 | for(size_t i=0; i < num; ++i) { 31 | dispatcher.dispatch(can::Frame(can::MsgHeader(i%max_id))); 32 | } 33 | boost::chrono::steady_clock::time_point now = boost::chrono::steady_clock::now(); 34 | double diff = boost::chrono::duration_cast >(now-start).count(); 35 | 36 | EXPECT_EQ(num, counter1.counter_+ counter2.counter_); 37 | EXPECT_EQ(counter1.counter_, counter2.counter_); 38 | std::cout << std::fixed << diff << "\t" << num << "\t" << num / diff << std::endl; 39 | 40 | } 41 | 42 | TEST(DispatcherTest, testSimpleDispatcher) 43 | { 44 | can::SimpleDispatcher dispatcher; 45 | Counter counter; 46 | can::CommInterface::FrameListenerConstSharedPtr listener = dispatcher.createListener(can::CommInterface::FrameDelegate(&counter, &Counter::count)); 47 | 48 | boost::chrono::steady_clock::time_point start = boost::chrono::steady_clock::now(); 49 | const size_t max_id = (1<<11); 50 | const size_t num = 1000*max_id; 51 | for(size_t i=0; i < num; ++i) { 52 | dispatcher.dispatch(can::Frame(can::MsgHeader(i%max_id))); 53 | } 54 | boost::chrono::steady_clock::time_point now = boost::chrono::steady_clock::now(); 55 | double diff = boost::chrono::duration_cast >(now-start).count(); 56 | 57 | EXPECT_EQ(num, counter.counter_); 58 | std::cout << std::fixed << diff << "\t" << num << "\t" << num / diff << std::endl; 59 | } 60 | 61 | TEST(DispatcherTest, testDelegateOnly) 62 | { 63 | Counter counter; 64 | can::CommInterface::FrameDelegate delegate(&counter, &Counter::count); 65 | 66 | boost::chrono::steady_clock::time_point start = boost::chrono::steady_clock::now(); 67 | const size_t max_id = (1<<11); 68 | const size_t num = 10000*max_id; 69 | for(size_t i=0; i < num; ++i) { 70 | delegate(can::Frame(can::MsgHeader(i%max_id))); 71 | } 72 | boost::chrono::steady_clock::time_point now = boost::chrono::steady_clock::now(); 73 | double diff = boost::chrono::duration_cast >(now-start).count(); 74 | 75 | EXPECT_EQ(num, counter.counter_); 76 | std::cout << std::fixed << diff << "\t" << num << "\t" << num / diff << std::endl; 77 | 78 | } 79 | 80 | // Run all the tests that were declared with TEST() 81 | int main(int argc, char **argv){ 82 | testing::InitGoogleTest(&argc, argv); 83 | return RUN_ALL_TESTS(); 84 | } 85 | -------------------------------------------------------------------------------- /socketcan_interface/include/socketcan_interface/bcm.h: -------------------------------------------------------------------------------- 1 | #ifndef H_CAN_BCM 2 | #define H_CAN_BCM 3 | 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | 17 | #include 18 | 19 | namespace can { 20 | 21 | class BCMsocket{ 22 | int s_; 23 | struct Message { 24 | size_t size; 25 | uint8_t *data; 26 | Message(size_t n) 27 | : size(sizeof(bcm_msg_head) + sizeof(can_frame)*n), data(new uint8_t[size]) 28 | { 29 | assert(n<=256); 30 | std::memset(data, 0, size); 31 | head().nframes = n; 32 | } 33 | bcm_msg_head& head() { 34 | return *(bcm_msg_head*)data; 35 | } 36 | template void setIVal2(T period){ 37 | long long usec = boost::chrono::duration_cast(period).count(); 38 | head().ival2.tv_sec = usec / 1000000; 39 | head().ival2.tv_usec = usec % 1000000; 40 | } 41 | void setHeader(Header header){ 42 | head().can_id = header.id | (header.is_extended?CAN_EFF_FLAG:0); 43 | } 44 | bool write(int s){ 45 | return ::write(s, data, size) > 0; 46 | } 47 | ~Message(){ 48 | delete[] data; 49 | data = 0; 50 | size = 0; 51 | } 52 | }; 53 | public: 54 | BCMsocket():s_(-1){ 55 | } 56 | bool init(const std::string &device){ 57 | s_ = socket(PF_CAN, SOCK_DGRAM, CAN_BCM); 58 | 59 | if(s_ < 0 ) return false; 60 | struct ifreq ifr; 61 | std::strcpy(ifr.ifr_name, device.c_str()); 62 | int ret = ioctl(s_, SIOCGIFINDEX, &ifr); 63 | 64 | if(ret != 0){ 65 | shutdown(); 66 | return false; 67 | } 68 | 69 | struct sockaddr_can addr = {0}; 70 | addr.can_family = AF_CAN; 71 | addr.can_ifindex = ifr.ifr_ifindex; 72 | 73 | ret = connect(s_, (struct sockaddr *)&addr, sizeof(addr)); 74 | 75 | if(ret < 0){ 76 | shutdown(); 77 | return false; 78 | } 79 | return true; 80 | } 81 | template bool startTX(DurationType period, Header header, size_t num, Frame *frames) { 82 | Message msg(num); 83 | msg.setHeader(header); 84 | msg.setIVal2(period); 85 | 86 | bcm_msg_head &head = msg.head(); 87 | 88 | head.opcode = TX_SETUP; 89 | head.flags |= SETTIMER | STARTTIMER; 90 | 91 | for(size_t i=0; i < num; ++i){ // msg nr 92 | head.frames[i].can_dlc = frames[i].dlc; 93 | head.frames[i].can_id = head.can_id; 94 | for(size_t j = 0; j < head.frames[i].can_dlc; ++j){ // byte nr 95 | head.frames[i].data[j] = frames[i].data[j]; 96 | } 97 | } 98 | return msg.write(s_); 99 | } 100 | bool stopTX(Header header){ 101 | Message msg(0); 102 | msg.head().opcode = TX_DELETE; 103 | msg.setHeader(header); 104 | return msg.write(s_); 105 | } 106 | void shutdown(){ 107 | if(s_ > 0){ 108 | close(s_); 109 | s_ = -1; 110 | } 111 | } 112 | 113 | virtual ~BCMsocket(){ 114 | shutdown(); 115 | } 116 | }; 117 | 118 | } 119 | 120 | #endif 121 | -------------------------------------------------------------------------------- /socketcan_interface/test/test_filter.cpp: -------------------------------------------------------------------------------- 1 | // Bring in my package's API, which is what I'm testing 2 | #include 3 | 4 | 5 | #include 6 | #include 7 | 8 | // Bring in gtest 9 | #include 10 | 11 | 12 | TEST(FilterTest, simpleMask) 13 | { 14 | const std::string msg1("123#"); 15 | const std::string msg2("124#"); 16 | 17 | can::FrameFilterSharedPtr f1 = can::tofilter("123"); 18 | 19 | EXPECT_TRUE(f1->pass(can::toframe(msg1))); 20 | EXPECT_FALSE(f1->pass(can::toframe(msg2))); 21 | } 22 | 23 | TEST(FilterTest, maskTests) 24 | { 25 | const std::string msg1("123#"); 26 | const std::string msg2("124#"); 27 | const std::string msg3("122#"); 28 | 29 | can::FrameFilterSharedPtr f1 = can::tofilter("123:123"); 30 | can::FrameFilterSharedPtr f2 = can::tofilter("123:ffe"); 31 | can::FrameFilterSharedPtr f3 = can::tofilter("123~123"); 32 | 33 | EXPECT_TRUE(f1->pass(can::toframe(msg1))); 34 | EXPECT_FALSE(f1->pass(can::toframe(msg2))); 35 | EXPECT_FALSE(f1->pass(can::toframe(msg3))); 36 | 37 | 38 | EXPECT_TRUE(f2->pass(can::toframe(msg1))); 39 | EXPECT_FALSE(f2->pass(can::toframe(msg2))); 40 | EXPECT_TRUE(f2->pass(can::toframe(msg3))); 41 | 42 | EXPECT_FALSE(f3->pass(can::toframe(msg1))); 43 | EXPECT_TRUE(f3->pass(can::toframe(msg2))); 44 | EXPECT_TRUE(f3->pass(can::toframe(msg3))); 45 | 46 | } 47 | 48 | TEST(FilterTest, rangeTest) 49 | { 50 | const std::string msg1("120#"); 51 | const std::string msg2("125#"); 52 | const std::string msg3("130#"); 53 | 54 | can::FrameFilterSharedPtr f1 = can::tofilter("120-120"); 55 | can::FrameFilterSharedPtr f2 = can::tofilter("120_120"); 56 | can::FrameFilterSharedPtr f3 = can::tofilter("120-125"); 57 | 58 | EXPECT_TRUE(f1->pass(can::toframe(msg1))); 59 | EXPECT_FALSE(f1->pass(can::toframe(msg2))); 60 | EXPECT_FALSE(f1->pass(can::toframe(msg3))); 61 | 62 | EXPECT_FALSE(f2->pass(can::toframe(msg1))); 63 | EXPECT_TRUE(f2->pass(can::toframe(msg2))); 64 | EXPECT_TRUE(f2->pass(can::toframe(msg3))); 65 | 66 | EXPECT_TRUE(f3->pass(can::toframe(msg1))); 67 | EXPECT_TRUE(f3->pass(can::toframe(msg2))); 68 | EXPECT_FALSE(f3->pass(can::toframe(msg3))); 69 | 70 | } 71 | 72 | class Counter { 73 | public: 74 | size_t count_; 75 | Counter(): count_(0) {} 76 | void count(const can::Frame &frame) { 77 | ++count_; 78 | } 79 | }; 80 | 81 | TEST(FilterTest, listenerTest) 82 | { 83 | 84 | Counter counter; 85 | can::DummyBus bus("listenerTest"); 86 | can::ThreadedDummyInterfaceSharedPtr dummy = std::make_shared(); 87 | dummy->init(bus.name, true, can::NoSettings::create()); 88 | 89 | 90 | can::FilteredFrameListener::FilterVector filters; 91 | filters.push_back(can::tofilter("123:FFE")); 92 | 93 | can::FrameListenerConstSharedPtr listener(new can::FilteredFrameListener(dummy,std::bind(&Counter::count, std::ref(counter), std::placeholders::_1), filters)); 94 | 95 | can::Frame f1 = can::toframe("123#"); 96 | can::Frame f2 = can::toframe("124#"); 97 | can::Frame f3 = can::toframe("122#"); 98 | 99 | dummy->send(f1); 100 | dummy->flush(); 101 | EXPECT_EQ(1, counter.count_); 102 | dummy->send(f2); 103 | dummy->flush(); 104 | EXPECT_EQ(1, counter.count_); 105 | dummy->send(f3); 106 | dummy->flush(); 107 | EXPECT_EQ(2, counter.count_); 108 | 109 | } 110 | 111 | // Run all the tests that were declared with TEST() 112 | int main(int argc, char **argv){ 113 | testing::InitGoogleTest(&argc, argv); 114 | return RUN_ALL_TESTS(); 115 | } 116 | -------------------------------------------------------------------------------- /socketcan_interface/test/test_delegates.cpp: -------------------------------------------------------------------------------- 1 | // Bring in my package's API, which is what I'm testing 2 | #include 3 | #include 4 | #include 5 | 6 | // Bring in gtest 7 | #include 8 | 9 | class Receiver { 10 | public: 11 | std::list responses; 12 | void handle(const can::Frame &f){ 13 | responses.push_back(can::tostring(f, true)); 14 | } 15 | Receiver() = default; 16 | ~Receiver() = default; 17 | 18 | private: 19 | Receiver(const Receiver&) = delete; 20 | Receiver& operator=(const Receiver&) = delete; 21 | }; 22 | 23 | Receiver g_r2; 24 | 25 | void fill_r2(const can::Frame &f){ 26 | g_r2.handle(f); 27 | } 28 | 29 | void fill_any(Receiver &r, const can::Frame &f){ 30 | r.handle(f); 31 | } 32 | 33 | TEST(DelegatesTest, testFrameDelegate) 34 | { 35 | 36 | can::DummyBus bus("testFrameDelegate"); 37 | can::ThreadedDummyInterface dummy; 38 | dummy.init(bus.name, true, can::NoSettings::create()); 39 | Receiver r1, r3, r4, r5; 40 | boost::shared_ptr r6(new Receiver()); 41 | std::shared_ptr r7(new Receiver()); 42 | 43 | can::FrameListenerConstSharedPtr l1 = dummy.createMsgListener(can::CommInterface::FrameDelegate(&r1, &Receiver::handle)); 44 | can::FrameListenerConstSharedPtr l2 = dummy.createMsgListener(can::CommInterface::FrameDelegate(fill_r2)); 45 | can::FrameListenerConstSharedPtr l3 = dummy.createMsgListener(can::CommInterface::FrameDelegate(std::bind(fill_any, std::ref(r3), std::placeholders::_1))); 46 | can::FrameListenerConstSharedPtr l4 = dummy.createMsgListener(std::bind(fill_any, std::ref(r4), std::placeholders::_1)); 47 | can::FrameListenerConstSharedPtr l5 = dummy.createMsgListener(boost::bind(fill_any, boost::ref(r5), boost::placeholders::_1)); 48 | can::FrameListenerConstSharedPtr l6 = dummy.createMsgListener(can::CommInterface::FrameDelegate(r6, &Receiver::handle)); 49 | can::FrameListenerConstSharedPtr l7 = dummy.createMsgListener(can::CommInterface::FrameDelegate(r7, &Receiver::handle)); 50 | 51 | std::list expected; 52 | dummy.send(can::toframe("0#8200")); 53 | dummy.flush(); 54 | expected.push_back("0#8200"); 55 | 56 | EXPECT_EQ(expected, r1.responses); 57 | EXPECT_EQ(expected, g_r2.responses); 58 | EXPECT_EQ(expected, r3.responses); 59 | EXPECT_EQ(expected, r4.responses); 60 | EXPECT_EQ(expected, r5.responses); 61 | EXPECT_EQ(expected, r6->responses); 62 | EXPECT_EQ(expected, r7->responses); 63 | } 64 | 65 | class BoolTest { 66 | bool ret_; 67 | public: 68 | bool test_bool() { return ret_; } 69 | BoolTest(bool ret) : ret_(ret) {} 70 | public: 71 | BoolTest(const BoolTest&) = delete; 72 | BoolTest& operator=(const BoolTest&) = delete; 73 | }; 74 | 75 | TEST(DelegatesTest, testBoolFunc) 76 | { 77 | 78 | using BoolFunc = std::function; 79 | using BoolDelegate = can::DelegateHelper; 80 | 81 | BoolDelegate d1([]() { return false; }); 82 | BoolDelegate d2([]() { return true; }); 83 | 84 | EXPECT_FALSE(d1()); 85 | EXPECT_TRUE(d2()); 86 | 87 | BoolTest b1(false); 88 | BoolTest b2(true); 89 | 90 | BoolDelegate d3(&b1, &BoolTest::test_bool); 91 | BoolDelegate d4(&b2, &BoolTest::test_bool); 92 | EXPECT_FALSE(d3()); 93 | EXPECT_TRUE(d4()); 94 | 95 | } 96 | 97 | 98 | // Run all the tests that were declared with TEST() 99 | int main(int argc, char **argv){ 100 | testing::InitGoogleTest(&argc, argv); 101 | return RUN_ALL_TESTS(); 102 | } 103 | -------------------------------------------------------------------------------- /socketcan_interface/include/socketcan_interface/reader.h: -------------------------------------------------------------------------------- 1 | #ifndef H_CAN_BUFFERED_READER 2 | #define H_CAN_BUFFERED_READER 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace can{ 12 | 13 | class BufferedReader { 14 | std::deque buffer_; 15 | boost::mutex mutex_; 16 | boost::condition_variable cond_; 17 | CommInterface::FrameListenerConstSharedPtr listener_; 18 | bool enabled_; 19 | size_t max_len_; 20 | 21 | void trim(){ 22 | if(max_len_ > 0){ 23 | while(buffer_.size() > max_len_){ 24 | ROSCANOPEN_ERROR("socketcan_interface", "buffer overflow, discarded oldest message " /*<< tostring(buffer_.front())*/); // enable message printing 25 | buffer_.pop_front(); 26 | } 27 | } 28 | } 29 | void handleFrame(const can::Frame & msg){ 30 | boost::mutex::scoped_lock lock(mutex_); 31 | if(enabled_){ 32 | buffer_.push_back(msg); 33 | trim(); 34 | cond_.notify_one(); 35 | }else{ 36 | ROSCANOPEN_WARN("socketcan_interface", "discarded message " /*<< tostring(msg)*/); // enable message printing 37 | } 38 | } 39 | public: 40 | class ScopedEnabler{ 41 | BufferedReader &reader_; 42 | bool before_; 43 | public: 44 | ScopedEnabler(BufferedReader &reader) : reader_(reader), before_(reader_.setEnabled(true)) {} 45 | ~ScopedEnabler() { reader_.setEnabled(before_); } 46 | }; 47 | 48 | BufferedReader() : enabled_(true), max_len_(0) {} 49 | BufferedReader(bool enable, size_t max_len = 0) : enabled_(enable), max_len_(max_len) {} 50 | 51 | void flush(){ 52 | boost::mutex::scoped_lock lock(mutex_); 53 | buffer_.clear(); 54 | } 55 | void setMaxLen(size_t max_len){ 56 | boost::mutex::scoped_lock lock(mutex_); 57 | max_len_ = max_len; 58 | trim(); 59 | } 60 | bool isEnabled(){ 61 | boost::mutex::scoped_lock lock(mutex_); 62 | return enabled_; 63 | } 64 | bool setEnabled(bool enabled){ 65 | boost::mutex::scoped_lock lock(mutex_); 66 | bool before = enabled_; 67 | enabled_ = enabled; 68 | return before; 69 | } 70 | void enable(){ 71 | boost::mutex::scoped_lock lock(mutex_); 72 | enabled_ = true; 73 | } 74 | 75 | void disable(){ 76 | boost::mutex::scoped_lock lock(mutex_); 77 | enabled_ = false; 78 | } 79 | 80 | void listen(CommInterfaceSharedPtr interface){ 81 | boost::mutex::scoped_lock lock(mutex_); 82 | listener_ = interface->createMsgListenerM(this, &BufferedReader::handleFrame); 83 | buffer_.clear(); 84 | } 85 | void listen(CommInterfaceSharedPtr interface, const Frame::Header& h){ 86 | boost::mutex::scoped_lock lock(mutex_); 87 | listener_ = interface->createMsgListenerM(h, this, &BufferedReader::handleFrame); 88 | buffer_.clear(); 89 | } 90 | 91 | template bool read(can::Frame * msg, const DurationType &duration){ 92 | return readUntil(msg, boost::chrono::high_resolution_clock::now() + duration); 93 | } 94 | bool readUntil(can::Frame * msg, boost::chrono::high_resolution_clock::time_point abs_time){ 95 | boost::mutex::scoped_lock lock(mutex_); 96 | 97 | while(buffer_.empty() && cond_.wait_until(lock,abs_time) != boost::cv_status::timeout) 98 | {} 99 | 100 | if(buffer_.empty()){ 101 | return false; 102 | } 103 | 104 | if(msg){ 105 | *msg = buffer_.front(); 106 | buffer_.pop_front(); 107 | } 108 | return true; 109 | } 110 | 111 | }; 112 | 113 | } // namespace can 114 | #endif 115 | -------------------------------------------------------------------------------- /socketcan_bridge/src/topic_to_socketcan.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016, Ivor Wanders 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are met: 6 | * * Redistributions of source code must retain the above copyright notice, 7 | * this list of conditions and the following disclaimer. 8 | * * Redistributions in binary form must reproduce the above copyright 9 | * notice, this list of conditions and the following disclaimer in the 10 | * documentation and/or other materials provided with the distribution. 11 | * * Neither the name of the copyright holder nor the names of its 12 | * contributors may be used to endorse or promote products derived from 13 | * this software without specific prior written permission. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | * POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | namespace socketcan_bridge 33 | { 34 | TopicToSocketCAN::TopicToSocketCAN(ros::NodeHandle* nh, ros::NodeHandle* nh_param, 35 | can::DriverInterfaceSharedPtr driver) 36 | { 37 | can_topic_ = nh->subscribe("sent_messages", nh_param->param("sent_messages_queue_size", 10), 38 | std::bind(&TopicToSocketCAN::msgCallback, this, std::placeholders::_1)); 39 | driver_ = driver; 40 | }; 41 | 42 | void TopicToSocketCAN::setup() 43 | { 44 | state_listener_ = driver_->createStateListener( 45 | std::bind(&TopicToSocketCAN::stateCallback, this, std::placeholders::_1)); 46 | }; 47 | 48 | void TopicToSocketCAN::msgCallback(const can_msgs::Frame::ConstPtr& msg) 49 | { 50 | // ROS_DEBUG("Message came from sent_messages topic"); 51 | 52 | // translate it to the socketcan frame type. 53 | 54 | can_msgs::Frame m = *msg.get(); // ROS message 55 | can::Frame f; // socketcan type 56 | 57 | // converts the can_msgs::Frame (ROS msg) to can::Frame (socketcan.h) 58 | convertMessageToSocketCAN(m, f); 59 | 60 | if (!f.isValid()) // check if the id and flags are appropriate. 61 | { 62 | // ROS_WARN("Refusing to send invalid frame: %s.", can::tostring(f, true).c_str()); 63 | // can::tostring cannot be used for dlc > 8 frames. It causes an crash 64 | // due to usage of boost::array for the data array. The should always work. 65 | ROS_ERROR("Invalid frame from topic: id: %#04x, length: %d, is_extended: %d", m.id, m.dlc, m.is_extended); 66 | return; 67 | } 68 | 69 | bool res = driver_->send(f); 70 | if (!res) 71 | { 72 | ROS_ERROR("Failed to send message: %s.", can::tostring(f, true).c_str()); 73 | } 74 | }; 75 | 76 | 77 | 78 | void TopicToSocketCAN::stateCallback(const can::State & s) 79 | { 80 | std::string err; 81 | driver_->translateError(s.internal_error, err); 82 | if (!s.internal_error) 83 | { 84 | ROS_INFO("State: %s, asio: %s", err.c_str(), s.error_code.message().c_str()); 85 | } 86 | else 87 | { 88 | ROS_ERROR("Error: %s, asio: %s", err.c_str(), s.error_code.message().c_str()); 89 | } 90 | }; 91 | }; // namespace socketcan_bridge 92 | -------------------------------------------------------------------------------- /socketcan_interface/test/test_settings.cpp: -------------------------------------------------------------------------------- 1 | // Bring in my package's API, which is what I'm testing 2 | #include 3 | #include 4 | #include 5 | 6 | // Bring in gtest 7 | #include 8 | 9 | TEST(SettingTest, socketcan_masks) 10 | { 11 | const can_err_mask_t fatal_errors = ( CAN_ERR_TX_TIMEOUT /* TX timeout (by netdevice driver) */ 12 | | CAN_ERR_BUSOFF /* bus off */ 13 | | CAN_ERR_BUSERROR /* bus error (may flood!) */ 14 | | CAN_ERR_RESTARTED /* controller restarted */ 15 | ); 16 | const can_err_mask_t report_errors = ( CAN_ERR_LOSTARB /* lost arbitration / data[0] */ 17 | | CAN_ERR_CRTL /* controller problems / data[1] */ 18 | | CAN_ERR_PROT /* protocol violations / data[2..3] */ 19 | | CAN_ERR_TRX /* transceiver status / data[4] */ 20 | | CAN_ERR_ACK /* received no ACK on transmission */ 21 | ); 22 | can::SocketCANInterface sci; 23 | 24 | // defaults 25 | sci.init("None", false, can::NoSettings::create()); 26 | EXPECT_EQ(sci.getErrorMask(), fatal_errors | report_errors); 27 | EXPECT_EQ(sci.getFatalErrorMask(), fatal_errors); 28 | 29 | // remove report-only flag 30 | auto m1 = can::SettingsMap::create(); 31 | m1->set("error_mask/CAN_ERR_LOSTARB", false); 32 | sci.init("None", false, m1); 33 | EXPECT_EQ(sci.getErrorMask(), (fatal_errors | report_errors) & (~CAN_ERR_LOSTARB)); 34 | EXPECT_EQ(sci.getFatalErrorMask(), fatal_errors); 35 | 36 | // cannot remove fatal flag from report only 37 | auto m2 = can::SettingsMap::create(); 38 | m2->set("error_mask/CAN_ERR_TX_TIMEOUT", false); 39 | sci.init("None", false, m2); 40 | EXPECT_EQ(sci.getErrorMask(), (fatal_errors | report_errors)); 41 | EXPECT_EQ(sci.getFatalErrorMask(), fatal_errors); 42 | 43 | // remove fatal flag 44 | auto m3 = can::SettingsMap::create(); 45 | m3->set("fatal_error_mask/CAN_ERR_TX_TIMEOUT", false); 46 | sci.init("None", false, m3); 47 | EXPECT_EQ(sci.getErrorMask(), (fatal_errors | report_errors) & (~CAN_ERR_TX_TIMEOUT)); 48 | EXPECT_EQ(sci.getFatalErrorMask(), fatal_errors & (~CAN_ERR_TX_TIMEOUT)); 49 | 50 | // remove fatal and report flag 51 | auto m4 = can::SettingsMap::create(); 52 | m4->set("fatal_error_mask/CAN_ERR_TX_TIMEOUT", false); 53 | m4->set("error_mask/CAN_ERR_LOSTARB", false); 54 | sci.init("None", false, m4); 55 | EXPECT_EQ(sci.getErrorMask(), (fatal_errors | report_errors) & ~(CAN_ERR_TX_TIMEOUT|CAN_ERR_LOSTARB)); 56 | EXPECT_EQ(sci.getFatalErrorMask(), fatal_errors & (~CAN_ERR_TX_TIMEOUT)); 57 | } 58 | 59 | TEST(SettingTest, xmlrpc) 60 | { 61 | XmlRpc::XmlRpcValue value; 62 | value["param"] = 1; 63 | XmlRpc::XmlRpcValue segment; 64 | segment["param"] = 2; 65 | value["segment"] = segment; 66 | XmlRpcSettings settings(value); 67 | 68 | ASSERT_TRUE(value["segment"].hasMember(std::string("param"))); 69 | 70 | int res; 71 | EXPECT_TRUE(settings.get("param", res)); 72 | EXPECT_EQ(res, 1); 73 | EXPECT_EQ(settings.get_optional("param", 0), 1); 74 | EXPECT_FALSE(settings.get("param2", res)); 75 | EXPECT_EQ(settings.get_optional("param2", 0), 0); 76 | 77 | EXPECT_TRUE(settings.get("segment/param", res)); 78 | EXPECT_EQ(res, 2); 79 | EXPECT_EQ(settings.get_optional("segment/param", 0), 2); 80 | 81 | EXPECT_FALSE(settings.get("segment/param2", res)); 82 | EXPECT_EQ(settings.get_optional("segment/param2", 0), 0); 83 | EXPECT_FALSE(settings.get("segment2/param", res)); 84 | EXPECT_EQ(settings.get_optional("segment2/param", 0), 0); 85 | } 86 | 87 | // Run all the tests that were declared with TEST() 88 | int main(int argc, char **argv){ 89 | testing::InitGoogleTest(&argc, argv); 90 | return RUN_ALL_TESTS(); 91 | } 92 | -------------------------------------------------------------------------------- /canopen_chain_node/src/sync_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | template std::string join(const T &container, const std::string &delim){ 9 | if(container.empty()) return std::string(); 10 | std::stringstream sstr; 11 | typename T::const_iterator it = container.begin(); 12 | sstr << *it; 13 | for(++it; it != container.end(); ++it){ 14 | sstr << delim << *it; 15 | } 16 | return sstr.str(); 17 | } 18 | void report_diagnostics(canopen::LayerStack &sync, diagnostic_updater::DiagnosticStatusWrapper &stat){ 19 | canopen::LayerReport r; 20 | sync.read(r); 21 | sync.diag(r); 22 | if(sync.getLayerState() !=canopen::Layer::Off && r.bounded()){ // valid 23 | stat.summary(r.get(), r.reason()); 24 | for(std::vector >::const_iterator it = r.values().begin(); it != r.values().end(); ++it){ 25 | stat.add(it->first, it->second); 26 | } 27 | if(!r.bounded()){ 28 | canopen::LayerStatus s; 29 | sync.recover(s); 30 | } 31 | }else{ 32 | stat.summary(stat.WARN, "sync not initilized"); 33 | canopen::LayerStatus s; 34 | sync.init(s); 35 | } 36 | } 37 | 38 | int main(int argc, char** argv){ 39 | ros::init(argc, argv, "canopen_sync_node"); 40 | 41 | ros::NodeHandle nh; 42 | ros::NodeHandle nh_priv("~"); 43 | 44 | ros::NodeHandle sync_nh(nh_priv, "sync"); 45 | int sync_ms; 46 | if(!sync_nh.getParam("interval_ms", sync_ms) || sync_ms <=0){ 47 | ROS_ERROR_STREAM("Sync interval "<< sync_ms << " is invalid"); 48 | return 1; 49 | } 50 | 51 | int sync_overflow = 0; 52 | if(!sync_nh.getParam("overflow", sync_overflow)){ 53 | ROS_WARN("Sync overflow was not specified, so overflow is disabled per default"); 54 | } 55 | if(sync_overflow == 1 || sync_overflow > 240){ 56 | ROS_ERROR_STREAM("Sync overflow "<< sync_overflow << " is invalid"); 57 | return 1; 58 | } 59 | 60 | 61 | std::string can_device; 62 | if(!nh_priv.getParam("bus/device",can_device)){ 63 | ROS_ERROR("Device not set"); 64 | return 1; 65 | } 66 | 67 | can::SocketCANDriverSharedPtr driver = std::make_shared(); 68 | canopen::SyncProperties sync_properties(can::MsgHeader(sync_nh.param("sync_id", 0x080)), sync_ms, sync_overflow); 69 | 70 | std::shared_ptr sync = std::make_shared(can_device, driver, sync_properties); 71 | 72 | std::vector nodes; 73 | 74 | if(sync_nh.getParam("monitored_nodes",nodes)){ 75 | sync->setMonitored(nodes); 76 | }else{ 77 | std::string msg; 78 | if(nh_priv.getParam("heartbeat/msg", msg)){ 79 | can::Frame f = can::toframe(msg); 80 | if(f.isValid() && (f.id & ~canopen::BCMsync::ALL_NODES_MASK) == canopen::BCMsync::HEARTBEAT_ID){ 81 | nodes.push_back(f.id & canopen::BCMsync::ALL_NODES_MASK); 82 | } 83 | } 84 | sync_nh.getParam("ignored_nodes",nodes); 85 | sync->setIgnored(nodes); 86 | } 87 | 88 | canopen::LayerStack stack("SyncNodeLayer"); 89 | 90 | stack.add(std::make_shared(driver, can_device, false, XmlRpcSettings::create(nh_priv, "bus"))); 91 | stack.add(sync); 92 | 93 | diagnostic_updater::Updater diag_updater(nh, nh_priv); 94 | diag_updater.setHardwareID(nh_priv.param("hardware_id", std::string("none"))); 95 | diag_updater.add("sync", std::bind(report_diagnostics,std::ref(stack), std::placeholders::_1)); 96 | 97 | ros::Timer diag_timer = nh.createTimer(ros::Duration(diag_updater.getPeriod()/2.0),std::bind(&diagnostic_updater::Updater::update, &diag_updater)); 98 | 99 | ros::spin(); 100 | return 0; 101 | } 102 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | ROS-Industrial is a community project. We welcome contributions from any source, from those who are extremely active to casual users. The following sections outline the steps on how to contribute to ROS-Industrial. It assumes there is an existing repository to which one would like to contribute (item 1 in the figure above) and one is familiar with the Git "Fork and Branch" workflow, detailed [here](http://blog.scottlowe.org/2015/01/27/using-fork-branch-git-workflow/). 2 | 3 | 1. Before any development is undertaken, a contributor would communicate a need and/or issue to the ROS-Industrial community. This can be done by submitting an issue on the appropriate GitHub repo, the [issues repo](https://github.com/ros-industrial/ros_industrial_issues), or by posting a message in the [ROS-Industrial category on ROS Discourse](//swri-ros-pkg-dev@googlegroups.com). . Doing so may save you time if similar development is underway and ensure that whatever approach you take is acceptable to the community of reviewers once it is submitted. 4 | 2. The second step (item 2) is to implement your change. If you are working on a code contribution, we highly recommend you utilize the [ROS Qt-Creator Plug-in](http://rosindustrial.org/news/2016/6/9/ros-qt-ide-plugin). Verify that your change successfully builds and passes all tests. 5 | 3. Next, push your changes to a "feature" branch in your personal fork of the repo and issue a pull request (PR)(item 3). The PR allows maintainers to review the submitted code. Before the PR can be accepted, the maintainer and contributor must agree that the contribution is implemented appropriately. This process can take several back-and-forth steps (see [example](https://github.com/ros-industrial/motoman/pull/89)). Contributors should expect to spend as much time reviewing/changing the code as on the initial implementation. This time can be minimized by communicating with the ROS-Industrial community before any contribution is made. 6 | 4. Issuing a Pull Request (PR) triggers the [Travis Continuous Integrations (CI)](https://github.com/ros-industrial/industrial_ci) step (item 4) which happens automatically in the background. The Travis CI performs several operations, and if any of the steps below fail, then the PR is marked accordingly for the maintainer. 7 | * Travis Workflow: 8 | * Installs a barebones ROS distribution on a fresh Ubuntu virtual machine. 9 | * Creates a catkin workspace and puts the repository in it. 10 | * Uses wstool to check out any from-source dependencies (i.e. other repositories). 11 | * Resolves package dependencies using rosdep (i.e. install packages using apt-get). 12 | * Compiles the catkin workspace. 13 | * Runs all available unit tests. 14 | 5. If the PR passes Travis CI and one of the maintainers is satisfied with the changes, they post a +1 as a comment on the PR (item 5). The +1 signifies that the PR is ready to be merged. All PRs require at least one +1 and pass Travis CI before it can be merged. 15 | 6. The next step (item 6) is for the PR to be merged into the main branch. This is done through the GitHub web interface by selecting the “Merge pull request” button. After the PR is merged, all status badges are updated automatically. 16 | 7. Periodically, the maintainer will release the package (item 7), which then gets sent to the [ROS Build Farm](http://wiki.ros.org/build.ros.org) for Debian creation. 17 | 8. The publishing of the released packages (item 8) is managed by OSRF and is not on a set schedule. This usually happens when all packages for a given distro are built successfully and stable. The current status for the distro kinetic can be found [here](http://repositories.ros.org/status_page/ros_kinetic_default.html) . Navigating to other distros can be done by changing the distro name in the link. 18 | 9. Once the package has been published, it is available to be installed by the developer (item 9). 19 | 10. After the install of a new version, the developer may have questions, experience issues or it may not have the necessary functionality which should all be reported on the packages GitHub repository as an issue (item 10). If an issue is identified or there is missing functionality that the developer requires, the cycle starts back at (item 2). 20 | 21 | For more details, please refer to the [ROS-I wiki](http://wiki.ros.org/Industrial/DevProcess). 22 | -------------------------------------------------------------------------------- /socketcan_interface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(socketcan_interface) 3 | 4 | find_package(catkin REQUIRED 5 | COMPONENTS 6 | class_loader 7 | ) 8 | 9 | find_package(console_bridge REQUIRED) 10 | 11 | find_package(Boost REQUIRED 12 | COMPONENTS 13 | chrono 14 | system 15 | thread 16 | ) 17 | 18 | find_package(Threads REQUIRED) 19 | 20 | catkin_package( 21 | INCLUDE_DIRS 22 | include 23 | LIBRARIES 24 | ${PROJECT_NAME}_string 25 | CATKIN_DEPENDS 26 | DEPENDS 27 | Boost 28 | console_bridge 29 | ) 30 | 31 | include_directories( 32 | include 33 | ${Boost_INCLUDE_DIRS} 34 | ${catkin_INCLUDE_DIRS} 35 | ${console_bridge_INCLUDE_DIRS} 36 | ) 37 | 38 | # ${PROJECT_NAME}_string 39 | add_library(${PROJECT_NAME}_string 40 | src/string.cpp 41 | ) 42 | 43 | # socketcan_dump 44 | add_executable(socketcan_dump 45 | src/candump.cpp 46 | ) 47 | 48 | target_link_libraries(socketcan_dump 49 | ${PROJECT_NAME}_string 50 | ${console_bridge_LIBRARIES} 51 | ${catkin_LIBRARIES} 52 | ${Boost_LIBRARIES} 53 | ${CMAKE_THREAD_LIBS_INIT} 54 | ) 55 | 56 | # socketcan_bcm 57 | add_executable(socketcan_bcm 58 | src/canbcm.cpp 59 | ) 60 | 61 | target_link_libraries(socketcan_bcm 62 | ${PROJECT_NAME}_string 63 | ${console_bridge_LIBRARIES} 64 | ${catkin_LIBRARIES} 65 | ${Boost_LIBRARIES} 66 | ) 67 | 68 | # ${PROJECT_NAME}_plugin 69 | add_library(${PROJECT_NAME}_plugin 70 | src/${PROJECT_NAME}_plugin.cpp 71 | ) 72 | target_link_libraries(${PROJECT_NAME}_plugin 73 | ${console_bridge_LIBRARIES} 74 | ${catkin_LIBRARIES} 75 | ${Boost_LIBRARIES} 76 | ) 77 | 78 | install( 79 | TARGETS 80 | socketcan_bcm 81 | socketcan_dump 82 | ${PROJECT_NAME}_plugin 83 | ${PROJECT_NAME}_string 84 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 85 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 86 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 87 | ) 88 | 89 | install( 90 | DIRECTORY 91 | include/${PROJECT_NAME}/ 92 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 93 | FILES_MATCHING PATTERN "*.h" 94 | ) 95 | 96 | install( 97 | FILES 98 | ${PROJECT_NAME}_plugin.xml 99 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 100 | ) 101 | if(CATKIN_ENABLE_TESTING) 102 | find_package(xmlrpcpp REQUIRED) 103 | 104 | catkin_add_gtest(${PROJECT_NAME}-test_dummy_interface 105 | test/test_dummy_interface.cpp 106 | ) 107 | target_link_libraries(${PROJECT_NAME}-test_dummy_interface 108 | ${PROJECT_NAME}_string 109 | ${console_bridge_LIBRARIES} 110 | ${catkin_LIBRARIES} 111 | ${Boost_LIBRARIES} 112 | ) 113 | 114 | catkin_add_gtest(${PROJECT_NAME}-test_delegates 115 | test/test_delegates.cpp 116 | ) 117 | target_link_libraries(${PROJECT_NAME}-test_delegates 118 | ${PROJECT_NAME}_string 119 | ${catkin_LIBRARIES} 120 | ${Boost_LIBRARIES} 121 | ) 122 | 123 | catkin_add_gtest(${PROJECT_NAME}-test_settings 124 | test/test_settings.cpp 125 | ) 126 | target_include_directories(${PROJECT_NAME}-test_settings PRIVATE 127 | ${xmlrpcpp_INCLUDE_DIRS} 128 | ) 129 | target_link_libraries(${PROJECT_NAME}-test_settings 130 | ${PROJECT_NAME}_string 131 | ${catkin_LIBRARIES} 132 | ${xmlrpcpp_LIBRARIES} 133 | ) 134 | 135 | catkin_add_gtest(${PROJECT_NAME}-test_string 136 | test/test_string.cpp 137 | ) 138 | target_link_libraries(${PROJECT_NAME}-test_string 139 | ${PROJECT_NAME}_string 140 | ${console_bridge_LIBRARIES} 141 | ${catkin_LIBRARIES} 142 | ) 143 | 144 | catkin_add_gtest(${PROJECT_NAME}-test_filter 145 | test/test_filter.cpp 146 | ) 147 | target_link_libraries(${PROJECT_NAME}-test_filter 148 | ${PROJECT_NAME}_string 149 | ${console_bridge_LIBRARIES} 150 | ${catkin_LIBRARIES} 151 | ${Boost_LIBRARIES} 152 | ) 153 | 154 | catkin_add_gtest(${PROJECT_NAME}-test_dispatcher 155 | test/test_dispatcher.cpp 156 | ) 157 | target_link_libraries(${PROJECT_NAME}-test_dispatcher 158 | ${PROJECT_NAME}_string 159 | ${catkin_LIBRARIES} 160 | ${Boost_LIBRARIES} 161 | ) 162 | target_compile_options(${PROJECT_NAME}-test_dispatcher PRIVATE -Wno-deprecated-declarations) 163 | endif() 164 | -------------------------------------------------------------------------------- /socketcan_interface/include/socketcan_interface/asio_base.h: -------------------------------------------------------------------------------- 1 | #ifndef H_ASIO_BASE 2 | #define H_ASIO_BASE 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace can{ 12 | 13 | 14 | template class AsioDriver : public DriverInterface{ 15 | using FrameDispatcher = FilteredDispatcher; 16 | using StateDispatcher = SimpleDispatcher; 17 | FrameDispatcher frame_dispatcher_; 18 | StateDispatcher state_dispatcher_; 19 | 20 | State state_; 21 | boost::mutex state_mutex_; 22 | boost::mutex socket_mutex_; 23 | 24 | void shutdown_internal(){ 25 | if(socket_.is_open()){ 26 | socket_.cancel(); 27 | socket_.close(); 28 | } 29 | io_service_.stop(); 30 | } 31 | 32 | protected: 33 | boost::asio::io_service io_service_; 34 | #if BOOST_ASIO_VERSION >= 101200 // Boost 1.66+ 35 | boost::asio::io_context::strand strand_; 36 | #else 37 | boost::asio::strand strand_; 38 | #endif 39 | Socket socket_; 40 | Frame input_; 41 | 42 | virtual void triggerReadSome() = 0; 43 | virtual bool enqueue(const Frame & msg) = 0; 44 | 45 | void dispatchFrame(const Frame &msg){ 46 | strand_.post([this, msg]{ frame_dispatcher_.dispatch(msg.key(), msg);} ); // copies msg 47 | } 48 | void setErrorCode(const boost::system::error_code& error){ 49 | boost::mutex::scoped_lock lock(state_mutex_); 50 | if(state_.error_code != error){ 51 | state_.error_code = error; 52 | state_dispatcher_.dispatch(state_); 53 | } 54 | } 55 | void setInternalError(unsigned int internal_error){ 56 | boost::mutex::scoped_lock lock(state_mutex_); 57 | if(state_.internal_error != internal_error){ 58 | state_.internal_error = internal_error; 59 | state_dispatcher_.dispatch(state_); 60 | } 61 | } 62 | 63 | void setDriverState(State::DriverState state){ 64 | boost::mutex::scoped_lock lock(state_mutex_); 65 | if(state_.driver_state != state){ 66 | state_.driver_state = state; 67 | state_dispatcher_.dispatch(state_); 68 | } 69 | } 70 | void setNotReady(){ 71 | setDriverState(socket_.is_open()?State::open : State::closed); 72 | } 73 | 74 | void frameReceived(const boost::system::error_code& error){ 75 | if(!error){ 76 | dispatchFrame(input_); 77 | triggerReadSome(); 78 | }else{ 79 | setErrorCode(error); 80 | setNotReady(); 81 | } 82 | } 83 | 84 | AsioDriver() 85 | : strand_(io_service_), socket_(io_service_) 86 | {} 87 | 88 | public: 89 | virtual ~AsioDriver() { shutdown_internal(); } 90 | 91 | State getState(){ 92 | boost::mutex::scoped_lock lock(state_mutex_); 93 | return state_; 94 | } 95 | virtual void run(){ 96 | setNotReady(); 97 | 98 | if(getState().driver_state == State::open){ 99 | io_service_.reset(); 100 | boost::asio::io_service::work work(io_service_); 101 | setDriverState(State::ready); 102 | 103 | boost::thread post_thread([this]() { io_service_.run(); }); 104 | 105 | triggerReadSome(); 106 | 107 | boost::system::error_code ec; 108 | io_service_.run(ec); 109 | setErrorCode(ec); 110 | 111 | setNotReady(); 112 | } 113 | state_dispatcher_.dispatch(getState()); 114 | } 115 | virtual bool send(const Frame & msg){ 116 | return getState().driver_state == State::ready && enqueue(msg); 117 | } 118 | 119 | virtual void shutdown(){ 120 | shutdown_internal(); 121 | } 122 | 123 | virtual FrameListenerConstSharedPtr createMsgListener(const FrameFunc &delegate){ 124 | return frame_dispatcher_.createListener(delegate); 125 | } 126 | virtual FrameListenerConstSharedPtr createMsgListener(const Frame::Header&h , const FrameFunc &delegate){ 127 | return frame_dispatcher_.createListener(h.key(), delegate); 128 | } 129 | virtual StateListenerConstSharedPtr createStateListener(const StateFunc &delegate){ 130 | return state_dispatcher_.createListener(delegate); 131 | } 132 | 133 | }; 134 | 135 | } // namespace can 136 | #endif 137 | -------------------------------------------------------------------------------- /canopen_master/include/canopen_master/can_layer.h: -------------------------------------------------------------------------------- 1 | #ifndef H_CAN_LAYER 2 | #define H_CAN_LAYER 3 | 4 | #include 5 | #include "layer.h" 6 | #include 7 | 8 | namespace canopen{ 9 | 10 | class CANLayer: public Layer{ 11 | boost::mutex mutex_; 12 | can::DriverInterfaceSharedPtr driver_; 13 | const std::string device_; 14 | const bool loopback_; 15 | can::SettingsConstSharedPtr settings_; 16 | can::Frame last_error_; 17 | can::FrameListenerConstSharedPtr error_listener_; 18 | void handleFrame(const can::Frame & msg){ 19 | boost::mutex::scoped_lock lock(mutex_); 20 | last_error_ = msg; 21 | ROSCANOPEN_ERROR("canopen_master", "Received error frame: " << msg); 22 | } 23 | std::shared_ptr thread_; 24 | 25 | public: 26 | CANLayer(const can::DriverInterfaceSharedPtr &driver, const std::string &device, bool loopback, can::SettingsConstSharedPtr settings) 27 | : Layer(device + " Layer"), driver_(driver), device_(device), loopback_(loopback), settings_(settings) { assert(driver_); } 28 | 29 | [[deprecated("provide settings explicitly")]] CANLayer(const can::DriverInterfaceSharedPtr &driver, const std::string &device, bool loopback) 30 | : Layer(device + " Layer"), driver_(driver), device_(device), loopback_(loopback), settings_(can::NoSettings::create()) { assert(driver_); } 31 | 32 | virtual void handleRead(LayerStatus &status, const LayerState ¤t_state) { 33 | if(current_state > Init){ 34 | if(!driver_->getState().isReady()) status.error("CAN not ready"); 35 | } 36 | } 37 | virtual void handleWrite(LayerStatus &status, const LayerState ¤t_state) { 38 | if(current_state > Init){ 39 | if(!driver_->getState().isReady()) status.error("CAN not ready"); 40 | } 41 | } 42 | 43 | virtual void handleDiag(LayerReport &report){ 44 | can::State s = driver_->getState(); 45 | if(!s.isReady()){ 46 | report.error("CAN layer not ready"); 47 | report.add("driver_state", int(s.driver_state)); 48 | } 49 | if(s.error_code){ 50 | report.add("socket_error", s.error_code); 51 | } 52 | if(s.internal_error != 0){ 53 | report.add("internal_error", int(s.internal_error)); 54 | std::string desc; 55 | if(driver_->translateError(s.internal_error, desc)) report.add("internal_error_desc", desc); 56 | std::stringstream sstr; 57 | sstr << std::hex; 58 | { 59 | boost::mutex::scoped_lock lock(mutex_); 60 | for(size_t i=0; i < last_error_.dlc; ++i){ 61 | sstr << (unsigned int) last_error_.data[i] << " "; 62 | } 63 | } 64 | report.add("can_error_frame", sstr.str()); 65 | 66 | } 67 | 68 | } 69 | 70 | virtual void handleInit(LayerStatus &status){ 71 | if(thread_){ 72 | status.warn("CAN thread already running"); 73 | } else if(!driver_->init(device_, loopback_, settings_)) { 74 | status.error("CAN init failed"); 75 | } else { 76 | can::StateWaiter waiter(driver_.get()); 77 | 78 | thread_.reset(new boost::thread(&can::DriverInterface::run, driver_)); 79 | error_listener_ = driver_->createMsgListenerM(can::ErrorHeader(),this, &CANLayer::handleFrame); 80 | 81 | if(!waiter.wait(can::State::ready, boost::posix_time::seconds(1))){ 82 | status.error("CAN init timed out"); 83 | } 84 | } 85 | if(!driver_->getState().isReady()){ 86 | status.error("CAN is not ready"); 87 | } 88 | } 89 | virtual void handleShutdown(LayerStatus &status){ 90 | can::StateWaiter waiter(driver_.get()); 91 | error_listener_.reset(); 92 | driver_->shutdown(); 93 | if(!waiter.wait(can::State::closed, boost::posix_time::seconds(1))){ 94 | status.warn("CAN shutdown timed out"); 95 | } 96 | if(thread_){ 97 | thread_->interrupt(); 98 | thread_->join(); 99 | thread_.reset(); 100 | } 101 | } 102 | 103 | virtual void handleHalt(LayerStatus &status) { /* nothing to do */ } 104 | 105 | virtual void handleRecover(LayerStatus &status){ 106 | if(!driver_->getState().isReady()){ 107 | handleShutdown(status); 108 | handleInit(status); 109 | } 110 | } 111 | 112 | }; 113 | } // namespace canopen 114 | 115 | #endif 116 | -------------------------------------------------------------------------------- /socketcan_bridge/test/test_conversion.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016, Ivor Wanders 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are met: 6 | * * Redistributions of source code must retain the above copyright notice, 7 | * this list of conditions and the following disclaimer. 8 | * * Redistributions in binary form must reproduce the above copyright 9 | * notice, this list of conditions and the following disclaimer in the 10 | * documentation and/or other materials provided with the distribution. 11 | * * Neither the name of the copyright holder nor the names of its 12 | * contributors may be used to endorse or promote products derived from 13 | * this software without specific prior written permission. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | * POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | 33 | // Bring in gtest 34 | #include 35 | 36 | // test whether the content of a conversion from a SocketCAN frame 37 | // to a ROS message correctly maintains the data. 38 | TEST(ConversionTest, socketCANToTopicStandard) 39 | { 40 | can::Frame f; 41 | can_msgs::Frame m; 42 | f.id = 127; 43 | f.dlc = 8; 44 | f.is_error = false; 45 | f.is_rtr = false; 46 | f.is_extended = false; 47 | for (uint8_t i = 0; i < f.dlc; ++i) 48 | { 49 | f.data[i] = i; 50 | } 51 | socketcan_bridge::convertSocketCANToMessage(f, m); 52 | EXPECT_EQ(127, m.id); 53 | EXPECT_EQ(8, m.dlc); 54 | EXPECT_EQ(false, m.is_error); 55 | EXPECT_EQ(false, m.is_rtr); 56 | EXPECT_EQ(false, m.is_extended); 57 | 58 | for (uint8_t i=0; i < 8; i++) 59 | { 60 | EXPECT_EQ(i, m.data[i]); 61 | } 62 | } 63 | 64 | // test all three flags seperately. 65 | TEST(ConversionTest, socketCANToTopicFlags) 66 | { 67 | can::Frame f; 68 | can_msgs::Frame m; 69 | 70 | f.is_error = true; 71 | socketcan_bridge::convertSocketCANToMessage(f, m); 72 | EXPECT_EQ(true, m.is_error); 73 | f.is_error = false; 74 | 75 | f.is_rtr = true; 76 | socketcan_bridge::convertSocketCANToMessage(f, m); 77 | EXPECT_EQ(true, m.is_rtr); 78 | f.is_rtr = false; 79 | 80 | f.is_extended = true; 81 | socketcan_bridge::convertSocketCANToMessage(f, m); 82 | EXPECT_EQ(true, m.is_extended); 83 | f.is_extended = false; 84 | } 85 | 86 | // idem, but the other way around. 87 | TEST(ConversionTest, topicToSocketCANStandard) 88 | { 89 | can::Frame f; 90 | can_msgs::Frame m; 91 | m.id = 127; 92 | m.dlc = 8; 93 | m.is_error = false; 94 | m.is_rtr = false; 95 | m.is_extended = false; 96 | for (uint8_t i = 0; i < m.dlc; ++i) 97 | { 98 | m.data[i] = i; 99 | } 100 | socketcan_bridge::convertMessageToSocketCAN(m, f); 101 | EXPECT_EQ(127, f.id); 102 | EXPECT_EQ(8, f.dlc); 103 | EXPECT_EQ(false, f.is_error); 104 | EXPECT_EQ(false, f.is_rtr); 105 | EXPECT_EQ(false, f.is_extended); 106 | 107 | 108 | for (uint8_t i=0; i < 8; i++) 109 | { 110 | EXPECT_EQ(i, f.data[i]); 111 | } 112 | } 113 | 114 | TEST(ConversionTest, topicToSocketCANFlags) 115 | { 116 | can::Frame f; 117 | can_msgs::Frame m; 118 | 119 | m.is_error = true; 120 | socketcan_bridge::convertMessageToSocketCAN(m, f); 121 | EXPECT_EQ(true, f.is_error); 122 | m.is_error = false; 123 | 124 | m.is_rtr = true; 125 | socketcan_bridge::convertMessageToSocketCAN(m, f); 126 | EXPECT_EQ(true, f.is_rtr); 127 | m.is_rtr = false; 128 | 129 | m.is_extended = true; 130 | socketcan_bridge::convertMessageToSocketCAN(m, f); 131 | EXPECT_EQ(true, f.is_extended); 132 | m.is_extended = false; 133 | } 134 | 135 | // Run all the tests that were declared with TEST() 136 | int main(int argc, char **argv) 137 | { 138 | testing::InitGoogleTest(&argc, argv); 139 | return RUN_ALL_TESTS(); 140 | } 141 | -------------------------------------------------------------------------------- /socketcan_interface/src/string.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | namespace can { 5 | 6 | bool hex2dec(uint8_t& d, const char& h) { 7 | if ('0' <= h && h <= '9') 8 | d = h - '0'; 9 | else if ('a' <= h && h <= 'f') 10 | d = h - 'a' + 10; 11 | else if ('A' <= h && h <= 'F') 12 | d = h - 'A' + 10; 13 | else 14 | return false; 15 | 16 | return true; 17 | } 18 | 19 | bool hex2buffer(std::string& out, const std::string& in_raw, bool pad) { 20 | std::string in(in_raw); 21 | if ((in.size() % 2) != 0) { 22 | if (pad) 23 | in.insert(0, "0"); 24 | else 25 | return false; 26 | } 27 | out.resize(in.size() >> 1); 28 | for (size_t i = 0; i < out.size(); ++i) { 29 | uint8_t hi, lo; 30 | if (!hex2dec(hi, in[i << 1]) || !hex2dec(lo, in[(i << 1) + 1])) 31 | return false; 32 | 33 | out[i] = (hi << 4) | lo; 34 | } 35 | return true; 36 | } 37 | 38 | bool dec2hex(char& h, const uint8_t& d, bool lc) { 39 | if (d < 10) { 40 | h = '0' + d; 41 | } else if (d < 16 && lc) { 42 | h = 'a' + (d - 10); 43 | } else if (d < 16 && !lc) { 44 | h = 'A' + (d - 10); 45 | } else { 46 | h='?'; 47 | return false; 48 | } 49 | return true; 50 | } 51 | 52 | std::string byte2hex(const uint8_t& d, bool pad, bool lc) { 53 | uint8_t hi = d >> 4; 54 | char c=0; 55 | std::string s; 56 | if (hi || pad) { 57 | dec2hex(c, hi, lc); 58 | s += c; 59 | } 60 | dec2hex(c, d & 0xf, lc); 61 | s += c; 62 | return s; 63 | } 64 | 65 | std::string buffer2hex(const std::string& in, bool lc) { 66 | std::string s; 67 | s.reserve(in.size() * 2); 68 | for (size_t i = 0; i < in.size(); ++i) { 69 | std::string b = byte2hex(in[i], true, lc); 70 | if (b.empty()) 71 | return b; 72 | 73 | s += b; 74 | } 75 | return s; 76 | } 77 | 78 | std::string tostring(const Header& h, bool lc) { 79 | std::string s; 80 | s.reserve(8); 81 | std::stringstream buf; 82 | buf << std::hex; 83 | if (lc) 84 | buf << std::nouppercase; 85 | else 86 | buf << std::uppercase; 87 | 88 | if (h.is_extended) 89 | buf << std::setfill('0') << std::setw(8); 90 | 91 | buf << (h.fullid() & ~Header::EXTENDED_MASK); 92 | return buf.str(); 93 | } 94 | 95 | uint32_t tohex(const std::string& s) { 96 | unsigned int h = 0; 97 | std::stringstream stream; 98 | stream << std::hex << s; 99 | stream >> h; 100 | return h; 101 | } 102 | 103 | Header toheader(const std::string& s) { 104 | unsigned int h = tohex(s); 105 | unsigned int id = h & Header::ID_MASK; 106 | return Header(id, h & Header::EXTENDED_MASK || (s.size() == 8 && id >= (1<<11)), 107 | h & Header::RTR_MASK, h & Header::ERROR_MASK); 108 | } 109 | 110 | std::string tostring(const Frame& f, bool lc) { 111 | std::string s; 112 | s.resize(f.dlc); 113 | for (uint8_t i = 0; i < f.dlc; ++i) { 114 | s[i] = f.data[i]; 115 | } 116 | return tostring((const Header&) (f), lc) + '#' + buffer2hex(s, lc); 117 | } 118 | 119 | Frame toframe(const std::string& s) { 120 | size_t sep = s.find('#'); 121 | if (sep == std::string::npos) 122 | return MsgHeader(0xfff); 123 | 124 | Header header = toheader(s.substr(0, sep)); 125 | Frame frame(header); 126 | std::string buffer; 127 | if (header.isValid() && hex2buffer(buffer, s.substr(sep + 1), false)) { 128 | if (buffer.size() > 8) 129 | return MsgHeader(0xfff); 130 | 131 | for (size_t i = 0; i < buffer.size(); ++i) { 132 | frame.data[i] = buffer[i]; 133 | } 134 | frame.dlc = buffer.size(); 135 | } 136 | return frame; 137 | } 138 | 139 | template<> FrameFilterSharedPtr tofilter(const std::string &s){ 140 | FrameFilter * filter = 0; 141 | size_t delim = s.find_first_of(":~-_"); 142 | 143 | uint32_t second = FrameMaskFilter::MASK_RELAXED; 144 | bool invert = false; 145 | char type = ':'; 146 | 147 | if(delim != std::string::npos) { 148 | type = s.at(delim); 149 | second = tohex(s.substr(delim +1)); 150 | } 151 | uint32_t first = toheader(s.substr(0, delim)).fullid(); 152 | switch (type) { 153 | case '~': 154 | invert = true; 155 | case ':': 156 | filter = new FrameMaskFilter(first, second, invert); 157 | break; 158 | case '_': 159 | invert = true; 160 | case '-': 161 | filter = new FrameRangeFilter(first, second, invert); 162 | break; 163 | } 164 | return FrameFilterSharedPtr(filter); 165 | } 166 | template<> FrameFilterSharedPtr tofilter(const uint32_t &id){ 167 | return FrameFilterSharedPtr(new FrameMaskFilter(id)); 168 | } 169 | 170 | FrameFilterSharedPtr tofilter(const char* s){ 171 | return tofilter(s); 172 | } 173 | 174 | std::ostream& operator <<(std::ostream& stream, const Header& h) { 175 | return stream << can::tostring(h, true); 176 | } 177 | 178 | std::ostream& operator <<(std::ostream& stream, const Frame& f) { 179 | return stream << can::tostring(f, true); 180 | } 181 | 182 | } 183 | -------------------------------------------------------------------------------- /canopen_master/src/emcy.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | using namespace canopen; 5 | 6 | #pragma pack(push) /* push current alignment to stack */ 7 | #pragma pack(1) /* set alignment to 1 byte boundary */ 8 | 9 | struct EMCYid{ 10 | uint32_t id:29; 11 | uint32_t extended:1; 12 | uint32_t :1; 13 | uint32_t invalid:1; 14 | EMCYid(uint32_t val){ 15 | *(uint32_t*) this = val; 16 | } 17 | can::Header header() { 18 | return can::Header(id, extended, false, false); 19 | } 20 | const uint32_t get() const { return *(uint32_t*) this; } 21 | }; 22 | 23 | struct EMCYfield{ 24 | uint32_t error_code:16; 25 | uint32_t addition_info:16; 26 | EMCYfield(uint32_t val){ 27 | *(uint32_t*) this = val; 28 | } 29 | }; 30 | 31 | struct EMCYmsg{ 32 | uint16_t error_code; 33 | uint8_t error_register; 34 | uint8_t manufacturer_specific_error_field[5]; 35 | 36 | struct Frame: public FrameOverlay{ 37 | Frame(const can::Frame &f) : FrameOverlay(f){ } 38 | }; 39 | }; 40 | 41 | #pragma pack(pop) /* pop previous alignment from stack */ 42 | 43 | void EMCYHandler::handleEMCY(const can::Frame & msg){ 44 | EMCYmsg::Frame em(msg); 45 | if (em.data.error_code == 0) { 46 | ROSCANOPEN_INFO("canopen_master", "EMCY reset: " << msg); 47 | } else { 48 | ROSCANOPEN_ERROR("canopen_master", "EMCY received: " << msg); 49 | } 50 | has_error_ = (em.data.error_register & ~32) != 0; 51 | } 52 | 53 | EMCYHandler::EMCYHandler(const can::CommInterfaceSharedPtr interface, const ObjectStorageSharedPtr storage): Layer("EMCY handler"), storage_(storage), has_error_(true){ 54 | storage_->entry(error_register_, 0x1001); 55 | try{ 56 | storage_->entry(num_errors_, 0x1003,0); 57 | } 58 | catch(...){ 59 | // pass, 1003 is optional 60 | } 61 | try{ 62 | EMCYid emcy_id(storage_->entry(0x1014).get_cached()); 63 | emcy_listener_ = interface->createMsgListenerM(emcy_id.header(), this, &EMCYHandler::handleEMCY); 64 | 65 | 66 | } 67 | catch(...){ 68 | // pass, EMCY is optional 69 | } 70 | } 71 | 72 | void EMCYHandler::handleRead(LayerStatus &status, const LayerState ¤t_state) { 73 | if(current_state == Ready){ 74 | if(has_error_){ 75 | status.error("Node has emergency error"); 76 | } 77 | } 78 | } 79 | void EMCYHandler::handleWrite(LayerStatus &status, const LayerState ¤t_state) { 80 | // noithing to do 81 | } 82 | 83 | void EMCYHandler::handleDiag(LayerReport &report){ 84 | uint8_t error_register = 0; 85 | if(!error_register_.get(error_register)){ 86 | report.error("Could not read error error_register"); 87 | return; 88 | } 89 | 90 | if(error_register){ 91 | if(error_register & 1){ // first bit should be set on all errors 92 | report.error("Node has emergency error"); 93 | }else if(error_register & ~32){ // filter profile-specific bit 94 | report.warn("Error register is not zero"); 95 | } 96 | report.add("error_register", (uint32_t) error_register); 97 | 98 | uint8_t num = num_errors_.valid() ? num_errors_.get() : 0; 99 | std::stringstream buf; 100 | for(size_t i = 0; i < num; ++i) { 101 | if( i!= 0){ 102 | buf << ", "; 103 | } 104 | try{ 105 | ObjectStorage::Entry error; 106 | storage_->entry(error, 0x1003,i+1); 107 | EMCYfield field(error.get()); 108 | buf << std::hex << field.error_code << "#" << field.addition_info; 109 | } 110 | catch (const std::out_of_range & e){ 111 | buf << "NOT_IN_DICT!"; 112 | } 113 | catch (const TimeoutException & e){ 114 | buf << "LIST_UNDERFLOW!"; 115 | break; 116 | } 117 | 118 | } 119 | report.add("errors", buf.str()); 120 | 121 | } 122 | } 123 | void EMCYHandler::handleInit(LayerStatus &status){ 124 | uint8_t error_register = 0; 125 | if(!error_register_.get(error_register)){ 126 | status.error("Could not read error error_register"); 127 | return; 128 | }else if(error_register & 1){ 129 | ROSCANOPEN_ERROR("canopen_master", "error register: " << int(error_register)); 130 | status.error("Node has emergency error"); 131 | return; 132 | } 133 | 134 | resetErrors(status); 135 | } 136 | void EMCYHandler::resetErrors(LayerStatus &status){ 137 | if(num_errors_.valid()) num_errors_.set(0); 138 | has_error_ = false; 139 | } 140 | 141 | void EMCYHandler::handleRecover(LayerStatus &status){ 142 | handleInit(status); 143 | } 144 | void EMCYHandler::handleShutdown(LayerStatus &status){ 145 | } 146 | void EMCYHandler::handleHalt(LayerStatus &status){ 147 | // do nothing 148 | } 149 | -------------------------------------------------------------------------------- /socketcan_interface/include/socketcan_interface/dispatcher.h: -------------------------------------------------------------------------------- 1 | #ifndef H_CAN_DISPATCHER 2 | #define H_CAN_DISPATCHER 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | namespace can{ 13 | 14 | template< typename Listener > class SimpleDispatcher{ 15 | public: 16 | using Callable = typename Listener::Callable; 17 | using Type = typename Listener::Type; 18 | using ListenerConstSharedPtr = typename Listener::ListenerConstSharedPtr; 19 | protected: 20 | class DispatcherBase; 21 | using DispatcherBaseSharedPtr = std::shared_ptr; 22 | class DispatcherBase { 23 | DispatcherBase(const DispatcherBase&) = delete; // prevent copies 24 | DispatcherBase& operator=(const DispatcherBase&) = delete; 25 | 26 | class GuardedListener: public Listener{ 27 | std::weak_ptr guard_; 28 | public: 29 | GuardedListener(DispatcherBaseSharedPtr g, const Callable &callable): Listener(callable), guard_(g){} 30 | virtual ~GuardedListener() { 31 | DispatcherBaseSharedPtr d = guard_.lock(); 32 | if(d){ 33 | d->remove(this); 34 | } 35 | } 36 | }; 37 | 38 | boost::mutex &mutex_; 39 | std::list listeners_; 40 | public: 41 | DispatcherBase(boost::mutex &mutex) : mutex_(mutex) {} 42 | void dispatch_nolock(const Type &obj, const Listener* loopback=nullptr) const{ 43 | for(typename std::list::const_iterator it=listeners_.begin(); it != listeners_.end(); ++it){ 44 | if (loopback != *it) { 45 | (**it)(obj); 46 | } 47 | } 48 | } 49 | void remove(Listener *d){ 50 | boost::mutex::scoped_lock lock(mutex_); 51 | listeners_.remove(d); 52 | } 53 | size_t numListeners(){ 54 | boost::mutex::scoped_lock lock(mutex_); 55 | return listeners_.size(); 56 | } 57 | 58 | static ListenerConstSharedPtr createListener(DispatcherBaseSharedPtr dispatcher, const Callable &callable){ 59 | ListenerConstSharedPtr l(new GuardedListener(dispatcher,callable)); 60 | dispatcher->listeners_.push_back(l.get()); 61 | return l; 62 | } 63 | }; 64 | boost::mutex mutex_; 65 | DispatcherBaseSharedPtr dispatcher_; 66 | public: 67 | SimpleDispatcher() : dispatcher_(new DispatcherBase(mutex_)) {} 68 | ListenerConstSharedPtr createListener(const Callable &callable){ 69 | boost::mutex::scoped_lock lock(mutex_); 70 | return DispatcherBase::createListener(dispatcher_, callable); 71 | } 72 | void dispatch(const Type &obj){ 73 | boost::mutex::scoped_lock lock(mutex_); 74 | dispatcher_->dispatch_nolock(obj); 75 | } 76 | void dispatch_filtered(const Type &obj, ListenerConstSharedPtr without){ 77 | boost::mutex::scoped_lock lock(mutex_); 78 | dispatcher_->dispatch_nolock(obj, without.get()); 79 | } 80 | size_t numListeners(){ 81 | return dispatcher_->numListeners(); 82 | } 83 | operator Callable() { return Callable(this,&SimpleDispatcher::dispatch); } 84 | }; 85 | 86 | template > class FilteredDispatcher: public SimpleDispatcher{ 87 | using BaseClass = SimpleDispatcher; 88 | std::unordered_map filtered_; 89 | public: 90 | using BaseClass::createListener; 91 | typename BaseClass::ListenerConstSharedPtr createListener(const K &key, const typename BaseClass::Callable &callable){ 92 | boost::mutex::scoped_lock lock(BaseClass::mutex_); 93 | typename BaseClass::DispatcherBaseSharedPtr &ptr = filtered_[key]; 94 | if(!ptr) ptr.reset(new typename BaseClass::DispatcherBase(BaseClass::mutex_)); 95 | return BaseClass::DispatcherBase::createListener(ptr, callable); 96 | } 97 | 98 | template 99 | [[deprecated("provide key explicitly")]] 100 | typename BaseClass::ListenerConstSharedPtr createListener(const T &key, const typename BaseClass::Callable &callable){ 101 | return createListener(static_cast(key), callable); 102 | } 103 | 104 | void dispatch(const K &key, const typename BaseClass::Type &obj){ 105 | boost::mutex::scoped_lock lock(BaseClass::mutex_); 106 | typename BaseClass::DispatcherBaseSharedPtr &ptr = filtered_[key]; 107 | if(ptr) ptr->dispatch_nolock(obj); 108 | BaseClass::dispatcher_->dispatch_nolock(obj); 109 | } 110 | 111 | [[deprecated("provide key explicitly")]] 112 | void dispatch(const typename BaseClass::Type &obj){ 113 | return dispatch(static_cast(obj), obj); 114 | } 115 | 116 | operator typename BaseClass::Callable() { return typename BaseClass::Callable(this,&FilteredDispatcher::dispatch); } 117 | }; 118 | 119 | } // namespace can 120 | #endif 121 | -------------------------------------------------------------------------------- /canopen_master/src/master_plugin.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | 7 | namespace canopen { 8 | 9 | class ManagingSyncLayer: public SyncLayer { 10 | protected: 11 | can::CommInterfaceSharedPtr interface_; 12 | boost::chrono::milliseconds step_, half_step_; 13 | 14 | std::set nodes_; 15 | boost::mutex nodes_mutex_; 16 | std::atomic nodes_size_; 17 | 18 | virtual void handleShutdown(LayerStatus &status) { 19 | } 20 | 21 | virtual void handleHalt(LayerStatus &status) { /* nothing to do */ } 22 | virtual void handleDiag(LayerReport &report) { /* TODO */ } 23 | virtual void handleRecover(LayerStatus &status) { /* TODO */ } 24 | 25 | public: 26 | ManagingSyncLayer(const SyncProperties &p, can::CommInterfaceSharedPtr interface) 27 | : SyncLayer(p), interface_(interface), step_(p.period_ms_), half_step_(p.period_ms_/2), nodes_size_(0) 28 | { 29 | } 30 | 31 | virtual void addNode(void * const ptr) { 32 | boost::mutex::scoped_lock lock(nodes_mutex_); 33 | nodes_.insert(ptr); 34 | nodes_size_ = nodes_.size(); 35 | } 36 | virtual void removeNode(void * const ptr) { 37 | boost::mutex::scoped_lock lock(nodes_mutex_); 38 | nodes_.erase(ptr); 39 | nodes_size_ = nodes_.size(); 40 | } 41 | }; 42 | 43 | 44 | class SimpleSyncLayer: public ManagingSyncLayer { 45 | time_point read_time_, write_time_; 46 | can::Frame frame_; 47 | uint8_t overflow_; 48 | 49 | void resetCounter(){ 50 | frame_.data[0] = 1; // SYNC counter starts at 1 51 | } 52 | void tryUpdateCounter(){ 53 | if (frame_.dlc > 0) { // sync counter is used 54 | if (frame_.data[0] >= overflow_) { 55 | resetCounter(); 56 | }else{ 57 | ++frame_.data[0]; 58 | } 59 | } 60 | } 61 | protected: 62 | virtual void handleRead(LayerStatus &status, const LayerState ¤t_state) { 63 | if(current_state > Init){ 64 | boost::this_thread::sleep_until(read_time_); 65 | write_time_ += step_; 66 | } 67 | } 68 | virtual void handleWrite(LayerStatus &status, const LayerState ¤t_state) { 69 | if(current_state > Init){ 70 | boost::this_thread::sleep_until(write_time_); 71 | tryUpdateCounter(); 72 | if(nodes_size_){ //) 73 | interface_->send(frame_); 74 | } 75 | read_time_ = get_abs_time(half_step_); 76 | } 77 | } 78 | 79 | virtual void handleInit(LayerStatus &status){ 80 | write_time_ = get_abs_time(step_); 81 | read_time_ = get_abs_time(half_step_); 82 | } 83 | public: 84 | SimpleSyncLayer(const SyncProperties &p, can::CommInterfaceSharedPtr interface) 85 | : ManagingSyncLayer(p, interface), frame_(p.header_, 0), overflow_(p.overflow_) { 86 | if(overflow_ == 1 || overflow_ > 240){ 87 | BOOST_THROW_EXCEPTION(Exception("SYNC counter overflow is invalid")); 88 | }else if(overflow_ > 1){ 89 | frame_.dlc = 1; 90 | resetCounter(); 91 | } 92 | } 93 | }; 94 | 95 | class ExternalSyncLayer: public ManagingSyncLayer { 96 | can::BufferedReader reader_; 97 | protected: 98 | virtual void handleRead(LayerStatus &status, const LayerState ¤t_state) { 99 | can::Frame msg; 100 | if(current_state > Init){ 101 | if(reader_.readUntil(&msg, get_abs_time(step_))){ // wait for sync 102 | boost::this_thread::sleep_until(get_abs_time(half_step_)); // shift readout to middle of period 103 | } 104 | } 105 | } 106 | virtual void handleWrite(LayerStatus &status, const LayerState ¤t_state) { 107 | // nothing to do here 108 | } 109 | virtual void handleInit(LayerStatus &status){ 110 | reader_.listen(interface_, properties.header_); 111 | } 112 | public: 113 | ExternalSyncLayer(const SyncProperties &p, can::CommInterfaceSharedPtr interface) 114 | : ManagingSyncLayer(p, interface), reader_(true,1) {} 115 | }; 116 | 117 | 118 | template class WrapMaster: public Master{ 119 | can::CommInterfaceSharedPtr interface_; 120 | public: 121 | virtual SyncLayerSharedPtr getSync(const SyncProperties &properties){ 122 | return std::make_shared(properties, interface_); 123 | } 124 | WrapMaster(can::CommInterfaceSharedPtr interface) : interface_(interface) {} 125 | 126 | class Allocator : public Master::Allocator{ 127 | public: 128 | virtual MasterSharedPtr allocate(const std::string &name, can::CommInterfaceSharedPtr interface){ 129 | return std::make_shared(interface); 130 | } 131 | }; 132 | }; 133 | 134 | typedef WrapMaster SimpleMaster; 135 | typedef WrapMaster ExternalMaster; 136 | } 137 | CLASS_LOADER_REGISTER_CLASS(canopen::SimpleMaster::Allocator, canopen::Master::Allocator); 138 | CLASS_LOADER_REGISTER_CLASS(canopen::ExternalMaster::Allocator, canopen::Master::Allocator); 139 | -------------------------------------------------------------------------------- /socketcan_bridge/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package socketcan_bridge 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.8.5 (2020-09-22) 6 | ------------------ 7 | 8 | 0.8.4 (2020-08-22) 9 | ------------------ 10 | * pass settings from ROS node to SocketCANInterface 11 | * Contributors: Mathias Lüdtke 12 | 13 | 0.8.3 (2020-05-07) 14 | ------------------ 15 | * add includes to 16 | * Bump CMake version to avoid CMP0048 warning 17 | Signed-off-by: ahcorde 18 | * Contributors: Mathias Lüdtke, ahcorde 19 | 20 | 0.8.2 (2019-11-04) 21 | ------------------ 22 | * fix roslint errors in socketcan_bridge 23 | * run roslint as part of run_tests 24 | * enable rosconsole_bridge bindings 25 | * Contributors: Mathias Lüdtke 26 | 27 | 0.8.1 (2019-07-14) 28 | ------------------ 29 | * Added configurable queue sizes 30 | * Set C++ standard to c++14 31 | * implemented create\*ListenerM helpers 32 | * Replacing FastDelegate with std::function and std::bind. 33 | * Contributors: Harsh Deshpande, JeremyZoss, Joshua Whitley, Mathias Lüdtke, rchristopher 34 | 35 | 0.8.0 (2018-07-11) 36 | ------------------ 37 | * keep NodeHandle alive in socketcan_bridge tests 38 | * migrated to std::function and std::bind 39 | * migrated to std pointers 40 | * compare can_msgs::Frame and can::Frame properly 41 | * Contributors: Mathias Lüdtke 42 | 43 | 0.7.8 (2018-05-04) 44 | ------------------ 45 | * Revert "pull make_shared into namespaces" 46 | This reverts commit 9b2cd05df76d223647ca81917d289ca6330cdee6. 47 | * Contributors: Mathias Lüdtke 48 | 49 | 0.7.7 (2018-05-04) 50 | ------------------ 51 | * pull make_shared into namespaces 52 | * added types for all shared_ptrs 53 | * address catkin_lint errors/warnings 54 | * protect tests from accessing front() or back() of empty list 55 | * added checkMaskFilter for socketcan_bridge 56 | * remove isValid work-around 57 | * added unit test for can id pass filter 58 | * add CAN filter to socketcan_bridge nodes 59 | * Contributors: Benjamin Maidel, Mathias Lüdtke 60 | 61 | 0.7.6 (2017-08-30) 62 | ------------------ 63 | 64 | 0.7.5 (2017-05-29) 65 | ------------------ 66 | 67 | 0.7.4 (2017-04-25) 68 | ------------------ 69 | 70 | 0.7.3 (2017-04-25) 71 | ------------------ 72 | 73 | 0.7.2 (2017-03-28) 74 | ------------------ 75 | 76 | 0.7.1 (2017-03-20) 77 | ------------------ 78 | 79 | 0.7.0 (2016-12-13) 80 | ------------------ 81 | 82 | 0.6.5 (2016-12-10) 83 | ------------------ 84 | * hamonized versions 85 | * styled and sorted CMakeLists.txt 86 | * removed boilerplate comments 87 | * indention 88 | * reviewed exported dependencies 89 | * styled and sorted package.xml 90 | * Removes gtest from test dependencies. 91 | This dependency is covered by the rosunit dependency. 92 | * Removes dependency on Boost, adds rosunit dependency. 93 | The dependency on Boost was unnecessary, rosunit is required for gtest. 94 | * Improves StateInterface implementation of the DummyInterface. 95 | The doesLoopBack() method now returns the correct value. A state change is 96 | correctly dispatched when the init() method is called. 97 | * Changes the exit code of the nodes if device init fails. 98 | Now exits with 1 if the initialization of the can device fails. 99 | * Changes the frame field for the published messages. 100 | An empty frame name is more commonly used to denote that there is no frame 101 | associated with the message. 102 | * Changes return type of setup() method. 103 | Setup() calls the CreateMsgListener and CreateStateListener, it does not attempt 104 | to verify if the interface is ready, which makes void more applicable. 105 | * Cleanup, fixes and improvements in CmakeLists. 106 | Adds the REQUIRED COMPONENTS packages to the CATKIN_DEPENDS. 107 | Improves add_dependency on the messages to be built. 108 | Removes unnecessary FILES_MATCHING. 109 | Moves the roslint_cpp macro to the testing block. 110 | * Finalizes work on the socketcan_bridge and can_msgs. 111 | Readies the packages socketcan_bridge and can_msgs for the merge with ros_canopen. 112 | Bumps the version for both packages to 0.1.0. Final cleanup in CMakeLists, added 113 | comments to the shell script and launchfile used for testing. 114 | * Adds tests for socketcan_bridge and bugfixes. 115 | Uses rostests and the modified DummyInterface to test whether behaviour 116 | is correct. Prevented possible crashes when can::tostring was called on 117 | invalid frames. 118 | * Adds conversion test between msg and SocketCAN Frame. 119 | This test only covers the conversion between the can_msgs::Frame message and can::Frame from SocketCAN. 120 | * Introduces topic_to_socketcan and the bridge. 121 | Adds TopicToSocketCAN, the counterpart to the SocketCANToTopic class. 122 | Also introduces a node to use this class and a node which combines the two 123 | classes into a bidirectional bridge. 124 | Contrary to the previous commit message the send() method appears to be 125 | available from the ThreadedSocketCANInterface afterall. 126 | * Introduces socketcan_to_topic and its node. 127 | 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. 128 | * Contributors: Ivor Wanders, Mathias Lüdtke 129 | -------------------------------------------------------------------------------- /socketcan_bridge/src/socketcan_to_topic.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016, Ivor Wanders 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are met: 6 | * * Redistributions of source code must retain the above copyright notice, 7 | * this list of conditions and the following disclaimer. 8 | * * Redistributions in binary form must reproduce the above copyright 9 | * notice, this list of conditions and the following disclaimer in the 10 | * documentation and/or other materials provided with the distribution. 11 | * * Neither the name of the copyright holder nor the names of its 12 | * contributors may be used to endorse or promote products derived from 13 | * this software without specific prior written permission. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | * POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | namespace can 34 | { 35 | template<> can::FrameFilterSharedPtr tofilter(const XmlRpc::XmlRpcValue &ct) 36 | { 37 | XmlRpc::XmlRpcValue t(ct); 38 | try // try to read as integer 39 | { 40 | uint32_t id = static_cast(t); 41 | return tofilter(id); 42 | } 43 | catch(...) // else read as string 44 | { 45 | return tofilter(static_cast(t)); 46 | } 47 | } 48 | } // namespace can 49 | 50 | namespace socketcan_bridge 51 | { 52 | SocketCANToTopic::SocketCANToTopic(ros::NodeHandle* nh, ros::NodeHandle* nh_param, 53 | can::DriverInterfaceSharedPtr driver) 54 | { 55 | can_topic_ = nh->advertise("received_messages", 56 | nh_param->param("received_messages_queue_size", 10)); 57 | driver_ = driver; 58 | }; 59 | 60 | void SocketCANToTopic::setup() 61 | { 62 | // register handler for frames and state changes. 63 | frame_listener_ = driver_->createMsgListenerM(this, &SocketCANToTopic::frameCallback); 64 | state_listener_ = driver_->createStateListenerM(this, &SocketCANToTopic::stateCallback); 65 | }; 66 | 67 | void SocketCANToTopic::setup(const can::FilteredFrameListener::FilterVector &filters) 68 | { 69 | frame_listener_.reset(new can::FilteredFrameListener(driver_, 70 | std::bind(&SocketCANToTopic::frameCallback, 71 | this, 72 | std::placeholders::_1), 73 | filters)); 74 | 75 | state_listener_ = driver_->createStateListenerM(this, &SocketCANToTopic::stateCallback); 76 | } 77 | 78 | void SocketCANToTopic::setup(XmlRpc::XmlRpcValue filters) 79 | { 80 | setup(can::tofilters(filters)); 81 | } 82 | void SocketCANToTopic::setup(ros::NodeHandle nh) 83 | { 84 | XmlRpc::XmlRpcValue filters; 85 | if (nh.getParam("can_ids", filters)) return setup(filters); 86 | return setup(); 87 | } 88 | 89 | 90 | void SocketCANToTopic::frameCallback(const can::Frame& f) 91 | { 92 | // ROS_DEBUG("Message came in: %s", can::tostring(f, true).c_str()); 93 | if (!f.isValid()) 94 | { 95 | ROS_ERROR("Invalid frame from SocketCAN: id: %#04x, length: %d, is_extended: %d, is_error: %d, is_rtr: %d", 96 | f.id, f.dlc, f.is_extended, f.is_error, f.is_rtr); 97 | return; 98 | } 99 | else 100 | { 101 | if (f.is_error) 102 | { 103 | // can::tostring cannot be used for dlc > 8 frames. It causes an crash 104 | // due to usage of boost::array for the data array. The should always work. 105 | ROS_WARN("Received frame is error: %s", can::tostring(f, true).c_str()); 106 | } 107 | } 108 | 109 | can_msgs::Frame msg; 110 | // converts the can::Frame (socketcan.h) to can_msgs::Frame (ROS msg) 111 | convertSocketCANToMessage(f, msg); 112 | 113 | msg.header.frame_id = ""; // empty frame is the de-facto standard for no frame. 114 | msg.header.stamp = ros::Time::now(); 115 | 116 | can_topic_.publish(msg); 117 | }; 118 | 119 | 120 | void SocketCANToTopic::stateCallback(const can::State & s) 121 | { 122 | std::string err; 123 | driver_->translateError(s.internal_error, err); 124 | if (!s.internal_error) 125 | { 126 | ROS_INFO("State: %s, asio: %s", err.c_str(), s.error_code.message().c_str()); 127 | } 128 | else 129 | { 130 | ROS_ERROR("Error: %s, asio: %s", err.c_str(), s.error_code.message().c_str()); 131 | } 132 | }; 133 | }; // namespace socketcan_bridge 134 | -------------------------------------------------------------------------------- /canopen_master/include/canopen_master/bcm_sync.h: -------------------------------------------------------------------------------- 1 | #ifndef H_BCM_SYNC 2 | #define H_BCM_SYNC 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | namespace canopen { 10 | 11 | template std::string join(const T &container, const std::string &delim){ 12 | if(container.empty()) return std::string(); 13 | std::stringstream sstr; 14 | typename T::const_iterator it = container.begin(); 15 | sstr << *it; 16 | for(++it; it != container.end(); ++it){ 17 | sstr << delim << *it; 18 | } 19 | return sstr.str(); 20 | } 21 | 22 | class BCMsync : public Layer { 23 | boost::mutex mutex_; 24 | 25 | std::string device_; 26 | 27 | std::set ignored_nodes_; 28 | std::set monitored_nodes_; 29 | std::set known_nodes_; 30 | std::set started_nodes_; 31 | 32 | bool sync_running_; 33 | can::BCMsocket bcm_; 34 | can::SocketCANDriverSharedPtr driver_; 35 | uint16_t sync_ms_; 36 | can::FrameListenerConstSharedPtr handler_; 37 | 38 | std::vector sync_frames_; 39 | 40 | bool skipNode(uint8_t id){ 41 | if(ignored_nodes_.find(id) != ignored_nodes_.end()) return true; 42 | if(!monitored_nodes_.empty() && monitored_nodes_.find(id) == monitored_nodes_.end()) return true; 43 | known_nodes_.insert(id); 44 | return false; 45 | } 46 | 47 | void handleFrame(const can::Frame &frame){ 48 | boost::mutex::scoped_lock lock(mutex_); 49 | 50 | if(frame.id == NMT_ID){ 51 | if(frame.dlc > 1){ 52 | uint8_t cmd = frame.data[0]; 53 | uint8_t id = frame.data[1]; 54 | if(skipNode(id)) return; 55 | 56 | if(cmd == 1){ // started 57 | if(id != 0) started_nodes_.insert(id); 58 | else started_nodes_.insert(known_nodes_.begin(), known_nodes_.end()); // start all 59 | }else{ 60 | if(id != 0) started_nodes_.erase(id); 61 | else started_nodes_.clear(); // stop all 62 | } 63 | } 64 | }else if((frame.id & ~ALL_NODES_MASK) == HEARTBEAT_ID){ 65 | int id = frame.id & ALL_NODES_MASK; 66 | if(skipNode(id)) return; 67 | 68 | if(frame.dlc > 0 && frame.data[0] == canopen::Node::Stopped) started_nodes_.erase(id); 69 | } 70 | 71 | // toggle sync if needed 72 | if(started_nodes_.empty() && sync_running_){ 73 | sync_running_ = !bcm_.stopTX(sync_frames_.front()); 74 | }else if(!started_nodes_.empty() && !sync_running_){ 75 | sync_running_ = bcm_.startTX(boost::chrono::milliseconds(sync_ms_),sync_frames_.front(), sync_frames_.size(), &sync_frames_[0]); 76 | } 77 | } 78 | protected: 79 | virtual void handleRead(LayerStatus &status, const LayerState ¤t_state) { 80 | if(current_state > Init){ 81 | } 82 | } 83 | virtual void handleWrite(LayerStatus &status, const LayerState ¤t_state) { 84 | if(current_state > Init){ 85 | } 86 | } 87 | virtual void handleDiag(LayerReport &report){ 88 | boost::mutex::scoped_lock lock(mutex_); 89 | if(!sync_running_) report.warn("sync is not running"); 90 | 91 | report.add("sync_running", sync_running_); 92 | report.add("known_nodes", join(known_nodes_, ", ")); 93 | report.add("started_nodes", join(started_nodes_, ", ")); 94 | } 95 | 96 | virtual void handleInit(LayerStatus &status){ 97 | boost::mutex::scoped_lock lock(mutex_); 98 | started_nodes_.clear(); 99 | 100 | if(!bcm_.init(device_)){ 101 | status.error("BCM_init failed"); 102 | return; 103 | } 104 | int sc = driver_->getInternalSocket(); 105 | 106 | struct can_filter filter[2]; 107 | 108 | filter[0].can_id = NMT_ID; 109 | filter[0].can_mask = CAN_SFF_MASK; 110 | filter[1].can_id = HEARTBEAT_ID; 111 | filter[1].can_mask = ~ALL_NODES_MASK; 112 | 113 | if(setsockopt(sc, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, sizeof(filter)) < 0){ 114 | status.warn("could not set filter"); 115 | return; 116 | } 117 | 118 | handler_ = driver_->createMsgListenerM(this, &BCMsync::handleFrame); 119 | } 120 | virtual void handleShutdown(LayerStatus &status){ 121 | boost::mutex::scoped_lock lock(mutex_); 122 | handler_.reset(); 123 | bcm_.shutdown(); 124 | } 125 | 126 | virtual void handleHalt(LayerStatus &status) { 127 | boost::mutex::scoped_lock lock(mutex_); 128 | if(sync_running_){ 129 | bcm_.stopTX(sync_frames_.front()); 130 | sync_running_ = false; 131 | started_nodes_.clear(); 132 | } 133 | } 134 | 135 | virtual void handleRecover(LayerStatus &status){ 136 | handleShutdown(status); 137 | handleInit(status); 138 | } 139 | public: 140 | static const uint32_t ALL_NODES_MASK = 127; 141 | static const uint32_t HEARTBEAT_ID = 0x700; 142 | static const uint32_t NMT_ID = 0x000; 143 | 144 | BCMsync(const std::string &device, can::SocketCANDriverSharedPtr driver, const SyncProperties &sync_properties) 145 | : Layer(device + " SyncLayer"), device_(device), sync_running_(false), sync_ms_(sync_properties.period_ms_), driver_(driver) { 146 | if(sync_properties.overflow_ == 0){ 147 | sync_frames_.resize(1); 148 | sync_frames_[0] = can::Frame(sync_properties.header_,0); 149 | }else{ 150 | sync_frames_.resize(sync_properties.overflow_); 151 | for(int i = 0; i < sync_properties.overflow_; ++i){ 152 | sync_frames_[i] = can::Frame(sync_properties.header_,1); 153 | sync_frames_[i].data[0] = i+1; 154 | } 155 | } 156 | } 157 | template void setMonitored(const T &other){ 158 | monitored_nodes_.clear(); 159 | monitored_nodes_.insert(other.begin(), other.end()); 160 | } 161 | template void setIgnored(const T &other){ 162 | ignored_nodes_.clear(); 163 | ignored_nodes_.insert(other.begin(), other.end()); 164 | } 165 | }; 166 | 167 | } 168 | #endif 169 | -------------------------------------------------------------------------------- /socketcan_interface/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package socketcan_interface 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.8.5 (2020-09-22) 6 | ------------------ 7 | * check settings pointer and print error if null 8 | * initalize settings properly in deprecated SocketCANInterface::init 9 | * Contributors: Mathias Lüdtke 10 | 11 | 0.8.4 (2020-08-22) 12 | ------------------ 13 | * make parse_error_mask a static member function 14 | * pass settings from ROS node to SocketCANInterface 15 | * add support for recursive XmlRpcSettings lookups 16 | * implemented report-only and fatal errors for SocketCANInterface 17 | * added settings parameter to DriverInterface::init 18 | * moved XmlRpcSettings to socketcan_interface 19 | * moved canopen::Settings into can namespace 20 | * Contributors: Mathias Lüdtke 21 | 22 | 0.8.3 (2020-05-07) 23 | ------------------ 24 | * Fixed Boost link in test-dispacher 25 | Signed-off-by: ahcorde 26 | * Bump CMake version to avoid CMP0048 warning 27 | Signed-off-by: ahcorde 28 | * do not print ERROR in candump 29 | * Contributors: Mathias Lüdtke, ahcorde 30 | 31 | 0.8.2 (2019-11-04) 32 | ------------------ 33 | * enable rosconsole_bridge bindings 34 | * switch to new logging macros 35 | * add logging based on console_bridge 36 | * handle extended frame strings like candump 37 | * implement Frame::fullid() 38 | * removed implicit Header operator 39 | * move stream operators into can namespace 40 | * Contributors: Mathias Lüdtke 41 | 42 | 0.8.1 (2019-07-14) 43 | ------------------ 44 | * Set C++ standard to c++14 45 | * implemented test for dispatcher 46 | * Replacing typedefs in socketcan_interface with using aliases. 47 | * added Delegate helpers for backwards compatibility 48 | * implemented create\*ListenerM helpers 49 | * Replacing FastDelegate with std::function and std::bind. 50 | * Contributors: Harsh Deshpande, Joshua Whitley, Mathias Lüdtke, pzzlr 51 | 52 | 0.8.0 (2018-07-11) 53 | ------------------ 54 | * migrated to std::function and std::bind 55 | * got rid of boost::noncopyable 56 | * replaced BOOST_FOREACH 57 | * migrated to std::unordered_map and std::unordered_set 58 | * migrated to std:array 59 | * migrated to std pointers 60 | * removed deprecated types 61 | * introduced ROSCANOPEN_MAKE_SHARED 62 | * added c_array access functons to can::Frame 63 | * Contributors: Mathias Lüdtke 64 | 65 | 0.7.8 (2018-05-04) 66 | ------------------ 67 | * Revert "pull make_shared into namespaces" 68 | This reverts commit 9b2cd05df76d223647ca81917d289ca6330cdee6. 69 | * Contributors: Mathias Lüdtke 70 | 71 | 0.7.7 (2018-05-04) 72 | ------------------ 73 | * pull make_shared into namespaces 74 | * added types for all shared_ptrs 75 | * fix catkin_lint warnings in filter tests 76 | * migrate to new classloader headers 77 | * find and link the thread library properly 78 | * compile also with boost >= 1.66.0 79 | * explicitly include iostream to compile with boost >= 1.65.0 80 | * address catkin_lint errors/warnings 81 | * added test for FilteredFrameListener 82 | * fix string parsers 83 | * default to relaxed filter handling 84 | works for standard and extended frames 85 | * fix string handling of extended frames 86 | * added filter parsers 87 | should work for vector, vector and custom vector-like classes 88 | * implemented mask and range filters for can::Frame 89 | * Contributors: Lukas Bulwahn, Mathias Lüdtke 90 | 91 | 0.7.6 (2017-08-30) 92 | ------------------ 93 | * make can::Header/Frame::isValid() const 94 | * Contributors: Mathias Lüdtke 95 | 96 | 0.7.5 (2017-05-29) 97 | ------------------ 98 | * fix rosdep dependency on kernel headers 99 | * Contributors: Mathias Lüdtke 100 | 101 | 0.7.4 (2017-04-25) 102 | ------------------ 103 | 104 | 0.7.3 (2017-04-25) 105 | ------------------ 106 | 107 | 0.7.2 (2017-03-28) 108 | ------------------ 109 | 110 | 0.7.1 (2017-03-20) 111 | ------------------ 112 | * stop CAN driver on read errors as well 113 | * expose socketcan handle 114 | * implemented BCMsocket 115 | * introduced BufferedReader::readUntil 116 | * Contributors: Mathias Lüdtke 117 | 118 | 0.7.0 (2016-12-13) 119 | ------------------ 120 | 121 | 0.6.5 (2016-12-10) 122 | ------------------ 123 | * removed Baseclass typedef since its use prevented virtual functions calls 124 | * add missing chrono dependency 125 | * Added catch-all features in BufferedReader 126 | * hardened code with the help of cppcheck 127 | * styled and sorted CMakeLists.txt 128 | * removed boilerplate comments 129 | * indention 130 | * reviewed exported dependencies 131 | * styled and sorted package.xml 132 | * update package URLs 133 | * Improves StateInterface implementation of the DummyInterface. 134 | The doesLoopBack() method now returns the correct value. A state change is 135 | correctly dispatched when the init() method is called. 136 | * Changes inheritance of DummyInterface to DriverInterface. 137 | Such that this interface can also be used for tests requiring a DriverInterface 138 | class. 139 | Test results of the socketcan_interface tests are unchanged by this 140 | modification as it only uses the CommInterface methods. 141 | * added socketcan_interface_string to test 142 | * moved string functions into separate lib 143 | * Introduced setNotReady, prevent enqueue() to switch from closed to open 144 | * Reading state\_ should be protected by lock 145 | * improved BufferedReader interface and ScopedEnabler 146 | * added flush() and max length support to BufferedReader 147 | * added BufferedReader 148 | * wake multiple waiting threads if needed 149 | * pad hex buffer strings in all cases 150 | * removed unstable StateWaiter::wait_for 151 | * Contributors: Ivor Wanders, Mathias Lüdtke, Michael Stoll 152 | 153 | 0.6.4 (2015-07-03) 154 | ------------------ 155 | * added missing include, revised depends etc. 156 | 157 | 158 | 0.6.3 (2015-06-30) 159 | ------------------ 160 | * dependencies revised 161 | * reordering fix for `#87 `_ 162 | * intialize structs 163 | * tostring fixed for headers 164 | * removed empty test 165 | * added DummyInterface with first test 166 | * added message string helper 167 | * added missing include 168 | * install socketcan_interface_plugin.xml 169 | * migrated to class_loader for non-ROS parts 170 | * moved ThreadedInterface to dedicated header 171 | * removed bitrate, added loopback to DriverInterface::init 172 | * added socketcan plugin 173 | * CommInterstate and StateInterface are now bases of DriverInterface. 174 | Therefore DispatchedInterface was moved into AsioBase. 175 | * remove debug prints 176 | * shutdown asio driver in destructor 177 | * proper mask shifts 178 | * Contributors: Mathias Lüdtke 179 | 180 | 0.6.2 (2014-12-18) 181 | ------------------ 182 | 183 | 0.6.1 (2014-12-15) 184 | ------------------ 185 | * remove ipa_* and IPA_* prefixes 186 | * fixed catkin_lint errors 187 | * added descriptions and authors 188 | * renamed ipa_can_interface to socketcaninterface 189 | * Contributors: Florian Weisshardt, Mathias Lüdtke 190 | -------------------------------------------------------------------------------- /socketcan_bridge/test/to_socketcan_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2016, Ivor Wanders 3 | * 4 | * Redistribution and use in source and binary forms, with or without 5 | * modification, are permitted provided that the following conditions are met: 6 | * * Redistributions of source code must retain the above copyright notice, 7 | * this list of conditions and the following disclaimer. 8 | * * Redistributions in binary form must reproduce the above copyright 9 | * notice, this list of conditions and the following disclaimer in the 10 | * documentation and/or other materials provided with the distribution. 11 | * * Neither the name of the copyright holder nor the names of its 12 | * contributors may be used to endorse or promote products derived from 13 | * this software without specific prior written permission. 14 | * 15 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | * POSSIBILITY OF SUCH DAMAGE. 26 | */ 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | class frameCollector 40 | { 41 | public: 42 | std::list frames; 43 | 44 | frameCollector() {} 45 | 46 | void frameCallback(const can::Frame& f) 47 | { 48 | frames.push_back(f); 49 | } 50 | }; 51 | 52 | TEST(TopicToSocketCANTest, checkCorrectData) 53 | { 54 | ros::NodeHandle nh(""), nh_param("~"); 55 | 56 | can::DummyBus bus("checkCorrectData"); 57 | // create the dummy interface 58 | can::ThreadedDummyInterfaceSharedPtr dummy = std::make_shared(); 59 | 60 | // start the to topic bridge. 61 | socketcan_bridge::TopicToSocketCAN to_socketcan_bridge(&nh, &nh_param, dummy); 62 | to_socketcan_bridge.setup(); 63 | 64 | // init the driver to test stateListener (not checked automatically). 65 | dummy->init(bus.name, true, can::NoSettings::create()); 66 | 67 | // register for messages on received_messages. 68 | ros::Publisher publisher_ = nh.advertise("sent_messages", 10); 69 | 70 | // create a frame collector. 71 | frameCollector frame_collector_; 72 | 73 | // driver->createMsgListener(&frameCallback); 74 | can::FrameListenerConstSharedPtr frame_listener_ = dummy->createMsgListener( 75 | 76 | std::bind(&frameCollector::frameCallback, &frame_collector_, std::placeholders::_1)); 77 | 78 | // create a message 79 | can_msgs::Frame msg; 80 | msg.is_extended = true; 81 | msg.is_rtr = false; 82 | msg.is_error = false; 83 | msg.id = 0x1337; 84 | msg.dlc = 8; 85 | for (uint8_t i=0; i < msg.dlc; i++) 86 | { 87 | msg.data[i] = i; 88 | } 89 | 90 | msg.header.frame_id = "0"; // "0" for no frame. 91 | msg.header.stamp = ros::Time::now(); 92 | 93 | // send the can_frame::Frame message to the sent_messages topic. 94 | publisher_.publish(msg); 95 | 96 | // give some time for the interface some time to process the message 97 | ros::WallDuration(1.0).sleep(); 98 | ros::spinOnce(); 99 | 100 | dummy->flush(); 101 | 102 | can_msgs::Frame received; 103 | can::Frame f = frame_collector_.frames.back(); 104 | socketcan_bridge::convertSocketCANToMessage(f, received); 105 | 106 | EXPECT_EQ(received.id, msg.id); 107 | EXPECT_EQ(received.dlc, msg.dlc); 108 | EXPECT_EQ(received.is_extended, msg.is_extended); 109 | EXPECT_EQ(received.is_rtr, msg.is_rtr); 110 | EXPECT_EQ(received.is_error, msg.is_error); 111 | EXPECT_EQ(received.data, msg.data); 112 | } 113 | 114 | TEST(TopicToSocketCANTest, checkInvalidFrameHandling) 115 | { 116 | // - tries to send a non-extended frame with an id larger than 11 bits. 117 | // that should not be sent. 118 | // - verifies that sending one larger than 11 bits actually works. 119 | // - tries sending a message with a dlc > 8 bytes, this should not be sent. 120 | // sending with 8 bytes is verified by the checkCorrectData testcase. 121 | 122 | ros::NodeHandle nh(""), nh_param("~"); 123 | 124 | 125 | can::DummyBus bus("checkInvalidFrameHandling"); 126 | 127 | // create the dummy interface 128 | can::ThreadedDummyInterfaceSharedPtr dummy = std::make_shared(); 129 | 130 | // start the to topic bridge. 131 | socketcan_bridge::TopicToSocketCAN to_socketcan_bridge(&nh, &nh_param, dummy); 132 | to_socketcan_bridge.setup(); 133 | 134 | dummy->init(bus.name, true, can::NoSettings::create()); 135 | 136 | // register for messages on received_messages. 137 | ros::Publisher publisher_ = nh.advertise("sent_messages", 10); 138 | 139 | // create a frame collector. 140 | frameCollector frame_collector_; 141 | 142 | // add callback to the dummy interface. 143 | can::FrameListenerConstSharedPtr frame_listener_ = dummy->createMsgListener( 144 | std::bind(&frameCollector::frameCallback, &frame_collector_, std::placeholders::_1)); 145 | 146 | // create a message 147 | can_msgs::Frame msg; 148 | msg.is_extended = false; 149 | msg.id = (1<<11)+1; // this is an illegal CAN packet... should not be sent. 150 | msg.header.frame_id = "0"; // "0" for no frame. 151 | msg.header.stamp = ros::Time::now(); 152 | 153 | // send the can_frame::Frame message to the sent_messages topic. 154 | publisher_.publish(msg); 155 | 156 | // give some time for the interface some time to process the message 157 | ros::WallDuration(1.0).sleep(); 158 | ros::spinOnce(); 159 | dummy->flush(); 160 | 161 | EXPECT_EQ(frame_collector_.frames.size(), 0); 162 | 163 | msg.is_extended = true; 164 | msg.id = (1<<11)+1; // now it should be alright. 165 | // send the can_frame::Frame message to the sent_messages topic. 166 | publisher_.publish(msg); 167 | ros::WallDuration(1.0).sleep(); 168 | ros::spinOnce(); 169 | dummy->flush(); 170 | 171 | EXPECT_EQ(frame_collector_.frames.size(), 1); 172 | 173 | // wipe the frame queue. 174 | frame_collector_.frames.clear(); 175 | 176 | 177 | // finally, check if frames with a dlc > 8 are discarded. 178 | msg.dlc = 10; 179 | publisher_.publish(msg); 180 | ros::WallDuration(1.0).sleep(); 181 | ros::spinOnce(); 182 | dummy->flush(); 183 | 184 | EXPECT_EQ(frame_collector_.frames.size(), 0); 185 | } 186 | 187 | int main(int argc, char **argv) 188 | { 189 | ros::init(argc, argv, "test_to_topic"); 190 | ros::NodeHandle nh; 191 | ros::WallDuration(1.0).sleep(); 192 | testing::InitGoogleTest(&argc, argv); 193 | return RUN_ALL_TESTS(); 194 | } 195 | -------------------------------------------------------------------------------- /canopen_master/src/node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | using namespace canopen; 4 | 5 | #pragma pack(push) /* push current alignment to stack */ 6 | #pragma pack(1) /* set alignment to 1 byte boundary */ 7 | 8 | 9 | struct NMTcommand{ 10 | enum Command{ 11 | Start = 1, 12 | Stop = 2, 13 | Prepare = 128, 14 | Reset = 129, 15 | Reset_Com = 130 16 | }; 17 | uint8_t command; 18 | uint8_t node_id; 19 | 20 | struct Frame: public FrameOverlay{ 21 | Frame(uint8_t node_id, const Command &c) : FrameOverlay(can::Header()) { 22 | data.command = c; 23 | data.node_id = node_id; 24 | } 25 | }; 26 | }; 27 | 28 | #pragma pack(pop) /* pop previous alignment from stack */ 29 | 30 | Node::Node(const can::CommInterfaceSharedPtr interface, const ObjectDictSharedPtr dict, uint8_t node_id, const SyncCounterSharedPtr sync) 31 | : Layer("Node 301"), node_id_(node_id), interface_(interface), sync_(sync) , state_(Unknown), sdo_(interface, dict, node_id), pdo_(interface){ 32 | try{ 33 | getStorage()->entry(heartbeat_, 0x1017); 34 | } 35 | catch(const std::out_of_range){ 36 | } 37 | } 38 | 39 | const Node::State Node::getState(){ 40 | boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock? 41 | return state_; 42 | } 43 | 44 | bool Node::reset_com(){ 45 | boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock? 46 | getStorage()->reset(); 47 | interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Reset_Com)); 48 | if(wait_for(BootUp, boost::chrono::seconds(10)) != 1){ 49 | return false; 50 | } 51 | state_ = PreOperational; 52 | setHeartbeatInterval(); 53 | return true; 54 | } 55 | bool Node::reset(){ 56 | boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock? 57 | getStorage()->reset(); 58 | 59 | interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Reset)); 60 | if(wait_for(BootUp, boost::chrono::seconds(10)) != 1){ 61 | return false; 62 | } 63 | state_ = PreOperational; 64 | setHeartbeatInterval(); 65 | return true; 66 | } 67 | 68 | bool Node::prepare(){ 69 | boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock? 70 | if(state_ == BootUp){ 71 | // ERROR 72 | } 73 | interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Prepare)); 74 | return 0 != wait_for(PreOperational, boost::chrono::seconds(2)); 75 | } 76 | bool Node::start(){ 77 | boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock? 78 | if(state_ == BootUp){ 79 | // ERROR 80 | } 81 | interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Start)); 82 | return 0 != wait_for(Operational, boost::chrono::seconds(2)); 83 | } 84 | bool Node::stop(){ 85 | boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock? 86 | if(sync_) sync_->removeNode(this); 87 | if(state_ == BootUp){ 88 | // ERROR 89 | } 90 | interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Stop)); 91 | return true; 92 | } 93 | 94 | void Node::switchState(const uint8_t &s){ 95 | bool changed = state_ != s; 96 | switch(s){ 97 | case Operational: 98 | if(changed && sync_) sync_->addNode(this); 99 | break; 100 | case BootUp: 101 | case PreOperational: 102 | case Stopped: 103 | if(changed && sync_) sync_->removeNode(this); 104 | break; 105 | default: 106 | //error 107 | ; 108 | } 109 | if(changed){ 110 | state_ = (State) s; 111 | state_dispatcher_.dispatch(state_); 112 | cond.notify_one(); 113 | } 114 | } 115 | void Node::handleNMT(const can::Frame & msg){ 116 | boost::mutex::scoped_lock cond_lock(cond_mutex); 117 | uint16_t interval = getHeartbeatInterval(); 118 | if(interval) heartbeat_timeout_ = get_abs_time(boost::chrono::milliseconds(3*interval)); 119 | assert(msg.dlc == 1); 120 | switchState(msg.data[0]); 121 | } 122 | template int Node::wait_for(const State &s, const T &timeout){ 123 | boost::mutex::scoped_lock cond_lock(cond_mutex); 124 | time_point abs_time = get_abs_time(timeout); 125 | 126 | while(s != state_) { 127 | if(cond.wait_until(cond_lock,abs_time) == boost::cv_status::timeout) 128 | { 129 | break; 130 | } 131 | } 132 | if( s!= state_){ 133 | if(getHeartbeatInterval() == 0){ 134 | switchState(s); 135 | return -1; 136 | } 137 | return 0; 138 | } 139 | return 1; 140 | } 141 | bool Node::checkHeartbeat(){ 142 | if(getHeartbeatInterval() == 0) return true; //disabled 143 | boost::mutex::scoped_lock cond_lock(cond_mutex); 144 | return heartbeat_timeout_ >= boost::chrono::high_resolution_clock::now(); 145 | } 146 | 147 | 148 | void Node::handleRead(LayerStatus &status, const LayerState ¤t_state) { 149 | if(current_state > Init){ 150 | if(!checkHeartbeat()){ 151 | status.error("heartbeat problem"); 152 | } else if(getState() != Operational){ 153 | status.error("not operational"); 154 | } else{ 155 | pdo_.read(status); 156 | } 157 | } 158 | } 159 | void Node::handleWrite(LayerStatus &status, const LayerState ¤t_state) { 160 | if(current_state > Init){ 161 | if(getState() != Operational) status.error("not operational"); 162 | else if(! pdo_.write()) status.error("PDO write problem"); 163 | } 164 | } 165 | 166 | 167 | void Node::handleDiag(LayerReport &report){ 168 | State state = getState(); 169 | if(state != Operational){ 170 | report.error("Mode not operational"); 171 | report.add("Node state", (int)state); 172 | }else if(!checkHeartbeat()){ 173 | report.error("Heartbeat timeout"); 174 | } 175 | } 176 | void Node::handleInit(LayerStatus &status){ 177 | nmt_listener_ = interface_->createMsgListenerM(can::MsgHeader(0x700 + node_id_), this, &Node::handleNMT); 178 | 179 | sdo_.init(); 180 | try{ 181 | if(!reset_com()) BOOST_THROW_EXCEPTION( TimeoutException("reset_timeout") ); 182 | } 183 | catch(const TimeoutException&){ 184 | status.error(boost::str(boost::format("could not reset node '%1%'") % (int)node_id_)); 185 | return; 186 | } 187 | 188 | if(!pdo_.init(getStorage(), status)){ 189 | return; 190 | } 191 | getStorage()->init_all(); 192 | sdo_.init(); // reread SDO paramters; 193 | // TODO: set SYNC data 194 | 195 | try{ 196 | if(!start()) BOOST_THROW_EXCEPTION( TimeoutException("start timeout") ); 197 | } 198 | catch(const TimeoutException&){ 199 | status.error(boost::str(boost::format("could not start node '%1%'") % (int)node_id_)); 200 | } 201 | } 202 | void Node::handleRecover(LayerStatus &status){ 203 | try{ 204 | start(); 205 | } 206 | catch(const TimeoutException&){ 207 | status.error(boost::str(boost::format("could not start node '%1%'") % (int)node_id_)); 208 | } 209 | } 210 | void Node::handleShutdown(LayerStatus &status){ 211 | if(getHeartbeatInterval()> 0) heartbeat_.set(0); 212 | stop(); 213 | nmt_listener_.reset(); 214 | switchState(Unknown); 215 | } 216 | void Node::handleHalt(LayerStatus &status){ 217 | // do nothing 218 | } 219 | --------------------------------------------------------------------------------