git commit -m "first commit for v2"
This commit is contained in:
263
Devices/Libraries/Ros/ros_canopen/canopen_master/CHANGELOG.rst
Executable file
263
Devices/Libraries/Ros/ros_canopen/canopen_master/CHANGELOG.rst
Executable file
@@ -0,0 +1,263 @@
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
Changelog for package canopen_master
|
||||
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
|
||||
|
||||
0.8.5 (2020-09-22)
|
||||
------------------
|
||||
|
||||
0.8.4 (2020-08-22)
|
||||
------------------
|
||||
* added settings parameter to DriverInterface::init
|
||||
* moved canopen::Settings into can namespace
|
||||
* Contributors: Mathias Lüdtke
|
||||
|
||||
0.8.3 (2020-05-07)
|
||||
------------------
|
||||
* Bump CMake version to avoid CMP0048 warning
|
||||
Signed-off-by: ahcorde <ahcorde@gmail.com>
|
||||
* Contributors: ahcorde
|
||||
|
||||
0.8.2 (2019-11-04)
|
||||
------------------
|
||||
* implemented LayerStatus::equals<>()
|
||||
* added support for SYNC counter in SimpleSyncLayer (`#349 <https://github.com/ipa-mdl/ros_canopen/issues/349>`_)
|
||||
* enable rosconsole_bridge bindings
|
||||
* switch to new logging macros
|
||||
* add logging based on console_bridge
|
||||
* removed implicit Header operator
|
||||
* Contributors: Mathias Lüdtke
|
||||
|
||||
0.8.1 (2019-07-14)
|
||||
------------------
|
||||
* Set C++ standard to c++14
|
||||
* added Delegate helpers for backwards compatibility
|
||||
* implemented create\*ListenerM helpers
|
||||
* Replacing FastDelegate with std::function and std::bind.
|
||||
* Contributors: Harsh Deshpande, Joshua Whitley, Mathias Lüdtke
|
||||
|
||||
0.8.0 (2018-07-11)
|
||||
------------------
|
||||
* migrated to std::function and std::bind
|
||||
* migrated to std::atomic
|
||||
* got rid of boost::noncopyable
|
||||
* replaced BOOST_FOREACH
|
||||
* migrated to std::unordered_map and std::unordered_set
|
||||
* migrated to std pointers
|
||||
* provided KeyHash
|
||||
for use with unordered containers
|
||||
* added c_array access functons to can::Frame
|
||||
* Contributors: Mathias Lüdtke
|
||||
|
||||
0.7.8 (2018-05-04)
|
||||
------------------
|
||||
* Revert "pull make_shared into namespaces"
|
||||
This reverts commit 9b2cd05df76d223647ca81917d289ca6330cdee6.
|
||||
* Contributors: Mathias Lüdtke
|
||||
|
||||
0.7.7 (2018-05-04)
|
||||
------------------
|
||||
* added types for all function objects
|
||||
* pull make_shared into namespaces
|
||||
* added types for all shared_ptrs
|
||||
* migrate to new classloader headers
|
||||
* throw bad_cast if datatype is not supported
|
||||
* special handling of std::bad_cast
|
||||
* address catkin_lint errors/warnings
|
||||
* removed IPC/SHM based sync masters
|
||||
* Contributors: Mathias Lüdtke
|
||||
|
||||
0.7.6 (2017-08-30)
|
||||
------------------
|
||||
|
||||
0.7.5 (2017-05-29)
|
||||
------------------
|
||||
* added EMCYHandler::resetErrors
|
||||
* added VectorHelper::callFunc
|
||||
generalized call templates
|
||||
* Contributors: Mathias Lüdtke
|
||||
|
||||
0.7.4 (2017-04-25)
|
||||
------------------
|
||||
|
||||
0.7.3 (2017-04-25)
|
||||
------------------
|
||||
* enforce boost::chrono-based timer
|
||||
* Contributors: Mathias Lüdtke
|
||||
|
||||
0.7.2 (2017-03-28)
|
||||
------------------
|
||||
* fix: handle EMCY as error, not as warning
|
||||
* Contributors: Mathias Lüdtke
|
||||
|
||||
0.7.1 (2017-03-20)
|
||||
------------------
|
||||
* refactored EMCY handling into separate layer
|
||||
* print EMCY to stdout
|
||||
* send node start on recover
|
||||
needed for external sync to work properly
|
||||
* pass halt on error unconditionally
|
||||
* added canopen_bcm_sync
|
||||
* implemented ExternalMaster
|
||||
* added object access services
|
||||
* implemented ObjectStorage::getStringReader
|
||||
* Contributors: Mathias Lüdtke
|
||||
|
||||
0.7.0 (2016-12-13)
|
||||
------------------
|
||||
|
||||
0.6.5 (2016-12-10)
|
||||
------------------
|
||||
* Merge pull request `#179 <https://github.com/ipa-mdl/ros_canopen/issues/179>`_ from ipa-mdl/mixed_case_access
|
||||
support mixed-case access strings in EDS
|
||||
* decouple listener initialization from 1003 binding
|
||||
* introduced THROW_WITH_KEY and ObjectDict::key_info
|
||||
* added access type tests
|
||||
* convert access string to lowercase
|
||||
* Do not remove shared memory automatically
|
||||
* hardened code with the help of cppcheck
|
||||
* throw verbose exception if AccessType is missing (`#64 <https://github.com/ipa-mdl/ros_canopen/issues/64>`_)
|
||||
* styled and sorted CMakeLists.txt
|
||||
* removed boilerplate comments
|
||||
* indention
|
||||
* reviewed exported dependencies
|
||||
* styled and sorted package.xml
|
||||
* canopen_master needs to depend on rosunit for gtest
|
||||
* update package URLs
|
||||
* fixed typo
|
||||
* do not reset PDO COB-ID if it is not writable
|
||||
* Do not recurse into sub-objects, handle them as simple data
|
||||
* strip string before searching for $NODEID
|
||||
* added NodeID/hex parser test
|
||||
* do full recover if if driver is not ready
|
||||
* wait for driver to be shutdown in run()
|
||||
* limit SDO reader to size of 1
|
||||
* do not send abort twice
|
||||
* removed unnecessary sleep (added for tests only)
|
||||
* catch all std exceptions in layer handlers
|
||||
* migrated SDOClient to BufferedReader
|
||||
* getter for LayerState
|
||||
* fixed lost wake-up condition, unified SDO accessors
|
||||
* minor NMT improvements
|
||||
* removed cond from PDOMapper, it does not wait on empty buffer anymore
|
||||
* Simple master counts nodes as well
|
||||
* throw exception on read from empty buffer
|
||||
* proper initialisation of PDO data from SDOs
|
||||
* change sync subscription only on change
|
||||
* shutdown and restart CAN layer on recover
|
||||
* canopen::Exception is now based on std::runtime_error
|
||||
* Merge pull request `#109 <https://github.com/ipa-mdl/ros_canopen/issues/109>`_ from ipa-mdl/shutdown-crashes
|
||||
Fix for pluginlib-related crashes on shutdown
|
||||
* stop after heartbeat was disabled, do not wait for state switch
|
||||
* added virtual destructor to SyncCounter
|
||||
* Use getHeartbeatInterval()
|
||||
* minor shutdown improvements
|
||||
* removed unstable StateWaiter::wait_for
|
||||
* Revert change to handleShutdown
|
||||
* Heartbeat interval is uint16, not double
|
||||
* Added validity check to heartbeat\_ (Some devices do not support heartbeat)
|
||||
* Contributors: Florian Weisshardt, Mathias Lüdtke, Michael Stoll
|
||||
|
||||
0.6.4 (2015-07-03)
|
||||
------------------
|
||||
* added missing include, revised depends etc.
|
||||
|
||||
0.6.3 (2015-06-30)
|
||||
------------------
|
||||
* added Settings class
|
||||
* added SimpleMaster
|
||||
* remove boost::posix_time::milliseconds from SyncProperties
|
||||
* removed support for silence_us since bus timing cannot be guaranteed
|
||||
* properly handle cases where def_val == init_val
|
||||
* implemented plugin-based Master allocators, defaults to LocalMaster
|
||||
* moved master/synclayer base classes to canopen.h
|
||||
* added support for non-continuous PDO ranges
|
||||
* added has() check to object dictionary interface
|
||||
* improved ObjectStorage entry interface
|
||||
* verbose out_of_range exception
|
||||
* improved timer: duration cast, autostart flag
|
||||
* reset sync waiter number after timeout
|
||||
* verbose timeout exception
|
||||
* little fix im EMCY diagnostics
|
||||
* string instead of mulit-char constant
|
||||
* Merge branch 'hwi_switch' into muparser
|
||||
* added std::string converters to ObjectDict::Key
|
||||
* do not warn on profile-only errors
|
||||
* added get_abs_time without parameter
|
||||
* link against boost_atomic for platforms with lock-based implementation
|
||||
* reset sent Reset and Reset_Com, c&p bug
|
||||
* stop heartbeat after node shutdown
|
||||
* protect reads of LayerState
|
||||
* protect layers in VectorHelper
|
||||
* protect buffer data
|
||||
* set error only if generic error bit is set, otherwise just warn about it
|
||||
* Fixes https://github.com/ipa320/ros_canopen/issues/81
|
||||
* Update emcy.cpp
|
||||
* removed debug outputs
|
||||
* refactored Layer mechanisms
|
||||
* simplified init
|
||||
* simplified EMCY handling
|
||||
* improved hearbeat handling
|
||||
* do not stop master on slave timeout
|
||||
* improved pending handling in complex layers
|
||||
* added set_cached for object entries
|
||||
* removed IPCLayer sync listener, loopback is disabled per default
|
||||
* Merge branch 'dummy_interface' into indigo_dev
|
||||
Conflicts:
|
||||
canopen_master/src/objdict.cpp
|
||||
* added sync silence feature
|
||||
* Merge remote-tracking branch 'origin/fix32bit' into indigo_dev
|
||||
* require message strings for error indicators, added missing strings, added ROS logging in sync loop
|
||||
* fix ambiguous buffer access with 32bit compilers
|
||||
* pad octet strings if necessary
|
||||
* reset pending to layers.begin()
|
||||
* enforce RPDO (device-side) transmimssion type to 1 if <=240
|
||||
* introduced LayerVector to unify pending support
|
||||
* introduced read_integer to enfoce hex parsing, closes `#74 <https://github.com/ros-industrial/ros_canopen/issues/74>`_
|
||||
* clear layer before plugin loader is deleted
|
||||
* Merge branch 'indigo_dev' of https://github.com/ipa320/ros_canopen into indigo_dev
|
||||
* Merge pull request `#70 <https://github.com/ros-industrial/ros_canopen/issues/70>`_ from ipa-mdl/pluginlib
|
||||
added plugin feature to socketcan_interface
|
||||
* exception-aware get functions
|
||||
* removed RPDO sync timeout in favour of LayerStatus
|
||||
* added message string helper
|
||||
* EDS files are case-insensitive, so switching to iptree
|
||||
* handle errors entries that are not in the dictionary
|
||||
* sub entry number must be hex coded
|
||||
* do not send initilized-only PDO data
|
||||
* init entries if init value was given and default value was not
|
||||
* implemented threading in CANLayer
|
||||
* removed bitrate, added loopback to DriverInterface::init
|
||||
* removed SimpleLayer, migrated to Layer
|
||||
* Layer::pending and Layer::halt are now virtual pure as well
|
||||
* schunk version of reset
|
||||
* Merge branch 'elmo_console' of https://github.com/ipa-mdl/ros_canopen into dcf_overlay
|
||||
* remove debug prints
|
||||
* resize buffer if needed in expedited SDO upload
|
||||
* fix SDO segment download
|
||||
* only access EMCY errors if available
|
||||
* added ObjectStorage:Entry::valid()
|
||||
* added ObjectDict overlay feature
|
||||
* Fixes the bus controller problems for the Elmo chain
|
||||
* Work-around for Elmo SDO bug(?)
|
||||
* improved PDO buffer initialization, buffer if filled per SDO if needed
|
||||
* pass permission object
|
||||
* disable threading interrupts while waiting for SDO response
|
||||
* Merge branch 'indigo_dev' into merge
|
||||
Conflicts:
|
||||
canopen_chain_node/include/canopen_chain_node/chain_ros.h
|
||||
canopen_master/include/canopen_master/canopen.h
|
||||
canopen_master/include/canopen_master/layer.h
|
||||
canopen_master/src/node.cpp
|
||||
canopen_motor_node/CMakeLists.txt
|
||||
canopen_motor_node/src/control_node.cpp
|
||||
* Contributors: Mathias Lüdtke, Thiago de Freitas Oliveira Araujo, ipa-cob4-2, ipa-fmw, thiagodefreitas
|
||||
|
||||
0.6.2 (2014-12-18)
|
||||
------------------
|
||||
|
||||
0.6.1 (2014-12-15)
|
||||
------------------
|
||||
* remove ipa_* and IPA_* prefixes
|
||||
* added descriptions and authors
|
||||
* renamed ipa_canopen_master to canopen_master
|
||||
* Contributors: Florian Weisshardt, Mathias Lüdtke
|
||||
102
Devices/Libraries/Ros/ros_canopen/canopen_master/CMakeLists.txt
Executable file
102
Devices/Libraries/Ros/ros_canopen/canopen_master/CMakeLists.txt
Executable file
@@ -0,0 +1,102 @@
|
||||
cmake_minimum_required(VERSION 3.0.2)
|
||||
project(canopen_master)
|
||||
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
set(CMAKE_CXX_STANDARD 14)
|
||||
endif()
|
||||
|
||||
find_package(catkin REQUIRED
|
||||
COMPONENTS
|
||||
class_loader
|
||||
socketcan_interface
|
||||
)
|
||||
|
||||
find_package(Boost REQUIRED
|
||||
COMPONENTS
|
||||
atomic
|
||||
chrono
|
||||
thread
|
||||
)
|
||||
|
||||
catkin_package(
|
||||
INCLUDE_DIRS
|
||||
include
|
||||
LIBRARIES
|
||||
${PROJECT_NAME}
|
||||
CATKIN_DEPENDS
|
||||
socketcan_interface
|
||||
DEPENDS
|
||||
Boost
|
||||
)
|
||||
include_directories(include ${catkin_INCLUDE_DIRS})
|
||||
|
||||
add_library(${PROJECT_NAME}
|
||||
src/emcy.cpp
|
||||
src/node.cpp
|
||||
src/objdict.cpp
|
||||
src/pdo.cpp
|
||||
src/sdo.cpp
|
||||
)
|
||||
target_link_libraries(${PROJECT_NAME}
|
||||
${catkin_LIBRARIES}
|
||||
${Boost_LIBRARIES}
|
||||
)
|
||||
|
||||
add_library(${PROJECT_NAME}_plugin
|
||||
src/master_plugin.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(${PROJECT_NAME}_plugin
|
||||
${catkin_LIBRARIES}
|
||||
${Boost_LIBRARIES}
|
||||
${PROJECT_NAME}
|
||||
)
|
||||
|
||||
# canopen_bcm_sync
|
||||
add_executable(canopen_bcm_sync
|
||||
src/bcm_sync.cpp
|
||||
)
|
||||
target_link_libraries(canopen_bcm_sync
|
||||
${Boost_LIBRARIES}
|
||||
${catkin_LIBRARIES}
|
||||
)
|
||||
|
||||
install(
|
||||
TARGETS
|
||||
canopen_bcm_sync
|
||||
${PROJECT_NAME}
|
||||
${PROJECT_NAME}_plugin
|
||||
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
|
||||
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
||||
install(
|
||||
DIRECTORY
|
||||
include/${PROJECT_NAME}/
|
||||
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
|
||||
FILES_MATCHING PATTERN "*.h"
|
||||
)
|
||||
|
||||
install(
|
||||
FILES
|
||||
master_plugin.xml
|
||||
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
|
||||
)
|
||||
|
||||
if(CATKIN_ENABLE_TESTING)
|
||||
catkin_add_gtest(${PROJECT_NAME}-test_parser
|
||||
test/test_parser.cpp
|
||||
)
|
||||
target_link_libraries(${PROJECT_NAME}-test_parser
|
||||
${PROJECT_NAME}
|
||||
)
|
||||
|
||||
catkin_add_gtest(${PROJECT_NAME}-test_node
|
||||
test/test_node.cpp
|
||||
)
|
||||
target_link_libraries(${PROJECT_NAME}-test_node
|
||||
${PROJECT_NAME}
|
||||
)
|
||||
endif()
|
||||
@@ -0,0 +1,167 @@
|
||||
#ifndef H_BCM_SYNC
|
||||
#define H_BCM_SYNC
|
||||
|
||||
#include <socketcan_interface/bcm.h>
|
||||
#include <socketcan_interface/socketcan.h>
|
||||
#include <canopen_master/canopen.h>
|
||||
|
||||
namespace canopen {
|
||||
|
||||
template<typename T > std::string join(const T &container, const std::string &delim){
|
||||
if(container.empty()) return std::string();
|
||||
std::stringstream sstr;
|
||||
typename T::const_iterator it = container.begin();
|
||||
sstr << *it;
|
||||
for(++it; it != container.end(); ++it){
|
||||
sstr << delim << *it;
|
||||
}
|
||||
return sstr.str();
|
||||
}
|
||||
|
||||
class BCMsync : public Layer {
|
||||
boost::mutex mutex_;
|
||||
|
||||
std::string device_;
|
||||
|
||||
std::set<int> ignored_nodes_;
|
||||
std::set<int> monitored_nodes_;
|
||||
std::set<int> known_nodes_;
|
||||
std::set<int> started_nodes_;
|
||||
|
||||
bool sync_running_;
|
||||
can::BCMsocket bcm_;
|
||||
can::SocketCANDriverSharedPtr driver_;
|
||||
uint16_t sync_ms_;
|
||||
can::FrameListenerConstSharedPtr handler_;
|
||||
|
||||
std::vector<can::Frame> sync_frames_;
|
||||
|
||||
bool skipNode(uint8_t id){
|
||||
if(ignored_nodes_.find(id) != ignored_nodes_.end()) return true;
|
||||
if(!monitored_nodes_.empty() && monitored_nodes_.find(id) == monitored_nodes_.end()) return true;
|
||||
known_nodes_.insert(id);
|
||||
return false;
|
||||
}
|
||||
|
||||
void handleFrame(const can::Frame &frame){
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
|
||||
if(frame.id == NMT_ID){
|
||||
if(frame.dlc > 1){
|
||||
uint8_t cmd = frame.data[0];
|
||||
uint8_t id = frame.data[1];
|
||||
if(skipNode(id)) return;
|
||||
|
||||
if(cmd == 1){ // started
|
||||
if(id != 0) started_nodes_.insert(id);
|
||||
else started_nodes_.insert(known_nodes_.begin(), known_nodes_.end()); // start all
|
||||
}else{
|
||||
if(id != 0) started_nodes_.erase(id);
|
||||
else started_nodes_.clear(); // stop all
|
||||
}
|
||||
}
|
||||
}else if((frame.id & ~ALL_NODES_MASK) == HEARTBEAT_ID){
|
||||
int id = frame.id & ALL_NODES_MASK;
|
||||
if(skipNode(id)) return;
|
||||
|
||||
if(frame.dlc > 0 && frame.data[0] == canopen::Node::Stopped) started_nodes_.erase(id);
|
||||
}
|
||||
|
||||
// toggle sync if needed
|
||||
if(started_nodes_.empty() && sync_running_){
|
||||
sync_running_ = !bcm_.stopTX(sync_frames_.front());
|
||||
}else if(!started_nodes_.empty() && !sync_running_){
|
||||
sync_running_ = bcm_.startTX(boost::chrono::milliseconds(sync_ms_),sync_frames_.front(), sync_frames_.size(), &sync_frames_[0]);
|
||||
}
|
||||
}
|
||||
protected:
|
||||
virtual void handleRead(LayerStatus &status, const LayerState ¤t_state) {
|
||||
if(current_state > Init){
|
||||
}
|
||||
}
|
||||
virtual void handleWrite(LayerStatus &status, const LayerState ¤t_state) {
|
||||
if(current_state > Init){
|
||||
}
|
||||
}
|
||||
virtual void handleDiag(LayerReport &report){
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
if(!sync_running_) report.warn("sync is not running");
|
||||
|
||||
report.add("sync_running", sync_running_);
|
||||
report.add("known_nodes", join(known_nodes_, ", "));
|
||||
report.add("started_nodes", join(started_nodes_, ", "));
|
||||
}
|
||||
|
||||
virtual void handleInit(LayerStatus &status){
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
started_nodes_.clear();
|
||||
|
||||
if(!bcm_.init(device_)){
|
||||
status.error("BCM_init failed");
|
||||
return;
|
||||
}
|
||||
int sc = driver_->getInternalSocket();
|
||||
|
||||
struct can_filter filter[2];
|
||||
|
||||
filter[0].can_id = NMT_ID;
|
||||
filter[0].can_mask = CAN_SFF_MASK;
|
||||
filter[1].can_id = HEARTBEAT_ID;
|
||||
filter[1].can_mask = ~ALL_NODES_MASK;
|
||||
|
||||
if(setsockopt(sc, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, sizeof(filter)) < 0){
|
||||
status.warn("could not set filter");
|
||||
return;
|
||||
}
|
||||
|
||||
handler_ = driver_->createMsgListenerM(this, &BCMsync::handleFrame);
|
||||
}
|
||||
virtual void handleShutdown(LayerStatus &status){
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
handler_.reset();
|
||||
bcm_.shutdown();
|
||||
}
|
||||
|
||||
virtual void handleHalt(LayerStatus &status) {
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
if(sync_running_){
|
||||
bcm_.stopTX(sync_frames_.front());
|
||||
sync_running_ = false;
|
||||
started_nodes_.clear();
|
||||
}
|
||||
}
|
||||
|
||||
virtual void handleRecover(LayerStatus &status){
|
||||
handleShutdown(status);
|
||||
handleInit(status);
|
||||
}
|
||||
public:
|
||||
static const uint32_t ALL_NODES_MASK = 127;
|
||||
static const uint32_t HEARTBEAT_ID = 0x700;
|
||||
static const uint32_t NMT_ID = 0x000;
|
||||
|
||||
BCMsync(const std::string &device, can::SocketCANDriverSharedPtr driver, const SyncProperties &sync_properties)
|
||||
: Layer(device + " SyncLayer"), device_(device), sync_running_(false), sync_ms_(sync_properties.period_ms_), driver_(driver) {
|
||||
if(sync_properties.overflow_ == 0){
|
||||
sync_frames_.resize(1);
|
||||
sync_frames_[0] = can::Frame(sync_properties.header_,0);
|
||||
}else{
|
||||
sync_frames_.resize(sync_properties.overflow_);
|
||||
for(int i = 0; i < sync_properties.overflow_; ++i){
|
||||
sync_frames_[i] = can::Frame(sync_properties.header_,1);
|
||||
sync_frames_[i].data[0] = i+1;
|
||||
}
|
||||
}
|
||||
}
|
||||
template <class T> void setMonitored(const T &other){
|
||||
monitored_nodes_.clear();
|
||||
monitored_nodes_.insert(other.begin(), other.end());
|
||||
}
|
||||
template <class T> void setIgnored(const T &other){
|
||||
ignored_nodes_.clear();
|
||||
ignored_nodes_.insert(other.begin(), other.end());
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
#endif
|
||||
@@ -0,0 +1,115 @@
|
||||
#ifndef H_CAN_LAYER
|
||||
#define H_CAN_LAYER
|
||||
|
||||
#include <socketcan_interface/threading.h>
|
||||
#include "layer.h"
|
||||
#include <socketcan_interface/string.h>
|
||||
|
||||
namespace canopen{
|
||||
|
||||
class CANLayer: public Layer{
|
||||
boost::mutex mutex_;
|
||||
can::DriverInterfaceSharedPtr driver_;
|
||||
const std::string device_;
|
||||
const bool loopback_;
|
||||
can::SettingsConstSharedPtr settings_;
|
||||
can::Frame last_error_;
|
||||
can::FrameListenerConstSharedPtr error_listener_;
|
||||
void handleFrame(const can::Frame & msg){
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
last_error_ = msg;
|
||||
ROSCANOPEN_ERROR("canopen_master", "Received error frame: " << msg);
|
||||
}
|
||||
std::shared_ptr<boost::thread> thread_;
|
||||
|
||||
public:
|
||||
CANLayer(const can::DriverInterfaceSharedPtr &driver, const std::string &device, bool loopback, can::SettingsConstSharedPtr settings)
|
||||
: Layer(device + " Layer"), driver_(driver), device_(device), loopback_(loopback), settings_(settings) { assert(driver_); }
|
||||
|
||||
[[deprecated("provide settings explicitly")]] CANLayer(const can::DriverInterfaceSharedPtr &driver, const std::string &device, bool loopback)
|
||||
: Layer(device + " Layer"), driver_(driver), device_(device), loopback_(loopback), settings_(can::NoSettings::create()) { assert(driver_); }
|
||||
|
||||
virtual void handleRead(LayerStatus &status, const LayerState ¤t_state) {
|
||||
if(current_state > Init){
|
||||
if(!driver_->getState().isReady()) status.error("CAN not ready");
|
||||
}
|
||||
}
|
||||
virtual void handleWrite(LayerStatus &status, const LayerState ¤t_state) {
|
||||
if(current_state > Init){
|
||||
if(!driver_->getState().isReady()) status.error("CAN not ready");
|
||||
}
|
||||
}
|
||||
|
||||
virtual void handleDiag(LayerReport &report){
|
||||
can::State s = driver_->getState();
|
||||
if(!s.isReady()){
|
||||
report.error("CAN layer not ready");
|
||||
report.add("driver_state", int(s.driver_state));
|
||||
}
|
||||
if(s.error_code){
|
||||
report.add("socket_error", s.error_code);
|
||||
}
|
||||
if(s.internal_error != 0){
|
||||
report.add("internal_error", int(s.internal_error));
|
||||
std::string desc;
|
||||
if(driver_->translateError(s.internal_error, desc)) report.add("internal_error_desc", desc);
|
||||
std::stringstream sstr;
|
||||
sstr << std::hex;
|
||||
{
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
for(size_t i=0; i < last_error_.dlc; ++i){
|
||||
sstr << (unsigned int) last_error_.data[i] << " ";
|
||||
}
|
||||
}
|
||||
report.add("can_error_frame", sstr.str());
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
virtual void handleInit(LayerStatus &status){
|
||||
if(thread_){
|
||||
status.warn("CAN thread already running");
|
||||
} else if(!driver_->init(device_, loopback_, settings_)) {
|
||||
status.error("CAN init failed");
|
||||
} else {
|
||||
can::StateWaiter waiter(driver_.get());
|
||||
|
||||
thread_.reset(new boost::thread(&can::DriverInterface::run, driver_));
|
||||
error_listener_ = driver_->createMsgListenerM(can::ErrorHeader(),this, &CANLayer::handleFrame);
|
||||
|
||||
if(!waiter.wait(can::State::ready, boost::posix_time::seconds(1))){
|
||||
status.error("CAN init timed out");
|
||||
}
|
||||
}
|
||||
if(!driver_->getState().isReady()){
|
||||
status.error("CAN is not ready");
|
||||
}
|
||||
}
|
||||
virtual void handleShutdown(LayerStatus &status){
|
||||
can::StateWaiter waiter(driver_.get());
|
||||
error_listener_.reset();
|
||||
driver_->shutdown();
|
||||
if(!waiter.wait(can::State::closed, boost::posix_time::seconds(1))){
|
||||
status.warn("CAN shutdown timed out");
|
||||
}
|
||||
if(thread_){
|
||||
thread_->interrupt();
|
||||
thread_->join();
|
||||
thread_.reset();
|
||||
}
|
||||
}
|
||||
|
||||
virtual void handleHalt(LayerStatus &status) { /* nothing to do */ }
|
||||
|
||||
virtual void handleRecover(LayerStatus &status){
|
||||
if(!driver_->getState().isReady()){
|
||||
handleShutdown(status);
|
||||
handleInit(status);
|
||||
}
|
||||
}
|
||||
|
||||
};
|
||||
} // namespace canopen
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,331 @@
|
||||
#ifndef H_CANOPEN
|
||||
#define H_CANOPEN
|
||||
|
||||
#include <socketcan_interface/interface.h>
|
||||
#include <socketcan_interface/dispatcher.h>
|
||||
#include <socketcan_interface/reader.h>
|
||||
#include "exceptions.h"
|
||||
#include "layer.h"
|
||||
#include "objdict.h"
|
||||
#include "timer.h"
|
||||
#include <stdexcept>
|
||||
#include <boost/thread/condition_variable.hpp>
|
||||
#include <boost/chrono/system_clocks.hpp>
|
||||
#include <boost/lexical_cast.hpp>
|
||||
|
||||
namespace canopen{
|
||||
|
||||
typedef boost::chrono::high_resolution_clock::time_point time_point;
|
||||
typedef boost::chrono::high_resolution_clock::duration time_duration;
|
||||
inline time_point get_abs_time(const time_duration& timeout) { return boost::chrono::high_resolution_clock::now() + timeout; }
|
||||
inline time_point get_abs_time() { return boost::chrono::high_resolution_clock::now(); }
|
||||
|
||||
|
||||
template<typename T> struct FrameOverlay: public can::Frame{
|
||||
T &data;
|
||||
FrameOverlay(const Header &h) : can::Frame(h,sizeof(T)), data(* (T*) can::Frame::c_array()) {
|
||||
can::Frame::data.fill(0);
|
||||
}
|
||||
FrameOverlay(const can::Frame &f) : can::Frame(f), data(* (T*) can::Frame::c_array()) { }
|
||||
};
|
||||
|
||||
class SDOClient{
|
||||
|
||||
can::Header client_id;
|
||||
|
||||
boost::timed_mutex mutex;
|
||||
|
||||
can::BufferedReader reader_;
|
||||
bool processFrame(const can::Frame & msg);
|
||||
|
||||
String buffer;
|
||||
size_t offset;
|
||||
size_t total;
|
||||
bool done;
|
||||
can::Frame last_msg;
|
||||
const canopen::ObjectDict::Entry * current_entry;
|
||||
|
||||
void transmitAndWait(const canopen::ObjectDict::Entry &entry, const String &data, String *result);
|
||||
void abort(uint32_t reason);
|
||||
|
||||
const can::CommInterfaceSharedPtr interface_;
|
||||
protected:
|
||||
void read(const canopen::ObjectDict::Entry &entry, String &data);
|
||||
void write(const canopen::ObjectDict::Entry &entry, const String &data);
|
||||
public:
|
||||
const ObjectStorageSharedPtr storage_;
|
||||
|
||||
void init();
|
||||
|
||||
SDOClient(const can::CommInterfaceSharedPtr interface, const ObjectDictSharedPtr dict, uint8_t node_id)
|
||||
: interface_(interface),
|
||||
storage_(std::make_shared<ObjectStorage>(dict, node_id,
|
||||
std::bind(&SDOClient::read, this, std::placeholders::_1, std::placeholders::_2),
|
||||
std::bind(&SDOClient::write, this, std::placeholders::_1, std::placeholders::_2))
|
||||
),
|
||||
reader_(false, 1)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
class PDOMapper{
|
||||
boost::mutex mutex_;
|
||||
|
||||
class Buffer{
|
||||
public:
|
||||
bool read(uint8_t* b, const size_t len);
|
||||
void write(const uint8_t* b, const size_t len);
|
||||
void read(const canopen::ObjectDict::Entry &entry, String &data);
|
||||
void write(const canopen::ObjectDict::Entry &, const String &data);
|
||||
void clean() { dirty = false; }
|
||||
const size_t size;
|
||||
Buffer(const size_t sz) : size(sz), dirty(false), empty(true), buffer(sz) {}
|
||||
|
||||
private:
|
||||
boost::mutex mutex;
|
||||
bool dirty;
|
||||
bool empty;
|
||||
std::vector<char> buffer;
|
||||
};
|
||||
typedef std::shared_ptr<Buffer> BufferSharedPtr;
|
||||
|
||||
class PDO {
|
||||
protected:
|
||||
void parse_and_set_mapping(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index, const bool &read, const bool &write);
|
||||
can::Frame frame;
|
||||
uint8_t transmission_type;
|
||||
std::vector<BufferSharedPtr>buffers;
|
||||
};
|
||||
|
||||
struct TPDO: public PDO{
|
||||
typedef std::shared_ptr<TPDO> TPDOSharedPtr;
|
||||
void sync();
|
||||
static TPDOSharedPtr create(const can::CommInterfaceSharedPtr interface, const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index){
|
||||
TPDOSharedPtr tpdo(new TPDO(interface));
|
||||
if(!tpdo->init(storage, com_index, map_index))
|
||||
tpdo.reset();
|
||||
return tpdo;
|
||||
}
|
||||
private:
|
||||
TPDO(const can::CommInterfaceSharedPtr interface) : interface_(interface){}
|
||||
bool init(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index);
|
||||
const can::CommInterfaceSharedPtr interface_;
|
||||
boost::mutex mutex;
|
||||
};
|
||||
|
||||
struct RPDO : public PDO{
|
||||
void sync(LayerStatus &status);
|
||||
typedef std::shared_ptr<RPDO> RPDOSharedPtr;
|
||||
static RPDOSharedPtr create(const can::CommInterfaceSharedPtr interface, const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index){
|
||||
RPDOSharedPtr rpdo(new RPDO(interface));
|
||||
if(!rpdo->init(storage, com_index, map_index))
|
||||
rpdo.reset();
|
||||
return rpdo;
|
||||
}
|
||||
private:
|
||||
bool init(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index);
|
||||
RPDO(const can::CommInterfaceSharedPtr interface) : interface_(interface), timeout(-1) {}
|
||||
boost::mutex mutex;
|
||||
const can::CommInterfaceSharedPtr interface_;
|
||||
|
||||
can::FrameListenerConstSharedPtr listener_;
|
||||
void handleFrame(const can::Frame & msg);
|
||||
int timeout;
|
||||
};
|
||||
|
||||
std::unordered_set<RPDO::RPDOSharedPtr> rpdos_;
|
||||
std::unordered_set<TPDO::TPDOSharedPtr> tpdos_;
|
||||
|
||||
const can::CommInterfaceSharedPtr interface_;
|
||||
|
||||
public:
|
||||
PDOMapper(const can::CommInterfaceSharedPtr interface);
|
||||
void read(LayerStatus &status);
|
||||
bool write();
|
||||
bool init(const ObjectStorageSharedPtr storage, LayerStatus &status);
|
||||
};
|
||||
|
||||
class EMCYHandler : public Layer {
|
||||
std::atomic<bool> has_error_;
|
||||
ObjectStorage::Entry<uint8_t> error_register_;
|
||||
ObjectStorage::Entry<uint8_t> num_errors_;
|
||||
can::FrameListenerConstSharedPtr emcy_listener_;
|
||||
void handleEMCY(const can::Frame & msg);
|
||||
const ObjectStorageSharedPtr storage_;
|
||||
|
||||
virtual void handleDiag(LayerReport &report);
|
||||
|
||||
virtual void handleInit(LayerStatus &status);
|
||||
virtual void handleRecover(LayerStatus &status);
|
||||
virtual void handleRead(LayerStatus &status, const LayerState ¤t_state);
|
||||
virtual void handleWrite(LayerStatus &status, const LayerState ¤t_state);
|
||||
virtual void handleHalt(LayerStatus &status);
|
||||
virtual void handleShutdown(LayerStatus &status);
|
||||
|
||||
public:
|
||||
EMCYHandler(const can::CommInterfaceSharedPtr interface, const ObjectStorageSharedPtr storage);
|
||||
void resetErrors(LayerStatus &status);
|
||||
};
|
||||
|
||||
struct SyncProperties{
|
||||
const can::Header header_;
|
||||
const uint16_t period_ms_;
|
||||
const uint8_t overflow_;
|
||||
SyncProperties(const can::Header &h, const uint16_t &p, const uint8_t &o) : header_(h), period_ms_(p), overflow_(o) {}
|
||||
bool operator==(const SyncProperties &p) const { return p.header_.key() == header_.key() && p.overflow_ == overflow_ && p.period_ms_ == period_ms_; }
|
||||
|
||||
};
|
||||
|
||||
class SyncCounter {
|
||||
public:
|
||||
const SyncProperties properties;
|
||||
SyncCounter(const SyncProperties &p) : properties(p) {}
|
||||
virtual void addNode(void * const ptr) = 0;
|
||||
virtual void removeNode(void * const ptr) = 0;
|
||||
virtual ~SyncCounter() {}
|
||||
};
|
||||
typedef std::shared_ptr<SyncCounter> SyncCounterSharedPtr;
|
||||
|
||||
class Node : public Layer{
|
||||
public:
|
||||
enum State{
|
||||
Unknown = 255, BootUp = 0, Stopped = 4, Operational = 5 , PreOperational = 127
|
||||
};
|
||||
const uint8_t node_id_;
|
||||
Node(const can::CommInterfaceSharedPtr interface, const ObjectDictSharedPtr dict, uint8_t node_id, const SyncCounterSharedPtr sync = SyncCounterSharedPtr());
|
||||
|
||||
const State getState();
|
||||
void enterState(const State &s);
|
||||
|
||||
const ObjectStorageSharedPtr getStorage() { return sdo_.storage_; }
|
||||
|
||||
bool start();
|
||||
bool stop();
|
||||
bool reset();
|
||||
bool reset_com();
|
||||
bool prepare();
|
||||
|
||||
using StateFunc = std::function<void(const State&)>;
|
||||
using StateDelegate [[deprecated("use StateFunc instead")]] = can::DelegateHelper<StateFunc>;
|
||||
|
||||
typedef can::Listener<const StateFunc, const State&> StateListener;
|
||||
typedef StateListener::ListenerConstSharedPtr StateListenerConstSharedPtr;
|
||||
|
||||
StateListenerConstSharedPtr addStateListener(const StateFunc & s){
|
||||
return state_dispatcher_.createListener(s);
|
||||
}
|
||||
|
||||
template<typename T> T get(const ObjectDict::Key& k){
|
||||
return getStorage()->entry<T>(k).get();
|
||||
}
|
||||
|
||||
private:
|
||||
virtual void handleDiag(LayerReport &report);
|
||||
|
||||
virtual void handleInit(LayerStatus &status);
|
||||
virtual void handleRecover(LayerStatus &status);
|
||||
virtual void handleRead(LayerStatus &status, const LayerState ¤t_state);
|
||||
virtual void handleWrite(LayerStatus &status, const LayerState ¤t_state);
|
||||
virtual void handleHalt(LayerStatus &status);
|
||||
virtual void handleShutdown(LayerStatus &status);
|
||||
|
||||
template<typename T> int wait_for(const State &s, const T &timeout);
|
||||
|
||||
boost::timed_mutex mutex;
|
||||
boost::mutex cond_mutex;
|
||||
boost::condition_variable cond;
|
||||
|
||||
const can::CommInterfaceSharedPtr interface_;
|
||||
const SyncCounterSharedPtr sync_;
|
||||
can::FrameListenerConstSharedPtr nmt_listener_;
|
||||
|
||||
ObjectStorage::Entry<ObjectStorage::DataType<ObjectDict::DEFTYPE_UNSIGNED16>::type> heartbeat_;
|
||||
|
||||
can::SimpleDispatcher<StateListener> state_dispatcher_;
|
||||
|
||||
void handleNMT(const can::Frame & msg);
|
||||
void switchState(const uint8_t &s);
|
||||
|
||||
State state_;
|
||||
SDOClient sdo_;
|
||||
PDOMapper pdo_;
|
||||
|
||||
boost::chrono::high_resolution_clock::time_point heartbeat_timeout_;
|
||||
uint16_t getHeartbeatInterval() { return heartbeat_.valid()?heartbeat_.get_cached() : 0; }
|
||||
void setHeartbeatInterval() { if(heartbeat_.valid()) heartbeat_.set(heartbeat_.desc().value().get<uint16_t>()); }
|
||||
bool checkHeartbeat();
|
||||
};
|
||||
typedef std::shared_ptr<Node> NodeSharedPtr;
|
||||
|
||||
template<typename T> class Chain{
|
||||
public:
|
||||
typedef std::shared_ptr<T> MemberSharedPtr;
|
||||
void call(void (T::*func)(void)){
|
||||
typename std::vector<MemberSharedPtr>::iterator it = elements.begin();
|
||||
while(it != elements.end()){
|
||||
((**it).*func)();
|
||||
++it;
|
||||
}
|
||||
}
|
||||
template<typename V> void call(void (T::*func)(const V&), const std::vector<V> &vs){
|
||||
typename std::vector<MemberSharedPtr>::iterator it = elements.begin();
|
||||
typename std::vector<V>::const_iterator it_v = vs.begin();
|
||||
while(it_v != vs.end() && it != elements.end()){
|
||||
((**it).*func)(*it_v);
|
||||
++it; ++it_v;
|
||||
}
|
||||
}
|
||||
template<typename V> void call(void (T::*func)(V&), std::vector<V> &vs){
|
||||
vs.resize(elements.size());
|
||||
|
||||
typename std::vector<MemberSharedPtr>::iterator it = elements.begin();
|
||||
typename std::vector<V>::iterator it_v = vs.begin();
|
||||
while(it_v != vs.end() && it != elements.end()){
|
||||
((**it).*func)(*it_v);
|
||||
++it; ++it_v;
|
||||
}
|
||||
}
|
||||
void add(MemberSharedPtr t){
|
||||
elements.push_back(t);
|
||||
}
|
||||
protected:
|
||||
std::vector<MemberSharedPtr> elements;
|
||||
};
|
||||
|
||||
template<typename T> class NodeChain: public Chain<T>{
|
||||
public:
|
||||
const std::vector<typename Chain<T>::MemberSharedPtr>& getElements() { return Chain<T>::elements; }
|
||||
void start() { this->call(&T::start); }
|
||||
void stop() { this->call(&T::stop); }
|
||||
void reset() { this->call(&T::reset); }
|
||||
void reset_com() { this->call(&T::reset_com); }
|
||||
void prepare() { this->call(&T::prepare); }
|
||||
};
|
||||
|
||||
class SyncLayer: public Layer, public SyncCounter{
|
||||
public:
|
||||
SyncLayer(const SyncProperties &p) : Layer("Sync layer"), SyncCounter(p) {}
|
||||
};
|
||||
typedef std::shared_ptr<SyncLayer> SyncLayerSharedPtr;
|
||||
|
||||
class Master{
|
||||
Master(const Master&) = delete; // prevent copies
|
||||
Master& operator=(const Master&) = delete;
|
||||
public:
|
||||
Master() = default;
|
||||
virtual SyncLayerSharedPtr getSync(const SyncProperties &properties) = 0;
|
||||
virtual ~Master() {}
|
||||
|
||||
typedef std::shared_ptr<Master> MasterSharedPtr;
|
||||
class Allocator {
|
||||
public:
|
||||
virtual MasterSharedPtr allocate(const std::string &name, can::CommInterfaceSharedPtr interface) = 0;
|
||||
virtual ~Allocator() {}
|
||||
};
|
||||
};
|
||||
typedef Master::MasterSharedPtr MasterSharedPtr;
|
||||
|
||||
using can::Settings;
|
||||
|
||||
} // canopen
|
||||
#endif // !H_CANOPEN
|
||||
@@ -0,0 +1,33 @@
|
||||
#ifndef H_EXCEPTIONS
|
||||
#define H_EXCEPTIONS
|
||||
|
||||
#include <exception>
|
||||
#include <boost/exception/all.hpp>
|
||||
#include <boost/format.hpp>
|
||||
|
||||
namespace canopen{
|
||||
|
||||
class Exception : public std::runtime_error {
|
||||
public:
|
||||
Exception(const std::string &w) : std::runtime_error(w) {}
|
||||
};
|
||||
|
||||
class PointerInvalid : public Exception{
|
||||
public:
|
||||
PointerInvalid(const std::string &w) : Exception("Pointer invalid") {}
|
||||
};
|
||||
|
||||
class ParseException : public Exception{
|
||||
public:
|
||||
ParseException(const std::string &w) : Exception(w) {}
|
||||
};
|
||||
|
||||
class TimeoutException : public Exception{
|
||||
public:
|
||||
TimeoutException(const std::string &w) : Exception(w) {}
|
||||
};
|
||||
|
||||
|
||||
} // canopen
|
||||
|
||||
#endif // !H_EXCEPTIONS
|
||||
260
Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/layer.h
Executable file
260
Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/layer.h
Executable file
@@ -0,0 +1,260 @@
|
||||
#ifndef H_CANOPEN_LAYER
|
||||
#define H_CANOPEN_LAYER
|
||||
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
#include <boost/thread/shared_mutex.hpp>
|
||||
#include <atomic>
|
||||
#include <boost/exception/diagnostic_information.hpp>
|
||||
|
||||
namespace canopen{
|
||||
|
||||
class LayerStatus{
|
||||
mutable boost::mutex write_mutex_;
|
||||
enum State{
|
||||
OK = 0, WARN = 1, ERROR= 2, STALE = 3, UNBOUNDED = 3
|
||||
};
|
||||
std::atomic<State> state;
|
||||
std::string reason_;
|
||||
|
||||
virtual void set(const State &s, const std::string &r){
|
||||
boost::mutex::scoped_lock lock(write_mutex_);
|
||||
if(s > state) state = s;
|
||||
if(!r.empty()){
|
||||
if(reason_.empty()) reason_ = r;
|
||||
else reason_ += "; " + r;
|
||||
}
|
||||
}
|
||||
public:
|
||||
struct Ok { static const State state = OK; private: Ok();};
|
||||
struct Warn { static const State state = WARN; private: Warn(); };
|
||||
struct Error { static const State state = ERROR; private: Error(); };
|
||||
struct Stale { static const State state = STALE; private: Stale(); };
|
||||
struct Unbounded { static const State state = UNBOUNDED; private: Unbounded(); };
|
||||
|
||||
template<typename T> bool bounded() const{ return state <= T::state; }
|
||||
template<typename T> bool equals() const{ return state == T::state; }
|
||||
|
||||
LayerStatus() : state(OK) {}
|
||||
|
||||
int get() const { return state; }
|
||||
|
||||
const std::string reason() const { boost::mutex::scoped_lock lock(write_mutex_); return reason_; }
|
||||
|
||||
const void warn(const std::string & r) { set(WARN, r); }
|
||||
const void error(const std::string & r) { set(ERROR, r); }
|
||||
const void stale(const std::string & r) { set(STALE, r); }
|
||||
};
|
||||
class LayerReport : public LayerStatus {
|
||||
std::vector<std::pair<std::string, std::string> > values_;
|
||||
public:
|
||||
const std::vector<std::pair<std::string, std::string> > &values() const { return values_; }
|
||||
template<typename T> void add(const std::string &key, const T &value) {
|
||||
std::stringstream str;
|
||||
str << value;
|
||||
values_.push_back(std::make_pair(key,str.str()));
|
||||
}
|
||||
};
|
||||
|
||||
#define CATCH_LAYER_HANDLER_EXCEPTIONS(command, status) \
|
||||
try{ command; } \
|
||||
catch(std::exception &e) {status.error(boost::diagnostic_information(e)); }
|
||||
|
||||
class Layer{
|
||||
public:
|
||||
enum LayerState{
|
||||
Off,
|
||||
Init,
|
||||
Shutdown,
|
||||
Error,
|
||||
Halt,
|
||||
Recover,
|
||||
Ready
|
||||
};
|
||||
|
||||
const std::string name;
|
||||
|
||||
void read(LayerStatus &status) {
|
||||
if(state > Off) CATCH_LAYER_HANDLER_EXCEPTIONS(handleRead(status, state), status);
|
||||
}
|
||||
void write(LayerStatus &status) {
|
||||
if(state > Off) CATCH_LAYER_HANDLER_EXCEPTIONS(handleWrite(status, state), status);
|
||||
}
|
||||
void diag(LayerReport &report) {
|
||||
if(state > Shutdown) CATCH_LAYER_HANDLER_EXCEPTIONS(handleDiag(report), report);
|
||||
}
|
||||
void init(LayerStatus &status) {
|
||||
if(state == Off){
|
||||
if(status.bounded<LayerStatus::Warn>()){
|
||||
state = Init;
|
||||
CATCH_LAYER_HANDLER_EXCEPTIONS(handleInit(status), status);
|
||||
}
|
||||
if(!status.bounded<LayerStatus::Warn>()) shutdown(status);
|
||||
else state = Ready;
|
||||
}
|
||||
}
|
||||
void shutdown(LayerStatus &status) {
|
||||
if(state != Off){
|
||||
state = Shutdown;
|
||||
CATCH_LAYER_HANDLER_EXCEPTIONS(handleShutdown(status), status);
|
||||
state = Off;
|
||||
}
|
||||
}
|
||||
void halt(LayerStatus &status) {
|
||||
if(state > Halt){
|
||||
state = Halt;
|
||||
CATCH_LAYER_HANDLER_EXCEPTIONS(handleHalt(status), status);
|
||||
state = Error;
|
||||
}
|
||||
}
|
||||
void recover(LayerStatus &status) {
|
||||
if(state == Error){
|
||||
if(status.bounded<LayerStatus::Warn>()){
|
||||
state = Recover;
|
||||
CATCH_LAYER_HANDLER_EXCEPTIONS(handleRecover(status), status);
|
||||
}
|
||||
if(!status.bounded<LayerStatus::Warn>()){
|
||||
halt(status);
|
||||
}else{
|
||||
state = Ready;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
LayerState getLayerState() { return state; }
|
||||
|
||||
Layer(const std::string &n) : name(n), state(Off) {}
|
||||
|
||||
virtual ~Layer() {}
|
||||
|
||||
protected:
|
||||
virtual void handleRead(LayerStatus &status, const LayerState ¤t_state) = 0;
|
||||
virtual void handleWrite(LayerStatus &status, const LayerState ¤t_state) = 0;
|
||||
|
||||
virtual void handleDiag(LayerReport &report) = 0;
|
||||
|
||||
virtual void handleInit(LayerStatus &status) = 0;
|
||||
virtual void handleShutdown(LayerStatus &status) = 0;
|
||||
|
||||
virtual void handleHalt(LayerStatus &status) = 0;
|
||||
virtual void handleRecover(LayerStatus &status) = 0;
|
||||
|
||||
private:
|
||||
std::atomic<LayerState> state;
|
||||
|
||||
};
|
||||
|
||||
template<typename T> class VectorHelper{
|
||||
public:
|
||||
typedef std::shared_ptr<T> VectorMemberSharedPtr;
|
||||
protected:
|
||||
typedef std::vector<VectorMemberSharedPtr> vector_type ;
|
||||
template<typename Bound, typename Data, typename FuncType> typename vector_type::iterator call(FuncType func, Data &status){
|
||||
boost::shared_lock<boost::shared_mutex> lock(mutex);
|
||||
return call<Bound>(func, status, layers.begin(), layers.end());
|
||||
}
|
||||
template<typename Data, typename FuncType> typename vector_type::iterator call(FuncType func, Data &status){
|
||||
boost::shared_lock<boost::shared_mutex> lock(mutex);
|
||||
return call<LayerStatus::Unbounded>(func, status, layers.begin(), layers.end());
|
||||
}
|
||||
template<typename Bound, typename Data, typename FuncType> typename vector_type::reverse_iterator call_rev(FuncType func, Data &status){
|
||||
boost::shared_lock<boost::shared_mutex> lock(mutex);
|
||||
return call<Bound>(func, status, layers.rbegin(), layers.rend());
|
||||
}
|
||||
template<typename Data, typename FuncType> typename vector_type::reverse_iterator call_rev(FuncType func, Data &status){
|
||||
boost::shared_lock<boost::shared_mutex> lock(mutex);
|
||||
return call<LayerStatus::Unbounded>(func, status, layers.rbegin(), layers.rend());
|
||||
}
|
||||
void destroy() { boost::unique_lock<boost::shared_mutex> lock(mutex); layers.clear(); }
|
||||
|
||||
public:
|
||||
virtual void add(const VectorMemberSharedPtr &l) { boost::unique_lock<boost::shared_mutex> lock(mutex); layers.push_back(l); }
|
||||
|
||||
template<typename Bound, typename Data, typename FuncType> bool callFunc(FuncType func, Data &status){
|
||||
boost::shared_lock<boost::shared_mutex> lock(mutex);
|
||||
return call<Bound>(func, status, layers.begin(), layers.end()) == layers.end();
|
||||
}
|
||||
private:
|
||||
vector_type layers;
|
||||
boost::shared_mutex mutex;
|
||||
|
||||
template<typename Bound, typename Iterator, typename Data, typename FuncType> Iterator call(FuncType func, Data &status, const Iterator &begin, const Iterator &end){
|
||||
bool okay_on_start = status.template bounded<Bound>();
|
||||
|
||||
for(Iterator it = begin; it != end; ++it){
|
||||
((**it).*func)(status);
|
||||
if(okay_on_start && !status.template bounded<Bound>()){
|
||||
return it;
|
||||
}
|
||||
}
|
||||
return end;
|
||||
}
|
||||
template<typename Iterator, typename Data, typename FuncType> Iterator call(FuncType func, Data &status, const Iterator &begin, const Iterator &end){
|
||||
return call<LayerStatus::Unbounded, Iterator, Data>(func, status, begin, end);
|
||||
}
|
||||
};
|
||||
|
||||
template<typename T=Layer> class LayerGroup : public Layer, public VectorHelper<T> {
|
||||
protected:
|
||||
template<typename Data, typename FuncType, typename FailType> void call_or_fail(FuncType func, FailType fail, Data &status){
|
||||
this->template call(func, status);
|
||||
if(!status.template bounded<LayerStatus::Warn>()){
|
||||
this->template call(fail, status);
|
||||
(this->*fail)(status);
|
||||
}
|
||||
}
|
||||
template<typename Data, typename FuncType, typename FailType> void call_or_fail_rev(FuncType func, FailType fail, Data &status){
|
||||
this->template call_rev(func, status);
|
||||
if(!status.template bounded<LayerStatus::Warn>()){
|
||||
this->template call_rev(fail, status);
|
||||
(this->*fail)(status);
|
||||
}
|
||||
}
|
||||
|
||||
virtual void handleRead(LayerStatus &status, const LayerState ¤t_state) {
|
||||
this->template call_or_fail(&Layer::read, &Layer::halt, status);
|
||||
}
|
||||
virtual void handleWrite(LayerStatus &status, const LayerState ¤t_state) {
|
||||
this->template call_or_fail(&Layer::write, &Layer::halt, status);
|
||||
}
|
||||
|
||||
virtual void handleDiag(LayerReport &report) { this->template call(&Layer::diag, report); }
|
||||
|
||||
virtual void handleInit(LayerStatus &status) { this->template call<LayerStatus::Warn>(&Layer::init, status); }
|
||||
virtual void handleShutdown(LayerStatus &status) { this->template call(&Layer::shutdown, status); }
|
||||
|
||||
virtual void handleHalt(LayerStatus &status) { this->template call(&Layer::halt, status); }
|
||||
virtual void handleRecover(LayerStatus &status) { this->template call<LayerStatus::Warn>(&Layer::recover, status); }
|
||||
public:
|
||||
LayerGroup(const std::string &n) : Layer(n) {}
|
||||
};
|
||||
|
||||
class LayerStack : public LayerGroup<>{
|
||||
|
||||
protected:
|
||||
virtual void handleWrite(LayerStatus &status, const LayerState ¤t_state) { call_or_fail_rev(&Layer::write, &Layer::halt, status);}
|
||||
virtual void handleShutdown(LayerStatus &status) { call_rev(&Layer::shutdown, status); }
|
||||
public:
|
||||
LayerStack(const std::string &n) : LayerGroup(n) {}
|
||||
};
|
||||
|
||||
template<typename T> class LayerGroupNoDiag : public LayerGroup<T>{
|
||||
public:
|
||||
LayerGroupNoDiag(const std::string &n) : LayerGroup<T>(n) {}
|
||||
virtual void handleDiag(LayerReport &report) {}
|
||||
};
|
||||
|
||||
template<typename T> class DiagGroup : public VectorHelper<T>{
|
||||
typedef VectorHelper<T> V;
|
||||
public:
|
||||
virtual void diag(LayerReport &report){
|
||||
this->template call(&Layer::diag, report);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
} // namespace canopen
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,587 @@
|
||||
#ifndef H_OBJDICT
|
||||
#define H_OBJDICT
|
||||
|
||||
#include <list>
|
||||
#include <memory>
|
||||
#include <unordered_map>
|
||||
#include <unordered_set>
|
||||
|
||||
#include <socketcan_interface/delegates.h>
|
||||
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <functional>
|
||||
#include <typeinfo>
|
||||
#include <vector>
|
||||
#include "exceptions.h"
|
||||
|
||||
namespace canopen{
|
||||
|
||||
class TypeGuard{
|
||||
const std::type_info& (*get_type)();
|
||||
size_t type_size;
|
||||
|
||||
template<typename T> class TypeInfo{
|
||||
public:
|
||||
static const std::type_info& id() { return typeid(T); }
|
||||
};
|
||||
TypeGuard(const std::type_info& (*ti)(), const size_t s): get_type(ti), type_size(s) {}
|
||||
public:
|
||||
|
||||
template<typename T> bool is_type() const {
|
||||
return valid() && get_type() == typeid(T);
|
||||
}
|
||||
|
||||
bool operator==(const TypeGuard &other) const {
|
||||
return valid() && other.valid() && (get_type() == other.get_type());
|
||||
}
|
||||
|
||||
TypeGuard(): get_type(0), type_size(0) {}
|
||||
bool valid() const { return get_type != 0; }
|
||||
size_t get_size() const { return type_size; }
|
||||
template<typename T> static TypeGuard create() { return TypeGuard(TypeInfo<T>::id, sizeof(T)); }
|
||||
};
|
||||
|
||||
class String: public std::vector<char>{
|
||||
public:
|
||||
String() {}
|
||||
String(const std::string &str) : std::vector<char>(str.begin(), str.end()) {}
|
||||
operator const char * () const {
|
||||
return &at(0);
|
||||
}
|
||||
operator const std::string () const {
|
||||
return std::string(begin(), end());
|
||||
}
|
||||
};
|
||||
|
||||
class HoldAny{
|
||||
String buffer;
|
||||
TypeGuard type_guard;
|
||||
bool empty;
|
||||
public:
|
||||
HoldAny() : empty(true) {}
|
||||
|
||||
const TypeGuard& type() const{ return type_guard; }
|
||||
|
||||
template<typename T> HoldAny(const T &t) : type_guard(TypeGuard::create<T>()), empty(false){
|
||||
buffer.resize(sizeof(T));
|
||||
*(T*)&(buffer.front()) = t;
|
||||
}
|
||||
HoldAny(const std::string &t): type_guard(TypeGuard::create<std::string>()), empty(false){
|
||||
if(!type_guard.is_type<std::string>()){
|
||||
BOOST_THROW_EXCEPTION(std::bad_cast());
|
||||
}
|
||||
buffer = t;
|
||||
}
|
||||
HoldAny(const TypeGuard &t): type_guard(t), empty(true){ }
|
||||
|
||||
bool is_empty() const { return empty; }
|
||||
|
||||
const String& data() const {
|
||||
if(empty){
|
||||
BOOST_THROW_EXCEPTION(std::length_error("buffer empty"));
|
||||
}
|
||||
return buffer;
|
||||
}
|
||||
|
||||
template<typename T> const T & get() const{
|
||||
if(!type_guard.is_type<T>()){
|
||||
BOOST_THROW_EXCEPTION(std::bad_cast());
|
||||
}else if(empty){
|
||||
BOOST_THROW_EXCEPTION(std::length_error("buffer empty"));
|
||||
}
|
||||
return *(T*)&(buffer.front());
|
||||
}
|
||||
};
|
||||
|
||||
template<> const String & HoldAny::get() const;
|
||||
|
||||
struct DeviceInfo{
|
||||
std::string vendor_name;
|
||||
uint32_t vendor_number;
|
||||
std::string product_name;
|
||||
uint32_t product_number;
|
||||
uint32_t revision_number;
|
||||
std::string order_code;
|
||||
std::unordered_set<uint32_t> baudrates;
|
||||
bool simple_boot_up_master;
|
||||
bool simple_boot_up_slave;
|
||||
uint8_t granularity;
|
||||
bool dynamic_channels_supported;
|
||||
bool group_messaging;
|
||||
uint16_t nr_of_rx_pdo;
|
||||
uint16_t nr_of_tx_pdo;
|
||||
bool lss_supported;
|
||||
std::unordered_set<uint16_t> dummy_usage;
|
||||
};
|
||||
|
||||
#define THROW_WITH_KEY(e,k) BOOST_THROW_EXCEPTION(boost::enable_error_info(e) << ObjectDict::key_info(k))
|
||||
|
||||
class ObjectDict{
|
||||
protected:
|
||||
public:
|
||||
class Key{
|
||||
static size_t fromString(const std::string &str);
|
||||
public:
|
||||
const size_t hash;
|
||||
Key(const uint16_t i) : hash((i<<16)| 0xFFFF) {}
|
||||
Key(const uint16_t i, const uint8_t s): hash((i<<16)| s) {}
|
||||
Key(const std::string &str): hash(fromString(str)) {}
|
||||
bool hasSub() const { return (hash & 0xFFFF) != 0xFFFF; }
|
||||
uint8_t sub_index() const { return hash & 0xFFFF; }
|
||||
uint16_t index() const { return hash >> 16;}
|
||||
bool operator==(const Key &other) const { return hash == other.hash; }
|
||||
operator std::string() const;
|
||||
};
|
||||
struct KeyHash {
|
||||
std::size_t operator()(const Key& k) const { return k.hash; }
|
||||
};
|
||||
|
||||
enum Code{
|
||||
NULL_DATA = 0x00,
|
||||
DOMAIN_DATA = 0x02,
|
||||
DEFTYPE = 0x05,
|
||||
DEFSTRUCT = 0x06,
|
||||
VAR = 0x07,
|
||||
ARRAY = 0x08,
|
||||
RECORD = 0x09
|
||||
};
|
||||
enum DataTypes{
|
||||
DEFTYPE_INTEGER8 = 0x0002,
|
||||
DEFTYPE_INTEGER16 = 0x0003,
|
||||
DEFTYPE_INTEGER32 = 0x0004,
|
||||
DEFTYPE_UNSIGNED8 = 0x0005,
|
||||
DEFTYPE_UNSIGNED16 = 0x0006,
|
||||
DEFTYPE_UNSIGNED32 = 0x0007,
|
||||
DEFTYPE_REAL32 = 0x0008,
|
||||
DEFTYPE_VISIBLE_STRING = 0x0009,
|
||||
DEFTYPE_OCTET_STRING = 0x000A,
|
||||
DEFTYPE_UNICODE_STRING = 0x000B,
|
||||
DEFTYPE_DOMAIN = 0x000F,
|
||||
DEFTYPE_REAL64 = 0x0010,
|
||||
DEFTYPE_INTEGER64 = 0x0015,
|
||||
DEFTYPE_UNSIGNED64 = 0x001B
|
||||
};
|
||||
struct Entry{
|
||||
Code obj_code;
|
||||
uint16_t index;
|
||||
uint8_t sub_index;
|
||||
uint16_t data_type;
|
||||
bool constant;
|
||||
bool readable;
|
||||
bool writable;
|
||||
bool mappable;
|
||||
std::string desc;
|
||||
HoldAny def_val;
|
||||
HoldAny init_val;
|
||||
|
||||
Entry() {}
|
||||
|
||||
Entry(const Code c, const uint16_t i, const uint16_t t, const std::string & d, const bool r = true, const bool w = true, bool m = false, const HoldAny def = HoldAny(), const HoldAny init = HoldAny()):
|
||||
obj_code(c), index(i), sub_index(0),data_type(t),readable(r), writable(w), mappable(m), desc(d), def_val(def), init_val(init) {}
|
||||
|
||||
Entry(const uint16_t i, const uint8_t s, const uint16_t t, const std::string & d, const bool r = true, const bool w = true, bool m = false, const HoldAny def = HoldAny(), const HoldAny init = HoldAny()):
|
||||
obj_code(VAR), index(i), sub_index(s),data_type(t),readable(r), writable(w), mappable(m), desc(d), def_val(def), init_val(init) {}
|
||||
|
||||
operator Key() const { return Key(index, sub_index); }
|
||||
const HoldAny & value() const { return !init_val.is_empty() ? init_val : def_val; }
|
||||
|
||||
};
|
||||
typedef std::shared_ptr<const Entry> EntryConstSharedPtr;
|
||||
|
||||
const Entry& operator()(uint16_t i) const{
|
||||
return *at(Key(i));
|
||||
}
|
||||
const Entry& operator()(uint16_t i, uint8_t s) const{
|
||||
return *at(Key(i,s));
|
||||
}
|
||||
const EntryConstSharedPtr& get(const Key &k) const{
|
||||
return at(k);
|
||||
}
|
||||
bool has(uint16_t i, uint8_t s) const{
|
||||
return dict_.find(Key(i,s)) != dict_.end();
|
||||
}
|
||||
bool has(uint16_t i) const{
|
||||
return dict_.find(Key(i)) != dict_.end();
|
||||
}
|
||||
bool has(const Key &k) const{
|
||||
return dict_.find(k) != dict_.end();
|
||||
}
|
||||
|
||||
bool insert(bool is_sub, EntryConstSharedPtr e){
|
||||
return dict_.insert(std::make_pair(is_sub?Key(e->index,e->sub_index):Key(e->index),e)).second;
|
||||
}
|
||||
|
||||
typedef std::unordered_map<Key, EntryConstSharedPtr, KeyHash> ObjectDictMap;
|
||||
bool iterate(ObjectDictMap::const_iterator &it) const;
|
||||
typedef std::list<std::pair<std::string, std::string> > Overlay;
|
||||
typedef std::shared_ptr<ObjectDict> ObjectDictSharedPtr;
|
||||
static ObjectDictSharedPtr fromFile(const std::string &path, const Overlay &overlay = Overlay());
|
||||
const DeviceInfo device_info;
|
||||
|
||||
ObjectDict(const DeviceInfo &info): device_info(info) {}
|
||||
typedef boost::error_info<struct tag_objectdict_key, ObjectDict::Key> key_info;
|
||||
protected:
|
||||
const EntryConstSharedPtr& at(const Key &key) const{
|
||||
try{
|
||||
return dict_.at(key);
|
||||
}
|
||||
catch(const std::out_of_range &e){
|
||||
THROW_WITH_KEY(e, key);
|
||||
}
|
||||
}
|
||||
|
||||
ObjectDictMap dict_;
|
||||
};
|
||||
typedef ObjectDict::ObjectDictSharedPtr ObjectDictSharedPtr;
|
||||
typedef std::shared_ptr<const ObjectDict> ObjectDictConstSharedPtr;
|
||||
|
||||
[[deprecated]]
|
||||
std::size_t hash_value(ObjectDict::Key const& k);
|
||||
|
||||
template<typename T> class NodeIdOffset{
|
||||
T offset;
|
||||
T (*adder)(const uint8_t &, const T &);
|
||||
|
||||
static T add(const uint8_t &u, const T &t) {
|
||||
return u+t;
|
||||
}
|
||||
public:
|
||||
NodeIdOffset(const T &t): offset(t), adder(add) {}
|
||||
|
||||
static const T apply(const HoldAny &val, const uint8_t &u){
|
||||
if(!val.is_empty()){
|
||||
if(TypeGuard::create<T>() == val.type() ){
|
||||
return val.get<T>();
|
||||
}else{
|
||||
const NodeIdOffset<T> &no = val.get< NodeIdOffset<T> >();
|
||||
return no.adder(u, no.offset);
|
||||
}
|
||||
}else{
|
||||
BOOST_THROW_EXCEPTION(std::bad_cast());
|
||||
}
|
||||
|
||||
}
|
||||
};
|
||||
|
||||
template<typename T> std::ostream& operator<<(std::ostream& stream, const NodeIdOffset<T> &n) {
|
||||
//stream << "Offset: " << n.apply(0);
|
||||
return stream;
|
||||
}
|
||||
std::ostream& operator<<(std::ostream& stream, const ObjectDict::Key &k);
|
||||
|
||||
class AccessException : public Exception{
|
||||
public:
|
||||
AccessException(const std::string &w) : Exception(w) {}
|
||||
};
|
||||
|
||||
|
||||
class ObjectStorage{
|
||||
public:
|
||||
using ReadFunc = std::function<void(const ObjectDict::Entry&, String &)>;
|
||||
using ReadDelegate [[deprecated("use ReadFunc instead")]] = can::DelegateHelper<ReadFunc>;
|
||||
|
||||
using WriteFunc = std::function<void(const ObjectDict::Entry&, const String &)>;
|
||||
using WriteDelegate [[deprecated("use WriteFunc instead")]] = can::DelegateHelper<WriteFunc>;
|
||||
|
||||
typedef std::shared_ptr<ObjectStorage> ObjectStorageSharedPtr;
|
||||
|
||||
protected:
|
||||
class Data {
|
||||
Data(const Data&) = delete; // prevent copies
|
||||
Data& operator=(const Data&) = delete;
|
||||
|
||||
boost::mutex mutex;
|
||||
String buffer;
|
||||
bool valid;
|
||||
|
||||
ReadFunc read_delegate;
|
||||
WriteFunc write_delegate;
|
||||
|
||||
template <typename T> T & access(){
|
||||
if(!valid){
|
||||
THROW_WITH_KEY(std::length_error("buffer not valid"), key);
|
||||
}
|
||||
return *(T*)&(buffer.front());
|
||||
}
|
||||
template <typename T> T & allocate(){
|
||||
if(!valid){
|
||||
buffer.resize(sizeof(T));
|
||||
valid = true;
|
||||
}
|
||||
return access<T>();
|
||||
}
|
||||
public:
|
||||
const TypeGuard type_guard;
|
||||
const ObjectDict::EntryConstSharedPtr entry;
|
||||
const ObjectDict::Key key;
|
||||
size_t size() { boost::mutex::scoped_lock lock(mutex); return buffer.size(); }
|
||||
|
||||
template<typename T> Data(const ObjectDict::Key &k, const ObjectDict::EntryConstSharedPtr &e, const T &val, const ReadFunc &r, const WriteFunc &w)
|
||||
: valid(false), read_delegate(r), write_delegate(w), type_guard(TypeGuard::create<T>()), entry(e), key(k){
|
||||
assert(r);
|
||||
assert(w);
|
||||
assert(e);
|
||||
allocate<T>() = val;
|
||||
}
|
||||
Data(const ObjectDict::Key &k, const ObjectDict::EntryConstSharedPtr &e, const TypeGuard &t, const ReadFunc &r, const WriteFunc &w)
|
||||
: valid(false), read_delegate(r), write_delegate(w), type_guard(t), entry(e), key(k){
|
||||
assert(r);
|
||||
assert(w);
|
||||
assert(e);
|
||||
assert(t.valid());
|
||||
buffer.resize(t.get_size());
|
||||
}
|
||||
void set_delegates(const ReadFunc &r, const WriteFunc &w){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
if(r) read_delegate = r;
|
||||
if(w) write_delegate = w;
|
||||
}
|
||||
template<typename T> const T get(bool cached) {
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
|
||||
if(!entry->readable){
|
||||
THROW_WITH_KEY(AccessException("no read access"), key);
|
||||
|
||||
}
|
||||
|
||||
if(entry->constant) cached = true;
|
||||
|
||||
if(!valid || !cached){
|
||||
allocate<T>();
|
||||
read_delegate(*entry, buffer);
|
||||
}
|
||||
return access<T>();
|
||||
}
|
||||
template<typename T> void set(const T &val) {
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
|
||||
if(!entry->writable){
|
||||
if(access<T>() != val){
|
||||
THROW_WITH_KEY(AccessException("no write access"), key);
|
||||
}
|
||||
}else{
|
||||
allocate<T>() = val;
|
||||
write_delegate(*entry, buffer);
|
||||
}
|
||||
}
|
||||
template<typename T> void set_cached(const T &val) {
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
if(!valid || val != access<T>() ){
|
||||
if(!entry->writable){
|
||||
THROW_WITH_KEY(AccessException("no write access and not cached"), key);
|
||||
}else{
|
||||
allocate<T>() = val;
|
||||
write_delegate(*entry, buffer);
|
||||
}
|
||||
}
|
||||
}
|
||||
void init();
|
||||
void reset();
|
||||
void force_write();
|
||||
|
||||
};
|
||||
typedef std::shared_ptr<Data> DataSharedPtr;
|
||||
public:
|
||||
template<const uint16_t dt> struct DataType{
|
||||
typedef void type;
|
||||
};
|
||||
|
||||
template<typename T> class Entry{
|
||||
DataSharedPtr data;
|
||||
public:
|
||||
typedef T type;
|
||||
bool valid() const { return data != 0; }
|
||||
const T get() {
|
||||
if(!data) BOOST_THROW_EXCEPTION( PointerInvalid("ObjectStorage::Entry::get()") );
|
||||
|
||||
return data->get<T>(false);
|
||||
}
|
||||
bool get(T & val){
|
||||
try{
|
||||
val = get();
|
||||
return true;
|
||||
}catch(...){
|
||||
return false;
|
||||
}
|
||||
}
|
||||
const T get_cached() {
|
||||
if(!data) BOOST_THROW_EXCEPTION( PointerInvalid("ObjectStorage::Entry::get_cached()") );
|
||||
|
||||
return data->get<T>(true);
|
||||
}
|
||||
bool get_cached(T & val){
|
||||
try{
|
||||
val = get_cached();
|
||||
return true;
|
||||
}catch(...){
|
||||
return false;
|
||||
}
|
||||
}
|
||||
void set(const T &val) {
|
||||
if(!data) BOOST_THROW_EXCEPTION( PointerInvalid("ObjectStorage::Entry::set(val)") );
|
||||
data->set(val);
|
||||
}
|
||||
bool set_cached(const T &val) {
|
||||
if(!data) return false;
|
||||
try{
|
||||
data->set_cached(val);
|
||||
return true;
|
||||
}catch(...){
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
Entry() {}
|
||||
Entry(DataSharedPtr &d)
|
||||
: data(d){
|
||||
assert(data);
|
||||
}
|
||||
Entry(ObjectStorageSharedPtr storage, uint16_t index)
|
||||
: data(storage->entry<type>(index).data) {
|
||||
assert(data);
|
||||
}
|
||||
Entry(ObjectStorageSharedPtr storage, uint16_t index, uint8_t sub_index)
|
||||
: data(storage->entry<type>(index, sub_index).data) {
|
||||
assert(data);
|
||||
}
|
||||
Entry(ObjectStorageSharedPtr storage, const ObjectDict::Key &k)
|
||||
: data(storage->entry<type>(k).data) {
|
||||
assert(data);
|
||||
}
|
||||
const ObjectDict::Entry & desc() const{
|
||||
return *(data->entry);
|
||||
}
|
||||
};
|
||||
|
||||
void reset();
|
||||
|
||||
protected:
|
||||
typedef std::unordered_map<ObjectDict::Key, DataSharedPtr, ObjectDict::KeyHash> ObjectStorageMap;
|
||||
ObjectStorageMap storage_;
|
||||
boost::mutex mutex_;
|
||||
|
||||
void init_nolock(const ObjectDict::Key &key, const ObjectDict::EntryConstSharedPtr &entry);
|
||||
|
||||
ReadFunc read_delegate_;
|
||||
WriteFunc write_delegate_;
|
||||
size_t map(const ObjectDict::EntryConstSharedPtr &e, const ObjectDict::Key &key, const ReadFunc & read_delegate, const WriteFunc & write_delegate);
|
||||
public:
|
||||
template<typename T> Entry<T> entry(const ObjectDict::Key &key){
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
|
||||
ObjectStorageMap::iterator it = storage_.find(key);
|
||||
|
||||
if(it == storage_.end()){
|
||||
const ObjectDict::EntryConstSharedPtr e = dict_->get(key);
|
||||
|
||||
DataSharedPtr data;
|
||||
TypeGuard type = TypeGuard::create<T>();
|
||||
|
||||
if(!e->def_val.is_empty()){
|
||||
T val = NodeIdOffset<T>::apply(e->def_val, node_id_);
|
||||
data = std::make_shared<Data>(key, e,val, read_delegate_, write_delegate_);
|
||||
}else{
|
||||
if(!e->def_val.type().valid() || e->def_val.type() == type) {
|
||||
data = std::make_shared<Data>(key,e,type, read_delegate_, write_delegate_);
|
||||
}else{
|
||||
THROW_WITH_KEY(std::bad_cast(), key);
|
||||
}
|
||||
}
|
||||
|
||||
std::pair<ObjectStorageMap::iterator, bool> ok = storage_.insert(std::make_pair(key, data));
|
||||
it = ok.first;
|
||||
}
|
||||
|
||||
if(!it->second->type_guard.is_type<T>()){
|
||||
THROW_WITH_KEY(std::bad_cast(), key);
|
||||
}
|
||||
return Entry<T>(it->second);
|
||||
}
|
||||
|
||||
size_t map(uint16_t index, uint8_t sub_index, const ReadFunc & read_delegate, const WriteFunc & write_delegate);
|
||||
|
||||
template<typename T> Entry<T> entry(uint16_t index){
|
||||
return entry<T>(ObjectDict::Key(index));
|
||||
}
|
||||
template<typename T> Entry<T> entry(uint16_t index, uint8_t sub_index){
|
||||
return entry<T>(ObjectDict::Key(index,sub_index));
|
||||
}
|
||||
|
||||
template<typename T> void entry(Entry<T> &e, uint16_t index){ // TODO: migrate to bool
|
||||
e = entry<T>(ObjectDict::Key(index));
|
||||
}
|
||||
template<typename T> void entry(Entry<T> &e, uint16_t index, uint8_t sub_index){ // TODO: migrate to bool
|
||||
e = entry<T>(ObjectDict::Key(index,sub_index));
|
||||
}
|
||||
template<typename T> bool entry(Entry<T> &e, const ObjectDict::Key &k){
|
||||
try{
|
||||
e = entry<T>(k);
|
||||
return true;
|
||||
}catch(...){
|
||||
return false;
|
||||
}
|
||||
}
|
||||
typedef std::function<std::string()> ReadStringFuncType;
|
||||
ReadStringFuncType getStringReader(const ObjectDict::Key &key, bool cached = false);
|
||||
typedef std::function<void(const std::string &)> WriteStringFuncType;
|
||||
WriteStringFuncType getStringWriter(const ObjectDict::Key &key, bool cached = false);
|
||||
|
||||
const ObjectDictConstSharedPtr dict_;
|
||||
const uint8_t node_id_;
|
||||
|
||||
ObjectStorage(ObjectDictConstSharedPtr dict, uint8_t node_id, ReadFunc read_delegate, WriteFunc write_delegate);
|
||||
|
||||
void init(const ObjectDict::Key &key);
|
||||
void init_all();
|
||||
};
|
||||
typedef ObjectStorage::ObjectStorageSharedPtr ObjectStorageSharedPtr;
|
||||
|
||||
template<> String & ObjectStorage::Data::access();
|
||||
template<> String & ObjectStorage::Data::allocate();
|
||||
|
||||
template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_INTEGER8> { typedef int8_t type;};
|
||||
template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_INTEGER16> { typedef int16_t type;};
|
||||
template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_INTEGER32> { typedef int32_t type;};
|
||||
template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_INTEGER64> { typedef int64_t type;};
|
||||
template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_UNSIGNED8> { typedef uint8_t type;};
|
||||
|
||||
template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_UNSIGNED16> { typedef uint16_t type;};
|
||||
template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_UNSIGNED32> { typedef uint32_t type;};
|
||||
template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_UNSIGNED64> { typedef uint64_t type;};
|
||||
|
||||
template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_REAL32> { typedef float type;};
|
||||
template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_REAL64> { typedef double type;};
|
||||
|
||||
template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_VISIBLE_STRING> { typedef String type;};
|
||||
template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_OCTET_STRING> { typedef String type;};
|
||||
template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_UNICODE_STRING> { typedef String type;};
|
||||
template<> struct ObjectStorage::DataType<ObjectDict::DEFTYPE_DOMAIN> { typedef String type;};
|
||||
|
||||
template<typename T, typename R> static R *branch_type(const uint16_t data_type){
|
||||
switch(ObjectDict::DataTypes(data_type)){
|
||||
case ObjectDict::DEFTYPE_INTEGER8: return T::template func< ObjectDict::DEFTYPE_INTEGER8 >;
|
||||
case ObjectDict::DEFTYPE_INTEGER16: return T::template func< ObjectDict::DEFTYPE_INTEGER16 >;
|
||||
case ObjectDict::DEFTYPE_INTEGER32: return T::template func< ObjectDict::DEFTYPE_INTEGER32 >;
|
||||
case ObjectDict::DEFTYPE_INTEGER64: return T::template func< ObjectDict::DEFTYPE_INTEGER64 >;
|
||||
|
||||
case ObjectDict::DEFTYPE_UNSIGNED8: return T::template func< ObjectDict::DEFTYPE_UNSIGNED8 >;
|
||||
case ObjectDict::DEFTYPE_UNSIGNED16: return T::template func< ObjectDict::DEFTYPE_UNSIGNED16 >;
|
||||
case ObjectDict::DEFTYPE_UNSIGNED32: return T::template func< ObjectDict::DEFTYPE_UNSIGNED32 >;
|
||||
case ObjectDict::DEFTYPE_UNSIGNED64: return T::template func< ObjectDict::DEFTYPE_UNSIGNED64 >;
|
||||
|
||||
case ObjectDict::DEFTYPE_REAL32: return T::template func< ObjectDict::DEFTYPE_REAL32 >;
|
||||
case ObjectDict::DEFTYPE_REAL64: return T::template func< ObjectDict::DEFTYPE_REAL64 >;
|
||||
|
||||
case ObjectDict::DEFTYPE_VISIBLE_STRING: return T::template func< ObjectDict::DEFTYPE_VISIBLE_STRING >;
|
||||
case ObjectDict::DEFTYPE_OCTET_STRING: return T::template func< ObjectDict::DEFTYPE_OCTET_STRING >;
|
||||
case ObjectDict::DEFTYPE_UNICODE_STRING: return T::template func< ObjectDict::DEFTYPE_UNICODE_STRING >;
|
||||
case ObjectDict::DEFTYPE_DOMAIN: return T::template func< ObjectDict::DEFTYPE_DOMAIN >;
|
||||
|
||||
default:
|
||||
throw std::bad_cast();
|
||||
}
|
||||
}
|
||||
|
||||
} // canopen
|
||||
|
||||
#endif // !H_OBJDICT
|
||||
@@ -0,0 +1,75 @@
|
||||
#ifndef H_CANOPEN_TIMER
|
||||
#define H_CANOPEN_TIMER
|
||||
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
|
||||
#include <boost/asio.hpp>
|
||||
#include <boost/thread/thread.hpp>
|
||||
#include <boost/asio/high_resolution_timer.hpp>
|
||||
|
||||
#include <socketcan_interface/delegates.h>
|
||||
|
||||
namespace canopen{
|
||||
|
||||
class Timer{
|
||||
public:
|
||||
using TimerFunc = std::function<bool(void)>;
|
||||
using TimerDelegate [[deprecated("use TimerFunc instead")]] = can::DelegateHelper<TimerFunc>;
|
||||
|
||||
Timer():work(io), timer(io),thread(std::bind(
|
||||
static_cast<size_t(boost::asio::io_service::*)(void)>(&boost::asio::io_service::run), &io))
|
||||
{
|
||||
}
|
||||
|
||||
void stop(){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
timer.cancel();
|
||||
}
|
||||
template<typename T> void start(const TimerFunc &del, const T &dur, bool start_now = true){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
delegate = del;
|
||||
period = boost::chrono::duration_cast<boost::chrono::high_resolution_clock::duration>(dur);
|
||||
if(start_now){
|
||||
timer.expires_from_now(period);
|
||||
timer.async_wait(std::bind(&Timer::handler, this, std::placeholders::_1));
|
||||
}
|
||||
}
|
||||
void restart(){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
timer.expires_from_now(period);
|
||||
timer.async_wait(std::bind(&Timer::handler, this, std::placeholders::_1));
|
||||
}
|
||||
const boost::chrono::high_resolution_clock::duration & getPeriod(){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
return period;
|
||||
}
|
||||
~Timer(){
|
||||
io.stop();
|
||||
thread.join();
|
||||
}
|
||||
|
||||
private:
|
||||
boost::asio::io_service io;
|
||||
boost::asio::io_service::work work;
|
||||
boost::asio::basic_waitable_timer<boost::chrono::high_resolution_clock> timer;
|
||||
boost::chrono::high_resolution_clock::duration period;
|
||||
boost::mutex mutex;
|
||||
boost::thread thread;
|
||||
|
||||
TimerFunc delegate;
|
||||
void handler(const boost::system::error_code& ec){
|
||||
if(!ec){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
if(delegate && delegate()){
|
||||
timer.expires_at(timer.expires_at() + period);
|
||||
timer.async_wait(std::bind(&Timer::handler, this, std::placeholders::_1));
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
8
Devices/Libraries/Ros/ros_canopen/canopen_master/master_plugin.xml
Executable file
8
Devices/Libraries/Ros/ros_canopen/canopen_master/master_plugin.xml
Executable file
@@ -0,0 +1,8 @@
|
||||
<library path="lib/libcanopen_master_plugin">
|
||||
<class type="canopen::SimpleMaster::Allocator" base_class_type="canopen::Master::Allocator">
|
||||
<description>Allocator for SimpleMaster instances</description>
|
||||
</class>
|
||||
<class type="canopen::ExternalMaster::Allocator" base_class_type="canopen::Master::Allocator">
|
||||
<description>Allocator for ExternalMaster instances</description>
|
||||
</class>
|
||||
</library>
|
||||
30
Devices/Libraries/Ros/ros_canopen/canopen_master/package.xml
Executable file
30
Devices/Libraries/Ros/ros_canopen/canopen_master/package.xml
Executable file
@@ -0,0 +1,30 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>canopen_master</name>
|
||||
<version>0.8.5</version>
|
||||
<description>CiA(r) CANopen 301 master implementation with support for interprocess master synchronisation.</description>
|
||||
|
||||
<maintainer email="mathias.luedtke@ipa.fraunhofer.de">Mathias Lüdtke</maintainer>
|
||||
<author email="mathias.luedtke@ipa.fraunhofer.de">Mathias Lüdtke</author>
|
||||
|
||||
<license>LGPLv3</license>
|
||||
|
||||
<url type="website">http://wiki.ros.org/canopen_master</url>
|
||||
<url type="repository">https://github.com/ros-industrial/ros_canopen</url>
|
||||
<url type="bugtracker">https://github.com/ros-industrial/ros_canopen/issues</url>
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
|
||||
<depend>libboost-dev</depend>
|
||||
<depend>libboost-atomic-dev</depend>
|
||||
<depend>libboost-chrono-dev</depend>
|
||||
<depend>libboost-thread-dev</depend>
|
||||
<depend>class_loader</depend>
|
||||
<depend>socketcan_interface</depend>
|
||||
|
||||
<test_depend>rosunit</test_depend>
|
||||
|
||||
<export>
|
||||
<canopen_master plugin="${prefix}/master_plugin.xml" />
|
||||
</export>
|
||||
</package>
|
||||
68
Devices/Libraries/Ros/ros_canopen/canopen_master/src/bcm_sync.cpp
Executable file
68
Devices/Libraries/Ros/ros_canopen/canopen_master/src/bcm_sync.cpp
Executable file
@@ -0,0 +1,68 @@
|
||||
#include <canopen_master/bcm_sync.h>
|
||||
#include <socketcan_interface/string.h>
|
||||
#include <iostream>
|
||||
|
||||
int main(int argc, char** argv){
|
||||
|
||||
if(argc < 4){
|
||||
std::cout << "Usage: " << argv[0] << " DEVICE PERIOD_MS HEADER [OVERFLOW] [+ID*] [-ID*] [--]" << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
std::string can_device = argv[1];
|
||||
int sync_ms = atoi(argv[2]);
|
||||
can::Header header = can::toheader(argv[3]);
|
||||
|
||||
if(!header.isValid()){
|
||||
std::cout << "header is invalid" << std::endl;
|
||||
return 1;
|
||||
}
|
||||
int sync_overflow = 0;
|
||||
|
||||
int start = 4;
|
||||
if(argc > start && argv[start][0] != '-' && argv[start][0] != '+'){
|
||||
sync_overflow = atoi(argv[4]);
|
||||
if(sync_overflow == 1 || sync_overflow < 0 || sync_overflow > 240){
|
||||
std::cout << "sync overflow is invalid" << std::endl;
|
||||
return 1;
|
||||
}
|
||||
++start;
|
||||
}
|
||||
|
||||
std::set<int> monitored, ignored;
|
||||
|
||||
for(; argc > start; ++start){
|
||||
if(strncmp("--", argv[start], 2) == 0) break;
|
||||
int id = atoi(argv[start]);
|
||||
|
||||
if(id > 0 && id < 128) monitored.insert(id);
|
||||
else if (id < 0 && id > -128) ignored.insert(-id);
|
||||
else{
|
||||
std::cout << "ID is invalid: " << id << std::endl;
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
can::SocketCANDriverSharedPtr driver = std::make_shared<can::SocketCANDriver>();
|
||||
if(!driver->init(can_device, false, can::NoSettings::create())){
|
||||
std::cout << "Could not initialize CAN" << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
canopen::SyncProperties sync_properties(header, sync_ms, sync_overflow);
|
||||
canopen::BCMsync sync(can_device, driver, sync_properties);
|
||||
|
||||
sync.setMonitored(monitored);
|
||||
sync.setIgnored(ignored);
|
||||
|
||||
canopen::LayerStatus status;
|
||||
sync.init(status);
|
||||
if(!status.bounded<canopen::LayerStatus::Warn>()){
|
||||
std::cout << "Could not initialize sync" << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
driver->run();
|
||||
|
||||
return 0;
|
||||
}
|
||||
148
Devices/Libraries/Ros/ros_canopen/canopen_master/src/emcy.cpp
Executable file
148
Devices/Libraries/Ros/ros_canopen/canopen_master/src/emcy.cpp
Executable file
@@ -0,0 +1,148 @@
|
||||
#include <canopen_master/canopen.h>
|
||||
#include <socketcan_interface/string.h>
|
||||
|
||||
using namespace canopen;
|
||||
|
||||
#pragma pack(push) /* push current alignment to stack */
|
||||
#pragma pack(1) /* set alignment to 1 byte boundary */
|
||||
|
||||
struct EMCYid{
|
||||
uint32_t id:29;
|
||||
uint32_t extended:1;
|
||||
uint32_t :1;
|
||||
uint32_t invalid:1;
|
||||
EMCYid(uint32_t val){
|
||||
*(uint32_t*) this = val;
|
||||
}
|
||||
can::Header header() {
|
||||
return can::Header(id, extended, false, false);
|
||||
}
|
||||
const uint32_t get() const { return *(uint32_t*) this; }
|
||||
};
|
||||
|
||||
struct EMCYfield{
|
||||
uint32_t error_code:16;
|
||||
uint32_t addition_info:16;
|
||||
EMCYfield(uint32_t val){
|
||||
*(uint32_t*) this = val;
|
||||
}
|
||||
};
|
||||
|
||||
struct EMCYmsg{
|
||||
uint16_t error_code;
|
||||
uint8_t error_register;
|
||||
uint8_t manufacturer_specific_error_field[5];
|
||||
|
||||
struct Frame: public FrameOverlay<EMCYmsg>{
|
||||
Frame(const can::Frame &f) : FrameOverlay(f){ }
|
||||
};
|
||||
};
|
||||
|
||||
#pragma pack(pop) /* pop previous alignment from stack */
|
||||
|
||||
void EMCYHandler::handleEMCY(const can::Frame & msg){
|
||||
EMCYmsg::Frame em(msg);
|
||||
if (em.data.error_code == 0) {
|
||||
ROSCANOPEN_INFO("canopen_master", "EMCY reset: " << msg);
|
||||
} else {
|
||||
ROSCANOPEN_ERROR("canopen_master", "EMCY received: " << msg);
|
||||
}
|
||||
has_error_ = (em.data.error_register & ~32) != 0;
|
||||
}
|
||||
|
||||
EMCYHandler::EMCYHandler(const can::CommInterfaceSharedPtr interface, const ObjectStorageSharedPtr storage): Layer("EMCY handler"), storage_(storage), has_error_(true){
|
||||
storage_->entry(error_register_, 0x1001);
|
||||
try{
|
||||
storage_->entry(num_errors_, 0x1003,0);
|
||||
}
|
||||
catch(...){
|
||||
// pass, 1003 is optional
|
||||
}
|
||||
try{
|
||||
EMCYid emcy_id(storage_->entry<uint32_t>(0x1014).get_cached());
|
||||
emcy_listener_ = interface->createMsgListenerM(emcy_id.header(), this, &EMCYHandler::handleEMCY);
|
||||
|
||||
|
||||
}
|
||||
catch(...){
|
||||
// pass, EMCY is optional
|
||||
}
|
||||
}
|
||||
|
||||
void EMCYHandler::handleRead(LayerStatus &status, const LayerState ¤t_state) {
|
||||
if(current_state == Ready){
|
||||
if(has_error_){
|
||||
status.error("Node has emergency error");
|
||||
}
|
||||
}
|
||||
}
|
||||
void EMCYHandler::handleWrite(LayerStatus &status, const LayerState ¤t_state) {
|
||||
// noithing to do
|
||||
}
|
||||
|
||||
void EMCYHandler::handleDiag(LayerReport &report){
|
||||
uint8_t error_register = 0;
|
||||
if(!error_register_.get(error_register)){
|
||||
report.error("Could not read error error_register");
|
||||
return;
|
||||
}
|
||||
|
||||
if(error_register){
|
||||
if(error_register & 1){ // first bit should be set on all errors
|
||||
report.error("Node has emergency error");
|
||||
}else if(error_register & ~32){ // filter profile-specific bit
|
||||
report.warn("Error register is not zero");
|
||||
}
|
||||
report.add("error_register", (uint32_t) error_register);
|
||||
|
||||
uint8_t num = num_errors_.valid() ? num_errors_.get() : 0;
|
||||
std::stringstream buf;
|
||||
for(size_t i = 0; i < num; ++i) {
|
||||
if( i!= 0){
|
||||
buf << ", ";
|
||||
}
|
||||
try{
|
||||
ObjectStorage::Entry<uint32_t> error;
|
||||
storage_->entry(error, 0x1003,i+1);
|
||||
EMCYfield field(error.get());
|
||||
buf << std::hex << field.error_code << "#" << field.addition_info;
|
||||
}
|
||||
catch (const std::out_of_range & e){
|
||||
buf << "NOT_IN_DICT!";
|
||||
}
|
||||
catch (const TimeoutException & e){
|
||||
buf << "LIST_UNDERFLOW!";
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
report.add("errors", buf.str());
|
||||
|
||||
}
|
||||
}
|
||||
void EMCYHandler::handleInit(LayerStatus &status){
|
||||
uint8_t error_register = 0;
|
||||
if(!error_register_.get(error_register)){
|
||||
status.error("Could not read error error_register");
|
||||
return;
|
||||
}else if(error_register & 1){
|
||||
ROSCANOPEN_ERROR("canopen_master", "error register: " << int(error_register));
|
||||
status.error("Node has emergency error");
|
||||
return;
|
||||
}
|
||||
|
||||
resetErrors(status);
|
||||
}
|
||||
void EMCYHandler::resetErrors(LayerStatus &status){
|
||||
if(num_errors_.valid()) num_errors_.set(0);
|
||||
has_error_ = false;
|
||||
}
|
||||
|
||||
void EMCYHandler::handleRecover(LayerStatus &status){
|
||||
handleInit(status);
|
||||
}
|
||||
void EMCYHandler::handleShutdown(LayerStatus &status){
|
||||
}
|
||||
void EMCYHandler::handleHalt(LayerStatus &status){
|
||||
// do nothing
|
||||
}
|
||||
138
Devices/Libraries/Ros/ros_canopen/canopen_master/src/master_plugin.cpp
Executable file
138
Devices/Libraries/Ros/ros_canopen/canopen_master/src/master_plugin.cpp
Executable file
@@ -0,0 +1,138 @@
|
||||
#include <class_loader/class_loader.hpp>
|
||||
#include <socketcan_interface/reader.h>
|
||||
#include <canopen_master/canopen.h>
|
||||
|
||||
#include <set>
|
||||
|
||||
namespace canopen {
|
||||
|
||||
class ManagingSyncLayer: public SyncLayer {
|
||||
protected:
|
||||
can::CommInterfaceSharedPtr interface_;
|
||||
boost::chrono::milliseconds step_, half_step_;
|
||||
|
||||
std::set<void *> nodes_;
|
||||
boost::mutex nodes_mutex_;
|
||||
std::atomic<size_t> nodes_size_;
|
||||
|
||||
virtual void handleShutdown(LayerStatus &status) {
|
||||
}
|
||||
|
||||
virtual void handleHalt(LayerStatus &status) { /* nothing to do */ }
|
||||
virtual void handleDiag(LayerReport &report) { /* TODO */ }
|
||||
virtual void handleRecover(LayerStatus &status) { /* TODO */ }
|
||||
|
||||
public:
|
||||
ManagingSyncLayer(const SyncProperties &p, can::CommInterfaceSharedPtr interface)
|
||||
: SyncLayer(p), interface_(interface), step_(p.period_ms_), half_step_(p.period_ms_/2), nodes_size_(0)
|
||||
{
|
||||
}
|
||||
|
||||
virtual void addNode(void * const ptr) {
|
||||
boost::mutex::scoped_lock lock(nodes_mutex_);
|
||||
nodes_.insert(ptr);
|
||||
nodes_size_ = nodes_.size();
|
||||
}
|
||||
virtual void removeNode(void * const ptr) {
|
||||
boost::mutex::scoped_lock lock(nodes_mutex_);
|
||||
nodes_.erase(ptr);
|
||||
nodes_size_ = nodes_.size();
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
class SimpleSyncLayer: public ManagingSyncLayer {
|
||||
time_point read_time_, write_time_;
|
||||
can::Frame frame_;
|
||||
uint8_t overflow_;
|
||||
|
||||
void resetCounter(){
|
||||
frame_.data[0] = 1; // SYNC counter starts at 1
|
||||
}
|
||||
void tryUpdateCounter(){
|
||||
if (frame_.dlc > 0) { // sync counter is used
|
||||
if (frame_.data[0] >= overflow_) {
|
||||
resetCounter();
|
||||
}else{
|
||||
++frame_.data[0];
|
||||
}
|
||||
}
|
||||
}
|
||||
protected:
|
||||
virtual void handleRead(LayerStatus &status, const LayerState ¤t_state) {
|
||||
if(current_state > Init){
|
||||
boost::this_thread::sleep_until(read_time_);
|
||||
write_time_ += step_;
|
||||
}
|
||||
}
|
||||
virtual void handleWrite(LayerStatus &status, const LayerState ¤t_state) {
|
||||
if(current_state > Init){
|
||||
boost::this_thread::sleep_until(write_time_);
|
||||
tryUpdateCounter();
|
||||
if(nodes_size_){ //)
|
||||
interface_->send(frame_);
|
||||
}
|
||||
read_time_ = get_abs_time(half_step_);
|
||||
}
|
||||
}
|
||||
|
||||
virtual void handleInit(LayerStatus &status){
|
||||
write_time_ = get_abs_time(step_);
|
||||
read_time_ = get_abs_time(half_step_);
|
||||
}
|
||||
public:
|
||||
SimpleSyncLayer(const SyncProperties &p, can::CommInterfaceSharedPtr interface)
|
||||
: ManagingSyncLayer(p, interface), frame_(p.header_, 0), overflow_(p.overflow_) {
|
||||
if(overflow_ == 1 || overflow_ > 240){
|
||||
BOOST_THROW_EXCEPTION(Exception("SYNC counter overflow is invalid"));
|
||||
}else if(overflow_ > 1){
|
||||
frame_.dlc = 1;
|
||||
resetCounter();
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
class ExternalSyncLayer: public ManagingSyncLayer {
|
||||
can::BufferedReader reader_;
|
||||
protected:
|
||||
virtual void handleRead(LayerStatus &status, const LayerState ¤t_state) {
|
||||
can::Frame msg;
|
||||
if(current_state > Init){
|
||||
if(reader_.readUntil(&msg, get_abs_time(step_))){ // wait for sync
|
||||
boost::this_thread::sleep_until(get_abs_time(half_step_)); // shift readout to middle of period
|
||||
}
|
||||
}
|
||||
}
|
||||
virtual void handleWrite(LayerStatus &status, const LayerState ¤t_state) {
|
||||
// nothing to do here
|
||||
}
|
||||
virtual void handleInit(LayerStatus &status){
|
||||
reader_.listen(interface_, properties.header_);
|
||||
}
|
||||
public:
|
||||
ExternalSyncLayer(const SyncProperties &p, can::CommInterfaceSharedPtr interface)
|
||||
: ManagingSyncLayer(p, interface), reader_(true,1) {}
|
||||
};
|
||||
|
||||
|
||||
template<typename SyncType> class WrapMaster: public Master{
|
||||
can::CommInterfaceSharedPtr interface_;
|
||||
public:
|
||||
virtual SyncLayerSharedPtr getSync(const SyncProperties &properties){
|
||||
return std::make_shared<SyncType>(properties, interface_);
|
||||
}
|
||||
WrapMaster(can::CommInterfaceSharedPtr interface) : interface_(interface) {}
|
||||
|
||||
class Allocator : public Master::Allocator{
|
||||
public:
|
||||
virtual MasterSharedPtr allocate(const std::string &name, can::CommInterfaceSharedPtr interface){
|
||||
return std::make_shared<WrapMaster>(interface);
|
||||
}
|
||||
};
|
||||
};
|
||||
|
||||
typedef WrapMaster<SimpleSyncLayer> SimpleMaster;
|
||||
typedef WrapMaster<ExternalSyncLayer> ExternalMaster;
|
||||
}
|
||||
CLASS_LOADER_REGISTER_CLASS(canopen::SimpleMaster::Allocator, canopen::Master::Allocator);
|
||||
CLASS_LOADER_REGISTER_CLASS(canopen::ExternalMaster::Allocator, canopen::Master::Allocator);
|
||||
218
Devices/Libraries/Ros/ros_canopen/canopen_master/src/node.cpp
Executable file
218
Devices/Libraries/Ros/ros_canopen/canopen_master/src/node.cpp
Executable file
@@ -0,0 +1,218 @@
|
||||
#include <canopen_master/canopen.h>
|
||||
|
||||
using namespace canopen;
|
||||
|
||||
#pragma pack(push) /* push current alignment to stack */
|
||||
#pragma pack(1) /* set alignment to 1 byte boundary */
|
||||
|
||||
|
||||
struct NMTcommand{
|
||||
enum Command{
|
||||
Start = 1,
|
||||
Stop = 2,
|
||||
Prepare = 128,
|
||||
Reset = 129,
|
||||
Reset_Com = 130
|
||||
};
|
||||
uint8_t command;
|
||||
uint8_t node_id;
|
||||
|
||||
struct Frame: public FrameOverlay<NMTcommand>{
|
||||
Frame(uint8_t node_id, const Command &c) : FrameOverlay(can::Header()) {
|
||||
data.command = c;
|
||||
data.node_id = node_id;
|
||||
}
|
||||
};
|
||||
};
|
||||
|
||||
#pragma pack(pop) /* pop previous alignment from stack */
|
||||
|
||||
Node::Node(const can::CommInterfaceSharedPtr interface, const ObjectDictSharedPtr dict, uint8_t node_id, const SyncCounterSharedPtr sync)
|
||||
: Layer("Node 301"), node_id_(node_id), interface_(interface), sync_(sync) , state_(Unknown), sdo_(interface, dict, node_id), pdo_(interface){
|
||||
try{
|
||||
getStorage()->entry(heartbeat_, 0x1017);
|
||||
}
|
||||
catch(const std::out_of_range){
|
||||
}
|
||||
}
|
||||
|
||||
const Node::State Node::getState(){
|
||||
boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
|
||||
return state_;
|
||||
}
|
||||
|
||||
bool Node::reset_com(){
|
||||
boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
|
||||
getStorage()->reset();
|
||||
interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Reset_Com));
|
||||
if(wait_for(BootUp, boost::chrono::seconds(10)) != 1){
|
||||
return false;
|
||||
}
|
||||
state_ = PreOperational;
|
||||
setHeartbeatInterval();
|
||||
return true;
|
||||
}
|
||||
bool Node::reset(){
|
||||
boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
|
||||
getStorage()->reset();
|
||||
|
||||
interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Reset));
|
||||
if(wait_for(BootUp, boost::chrono::seconds(10)) != 1){
|
||||
return false;
|
||||
}
|
||||
state_ = PreOperational;
|
||||
setHeartbeatInterval();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Node::prepare(){
|
||||
boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
|
||||
if(state_ == BootUp){
|
||||
// ERROR
|
||||
}
|
||||
interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Prepare));
|
||||
return 0 != wait_for(PreOperational, boost::chrono::seconds(2));
|
||||
}
|
||||
bool Node::start(){
|
||||
boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
|
||||
if(state_ == BootUp){
|
||||
// ERROR
|
||||
}
|
||||
interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Start));
|
||||
return 0 != wait_for(Operational, boost::chrono::seconds(2));
|
||||
}
|
||||
bool Node::stop(){
|
||||
boost::timed_mutex::scoped_lock lock(mutex); // TODO: timed lock?
|
||||
if(sync_) sync_->removeNode(this);
|
||||
if(state_ == BootUp){
|
||||
// ERROR
|
||||
}
|
||||
interface_->send(NMTcommand::Frame(node_id_, NMTcommand::Stop));
|
||||
return true;
|
||||
}
|
||||
|
||||
void Node::switchState(const uint8_t &s){
|
||||
bool changed = state_ != s;
|
||||
switch(s){
|
||||
case Operational:
|
||||
if(changed && sync_) sync_->addNode(this);
|
||||
break;
|
||||
case BootUp:
|
||||
case PreOperational:
|
||||
case Stopped:
|
||||
if(changed && sync_) sync_->removeNode(this);
|
||||
break;
|
||||
default:
|
||||
//error
|
||||
;
|
||||
}
|
||||
if(changed){
|
||||
state_ = (State) s;
|
||||
state_dispatcher_.dispatch(state_);
|
||||
cond.notify_one();
|
||||
}
|
||||
}
|
||||
void Node::handleNMT(const can::Frame & msg){
|
||||
boost::mutex::scoped_lock cond_lock(cond_mutex);
|
||||
uint16_t interval = getHeartbeatInterval();
|
||||
if(interval) heartbeat_timeout_ = get_abs_time(boost::chrono::milliseconds(3*interval));
|
||||
assert(msg.dlc == 1);
|
||||
switchState(msg.data[0]);
|
||||
}
|
||||
template<typename T> int Node::wait_for(const State &s, const T &timeout){
|
||||
boost::mutex::scoped_lock cond_lock(cond_mutex);
|
||||
time_point abs_time = get_abs_time(timeout);
|
||||
|
||||
while(s != state_) {
|
||||
if(cond.wait_until(cond_lock,abs_time) == boost::cv_status::timeout)
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
if( s!= state_){
|
||||
if(getHeartbeatInterval() == 0){
|
||||
switchState(s);
|
||||
return -1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
bool Node::checkHeartbeat(){
|
||||
if(getHeartbeatInterval() == 0) return true; //disabled
|
||||
boost::mutex::scoped_lock cond_lock(cond_mutex);
|
||||
return heartbeat_timeout_ >= boost::chrono::high_resolution_clock::now();
|
||||
}
|
||||
|
||||
|
||||
void Node::handleRead(LayerStatus &status, const LayerState ¤t_state) {
|
||||
if(current_state > Init){
|
||||
if(!checkHeartbeat()){
|
||||
status.error("heartbeat problem");
|
||||
} else if(getState() != Operational){
|
||||
status.error("not operational");
|
||||
} else{
|
||||
pdo_.read(status);
|
||||
}
|
||||
}
|
||||
}
|
||||
void Node::handleWrite(LayerStatus &status, const LayerState ¤t_state) {
|
||||
if(current_state > Init){
|
||||
if(getState() != Operational) status.error("not operational");
|
||||
else if(! pdo_.write()) status.error("PDO write problem");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void Node::handleDiag(LayerReport &report){
|
||||
State state = getState();
|
||||
if(state != Operational){
|
||||
report.error("Mode not operational");
|
||||
report.add("Node state", (int)state);
|
||||
}else if(!checkHeartbeat()){
|
||||
report.error("Heartbeat timeout");
|
||||
}
|
||||
}
|
||||
void Node::handleInit(LayerStatus &status){
|
||||
nmt_listener_ = interface_->createMsgListenerM(can::MsgHeader(0x700 + node_id_), this, &Node::handleNMT);
|
||||
|
||||
sdo_.init();
|
||||
try{
|
||||
if(!reset_com()) BOOST_THROW_EXCEPTION( TimeoutException("reset_timeout") );
|
||||
}
|
||||
catch(const TimeoutException&){
|
||||
status.error(boost::str(boost::format("could not reset node '%1%'") % (int)node_id_));
|
||||
return;
|
||||
}
|
||||
|
||||
if(!pdo_.init(getStorage(), status)){
|
||||
return;
|
||||
}
|
||||
getStorage()->init_all();
|
||||
sdo_.init(); // reread SDO paramters;
|
||||
// TODO: set SYNC data
|
||||
|
||||
try{
|
||||
if(!start()) BOOST_THROW_EXCEPTION( TimeoutException("start timeout") );
|
||||
}
|
||||
catch(const TimeoutException&){
|
||||
status.error(boost::str(boost::format("could not start node '%1%'") % (int)node_id_));
|
||||
}
|
||||
}
|
||||
void Node::handleRecover(LayerStatus &status){
|
||||
try{
|
||||
start();
|
||||
}
|
||||
catch(const TimeoutException&){
|
||||
status.error(boost::str(boost::format("could not start node '%1%'") % (int)node_id_));
|
||||
}
|
||||
}
|
||||
void Node::handleShutdown(LayerStatus &status){
|
||||
if(getHeartbeatInterval()> 0) heartbeat_.set(0);
|
||||
stop();
|
||||
nmt_listener_.reset();
|
||||
switchState(Unknown);
|
||||
}
|
||||
void Node::handleHalt(LayerStatus &status){
|
||||
// do nothing
|
||||
}
|
||||
480
Devices/Libraries/Ros/ros_canopen/canopen_master/src/objdict.cpp
Executable file
480
Devices/Libraries/Ros/ros_canopen/canopen_master/src/objdict.cpp
Executable file
@@ -0,0 +1,480 @@
|
||||
#include <canopen_master/objdict.h>
|
||||
#include <socketcan_interface/string.h>
|
||||
#include <boost/property_tree/ptree.hpp>
|
||||
#include <boost/property_tree/ini_parser.hpp>
|
||||
#include <boost/lexical_cast.hpp>
|
||||
#include <boost/algorithm/string.hpp>
|
||||
|
||||
namespace canopen{
|
||||
size_t hash_value(ObjectDict::Key const& k) { return k.hash; }
|
||||
std::ostream& operator<<(std::ostream& stream, const ObjectDict::Key &k) { return stream << std::string(k); }
|
||||
}
|
||||
|
||||
using namespace canopen;
|
||||
|
||||
template<> const String & HoldAny::get() const{
|
||||
return buffer;
|
||||
}
|
||||
|
||||
template<> String & ObjectStorage::Data::access(){
|
||||
if(!valid){
|
||||
THROW_WITH_KEY(std::length_error("buffer not valid") , key);
|
||||
}
|
||||
return buffer;
|
||||
}
|
||||
template<> String & ObjectStorage::Data::allocate(){
|
||||
buffer.resize(0);
|
||||
valid = true;
|
||||
return buffer;
|
||||
}
|
||||
|
||||
size_t ObjectDict::Key::fromString(const std::string &str){
|
||||
uint16_t index = 0;
|
||||
uint8_t sub_index = 0;
|
||||
int num = sscanf(str.c_str(),"%hxsub%hhx", &index, &sub_index);
|
||||
return (index << 16) | (num==2?sub_index:0xFFFF);
|
||||
}
|
||||
ObjectDict::Key::operator std::string() const{
|
||||
std::stringstream sstr;
|
||||
sstr << std::hex << index();
|
||||
if(hasSub()) sstr << "sub" << (int) sub_index();
|
||||
return sstr.str();
|
||||
}
|
||||
void ObjectStorage::Data::init(){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
|
||||
if(entry->init_val.is_empty()) return;
|
||||
|
||||
if(valid && !entry->def_val.is_empty() && buffer != entry->def_val.data()) return; // buffer was changed
|
||||
|
||||
if(!valid || buffer != entry->init_val.data()){
|
||||
buffer = entry->init_val.data();
|
||||
valid = true;
|
||||
if(entry->writable && (entry->def_val.is_empty() || entry->init_val.data() != entry->def_val.data()))
|
||||
write_delegate(*entry, buffer);
|
||||
}
|
||||
}
|
||||
void ObjectStorage::Data::force_write(){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
|
||||
if(!valid && entry->readable){
|
||||
read_delegate(*entry, buffer);
|
||||
valid = true;
|
||||
}
|
||||
if(valid) write_delegate(*entry, buffer);
|
||||
}
|
||||
|
||||
void ObjectStorage::Data::reset(){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
if(!entry->def_val.is_empty() && entry->def_val.type() == type_guard){
|
||||
buffer = entry->def_val.data();
|
||||
valid = true;
|
||||
}else{
|
||||
valid = false;
|
||||
}
|
||||
}
|
||||
|
||||
bool ObjectDict::iterate(ObjectDict::ObjectDictMap::const_iterator &it) const{
|
||||
if(it != ObjectDict::ObjectDictMap::const_iterator()){
|
||||
++it;
|
||||
}else it = dict_.begin();
|
||||
return it != dict_.end();
|
||||
}
|
||||
void set_access( ObjectDict::Entry &entry, std::string access){
|
||||
boost::algorithm::to_lower(access);
|
||||
entry.constant = false;
|
||||
if(access == "ro"){
|
||||
entry.readable = true;
|
||||
entry.writable = false;
|
||||
}else if (access == "wo"){
|
||||
entry.readable = false;
|
||||
entry.writable = true;
|
||||
}else if (access == "rw"){
|
||||
entry.readable = true;
|
||||
entry.writable = true;
|
||||
}else if (access == "rwr"){
|
||||
entry.readable = true;
|
||||
entry.writable = true;
|
||||
}else if (access == "rww"){
|
||||
entry.readable = true;
|
||||
entry.writable = true;
|
||||
}else if (access == "const"){
|
||||
entry.readable = true;
|
||||
entry.writable = false;
|
||||
entry.constant = true;
|
||||
}else{
|
||||
THROW_WITH_KEY(ParseException("Cannot determine access"), ObjectDict::Key(entry));
|
||||
}
|
||||
}
|
||||
|
||||
template<typename T> T int_from_string(const std::string &s);
|
||||
|
||||
template<> int8_t int_from_string(const std::string &s){
|
||||
return strtol(s.c_str(), 0, 0);
|
||||
}
|
||||
template<> uint8_t int_from_string(const std::string &s){
|
||||
return strtoul(s.c_str(), 0, 0);
|
||||
}
|
||||
template<> int16_t int_from_string(const std::string &s){
|
||||
return strtol(s.c_str(), 0, 0);
|
||||
}
|
||||
template<> uint16_t int_from_string(const std::string &s){
|
||||
return strtoul(s.c_str(), 0, 0);
|
||||
}
|
||||
template<> int32_t int_from_string(const std::string &s){
|
||||
return strtol(s.c_str(), 0, 0);
|
||||
}
|
||||
template<> uint32_t int_from_string(const std::string &s){
|
||||
return strtoul(s.c_str(), 0, 0);
|
||||
}
|
||||
|
||||
template<> int64_t int_from_string(const std::string &s){
|
||||
return strtoll(s.c_str(), 0, 0);
|
||||
}
|
||||
template<> uint64_t int_from_string(const std::string &s){
|
||||
return strtoull(s.c_str(), 0, 0);
|
||||
}
|
||||
|
||||
template<typename T> HoldAny parse_int(boost::property_tree::iptree &pt, const std::string &key){
|
||||
if(pt.count(key) == 0) return HoldAny(TypeGuard::create<T>());
|
||||
|
||||
std::string str = boost::trim_copy(pt.get<std::string>(key));
|
||||
if(boost::istarts_with(str,"$NODEID")){
|
||||
return HoldAny(NodeIdOffset<T>(int_from_string<T>(boost::trim_copy(str.substr(str.find("+",7)+1)))));
|
||||
}else if (boost::iends_with(str,"$NODEID")){
|
||||
return HoldAny(NodeIdOffset<T>(int_from_string<T>(boost::trim_copy(str.substr(0, str.find("+")+1)))));
|
||||
}else return HoldAny(int_from_string<T>(str));
|
||||
}
|
||||
|
||||
template<typename T> HoldAny parse_octets(boost::property_tree::iptree &pt, const std::string &key){
|
||||
std::string out;
|
||||
if(pt.count(key) == 0 || can::hex2buffer(out,pt.get<std::string>(key), true)) return HoldAny(TypeGuard::create<T>());
|
||||
return HoldAny(T(out));
|
||||
}
|
||||
|
||||
template<typename T> HoldAny parse_typed_value(boost::property_tree::iptree &pt, const std::string &key){
|
||||
if(pt.count(key) == 0) return HoldAny(TypeGuard::create<T>());
|
||||
return HoldAny(pt.get<T>(key));
|
||||
}
|
||||
template<> HoldAny parse_typed_value<String>(boost::property_tree::iptree &pt, const std::string &key){
|
||||
if(pt.count(key) == 0) return HoldAny(TypeGuard::create<String>());
|
||||
return HoldAny(String(pt.get<std::string>(key)));
|
||||
}
|
||||
|
||||
struct ReadAnyValue{
|
||||
template<const ObjectDict::DataTypes dt> static HoldAny func(boost::property_tree::iptree &pt, const std::string &key);
|
||||
static HoldAny read_value(boost::property_tree::iptree &pt, uint16_t data_type, const std::string &key){
|
||||
return branch_type<ReadAnyValue, HoldAny (boost::property_tree::iptree &, const std::string &)>(data_type)(pt, key);
|
||||
}
|
||||
};
|
||||
template<> HoldAny ReadAnyValue::func<ObjectDict::DEFTYPE_INTEGER8>(boost::property_tree::iptree &pt, const std::string &key){ return parse_int<int8_t>(pt,key); }
|
||||
template<> HoldAny ReadAnyValue::func<ObjectDict::DEFTYPE_INTEGER16>(boost::property_tree::iptree &pt, const std::string &key){ return parse_int<int16_t>(pt,key); }
|
||||
template<> HoldAny ReadAnyValue::func<ObjectDict::DEFTYPE_INTEGER32>(boost::property_tree::iptree &pt, const std::string &key){ return parse_int<int32_t>(pt,key); }
|
||||
template<> HoldAny ReadAnyValue::func<ObjectDict::DEFTYPE_INTEGER64>(boost::property_tree::iptree &pt, const std::string &key){ return parse_int<int64_t>(pt,key); }
|
||||
|
||||
template<> HoldAny ReadAnyValue::func<ObjectDict::DEFTYPE_UNSIGNED8>(boost::property_tree::iptree &pt, const std::string &key){ return parse_int<uint8_t>(pt,key); }
|
||||
template<> HoldAny ReadAnyValue::func<ObjectDict::DEFTYPE_UNSIGNED16>(boost::property_tree::iptree &pt, const std::string &key){ return parse_int<uint16_t>(pt,key); }
|
||||
template<> HoldAny ReadAnyValue::func<ObjectDict::DEFTYPE_UNSIGNED32>(boost::property_tree::iptree &pt, const std::string &key){ return parse_int<uint32_t>(pt,key); }
|
||||
template<> HoldAny ReadAnyValue::func<ObjectDict::DEFTYPE_UNSIGNED64>(boost::property_tree::iptree &pt, const std::string &key){ return parse_int<uint64_t>(pt,key); }
|
||||
|
||||
template<> HoldAny ReadAnyValue::func<ObjectDict::DEFTYPE_DOMAIN>(boost::property_tree::iptree &pt, const std::string &key)
|
||||
{ return parse_octets<ObjectStorage::DataType<ObjectDict::DEFTYPE_DOMAIN>::type>(pt,key); }
|
||||
|
||||
template<> HoldAny ReadAnyValue::func<ObjectDict::DEFTYPE_OCTET_STRING>(boost::property_tree::iptree &pt, const std::string &key)
|
||||
{ return parse_octets<ObjectStorage::DataType<ObjectDict::DEFTYPE_OCTET_STRING>::type>(pt,key); }
|
||||
|
||||
template<const ObjectDict::DataTypes dt> HoldAny ReadAnyValue::func(boost::property_tree::iptree &pt, const std::string &key){
|
||||
return parse_typed_value<typename ObjectStorage::DataType<dt>::type>(pt, key);
|
||||
}
|
||||
|
||||
template<typename T> void read_optional(T& var, boost::property_tree::iptree &pt, const std::string &key){
|
||||
var = pt.get(key, T());
|
||||
}
|
||||
|
||||
template<typename T> void read_integer(T& var, boost::property_tree::iptree &pt, const std::string &key){
|
||||
var = int_from_string<T>(pt.get<std::string>(key));
|
||||
}
|
||||
|
||||
template<typename T> T read_integer(boost::property_tree::iptree &pt, const std::string &key){
|
||||
return int_from_string<T>(pt.get<std::string>(key, std::string()));
|
||||
}
|
||||
|
||||
|
||||
void read_var(ObjectDict::Entry &entry, boost::property_tree::iptree &object){
|
||||
read_integer<uint16_t>(entry.data_type, object, "DataType");
|
||||
entry.mappable = object.get<bool>("PDOMapping", false);
|
||||
try{
|
||||
set_access(entry, object.get<std::string>("AccessType"));
|
||||
}
|
||||
catch(...){
|
||||
THROW_WITH_KEY(ParseException("No AccessType") , ObjectDict::Key(entry));
|
||||
|
||||
}
|
||||
entry.def_val = ReadAnyValue::read_value(object, entry.data_type, "DefaultValue");
|
||||
entry.init_val = ReadAnyValue::read_value(object, entry.data_type, "ParameterValue");
|
||||
}
|
||||
|
||||
void parse_object(ObjectDictSharedPtr dict, boost::property_tree::iptree &pt, const std::string &name, const uint8_t* sub_index = 0){
|
||||
boost::optional<boost::property_tree::iptree&> object = pt.get_child_optional(name.substr(2));
|
||||
if(!object) return;
|
||||
|
||||
std::shared_ptr<ObjectDict::Entry> entry = std::make_shared<ObjectDict::Entry>();
|
||||
try{
|
||||
entry->index = int_from_string<uint16_t>(name);
|
||||
entry->obj_code = ObjectDict::Code(int_from_string<uint16_t>(object->get<std::string>("ObjectType", boost::lexical_cast<std::string>((uint16_t)ObjectDict::VAR))));
|
||||
entry->desc = object->get<std::string>("Denotation",object->get<std::string>("ParameterName"));
|
||||
|
||||
// std::cout << name << ": "<< entry->desc << std::endl;
|
||||
if(entry->obj_code == ObjectDict::VAR || entry->obj_code == ObjectDict::DOMAIN_DATA || sub_index){
|
||||
entry->sub_index = sub_index? *sub_index: 0;
|
||||
read_var(*entry, *object);
|
||||
dict->insert(sub_index != 0, entry);
|
||||
}else if(entry->obj_code == ObjectDict::ARRAY || entry->obj_code == ObjectDict::RECORD){
|
||||
uint8_t subs = read_integer<uint8_t>(*object, "CompactSubObj");
|
||||
if(subs){ // compact
|
||||
dict->insert(true, std::make_shared<const canopen::ObjectDict::Entry>(entry->index, 0, ObjectDict::DEFTYPE_UNSIGNED8, "NrOfObjects", true, false, false, HoldAny(subs)));
|
||||
|
||||
read_var(*entry, *object);
|
||||
|
||||
for(uint8_t i=1; i< subs; ++i){
|
||||
std::string subname = pt.get<std::string>(name.substr(2)+"Name." + boost::lexical_cast<std::string>((int)i),entry->desc + boost::lexical_cast<std::string>((int)i));
|
||||
subname = pt.get<std::string>(name.substr(2)+"Denotation." + boost::lexical_cast<std::string>((int)i), subname);
|
||||
|
||||
dict->insert(true, std::make_shared<const canopen::ObjectDict::Entry>(entry->index, i, entry->data_type, name, entry->readable, entry->writable, entry->mappable, entry->def_val,
|
||||
ReadAnyValue::read_value(pt, entry->data_type, name.substr(2)+"Value." + boost::lexical_cast<std::string>((int)i))));
|
||||
}
|
||||
}else{
|
||||
read_integer(subs, *object, "SubNumber");
|
||||
for(uint8_t i=0; i< subs; ++i){
|
||||
std::stringstream buf;
|
||||
buf << name << "sub" << std::hex << int(i);
|
||||
// std::cout << "added " << buf.str() << " " << int(i) << std::endl;
|
||||
parse_object(dict, pt, buf.str(), &i);
|
||||
}
|
||||
}
|
||||
}else{
|
||||
THROW_WITH_KEY(ParseException("Object type not supported") , ObjectDict::Key(*entry));
|
||||
}
|
||||
}
|
||||
catch(const std::bad_cast &e){
|
||||
throw ParseException(std::string("Type of ") + name + " does not match or is not supported");
|
||||
}
|
||||
catch(const std::exception&e){
|
||||
throw ParseException(std::string("Cannot process ") + name + ": " + e.what());
|
||||
}
|
||||
}
|
||||
void parse_objects(ObjectDictSharedPtr dict, boost::property_tree::iptree &pt, const std::string &key){
|
||||
if(!pt.count(key)) return;
|
||||
|
||||
boost::property_tree::iptree objects = pt.get_child(key);
|
||||
uint16_t count = read_integer<uint16_t>(objects, "SupportedObjects");
|
||||
for(uint16_t i=0; i < count; ++i){
|
||||
std::string name = objects.get<std::string>(boost::lexical_cast<std::string>(i+1));
|
||||
parse_object(dict, pt, name);
|
||||
}
|
||||
}
|
||||
ObjectDictSharedPtr ObjectDict::fromFile(const std::string &path, const ObjectDict::Overlay &overlay){
|
||||
DeviceInfo info;
|
||||
boost::property_tree::iptree pt;
|
||||
ObjectDictSharedPtr dict;
|
||||
|
||||
boost::property_tree::read_ini(path, pt);
|
||||
|
||||
boost::property_tree::iptree di = pt.get_child("DeviceInfo");
|
||||
|
||||
read_optional(info.vendor_name, di, "VendorName");
|
||||
read_optional(info.vendor_number, di, "VendorNumber");
|
||||
read_optional(info.product_name, di, "ProductName");
|
||||
read_optional(info.product_number, di, "ProductNumber");
|
||||
read_optional(info.revision_number, di, "RevisionNumber");
|
||||
read_optional(info.order_code, di, "OrderCode");
|
||||
read_optional(info.simple_boot_up_master, di, "SimpleBootUpMaster");
|
||||
read_optional(info.simple_boot_up_slave, di, "SimpleBootUpSlave");
|
||||
read_optional(info.granularity, di, "Granularity");
|
||||
read_optional(info.dynamic_channels_supported, di, "DynamicChannelsSupported");
|
||||
read_optional(info.group_messaging, di, "GroupMessaging");
|
||||
read_optional(info.nr_of_rx_pdo, di, "NrOfRXPDO");
|
||||
read_optional(info.nr_of_tx_pdo, di, "NrOfTXPDO");
|
||||
read_optional(info.lss_supported, di, "LSS_Supported");
|
||||
|
||||
std::unordered_set<uint32_t> baudrates;
|
||||
std::unordered_set<uint16_t> dummy_usage;
|
||||
|
||||
for(boost::property_tree::iptree::value_type &v: di){
|
||||
if(v.first.find("BaudRate_") == 0){
|
||||
uint16_t rate = int_from_string<uint16_t>(v.first.substr(9));
|
||||
if(v.second.get_value<bool>())
|
||||
info.baudrates.insert(rate * 1000);
|
||||
}
|
||||
}
|
||||
|
||||
if(pt.count("DummyUsage")){
|
||||
for(boost::property_tree::iptree::value_type &v: pt.get_child("DummyUsage")){
|
||||
if(v.first.find("Dummy") == 0){
|
||||
// std::cout << ("0x"+v.first.substr(5)) << std::endl;
|
||||
uint16_t dummy = int_from_string<uint16_t>("0x"+v.first.substr(5));
|
||||
if(v.second.get_value<bool>())
|
||||
info.dummy_usage.insert(dummy);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
dict = std::make_shared<ObjectDict>(info);
|
||||
|
||||
for(Overlay::const_iterator it= overlay.begin(); it != overlay.end(); ++it){
|
||||
pt.get_child(it->first).put("ParameterValue", it->second);
|
||||
}
|
||||
|
||||
parse_objects(dict, pt, "MandatoryObjects");
|
||||
parse_objects(dict, pt, "OptionalObjects");
|
||||
parse_objects(dict, pt, "ManufacturerObjects");
|
||||
|
||||
return dict;
|
||||
}
|
||||
|
||||
size_t ObjectStorage::map(const ObjectDict::EntryConstSharedPtr &e, const ObjectDict::Key &key, const ReadFunc & read_delegate, const WriteFunc & write_delegate){
|
||||
ObjectStorageMap::iterator it = storage_.find(key);
|
||||
|
||||
if(it == storage_.end()){
|
||||
|
||||
DataSharedPtr data;
|
||||
|
||||
|
||||
if(!e->def_val.type().valid()){
|
||||
THROW_WITH_KEY(std::bad_cast() , key);
|
||||
}
|
||||
|
||||
data = std::make_shared<Data>(key, e,e->def_val.type(),read_delegate_, write_delegate_);
|
||||
|
||||
std::pair<ObjectStorageMap::iterator, bool> ok = storage_.insert(std::make_pair(key, data));
|
||||
it = ok.first;
|
||||
it->second->reset();
|
||||
|
||||
}
|
||||
|
||||
if(read_delegate && write_delegate){
|
||||
it->second->set_delegates(read_delegate_, write_delegate);
|
||||
it->second->force_write(); // update buffer
|
||||
it->second->set_delegates(read_delegate, write_delegate_);
|
||||
}else if(write_delegate) {
|
||||
it->second->set_delegates(read_delegate_, write_delegate);
|
||||
it->second->force_write(); // update buffer
|
||||
}else if(read_delegate){
|
||||
it->second->set_delegates(read_delegate, write_delegate_);
|
||||
}
|
||||
return it->second->size();
|
||||
}
|
||||
|
||||
size_t ObjectStorage::map(uint16_t index, uint8_t sub_index, const ReadFunc & read_delegate, const WriteFunc & write_delegate){
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
|
||||
try{
|
||||
ObjectDict::Key key(index,sub_index);
|
||||
const ObjectDict::EntryConstSharedPtr e = dict_->get(key);
|
||||
return map(e, key,read_delegate, write_delegate);
|
||||
}
|
||||
catch(std::out_of_range) {
|
||||
if(sub_index != 0) throw;
|
||||
|
||||
ObjectDict::Key key(index);
|
||||
const ObjectDict::EntryConstSharedPtr e = dict_->get(key);
|
||||
return map(e, key, read_delegate, write_delegate);
|
||||
}
|
||||
}
|
||||
|
||||
ObjectStorage::ObjectStorage(ObjectDictConstSharedPtr dict, uint8_t node_id, ReadFunc read_delegate, WriteFunc write_delegate)
|
||||
:read_delegate_(read_delegate), write_delegate_(write_delegate), dict_(dict), node_id_(node_id){
|
||||
assert(dict_);
|
||||
assert(read_delegate_);
|
||||
assert(write_delegate_);
|
||||
}
|
||||
|
||||
void ObjectStorage::init_nolock(const ObjectDict::Key &key, const ObjectDict::EntryConstSharedPtr &entry){
|
||||
|
||||
if(!entry->init_val.is_empty()){
|
||||
ObjectStorageMap::iterator it = storage_.find(key);
|
||||
|
||||
if(it == storage_.end()){
|
||||
DataSharedPtr data = std::make_shared<Data>(key,entry, entry->init_val.type(), read_delegate_, write_delegate_);
|
||||
std::pair<ObjectStorageMap::iterator, bool> ok = storage_.insert(std::make_pair(key, data));
|
||||
it = ok.first;
|
||||
if(!ok.second){
|
||||
THROW_WITH_KEY(std::bad_alloc() , key);
|
||||
}
|
||||
}
|
||||
it->second->init();
|
||||
}
|
||||
}
|
||||
void ObjectStorage::init(const ObjectDict::Key &key){
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
init_nolock(key, dict_->get(key));
|
||||
}
|
||||
void ObjectStorage::init_all(){
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
|
||||
ObjectDict::ObjectDictMap::const_iterator entry_it;
|
||||
while(dict_->iterate(entry_it)){
|
||||
init_nolock(entry_it->first, entry_it->second);
|
||||
}
|
||||
}
|
||||
|
||||
void ObjectStorage::reset(){
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
for(ObjectStorageMap::iterator it = storage_.begin(); it != storage_.end(); ++it){
|
||||
it->second->reset();
|
||||
}
|
||||
}
|
||||
|
||||
template<const ObjectDict::DataTypes dt, typename T> std::string formatValue(const T &value){
|
||||
std::stringstream sstr;
|
||||
sstr << value;
|
||||
return sstr.str();
|
||||
}
|
||||
template<> std::string formatValue<ObjectDict::DEFTYPE_DOMAIN>(const std::string &value){
|
||||
return can::buffer2hex(value, false);
|
||||
}
|
||||
template<> std::string formatValue<ObjectDict::DEFTYPE_OCTET_STRING>(const std::string &value){
|
||||
return can::buffer2hex(value, false);
|
||||
}
|
||||
|
||||
struct PrintValue {
|
||||
template<const ObjectDict::DataTypes dt> static std::string func(ObjectStorage& storage, const ObjectDict::Key &key, bool cached){
|
||||
ObjectStorage::Entry<typename ObjectStorage::DataType<dt>::type> entry = storage.entry<typename ObjectStorage::DataType<dt>::type>(key);
|
||||
return formatValue<dt>(cached? entry.get_cached() : entry.get() );
|
||||
}
|
||||
static std::function<std::string()> getReader(ObjectStorage& storage, const ObjectDict::Key &key, bool cached){
|
||||
ObjectDict::DataTypes data_type = (ObjectDict::DataTypes) storage.dict_->get(key)->data_type;
|
||||
return std::bind(branch_type<PrintValue, std::string (ObjectStorage&, const ObjectDict::Key &, bool)>(data_type),std::ref(storage), key, cached);
|
||||
}
|
||||
};
|
||||
|
||||
ObjectStorage::ReadStringFuncType ObjectStorage::getStringReader(const ObjectDict::Key &key, bool cached){
|
||||
return PrintValue::getReader(*this, key, cached);
|
||||
}
|
||||
|
||||
struct WriteStringValue {
|
||||
typedef HoldAny (*reader_type)(boost::property_tree::iptree &, const std::string &);
|
||||
template<typename T> static void write(ObjectStorage::Entry<T> entry, bool cached, reader_type reader, const std::string &value){
|
||||
boost::property_tree::iptree pt;
|
||||
pt.put("value", value);
|
||||
HoldAny any = reader(pt, "value");
|
||||
if(cached){
|
||||
entry.set_cached(any.get<T>());
|
||||
} else {
|
||||
entry.set(any.get<T>());
|
||||
}
|
||||
}
|
||||
template<const ObjectDict::DataTypes dt> static std::function<void (const std::string&)> func(ObjectStorage& storage, const ObjectDict::Key &key, bool cached){
|
||||
ObjectStorage::Entry<typename ObjectStorage::DataType<dt>::type> entry = storage.entry<typename ObjectStorage::DataType<dt>::type>(key);
|
||||
reader_type reader = branch_type<ReadAnyValue, HoldAny (boost::property_tree::iptree &, const std::string &)>(dt);
|
||||
return std::bind(&WriteStringValue::write<typename ObjectStorage::DataType<dt>::type >, entry, cached, reader, std::placeholders::_1);
|
||||
}
|
||||
static std::function<void (const std::string&)> getWriter(ObjectStorage& storage, const ObjectDict::Key &key, bool cached){
|
||||
ObjectDict::DataTypes data_type = (ObjectDict::DataTypes) storage.dict_->get(key)->data_type;
|
||||
return branch_type<WriteStringValue, std::function<void (const std::string&)> (ObjectStorage&, const ObjectDict::Key &, bool)>(data_type)(storage, key, cached);
|
||||
}
|
||||
};
|
||||
|
||||
ObjectStorage::WriteStringFuncType ObjectStorage::getStringWriter(const ObjectDict::Key &key, bool cached){
|
||||
return WriteStringValue::getWriter(*this, key, cached);
|
||||
}
|
||||
383
Devices/Libraries/Ros/ros_canopen/canopen_master/src/pdo.cpp
Executable file
383
Devices/Libraries/Ros/ros_canopen/canopen_master/src/pdo.cpp
Executable file
@@ -0,0 +1,383 @@
|
||||
#include <canopen_master/canopen.h>
|
||||
|
||||
using namespace canopen;
|
||||
|
||||
#pragma pack(push) /* push current alignment to stack */
|
||||
#pragma pack(1) /* set alignment to 1 byte boundary */
|
||||
|
||||
class PDOid{
|
||||
const uint32_t value_;
|
||||
public:
|
||||
static const unsigned int ID_MASK = (1u << 29)-1;
|
||||
static const unsigned int EXTENDED_MASK = (1u << 29);
|
||||
static const unsigned int NO_RTR_MASK = (1u << 30);
|
||||
static const unsigned int INVALID_MASK = (1u << 31);
|
||||
|
||||
PDOid(const uint32_t &val)
|
||||
: value_(val)
|
||||
{}
|
||||
can::Header header(bool fill_rtr = false) const {
|
||||
return can::Header(value_ & ID_MASK, value_ & EXTENDED_MASK, fill_rtr && !(value_ & NO_RTR_MASK), false);
|
||||
}
|
||||
bool isInvalid() const { return value_ & INVALID_MASK; }
|
||||
};
|
||||
|
||||
struct PDOmap{
|
||||
uint8_t length;
|
||||
uint8_t sub_index;
|
||||
uint16_t index;
|
||||
PDOmap(uint32_t val)
|
||||
: length(val & 0xFF),
|
||||
sub_index((val>>8) & 0xFF),
|
||||
index(val>>16)
|
||||
{}
|
||||
};
|
||||
|
||||
#pragma pack(pop) /* pop previous alignment from stack */
|
||||
|
||||
|
||||
const uint8_t SUB_COM_NUM = 0;
|
||||
const uint8_t SUB_COM_COB_ID = 1;
|
||||
const uint8_t SUB_COM_TRANSMISSION_TYPE = 2;
|
||||
const uint8_t SUB_COM_RESERVED = 4;
|
||||
|
||||
const uint8_t SUB_MAP_NUM = 0;
|
||||
|
||||
const uint16_t RPDO_COM_BASE =0x1400;
|
||||
const uint16_t RPDO_MAP_BASE =0x1600;
|
||||
const uint16_t TPDO_COM_BASE =0x1800;
|
||||
const uint16_t TPDO_MAP_BASE =0x1A00;
|
||||
|
||||
bool check_com_changed(const ObjectDict &dict, const uint16_t com_id){
|
||||
bool com_changed = false;
|
||||
|
||||
// check if com parameter has to be set
|
||||
for(uint8_t sub = 0; sub <=6 ; ++sub){
|
||||
try{
|
||||
if(!dict(com_id,sub).init_val.is_empty()){
|
||||
com_changed = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
catch (std::out_of_range) {}
|
||||
}
|
||||
return com_changed;
|
||||
}
|
||||
|
||||
bool check_map_changed(const uint8_t &num, const ObjectDict &dict, const uint16_t &map_index){
|
||||
bool map_changed = false;
|
||||
|
||||
// check if mapping has to be set
|
||||
if(num <= 0x40){
|
||||
for(uint8_t sub = 1; sub <=num ; ++sub){
|
||||
try{
|
||||
if(!dict(map_index,sub).init_val.is_empty()){
|
||||
map_changed = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
catch (std::out_of_range) {}
|
||||
}
|
||||
}else{
|
||||
map_changed = dict( map_index ,0 ).init_val.is_empty();
|
||||
}
|
||||
return map_changed;
|
||||
}
|
||||
void PDOMapper::PDO::parse_and_set_mapping(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index, const bool &read, const bool &write){
|
||||
|
||||
const canopen::ObjectDict & dict = *storage->dict_;
|
||||
|
||||
ObjectStorage::Entry<uint8_t> num_entry;
|
||||
storage->entry(num_entry, map_index, SUB_MAP_NUM);
|
||||
|
||||
uint8_t map_num;
|
||||
|
||||
try{
|
||||
map_num = num_entry.desc().value().get<uint8_t>();
|
||||
}catch(...){
|
||||
map_num = 0;
|
||||
}
|
||||
|
||||
bool map_changed = check_map_changed(map_num, dict, map_index);
|
||||
|
||||
// disable PDO if needed
|
||||
ObjectStorage::Entry<uint32_t> cob_id;
|
||||
storage->entry(cob_id, com_index, SUB_COM_COB_ID);
|
||||
|
||||
bool com_changed = check_com_changed(dict, map_index);
|
||||
if((map_changed || com_changed) && cob_id.desc().writable){
|
||||
cob_id.set(cob_id.get() | PDOid::INVALID_MASK);
|
||||
}
|
||||
if(map_num > 0 && map_num <= 0x40){ // actual mapping
|
||||
if(map_changed){
|
||||
num_entry.set(0);
|
||||
}
|
||||
|
||||
frame.dlc = 0;
|
||||
for(uint8_t sub = 1; sub <=map_num; ++sub){
|
||||
ObjectStorage::Entry<uint32_t> mapentry;
|
||||
storage->entry(mapentry, map_index, sub);
|
||||
const HoldAny init = dict(map_index ,sub).init_val;
|
||||
if(!init.is_empty()) mapentry.set(init.get<uint32_t>());
|
||||
|
||||
PDOmap param(mapentry.get_cached());
|
||||
BufferSharedPtr b = std::make_shared<Buffer>(param.length/8);
|
||||
if(param.index < 0x1000){
|
||||
// TODO: check DummyUsage
|
||||
}else{
|
||||
ObjectStorage::ReadFunc rd;
|
||||
ObjectStorage::WriteFunc wd;
|
||||
|
||||
if(read){
|
||||
rd = std::bind<void(Buffer::*)(const canopen::ObjectDict::Entry&, String&)>(&Buffer::read, b.get(), std::placeholders::_1, std::placeholders::_2);
|
||||
}
|
||||
if(read || write)
|
||||
{
|
||||
wd = std::bind<void(Buffer::*)(const canopen::ObjectDict::Entry&, const String&)>(&Buffer::write, b.get(), std::placeholders::_1, std::placeholders::_2);
|
||||
size_t l = storage->map(param.index, param.sub_index, rd, wd);
|
||||
assert(l == param.length/8);
|
||||
}
|
||||
}
|
||||
|
||||
frame.dlc += b->size;
|
||||
assert( frame.dlc <= 8 );
|
||||
b->clean();
|
||||
buffers.push_back(b);
|
||||
}
|
||||
}
|
||||
if(com_changed){
|
||||
uint8_t subs = dict(com_index, SUB_COM_NUM).value().get<uint8_t>();
|
||||
for(uint8_t i = SUB_COM_NUM+1; i <= subs; ++i){
|
||||
if(i == SUB_COM_COB_ID || i == SUB_COM_RESERVED) continue;
|
||||
try{
|
||||
storage->init(ObjectDict::Key(com_index, i));
|
||||
}
|
||||
catch (const std::out_of_range &){
|
||||
// entry was not provided, so skip it
|
||||
}
|
||||
}
|
||||
}
|
||||
if(map_changed){
|
||||
num_entry.set(map_num);
|
||||
}
|
||||
if((com_changed || map_changed) && cob_id.desc().writable){
|
||||
storage->init(ObjectDict::Key(com_index, SUB_COM_COB_ID));
|
||||
|
||||
cob_id.set(NodeIdOffset<uint32_t>::apply(dict(com_index, SUB_COM_COB_ID).value(), storage->node_id_));
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
PDOMapper::PDOMapper(const can::CommInterfaceSharedPtr interface)
|
||||
:interface_(interface)
|
||||
{
|
||||
}
|
||||
bool PDOMapper::init(const ObjectStorageSharedPtr storage, LayerStatus &status){
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
|
||||
try{
|
||||
rpdos_.clear();
|
||||
|
||||
const canopen::ObjectDict & dict = *storage->dict_;
|
||||
for(uint16_t i=0; i < 512 && rpdos_.size() < dict.device_info.nr_of_tx_pdo;++i){ // TPDOs of device
|
||||
if(!dict.has(TPDO_COM_BASE + i,0) && !dict.has(TPDO_MAP_BASE + i,0)) continue;
|
||||
|
||||
RPDO::RPDOSharedPtr rpdo = RPDO::create(interface_,storage, TPDO_COM_BASE + i, TPDO_MAP_BASE + i);
|
||||
if(rpdo){
|
||||
rpdos_.insert(rpdo);
|
||||
}
|
||||
}
|
||||
// ROSCANOPEN_DEBUG("canopen_master", "RPDOs: " << rpdos_.size());
|
||||
|
||||
tpdos_.clear();
|
||||
for(uint16_t i=0; i < 512 && tpdos_.size() < dict.device_info.nr_of_rx_pdo;++i){ // RPDOs of device
|
||||
if(!dict.has(RPDO_COM_BASE + i,0) && !dict.has(RPDO_MAP_BASE + i,0)) continue;
|
||||
|
||||
TPDO::TPDOSharedPtr tpdo = TPDO::create(interface_,storage, RPDO_COM_BASE + i, RPDO_MAP_BASE + i);
|
||||
if(tpdo){
|
||||
tpdos_.insert(tpdo);
|
||||
}
|
||||
}
|
||||
// ROSCANOPEN_DEBUG("canopen_master", "TPDOs: " << tpdos_.size());
|
||||
|
||||
return true;
|
||||
}
|
||||
catch(const std::out_of_range &e){
|
||||
status.error(std::string("PDO error: ") + e.what());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool PDOMapper::RPDO::init(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
listener_.reset();
|
||||
const canopen::ObjectDict & dict = *storage->dict_;
|
||||
parse_and_set_mapping(storage, com_index, map_index, true, false);
|
||||
|
||||
PDOid pdoid( NodeIdOffset<uint32_t>::apply(dict(com_index, SUB_COM_COB_ID).value(), storage->node_id_) );
|
||||
|
||||
if(buffers.empty() || pdoid.isInvalid()){
|
||||
return false;
|
||||
}
|
||||
|
||||
frame = pdoid.header(true);
|
||||
|
||||
transmission_type = dict(com_index, SUB_COM_TRANSMISSION_TYPE).value().get<uint8_t>();
|
||||
|
||||
listener_ = interface_->createMsgListenerM(pdoid.header(), this, &RPDO::handleFrame);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool PDOMapper::TPDO::init(const ObjectStorageSharedPtr &storage, const uint16_t &com_index, const uint16_t &map_index){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
const canopen::ObjectDict & dict = *storage->dict_;
|
||||
|
||||
|
||||
PDOid pdoid( NodeIdOffset<uint32_t>::apply(dict(com_index, SUB_COM_COB_ID).value(), storage->node_id_) );
|
||||
frame = pdoid.header();
|
||||
|
||||
parse_and_set_mapping(storage, com_index, map_index, false, true);
|
||||
if(buffers.empty() || pdoid.isInvalid()){
|
||||
return false;
|
||||
}
|
||||
ObjectStorage::Entry<uint8_t> tt;
|
||||
storage->entry(tt, com_index, SUB_COM_TRANSMISSION_TYPE);
|
||||
transmission_type = tt.desc().value().get<uint8_t>();
|
||||
|
||||
if(transmission_type != 1 && transmission_type <=240){
|
||||
tt.set(1); // enforce 1 for compatibility
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void PDOMapper::TPDO::sync(){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
|
||||
bool updated = false;
|
||||
size_t len = frame.dlc;
|
||||
can::Frame::value_type * dest = frame.c_array();
|
||||
for(std::vector< BufferSharedPtr >::iterator b_it = buffers.begin(); b_it != buffers.end(); ++b_it){
|
||||
Buffer &b = **b_it;
|
||||
if(len >= b.size){
|
||||
updated = b.read(dest, len) || updated;
|
||||
len -= b.size;
|
||||
dest += b.size;
|
||||
}else{
|
||||
// ERROR
|
||||
}
|
||||
}
|
||||
|
||||
if( len != 0){
|
||||
// ERROR
|
||||
}
|
||||
if(updated){
|
||||
interface_->send( frame );
|
||||
}else{
|
||||
// TODO: Notify
|
||||
}
|
||||
}
|
||||
|
||||
void PDOMapper::RPDO::sync(LayerStatus &status){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
if((transmission_type >= 1 && transmission_type <= 240) || transmission_type == 0xFC){ // cyclic
|
||||
if(timeout > 0){
|
||||
--timeout;
|
||||
}else if(timeout == 0) {
|
||||
status.warn("RPDO timeout");
|
||||
}
|
||||
}
|
||||
if(transmission_type == 0xFC || transmission_type == 0xFD){
|
||||
if(frame.is_rtr){
|
||||
interface_->send(frame);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void PDOMapper::RPDO::handleFrame(const can::Frame & msg){
|
||||
size_t offset = 0;
|
||||
const uint8_t * src = msg.data.data();
|
||||
for(std::vector<BufferSharedPtr >::iterator it = buffers.begin(); it != buffers.end(); ++it){
|
||||
Buffer &b = **it;
|
||||
|
||||
if( offset + b.size <= msg.dlc ){
|
||||
b.write(src+offset, b.size);
|
||||
offset += b.size;
|
||||
}else{
|
||||
// ERROR
|
||||
}
|
||||
}
|
||||
if( offset != msg.dlc ){
|
||||
// ERROR
|
||||
}
|
||||
{
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
if(transmission_type >= 1 && transmission_type <= 240){
|
||||
timeout = transmission_type + 2;
|
||||
}else if(transmission_type == 0xFC || transmission_type == 0xFD){
|
||||
if(frame.is_rtr){
|
||||
timeout = 1+2;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void PDOMapper::read(LayerStatus &status){
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
for(std::unordered_set<RPDO::RPDOSharedPtr >::iterator it = rpdos_.begin(); it != rpdos_.end(); ++it){
|
||||
(*it)->sync(status);
|
||||
}
|
||||
}
|
||||
bool PDOMapper::write(){
|
||||
boost::mutex::scoped_lock lock(mutex_);
|
||||
for(std::unordered_set<TPDO::TPDOSharedPtr >::iterator it = tpdos_.begin(); it != tpdos_.end(); ++it){
|
||||
(*it)->sync();
|
||||
}
|
||||
return true; // TODO: check for errors
|
||||
}
|
||||
|
||||
bool PDOMapper::Buffer::read(uint8_t* b, const size_t len){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
if(size > len){
|
||||
BOOST_THROW_EXCEPTION( std::bad_cast() );
|
||||
}
|
||||
if(empty) return false;
|
||||
|
||||
memcpy(b,&buffer[0], size);
|
||||
bool was_dirty = dirty;
|
||||
dirty = false;
|
||||
return was_dirty;
|
||||
}
|
||||
void PDOMapper::Buffer::write(const uint8_t* b, const size_t len){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
if(size > len){
|
||||
BOOST_THROW_EXCEPTION( std::bad_cast() );
|
||||
}
|
||||
empty = false;
|
||||
dirty = true;
|
||||
memcpy(&buffer[0], b, size);
|
||||
}
|
||||
void PDOMapper::Buffer::read(const canopen::ObjectDict::Entry &entry, String &data){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
time_point abs_time = get_abs_time(boost::chrono::seconds(1));
|
||||
if(size != data.size()){
|
||||
THROW_WITH_KEY(std::bad_cast(), ObjectDict::Key(entry));
|
||||
}
|
||||
if(empty){
|
||||
THROW_WITH_KEY(TimeoutException("PDO data empty"), ObjectDict::Key(entry));
|
||||
}
|
||||
if(dirty){
|
||||
data.assign(buffer.begin(), buffer.end());
|
||||
dirty = false;
|
||||
}
|
||||
}
|
||||
void PDOMapper::Buffer::write(const canopen::ObjectDict::Entry &entry, const String &data){
|
||||
boost::mutex::scoped_lock lock(mutex);
|
||||
if(size != data.size()){
|
||||
THROW_WITH_KEY(std::bad_cast(), ObjectDict::Key(entry));
|
||||
}
|
||||
empty = false;
|
||||
dirty = true;
|
||||
buffer.assign(data.begin(),data.end());
|
||||
}
|
||||
451
Devices/Libraries/Ros/ros_canopen/canopen_master/src/sdo.cpp
Executable file
451
Devices/Libraries/Ros/ros_canopen/canopen_master/src/sdo.cpp
Executable file
@@ -0,0 +1,451 @@
|
||||
#include <canopen_master/canopen.h>
|
||||
|
||||
using namespace canopen;
|
||||
|
||||
const uint8_t COMMAND_MASK = (1<<7) | (1<<6) | (1<<5);
|
||||
const uint8_t INITIATE_DOWNLOAD_REQUEST = (0 << 5);
|
||||
const uint8_t INITIATE_DOWNLOAD_RESPONSE = (1 << 5);
|
||||
const uint8_t DOWNLOAD_SEGMENT_REQUEST = (1 << 5);
|
||||
const uint8_t DOWNLOAD_SEGMENT_RESPONSE = (3 << 5);
|
||||
const uint8_t INITIATE_UPLOAD_REQUEST = (2 << 5);
|
||||
const uint8_t INITIATE_UPLOAD_RESPONSE = (2 << 5);
|
||||
const uint8_t UPLOAD_SEGMENT_REQUEST = (3 << 5);
|
||||
const uint8_t UPLOAD_SEGMENT_RESPONSE = (0 << 5);
|
||||
const uint8_t ABORT_TRANSFER_REQUEST = (4 << 5);
|
||||
|
||||
|
||||
#pragma pack(push) /* push current alignment to stack */
|
||||
#pragma pack(1) /* set alignment to 1 byte boundary */
|
||||
|
||||
struct SDOid{
|
||||
uint32_t id:29;
|
||||
uint32_t extended:1;
|
||||
uint32_t dynamic:1;
|
||||
uint32_t invalid:1;
|
||||
SDOid(uint32_t val){
|
||||
*(uint32_t*) this = val;
|
||||
}
|
||||
can::Header header() {
|
||||
return can::Header(id, extended, false, false);
|
||||
}
|
||||
};
|
||||
|
||||
struct InitiateShort{
|
||||
uint8_t :5;
|
||||
uint8_t command:3;
|
||||
uint16_t index;
|
||||
uint8_t sub_index;
|
||||
uint8_t reserved[4];
|
||||
};
|
||||
|
||||
struct InitiateLong{
|
||||
uint8_t size_indicated:1;
|
||||
uint8_t expedited:1;
|
||||
uint8_t num:2;
|
||||
uint8_t :1;
|
||||
uint8_t command:3;
|
||||
uint16_t index;
|
||||
uint8_t sub_index;
|
||||
uint8_t payload[4];
|
||||
|
||||
size_t data_size(){
|
||||
if(expedited && size_indicated) return 4-num;
|
||||
else if(!expedited && size_indicated) return payload[0] | (payload[3]<<8);
|
||||
else return 0;
|
||||
}
|
||||
size_t apply_buffer(const String &buffer){
|
||||
size_t size = buffer.size();
|
||||
size_indicated = 1;
|
||||
if(size > 4){
|
||||
expedited = 0;
|
||||
payload[0] = size & 0xFF;
|
||||
payload[3] = (size >> 8) & 0xFF;
|
||||
return 0;
|
||||
}else{
|
||||
expedited = 1;
|
||||
size_indicated = 1;
|
||||
num = 4-size;
|
||||
memcpy(payload, buffer.data(), size);
|
||||
return size;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
struct SegmentShort{
|
||||
uint8_t :4;
|
||||
uint8_t toggle:1;
|
||||
uint8_t command:3;
|
||||
uint8_t reserved[7];
|
||||
};
|
||||
|
||||
struct SegmentLong{
|
||||
uint8_t done:1;
|
||||
uint8_t num:3;
|
||||
uint8_t toggle:1;
|
||||
uint8_t command:3;
|
||||
uint8_t payload[7];
|
||||
size_t data_size(){
|
||||
return 7-num;
|
||||
}
|
||||
size_t apply_buffer(const String &buffer, const size_t offset){
|
||||
size_t size = buffer.size() - offset;
|
||||
if(size > 7) size = 7;
|
||||
else done = 1;
|
||||
num = 7 - size;
|
||||
memcpy(payload, buffer.data() + offset, size);
|
||||
return offset + size;
|
||||
}
|
||||
};
|
||||
|
||||
struct DownloadInitiateRequest: public FrameOverlay<InitiateLong>{
|
||||
static const uint8_t command = 1;
|
||||
|
||||
DownloadInitiateRequest(const Header &h, const canopen::ObjectDict::Entry &entry, const String &buffer, size_t &offset) : FrameOverlay(h) {
|
||||
data.command = command;
|
||||
data.index = entry.index;
|
||||
data.sub_index = entry.sub_index;
|
||||
offset = data.apply_buffer(buffer);
|
||||
}
|
||||
DownloadInitiateRequest(const can::Frame &f) : FrameOverlay(f){ }
|
||||
};
|
||||
|
||||
struct DownloadInitiateResponse: public FrameOverlay<InitiateShort>{
|
||||
static const uint8_t command = 3;
|
||||
|
||||
DownloadInitiateResponse(const can::Frame &f) : FrameOverlay(f){ }
|
||||
|
||||
bool test(const can::Frame &msg, uint32_t &reason){
|
||||
DownloadInitiateRequest req(msg);
|
||||
if(req.data.command == DownloadInitiateRequest::command && data.index == req.data.index && data.sub_index == req.data.sub_index){
|
||||
return true;
|
||||
}
|
||||
reason = 0x08000000; // General error
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
struct DownloadSegmentRequest: public FrameOverlay<SegmentLong>{
|
||||
static const uint8_t command = 0;
|
||||
|
||||
DownloadSegmentRequest(const can::Frame &f) : FrameOverlay(f){ }
|
||||
|
||||
DownloadSegmentRequest(const Header &h, bool toggle, const String &buffer, size_t& offset) : FrameOverlay(h) {
|
||||
data.command = command;
|
||||
data.toggle = toggle?1:0;
|
||||
offset = data.apply_buffer(buffer, offset);
|
||||
}
|
||||
};
|
||||
|
||||
struct DownloadSegmentResponse : public FrameOverlay<SegmentShort>{
|
||||
static const uint8_t command = 1;
|
||||
DownloadSegmentResponse(const can::Frame &f) : FrameOverlay(f) {
|
||||
}
|
||||
bool test(const can::Frame &msg, uint32_t &reason){
|
||||
DownloadSegmentRequest req(msg);
|
||||
if (req.data.command != DownloadSegmentRequest::command){
|
||||
reason = 0x08000000; // General error
|
||||
return false;
|
||||
}else if( data.toggle != req.data.toggle){
|
||||
reason = 0x05030000; // Toggle bit not alternated
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
struct UploadInitiateRequest: public FrameOverlay<InitiateShort>{
|
||||
static const uint8_t command = 2;
|
||||
UploadInitiateRequest(const Header &h, const canopen::ObjectDict::Entry &entry) : FrameOverlay(h) {
|
||||
data.command = command;
|
||||
data.index = entry.index;
|
||||
data.sub_index = entry.sub_index;
|
||||
}
|
||||
UploadInitiateRequest(const can::Frame &f) : FrameOverlay(f){ }
|
||||
};
|
||||
|
||||
struct UploadInitiateResponse: public FrameOverlay<InitiateLong>{
|
||||
static const uint8_t command = 2;
|
||||
UploadInitiateResponse(const can::Frame &f) : FrameOverlay(f) { }
|
||||
bool test(const can::Frame &msg, size_t size, uint32_t &reason){
|
||||
UploadInitiateRequest req(msg);
|
||||
if(req.data.command == UploadInitiateRequest::command && data.index == req.data.index && data.sub_index == req.data.sub_index){
|
||||
size_t ds = data.data_size();
|
||||
if(ds == 0 || size == 0 || ds >= size) { // should be ==, but >= is needed for Elmo, it responses with more byte than requested
|
||||
if(!data.expedited || (ds <= 4 && size <= 4)) return true;
|
||||
}else{
|
||||
reason = 0x06070010; // Data type does not match, length of service parameter does not match
|
||||
return false;
|
||||
}
|
||||
}
|
||||
reason = 0x08000000; // General error
|
||||
return false;
|
||||
}
|
||||
bool read_data(String & buffer, size_t & offset, size_t & total){
|
||||
if(data.size_indicated && total == 0){
|
||||
total = data.data_size();
|
||||
buffer.resize(total);
|
||||
}
|
||||
if(data.expedited){
|
||||
memcpy(&buffer.front(), data.payload, buffer.size());
|
||||
offset = buffer.size();
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
};
|
||||
struct UploadSegmentRequest: public FrameOverlay<SegmentShort>{
|
||||
static const uint8_t command = 3;
|
||||
UploadSegmentRequest(const Header &h, bool toggle) : FrameOverlay(h) {
|
||||
data.command = command;
|
||||
data.toggle = toggle?1:0;
|
||||
}
|
||||
UploadSegmentRequest(const can::Frame &f) : FrameOverlay(f) { }
|
||||
};
|
||||
|
||||
struct UploadSegmentResponse : public FrameOverlay<SegmentLong>{
|
||||
static const uint8_t command = 0;
|
||||
UploadSegmentResponse(const can::Frame &f) : FrameOverlay(f) {
|
||||
}
|
||||
bool test(const can::Frame &msg, uint32_t &reason){
|
||||
UploadSegmentRequest req(msg);
|
||||
if(req.data.command != UploadSegmentRequest::command){
|
||||
reason = 0x08000000; // General error
|
||||
return false;
|
||||
}else if( data.toggle != req.data.toggle){
|
||||
reason = 0x05030000; // Toggle bit not alternated
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
bool read_data(String & buffer, size_t & offset, const size_t & total){
|
||||
uint32_t n = data.data_size();
|
||||
if(total == 0){
|
||||
buffer.resize(offset + n);
|
||||
}
|
||||
if(offset + n <= buffer.size()){
|
||||
memcpy(&buffer[offset], data.payload, n);
|
||||
offset += n;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
struct AbortData{
|
||||
uint8_t :5;
|
||||
uint8_t command:3;
|
||||
uint16_t index;
|
||||
uint8_t sub_index;
|
||||
uint32_t reason;
|
||||
|
||||
const char * text(){
|
||||
switch(reason){
|
||||
case 0x05030000: return "Toggle bit not alternated.";
|
||||
case 0x05040000: return "SDO protocol timed out.";
|
||||
case 0x05040001: return "Client/server command specifier not valid or unknown.";
|
||||
case 0x05040002: return "Invalid block size (block mode only).";
|
||||
case 0x05040003: return "Invalid sequence number (block mode only).";
|
||||
case 0x05040004: return "CRC error (block mode only).";
|
||||
case 0x05040005: return "Out of memory.";
|
||||
case 0x06010000: return "Unsupported access to an object.";
|
||||
case 0x06010001: return "Attempt to read a write only object.";
|
||||
case 0x06010002: return "Attempt to write a read only object.";
|
||||
case 0x06020000: return "Object does not exist in the object dictionary.";
|
||||
case 0x06040041: return "Object cannot be mapped to the PDO.";
|
||||
case 0x06040042: return "The number and length of the objects to be mapped would exceed PDO length.";
|
||||
case 0x06040043: return "General parameter incompatibility reason.";
|
||||
case 0x06040047: return "General internal incompatibility in the device.";
|
||||
case 0x06060000: return "Access failed due to an hardware error.";
|
||||
case 0x06070010: return "Data type does not match, length of service parameter does not match";
|
||||
case 0x06070012: return "Data type does not match, length of service parameter too high";
|
||||
case 0x06070013: return "Data type does not match, length of service parameter too low";
|
||||
case 0x06090011: return "Sub-index does not exist.";
|
||||
case 0x06090030: return "Invalid value for parameter (download only).";
|
||||
case 0x06090031: return "Value of parameter written too high (download only).";
|
||||
case 0x06090032: return "Value of parameter written too low (download only).";
|
||||
case 0x06090036: return "Maximum value is less than minimum value.";
|
||||
case 0x060A0023: return "Resource not available: SDO connection";
|
||||
case 0x08000000: return "General error";
|
||||
case 0x08000020: return "Data cannot be transferred or stored to the application.";
|
||||
case 0x08000021: return "Data cannot be transferred or stored to the application because of local control.";
|
||||
case 0x08000022: return "Data cannot be transferred or stored to the application because of the present device state.";
|
||||
case 0x08000023: return "Object dictionary dynamic generation fails or no object dictionary is present (e.g.object dictionary is generated from file and generation fails because of an file error).";
|
||||
case 0x08000024: return "No data available";
|
||||
default: return "Abort code is reserved";
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
struct AbortTranserRequest: public FrameOverlay<AbortData>{
|
||||
static const uint8_t command = 4;
|
||||
AbortTranserRequest(const can::Frame &f) : FrameOverlay(f) {}
|
||||
AbortTranserRequest(const Header &h, uint16_t index, uint8_t sub_index, uint32_t reason) : FrameOverlay(h) {
|
||||
data.command = command;
|
||||
data.index = index;
|
||||
data.sub_index = sub_index;
|
||||
data.reason = reason;
|
||||
}
|
||||
};
|
||||
|
||||
#pragma pack(pop) /* pop previous alignment from stack */
|
||||
|
||||
void SDOClient::abort(uint32_t reason){
|
||||
if(current_entry){
|
||||
interface_->send(last_msg = AbortTranserRequest(client_id, current_entry->index, current_entry->sub_index, reason));
|
||||
}
|
||||
}
|
||||
|
||||
bool SDOClient::processFrame(const can::Frame & msg){
|
||||
if(msg.dlc != 8) return false;
|
||||
|
||||
uint32_t reason = 0;
|
||||
switch(msg.data[0] >> 5){
|
||||
case DownloadInitiateResponse::command:
|
||||
{
|
||||
DownloadInitiateResponse resp(msg);
|
||||
if(resp.test(last_msg, reason) ){
|
||||
if(offset < total){
|
||||
interface_->send(last_msg = DownloadSegmentRequest(client_id, false, buffer, offset));
|
||||
}else{
|
||||
done = true;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
case DownloadSegmentResponse::command:
|
||||
{
|
||||
DownloadSegmentResponse resp(msg);
|
||||
if( resp.test(last_msg, reason) ){
|
||||
if(offset < total){
|
||||
interface_->send(last_msg = DownloadSegmentRequest(client_id, !resp.data.toggle, buffer, offset));
|
||||
}else{
|
||||
done = true;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case UploadInitiateResponse::command:
|
||||
{
|
||||
UploadInitiateResponse resp(msg);
|
||||
if( resp.test(last_msg, total, reason) ){
|
||||
if(resp.read_data(buffer, offset, total)){
|
||||
done = true;
|
||||
}else{
|
||||
interface_->send(last_msg = UploadSegmentRequest(client_id, false));
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
case UploadSegmentResponse::command:
|
||||
{
|
||||
UploadSegmentResponse resp(msg);
|
||||
if( resp.test(last_msg, reason) ){
|
||||
if(resp.read_data(buffer, offset, total)){
|
||||
if(resp.data.done || offset == total){
|
||||
done = true;
|
||||
}else{
|
||||
interface_->send(last_msg = UploadSegmentRequest(client_id, !resp.data.toggle));
|
||||
}
|
||||
}else{
|
||||
// abort, size mismatch
|
||||
ROSCANOPEN_ERROR("canopen_master", "abort, size mismatch" << buffer.size() << " " << resp.data.data_size());
|
||||
reason = 0x06070010; // Data type does not match, length of service parameter does not match
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
case AbortTranserRequest::command:
|
||||
ROSCANOPEN_ERROR("canopen_master", "abort" << std::hex << (uint32_t) AbortTranserRequest(msg).data.index << "#"<< std::dec << (uint32_t) AbortTranserRequest(msg).data.sub_index << ", reason: " << AbortTranserRequest(msg).data.text());
|
||||
offset = 0;
|
||||
return false;
|
||||
break;
|
||||
}
|
||||
if(reason){
|
||||
abort(reason);
|
||||
offset = 0;
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
|
||||
}
|
||||
|
||||
void SDOClient::init(){
|
||||
assert(storage_);
|
||||
assert(interface_);
|
||||
const canopen::ObjectDict & dict = *storage_->dict_;
|
||||
|
||||
try{
|
||||
client_id = SDOid(NodeIdOffset<uint32_t>::apply(dict(0x1200, 1).value(), storage_->node_id_)).header();
|
||||
}
|
||||
catch(...){
|
||||
client_id = can::MsgHeader(0x600+ storage_->node_id_);
|
||||
}
|
||||
|
||||
last_msg = AbortTranserRequest(client_id, 0,0,0);
|
||||
current_entry = 0;
|
||||
|
||||
can::Header server_id;
|
||||
try{
|
||||
server_id = SDOid(NodeIdOffset<uint32_t>::apply(dict(0x1200, 2).value(), storage_->node_id_)).header();
|
||||
}
|
||||
catch(...){
|
||||
server_id = can::MsgHeader(0x580+ storage_->node_id_);
|
||||
}
|
||||
reader_.listen(interface_, server_id);
|
||||
}
|
||||
|
||||
void SDOClient::transmitAndWait(const canopen::ObjectDict::Entry &entry, const String &data, String *result){
|
||||
buffer = data;
|
||||
offset = 0;
|
||||
total = buffer.size();
|
||||
current_entry = &entry;
|
||||
done = false;
|
||||
|
||||
can::BufferedReader::ScopedEnabler enabler(reader_);
|
||||
|
||||
if(result){
|
||||
interface_->send(last_msg = UploadInitiateRequest(client_id, entry));
|
||||
}else{
|
||||
interface_->send(last_msg = DownloadInitiateRequest(client_id, entry, buffer, offset));
|
||||
}
|
||||
|
||||
boost::this_thread::disable_interruption di;
|
||||
can::Frame msg;
|
||||
|
||||
while(!done){
|
||||
if(!reader_.read(&msg,boost::chrono::seconds(1)))
|
||||
{
|
||||
abort(0x05040000); // SDO protocol timed out.
|
||||
ROSCANOPEN_ERROR("canopen_master", "Did not receive a response message");
|
||||
break;
|
||||
}
|
||||
if(!processFrame(msg)){
|
||||
ROSCANOPEN_ERROR("canopen_master", "Could not process message");
|
||||
break;
|
||||
}
|
||||
}
|
||||
if(offset == 0 || offset != total){
|
||||
THROW_WITH_KEY(TimeoutException("SDO"), ObjectDict::Key(*current_entry));
|
||||
}
|
||||
|
||||
if(result) *result=buffer;
|
||||
|
||||
}
|
||||
|
||||
void SDOClient::read(const canopen::ObjectDict::Entry &entry, String &data){
|
||||
boost::timed_mutex::scoped_lock lock(mutex, boost::chrono::seconds(2));
|
||||
if(lock){
|
||||
transmitAndWait(entry, data, &data);
|
||||
}else{
|
||||
THROW_WITH_KEY(TimeoutException("SDO read"), ObjectDict::Key(entry));
|
||||
}
|
||||
}
|
||||
void SDOClient::write(const canopen::ObjectDict::Entry &entry, const String &data){
|
||||
boost::timed_mutex::scoped_lock lock(mutex, boost::chrono::seconds(2));
|
||||
if(lock){
|
||||
transmitAndWait(entry, data, 0);
|
||||
}else{
|
||||
THROW_WITH_KEY(TimeoutException("SDO write"), ObjectDict::Key(entry));
|
||||
}
|
||||
}
|
||||
65
Devices/Libraries/Ros/ros_canopen/canopen_master/test/test_node.cpp
Executable file
65
Devices/Libraries/Ros/ros_canopen/canopen_master/test/test_node.cpp
Executable file
@@ -0,0 +1,65 @@
|
||||
#include <socketcan_interface/dummy.h>
|
||||
#include <canopen_master/canopen.h>
|
||||
|
||||
// Bring in gtest
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
canopen::ObjectDictSharedPtr make_dict(){
|
||||
canopen::DeviceInfo info;
|
||||
info.nr_of_rx_pdo = 0;
|
||||
info.nr_of_tx_pdo = 0;
|
||||
|
||||
canopen::ObjectDictSharedPtr dict = std::make_shared<canopen::ObjectDict>(info);
|
||||
dict->insert(false, std::make_shared<const canopen::ObjectDict::Entry>(canopen::ObjectDict::VAR,
|
||||
0x1017,
|
||||
canopen::ObjectDict::DEFTYPE_UNSIGNED16,
|
||||
"producer heartbeat",
|
||||
true, true, false,
|
||||
canopen::HoldAny((uint16_t)0),
|
||||
canopen::HoldAny((uint16_t)100)
|
||||
));
|
||||
return dict;
|
||||
}
|
||||
TEST(TestNode, testInitandShutdown){
|
||||
|
||||
can::DummyBus bus("testInitandShutdown");
|
||||
|
||||
can::ThreadedDummyInterfaceSharedPtr driver = std::make_shared<can::ThreadedDummyInterface>();
|
||||
|
||||
can::DummyReplay replay;
|
||||
|
||||
replay.add("0#8201", "701#00");
|
||||
replay.add("601#2b17100064000000", "581#6017100000000000");
|
||||
replay.add("0#0101", "701#05");
|
||||
replay.add("601#2b17100000000000", "581#6017100000000000");
|
||||
replay.init(bus);
|
||||
|
||||
EXPECT_FALSE(replay.done());
|
||||
|
||||
auto settings = can::SettingsMap::create();
|
||||
settings->set("trace", true);
|
||||
|
||||
driver->init(bus.name, false, settings);
|
||||
|
||||
canopen::NodeSharedPtr node = std::make_shared<canopen::Node>(driver, make_dict(), 1);
|
||||
|
||||
{
|
||||
canopen::LayerStatus status;
|
||||
node->init(status);
|
||||
ASSERT_TRUE(status.bounded<canopen::LayerStatus::Ok>());
|
||||
ASSERT_EQ(canopen::Node::Operational, node->getState());
|
||||
}
|
||||
|
||||
{
|
||||
canopen::LayerStatus status;
|
||||
node->shutdown(status);
|
||||
ASSERT_TRUE(status.bounded<canopen::LayerStatus::Ok>());
|
||||
}
|
||||
EXPECT_TRUE(replay.done());
|
||||
}
|
||||
|
||||
// Run all the tests that were declared with TEST()
|
||||
int main(int argc, char **argv){
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
182
Devices/Libraries/Ros/ros_canopen/canopen_master/test/test_parser.cpp
Executable file
182
Devices/Libraries/Ros/ros_canopen/canopen_master/test/test_parser.cpp
Executable file
@@ -0,0 +1,182 @@
|
||||
// Bring in my package's API, which is what I'm testing
|
||||
#include <boost/property_tree/ptree.hpp>
|
||||
#include <canopen_master/objdict.h>
|
||||
|
||||
// Bring in gtest
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
|
||||
template<typename T> canopen::HoldAny parse_int(boost::property_tree::iptree &pt, const std::string &key);
|
||||
|
||||
template<typename T> canopen::HoldAny prepare_test(const std::string &str){
|
||||
boost::property_tree::iptree pt;
|
||||
pt.put("test", str);
|
||||
return parse_int<T>(pt, "test");
|
||||
}
|
||||
|
||||
template<typename T> class TestHexTypes : public ::testing::Test{
|
||||
public:
|
||||
static void test_hex(const T& val, const std::string &str){
|
||||
EXPECT_EQ(val, prepare_test<T>(str).template get<T>());
|
||||
}
|
||||
static void test_hex_node(const T& val, const std::string &str, const uint8_t offset){
|
||||
EXPECT_EQ(val, canopen::NodeIdOffset<T>::apply(prepare_test<T>(str),offset));
|
||||
}
|
||||
};
|
||||
|
||||
typedef ::testing::Types<uint8_t, uint16_t, uint32_t, uint64_t> PosTypes;
|
||||
|
||||
TYPED_TEST_CASE(TestHexTypes, PosTypes);
|
||||
|
||||
TYPED_TEST(TestHexTypes, checkZero){
|
||||
TestFixture::test_hex(0,"0");
|
||||
TestFixture::test_hex(0,"0x0");
|
||||
|
||||
for(uint8_t i = 0; i <=127; ++i){
|
||||
TestFixture::test_hex_node(0,"0",i);
|
||||
TestFixture::test_hex_node(0,"0x0",i);
|
||||
TestFixture::test_hex_node(0+i,"$NODEID+0",i);
|
||||
TestFixture::test_hex_node(0+i,"$NODEID+0x0",i);
|
||||
TestFixture::test_hex_node(0+i,"0+$NODEID",i);
|
||||
TestFixture::test_hex_node(0+i,"0x0+$NODEID",i);
|
||||
}
|
||||
}
|
||||
TEST(TestHex, checkCamelCase){
|
||||
TestHexTypes<uint16_t>::test_hex(0xABCD, "0xABCD");
|
||||
TestHexTypes<uint16_t>::test_hex(0xABCD, "0xabcd");
|
||||
TestHexTypes<uint16_t>::test_hex(0xABCD, "0xAbCd");
|
||||
TestHexTypes<uint16_t>::test_hex(0xABCD, "0xabCD");
|
||||
}
|
||||
|
||||
TEST(TestHex, checkNodeCamelCase){
|
||||
for(uint8_t i = 0; i <=127; ++i){
|
||||
TestHexTypes<uint16_t>::test_hex_node(i," $NODEID",i);
|
||||
TestHexTypes<uint16_t>::test_hex_node(i," $NODeID",i);
|
||||
TestHexTypes<uint16_t>::test_hex_node(i," $NodeId",i);
|
||||
TestHexTypes<uint16_t>::test_hex_node(i," $NodeID",i);
|
||||
TestHexTypes<uint16_t>::test_hex_node(i," $nodeID",i);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(TestHex, checkSpaces){
|
||||
TestHexTypes<uint16_t>::test_hex(0xABCD, " 0xABCD ");
|
||||
TestHexTypes<uint16_t>::test_hex(0xABCD, "0xABCD ");
|
||||
TestHexTypes<uint16_t>::test_hex(0xABCD, " 0xABCD");
|
||||
}
|
||||
|
||||
TEST(TestHex, checkNodeSpaces){
|
||||
for(uint8_t i = 0; i <=127; ++i){
|
||||
TestHexTypes<uint16_t>::test_hex_node(i," $NODEID ",i);
|
||||
TestHexTypes<uint16_t>::test_hex_node(i," $NODEID",i);
|
||||
TestHexTypes<uint16_t>::test_hex_node(i,"$NODEID ",i);
|
||||
TestHexTypes<uint16_t>::test_hex_node(i+1,"$NODEID + 1",i);
|
||||
TestHexTypes<uint16_t>::test_hex_node(i+1,"$NODEID+ 1",i);
|
||||
TestHexTypes<uint16_t>::test_hex_node(i+1,"$NODEID+1",i);
|
||||
TestHexTypes<uint16_t>::test_hex_node(i+1,"$NODEID +1",i);
|
||||
TestHexTypes<uint16_t>::test_hex_node(i+1,"$NODEID +0x1 ",i);
|
||||
TestHexTypes<uint16_t>::test_hex_node(i+1,"$NODEID + 0x1",i);
|
||||
|
||||
TestHexTypes<uint16_t>::test_hex_node(i+1,"1 + $NODEID",i);
|
||||
TestHexTypes<uint16_t>::test_hex_node(i+1,"1+ $NODEID",i);
|
||||
TestHexTypes<uint16_t>::test_hex_node(i+1,"1+$NODEID",i);
|
||||
TestHexTypes<uint16_t>::test_hex_node(i+1,"1 +$NODEID",i);
|
||||
TestHexTypes<uint16_t>::test_hex_node(i+1,"0x1 + $NODEID ",i);
|
||||
TestHexTypes<uint16_t>::test_hex_node(i+1,"0x1 +$NODEID",i);
|
||||
}
|
||||
}
|
||||
TEST(TestHex, checkCommonObjects){
|
||||
for(uint8_t i = 0; i <=127; ++i){
|
||||
TestHexTypes<uint32_t>::test_hex_node( 0x80+i,"$NODEID+0x80",i); // EMCY
|
||||
|
||||
TestHexTypes<uint32_t>::test_hex_node(0x180+i,"$NODEID+0x180",i); //TPDO1
|
||||
TestHexTypes<uint32_t>::test_hex_node(0x200+i,"$NODEID+0x200",i); //RPDO1
|
||||
|
||||
TestHexTypes<uint32_t>::test_hex_node(0x280+i,"$NODEID+0x280",i); //TPDO2
|
||||
TestHexTypes<uint32_t>::test_hex_node(0x300+i,"$NODEID+0x300",i); //RPDO2
|
||||
|
||||
TestHexTypes<uint32_t>::test_hex_node(0x380+i,"$NODEID+0x380",i); //TPDO3
|
||||
TestHexTypes<uint32_t>::test_hex_node(0x400+i,"$NODEID+0x400",i); //RPDO3
|
||||
|
||||
TestHexTypes<uint32_t>::test_hex_node(0x480+i,"$NODEID+0x480",i); //TPDO4
|
||||
TestHexTypes<uint32_t>::test_hex_node(0x500+i,"$NODEID+0x500",i); //RPDO4
|
||||
|
||||
TestHexTypes<uint32_t>::test_hex_node(0x580+i,"$NODEID+0x580",i); // TSDO
|
||||
TestHexTypes<uint32_t>::test_hex_node(0x600+i,"$NODEID+0x600",i); // RSDO
|
||||
|
||||
TestHexTypes<uint32_t>::test_hex_node(0x700+i,"$NODEID+0x700",i); //NMT
|
||||
|
||||
TestHexTypes<uint32_t>::test_hex_node( 0x80+i,"0x80+$NODEID",i); // EMCY
|
||||
|
||||
TestHexTypes<uint32_t>::test_hex_node(0x180+i,"0x180+$NODEID",i); //TPDO1
|
||||
TestHexTypes<uint32_t>::test_hex_node(0x200+i,"0x200+$NODEID",i); //RPDO1
|
||||
|
||||
TestHexTypes<uint32_t>::test_hex_node(0x280+i,"0x280+$NODEID",i); //TPDO2
|
||||
TestHexTypes<uint32_t>::test_hex_node(0x300+i,"0x300+$NODEID",i); //RPDO2
|
||||
TestHexTypes<uint32_t>::test_hex_node(0x380+i,"0x380+$NODEID",i); //TPDO3
|
||||
TestHexTypes<uint32_t>::test_hex_node(0x400+i,"0x400+$NODEID",i); //RPDO3
|
||||
|
||||
TestHexTypes<uint32_t>::test_hex_node(0x480+i,"0x480+$NODEID",i); //TPDO4
|
||||
TestHexTypes<uint32_t>::test_hex_node(0x500+i,"0x500+$NODEID",i); //RPDO4
|
||||
|
||||
TestHexTypes<uint32_t>::test_hex_node(0x580+i,"0x580+$NODEID",i); // TSDO
|
||||
TestHexTypes<uint32_t>::test_hex_node(0x600+i,"0x600+$NODEID",i); // RSDO
|
||||
|
||||
TestHexTypes<uint32_t>::test_hex_node(0x700+i,"0x700+$NODEID",i); //NMT
|
||||
}
|
||||
}
|
||||
|
||||
void set_access( canopen::ObjectDict::Entry &entry, std::string access);
|
||||
|
||||
void testAccess(bool c, bool r, bool w, const char* variants[]){
|
||||
canopen::ObjectDict::Entry entry;
|
||||
for(const char ** v = variants; *v; ++v){
|
||||
SCOPED_TRACE(*v);
|
||||
entry.constant = !c;
|
||||
entry.readable = !r;
|
||||
entry.writable = !w;
|
||||
|
||||
set_access(entry, *v);
|
||||
|
||||
ASSERT_EQ(c, entry.constant);
|
||||
ASSERT_EQ(r, entry.readable);
|
||||
ASSERT_EQ(w, entry.writable);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(TestAccessString, TestRO)
|
||||
{
|
||||
const char* variants[] = {"ro", "Ro", "rO", "RO", 0};
|
||||
testAccess(false, true, false, variants);
|
||||
}
|
||||
|
||||
TEST(TestAccessString, TestWO)
|
||||
{
|
||||
const char* variants[] = {"wo", "Wo", "wO", "WO", 0};
|
||||
testAccess(false, false, true, variants);
|
||||
}
|
||||
|
||||
TEST(TestAccessString, TestRW)
|
||||
{
|
||||
const char* variants[] = {
|
||||
"rw" , "Rw" , "rW" , "Rw",
|
||||
"rwr", "Rwr", "rWr", "Rwr",
|
||||
"rwR", "RwR", "rWR", "RwR",
|
||||
"rww", "Rww", "rWw", "Rww",
|
||||
"rwW", "RwW", "rWW", "RwW",
|
||||
0};
|
||||
testAccess(false, true, true, variants);
|
||||
}
|
||||
|
||||
TEST(TestAccessString, TestConst)
|
||||
{
|
||||
const char* variants[] = {
|
||||
"const" , "Const" , "CONST", 0};
|
||||
testAccess(true, true, false, variants);
|
||||
}
|
||||
|
||||
|
||||
// Run all the tests that were declared with TEST()
|
||||
int main(int argc, char **argv){
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
Reference in New Issue
Block a user