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

git commit -m "first commit for v2"

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

34
Devices/Libraries/Ros/ros_canopen/.github/workflows/main.yml vendored Executable file
View File

@@ -0,0 +1,34 @@
# This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git).
# For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst)
name: CI
on:
push:
pull_request:
schedule:
- cron: '0 4 * * *' # every day at 4 AM (UTC)
workflow_dispatch: # allow manually starting this workflow
jobs:
industrial_ci:
name: ROS ${{ matrix.ROS_DISTRO }} (${{ matrix.ROS_REPO }})
runs-on: ubuntu-latest
strategy:
matrix: # matrix is the product of entries
ROS_DISTRO: [melodic, noetic]
ROS_REPO: [testing, main]
env:
CCACHE_DIR: "${{ github.workspace }}/.ccache" # directory for ccache (and how we enable ccache in industrial_ci)
steps:
- uses: actions/checkout@v2 # clone target repository
- uses: actions/cache@v2 # fetch/store the directory used by ccache before/after the ci run
with:
path: ${{ env.CCACHE_DIR }}
key: ccache-${{ matrix.ROS_DISTRO }}-${{ matrix.ROS_REPO }}-${{github.run_id}}
restore-keys: |
ccache-${{ matrix.ROS_DISTRO }}-${{ matrix.ROS_REPO }}-
- uses: 'ros-industrial/industrial_ci@master' # run industrial_ci
env:
ROS_DISTRO: ${{ matrix.ROS_DISTRO }}
ROS_REPO: ${{ matrix.ROS_REPO }}

20
Devices/Libraries/Ros/ros_canopen/.github/workflows/prerelease.yml vendored Executable file
View File

@@ -0,0 +1,20 @@
name: Pre-release
on: [workflow_dispatch]
jobs:
default:
strategy:
matrix:
distro: [melodic, noetic]
env:
BUILDER: catkin_make_isolated
ROS_DISTRO: ${{ matrix.distro }}
PRERELEASE: true
name: "${{ matrix.distro }}"
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v2
- uses: ros-industrial/industrial_ci@master

440
Devices/Libraries/Ros/ros_canopen/.gitignore vendored Executable file
View File

@@ -0,0 +1,440 @@
# ---> VisualStudio
## Ignore Visual Studio temporary files, build results, and
## files generated by popular Visual Studio add-ons.
##
## Get latest from https://github.com/github/gitignore/blob/main/VisualStudio.gitignore
# User-specific files
*.rsuser
*.suo
*.user
*.userosscache
*.sln.docstates
# User-specific files (MonoDevelop/Xamarin Studio)
*.userprefs
# Mono auto generated files
mono_crash.*
# Build results
[Dd]ebug/
[Dd]ebugPublic/
[Rr]elease/
[Rr]eleases/
x64/
x86/
[Ww][Ii][Nn]32/
[Aa][Rr][Mm]/
[Aa][Rr][Mm]64/
bld/
[Bb]in/
[Oo]bj/
[Ll]og/
[Ll]ogs/
# Visual Studio 2015/2017 cache/options directory
.vs/
# Uncomment if you have tasks that create the project's static files in wwwroot
#wwwroot/
# Visual Studio 2017 auto generated files
Generated\ Files/
# MSTest test Results
[Tt]est[Rr]esult*/
[Bb]uild[Ll]og.*
# NUnit
*.VisualState.xml
TestResult.xml
nunit-*.xml
# Build Results of an ATL Project
[Dd]ebugPS/
[Rr]eleasePS/
dlldata.c
# Benchmark Results
BenchmarkDotNet.Artifacts/
# .NET Core
project.lock.json
project.fragment.lock.json
artifacts/
# ASP.NET Scaffolding
ScaffoldingReadMe.txt
# StyleCop
StyleCopReport.xml
# Files built by Visual Studio
*_i.c
*_p.c
*_h.h
*.ilk
*.meta
*.obj
*.iobj
*.pch
*.pdb
*.ipdb
*.pgc
*.pgd
*.rsp
*.sbr
*.tlb
*.tli
*.tlh
*.tmp
*.tmp_proj
*_wpftmp.csproj
*.log
*.tlog
*.vspscc
*.vssscc
.builds
*.pidb
*.svclog
*.scc
# Chutzpah Test files
_Chutzpah*
# Visual C++ cache files
ipch/
*.aps
*.ncb
*.opendb
*.opensdf
*.sdf
*.cachefile
*.VC.db
*.VC.VC.opendb
# Visual Studio profiler
*.psess
*.vsp
*.vspx
*.sap
# Visual Studio Trace Files
*.e2e
# TFS 2012 Local Workspace
$tf/
# Guidance Automation Toolkit
*.gpState
# ReSharper is a .NET coding add-in
_ReSharper*/
*.[Rr]e[Ss]harper
*.DotSettings.user
# TeamCity is a build add-in
_TeamCity*
# DotCover is a Code Coverage Tool
*.dotCover
# AxoCover is a Code Coverage Tool
.axoCover/*
!.axoCover/settings.json
# Coverlet is a free, cross platform Code Coverage Tool
coverage*.json
coverage*.xml
coverage*.info
# Visual Studio code coverage results
*.coverage
*.coveragexml
# NCrunch
_NCrunch_*
.*crunch*.local.xml
nCrunchTemp_*
# MightyMoose
*.mm.*
AutoTest.Net/
# Web workbench (sass)
.sass-cache/
# Installshield output folder
[Ee]xpress/
# DocProject is a documentation generator add-in
DocProject/buildhelp/
DocProject/Help/*.HxT
DocProject/Help/*.HxC
DocProject/Help/*.hhc
DocProject/Help/*.hhk
DocProject/Help/*.hhp
DocProject/Help/Html2
DocProject/Help/html
# Click-Once directory
publish/
# Publish Web Output
*.[Pp]ublish.xml
*.azurePubxml
# Note: Comment the next line if you want to checkin your web deploy settings,
# but database connection strings (with potential passwords) will be unencrypted
*.pubxml
*.publishproj
# Microsoft Azure Web App publish settings. Comment the next line if you want to
# checkin your Azure Web App publish settings, but sensitive information contained
# in these scripts will be unencrypted
PublishScripts/
# NuGet Packages
*.nupkg
# NuGet Symbol Packages
*.snupkg
# The packages folder can be ignored because of Package Restore
**/[Pp]ackages/*
# except build/, which is used as an MSBuild target.
!**/[Pp]ackages/build/
# Uncomment if necessary however generally it will be regenerated when needed
#!**/[Pp]ackages/repositories.config
# NuGet v3's project.json files produces more ignorable files
*.nuget.props
*.nuget.targets
# Microsoft Azure Build Output
csx/
*.build.csdef
# Microsoft Azure Emulator
ecf/
rcf/
# Windows Store app package directories and files
AppPackages/
BundleArtifacts/
Package.StoreAssociation.xml
_pkginfo.txt
*.appx
*.appxbundle
*.appxupload
# Visual Studio cache files
# files ending in .cache can be ignored
*.[Cc]ache
# but keep track of directories ending in .cache
!?*.[Cc]ache/
# Others
ClientBin/
~$*
*~
*.dbmdl
*.dbproj.schemaview
*.jfm
*.pfx
*.publishsettings
orleans.codegen.cs
# Including strong name files can present a security risk
# (https://github.com/github/gitignore/pull/2483#issue-259490424)
#*.snk
# Since there are multiple workflows, uncomment next line to ignore bower_components
# (https://github.com/github/gitignore/pull/1529#issuecomment-104372622)
#bower_components/
# RIA/Silverlight projects
Generated_Code/
# Backup & report files from converting an old project file
# to a newer Visual Studio version. Backup files are not needed,
# because we have git ;-)
_UpgradeReport_Files/
Backup*/
UpgradeLog*.XML
UpgradeLog*.htm
ServiceFabricBackup/
*.rptproj.bak
# SQL Server files
*.mdf
*.ldf
*.ndf
# Business Intelligence projects
*.rdl.data
*.bim.layout
*.bim_*.settings
*.rptproj.rsuser
*- [Bb]ackup.rdl
*- [Bb]ackup ([0-9]).rdl
*- [Bb]ackup ([0-9][0-9]).rdl
# Microsoft Fakes
FakesAssemblies/
# GhostDoc plugin setting file
*.GhostDoc.xml
# Node.js Tools for Visual Studio
.ntvs_analysis.dat
node_modules/
# Visual Studio 6 build log
*.plg
# Visual Studio 6 workspace options file
*.opt
# Visual Studio 6 auto-generated workspace file (contains which files were open etc.)
*.vbw
# Visual Studio 6 auto-generated project file (contains which files were open etc.)
*.vbp
# Visual Studio 6 workspace and project file (working project files containing files to include in project)
*.dsw
*.dsp
# Visual Studio 6 technical files
*.ncb
*.aps
# Visual Studio LightSwitch build output
**/*.HTMLClient/GeneratedArtifacts
**/*.DesktopClient/GeneratedArtifacts
**/*.DesktopClient/ModelManifest.xml
**/*.Server/GeneratedArtifacts
**/*.Server/ModelManifest.xml
_Pvt_Extensions
# Paket dependency manager
.paket/paket.exe
paket-files/
# FAKE - F# Make
.fake/
# CodeRush personal settings
.cr/personal
# Python Tools for Visual Studio (PTVS)
__pycache__/
*.pyc
# Cake - Uncomment if you are using it
# tools/**
# !tools/packages.config
# Tabs Studio
*.tss
# Telerik's JustMock configuration file
*.jmconfig
# BizTalk build output
*.btp.cs
*.btm.cs
*.odx.cs
*.xsd.cs
# OpenCover UI analysis results
OpenCover/
# Azure Stream Analytics local run output
ASALocalRun/
# MSBuild Binary and Structured Log
*.binlog
# NVidia Nsight GPU debugger configuration file
*.nvuser
# MFractors (Xamarin productivity tool) working folder
.mfractor/
# Local History for Visual Studio
.localhistory/
# Visual Studio History (VSHistory) files
.vshistory/
# BeatPulse healthcheck temp database
healthchecksdb
# Backup folder for Package Reference Convert tool in Visual Studio 2017
MigrationBackup/
# Ionide (cross platform F# VS Code tools) working folder
.ionide/
# Fody - auto-generated XML schema
FodyWeavers.xsd
# VS Code files for those working on multiple tools
.vscode/*
!.vscode/settings.json
!.vscode/tasks.json
!.vscode/launch.json
!.vscode/extensions.json
*.code-workspace
# Local History for Visual Studio Code
.history/
# Windows Installer files from build outputs
*.cab
*.msi
*.msix
*.msm
*.msp
# JetBrains Rider
*.sln.iml
# ---> VisualStudioCode
.vscode/*
!.vscode/settings.json
!.vscode/tasks.json
!.vscode/launch.json
!.vscode/extensions.json
!.vscode/*.code-snippets
# Local History for Visual Studio Code
.history/
# Built Visual Studio Code Extensions
*.vsix
# temporary files
*~
# autogenerated folders
bin
build
lib
__pycache__
msg_gen
srv_gen
# autogenerated msgs
*Action.msg
*Action.py
*ActionFeedback.msg
*ActionFeedback.py
*ActionGoal.msg
*ActionGoal.py
*Feedback.msg
*Feedback.py
*Goal.msg
*Goal.py
*Result.msg
*Result.py
*.pyc

27
Devices/Libraries/Ros/ros_canopen/.travis.yml Executable file
View File

@@ -0,0 +1,27 @@
language: generic
services:
- docker
cache:
directories:
- $HOME/.ccache
env:
global:
- CCACHE_DIR=$HOME/.ccache
matrix:
- ROS_DISTRO="melodic" ROS_REPO=ros-shadow-fixed
- ROS_DISTRO="melodic" ROS_REPO=ros CATKIN_LINT=pedantic
- ROS_DISTRO="melodic" ROSDEP_SKIP_KEYS=muparser EXPECT_EXIT_CODE=1
- ROS_DISTRO="melodic" ABICHECK_URL='github:ros-industrial/ros_canopen#melodic'
- ROS_DISTRO="noetic"
- ROS_DISTRO="noetic" ROS_REPO=ros OS_NAME=debian OS_CODE_NAME=buster
matrix:
allow_failures:
- env: ROS_DISTRO="melodic" ABICHECK_URL='github:ros-industrial/ros_canopen#melodic'
install:
- git clone --quiet --depth=1 https://github.com/ros-industrial/industrial_ci.git .industrial_ci -b master
script:
- .industrial_ci/travis.sh

21
Devices/Libraries/Ros/ros_canopen/CONTRIBUTING.md Executable file
View File

@@ -0,0 +1,21 @@
ROS-Industrial is a community project. We welcome contributions from any source, from those who are extremely active to casual users. The following sections outline the steps on how to contribute to ROS-Industrial. It assumes there is an existing repository to which one would like to contribute (item 1 in the figure above) and one is familiar with the Git "Fork and Branch" workflow, detailed [here](http://blog.scottlowe.org/2015/01/27/using-fork-branch-git-workflow/).
1. Before any development is undertaken, a contributor would communicate a need and/or issue to the ROS-Industrial community. This can be done by submitting an issue on the appropriate GitHub repo, the [issues repo](https://github.com/ros-industrial/ros_industrial_issues), or by posting a message in the [ROS-Industrial category on ROS Discourse](//swri-ros-pkg-dev@googlegroups.com). . Doing so may save you time if similar development is underway and ensure that whatever approach you take is acceptable to the community of reviewers once it is submitted.
2. The second step (item 2) is to implement your change. If you are working on a code contribution, we highly recommend you utilize the [ROS Qt-Creator Plug-in](http://rosindustrial.org/news/2016/6/9/ros-qt-ide-plugin). Verify that your change successfully builds and passes all tests.
3. Next, push your changes to a "feature" branch in your personal fork of the repo and issue a pull request (PR)(item 3). The PR allows maintainers to review the submitted code. Before the PR can be accepted, the maintainer and contributor must agree that the contribution is implemented appropriately. This process can take several back-and-forth steps (see [example](https://github.com/ros-industrial/motoman/pull/89)). Contributors should expect to spend as much time reviewing/changing the code as on the initial implementation. This time can be minimized by communicating with the ROS-Industrial community before any contribution is made.
4. Issuing a Pull Request (PR) triggers the [Travis Continuous Integrations (CI)](https://github.com/ros-industrial/industrial_ci) step (item 4) which happens automatically in the background. The Travis CI performs several operations, and if any of the steps below fail, then the PR is marked accordingly for the maintainer.
* Travis Workflow:
* Installs a barebones ROS distribution on a fresh Ubuntu virtual machine.
* Creates a catkin workspace and puts the repository in it.
* Uses wstool to check out any from-source dependencies (i.e. other repositories).
* Resolves package dependencies using rosdep (i.e. install packages using apt-get).
* Compiles the catkin workspace.
* Runs all available unit tests.
5. If the PR passes Travis CI and one of the maintainers is satisfied with the changes, they post a +1 as a comment on the PR (item 5). The +1 signifies that the PR is ready to be merged. All PRs require at least one +1 and pass Travis CI before it can be merged.
6. The next step (item 6) is for the PR to be merged into the main branch. This is done through the GitHub web interface by selecting the “Merge pull request” button. After the PR is merged, all status badges are updated automatically.
7. Periodically, the maintainer will release the package (item 7), which then gets sent to the [ROS Build Farm](http://wiki.ros.org/build.ros.org) for Debian creation.
8. The publishing of the released packages (item 8) is managed by OSRF and is not on a set schedule. This usually happens when all packages for a given distro are built successfully and stable. The current status for the distro kinetic can be found [here](http://repositories.ros.org/status_page/ros_kinetic_default.html) . Navigating to other distros can be done by changing the distro name in the link.
9. Once the package has been published, it is available to be installed by the developer (item 9).
10. After the install of a new version, the developer may have questions, experience issues or it may not have the necessary functionality which should all be reported on the packages GitHub repository as an issue (item 10). If an issue is identified or there is missing functionality that the developer requires, the cycle starts back at (item 2).
For more details, please refer to the [ROS-I wiki](http://wiki.ros.org/Industrial/DevProcess).

165
Devices/Libraries/Ros/ros_canopen/LICENSE Executable file
View File

@@ -0,0 +1,165 @@
GNU LESSER GENERAL PUBLIC LICENSE
Version 3, 29 June 2007
Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
Everyone is permitted to copy and distribute verbatim copies
of this license document, but changing it is not allowed.
This version of the GNU Lesser General Public License incorporates
the terms and conditions of version 3 of the GNU General Public
License, supplemented by the additional permissions listed below.
0. Additional Definitions.
As used herein, "this License" refers to version 3 of the GNU Lesser
General Public License, and the "GNU GPL" refers to version 3 of the GNU
General Public License.
"The Library" refers to a covered work governed by this License,
other than an Application or a Combined Work as defined below.
An "Application" is any work that makes use of an interface provided
by the Library, but which is not otherwise based on the Library.
Defining a subclass of a class defined by the Library is deemed a mode
of using an interface provided by the Library.
A "Combined Work" is a work produced by combining or linking an
Application with the Library. The particular version of the Library
with which the Combined Work was made is also called the "Linked
Version".
The "Minimal Corresponding Source" for a Combined Work means the
Corresponding Source for the Combined Work, excluding any source code
for portions of the Combined Work that, considered in isolation, are
based on the Application, and not on the Linked Version.
The "Corresponding Application Code" for a Combined Work means the
object code and/or source code for the Application, including any data
and utility programs needed for reproducing the Combined Work from the
Application, but excluding the System Libraries of the Combined Work.
1. Exception to Section 3 of the GNU GPL.
You may convey a covered work under sections 3 and 4 of this License
without being bound by section 3 of the GNU GPL.
2. Conveying Modified Versions.
If you modify a copy of the Library, and, in your modifications, a
facility refers to a function or data to be supplied by an Application
that uses the facility (other than as an argument passed when the
facility is invoked), then you may convey a copy of the modified
version:
a) under this License, provided that you make a good faith effort to
ensure that, in the event an Application does not supply the
function or data, the facility still operates, and performs
whatever part of its purpose remains meaningful, or
b) under the GNU GPL, with none of the additional permissions of
this License applicable to that copy.
3. Object Code Incorporating Material from Library Header Files.
The object code form of an Application may incorporate material from
a header file that is part of the Library. You may convey such object
code under terms of your choice, provided that, if the incorporated
material is not limited to numerical parameters, data structure
layouts and accessors, or small macros, inline functions and templates
(ten or fewer lines in length), you do both of the following:
a) Give prominent notice with each copy of the object code that the
Library is used in it and that the Library and its use are
covered by this License.
b) Accompany the object code with a copy of the GNU GPL and this license
document.
4. Combined Works.
You may convey a Combined Work under terms of your choice that,
taken together, effectively do not restrict modification of the
portions of the Library contained in the Combined Work and reverse
engineering for debugging such modifications, if you also do each of
the following:
a) Give prominent notice with each copy of the Combined Work that
the Library is used in it and that the Library and its use are
covered by this License.
b) Accompany the Combined Work with a copy of the GNU GPL and this license
document.
c) For a Combined Work that displays copyright notices during
execution, include the copyright notice for the Library among
these notices, as well as a reference directing the user to the
copies of the GNU GPL and this license document.
d) Do one of the following:
0) Convey the Minimal Corresponding Source under the terms of this
License, and the Corresponding Application Code in a form
suitable for, and under terms that permit, the user to
recombine or relink the Application with a modified version of
the Linked Version to produce a modified Combined Work, in the
manner specified by section 6 of the GNU GPL for conveying
Corresponding Source.
1) Use a suitable shared library mechanism for linking with the
Library. A suitable mechanism is one that (a) uses at run time
a copy of the Library already present on the user's computer
system, and (b) will operate properly with a modified version
of the Library that is interface-compatible with the Linked
Version.
e) Provide Installation Information, but only if you would otherwise
be required to provide such information under section 6 of the
GNU GPL, and only to the extent that such information is
necessary to install and execute a modified version of the
Combined Work produced by recombining or relinking the
Application with a modified version of the Linked Version. (If
you use option 4d0, the Installation Information must accompany
the Minimal Corresponding Source and Corresponding Application
Code. If you use option 4d1, you must provide the Installation
Information in the manner specified by section 6 of the GNU GPL
for conveying Corresponding Source.)
5. Combined Libraries.
You may place library facilities that are a work based on the
Library side by side in a single library together with other library
facilities that are not Applications and are not covered by this
License, and convey such a combined library under terms of your
choice, if you do both of the following:
a) Accompany the combined library with a copy of the same work based
on the Library, uncombined with any other library facilities,
conveyed under the terms of this License.
b) Give prominent notice with the combined library that part of it
is a work based on the Library, and explaining where to find the
accompanying uncombined form of the same work.
6. Revised Versions of the GNU Lesser General Public License.
The Free Software Foundation may publish revised and/or new versions
of the GNU Lesser General Public License from time to time. Such new
versions will be similar in spirit to the present version, but may
differ in detail to address new problems or concerns.
Each version is given a distinguishing version number. If the
Library as you received it specifies that a certain numbered version
of the GNU Lesser General Public License "or any later version"
applies to it, you have the option of following the terms and
conditions either of that published version or of any later version
published by the Free Software Foundation. If the Library as you
received it does not specify a version number of the GNU Lesser
General Public License, you may choose any version of the GNU Lesser
General Public License ever published by the Free Software Foundation.
If the Library as you received it specifies that a proxy can decide
whether future versions of the GNU Lesser General Public License shall
apply, that proxy's public statement of acceptance of any version is
permanent authorization for you to choose that version for the
Library.

11
Devices/Libraries/Ros/ros_canopen/README.md Executable file
View File

@@ -0,0 +1,11 @@
ros_canopen
===========
Canopen implementation for ROS.
[![Build Status](https://travis-ci.com/ros-industrial/ros_canopen.svg?branch=melodic-devel)](https://travis-ci.com/ros-industrial/ros_canopen)
[![License: LGPL v3](https://img.shields.io/badge/License-LGPL%20v3-blue.svg)](https://www.gnu.org/licenses/lgpl-3.0)
([![License: BSD 3-Clause](https://img.shields.io/badge/License-BSD%203--Clause-blue.svg)](https://opensource.org/licenses/BSD-3-Clause) for `can_msgs` and `socketcan_bridge`)
The current develop branch is `melodic-devel`, it targets ROS `melodic`. Needs C++14 compiler.
The released version gets synced over to the distro branch for each release.

72
Devices/Libraries/Ros/ros_canopen/can_msgs/CHANGELOG.rst Executable file
View File

@@ -0,0 +1,72 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package can_msgs
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.8.5 (2020-09-22)
------------------
0.8.4 (2020-08-22)
------------------
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)
------------------
0.8.1 (2019-07-14)
------------------
* Set C++ standard to c++14
* Contributors: Harsh Deshpande
0.8.0 (2018-07-11)
------------------
0.7.8 (2018-05-04)
------------------
0.7.7 (2018-05-04)
------------------
0.7.6 (2017-08-30)
------------------
0.7.5 (2017-05-29)
------------------
0.7.4 (2017-04-25)
------------------
0.7.3 (2017-04-25)
------------------
0.7.2 (2017-03-28)
------------------
0.7.1 (2017-03-20)
------------------
0.7.0 (2016-12-13)
------------------
0.6.5 (2016-12-10)
------------------
* hamonized versions
* styled and sorted CMakeLists.txt
* removed boilerplate comments
* indention
* reviewed exported dependencies
* styled and sorted package.xml
* Adds message_runtime to can_msgs dependencies.
Added the missing dependency, also changes message_generation to a build_depend.
* Finalizes work on the socketcan_bridge and can_msgs.
Readies the packages socketcan_bridge and can_msgs for the merge with ros_canopen.
Bumps the version for both packages to 0.1.0. Final cleanup in CMakeLists, added
comments to the shell script and launchfile used for testing.
* Introduces the can_msgs package for message types.
Package to hold CAN message types, the Frame message type can contain the data
as returned by SocketCAN.
* Contributors: Ivor Wanders, Mathias Lüdtke

29
Devices/Libraries/Ros/ros_canopen/can_msgs/CMakeLists.txt Executable file
View File

@@ -0,0 +1,29 @@
cmake_minimum_required(VERSION 3.0.2)
project(can_msgs)
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_STANDARD 14)
endif()
find_package(catkin REQUIRED
COMPONENTS
message_generation
std_msgs
)
add_message_files(DIRECTORY msg
FILES
Frame.msg
)
generate_messages(
DEPENDENCIES
std_msgs
)
catkin_package(
CATKIN_DEPENDS
message_runtime
std_msgs
)

7
Devices/Libraries/Ros/ros_canopen/can_msgs/msg/Frame.msg Executable file
View File

@@ -0,0 +1,7 @@
Header header
uint32 id
bool is_rtr
bool is_extended
bool is_error
uint8 dlc
uint8[8] data

24
Devices/Libraries/Ros/ros_canopen/can_msgs/package.xml Executable file
View File

@@ -0,0 +1,24 @@
<?xml version="1.0"?>
<package format="2">.
<name>can_msgs</name>
<version>0.8.5</version>
<description>CAN related message types.</description>
<maintainer email="mathias.luedtke@ipa.fraunhofer.de">Mathias Lüdtke</maintainer>
<author email="ivor@iwanders.net">Ivor Wanders</author>
<license>BSD</license>
<url type="website">http://wiki.ros.org/can_msgs</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>std_msgs</depend>
<build_depend>message_generation</build_depend>
<build_export_depend>message_runtime</build_export_depend>
<exec_depend>message_runtime</exec_depend>
</package>

233
Devices/Libraries/Ros/ros_canopen/canopen_402/CHANGELOG.rst Executable file
View File

@@ -0,0 +1,233 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package canopen_402
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.8.5 (2020-09-22)
------------------
0.8.4 (2020-08-22)
------------------
* handle illegal states in nextStateForEnabling
* tranistion -> transition
* Contributors: Mathias Lüdtke, Mikael Arguedas
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)
------------------
* enable rosconsole_bridge bindings
* switch to new logging macros
* Contributors: Mathias Lüdtke
0.8.1 (2019-07-14)
------------------
* Set C++ standard to c++14
* Contributors: Harsh Deshpande
0.8.0 (2018-07-11)
------------------
* handle invalid supported drive modes object
* made Mode402::registerMode a variadic template
* use std::isnan
* migrated to std::atomic
* migrated to std::unordered_map and std::unordered_set
* migrated to std pointers
* fix initialization bug in ProfiledPositionMode
* 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 state_switch_timeout parameter to motor.
* added types for all function objects
* pull make_shared into namespaces
* added types for all shared_ptrs
* migrate to new classloader headers
* address catkin_lint errors/warnings
* Contributors: Alexander Gutenkunst, Mathias Lüdtke
0.7.6 (2017-08-30)
------------------
0.7.5 (2017-05-29)
------------------
0.7.4 (2017-04-25)
------------------
* use portable boost::math::isnan
* Contributors: Mathias Lüdtke
0.7.3 (2017-04-25)
------------------
0.7.2 (2017-03-28)
------------------
0.7.1 (2017-03-20)
------------------
* do quickstop for halt only if operation is enabled
* Contributors: Mathias Lüdtke
0.7.0 (2016-12-13)
------------------
0.6.5 (2016-12-10)
------------------
* stop on internal limit only if limit was not reached before
* hardened code with the help of cppcheck
* Do not send control if it was not changed
Otherwise invalid state machine transitions might get commanded
* styled and sorted CMakeLists.txt
* removed boilerplate comments
* indention
* reviewed exported dependencies
* styled and sorted package.xml
* update package URLs
* added option to turn off mode monitor
* reset Fault_Reset bit in output only
* enforce rising edge on fault reset bit on init and recover
* Revert "Enforce rising edge on fault reset bit on init and recover"
* enforce rising edge on fault reset bit on init and recover
* Merge pull request `#117 <https://github.com/ipa-mdl/ros_canopen/issues/117>`_ from ipa-mdl/condvars
Reviewed condition variables
* use boost::numeric_cast for limit checks since it handles 64bit values right
* add clamping test
* enforce target type limits
* ModeTargetHelper is now template
* readability fix
* simplified State402::waitForNewState and Motor402::switchState
* Set Halt bit unless current mode releases it
* Contributors: Mathias Lüdtke
0.6.4 (2015-07-03)
------------------
0.6.3 (2015-06-30)
------------------
* improved PP mode
* do not quickstop in fault states
* do not diable selected mode on recover, just restart it
* initialize to No_Mode
* removed some empty lines
* implemented support for mode switching in a certain 402 state, defaults to Operation_Enable
* pass LayerStatus to switchMode
* remove enableMotor, introduced switchState
* added motor_layer settings
* fixed PP mode
* explicit find for class_loader
* fail-safe setting of op_mode to No_Mode
* Improved behaviour of concurrent commands
* added alternative Transitions 7 and 10 via Quickstop
* added alternative success condition to waitForNewState
* make motor init/recover interruptable
* changed maintainer
* removed SM-based 402 implementation
* added Motor402 plugin
* added new 402 implementation
* added MotorBase
* Added validity checks
* Removed overloaded functions and makes the handle functions protected
* Removes test executable
* Removes unnecessary configure_drive and clears the set Targets
* Some position fixes
* Removed timeout
Reduced recover trials
* Removes some logs
* Homing integrated
* handleRead, handleWrite fixes
* Merge remote-tracking branch 'mdl/indigo_dev' into refactor_sm
Conflicts:
canopen_402/include/canopen_402/canopen_402.h
canopen_402/src/canopen_402/canopen_402.cpp
canopen_motor_node/src/control_node.cpp
* Moved supported_drive_modes to ModeSpecificEntries
* * Init, Recover, Halt for SCHUNK
* Removed sleeps from the state machine
* Now works as reentrant states
* refactored Layer mechanisms
* Recover failure
* Merge remote-tracking branch 'mdl/indigo_dev' into refactor_sm
Conflicts:
canopen_402/include/canopen_402/canopen_402.h
canopen_402/src/canopen_402/canopen_402.cpp
* Removing some unnecessary couts
* First version with Recover
* Tested on SCHUNK LWA4D
* Initializing all modules at once
* Moving SCHUNK using the IP mode sub-state machine
* Schunk does not set operation mode via synchronized RPDO
* initialize homing_needed to false
* Working with the guard handling and some scoped_locks to prevent unwanted access
* Passing ``state_`` to motor machine
* Fixes crash for unitialized boost pointer for ``target_vel_`` and ``target_pos_``
* Thread running
* Separates the hw with the SM test
Advance on the Mode Switching Machine
* Organizing IPMode State Machine
* Adds mode switch and a pre-version of the turnOn sequence
* Event argument passed to the Motor state machine
* Adds the internal actions
* Control_word is set from motor state machine
* Motor abstraction on higher level machine
Some pointers organization
* * Begins with the Higher Level Machine
* Separates the status and control from the 402 node
* Ip mode sub-machine
* Organizing the status and control machine
* do not read homing method if homing mode is not supported
* inti ``enter_mode_failure_`` to false
* require message strings for error indicators, added missing strings, added ROS logging in sync loop
* Merge pull request `#75 <https://github.com/ros-industrial/ros_canopen/issues/75>`_ from mistoll/indigo_release_candidate
Move ip_mode_sub_mode to configureModeSpecificEntries
* Fixed tabs/spaces
* bind IP sub mode only if IP is supported
* Move ip_mode_sub_mode to configureModeSpecificEntries
* Update state_machine.h
* Ongoing changes for the state machine
* * Eliminates Internal State conflict
* Treats exceptions inside the state machine
* Cleaning the 402.cpp file
* Test file
* Adds state machine definition
* Adds state machine simple test
* Some cleaning necessary to proceed
* Header files for isolating the 402 state machine
* Effort value
* Merge pull request `#6 <https://github.com/ros-industrial/ros_canopen/issues/6>`_ from ipa-mdl/indigo_dev
Work-around for https://github.com/ipa320/ros_canopen/issues/62
* Merge branch 'indigo_dev' of https://github.com/ipa-mdl/ros_canopen into indigo_dev
* fixed unintialized members
* Mode Error priority
* Order issue
* Merge branch 'indigo_dev' of https://github.com/ipa-mdl/ros_canopen into indigo_dev
Conflicts:
canopen_motor_node/CMakeLists.txt
* Error status
* 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: Florian Weisshardt, Mathias Lüdtke, Michael Stoll, Thiago de Freitas Oliveira Araujo, 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_402 to canopen_402
* Contributors: Florian Weisshardt, Mathias Lüdtke

78
Devices/Libraries/Ros/ros_canopen/canopen_402/CMakeLists.txt Executable file
View File

@@ -0,0 +1,78 @@
cmake_minimum_required(VERSION 3.0.2)
project(canopen_402)
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_STANDARD 14)
endif()
find_package(catkin REQUIRED
COMPONENTS
canopen_master
class_loader
)
find_package(Boost REQUIRED
)
catkin_package(
INCLUDE_DIRS
include
LIBRARIES
${PROJECT_NAME}
CATKIN_DEPENDS
canopen_master
DEPENDS
Boost
)
include_directories(
include
${Boost_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
)
# canopen_402
add_library(${PROJECT_NAME}
src/motor.cpp
)
target_link_libraries(${PROJECT_NAME}
${catkin_LIBRARIES}
)
# canopen_402_plugin
add_library(${PROJECT_NAME}_plugin
src/plugin.cpp
)
target_link_libraries(${PROJECT_NAME}_plugin
${catkin_LIBRARIES}
${PROJECT_NAME}
)
install(
TARGETS
${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
${PROJECT_NAME}_plugin.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
if(CATKIN_ENABLE_TESTING)
catkin_add_gtest(${PROJECT_NAME}-test_clamping
test/clamping.cpp
)
target_link_libraries(${PROJECT_NAME}-test_clamping
${catkin_LIBRARIES}
)
endif()

5
Devices/Libraries/Ros/ros_canopen/canopen_402/canopen_402_plugin.xml Executable file
View File

@@ -0,0 +1,5 @@
<library path="lib/libcanopen_402_plugin">
<class type="canopen::Motor402::Allocator" base_class_type="canopen::MotorBase::Allocator">
<description>Allocator for Motor402 instances</description>
</class>
</library>

45
Devices/Libraries/Ros/ros_canopen/canopen_402/include/canopen_402/base.h Executable file
View File

@@ -0,0 +1,45 @@
#ifndef CANOPEN_402_BASE_H
#define CANOPEN_402_BASE_H
#include <canopen_master/canopen.h>
namespace canopen
{
class MotorBase : public canopen::Layer {
protected:
MotorBase(const std::string &name) : Layer(name) {}
public:
enum OperationMode
{
No_Mode = 0,
Profiled_Position = 1,
Velocity = 2,
Profiled_Velocity = 3,
Profiled_Torque = 4,
Reserved = 5,
Homing = 6,
Interpolated_Position = 7,
Cyclic_Synchronous_Position = 8,
Cyclic_Synchronous_Velocity = 9,
Cyclic_Synchronous_Torque = 10,
};
virtual bool setTarget(double val) = 0;
virtual bool enterModeAndWait(uint16_t mode) = 0;
virtual bool isModeSupported(uint16_t mode) = 0;
virtual uint16_t getMode() = 0;
virtual void registerDefaultModes(ObjectStorageSharedPtr storage) {}
typedef std::shared_ptr<MotorBase> MotorBaseSharedPtr;
class Allocator {
public:
virtual MotorBaseSharedPtr allocate(const std::string &name, ObjectStorageSharedPtr storage, const canopen::Settings &settings) = 0;
virtual ~Allocator() {}
};
};
typedef MotorBase::MotorBaseSharedPtr MotorBaseSharedPtr;
}
#endif

390
Devices/Libraries/Ros/ros_canopen/canopen_402/include/canopen_402/motor.h Executable file
View File

@@ -0,0 +1,390 @@
#ifndef CANOPEN_402_MOTOR_H
#define CANOPEN_402_MOTOR_H
#include <canopen_402/base.h>
#include <canopen_master/canopen.h>
#include <functional>
#include <boost/container/flat_map.hpp>
#include <boost/numeric/conversion/cast.hpp>
#include <limits>
#include <algorithm>
namespace canopen
{
class State402{
public:
enum StatusWord
{
SW_Ready_To_Switch_On=0,
SW_Switched_On=1,
SW_Operation_enabled=2,
SW_Fault=3,
SW_Voltage_enabled=4,
SW_Quick_stop=5,
SW_Switch_on_disabled=6,
SW_Warning=7,
SW_Manufacturer_specific0=8,
SW_Remote=9,
SW_Target_reached=10,
SW_Internal_limit=11,
SW_Operation_mode_specific0=12,
SW_Operation_mode_specific1=13,
SW_Manufacturer_specific1=14,
SW_Manufacturer_specific2=15
};
enum InternalState
{
Unknown = 0,
Start = 0,
Not_Ready_To_Switch_On = 1,
Switch_On_Disabled = 2,
Ready_To_Switch_On = 3,
Switched_On = 4,
Operation_Enable = 5,
Quick_Stop_Active = 6,
Fault_Reaction_Active = 7,
Fault = 8,
};
InternalState getState();
InternalState read(uint16_t sw);
bool waitForNewState(const time_point &abstime, InternalState &state);
State402() : state_(Unknown) {}
private:
boost::condition_variable cond_;
boost::mutex mutex_;
InternalState state_;
};
class Command402 {
struct Op {
uint16_t to_set_;
uint16_t to_reset_;
Op(uint16_t to_set ,uint16_t to_reset) : to_set_(to_set), to_reset_(to_reset) {}
void operator()(uint16_t &val) const {
val = (val & ~to_reset_) | to_set_;
}
};
class TransitionTable {
boost::container::flat_map< std::pair<State402::InternalState, State402::InternalState>, Op > transitions_;
void add(const State402::InternalState &from, const State402::InternalState &to, Op op){
transitions_.insert(std::make_pair(std::make_pair(from, to), op));
}
public:
TransitionTable();
const Op& get(const State402::InternalState &from, const State402::InternalState &to) const {
return transitions_.at(std::make_pair(from, to));
}
};
static const TransitionTable transitions_;
static State402::InternalState nextStateForEnabling(State402::InternalState state);
Command402();
public:
enum ControlWord
{
CW_Switch_On=0,
CW_Enable_Voltage=1,
CW_Quick_Stop=2,
CW_Enable_Operation=3,
CW_Operation_mode_specific0=4,
CW_Operation_mode_specific1=5,
CW_Operation_mode_specific2=6,
CW_Fault_Reset=7,
CW_Halt=8,
CW_Operation_mode_specific3=9,
// CW_Reserved1=10,
CW_Manufacturer_specific0=11,
CW_Manufacturer_specific1=12,
CW_Manufacturer_specific2=13,
CW_Manufacturer_specific3=14,
CW_Manufacturer_specific4=15,
};
static bool setTransition(uint16_t &cw, const State402::InternalState &from, const State402::InternalState &to, State402::InternalState *next);
};
template<uint16_t MASK> class WordAccessor{
uint16_t &word_;
public:
WordAccessor(uint16_t &word) : word_(word) {}
bool set(uint8_t bit){
uint16_t val = MASK & (1<<bit);
word_ |= val;
return val;
}
bool reset(uint8_t bit){
uint16_t val = MASK & (1<<bit);
word_ &= ~val;
return val;
}
bool get(uint8_t bit) const { return word_ & (1<<bit); }
uint16_t get() const { return word_ & MASK; }
WordAccessor & operator=(const uint16_t &val){
uint16_t was = word_;
word_ = (word_ & ~MASK) | (val & MASK);
return *this;
}
};
class Mode {
public:
const uint16_t mode_id_;
Mode(uint16_t id) : mode_id_(id) {}
typedef WordAccessor<(1<<Command402::CW_Operation_mode_specific0)|(1<<Command402::CW_Operation_mode_specific1)|(1<<Command402::CW_Operation_mode_specific2)|(1<<Command402::CW_Operation_mode_specific3)> OpModeAccesser;
virtual bool start() = 0;
virtual bool read(const uint16_t &sw) = 0;
virtual bool write(OpModeAccesser& cw) = 0;
virtual bool setTarget(const double &val) { ROSCANOPEN_ERROR("canopen_402", "Mode::setTarget not implemented"); return false; }
virtual ~Mode() {}
};
typedef std::shared_ptr<Mode> ModeSharedPtr;
template<typename T> class ModeTargetHelper : public Mode {
T target_;
std::atomic<bool> has_target_;
public:
ModeTargetHelper(uint16_t mode) : Mode (mode) {}
bool hasTarget() { return has_target_; }
T getTarget() { return target_; }
virtual bool setTarget(const double &val) {
if(std::isnan(val)){
ROSCANOPEN_ERROR("canopen_402", "target command is not a number");
return false;
}
using boost::numeric_cast;
using boost::numeric::positive_overflow;
using boost::numeric::negative_overflow;
try
{
target_= numeric_cast<T>(val);
}
catch(negative_overflow&) {
ROSCANOPEN_WARN("canopen_402", "Command " << val << " does not fit into target, clamping to min limit");
target_= std::numeric_limits<T>::min();
}
catch(positive_overflow&) {
ROSCANOPEN_WARN("canopen_402", "Command " << val << " does not fit into target, clamping to max limit");
target_= std::numeric_limits<T>::max();
}
catch(...){
ROSCANOPEN_ERROR("canopen_402", "Was not able to cast command " << val);
return false;
}
has_target_ = true;
return true;
}
virtual bool start() { has_target_ = false; return true; }
};
template<uint16_t ID, typename TYPE, uint16_t OBJ, uint8_t SUB, uint16_t CW_MASK> class ModeForwardHelper : public ModeTargetHelper<TYPE> {
canopen::ObjectStorage::Entry<TYPE> target_entry_;
public:
ModeForwardHelper(ObjectStorageSharedPtr storage) : ModeTargetHelper<TYPE>(ID) {
if(SUB) storage->entry(target_entry_, OBJ, SUB);
else storage->entry(target_entry_, OBJ);
}
virtual bool read(const uint16_t &sw) { return true;}
virtual bool write(Mode::OpModeAccesser& cw) {
if(this->hasTarget()){
cw = cw.get() | CW_MASK;
target_entry_.set(this->getTarget());
return true;
}else{
cw = cw.get() & ~CW_MASK;
return false;
}
}
};
typedef ModeForwardHelper<MotorBase::Profiled_Velocity, int32_t, 0x60FF, 0, 0> ProfiledVelocityMode;
typedef ModeForwardHelper<MotorBase::Profiled_Torque, int16_t, 0x6071, 0, 0> ProfiledTorqueMode;
typedef ModeForwardHelper<MotorBase::Cyclic_Synchronous_Position, int32_t, 0x607A, 0, 0> CyclicSynchronousPositionMode;
typedef ModeForwardHelper<MotorBase::Cyclic_Synchronous_Velocity, int32_t, 0x60FF, 0, 0> CyclicSynchronousVelocityMode;
typedef ModeForwardHelper<MotorBase::Cyclic_Synchronous_Torque, int16_t, 0x6071, 0, 0> CyclicSynchronousTorqueMode;
typedef ModeForwardHelper<MotorBase::Velocity, int16_t, 0x6042, 0, (1<<Command402::CW_Operation_mode_specific0)|(1<<Command402::CW_Operation_mode_specific1)|(1<<Command402::CW_Operation_mode_specific2)> VelocityMode;
typedef ModeForwardHelper<MotorBase::Interpolated_Position, int32_t, 0x60C1, 0x01, (1<<Command402::CW_Operation_mode_specific0)> InterpolatedPositionMode;
class ProfiledPositionMode : public ModeTargetHelper<int32_t> {
canopen::ObjectStorage::Entry<int32_t> target_position_;
double last_target_;
uint16_t sw_;
public:
enum SW_masks {
MASK_Reached = (1<<State402::SW_Target_reached),
MASK_Acknowledged = (1<<State402::SW_Operation_mode_specific0),
MASK_Error = (1<<State402::SW_Operation_mode_specific1),
};
enum CW_bits {
CW_NewPoint = Command402::CW_Operation_mode_specific0,
CW_Immediate = Command402::CW_Operation_mode_specific1,
CW_Blending = Command402::CW_Operation_mode_specific3,
};
ProfiledPositionMode(ObjectStorageSharedPtr storage) : ModeTargetHelper(MotorBase::Profiled_Position) {
storage->entry(target_position_, 0x607A);
}
virtual bool start() { sw_ = 0; last_target_= std::numeric_limits<double>::quiet_NaN(); return ModeTargetHelper::start(); }
virtual bool read(const uint16_t &sw) { sw_ = sw; return (sw & MASK_Error) == 0; }
virtual bool write(OpModeAccesser& cw) {
cw.set(CW_Immediate);
if(hasTarget()){
int32_t target = getTarget();
if((sw_ & MASK_Acknowledged) == 0 && target != last_target_){
if(cw.get(CW_NewPoint)){
cw.reset(CW_NewPoint); // reset if needed
}else{
target_position_.set(target);
cw.set(CW_NewPoint);
last_target_ = target;
}
} else if (sw_ & MASK_Acknowledged){
cw.reset(CW_NewPoint);
}
return true;
}
return false;
}
};
class HomingMode: public Mode{
protected:
enum SW_bits {
SW_Attained = State402::SW_Operation_mode_specific0,
SW_Error = State402::SW_Operation_mode_specific1,
};
enum CW_bits {
CW_StartHoming = Command402::CW_Operation_mode_specific0,
};
public:
HomingMode() : Mode(MotorBase::Homing) {}
virtual bool executeHoming(canopen::LayerStatus &status) = 0;
};
class DefaultHomingMode: public HomingMode{
canopen::ObjectStorage::Entry<int8_t> homing_method_;
std::atomic<bool> execute_;
boost::mutex mutex_;
boost::condition_variable cond_;
uint16_t status_;
enum SW_masks {
MASK_Reached = (1<<State402::SW_Target_reached),
MASK_Attained = (1<<SW_Attained),
MASK_Error = (1<<SW_Error),
};
bool error(canopen::LayerStatus &status, const std::string& msg) { execute_= false; status.error(msg); return false; }
public:
DefaultHomingMode(ObjectStorageSharedPtr storage) {
storage->entry(homing_method_, 0x6098);
}
virtual bool start();
virtual bool read(const uint16_t &sw);
virtual bool write(OpModeAccesser& cw);
virtual bool executeHoming(canopen::LayerStatus &status);
};
class Motor402 : public MotorBase
{
public:
Motor402(const std::string &name, ObjectStorageSharedPtr storage, const canopen::Settings &settings)
: MotorBase(name), status_word_(0),control_word_(0),
switching_state_(State402::InternalState(settings.get_optional<unsigned int>("switching_state", static_cast<unsigned int>(State402::Operation_Enable)))),
monitor_mode_(settings.get_optional<bool>("monitor_mode", true)),
state_switch_timeout_(settings.get_optional<unsigned int>("state_switch_timeout", 5))
{
storage->entry(status_word_entry_, 0x6041);
storage->entry(control_word_entry_, 0x6040);
storage->entry(op_mode_display_, 0x6061);
storage->entry(op_mode_, 0x6060);
try{
storage->entry(supported_drive_modes_, 0x6502);
}
catch(...){
}
}
virtual bool setTarget(double val);
virtual bool enterModeAndWait(uint16_t mode);
virtual bool isModeSupported(uint16_t mode);
virtual uint16_t getMode();
template<typename T, typename ...Args>
bool registerMode(uint16_t mode, Args&&... args) {
return mode_allocators_.insert(std::make_pair(mode, [args..., mode, this](){
if(isModeSupportedByDevice(mode)) registerMode(mode, ModeSharedPtr(new T(args...)));
})).second;
}
virtual void registerDefaultModes(ObjectStorageSharedPtr storage){
registerMode<ProfiledPositionMode> (MotorBase::Profiled_Position, storage);
registerMode<VelocityMode> (MotorBase::Velocity, storage);
registerMode<ProfiledVelocityMode> (MotorBase::Profiled_Velocity, storage);
registerMode<ProfiledTorqueMode> (MotorBase::Profiled_Torque, storage);
registerMode<DefaultHomingMode> (MotorBase::Homing, storage);
registerMode<InterpolatedPositionMode> (MotorBase::Interpolated_Position, storage);
registerMode<CyclicSynchronousPositionMode> (MotorBase::Cyclic_Synchronous_Position, storage);
registerMode<CyclicSynchronousVelocityMode> (MotorBase::Cyclic_Synchronous_Velocity, storage);
registerMode<CyclicSynchronousTorqueMode> (MotorBase::Cyclic_Synchronous_Torque, storage);
}
class Allocator : public MotorBase::Allocator{
public:
virtual MotorBaseSharedPtr allocate(const std::string &name, ObjectStorageSharedPtr storage, const canopen::Settings &settings);
};
protected:
virtual void handleRead(LayerStatus &status, const LayerState &current_state);
virtual void handleWrite(LayerStatus &status, const LayerState &current_state);
virtual void handleDiag(LayerReport &report);
virtual void handleInit(LayerStatus &status);
virtual void handleShutdown(LayerStatus &status);
virtual void handleHalt(LayerStatus &status);
virtual void handleRecover(LayerStatus &status);
private:
virtual bool isModeSupportedByDevice(uint16_t mode);
void registerMode(uint16_t id, const ModeSharedPtr &m);
ModeSharedPtr allocMode(uint16_t mode);
bool readState(LayerStatus &status, const LayerState &current_state);
bool switchMode(LayerStatus &status, uint16_t mode);
bool switchState(LayerStatus &status, const State402::InternalState &target);
std::atomic<uint16_t> status_word_;
uint16_t control_word_;
boost::mutex cw_mutex_;
std::atomic<bool> start_fault_reset_;
std::atomic<State402::InternalState> target_state_;
State402 state_handler_;
boost::mutex map_mutex_;
std::unordered_map<uint16_t, ModeSharedPtr > modes_;
typedef std::function<void()> AllocFuncType;
std::unordered_map<uint16_t, AllocFuncType> mode_allocators_;
ModeSharedPtr selected_mode_;
uint16_t mode_id_;
boost::condition_variable mode_cond_;
boost::mutex mode_mutex_;
const State402::InternalState switching_state_;
const bool monitor_mode_;
const boost::chrono::seconds state_switch_timeout_;
canopen::ObjectStorage::Entry<uint16_t> status_word_entry_;
canopen::ObjectStorage::Entry<uint16_t > control_word_entry_;
canopen::ObjectStorage::Entry<int8_t> op_mode_display_;
canopen::ObjectStorage::Entry<int8_t> op_mode_;
canopen::ObjectStorage::Entry<uint32_t> supported_drive_modes_;
};
}
#endif

28
Devices/Libraries/Ros/ros_canopen/canopen_402/package.xml Executable file
View File

@@ -0,0 +1,28 @@
<?xml version="1.0"?>
<package format="2">
<name>canopen_402</name>
<version>0.8.5</version>
<description>This implements the CANopen device profile for drives and motion control. CiA(r) 402</description>
<maintainer email="mathias.luedtke@ipa.fraunhofer.de">Mathias Lüdtke</maintainer>
<author email="tdf@ipa.fhg.de">Thiago de Freitas</author>
<author email="mathias.luedtke@ipa.fraunhofer.de">Mathias Lüdtke</author>
<license>LGPLv3</license>
<url type="website">http://wiki.ros.org/canopen_402</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>canopen_master</depend>
<depend>class_loader</depend>
<test_depend>rosunit</test_depend>
<export>
<canopen_402 plugin="${prefix}/canopen_402_plugin.xml" />
</export>
</package>

549
Devices/Libraries/Ros/ros_canopen/canopen_402/src/motor.cpp Executable file
View File

@@ -0,0 +1,549 @@
#include <canopen_402/motor.h>
#include <boost/thread/reverse_lock.hpp>
namespace canopen
{
State402::InternalState State402::getState(){
boost::mutex::scoped_lock lock(mutex_);
return state_;
}
State402::InternalState State402::read(uint16_t sw) {
static const uint16_t r = (1 << SW_Ready_To_Switch_On);
static const uint16_t s = (1 << SW_Switched_On);
static const uint16_t o = (1 << SW_Operation_enabled);
static const uint16_t f = (1 << SW_Fault);
static const uint16_t q = (1 << SW_Quick_stop);
static const uint16_t d = (1 << SW_Switch_on_disabled);
InternalState new_state = Unknown;
uint16_t state = sw & ( d | q | f | o | s | r );
switch ( state )
{
// ( d | q | f | o | s | r ):
case ( 0 | 0 | 0 | 0 | 0 | 0 ):
case ( 0 | q | 0 | 0 | 0 | 0 ):
new_state = Not_Ready_To_Switch_On;
break;
case ( d | 0 | 0 | 0 | 0 | 0 ):
case ( d | q | 0 | 0 | 0 | 0 ):
new_state = Switch_On_Disabled;
break;
case ( 0 | q | 0 | 0 | 0 | r ):
new_state = Ready_To_Switch_On;
break;
case ( 0 | q | 0 | 0 | s | r ):
new_state = Switched_On;
break;
case ( 0 | q | 0 | o | s | r ):
new_state = Operation_Enable;
break;
case ( 0 | 0 | 0 | o | s | r ):
new_state = Quick_Stop_Active;
break;
case ( 0 | 0 | f | o | s | r ):
case ( 0 | q | f | o | s | r ):
new_state = Fault_Reaction_Active;
break;
case ( 0 | 0 | f | 0 | 0 | 0 ):
case ( 0 | q | f | 0 | 0 | 0 ):
new_state = Fault;
break;
default:
ROSCANOPEN_WARN("canopen_402", "Motor is currently in an unknown state: " << std::hex << state << std::dec);
}
boost::mutex::scoped_lock lock(mutex_);
if(new_state != state_){
state_ = new_state;
cond_.notify_all();
}
return state_;
}
bool State402::waitForNewState(const time_point &abstime, State402::InternalState &state){
boost::mutex::scoped_lock lock(mutex_);
while(state_ == state && cond_.wait_until(lock, abstime) == boost::cv_status::no_timeout) {}
bool res = state != state_;
state = state_;
return res;
}
const Command402::TransitionTable Command402::transitions_;
Command402::TransitionTable::TransitionTable(){
typedef State402 s;
transitions_.reserve(32);
Op disable_voltage(0,(1<<CW_Fault_Reset) | (1<<CW_Enable_Voltage));
/* 7*/ add(s::Ready_To_Switch_On, s::Switch_On_Disabled, disable_voltage);
/* 9*/ add(s::Operation_Enable, s::Switch_On_Disabled, disable_voltage);
/*10*/ add(s::Switched_On, s::Switch_On_Disabled, disable_voltage);
/*12*/ add(s::Quick_Stop_Active, s::Switch_On_Disabled, disable_voltage);
Op automatic(0,0);
/* 0*/ add(s::Start, s::Not_Ready_To_Switch_On, automatic);
/* 1*/ add(s::Not_Ready_To_Switch_On, s::Switch_On_Disabled, automatic);
/*14*/ add(s::Fault_Reaction_Active, s::Fault, automatic);
Op shutdown((1<<CW_Quick_Stop) | (1<<CW_Enable_Voltage), (1<<CW_Fault_Reset) | (1<<CW_Switch_On));
/* 2*/ add(s::Switch_On_Disabled, s::Ready_To_Switch_On, shutdown);
/* 6*/ add(s::Switched_On, s::Ready_To_Switch_On, shutdown);
/* 8*/ add(s::Operation_Enable, s::Ready_To_Switch_On, shutdown);
Op switch_on((1<<CW_Quick_Stop) | (1<<CW_Enable_Voltage) | (1<<CW_Switch_On), (1<<CW_Fault_Reset) | (1<<CW_Enable_Operation));
/* 3*/ add(s::Ready_To_Switch_On, s::Switched_On, switch_on);
/* 5*/ add(s::Operation_Enable, s::Switched_On, switch_on);
Op enable_operation((1<<CW_Quick_Stop) | (1<<CW_Enable_Voltage) | (1<<CW_Switch_On) | (1<<CW_Enable_Operation), (1<<CW_Fault_Reset));
/* 4*/ add(s::Switched_On, s::Operation_Enable, enable_operation);
/*16*/ add(s::Quick_Stop_Active, s::Operation_Enable, enable_operation);
Op quickstop((1<<CW_Enable_Voltage), (1<<CW_Fault_Reset) | (1<<CW_Quick_Stop));
/* 7*/ add(s::Ready_To_Switch_On, s::Quick_Stop_Active, quickstop); // transit to Switch_On_Disabled
/*10*/ add(s::Switched_On, s::Quick_Stop_Active, quickstop); // transit to Switch_On_Disabled
/*11*/ add(s::Operation_Enable, s::Quick_Stop_Active, quickstop);
// fault reset
/*15*/ add(s::Fault, s::Switch_On_Disabled, Op((1<<CW_Fault_Reset), 0));
}
State402::InternalState Command402::nextStateForEnabling(State402::InternalState state){
switch(state){
case State402::Start:
return State402::Not_Ready_To_Switch_On;
case State402::Fault:
case State402::Not_Ready_To_Switch_On:
return State402::Switch_On_Disabled;
case State402::Switch_On_Disabled:
return State402::Ready_To_Switch_On;
case State402::Ready_To_Switch_On:
return State402::Switched_On;
case State402::Switched_On:
case State402::Quick_Stop_Active:
case State402::Operation_Enable:
return State402::Operation_Enable;
case State402::Fault_Reaction_Active:
return State402::Fault;
}
throw std::out_of_range ("state value is illegal");
}
bool Command402::setTransition(uint16_t &cw, const State402::InternalState &from, const State402::InternalState &to, State402::InternalState *next){
try{
if(from != to){
State402::InternalState hop = to;
if(next){
if(to == State402::Operation_Enable) hop = nextStateForEnabling(from);
*next = hop;
}
transitions_.get(from, hop)(cw);
}
return true;
}
catch(...){
ROSCANOPEN_WARN("canopen_402", "illegal transition " << from << " -> " << to);
}
return false;
}
template<uint16_t mask, uint16_t not_equal> struct masked_status_not_equal {
uint16_t &status_;
masked_status_not_equal(uint16_t &status) : status_(status) {}
bool operator()() const { return (status_ & mask) != not_equal; }
};
bool DefaultHomingMode::start() {
execute_ = false;
return read(0);
}
bool DefaultHomingMode::read(const uint16_t &sw) {
boost::mutex::scoped_lock lock(mutex_);
uint16_t old = status_;
status_ = sw & (MASK_Reached | MASK_Attained | MASK_Error);
if(old != status_){
cond_.notify_all();
}
return true;
}
bool DefaultHomingMode::write(Mode::OpModeAccesser& cw) {
cw = 0;
if(execute_){
cw.set(CW_StartHoming);
return true;
}
return false;
}
bool DefaultHomingMode::executeHoming(canopen::LayerStatus &status) {
if(!homing_method_.valid()){
return error(status, "homing method entry is not valid");
}
if(homing_method_.get_cached() == 0){
return true;
}
time_point prepare_time = get_abs_time(boost::chrono::seconds(1));
// ensure homing is not running
boost::mutex::scoped_lock lock(mutex_);
if(!cond_.wait_until(lock, prepare_time, masked_status_not_equal<MASK_Error | MASK_Reached, 0> (status_))){
return error(status, "could not prepare homing");
}
if(status_ & MASK_Error){
return error(status, "homing error before start");
}
execute_ = true;
// ensure start
if(!cond_.wait_until(lock, prepare_time, masked_status_not_equal<MASK_Error | MASK_Attained | MASK_Reached, MASK_Reached> (status_))){
return error(status, "homing did not start");
}
if(status_ & MASK_Error){
return error(status, "homing error at start");
}
time_point finish_time = get_abs_time(boost::chrono::seconds(10)); //
// wait for attained
if(!cond_.wait_until(lock, finish_time, masked_status_not_equal<MASK_Error | MASK_Attained, 0> (status_))){
return error(status, "homing not attained");
}
if(status_ & MASK_Error){
return error(status, "homing error during process");
}
// wait for motion stop
if(!cond_.wait_until(lock, finish_time, masked_status_not_equal<MASK_Error | MASK_Reached, 0> (status_))){
return error(status, "homing did not stop");
}
if(status_ & MASK_Error){
return error(status, "homing error during stop");
}
if((status_ & MASK_Reached) && (status_ & MASK_Attained)){
execute_ = false;
return true;
}
return error(status, "something went wrong while homing");
}
bool Motor402::setTarget(double val){
if(state_handler_.getState() == State402::Operation_Enable){
boost::mutex::scoped_lock lock(mode_mutex_);
return selected_mode_ && selected_mode_->setTarget(val);
}
return false;
}
bool Motor402::isModeSupported(uint16_t mode) { return mode != MotorBase::Homing && allocMode(mode); }
bool Motor402::enterModeAndWait(uint16_t mode) {
LayerStatus s;
bool okay = mode != MotorBase::Homing && switchMode(s, mode);
if(!s.bounded<LayerStatus::Ok>()){
ROSCANOPEN_ERROR("canopen_402", "Could not switch to mode " << mode << ", reason: " << s.reason());
}
return okay;
}
uint16_t Motor402::getMode() {
boost::mutex::scoped_lock lock(mode_mutex_);
return selected_mode_ ? selected_mode_->mode_id_ : MotorBase::No_Mode;
}
bool Motor402::isModeSupportedByDevice(uint16_t mode){
if(!supported_drive_modes_.valid()) {
BOOST_THROW_EXCEPTION( std::runtime_error("Supported drive modes (object 6502) is not valid"));
}
return mode > 0 && mode <= 32 && (supported_drive_modes_.get_cached() & (1<<(mode-1)));
}
void Motor402::registerMode(uint16_t id, const ModeSharedPtr &m){
boost::mutex::scoped_lock map_lock(map_mutex_);
if(m && m->mode_id_ == id) modes_.insert(std::make_pair(id, m));
}
ModeSharedPtr Motor402::allocMode(uint16_t mode){
ModeSharedPtr res;
if(isModeSupportedByDevice(mode)){
boost::mutex::scoped_lock map_lock(map_mutex_);
std::unordered_map<uint16_t, ModeSharedPtr >::iterator it = modes_.find(mode);
if(it != modes_.end()){
res = it->second;
}
}
return res;
}
bool Motor402::switchMode(LayerStatus &status, uint16_t mode) {
if(mode == MotorBase::No_Mode){
boost::mutex::scoped_lock lock(mode_mutex_);
selected_mode_.reset();
try{ // try to set mode
op_mode_.set(mode);
}catch(...){}
return true;
}
ModeSharedPtr next_mode = allocMode(mode);
if(!next_mode){
status.error("Mode is not supported.");
return false;
}
if(!next_mode->start()){
status.error("Could not start mode.");
return false;
}
{ // disable mode handler
boost::mutex::scoped_lock lock(mode_mutex_);
if(mode_id_ == mode && selected_mode_ && selected_mode_->mode_id_ == mode){
// nothing to do
return true;
}
selected_mode_.reset();
}
if(!switchState(status, switching_state_)) return false;
op_mode_.set(mode);
bool okay = false;
{ // wait for switch
boost::mutex::scoped_lock lock(mode_mutex_);
time_point abstime = get_abs_time(boost::chrono::seconds(5));
if(monitor_mode_){
while(mode_id_ != mode && mode_cond_.wait_until(lock, abstime) == boost::cv_status::no_timeout) {}
}else{
while(mode_id_ != mode && get_abs_time() < abstime){
boost::reverse_lock<boost::mutex::scoped_lock> reverse(lock); // unlock inside loop
op_mode_display_.get(); // poll
boost::this_thread::sleep_for(boost::chrono::milliseconds(20)); // wait some time
}
}
if(mode_id_ == mode){
selected_mode_ = next_mode;
okay = true;
}else{
status.error("Mode switch timed out.");
op_mode_.set(mode_id_);
}
}
if(!switchState(status, State402::Operation_Enable)) return false;
return okay;
}
bool Motor402::switchState(LayerStatus &status, const State402::InternalState &target){
time_point abstime = get_abs_time(state_switch_timeout_);
State402::InternalState state = state_handler_.getState();
target_state_ = target;
while(state != target_state_){
boost::mutex::scoped_lock lock(cw_mutex_);
State402::InternalState next = State402::Unknown;
if(!Command402::setTransition(control_word_ ,state, target_state_ , &next)){
status.error("Could not set transition");
return false;
}
lock.unlock();
if(state != next && !state_handler_.waitForNewState(abstime, state)){
status.error("Transition timeout");
return false;
}
}
return state == target;
}
bool Motor402::readState(LayerStatus &status, const LayerState &current_state){
uint16_t old_sw, sw = status_word_entry_.get(); // TODO: added error handling
old_sw = status_word_.exchange(sw);
state_handler_.read(sw);
boost::mutex::scoped_lock lock(mode_mutex_);
uint16_t new_mode = monitor_mode_ ? op_mode_display_.get() : op_mode_display_.get_cached();
if(selected_mode_ && selected_mode_->mode_id_ == new_mode){
if(!selected_mode_->read(sw)){
status.error("Mode handler has error");
}
}
if(new_mode != mode_id_){
mode_id_ = new_mode;
mode_cond_.notify_all();
}
if(selected_mode_ && selected_mode_->mode_id_ != new_mode){
status.warn("mode does not match");
}
if(sw & (1<<State402::SW_Internal_limit)){
if(old_sw & (1<<State402::SW_Internal_limit) || current_state != Ready){
status.warn("Internal limit active");
}else{
status.error("Internal limit active");
}
}
return true;
}
void Motor402::handleRead(LayerStatus &status, const LayerState &current_state){
if(current_state > Off){
readState(status, current_state);
}
}
void Motor402::handleWrite(LayerStatus &status, const LayerState &current_state){
if(current_state > Off){
boost::mutex::scoped_lock lock(cw_mutex_);
control_word_ |= (1<<Command402::CW_Halt);
if(state_handler_.getState() == State402::Operation_Enable){
boost::mutex::scoped_lock lock(mode_mutex_);
Mode::OpModeAccesser cwa(control_word_);
bool okay = false;
if(selected_mode_ && selected_mode_->mode_id_ == mode_id_){
okay = selected_mode_->write(cwa);
} else {
cwa = 0;
}
if(okay) {
control_word_ &= ~(1<<Command402::CW_Halt);
}
}
if(start_fault_reset_.exchange(false)){
control_word_entry_.set_cached(control_word_ & ~(1<<Command402::CW_Fault_Reset));
}else{
control_word_entry_.set_cached(control_word_);
}
}
}
void Motor402::handleDiag(LayerReport &report){
uint16_t sw = status_word_;
State402::InternalState state = state_handler_.getState();
switch(state){
case State402::Not_Ready_To_Switch_On:
case State402::Switch_On_Disabled:
case State402::Ready_To_Switch_On:
case State402::Switched_On:
report.warn("Motor operation is not enabled");
case State402::Operation_Enable:
break;
case State402::Quick_Stop_Active:
report.error("Quick stop is active");
break;
case State402::Fault:
case State402::Fault_Reaction_Active:
report.error("Motor has fault");
break;
case State402::Unknown:
report.error("State is unknown");
report.add("status_word", sw);
break;
}
if(sw & (1<<State402::SW_Warning)){
report.warn("Warning bit is set");
}
if(sw & (1<<State402::SW_Internal_limit)){
report.error("Internal limit active");
}
}
void Motor402::handleInit(LayerStatus &status){
for(std::unordered_map<uint16_t, AllocFuncType>::iterator it = mode_allocators_.begin(); it != mode_allocators_.end(); ++it){
(it->second)();
}
if(!readState(status, Init)){
status.error("Could not read motor state");
return;
}
{
boost::mutex::scoped_lock lock(cw_mutex_);
control_word_ = 0;
start_fault_reset_ = true;
}
if(!switchState(status, State402::Operation_Enable)){
status.error("Could not enable motor");
return;
}
ModeSharedPtr m = allocMode(MotorBase::Homing);
if(!m){
return; // homing not supported
}
HomingMode *homing = dynamic_cast<HomingMode*>(m.get());
if(!homing){
status.error("Homing mode has incorrect handler");
return;
}
if(!switchMode(status, MotorBase::Homing)){
status.error("Could not enter homing mode");
return;
}
if(!homing->executeHoming(status)){
status.error("Homing failed");
return;
}
switchMode(status, No_Mode);
}
void Motor402::handleShutdown(LayerStatus &status){
switchMode(status, MotorBase::No_Mode);
switchState(status, State402::Switch_On_Disabled);
}
void Motor402::handleHalt(LayerStatus &status){
State402::InternalState state = state_handler_.getState();
boost::mutex::scoped_lock lock(cw_mutex_);
// do not demand quickstop in case of fault
if(state == State402::Fault_Reaction_Active || state == State402::Fault) return;
if(state != State402::Operation_Enable){
target_state_ = state;
}else{
target_state_ = State402::Quick_Stop_Active;
if(!Command402::setTransition(control_word_ ,state, State402::Quick_Stop_Active, 0)){
status.warn("Could not quick stop");
}
}
}
void Motor402::handleRecover(LayerStatus &status){
start_fault_reset_ = true;
{
boost::mutex::scoped_lock lock(mode_mutex_);
if(selected_mode_ && !selected_mode_->start()){
status.error("Could not restart mode.");
return;
}
}
if(!switchState(status, State402::Operation_Enable)){
status.error("Could not enable motor");
return;
}
}
} // namespace

8
Devices/Libraries/Ros/ros_canopen/canopen_402/src/plugin.cpp Executable file
View File

@@ -0,0 +1,8 @@
#include <class_loader/class_loader.hpp>
#include <canopen_402/motor.h>
canopen::MotorBaseSharedPtr canopen::Motor402::Allocator::allocate(const std::string &name, canopen::ObjectStorageSharedPtr storage, const canopen::Settings &settings) {
return std::make_shared<canopen::Motor402>(name, storage, settings);
}
CLASS_LOADER_REGISTER_CLASS(canopen::Motor402::Allocator, canopen::MotorBase::Allocator);

57
Devices/Libraries/Ros/ros_canopen/canopen_402/test/clamping.cpp Executable file
View File

@@ -0,0 +1,57 @@
#include <canopen_402/motor.h>
#include <gtest/gtest.h>
template<typename T> class ModeTargetHelperTest : public canopen::ModeTargetHelper<T>, public ::testing::Test{
public:
ModeTargetHelperTest() : canopen::ModeTargetHelper<T>(0) {}
virtual bool read(const uint16_t &sw) { return false; }
virtual bool write(canopen::Mode::OpModeAccesser& cw) { return false; }
};
typedef ::testing::Types<uint8_t, int8_t, uint16_t, int16_t, uint32_t, int32_t, uint64_t, int64_t> MyTypes;
TYPED_TEST_CASE(ModeTargetHelperTest, MyTypes);
TYPED_TEST(ModeTargetHelperTest, CheckNaN){
ASSERT_FALSE(this->setTarget(std::numeric_limits<double>::quiet_NaN()));
}
TYPED_TEST(ModeTargetHelperTest, CheckZero){
ASSERT_TRUE(this->setTarget(0.0));
}
TYPED_TEST(ModeTargetHelperTest, CheckOne){
ASSERT_TRUE(this->setTarget(1.0));
}
TYPED_TEST(ModeTargetHelperTest, CheckMax){
double max = static_cast<double>(std::numeric_limits<TypeParam>::max());
ASSERT_TRUE(this->setTarget(max));
ASSERT_EQ(max, this->getTarget());
ASSERT_TRUE(this->setTarget(max-1));
ASSERT_EQ(max-1,this->getTarget());
ASSERT_TRUE(this->setTarget(max+1));
ASSERT_EQ(max, this->getTarget());
}
TYPED_TEST(ModeTargetHelperTest, CheckMin){
double min = static_cast<double>(std::numeric_limits<TypeParam>::min());
ASSERT_TRUE(this->setTarget(min));
ASSERT_EQ(min, this->getTarget());
ASSERT_TRUE(this->setTarget(min-1));
ASSERT_EQ(min, this->getTarget());
ASSERT_TRUE(this->setTarget(min+1));
ASSERT_EQ(min+1,this->getTarget());
}
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

199
Devices/Libraries/Ros/ros_canopen/canopen_chain_node/CHANGELOG.rst Executable file
View File

@@ -0,0 +1,199 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package canopen_chain_node
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.8.5 (2020-09-22)
------------------
0.8.4 (2020-08-22)
------------------
* pass settings from ROS node to SocketCANInterface
* Contributors: Mathias Lüdtke
0.8.3 (2020-05-07)
------------------
* Bump CMake version to avoid CMP0048 warning
Signed-off-by: ahcorde <ahcorde@gmail.com>
* Contributors: ahcorde
0.8.2 (2019-11-04)
------------------
* rename to logWarning to fix build on Debian stretch
* log the result of all services in RosChain
* enable rosconsole_bridge bindings
* switch to new logging macros
* Contributors: Mathias Lüdtke
0.8.1 (2019-07-14)
------------------
* Set C++ standard to c++14
* implemented create\*ListenerM helpers
* Replacing FastDelegate with std::function and std::bind.
* Contributors: Alexander Gutenkunst, Harsh Deshpande, Joshua Whitley, Mathias Lüdtke
0.8.0 (2018-07-11)
------------------
* migrated to std::function and std::bind
* make sync_node return proper error codes
* refactored PublishFunc
* migrated to std::atomic
* migrated to std pointers
* removed deprecated types
* 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
* 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 reset_errors_before_recover option
* Contributors: Mathias Lüdtke
0.7.4 (2017-04-25)
------------------
0.7.3 (2017-04-25)
------------------
0.7.2 (2017-03-28)
------------------
0.7.1 (2017-03-20)
------------------
* refactored EMCY handling into separate layer
* do not reset thread for recover
* properly stop run thread if init failed
* deprecation warning for SHM-based master implementations
* implemented canopen_sync_node
* wait only if sync is disabled
* added object access services
* implement level-based object logging
* added node name lookup
* Contributors: Mathias Lüdtke
0.7.0 (2016-12-13)
------------------
0.6.5 (2016-12-10)
------------------
* protect MotorChain setup with RosChain lock
* added include to <boost/scoped_ptr.hpp>; solving `#177 <https://github.com/ipa-mdl/ros_canopen/issues/177>`_
* default to canopen::SimpleMaster::Allocator (`#71 <https://github.com/ipa-mdl/ros_canopen/issues/71>`_)
* exit code for generic error should be 1, not -1
* styled and sorted CMakeLists.txt
* removed boilerplate comments
* indention
* reviewed exported dependencies
* styled and sorted package.xml
* update package URLs
* moved roslib include into source file
* renamed chain_ros.h to ros_chain.h, fixes `#126 <https://github.com/ipa-mdl/ros_canopen/issues/126>`_
* Use of catkin_EXPORTED_TARGETS
Install target for canopen_ros_chain
* Splitted charn_ros.h into chain_ros.h and ros_chain.cpp
* Revert "stop heartbeat after stack was shutdown"
This reverts commit de985b5e9664edbbcc4f743fff3e2a2391e1bf8f.
* improve failure handling in init service callback
* improved excetion handling in init and recover callback
* 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
* catch std::exception instead of canopen::Exception (`#110 <https://github.com/ipa-mdl/ros_canopen/issues/110>`_)
* call to detroy is not needed anymore
* added GuardedClassLoader implementation
* minor shutdown improvements
* Contributors: Mathias Lüdtke, Michael Stoll, xaedes
0.6.4 (2015-07-03)
------------------
0.6.3 (2015-06-30)
------------------
* added motor_layer settings
* remove boost::posix_time::milliseconds from SyncProperties
* removed support for silence_us since bus timing cannot be guaranteed
* implemented plugin-based Master allocators, defaults to LocalMaster
* set initialized to false explicitly if init failed
* include for std_msgs::String was missing
* Merge remote-tracking branch 'origin/std_trigger' into new_402
Conflicts:
canopen_chain_node/CMakeLists.txt
canopen_chain_node/include/canopen_chain_node/chain_ros.h
* halt explicitly on shutdown
* stop heartbeat after stack was shutdown
* migrated to Timer instead of ros::Timer to send heartbeat even after ros was shutdown
* run loop even if ros is shutdown
* improved chain shutdown behaviour
* fix for g++: proper message generation
* Merge branch 'publisher' into muparser
Conflicts:
canopen_motor_node/src/control_node.cpp
* added generic object publishers
* migrated to std_srvs/Trigger
* use atomic flag instead of thread pointer for synchronization
* do not run diagnostics if chain was not initalized, output warning instead
* Changes Layer Status to Warning during the service calls
* refactored Layer mechanisms
* heartbeat works now
* check XmlRpcValue types in dcf_overlay
* removed IPCLayer sync listener, loopback is disabled per default
* added simple heartbeat timer
* added sync silence feature
* parse sync properties only if sync_ms is valid
* require message strings for error indicators, added missing strings, added ROS logging in sync loop
* skip "eds_pkg" if not provided
* clear layer before plugin loader is deleted
* implemented node list as struct
* 'modules' was renamed to 'nodes'
* removed chain name
* added driver_plugin parameter for pluginlib look-up
* implemented threading in CANLayer
* removed bitrate, added loopback to DriverInterface::init
* allow dcf_overlay in defaults as well
* recursive merge of MergedXmlRpcStruct
* added dcf_overlay parameter
* Merge branch 'auto_scale' into indigo_dev
Conflicts:
canopen_chain_node/include/canopen_chain_node/chain_ros.h
* Merge remote-tracking branch 'ipa320/indigo_dev' into indigo_dev
Conflicts:
canopen_chain_node/include/canopen_chain_node/chain_ros.h
canopen_motor_node/src/control_node.cpp
* catch exceptions during master creation
* removed MasterType form template
* added master_type parameter
* 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
* added MergedXmlRpcStruct as replacement for read_xmlrpc_or_praram
* Contributors: Mathias Lüdtke, 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_chain_ros to canopen_chain_node
* Contributors: Florian Weisshardt, Mathias Lüdtke

114
Devices/Libraries/Ros/ros_canopen/canopen_chain_node/CMakeLists.txt Executable file
View File

@@ -0,0 +1,114 @@
cmake_minimum_required(VERSION 3.0.2)
project(canopen_chain_node)
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_STANDARD 14)
endif()
find_package(catkin REQUIRED
COMPONENTS
canopen_master
diagnostic_updater
message_generation
pluginlib
rosconsole_bridge
roscpp
roslib
socketcan_interface
std_msgs
std_srvs
)
find_package(Boost REQUIRED
COMPONENTS
filesystem
)
add_service_files(DIRECTORY srv
FILES
GetObject.srv
SetObject.srv)
generate_messages(DEPENDENCIES)
catkin_package(
INCLUDE_DIRS
include
LIBRARIES
canopen_ros_chain
CATKIN_DEPENDS
canopen_master
diagnostic_updater
message_runtime
pluginlib
rosconsole_bridge
roscpp
socketcan_interface
std_srvs
DEPENDS
Boost
)
include_directories(
include
${Boost_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
)
# canopen_ros_chain
add_library(canopen_ros_chain
src/ros_chain.cpp
src/rosconsole_bridge.cpp
)
target_link_libraries(canopen_ros_chain
${Boost_LIBRARIES}
${catkin_LIBRARIES}
)
add_dependencies(canopen_ros_chain
${catkin_EXPORTED_TARGETS}
${${PROJECT_NAME}_EXPORTED_TARGETS}
)
# canopen_chain_node
add_executable(${PROJECT_NAME}
src/chain_node.cpp
)
target_link_libraries(${PROJECT_NAME}
canopen_ros_chain
${Boost_LIBRARIES}
${catkin_LIBRARIES}
)
# canopen_sync_node
add_executable(canopen_sync_node
src/rosconsole_bridge.cpp
src/sync_node.cpp
)
target_link_libraries(canopen_sync_node
${Boost_LIBRARIES}
${catkin_LIBRARIES}
)
install(
TARGETS
${PROJECT_NAME}
canopen_ros_chain
canopen_sync_node
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(
DIRECTORY
include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
)
install(
DIRECTORY
launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

215
Devices/Libraries/Ros/ros_canopen/canopen_chain_node/include/canopen_chain_node/ros_chain.h Executable file
View File

@@ -0,0 +1,215 @@
#ifndef H_CANOPEN_ROS_CHAIN
#define H_CANOPEN_ROS_CHAIN
#include <memory>
#include <canopen_master/canopen.h>
#include <canopen_master/can_layer.h>
#include <canopen_chain_node/GetObject.h>
#include <canopen_chain_node/SetObject.h>
#include <socketcan_interface/string.h>
#include <ros/ros.h>
#include <std_srvs/Trigger.h>
#include <diagnostic_updater/diagnostic_updater.h>
#include <boost/filesystem/path.hpp>
#include <pluginlib/class_loader.hpp>
namespace canopen{
typedef std::function<void()> PublishFuncType;
PublishFuncType createPublishFunc(ros::NodeHandle &nh, const std::string &name, canopen::NodeSharedPtr node, const std::string &key, bool force);
class MergedXmlRpcStruct : public XmlRpc::XmlRpcValue{
MergedXmlRpcStruct(const XmlRpc::XmlRpcValue& a) :XmlRpc::XmlRpcValue(a){ assertStruct(); }
public:
MergedXmlRpcStruct(){ assertStruct(); }
MergedXmlRpcStruct(const XmlRpc::XmlRpcValue& a, const MergedXmlRpcStruct &b, bool recursive= true) :XmlRpc::XmlRpcValue(a){
assertStruct();
for(ValueStruct::const_iterator it = b._value.asStruct->begin(); it != b._value.asStruct->end(); ++it){
std::pair<XmlRpc::XmlRpcValue::iterator,bool> res = _value.asStruct->insert(*it);
if(recursive && !res.second && res.first->second.getType() == XmlRpc::XmlRpcValue::TypeStruct && it->second.getType() == XmlRpc::XmlRpcValue::TypeStruct){
res.first->second = MergedXmlRpcStruct(res.first->second, it->second); // recursive struct merge with implicit cast
}
}
}
};
class Logger: public DiagGroup<canopen::Layer>{
const canopen::NodeSharedPtr node_;
std::vector<std::function< void (diagnostic_updater::DiagnosticStatusWrapper &)> > entries_;
static void log_entry(diagnostic_updater::DiagnosticStatusWrapper &stat, uint8_t level, const std::string &name, std::function<std::string()> getter){
if(stat.level >= level){
try{
stat.add(name, getter());
}catch(...){
stat.add(name, "<ERROR>");
}
}
}
public:
Logger(canopen::NodeSharedPtr node): node_(node) { add(node_); }
bool add(uint8_t level, const std::string &key, bool forced){
try{
ObjectDict::Key k(key);
const ObjectDict::EntryConstSharedPtr entry = node_->getStorage()->dict_->get(k);
std::string name = entry->desc.empty() ? key : entry->desc;
entries_.push_back(std::bind(log_entry, std::placeholders::_1, level, name, node_->getStorage()->getStringReader(k, !forced)));
return true;
}
catch(std::exception& e){
ROS_ERROR_STREAM(boost::diagnostic_information(e));
return false;
}
}
template<typename T> void add(const std::shared_ptr<T> &n){
DiagGroup::add(std::static_pointer_cast<canopen::Layer>(n));
}
virtual void log(diagnostic_updater::DiagnosticStatusWrapper &stat){
if(node_->getState() == canopen::Node::Unknown){
stat.summary(stat.WARN,"Not initialized");
}else{
LayerReport r;
diag(r);
if(r.bounded<LayerStatus::Unbounded>()){ // valid
stat.summary(r.get(), r.reason());
for(std::vector<std::pair<std::string, std::string> >::const_iterator it = r.values().begin(); it != r.values().end(); ++it){
stat.add(it->first, it->second);
}
for(size_t i=0; i < entries_.size(); ++i) entries_[i](stat);
}
}
}
virtual ~Logger() {}
};
typedef std::shared_ptr<Logger> LoggerSharedPtr;
class GuardedClassLoaderList {
public:
typedef std::shared_ptr<pluginlib::ClassLoaderBase> ClassLoaderBaseSharedPtr;
static void addLoader(ClassLoaderBaseSharedPtr b){
guarded_loaders().push_back(b);
}
~GuardedClassLoaderList(){
guarded_loaders().clear();
}
private:
static std::vector< ClassLoaderBaseSharedPtr>& guarded_loaders(){
static std::vector<ClassLoaderBaseSharedPtr> loaders;
return loaders;
}
};
template<typename T> class GuardedClassLoader {
typedef pluginlib::ClassLoader<T> Loader;
std::shared_ptr<Loader> loader_;
public:
typedef std::shared_ptr<T> ClassSharedPtr;
GuardedClassLoader(const std::string& package, const std::string& allocator_base_class)
: loader_(new Loader(package, allocator_base_class)) {
GuardedClassLoaderList::addLoader(loader_);
}
ClassSharedPtr createInstance(const std::string& lookup_name){
return loader_->createUniqueInstance(lookup_name);
}
};
template<typename T> class ClassAllocator : public GuardedClassLoader<typename T::Allocator> {
public:
typedef std::shared_ptr<T> ClassSharedPtr;
ClassAllocator (const std::string& package, const std::string& allocator_base_class) : GuardedClassLoader<typename T::Allocator>(package, allocator_base_class) {}
template<typename T1> ClassSharedPtr allocateInstance(const std::string& lookup_name, const T1 & t1){
return this->createInstance(lookup_name)->allocate(t1);
}
template<typename T1, typename T2> ClassSharedPtr allocateInstance(const std::string& lookup_name, const T1 & t1, const T2 & t2){
return this->createInstance(lookup_name)->allocate(t1, t2);
}
template<typename T1, typename T2, typename T3> ClassSharedPtr allocateInstance(const std::string& lookup_name, const T1 & t1, const T2 & t2, const T3 & t3){
return this->createInstance(lookup_name)->allocate(t1, t2, t3);
}
};
class RosChain : GuardedClassLoaderList, public canopen::LayerStack {
private:
GuardedClassLoader<can::DriverInterface> driver_loader_;
ClassAllocator<canopen::Master> master_allocator_;
bool setup_node(const XmlRpc::XmlRpcValue &params, const std::string& name, const MergedXmlRpcStruct &defaults);
protected:
can::DriverInterfaceSharedPtr interface_;
MasterSharedPtr master_;
std::shared_ptr<canopen::LayerGroupNoDiag<canopen::Node> > nodes_;
std::shared_ptr<canopen::LayerGroupNoDiag<canopen::EMCYHandler> > emcy_handlers_;
std::map<std::string, canopen::NodeSharedPtr > nodes_lookup_;
canopen::SyncLayerSharedPtr sync_;
std::vector<LoggerSharedPtr > loggers_;
std::vector<PublishFuncType> publishers_;
can::StateListenerConstSharedPtr state_listener_;
std::unique_ptr<boost::thread> thread_;
ros::NodeHandle nh_;
ros::NodeHandle nh_priv_;
diagnostic_updater::Updater diag_updater_;
ros::Timer diag_timer_;
boost::mutex mutex_;
ros::ServiceServer srv_init_;
ros::ServiceServer srv_recover_;
ros::ServiceServer srv_halt_;
ros::ServiceServer srv_shutdown_;
ros::ServiceServer srv_get_object_;
ros::ServiceServer srv_set_object_;
time_duration update_duration_;
struct HeartbeatSender{
can::Frame frame;
can::DriverInterfaceSharedPtr interface;
bool send(){
return interface && interface->send(frame);
}
} hb_sender_;
Timer heartbeat_timer_;
std::atomic<bool> running_;
boost::mutex diag_mutex_;
bool reset_errors_before_recover_;
void logState(const can::State &s);
void run();
virtual bool handle_init(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res);
virtual bool handle_recover(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res);
virtual void handleWrite(LayerStatus &status, const LayerState &current_state);
virtual void handleShutdown(LayerStatus &status);
virtual bool handle_shutdown(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res);
virtual bool handle_halt(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res);
bool handle_get_object(canopen_chain_node::GetObject::Request &req, canopen_chain_node::GetObject::Response &res);
bool handle_set_object(canopen_chain_node::SetObject::Request &req, canopen_chain_node::SetObject::Response &res);
bool setup_bus();
bool setup_sync();
bool setup_heartbeat();
bool setup_nodes();
virtual bool nodeAdded(XmlRpc::XmlRpcValue &params, const canopen::NodeSharedPtr &node, const LoggerSharedPtr &logger);
void report_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat);
virtual bool setup_chain();
public:
RosChain(const ros::NodeHandle &nh, const ros::NodeHandle &nh_priv);
bool setup();
virtual ~RosChain();
};
} //namespace canopen
#endif

6
Devices/Libraries/Ros/ros_canopen/canopen_chain_node/launch/chain.launch Executable file
View File

@@ -0,0 +1,6 @@
<launch>
<arg name="yaml"/>
<node name="canopen_chain" pkg="canopen_chain_node" type="canopen_chain_node" output="screen" clear_params="true">
<rosparam command="load" file="$(arg yaml)" />
</node>
</launch>

36
Devices/Libraries/Ros/ros_canopen/canopen_chain_node/package.xml Executable file
View File

@@ -0,0 +1,36 @@
<?xml version="1.0"?>
<package format="2">
<name>canopen_chain_node</name>
<version>0.8.5</version>
<description>Base implementation for CANopen chains node with support for management services and diagnostics</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_chain_node</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>
<build_depend>message_generation</build_depend>
<build_export_depend>message_runtime</build_export_depend>
<exec_depend>message_runtime</exec_depend>
<depend>libboost-dev</depend>
<depend>libboost-filesystem-dev</depend>
<depend>canopen_master</depend>
<depend>diagnostic_updater</depend>
<depend>pluginlib</depend>
<depend>rosconsole_bridge</depend>
<depend>roscpp</depend>
<depend>roslib</depend>
<depend>socketcan_interface</depend> <!-- direct dependency needed for pluginlib look-up -->
<depend>std_srvs</depend>
<build_depend>std_msgs</build_depend>
<exec_depend>std_msgs</exec_depend>
</package>

21
Devices/Libraries/Ros/ros_canopen/canopen_chain_node/src/chain_node.cpp Executable file
View File

@@ -0,0 +1,21 @@
#include <socketcan_interface/dispatcher.h>
#include <socketcan_interface/socketcan.h>
#include <canopen_chain_node/ros_chain.h>
using namespace can;
using namespace canopen;
int main(int argc, char** argv){
ros::init(argc, argv, "canopen_chain_node_node");
ros::NodeHandle nh;
ros::NodeHandle nh_priv("~");
RosChain chain(nh, nh_priv);
if(!chain.setup()){
return 1;
}
ros::spin();
return 0;
}

644
Devices/Libraries/Ros/ros_canopen/canopen_chain_node/src/ros_chain.cpp Executable file
View File

@@ -0,0 +1,644 @@
#include <ros/package.h>
#include <socketcan_interface/xmlrpc_settings.h>
#include <canopen_chain_node/ros_chain.h>
#include <std_msgs/Int8.h>
#include <std_msgs/Int16.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Int64.h>
#include <std_msgs/UInt8.h>
#include <std_msgs/UInt16.h>
#include <std_msgs/UInt32.h>
#include <std_msgs/UInt64.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float64.h>
#include <std_msgs/String.h>
namespace canopen {
template<typename Tpub, int dt>
static PublishFuncType create(ros::NodeHandle &nh, const std::string &name, ObjectStorageSharedPtr storage, const std::string &key, const bool force){
using data_type = typename ObjectStorage::DataType<dt>::type;
using entry_type = ObjectStorage::Entry<data_type>;
entry_type entry = storage->entry<data_type>(key);
if(!entry.valid()) return 0;
const ros::Publisher pub = nh.advertise<Tpub>(name, 1);
typedef const data_type(entry_type::*getter_type)(void);
const getter_type getter = force ? static_cast<getter_type>(&entry_type::get) : static_cast<getter_type>(&entry_type::get_cached);
return [force, pub, entry, getter] () mutable {
Tpub msg;
msg.data = (const typename Tpub::_data_type &) (entry.*getter)();
pub.publish(msg);
};
}
PublishFuncType createPublishFunc(ros::NodeHandle &nh, const std::string &name, canopen::NodeSharedPtr node, const std::string &key, bool force){
ObjectStorageSharedPtr s = node->getStorage();
switch(ObjectDict::DataTypes(s->dict_->get(key)->data_type)){
case ObjectDict::DEFTYPE_INTEGER8: return create< std_msgs::Int8, ObjectDict::DEFTYPE_INTEGER8 >(nh, name, s, key, force);
case ObjectDict::DEFTYPE_INTEGER16: return create< std_msgs::Int16, ObjectDict::DEFTYPE_INTEGER16 >(nh, name, s, key, force);
case ObjectDict::DEFTYPE_INTEGER32: return create< std_msgs::Int32, ObjectDict::DEFTYPE_INTEGER32 >(nh, name, s, key, force);
case ObjectDict::DEFTYPE_INTEGER64: return create< std_msgs::Int64, ObjectDict::DEFTYPE_INTEGER64 >(nh, name, s, key, force);
case ObjectDict::DEFTYPE_UNSIGNED8: return create< std_msgs::UInt8, ObjectDict::DEFTYPE_UNSIGNED8 >(nh, name, s, key, force);
case ObjectDict::DEFTYPE_UNSIGNED16: return create< std_msgs::UInt16, ObjectDict::DEFTYPE_UNSIGNED16 >(nh, name, s, key, force);
case ObjectDict::DEFTYPE_UNSIGNED32: return create< std_msgs::UInt32, ObjectDict::DEFTYPE_UNSIGNED32 >(nh, name, s, key, force);
case ObjectDict::DEFTYPE_UNSIGNED64: return create< std_msgs::UInt64, ObjectDict::DEFTYPE_UNSIGNED64 >(nh, name, s, key, force);
case ObjectDict::DEFTYPE_REAL32: return create< std_msgs::Float32, ObjectDict::DEFTYPE_REAL32 >(nh, name, s, key, force);
case ObjectDict::DEFTYPE_REAL64: return create< std_msgs::Float64, ObjectDict::DEFTYPE_REAL64 >(nh, name, s, key, force);
case ObjectDict::DEFTYPE_VISIBLE_STRING: return create< std_msgs::String, ObjectDict::DEFTYPE_VISIBLE_STRING >(nh, name, s, key, force);
case ObjectDict::DEFTYPE_OCTET_STRING: return create< std_msgs::String, ObjectDict::DEFTYPE_DOMAIN >(nh, name, s, key, force);
case ObjectDict::DEFTYPE_UNICODE_STRING: return create< std_msgs::String, ObjectDict::DEFTYPE_UNICODE_STRING >(nh, name, s, key, force);
case ObjectDict::DEFTYPE_DOMAIN: return create< std_msgs::String, ObjectDict::DEFTYPE_DOMAIN >(nh, name, s, key, force);
default: return 0;
}
}
void RosChain::logState(const can::State &s){
can::DriverInterfaceSharedPtr interface = interface_;
std::string msg;
if(interface && !interface->translateError(s.internal_error, msg)) msg = "Undefined"; ;
ROS_INFO_STREAM("Current state: " << s.driver_state << " device error: " << s.error_code << " internal_error: " << s.internal_error << " (" << msg << ")");
}
void RosChain::run(){
running_ = true;
time_point abs_time = boost::chrono::high_resolution_clock::now();
while(running_){
LayerStatus s;
try{
read(s);
write(s);
if(!s.bounded<LayerStatus::Warn>()) ROS_ERROR_STREAM_THROTTLE(10, s.reason());
else if(!s.bounded<LayerStatus::Ok>()) ROS_WARN_STREAM_THROTTLE(10, s.reason());
}
catch(const canopen::Exception& e){
ROS_ERROR_STREAM_THROTTLE(1, boost::diagnostic_information(e));
}
if(!sync_){
abs_time += update_duration_;
boost::this_thread::sleep_until(abs_time);
}
}
}
template <typename T> class ResponseLogger {
protected:
bool logged;
const T& res;
const std::string command;
public:
ResponseLogger(const T& res, const std::string &command) : logged(false), res(res), command(command){}
~ResponseLogger() {
if(!logged && !res.success){
if (res.message.empty()){
ROS_ERROR_STREAM(command << " failed");
}else{
ROS_ERROR_STREAM(command << " failed: " << res.message);
}
logged = true;
}
}
};
class TriggerResponseLogger: public ResponseLogger<std_srvs::Trigger::Response> {
public:
TriggerResponseLogger(const std_srvs::Trigger::Response& res, const std::string &command) : ResponseLogger(res, command){
ROS_INFO_STREAM(command << "...");
}
void logWarning() {
ROS_WARN_STREAM(command << " successful with warning(s): " << res.message);
logged = true;
}
~TriggerResponseLogger() {
if(!logged && res.success){
if (res.message.empty()){
ROS_INFO_STREAM(command << " successful");
}else{
ROS_INFO_STREAM(command << " successful: " << res.message);
}
logged = true;
}
}
};
bool RosChain::handle_init(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res){
TriggerResponseLogger rl(res, "Initializing");
boost::mutex::scoped_lock lock(mutex_);
if(getLayerState() > Off){
res.success = true;
res.message = "already initialized";
return true;
}
thread_.reset(new boost::thread(&RosChain::run, this));
LayerReport status;
try{
init(status);
res.success = status.bounded<LayerStatus::Ok>();
res.message = status.reason();
if(!status.bounded<LayerStatus::Warn>()){
diag(status);
res.message = status.reason();
}else{
heartbeat_timer_.restart();
return true;
}
}
catch( const std::exception &e){
std::string info = boost::diagnostic_information(e);
ROS_ERROR_STREAM(info);
res.message = info;
status.error(res.message);
}
catch(...){
res.message = "Unknown exception";
status.error(res.message);
}
res.success = false;
shutdown(status);
return true;
}
bool RosChain::handle_recover(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res){
TriggerResponseLogger rl(res, "Recovering");
boost::mutex::scoped_lock lock(mutex_);
res.success = false;
if(getLayerState() > Init){
LayerReport status;
try{
if(!reset_errors_before_recover_ || emcy_handlers_->callFunc<LayerStatus::Warn>(&EMCYHandler::resetErrors, status)){
recover(status);
}
if(!status.bounded<LayerStatus::Warn>()){
diag(status);
}
res.success = status.bounded<LayerStatus::Warn>();
res.message = status.reason();
if(status.equals<LayerStatus::Warn>()) rl.logWarning();
}
catch( const std::exception &e){
std::string info = boost::diagnostic_information(e);
ROS_ERROR_STREAM(info);
res.message = info;
}
catch(...){
res.message = "Unknown exception";
}
}else{
res.message = "not running";
}
return true;
}
void RosChain::handleWrite(LayerStatus &status, const LayerState &current_state) {
LayerStack::handleWrite(status, current_state);
if(current_state > Shutdown){
for(const PublishFuncType& func: publishers_) func();
}
}
void RosChain::handleShutdown(LayerStatus &status){
boost::mutex::scoped_lock lock(diag_mutex_);
heartbeat_timer_.stop();
LayerStack::handleShutdown(status);
if(running_){
running_ = false;
thread_->interrupt();
thread_->join();
thread_.reset();
}
}
bool RosChain::handle_shutdown(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res){
TriggerResponseLogger rl(res, "Shutting down");
boost::mutex::scoped_lock lock(mutex_);
res.success = true;
if(getLayerState() > Init){
LayerStatus s;
halt(s);
shutdown(s);
}else{
res.message = "not running";
}
return true;
}
bool RosChain::handle_halt(std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res){
TriggerResponseLogger rl(res, "Halting down");
boost::mutex::scoped_lock lock(mutex_);
res.success = true;
if(getLayerState() > Init){
LayerStatus s;
halt(s);
}else{
res.message = "not running";
}
return true;
}
bool RosChain::handle_get_object(canopen_chain_node::GetObject::Request &req, canopen_chain_node::GetObject::Response &res){
ResponseLogger<canopen_chain_node::GetObject::Response> rl(res, "Getting object " + req.node);
std::map<std::string, canopen::NodeSharedPtr >::iterator it = nodes_lookup_.find(req.node);
if(it == nodes_lookup_.end()){
res.message = "node not found";
}else{
try {
res.value = it->second->getStorage()->getStringReader(canopen::ObjectDict::Key(req.object), req.cached)();
res.success = true;
} catch(std::exception& e) {
res.message = boost::diagnostic_information(e);
}
}
return true;
}
bool RosChain::handle_set_object(canopen_chain_node::SetObject::Request &req, canopen_chain_node::SetObject::Response &res){
ResponseLogger<canopen_chain_node::SetObject::Response> rl(res, "Setting object " + req.node);
std::map<std::string, canopen::NodeSharedPtr >::iterator it = nodes_lookup_.find(req.node);
if(it == nodes_lookup_.end()){
res.message = "node not found";
}else{
try {
it->second->getStorage()->getStringWriter(canopen::ObjectDict::Key(req.object), req.cached)(req.value);
res.success = true;
} catch(std::exception& e) {
res.message = boost::diagnostic_information(e);
}
}
return true;
}
bool RosChain::setup_bus(){
ros::NodeHandle bus_nh(nh_priv_,"bus");
std::string can_device;
std::string driver_plugin;
std::string master_alloc;
bool loopback;
if(!bus_nh.getParam("device",can_device)){
ROS_ERROR("Device not set");
return false;
}
bus_nh.param("loopback",loopback, false);
bus_nh.param("driver_plugin",driver_plugin, std::string("can::SocketCANInterface"));
try{
interface_ = driver_loader_.createInstance(driver_plugin);
}
catch(pluginlib::PluginlibException& ex){
ROS_ERROR_STREAM(ex.what());
return false;
}
state_listener_ = interface_->createStateListenerM(this, &RosChain::logState);
if(bus_nh.getParam("master_type",master_alloc)){
ROS_ERROR("please migrate to master allocators");
return false;
}
bus_nh.param("master_allocator",master_alloc, std::string("canopen::SimpleMaster::Allocator"));
try{
master_= master_allocator_.allocateInstance(master_alloc, can_device, interface_);
}
catch( const std::exception &e){
std::string info = boost::diagnostic_information(e);
ROS_ERROR_STREAM(info);
return false;
}
if(!master_){
ROS_ERROR_STREAM("Could not allocate master.");
return false;
}
add(std::make_shared<CANLayer>(interface_, can_device, loopback, XmlRpcSettings::create(bus_nh)));
return true;
}
bool RosChain::setup_sync(){
ros::NodeHandle sync_nh(nh_priv_,"sync");
int sync_ms = 0;
int sync_overflow = 0;
if(!sync_nh.getParam("interval_ms", sync_ms)){
ROS_WARN("Sync interval was not specified, so sync is disabled per default");
}
if(sync_ms < 0){
ROS_ERROR_STREAM("Sync interval "<< sync_ms << " is invalid");
return false;
}
int update_ms = sync_ms;
if(sync_ms == 0) nh_priv_.getParam("update_ms", update_ms);
if(update_ms == 0){
ROS_ERROR_STREAM("Update interval "<< sync_ms << " is invalid");
return false;
}else{
update_duration_ = boost::chrono::milliseconds(update_ms);
}
if(sync_ms){
if(!sync_nh.getParam("overflow", sync_overflow)){
ROS_WARN("Sync overflow was not specified, so overflow is disabled per default");
}
if(sync_overflow == 1 || sync_overflow > 240){
ROS_ERROR_STREAM("Sync overflow "<< sync_overflow << " is invalid");
return false;
}
if(sync_nh.param("silence_us", 0) != 0){
ROS_WARN("silence_us is not supported anymore");
}
// TODO: parse header
sync_ = master_->getSync(SyncProperties(can::MsgHeader(0x80), sync_ms, sync_overflow));
if(!sync_ && sync_ms){
ROS_ERROR_STREAM("Initializing sync master failed");
return false;
}
add(sync_);
}
return true;
}
bool RosChain::setup_heartbeat(){
ros::NodeHandle hb_nh(nh_priv_,"heartbeat");
std::string msg;
double rate = 0;
bool got_any = hb_nh.getParam("msg", msg);
got_any = hb_nh.getParam("rate", rate) || got_any;
if( !got_any) return true; // nothing todo
if(rate <=0 ){
ROS_ERROR_STREAM("Rate '"<< rate << "' is invalid");
return false;
}
hb_sender_.frame = can::toframe(msg);
if(!hb_sender_.frame.isValid()){
ROS_ERROR_STREAM("Message '"<< msg << "' is invalid");
return false;
}
hb_sender_.interface = interface_;
heartbeat_timer_.start(std::bind(&HeartbeatSender::send, &hb_sender_), boost::chrono::duration<double>(1.0/rate), false);
return true;
}
std::pair<std::string, bool> parseObjectName(std::string obj_name){
size_t pos = obj_name.find('!');
bool force = pos != std::string::npos;
if(force) obj_name.erase(pos);
return std::make_pair(obj_name, force);
}
bool addLoggerEntries(XmlRpc::XmlRpcValue merged, const std::string param, uint8_t level, Logger &logger){
if(merged.hasMember(param)){
try{
XmlRpc::XmlRpcValue objs = merged[param];
for(int i = 0; i < objs.size(); ++i){
std::pair<std::string, bool> obj_name = parseObjectName(objs[i]);
if(!logger.add(level, obj_name.first, obj_name.second)){
ROS_ERROR_STREAM("Could not create logger for '" << obj_name.first << "'");
return false;
}
}
}
catch(...){
ROS_ERROR_STREAM("Could not parse " << param << " parameter");
return false;
}
}
return true;
}
bool RosChain::setup_nodes(){
nodes_.reset(new canopen::LayerGroupNoDiag<canopen::Node>("301 layer"));
add(nodes_);
emcy_handlers_.reset(new canopen::LayerGroupNoDiag<canopen::EMCYHandler>("EMCY layer"));
XmlRpc::XmlRpcValue nodes;
if(!nh_priv_.getParam("nodes", nodes)){
ROS_WARN("falling back to 'modules', please switch to 'nodes'");
nh_priv_.getParam("modules", nodes);
}
MergedXmlRpcStruct defaults;
nh_priv_.getParam("defaults", defaults);
if(nodes.getType() == XmlRpc::XmlRpcValue::TypeArray){
for(size_t i = 0; i < nodes.size(); ++i){
if(nodes[i].hasMember("name")){
if(!setup_node(nodes[i], nodes[i]["name"], defaults)) return false;
}else{
ROS_ERROR_STREAM("Node at list index " << i << " has no name");
return false;
}
}
}else{
for(XmlRpc::XmlRpcValue::iterator it = nodes.begin(); it != nodes.end(); ++it){
if(!setup_node(it->second, it->first, defaults)) return false;
}
}
return true;
}
bool RosChain::setup_node(const XmlRpc::XmlRpcValue& params, const std::string &name, const MergedXmlRpcStruct &defaults){
int node_id;
try{
node_id = params["id"];
}
catch(...){
ROS_ERROR_STREAM("Node '" << name << "' has no id");
return false;
}
MergedXmlRpcStruct merged(params, defaults);
if(!merged.hasMember("name")){
merged["name"]=name;
}
ObjectDict::Overlay overlay;
if(merged.hasMember("dcf_overlay")){
XmlRpc::XmlRpcValue dcf_overlay = merged["dcf_overlay"];
if(dcf_overlay.getType() != XmlRpc::XmlRpcValue::TypeStruct){
ROS_ERROR_STREAM("dcf_overlay is no struct");
return false;
}
for(XmlRpc::XmlRpcValue::iterator ito = dcf_overlay.begin(); ito!= dcf_overlay.end(); ++ito){
if(ito->second.getType() != XmlRpc::XmlRpcValue::TypeString){
ROS_ERROR_STREAM("dcf_overlay '" << ito->first << "' must be string");
return false;
}
overlay.push_back(ObjectDict::Overlay::value_type(ito->first, ito->second));
}
}
std::string eds;
try{
eds = (std::string) merged["eds_file"];
}
catch(...){
ROS_ERROR_STREAM("EDS path '" << eds << "' invalid");
return false;
}
try{
if(merged.hasMember("eds_pkg")){
std::string pkg = merged["eds_pkg"];
std::string p = ros::package::getPath(pkg);
if(p.empty()){
ROS_WARN_STREAM("Package '" << pkg << "' was not found");
}else{
eds = (boost::filesystem::path(p)/eds).make_preferred().native();;
}
}
}
catch(...){
}
ObjectDictSharedPtr dict = ObjectDict::fromFile(eds, overlay);
if(!dict){
ROS_ERROR_STREAM("EDS '" << eds << "' could not be parsed");
return false;
}
canopen::NodeSharedPtr node = std::make_shared<canopen::Node>(interface_, dict, node_id, sync_);
LoggerSharedPtr logger = std::make_shared<Logger>(node);
if(!nodeAdded(merged, node, logger)) return false;
if(!addLoggerEntries(merged, "log", diagnostic_updater::DiagnosticStatusWrapper::OK, *logger)) return false;
if(!addLoggerEntries(merged, "log_warn", diagnostic_updater::DiagnosticStatusWrapper::WARN, *logger)) return false;
if(!addLoggerEntries(merged, "log_error", diagnostic_updater::DiagnosticStatusWrapper::ERROR, *logger)) return false;
loggers_.push_back(logger);
diag_updater_.add(name, std::bind(&Logger::log, logger, std::placeholders::_1));
std::string node_name = std::string(merged["name"]);
if(merged.hasMember("publish")){
try{
XmlRpc::XmlRpcValue objs = merged["publish"];
for(int i = 0; i < objs.size(); ++i){
std::pair<std::string, bool> obj_name = parseObjectName(objs[i]);
PublishFuncType pub = createPublishFunc(nh_, node_name +"_"+obj_name.first, node, obj_name.first, obj_name.second);
if(!pub){
ROS_ERROR_STREAM("Could not create publisher for '" << obj_name.first << "'");
return false;
}
publishers_.push_back(pub);
}
}
catch(...){
ROS_ERROR("Could not parse publish parameter");
return false;
}
}
nodes_->add(node);
nodes_lookup_.insert(std::make_pair(node_name, node));
std::shared_ptr<canopen::EMCYHandler> emcy = std::make_shared<canopen::EMCYHandler>(interface_, node->getStorage());
emcy_handlers_->add(emcy);
logger->add(emcy);
return true;
}
bool RosChain::nodeAdded(XmlRpc::XmlRpcValue &params, const canopen::NodeSharedPtr &node, const LoggerSharedPtr &logger){
return true;
}
void RosChain::report_diagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat){
boost::mutex::scoped_lock lock(diag_mutex_);
LayerReport r;
if(getLayerState() == Off){
stat.summary(stat.WARN,"Not initialized");
}else if(!running_){
stat.summary(stat.ERROR,"Thread is not running");
}else{
diag(r);
if(r.bounded<LayerStatus::Unbounded>()){ // valid
stat.summary(r.get(), r.reason());
for(std::vector<std::pair<std::string, std::string> >::const_iterator it = r.values().begin(); it != r.values().end(); ++it){
stat.add(it->first, it->second);
}
}
}
}
RosChain::RosChain(const ros::NodeHandle &nh, const ros::NodeHandle &nh_priv)
: LayerStack("ROS stack"),driver_loader_("socketcan_interface", "can::DriverInterface"),
master_allocator_("canopen_master", "canopen::Master::Allocator"),
nh_(nh), nh_priv_(nh_priv),
diag_updater_(nh_,nh_priv_),
running_(false),
reset_errors_before_recover_(false){}
bool RosChain::setup(){
boost::mutex::scoped_lock lock(mutex_);
bool okay = setup_chain();
if(okay) add(emcy_handlers_);
return okay;
}
bool RosChain::setup_chain(){
std::string hw_id;
nh_priv_.param("hardware_id", hw_id, std::string("none"));
nh_priv_.param("reset_errors_before_recover", reset_errors_before_recover_, false);
diag_updater_.setHardwareID(hw_id);
diag_updater_.add("chain", this, &RosChain::report_diagnostics);
diag_timer_ = nh_.createTimer(ros::Duration(diag_updater_.getPeriod()/2.0),std::bind(&diagnostic_updater::Updater::update, &diag_updater_));
ros::NodeHandle nh_driver(nh_, "driver");
srv_init_ = nh_driver.advertiseService("init",&RosChain::handle_init, this);
srv_recover_ = nh_driver.advertiseService("recover",&RosChain::handle_recover, this);
srv_halt_ = nh_driver.advertiseService("halt",&RosChain::handle_halt, this);
srv_shutdown_ = nh_driver.advertiseService("shutdown",&RosChain::handle_shutdown, this);
srv_get_object_ = nh_driver.advertiseService("get_object",&RosChain::handle_get_object, this);
srv_set_object_ = nh_driver.advertiseService("set_object",&RosChain::handle_set_object, this);
return setup_bus() && setup_sync() && setup_heartbeat() && setup_nodes();
}
RosChain::~RosChain(){
try{
LayerStatus s;
halt(s);
shutdown(s);
}catch(...){ ROS_ERROR("CATCH"); }
}
}

2
Devices/Libraries/Ros/ros_canopen/canopen_chain_node/src/rosconsole_bridge.cpp Executable file
View File

@@ -0,0 +1,2 @@
#include <rosconsole_bridge/bridge.h>
REGISTER_ROSCONSOLE_BRIDGE;

101
Devices/Libraries/Ros/ros_canopen/canopen_chain_node/src/sync_node.cpp Executable file
View File

@@ -0,0 +1,101 @@
#include <canopen_master/bcm_sync.h>
#include <socketcan_interface/string.h>
#include <socketcan_interface/xmlrpc_settings.h>
#include <canopen_master/can_layer.h>
#include <ros/ros.h>
#include <diagnostic_updater/diagnostic_updater.h>
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();
}
void report_diagnostics(canopen::LayerStack &sync, diagnostic_updater::DiagnosticStatusWrapper &stat){
canopen::LayerReport r;
sync.read(r);
sync.diag(r);
if(sync.getLayerState() !=canopen::Layer::Off && r.bounded<canopen::LayerStatus::Unbounded>()){ // valid
stat.summary(r.get(), r.reason());
for(std::vector<std::pair<std::string, std::string> >::const_iterator it = r.values().begin(); it != r.values().end(); ++it){
stat.add(it->first, it->second);
}
if(!r.bounded<canopen::LayerStatus::Warn>()){
canopen::LayerStatus s;
sync.recover(s);
}
}else{
stat.summary(stat.WARN, "sync not initilized");
canopen::LayerStatus s;
sync.init(s);
}
}
int main(int argc, char** argv){
ros::init(argc, argv, "canopen_sync_node");
ros::NodeHandle nh;
ros::NodeHandle nh_priv("~");
ros::NodeHandle sync_nh(nh_priv, "sync");
int sync_ms;
if(!sync_nh.getParam("interval_ms", sync_ms) || sync_ms <=0){
ROS_ERROR_STREAM("Sync interval "<< sync_ms << " is invalid");
return 1;
}
int sync_overflow = 0;
if(!sync_nh.getParam("overflow", sync_overflow)){
ROS_WARN("Sync overflow was not specified, so overflow is disabled per default");
}
if(sync_overflow == 1 || sync_overflow > 240){
ROS_ERROR_STREAM("Sync overflow "<< sync_overflow << " is invalid");
return 1;
}
std::string can_device;
if(!nh_priv.getParam("bus/device",can_device)){
ROS_ERROR("Device not set");
return 1;
}
can::SocketCANDriverSharedPtr driver = std::make_shared<can::SocketCANDriver>();
canopen::SyncProperties sync_properties(can::MsgHeader(sync_nh.param("sync_id", 0x080)), sync_ms, sync_overflow);
std::shared_ptr<canopen::BCMsync> sync = std::make_shared<canopen::BCMsync>(can_device, driver, sync_properties);
std::vector<int> nodes;
if(sync_nh.getParam("monitored_nodes",nodes)){
sync->setMonitored(nodes);
}else{
std::string msg;
if(nh_priv.getParam("heartbeat/msg", msg)){
can::Frame f = can::toframe(msg);
if(f.isValid() && (f.id & ~canopen::BCMsync::ALL_NODES_MASK) == canopen::BCMsync::HEARTBEAT_ID){
nodes.push_back(f.id & canopen::BCMsync::ALL_NODES_MASK);
}
}
sync_nh.getParam("ignored_nodes",nodes);
sync->setIgnored(nodes);
}
canopen::LayerStack stack("SyncNodeLayer");
stack.add(std::make_shared<canopen::CANLayer>(driver, can_device, false, XmlRpcSettings::create(nh_priv, "bus")));
stack.add(sync);
diagnostic_updater::Updater diag_updater(nh, nh_priv);
diag_updater.setHardwareID(nh_priv.param("hardware_id", std::string("none")));
diag_updater.add("sync", std::bind(report_diagnostics,std::ref(stack), std::placeholders::_1));
ros::Timer diag_timer = nh.createTimer(ros::Duration(diag_updater.getPeriod()/2.0),std::bind(&diagnostic_updater::Updater::update, &diag_updater));
ros::spin();
return 0;
}

8
Devices/Libraries/Ros/ros_canopen/canopen_chain_node/srv/GetObject.srv Executable file
View File

@@ -0,0 +1,8 @@
string node
string object
bool cached
---
bool success
string message
string value

8
Devices/Libraries/Ros/ros_canopen/canopen_chain_node/srv/SetObject.srv Executable file
View File

@@ -0,0 +1,8 @@
string node
string object
string value
bool cached
---
bool success
string message

263
Devices/Libraries/Ros/ros_canopen/canopen_master/CHANGELOG.rst Executable file
View 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
View 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()

167
Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/bcm_sync.h Executable file
View File

@@ -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 &current_state) {
if(current_state > Init){
}
}
virtual void handleWrite(LayerStatus &status, const LayerState &current_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

115
Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/can_layer.h Executable file
View File

@@ -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 &current_state) {
if(current_state > Init){
if(!driver_->getState().isReady()) status.error("CAN not ready");
}
}
virtual void handleWrite(LayerStatus &status, const LayerState &current_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

331
Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/canopen.h Executable file
View File

@@ -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 &current_state);
virtual void handleWrite(LayerStatus &status, const LayerState &current_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 &current_state);
virtual void handleWrite(LayerStatus &status, const LayerState &current_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

33
Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/exceptions.h Executable file
View File

@@ -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
View 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 &current_state) = 0;
virtual void handleWrite(LayerStatus &status, const LayerState &current_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 &current_state) {
this->template call_or_fail(&Layer::read, &Layer::halt, status);
}
virtual void handleWrite(LayerStatus &status, const LayerState &current_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 &current_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

587
Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/objdict.h Executable file
View File

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

75
Devices/Libraries/Ros/ros_canopen/canopen_master/include/canopen_master/timer.h Executable file
View File

@@ -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
View 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
View 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
View 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
View 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 &current_state) {
if(current_state == Ready){
if(has_error_){
status.error("Node has emergency error");
}
}
}
void EMCYHandler::handleWrite(LayerStatus &status, const LayerState &current_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
View 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 &current_state) {
if(current_state > Init){
boost::this_thread::sleep_until(read_time_);
write_time_ += step_;
}
}
virtual void handleWrite(LayerStatus &status, const LayerState &current_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 &current_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 &current_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
View 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 &current_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 &current_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
View 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
View 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
View 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
View 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
View 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();
}

210
Devices/Libraries/Ros/ros_canopen/canopen_motor_node/CHANGELOG.rst Executable file
View File

@@ -0,0 +1,210 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package canopen_motor_node
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.8.5 (2020-09-22)
------------------
0.8.4 (2020-08-22)
------------------
* moved XmlRpcSettings to socketcan_interface
* 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)
------------------
0.8.1 (2019-07-14)
------------------
* Set C++ standard to c++14
* inherit LimitsHandle from LimitsHandleBase
* Contributors: Harsh Deshpande, Mathias Lüdtke
0.8.0 (2018-07-11)
------------------
* migrated to std::function and std::bind
* use std::isnan
* migrated to std::atomic
* migrated to std::unordered_map and std::unordered_set
* migrated to std pointers
* removed deprecated types
* introduced HandleLayerSharedPtr
* Contributors: Mathias Lüdtke
0.7.8 (2018-05-04)
------------------
0.7.7 (2018-05-04)
------------------
* added types for all function objects
* added types for all shared_ptrs
* error if muparser is not available
* address catkin_lint errors/warnings
* Contributors: Mathias Lüdtke
0.7.6 (2017-08-30)
------------------
0.7.5 (2017-05-29)
------------------
0.7.4 (2017-04-25)
------------------
* use portable boost::math::isnan
* Contributors: Mathias Lüdtke
0.7.3 (2017-04-25)
------------------
* use urdf::JointConstSharedPtr
* Contributors: Mathias Lüdtke
0.7.2 (2017-03-28)
------------------
0.7.1 (2017-03-20)
------------------
* Decouble RobotLayer by introducing HandleLayerBase
* Split layers into different headers and compile units
* do not call handleReadread in HandleLayer::handleRecover
this prevents a race condition, it is not needed anyway.
* protect ObjectVariables with mutex
* added test for norm function
* fix for joint limit handling
* introduced per-controller enforce_limits parameter
* implemented per-joint limits handling
* check if hardware interface matches mode
* implemented mixed-mode switching (`#197 <https://github.com/ipa-mdl/ros_canopen/issues/197>`_)
* introduced joint reference for *res_it
* Contributors: Mathias Lüdtke, Michael Stoll
0.7.0 (2016-12-13)
------------------
* multi-mode controllers are not supported (`#197 <https://github.com/ros-industrial/ros_canopen/issues/197>`_)
* Adaption to https://github.com/ros-controls/ros_control/commit/afaf9403d1daf6e7f0a93e4a06aa9695e2883632
* Contributors: Mathias Lüdtke, Michael Stoll
0.6.5 (2016-12-10)
------------------
* protect MotorChain setup with RosChain lock
* Merge pull request `#153 <https://github.com/ipa-mdl/ros_canopen/issues/153>`_ from ipa-mdl/deprecated-canswitch
deprecated canSwitch
* fix for issue `#171 <https://github.com/ipa-mdl/ros_canopen/issues/171>`_
* Merge pull request `#168 <https://github.com/ipa-mdl/ros_canopen/issues/168>`_ from ipa-mdl/state-filters
added filter chain for state values
* do not start driver if filter config fails
* added filter chain for state values
* log control period settings
* use update_period\_ for controll unless use_realtime_period is set true
* better initialize last_time\_
* removed canSwitch implementation, added compile-time check for prepareSwitch
* exit code for generic error should be 1, not -1
* styled and sorted CMakeLists.txt
* removed boilerplate comments
* indention
* reviewed exported dependencies
* styled and sorted package.xml
* update package URLs
* foward commands ony if enabled in doSwitch
* moved switch implemenation to non-RT prepareSwitch
* migrated to non-const prepareSwitch
* Splitted control_node.cpp into control_node.cpp, robot_layer.cpp and robot_layer.h
* renamed chain_ros.h to ros_chain.h, fixes `#126 <https://github.com/ipa-mdl/ros_canopen/issues/126>`_
* added strictness to service call, extend error message for doSwitch fails
* stop controllers that failed switching via service call
* stop all cotnroller joints if one failed to switch
* check for ready state before controller/mode switching
* improved init bevaviour:
* URDF is not read again (was not needed anyway=
* register interfaces only of first init
* remove unnecessary atomic reads
* halt motor if switch failed
* Fix for switching controllers with same mode
* More expressive comments for compile-time check
* Contributors: Mathias Lüdtke, Michael Stoll
0.6.4 (2015-07-03)
------------------
0.6.3 (2015-06-30)
------------------
* added motor prefix to allocator entry
* only register limit interfaces with actual limits
* added motor_layer settings
* Migrated to ClassAllocator helper
* do not run controller manager on shutdown
* migrated to motor plug-in
* working compile-time check
* reset commands without controllers to current value
* got rid of getModeMask
* added check for old unit factors
* added closing braces in default conversion strings
* forgot var_func assignment in constructor
* ensured UnitConverter access to factory is valid during lifetime
* add unit conversion based on muparser
* dependency on muparser
* Refer to ipa320/ros_control overlay
* migrated to new hwi switch interface
* atomic joint handle pointer
* test if mode is support, add No_Mode
* enabled limit enforcing again
* removed debug output
* Fixes https://github.com/ipa320/ros_canopen/issues/81
* Enforce limits and current_state necessary for writing
* Merge remote-tracking branch 'mdl/indigo_dev' into refactor_sm
Conflicts:
canopen_402/include/canopen_402/canopen_402.h
canopen_402/src/canopen_402/canopen_402.cpp
canopen_motor_node/src/control_node.cpp
* refactored Layer mechanisms
* Fixes crash for unitialized boost pointer for ``target_vel_`` and ``target_pos_``
* MotorChain is now a template
* early check if joint is listed in URDF
* introduced 'joint' parameter (defaults to 'name')
* 'modules' was renamed to 'nodes'
* 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
* compile-time check for ros_control notifyHardwareInterface supportcompü
* added driver_plugin parameter for pluginlib look-up
* implemented threading in CANLayer
* removed SimpleLayer, migrated to Layer
* Layer::pending and Layer::halt are now virtual pure as well
* * Eliminates Internal State conflict
* Treats exceptions inside the state machine
* keep loop running
* proper locking for hardware interface switch (might fix `#61 <https://github.com/ros-industrial/ros_canopen/issues/61>`_)
* Merge branch 'auto_scale' into indigo_dev
Conflicts:
canopen_chain_node/include/canopen_chain_node/chain_ros.h
* Merge remote-tracking branch 'ipa320/indigo_dev' into indigo_dev
Conflicts:
canopen_chain_node/include/canopen_chain_node/chain_ros.h
canopen_motor_node/src/control_node.cpp
* removed MasterType form template
* 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
* added unit factor parameter parsing
* Scale factor acquired from yaml file
* Contributors: Mathias Lüdtke, thiagodefreitas
0.6.2 (2014-12-18)
------------------
0.6.1 (2014-12-15)
------------------
* remove ipa_* and IPA_* prefixes
* fixed catkin_lint errors
* added descriptions and authors
* renamed ipa_canopen_motor_control to canopen_motor_node
* Contributors: Florian Weisshardt, Mathias Lüdtke

128
Devices/Libraries/Ros/ros_canopen/canopen_motor_node/CMakeLists.txt Executable file
View File

@@ -0,0 +1,128 @@
cmake_minimum_required(VERSION 3.0.2)
project(canopen_motor_node)
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_STANDARD 14)
endif()
find_package(catkin REQUIRED
COMPONENTS
canopen_402
canopen_chain_node
canopen_master
controller_manager
controller_manager_msgs
filters
hardware_interface
joint_limits_interface
roscpp
urdf
)
find_package(Boost REQUIRED
COMPONENTS
thread
)
find_package(PkgConfig)
pkg_check_modules(PC_MUPARSER QUIET muparser)
set(MUPARSER_DEFINITIONS ${PC_MUPARSER_CFLAGS_OTHER})
find_path(MUPARSER_INCLUDE_DIR muParser.h
HINTS ${PC_MUPARSER_INCLUDEDIR} ${PC_MUPARSER_INCLUDE_DIRS}
)
find_library(MUPARSER_LIBRARY NAMES muparser libmurser
HINTS ${PC_MUPARSER_LIBDIR} ${PC_MUPARSER_LIBRARY_DIRS} )
include(FindPackageHandleStandardArgs)
# handle the QUIETLY and REQUIRED arguments and set MUPARSER_FOUND to TRUE
# if all listed variables are TRUE
find_package_handle_standard_args(MUPARSER DEFAULT_MSG
MUPARSER_LIBRARY MUPARSER_INCLUDE_DIR)
mark_as_advanced(MUPARSER_INCLUDE_DIR MUPARSER_LIBRARY )
if(NOT ${MUPARSER_FOUND})
message(FATAL_ERROR "muparser library not found")
endif()
set(MUPARSER_LIBRARIES ${MUPARSER_LIBRARY} )
set(MUPARSER_INCLUDE_DIRS ${MUPARSER_INCLUDE_DIR} )
catkin_package(
INCLUDE_DIRS
include
LIBRARIES
canopen_motor
CATKIN_DEPENDS
canopen_402
canopen_chain_node
canopen_master
controller_manager
hardware_interface
joint_limits_interface
roscpp
urdf
DEPENDS
Boost
MUPARSER
)
include_directories(
include
${catkin_INCLUDE_DIRS}
${MUPARSER_INCLUDE_DIRS}
)
# canopen_motor
add_library(canopen_motor
src/controller_manager_layer.cpp
src/handle_layer.cpp
src/motor_chain.cpp
src/robot_layer.cpp
)
target_link_libraries(canopen_motor
${catkin_LIBRARIES}
${MUPARSER_LIBRARIES}
)
add_dependencies(canopen_motor
${catkin_EXPORTED_TARGETS}
)
# canopen_motor_node
add_executable(${PROJECT_NAME}
src/canopen_motor_chain_node.cpp
)
target_link_libraries(${PROJECT_NAME}
canopen_motor
${catkin_LIBRARIES}
)
add_dependencies(${PROJECT_NAME}
${catkin_EXPORTED_TARGETS}
)
install(
TARGETS
canopen_motor
${PROJECT_NAME}
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"
)
if(CATKIN_ENABLE_TESTING)
catkin_add_gtest(${PROJECT_NAME}-test_muparser
test/test_muparser.cpp
)
target_link_libraries(${PROJECT_NAME}-test_muparser
canopen_motor
${catkin_LIBRARIES}
${MUPARSER_LIBRARIES}
)
endif()

44
Devices/Libraries/Ros/ros_canopen/canopen_motor_node/include/canopen_motor_node/controller_manager_layer.h Executable file
View File

@@ -0,0 +1,44 @@
#ifndef CANOPEN_MOTOR_NODE_CONTROLLER_MANAGER_LAYER_H_
#define CANOPEN_MOTOR_NODE_CONTROLLER_MANAGER_LAYER_H_
#include <memory>
#include <ros/node_handle.h>
#include <atomic>
#include <canopen_master/canopen.h>
#include <canopen_motor_node/robot_layer.h>
// forward declarations
namespace controller_manager {
class ControllerManager;
}
namespace canopen {
class ControllerManagerLayer : public canopen::Layer {
std::shared_ptr<controller_manager::ControllerManager> cm_;
canopen::RobotLayerSharedPtr robot_;
ros::NodeHandle nh_;
canopen::time_point last_time_;
std::atomic<bool> recover_;
const ros::Duration fixed_period_;
public:
ControllerManagerLayer(const canopen::RobotLayerSharedPtr robot, const ros::NodeHandle &nh, const ros::Duration &fixed_period)
:Layer("ControllerManager"), robot_(robot), nh_(nh), fixed_period_(fixed_period) {
}
virtual void handleRead(canopen::LayerStatus &status, const LayerState &current_state);
virtual void handleWrite(canopen::LayerStatus &status, const LayerState &current_state);
virtual void handleDiag(canopen::LayerReport &report) { /* nothing to do */ }
virtual void handleHalt(canopen::LayerStatus &status) { /* nothing to do (?) */ }
virtual void handleInit(canopen::LayerStatus &status);
virtual void handleRecover(canopen::LayerStatus &status);
virtual void handleShutdown(canopen::LayerStatus &status);
};
} // namespace canopen
#endif /* CANOPEN_MOTOR_NODE_CONTROLLER_MANAGER_LAYER_H_ */

176
Devices/Libraries/Ros/ros_canopen/canopen_motor_node/include/canopen_motor_node/handle_layer.h Executable file
View File

@@ -0,0 +1,176 @@
#ifndef CANOPEN_MOTOR_NODE_HANDLE_LAYER_H_
#define CANOPEN_MOTOR_NODE_HANDLE_LAYER_H_
#include <memory>
#include <unordered_map>
#include <atomic>
#include <functional>
#include <boost/thread/mutex.hpp>
#include <ros/common.h> // for ROS_VERSION_MINIMUM
#if ROS_VERSION_MINIMUM(1, 15, 0)
#include <filters/filter_chain.hpp>
#else
#include <filters/filter_chain.h>
#endif
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <joint_limits_interface/joint_limits_interface.h>
#include <canopen_master/objdict.h>
#include <canopen_master/layer.h>
#include <canopen_402/base.h>
#include <canopen_motor_node/unit_converter.h>
#include <canopen_motor_node/handle_layer_base.h>
namespace canopen {
class LimitsHandleBase {
public:
virtual void enforce(const ros::Duration &period) = 0;
virtual void reset() = 0;
virtual ~LimitsHandleBase() = default;
};
typedef std::shared_ptr<LimitsHandleBase> LimitsHandleBaseSharedPtr;
class ObjectVariables {
const ObjectStorageSharedPtr storage_;
struct Getter {
std::shared_ptr<double> val_ptr;
std::function<bool(double&)> func;
bool operator ()() { return func(*val_ptr); }
template<typename T> Getter(const ObjectStorage::Entry<T> &entry): func(std::bind(&Getter::readObject<T>, entry, std::placeholders::_1)), val_ptr(new double) { }
template<typename T> static bool readObject(ObjectStorage::Entry<T> &entry, double &res){
T val;
if(!entry.get(val)) return false;
res = val;
return true;
}
operator double*() const { return val_ptr.get(); }
};
typedef std::unordered_map<ObjectDict::Key, Getter, ObjectDict::KeyHash> GetterMap;
GetterMap getters_;
boost::mutex mutex_;
public:
template<const uint16_t dt> static double* func(ObjectVariables &list, const canopen::ObjectDict::Key &key){
typedef typename ObjectStorage::DataType<dt>::type type;
return list.getters_.insert(std::make_pair(key, Getter(list.storage_->entry<type>(key)))).first->second;
}
ObjectVariables(const ObjectStorageSharedPtr storage) : storage_(storage) {}
bool sync(){
boost::mutex::scoped_lock lock(mutex_);
bool ok = true;
for(GetterMap::iterator it = getters_.begin(); it != getters_.end(); ++it){
ok = it->second() && ok;
}
return ok;
}
double * getVariable(const std::string &n) {
boost::mutex::scoped_lock lock(mutex_);
try{
if(n.find("obj") == 0){
canopen::ObjectDict::Key key(n.substr(3));
GetterMap::const_iterator it = getters_.find(key);
if(it != getters_.end()) return it->second;
return canopen::branch_type<ObjectVariables, double * (ObjectVariables &list, const canopen::ObjectDict::Key &k)>(storage_->dict_->get(key)->data_type)(*this, key);
}
}
catch( const std::exception &e){
ROS_ERROR_STREAM("Could not find variable '" << n << "', reason: " << boost::diagnostic_information(e));
}
return 0;
}
};
template<> inline double* ObjectVariables::func<canopen::ObjectDict::DEFTYPE_VISIBLE_STRING >(ObjectVariables &, const canopen::ObjectDict::Key &){ return 0; }
template<> inline double* ObjectVariables::func<canopen::ObjectDict::DEFTYPE_OCTET_STRING >(ObjectVariables &, const canopen::ObjectDict::Key &){ return 0; }
template<> inline double* ObjectVariables::func<canopen::ObjectDict::DEFTYPE_UNICODE_STRING >(ObjectVariables &, const canopen::ObjectDict::Key &){ return 0; }
template<> inline double* ObjectVariables::func<canopen::ObjectDict::DEFTYPE_DOMAIN >(ObjectVariables &, const canopen::ObjectDict::Key &){ return 0; }
class HandleLayer: public canopen::HandleLayerBase {
canopen::MotorBaseSharedPtr motor_;
double pos_, vel_, eff_;
double cmd_pos_, cmd_vel_, cmd_eff_;
ObjectVariables variables_;
std::unique_ptr<UnitConverter> conv_target_pos_, conv_target_vel_, conv_target_eff_;
std::unique_ptr<UnitConverter> conv_pos_, conv_vel_, conv_eff_;
filters::FilterChain<double> filter_pos_, filter_vel_, filter_eff_;
XmlRpc::XmlRpcValue options_;
hardware_interface::JointStateHandle jsh_;
hardware_interface::JointHandle jph_, jvh_, jeh_;
std::atomic<hardware_interface::JointHandle*> jh_;
std::atomic<bool> forward_command_;
typedef std::unordered_map< MotorBase::OperationMode,hardware_interface::JointHandle* > CommandMap;
CommandMap commands_;
template <typename T> hardware_interface::JointHandle* addHandle( T &iface, hardware_interface::JointHandle *jh, const std::vector<MotorBase::OperationMode> & modes){
bool supported = false;
for(size_t i=0; i < modes.size(); ++i){
if(motor_->isModeSupported(modes[i])){
supported = true;
break;
}
}
if(!supported) return 0;
iface.registerHandle(*jh);
for(size_t i=0; i < modes.size(); ++i){
commands_[modes[i]] = jh;
}
return jh;
}
bool select(const canopen::MotorBase::OperationMode &m);
std::vector<LimitsHandleBaseSharedPtr> limits_;
bool enable_limits_;
public:
HandleLayer(const std::string &name, const canopen::MotorBaseSharedPtr & motor, const canopen::ObjectStorageSharedPtr storage, XmlRpc::XmlRpcValue & options);
static double * assignVariable(const std::string &name, double * ptr, const std::string &req) { return name == req ? ptr : 0; }
CanSwitchResult canSwitch(const canopen::MotorBase::OperationMode &m);
bool switchMode(const canopen::MotorBase::OperationMode &m);
bool forwardForMode(const canopen::MotorBase::OperationMode &m);
void registerHandle(hardware_interface::JointStateInterface &iface){
iface.registerHandle(jsh_);
}
hardware_interface::JointHandle* registerHandle(hardware_interface::PositionJointInterface &iface,
const joint_limits_interface::JointLimits &limits,
const joint_limits_interface::SoftJointLimits *soft_limits = 0);
hardware_interface::JointHandle* registerHandle(hardware_interface::VelocityJointInterface &iface,
const joint_limits_interface::JointLimits &limits,
const joint_limits_interface::SoftJointLimits *soft_limits = 0);
hardware_interface::JointHandle* registerHandle(hardware_interface::EffortJointInterface &iface,
const joint_limits_interface::JointLimits &limits,
const joint_limits_interface::SoftJointLimits *soft_limits = 0);
void enforceLimits(const ros::Duration &period, bool reset);
void enableLimits(bool enable);
bool prepareFilters(canopen::LayerStatus &status);
private:
virtual void handleRead(canopen::LayerStatus &status, const LayerState &current_state);
virtual void handleWrite(canopen::LayerStatus &status, const LayerState &current_state);
virtual void handleInit(canopen::LayerStatus &status);
virtual void handleDiag(canopen::LayerReport &report) { /* nothing to do */ }
virtual void handleShutdown(canopen::LayerStatus &status) { /* nothing to do */ }
virtual void handleHalt(canopen::LayerStatus &status) { /* TODO */ }
virtual void handleRecover(canopen::LayerStatus &status) { /* nothing to do */ }
};
typedef std::shared_ptr<HandleLayer> HandleLayerSharedPtr;
} // namespace canopen
#endif /* INCLUDE_CANOPEN_MOTOR_NODE_HANDLE_LAYER_H_ */

45
Devices/Libraries/Ros/ros_canopen/canopen_motor_node/include/canopen_motor_node/handle_layer_base.h Executable file
View File

@@ -0,0 +1,45 @@
#ifndef CANOPEN_MOTOR_NODE_HANDLE_LAYER_BASE_H_
#define CANOPEN_MOTOR_NODE_HANDLE_LAYER_BASE_H_
#include <memory>
#include <string>
#include <canopen_master/layer.h>
namespace canopen {
class HandleLayerBase: public canopen::Layer{
public:
HandleLayerBase(const std::string &name) : Layer(name) {}
enum CanSwitchResult{
NotSupported,
NotReadyToSwitch,
ReadyToSwitch,
NoNeedToSwitch
};
virtual CanSwitchResult canSwitch(const canopen::MotorBase::OperationMode &m) = 0;
virtual bool switchMode(const canopen::MotorBase::OperationMode &m) = 0;
virtual bool forwardForMode(const canopen::MotorBase::OperationMode &m) = 0;
virtual void registerHandle(hardware_interface::JointStateInterface &iface) = 0;
virtual hardware_interface::JointHandle* registerHandle(hardware_interface::PositionJointInterface &iface,
const joint_limits_interface::JointLimits &limits,
const joint_limits_interface::SoftJointLimits *soft_limits = 0) = 0;
virtual hardware_interface::JointHandle* registerHandle(hardware_interface::VelocityJointInterface &iface,
const joint_limits_interface::JointLimits &limits,
const joint_limits_interface::SoftJointLimits *soft_limits = 0) = 0;
virtual hardware_interface::JointHandle* registerHandle(hardware_interface::EffortJointInterface &iface,
const joint_limits_interface::JointLimits &limits,
const joint_limits_interface::SoftJointLimits *soft_limits = 0) = 0;
virtual void enforceLimits(const ros::Duration &period, bool reset) = 0;
virtual void enableLimits(bool enable) = 0;
};
typedef std::shared_ptr<HandleLayerBase> HandleLayerBaseSharedPtr;
} // namespace canopen
#endif /* CANOPEN_MOTOR_NODE_HANDLE_LAYER_BASE_H_ */

33
Devices/Libraries/Ros/ros_canopen/canopen_motor_node/include/canopen_motor_node/motor_chain.h Executable file
View File

@@ -0,0 +1,33 @@
#ifndef CANOPEN_MOTOR_NODE_MOTOR_CHAIN_H_
#define CANOPEN_MOTOR_NODE_MOTOR_CHAIN_H_
#include <memory>
#include <ros/node_handle.h>
#include <canopen_chain_node/ros_chain.h>
#include <canopen_motor_node/robot_layer.h>
#include <canopen_motor_node/controller_manager_layer.h>
namespace canopen {
class MotorChain : public canopen::RosChain {
ClassAllocator<canopen::MotorBase> motor_allocator_;
std::shared_ptr< canopen::LayerGroupNoDiag<canopen::MotorBase> > motors_;
RobotLayerSharedPtr robot_layer_;
std::shared_ptr<ControllerManagerLayer> cm_;
virtual bool nodeAdded(XmlRpc::XmlRpcValue &params, const canopen::NodeSharedPtr &node, const LoggerSharedPtr &logger);
public:
MotorChain(const ros::NodeHandle &nh, const ros::NodeHandle &nh_priv);
virtual bool setup_chain();
};
} // namespace canopen
#endif /* INCLUDE_CANOPEN_MOTOR_NODE_MOTOR_CHAIN_H_ */

64
Devices/Libraries/Ros/ros_canopen/canopen_motor_node/include/canopen_motor_node/robot_layer.h Executable file
View File

@@ -0,0 +1,64 @@
#ifndef CANOPEN_MOTOR_NODE_ROBOT_LAYER_H_
#define CANOPEN_MOTOR_NODE_ROBOT_LAYER_H_
#include <unordered_map>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <joint_limits_interface/joint_limits_interface.h>
#include <hardware_interface/robot_hw.h>
#include <urdf/urdfdom_compatibility.h>
#include <urdf/model.h>
#include <canopen_402/base.h>
#include <canopen_motor_node/handle_layer_base.h>
namespace canopen {
class RobotLayer : public LayerGroupNoDiag<HandleLayerBase>, public hardware_interface::RobotHW{
hardware_interface::JointStateInterface state_interface_;
hardware_interface::PositionJointInterface pos_interface_;
hardware_interface::VelocityJointInterface vel_interface_;
hardware_interface::EffortJointInterface eff_interface_;
joint_limits_interface::PositionJointSoftLimitsInterface pos_soft_limits_interface_;
joint_limits_interface::PositionJointSaturationInterface pos_saturation_interface_;
joint_limits_interface::VelocityJointSoftLimitsInterface vel_soft_limits_interface_;
joint_limits_interface::VelocityJointSaturationInterface vel_saturation_interface_;
joint_limits_interface::EffortJointSoftLimitsInterface eff_soft_limits_interface_;
joint_limits_interface::EffortJointSaturationInterface eff_saturation_interface_;
ros::NodeHandle nh_;
urdf::Model urdf_;
typedef std::unordered_map< std::string, HandleLayerBaseSharedPtr > HandleMap;
HandleMap handles_;
struct SwitchData {
HandleLayerBaseSharedPtr handle;
canopen::MotorBase::OperationMode mode;
bool enforce_limits;
};
typedef std::vector<SwitchData> SwitchContainer;
typedef std::unordered_map<std::string, SwitchContainer> SwitchMap;
SwitchMap switch_map_;
std::atomic<bool> first_init_;
void stopControllers(const std::vector<std::string> controllers);
public:
void add(const std::string &name, HandleLayerBaseSharedPtr handle);
RobotLayer(ros::NodeHandle nh);
urdf::JointConstSharedPtr getJoint(const std::string &n) const { return urdf_.getJoint(n); }
virtual void handleInit(canopen::LayerStatus &status);
void enforce(const ros::Duration &period, bool reset);
virtual bool prepareSwitch(const std::list<hardware_interface::ControllerInfo> &start_list, const std::list<hardware_interface::ControllerInfo> &stop_list);
virtual void doSwitch(const std::list<hardware_interface::ControllerInfo> &start_list, const std::list<hardware_interface::ControllerInfo> &stop_list);
};
typedef std::shared_ptr<RobotLayer> RobotLayerSharedPtr;
} // namespace canopen
#endif

89
Devices/Libraries/Ros/ros_canopen/canopen_motor_node/include/canopen_motor_node/unit_converter.h Executable file
View File

@@ -0,0 +1,89 @@
#ifndef CANOPEN_MOTOR_NODE_UNIT_CONVERTER_H_
#define CANOPEN_MOTOR_NODE_UNIT_CONVERTER_H_
#include <string>
#include <list>
#include <memory>
#include <functional>
#include "muParser.h"
namespace canopen {
class UnitConverter{
public:
typedef std::function<double * (const std::string &) > GetVarFuncType;
UnitConverter(const std::string &expression, GetVarFuncType var_func)
: var_func_(var_func)
{
parser_.SetVarFactory(UnitConverter::createVariable, this);
parser_.DefineConst("pi", M_PI);
parser_.DefineConst("nan", std::numeric_limits<double>::quiet_NaN());
parser_.DefineFun("rad2deg", UnitConverter::rad2deg);
parser_.DefineFun("deg2rad", UnitConverter::deg2rad);
parser_.DefineFun("norm", UnitConverter::norm);
parser_.DefineFun("smooth", UnitConverter::smooth);
parser_.DefineFun("avg", UnitConverter::avg);
parser_.SetExpr(expression);
}
void reset(){
for(variable_ptr_list::iterator it = var_list_.begin(); it != var_list_.end(); ++it){
**it = std::numeric_limits<double>::quiet_NaN();
}
}
double evaluate() { int num; return parser_.Eval(num)[0]; }
private:
typedef std::shared_ptr<double> variable_ptr;
typedef std::list<variable_ptr> variable_ptr_list;
static double* createVariable(const char *name, void * userdata) {
UnitConverter * uc = static_cast<UnitConverter*>(userdata);
double *p = uc->var_func_ ? uc->var_func_(name) : 0;
if(!p){
p = new double(std::numeric_limits<double>::quiet_NaN());
uc->var_list_.push_back(variable_ptr(p));
}
return p;
}
variable_ptr_list var_list_;
GetVarFuncType var_func_;
mu::Parser parser_;
static double rad2deg(double r){
return r*180.0/M_PI;
}
static double deg2rad(double d){
return d*M_PI/180.0;
}
static double norm(double val, double min, double max){
while(val >= max) val -= (max-min);
while(val < min) val += (max-min);
return val;
}
static double smooth(double val, double old_val, double alpha){
if(std::isnan(val)) return 0;
if(std::isnan(old_val)) return val;
return alpha*val + (1.0-alpha)*old_val;
}
static double avg(const double *vals, int num)
{
double s = 0.0;
int i=0;
for (; i<num; ++i){
const double &val = vals[i];
if(std::isnan(val)) break;
s += val;
}
return s / double(i+1);
}
};
}
#endif /* CANOPEN_MOTOR_NODE_UNIT_CONVERTER_H_ */

36
Devices/Libraries/Ros/ros_canopen/canopen_motor_node/package.xml Executable file
View File

@@ -0,0 +1,36 @@
<?xml version="1.0"?>
<package format="2">
<name>canopen_motor_node</name>
<version>0.8.5</version>
<description>canopen_chain_node specialization for handling of canopen_402 motor devices. It facilitates interface abstraction with ros_control.</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_motor_node</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-thread-dev</depend>
<depend>canopen_402</depend>
<depend>canopen_chain_node</depend>
<depend>canopen_master</depend>
<depend>controller_manager</depend>
<depend>filters</depend>
<depend>hardware_interface</depend>
<depend>joint_limits_interface</depend>
<depend>muparser</depend>
<depend>roscpp</depend>
<depend>urdf</depend>
<build_depend>controller_manager_msgs</build_depend>
<exec_depend>controller_manager_msgs</exec_depend>
<test_depend>rosunit</test_depend>
</package>

22
Devices/Libraries/Ros/ros_canopen/canopen_motor_node/src/canopen_motor_chain_node.cpp Executable file
View File

@@ -0,0 +1,22 @@
#include <canopen_motor_node/motor_chain.h>
using namespace canopen;
int main(int argc, char** argv){
ros::init(argc, argv, "canopen_motor_chain_node");
ros::AsyncSpinner spinner(0);
spinner.start();
ros::NodeHandle nh;
ros::NodeHandle nh_priv("~");
MotorChain chain(nh, nh_priv);
if(!chain.setup()){
return 1;
}
ros::waitForShutdown();
return 0;
}

54
Devices/Libraries/Ros/ros_canopen/canopen_motor_node/src/controller_manager_layer.cpp Executable file
View File

@@ -0,0 +1,54 @@
#include <canopen_motor_node/controller_manager_layer.h>
#include <controller_manager/controller_manager.h>
using namespace canopen;
void ControllerManagerLayer::handleRead(canopen::LayerStatus &status, const LayerState &current_state) {
if(current_state > Shutdown){
if(!cm_) status.error("controller_manager is not intialized");
}
}
void ControllerManagerLayer::handleWrite(canopen::LayerStatus &status, const LayerState &current_state) {
if(current_state > Shutdown){
if(!cm_){
status.error("controller_manager is not intialized");
}else{
time_point abs_now = canopen::get_abs_time();
ros::Time now = ros::Time::now();
ros::Duration period = fixed_period_;
if(period.isZero()) {
period.fromSec(boost::chrono::duration<double>(abs_now -last_time_).count());
}
last_time_ = abs_now;
bool recover = recover_.exchange(false);
cm_->update(now, period, recover);
robot_->enforce(period, recover);
}
}
}
void ControllerManagerLayer::handleInit(canopen::LayerStatus &status) {
if(cm_){
status.warn("controller_manager is already intialized");
}else{
recover_ = true;
last_time_ = canopen::get_abs_time();
cm_.reset(new controller_manager::ControllerManager(robot_.get(), nh_));
}
}
void ControllerManagerLayer::handleRecover(canopen::LayerStatus &status) {
if(!cm_) status.error("controller_manager is not intialized");
else recover_ = true;
}
void ControllerManagerLayer::handleShutdown(canopen::LayerStatus &status) {
cm_.reset();
}

215
Devices/Libraries/Ros/ros_canopen/canopen_motor_node/src/handle_layer.cpp Executable file
View File

@@ -0,0 +1,215 @@
#include <canopen_motor_node/handle_layer.h>
#include "interface_mapping.h"
using namespace canopen;
template<typename T > class LimitsHandle : public LimitsHandleBase {
T limits_handle_;
public:
LimitsHandle(const T &handle) : limits_handle_(handle) {}
virtual void enforce(const ros::Duration &period) override { limits_handle_.enforceLimits(period); }
virtual void reset() override {}
};
template<> void LimitsHandle<joint_limits_interface::PositionJointSaturationHandle>::reset() { limits_handle_.reset(); }
template<>void LimitsHandle<joint_limits_interface::PositionJointSoftLimitsHandle>::reset() { limits_handle_.reset(); }
bool HandleLayer::select(const MotorBase::OperationMode &m){
CommandMap::iterator it = commands_.find(m);
if(it == commands_.end()) return false;
jh_ = it->second;
return true;
}
HandleLayer::HandleLayer(const std::string &name, const MotorBaseSharedPtr & motor, const ObjectStorageSharedPtr storage, XmlRpc::XmlRpcValue & options)
: HandleLayerBase(name + " Handle"), motor_(motor), variables_(storage), jsh_(name, &pos_, &vel_, &eff_), jph_(jsh_, &cmd_pos_), jvh_(jsh_, &cmd_vel_), jeh_(jsh_, &cmd_eff_), jh_(0), forward_command_(false),
filter_pos_("double"), filter_vel_("double"), filter_eff_("double"), options_(options), enable_limits_(true)
{
commands_[MotorBase::No_Mode] = 0;
std::string p2d("rint(rad2deg(pos)*1000)"), v2d("rint(rad2deg(vel)*1000)"), e2d("rint(eff)");
std::string p2r("deg2rad(obj6064)/1000"), v2r("deg2rad(obj606C)/1000"), e2r("0");
if(options.hasMember("pos_unit_factor") || options.hasMember("vel_unit_factor") || options.hasMember("eff_unit_factor")){
const std::string reason("*_unit_factor parameters are not supported anymore, please migrate to conversion functions.");
ROS_FATAL_STREAM(reason);
throw std::invalid_argument(reason);
}
if(options.hasMember("pos_to_device")) p2d = (const std::string&) options["pos_to_device"];
if(options.hasMember("pos_from_device")) p2r = (const std::string&) options["pos_from_device"];
if(options.hasMember("vel_to_device")) v2d = (const std::string&) options["vel_to_device"];
if(options.hasMember("vel_from_device")) v2r = (const std::string&) options["vel_from_device"];
if(options.hasMember("eff_to_device")) e2d = (const std::string&) options["eff_to_device"];
if(options.hasMember("eff_from_device")) e2r = (const std::string&) options["eff_from_device"];
conv_target_pos_.reset(new UnitConverter(p2d, std::bind(assignVariable, "pos", &cmd_pos_, std::placeholders::_1)));
conv_target_vel_.reset(new UnitConverter(v2d, std::bind(assignVariable, "vel", &cmd_vel_, std::placeholders::_1)));
conv_target_eff_.reset(new UnitConverter(e2d, std::bind(assignVariable, "eff", &cmd_eff_, std::placeholders::_1)));
conv_pos_.reset(new UnitConverter(p2r, std::bind(&ObjectVariables::getVariable, &variables_, std::placeholders::_1)));
conv_vel_.reset(new UnitConverter(v2r, std::bind(&ObjectVariables::getVariable, &variables_, std::placeholders::_1)));
conv_eff_.reset(new UnitConverter(e2r, std::bind(&ObjectVariables::getVariable, &variables_, std::placeholders::_1)));
}
HandleLayer::CanSwitchResult HandleLayer::canSwitch(const MotorBase::OperationMode &m){
if(!motor_->isModeSupported(m) || commands_.find(m) == commands_.end()){
return NotSupported;
}else if(motor_->getMode() == m){
return NoNeedToSwitch;
}else if(motor_->getLayerState() == Ready){
return ReadyToSwitch;
}else{
return NotReadyToSwitch;
}
}
bool HandleLayer::switchMode(const MotorBase::OperationMode &m){
if(motor_->getMode() != m){
forward_command_ = false;
jh_ = 0; // disconnect handle
if(!motor_->enterModeAndWait(m)){
ROS_ERROR_STREAM(jsh_.getName() << "could not enter mode " << (int)m);
LayerStatus s;
motor_->halt(s);
return false;
}
}
return select(m);
}
bool HandleLayer::forwardForMode(const MotorBase::OperationMode &m){
if(motor_->getMode() == m){
forward_command_ = true;
return true;
}
return false;
}
template<typename T> void addLimitsHandle(std::vector<LimitsHandleBaseSharedPtr> &limits, const T &t) {
LimitsHandleBaseSharedPtr p = std::make_shared<LimitsHandle<T>>(t);
limits.push_back(p);
}
hardware_interface::JointHandle* HandleLayer::registerHandle(hardware_interface::PositionJointInterface &iface,
const joint_limits_interface::JointLimits &limits,
const joint_limits_interface::SoftJointLimits *soft_limits){
hardware_interface::JointHandle* h = addHandle(iface, &jph_, g_interface_mapping.getInterfaceModes("hardware_interface::PositionJointInterface"));
if(h && limits.has_position_limits){
addLimitsHandle(limits_, joint_limits_interface::PositionJointSaturationHandle(*h, limits));
if(soft_limits){
addLimitsHandle(limits_, joint_limits_interface::PositionJointSoftLimitsHandle(*h, limits, *soft_limits));
}
}
return h;
}
hardware_interface::JointHandle* HandleLayer::registerHandle(hardware_interface::VelocityJointInterface &iface,
const joint_limits_interface::JointLimits&limits,
const joint_limits_interface::SoftJointLimits *soft_limits){
hardware_interface::JointHandle* h = addHandle(iface, &jvh_, g_interface_mapping.getInterfaceModes("hardware_interface::VelocityJointInterface"));
if(h && limits.has_velocity_limits){
addLimitsHandle(limits_, joint_limits_interface::VelocityJointSaturationHandle(*h, limits));
if(soft_limits){
addLimitsHandle(limits_, joint_limits_interface::VelocityJointSoftLimitsHandle(*h, limits, *soft_limits));
}
}
return h;
}
hardware_interface::JointHandle* HandleLayer::registerHandle(hardware_interface::EffortJointInterface &iface,
const joint_limits_interface::JointLimits&limits,
const joint_limits_interface::SoftJointLimits *soft_limits){
hardware_interface::JointHandle* h = addHandle(iface, &jeh_, g_interface_mapping.getInterfaceModes("hardware_interface::EffortJointInterface"));
if(h && limits.has_effort_limits){
addLimitsHandle(limits_, joint_limits_interface::EffortJointSaturationHandle(*h, limits));
if(soft_limits){
addLimitsHandle(limits_, joint_limits_interface::EffortJointSoftLimitsHandle(*h, limits, *soft_limits));
}
}
return h;
}
void HandleLayer::handleRead(LayerStatus &status, const LayerState &current_state) {
if(current_state > Shutdown){
variables_.sync();
filter_pos_.update(conv_pos_->evaluate(), pos_);
filter_vel_.update(conv_vel_->evaluate(), vel_);
filter_eff_.update(conv_eff_->evaluate(), eff_);
}
}
void HandleLayer::handleWrite(LayerStatus &status, const LayerState &current_state) {
if(current_state == Ready){
hardware_interface::JointHandle* jh = 0;
if(forward_command_) jh = jh_;
if(jh == &jph_){
motor_->setTarget(conv_target_pos_->evaluate());
cmd_vel_ = vel_;
cmd_eff_ = eff_;
}else if(jh == &jvh_){
motor_->setTarget(conv_target_vel_->evaluate());
cmd_pos_ = pos_;
cmd_eff_ = eff_;
}else if(jh == &jeh_){
motor_->setTarget(conv_target_eff_->evaluate());
cmd_pos_ = pos_;
cmd_vel_ = vel_;
}else{
cmd_pos_ = pos_;
cmd_vel_ = vel_;
cmd_eff_ = eff_;
if(jh) status.warn("unsupported mode active");
}
}
}
bool prepareFilter(const std::string& joint_name, const std::string& filter_name, filters::FilterChain<double> &filter, XmlRpc::XmlRpcValue & options, canopen::LayerStatus &status){
filter.clear();
if(options.hasMember(filter_name)){
if(!filter.configure(options[filter_name],joint_name + "/" + filter_name)){
status.error("could not configure " + filter_name+ " for " + joint_name);
return false;
}
}
return true;
}
bool HandleLayer::prepareFilters(canopen::LayerStatus &status){
return prepareFilter(jsh_.getName(), "position_filters", filter_pos_, options_, status) &&
prepareFilter(jsh_.getName(), "velocity_filters", filter_vel_, options_, status) &&
prepareFilter(jsh_.getName(), "effort_filters", filter_eff_, options_, status);
}
void HandleLayer::handleInit(LayerStatus &status){
// TODO: implement proper init
conv_pos_->reset();
conv_vel_->reset();
conv_eff_->reset();
conv_target_pos_->reset();
conv_target_vel_->reset();
conv_target_eff_->reset();
if(prepareFilters(status))
{
handleRead(status, Layer::Ready);
}
}
void HandleLayer::enforceLimits(const ros::Duration &period, bool reset){
for(std::vector<LimitsHandleBaseSharedPtr>::iterator it = limits_.begin(); it != limits_.end(); ++it){
if(reset) (*it)->reset();
if(enable_limits_) (*it)->enforce(period);
}
}
void HandleLayer::enableLimits(bool enable){
enable_limits_ = enable;
}

48
Devices/Libraries/Ros/ros_canopen/canopen_motor_node/src/interface_mapping.h Executable file
View File

@@ -0,0 +1,48 @@
#ifndef INTERFACE_MAPPING_H_
#define INTERFACE_MAPPING_H_
#include <string>
#include <boost/bimap.hpp>
#include <boost/bimap/multiset_of.hpp>
#include <boost/foreach.hpp>
#include <canopen_402/base.h>
class InterfaceMapping {
typedef boost::bimap<boost::bimaps::multiset_of<std::string>, boost::bimaps::set_of<canopen::MotorBase::OperationMode> > bimap_type;
bimap_type mapping_;
public:
InterfaceMapping(){
mapping_.insert(bimap_type::value_type("hardware_interface::PositionJointInterface" ,canopen::MotorBase::Profiled_Position));
mapping_.insert(bimap_type::value_type("hardware_interface::PositionJointInterface" ,canopen::MotorBase::Interpolated_Position));
mapping_.insert(bimap_type::value_type("hardware_interface::PositionJointInterface" ,canopen::MotorBase::Cyclic_Synchronous_Position));
mapping_.insert(bimap_type::value_type("hardware_interface::VelocityJointInterface" ,canopen::MotorBase::Velocity));
mapping_.insert(bimap_type::value_type("hardware_interface::VelocityJointInterface" ,canopen::MotorBase::Profiled_Velocity));
mapping_.insert(bimap_type::value_type("hardware_interface::VelocityJointInterface" ,canopen::MotorBase::Cyclic_Synchronous_Velocity));
mapping_.insert(bimap_type::value_type("hardware_interface::EffortJointInterface" ,canopen::MotorBase::Profiled_Torque));
mapping_.insert(bimap_type::value_type("hardware_interface::EffortJointInterface" ,canopen::MotorBase::Cyclic_Synchronous_Torque));
}
std::vector<canopen::MotorBase::OperationMode> getInterfaceModes(const std::string &interface){
std::vector<canopen::MotorBase::OperationMode> modes;
BOOST_FOREACH(bimap_type::left_reference i, mapping_.left.equal_range(interface)){
modes.push_back(i.second);
}
return modes;
}
bool hasConflict(const std::string &interface, canopen::MotorBase::OperationMode mode){
bimap_type::right_const_iterator it;
if((it = mapping_.right.find(mode)) != mapping_.right.end()){
return it->second != interface;
}
return false;
}
};
extern InterfaceMapping g_interface_mapping;
#endif /* INTERFACE_MAPPING_H_ */

84
Devices/Libraries/Ros/ros_canopen/canopen_motor_node/src/motor_chain.cpp Executable file
View File

@@ -0,0 +1,84 @@
#include <canopen_motor_node/motor_chain.h>
#include <canopen_motor_node/handle_layer.h>
#include <socketcan_interface/xmlrpc_settings.h>
using namespace canopen;
MotorChain::MotorChain(const ros::NodeHandle &nh, const ros::NodeHandle &nh_priv) :
RosChain(nh, nh_priv), motor_allocator_("canopen_402", "canopen::MotorBase::Allocator") {}
bool MotorChain::nodeAdded(XmlRpc::XmlRpcValue &params, const canopen::NodeSharedPtr &node, const LoggerSharedPtr &logger)
{
std::string name = params["name"];
std::string &joint = name;
if(params.hasMember("joint")) joint.assign(params["joint"]);
if(!robot_layer_->getJoint(joint)){
ROS_ERROR_STREAM("joint " + joint + " was not found in URDF");
return false;
}
std::string alloc_name = "canopen::Motor402::Allocator";
if(params.hasMember("motor_allocator")) alloc_name.assign(params["motor_allocator"]);
XmlRpcSettings settings;
if(params.hasMember("motor_layer")) settings = params["motor_layer"];
MotorBaseSharedPtr motor;
try{
motor = motor_allocator_.allocateInstance(alloc_name, name + "_motor", node->getStorage(), settings);
}
catch( const std::exception &e){
std::string info = boost::diagnostic_information(e);
ROS_ERROR_STREAM(info);
return false;
}
if(!motor){
ROS_ERROR_STREAM("Could not allocate motor.");
return false;
}
motor->registerDefaultModes(node->getStorage());
motors_->add(motor);
logger->add(motor);
HandleLayerSharedPtr handle = std::make_shared<HandleLayer>(joint, motor, node->getStorage(), params);
canopen::LayerStatus s;
if(!handle->prepareFilters(s)){
ROS_ERROR_STREAM(s.reason());
return false;
}
robot_layer_->add(joint, handle);
logger->add(handle);
return true;
}
bool MotorChain::setup_chain() {
motors_.reset(new LayerGroupNoDiag<MotorBase>("402 Layer"));
robot_layer_.reset(new RobotLayer(nh_));
ros::Duration dur(0.0) ;
if(RosChain::setup_chain()){
add(motors_);
add(robot_layer_);
if(!nh_.param("use_realtime_period", false)){
dur.fromSec(boost::chrono::duration<double>(update_duration_).count());
ROS_INFO_STREAM("Using fixed control period: " << dur);
}else{
ROS_INFO("Using real-time control period");
}
cm_.reset(new ControllerManagerLayer(robot_layer_, nh_, dur));
add(cm_);
return true;
}
return false;
}

246
Devices/Libraries/Ros/ros_canopen/canopen_motor_node/src/robot_layer.cpp Executable file
View File

@@ -0,0 +1,246 @@
#include <joint_limits_interface/joint_limits.h>
#include <joint_limits_interface/joint_limits_urdf.h>
#include <joint_limits_interface/joint_limits_rosparam.h>
#include <controller_manager/controller_manager.h>
#include <controller_manager_msgs/SwitchController.h>
#include <canopen_motor_node/robot_layer.h>
#include "interface_mapping.h"
using namespace canopen;
InterfaceMapping g_interface_mapping;
void RobotLayer::stopControllers(const std::vector<std::string> controllers){
controller_manager_msgs::SwitchController srv;
srv.request.stop_controllers = controllers;
srv.request.strictness = srv.request.BEST_EFFORT;
boost::thread call(std::bind(ros::service::call<controller_manager_msgs::SwitchController>, "controller_manager/switch_controller", srv));
call.detach();
}
void RobotLayer::add(const std::string &name, HandleLayerBaseSharedPtr handle){
LayerGroupNoDiag::add(handle);
handles_.insert(std::make_pair(name, handle));
}
RobotLayer::RobotLayer(ros::NodeHandle nh) : LayerGroupNoDiag<HandleLayerBase>("RobotLayer"), nh_(nh), first_init_(true)
{
registerInterface(&state_interface_);
registerInterface(&pos_interface_);
registerInterface(&vel_interface_);
registerInterface(&eff_interface_);
urdf_.initParam("robot_description");
}
void RobotLayer::handleInit(LayerStatus &status){
if(first_init_){
for(HandleMap::iterator it = handles_.begin(); it != handles_.end(); ++it){
joint_limits_interface::JointLimits limits;
joint_limits_interface::SoftJointLimits soft_limits;
urdf::JointConstSharedPtr joint = getJoint(it->first);
if(!joint){
status.error("joint " + it->first + " not found");
return;
}
bool has_joint_limits = joint_limits_interface::getJointLimits(joint, limits);
has_joint_limits = joint_limits_interface::getJointLimits(it->first, nh_, limits) || has_joint_limits;
bool has_soft_limits = has_joint_limits && joint_limits_interface::getSoftJointLimits(joint, soft_limits);
if(!has_joint_limits){
ROS_WARN_STREAM("No limits found for " << it->first);
}
it->second->registerHandle(state_interface_);
const hardware_interface::JointHandle *h = 0;
it->second->registerHandle(pos_interface_, limits, has_soft_limits ? &soft_limits : 0);
it->second->registerHandle(vel_interface_, limits, has_soft_limits ? &soft_limits : 0);
it->second->registerHandle(eff_interface_, limits, has_soft_limits ? &soft_limits : 0);
}
first_init_ = false;
}
LayerGroupNoDiag::handleInit(status);
}
void RobotLayer::enforce(const ros::Duration &period, bool reset){
for(HandleMap::iterator it = handles_.begin(); it != handles_.end(); ++it){
it->second->enforceLimits(period, reset);
}
}
class ModeLookup {
int default_mode_;
bool has_default_mode_;
std::map<std::string, int> lookup_;
bool has_lookup_;
public:
ModeLookup(ros::NodeHandle &nh_c){
has_default_mode_ = nh_c.getParam("required_drive_mode", default_mode_);
has_lookup_ = nh_c.getParam("required_drive_modes", lookup_);
}
bool hasModes() { return has_default_mode_ || has_lookup_; }
bool hasMixedModes() { return has_lookup_; }
bool getMode(MotorBase::OperationMode &om, const std::string &key) {
std::map<std::string, int>::iterator f = lookup_.find(key);
if(f != lookup_.end()){
om = MotorBase::OperationMode(f->second);
return true;
}else if (has_default_mode_) {
om = MotorBase::OperationMode(default_mode_);
return true;
}
return false;
}
};
bool RobotLayer::prepareSwitch(const std::list<hardware_interface::ControllerInfo> &start_list, const std::list<hardware_interface::ControllerInfo> &stop_list) {
// compile-time check for mode switching support in ros_control
(void) &hardware_interface::RobotHW::prepareSwitch; // please upgrade to ros_control/contoller_manager 0.9.4 or newer
// stop handles
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = stop_list.begin(); controller_it != stop_list.end(); ++controller_it){
if(switch_map_.find(controller_it->name) == switch_map_.end()){
ROS_ERROR_STREAM(controller_it->name << " was not started before");
return false;
}
}
// start handles
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = start_list.begin(); controller_it != start_list.end(); ++controller_it){
SwitchContainer to_switch;
ros::NodeHandle nh(nh_, controller_it->name);
ModeLookup ml(nh);
std::set<std::string> claimed_interfaces;
if(controller_it->claimed_resources.size() > 0){
for (std::vector<hardware_interface::InterfaceResources>::const_iterator cres_it = controller_it->claimed_resources.begin(); cres_it != controller_it->claimed_resources.end(); ++cres_it){
for (std::set<std::string>::const_iterator res_it = cres_it->resources.begin(); res_it != cres_it->resources.end(); ++res_it){
claimed_interfaces.insert(cres_it->hardware_interface);
if(!ml.hasModes()){
ROS_ERROR_STREAM("Please set required_drive_mode(s) for controller " << controller_it->name);
return false;
}
if(claimed_interfaces.size() > 1 && !ml.hasMixedModes()){
ROS_ERROR_STREAM("controller "<< controller_it->name << " has mixed interfaces, please set required_drive_modes.");
return false;
}
std::unordered_map< std::string, HandleLayerBaseSharedPtr >::const_iterator h_it = handles_.find(*res_it);
const std::string & joint = *res_it;
if(h_it == handles_.end()){
ROS_ERROR_STREAM(joint << " not found");
return false;
}
SwitchData sd;
sd.enforce_limits = nh.param("enforce_limits", true);
if(!ml.getMode(sd.mode, joint)){
ROS_ERROR_STREAM("could not determine drive mode for " << joint);
return false;
}
if(g_interface_mapping.hasConflict(cres_it->hardware_interface, sd.mode)){
ROS_ERROR_STREAM(cres_it->hardware_interface << " cannot be provided in mode " << sd.mode);
return false;
}
HandleLayerBase::CanSwitchResult res = h_it->second->canSwitch(sd.mode);
switch(res){
case HandleLayerBase::NotSupported:
ROS_ERROR_STREAM("Mode " << sd.mode << " is not available for " << joint);
return false;
case HandleLayerBase::NotReadyToSwitch:
ROS_ERROR_STREAM(joint << " is not ready to switch mode");
return false;
case HandleLayerBase::ReadyToSwitch:
case HandleLayerBase::NoNeedToSwitch:
sd.handle = h_it->second;
to_switch.push_back(sd);
}
}
}
}
switch_map_.insert(std::make_pair(controller_it->name, to_switch));
}
// perform mode switches
std::unordered_set<HandleLayerBaseSharedPtr > to_stop;
std::vector<std::string> failed_controllers;
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = stop_list.begin(); controller_it != stop_list.end(); ++controller_it){
SwitchContainer &to_switch = switch_map_.at(controller_it->name);
for(RobotLayer::SwitchContainer::iterator it = to_switch.begin(); it != to_switch.end(); ++it){
to_stop.insert(it->handle);
}
}
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = start_list.begin(); controller_it != start_list.end(); ++controller_it){
SwitchContainer &to_switch = switch_map_.at(controller_it->name);
bool okay = true;
for(RobotLayer::SwitchContainer::iterator it = to_switch.begin(); it != to_switch.end(); ++it){
it->handle->switchMode(MotorBase::No_Mode); // stop all
}
for(RobotLayer::SwitchContainer::iterator it = to_switch.begin(); it != to_switch.end(); ++it){
if(!it->handle->switchMode(it->mode)){
failed_controllers.push_back(controller_it->name);
ROS_ERROR_STREAM("Could not switch one joint for " << controller_it->name << ", will stop all related joints and the controller.");
for(RobotLayer::SwitchContainer::iterator stop_it = to_switch.begin(); stop_it != to_switch.end(); ++stop_it){
to_stop.insert(stop_it->handle);
}
okay = false;
break;
}else{
it->handle->enableLimits(it->enforce_limits);
}
to_stop.erase(it->handle);
}
}
for(std::unordered_set<HandleLayerBaseSharedPtr >::iterator it = to_stop.begin(); it != to_stop.end(); ++it){
(*it)->switchMode(MotorBase::No_Mode);
}
if(!failed_controllers.empty()){
stopControllers(failed_controllers);
// will not return false here since this would prevent other controllers to be started and therefore lead to an inconsistent state
}
return true;
}
void RobotLayer::doSwitch(const std::list<hardware_interface::ControllerInfo> &start_list, const std::list<hardware_interface::ControllerInfo> &stop_list) {
std::vector<std::string> failed_controllers;
for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it = start_list.begin(); controller_it != start_list.end(); ++controller_it){
try{
SwitchContainer &to_switch = switch_map_.at(controller_it->name);
for(RobotLayer::SwitchContainer::iterator it = to_switch.begin(); it != to_switch.end(); ++it){
if(!it->handle->forwardForMode(it->mode)){
failed_controllers.push_back(controller_it->name);
ROS_ERROR_STREAM("Could not switch one joint for " << controller_it->name << ", will stop all related joints and the controller.");
for(RobotLayer::SwitchContainer::iterator stop_it = to_switch.begin(); stop_it != to_switch.end(); ++stop_it){
it->handle->switchMode(MotorBase::No_Mode);
}
break;
}
}
}catch(const std::out_of_range&){
ROS_ERROR_STREAM("Conttroller " << controller_it->name << "not found, will stop it");
failed_controllers.push_back(controller_it->name);
}
}
if(!failed_controllers.empty()){
stopControllers(failed_controllers);
}
}

29
Devices/Libraries/Ros/ros_canopen/canopen_motor_node/test/test_muparser.cpp Executable file
View File

@@ -0,0 +1,29 @@
#include <gtest/gtest.h>
#include <canopen_motor_node/unit_converter.h>
#include <canopen_motor_node/handle_layer.h>
#include <functional>
using namespace canopen;
double * mapVariable(const std::string &, double *p) {
return p;
}
TEST(TestMuparser, CheckNorm){
double input = 0;
UnitConverter uc("norm(in,-1000,1000)", std::bind(HandleLayer::assignVariable, std::placeholders::_1, &input, "in"));
input = 0; EXPECT_EQ(0, uc.evaluate());
input = 10; EXPECT_EQ(10, uc.evaluate());
input = -10; EXPECT_EQ(-10, uc.evaluate());
input = 1000; EXPECT_EQ(-1000, uc.evaluate());
input = 1001; EXPECT_EQ(-999, uc.evaluate());
input = 2000; EXPECT_EQ(0, uc.evaluate());
input = 2001; EXPECT_EQ(1, uc.evaluate());
input = -1000; EXPECT_EQ(-1000, uc.evaluate());
input = 999; EXPECT_EQ(999, uc.evaluate());
}
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

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

@@ -0,0 +1,76 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package ros_canopen
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.8.5 (2020-09-22)
------------------
0.8.4 (2020-08-22)
------------------
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)
------------------
0.8.1 (2019-07-14)
------------------
0.8.0 (2018-07-11)
------------------
0.7.8 (2018-05-04)
------------------
0.7.7 (2018-05-04)
------------------
0.7.6 (2017-08-30)
------------------
0.7.5 (2017-05-29)
------------------
0.7.4 (2017-04-25)
------------------
0.7.3 (2017-04-25)
------------------
0.7.2 (2017-03-28)
------------------
0.7.1 (2017-03-20)
------------------
0.7.0 (2016-12-13)
------------------
0.6.5 (2016-12-10)
------------------
* updated metapackage
* format 2
* updated maintaner
* added new packages
* update package URLs
* Contributors: Mathias Lüdtke
0.6.4 (2015-07-03)
------------------
0.6.3 (2015-06-30)
------------------
0.6.2 (2014-12-18)
------------------
* remove canopen_test_utils from metapackage
* Contributors: Florian Weisshardt
0.6.1 (2014-12-15)
------------------
* add metapackage
* Contributors: Florian Weisshardt

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

@@ -0,0 +1,4 @@
cmake_minimum_required(VERSION 3.0.2)
project(ros_canopen)
find_package(catkin REQUIRED)
catkin_metapackage()

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

@@ -0,0 +1,30 @@
<?xml version="1.0"?>
<package format="2">
<name>ros_canopen</name>
<version>0.8.5</version>
<description>A generic canopen implementation for ROS</description>
<license>LGPL</license>
<url type="website">http://ros.org/wiki/ros_canopen</url>
<url type="repository">https://github.com/ros-industrial/ros_canopen</url>
<url type="bugtracker">https://github.com/ros-industrial/ros_canopen/issues</url>
<maintainer email="mathias.luedtke@ipa.fraunhofer.de">Mathias Lüdtke</maintainer>
<author email="fmw@ipa.fhg.de">Florian Weisshardt</author>
<buildtool_depend>catkin</buildtool_depend>
<exec_depend>can_msgs</exec_depend>
<exec_depend>canopen_402</exec_depend>
<exec_depend>canopen_chain_node</exec_depend>
<exec_depend>canopen_master</exec_depend>
<exec_depend>canopen_motor_node</exec_depend>
<!--exec_depend>canopen_test_utils</exec_depend-->
<exec_depend>socketcan_bridge</exec_depend>
<exec_depend>socketcan_interface</exec_depend>
<export>
<metapackage/>
</export>
</package>

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

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

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

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

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

@@ -0,0 +1,76 @@
/*
* Copyright (c) 2016, Ivor Wanders
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef SOCKETCAN_BRIDGE_SOCKETCAN_TO_TOPIC_H
#define SOCKETCAN_BRIDGE_SOCKETCAN_TO_TOPIC_H
#include <socketcan_interface/socketcan.h>
#include <socketcan_interface/filter.h>
#include <can_msgs/Frame.h>
#include <ros/ros.h>
namespace socketcan_bridge
{
class SocketCANToTopic
{
public:
SocketCANToTopic(ros::NodeHandle* nh, ros::NodeHandle* nh_param, can::DriverInterfaceSharedPtr driver);
void setup();
void setup(const can::FilteredFrameListener::FilterVector &filters);
void setup(XmlRpc::XmlRpcValue filters);
void setup(ros::NodeHandle nh);
private:
ros::Publisher can_topic_;
can::DriverInterfaceSharedPtr driver_;
can::FrameListenerConstSharedPtr frame_listener_;
can::StateListenerConstSharedPtr state_listener_;
void frameCallback(const can::Frame& f);
void stateCallback(const can::State & s);
};
void convertSocketCANToMessage(const can::Frame& f, can_msgs::Frame& m)
{
m.id = f.id;
m.dlc = f.dlc;
m.is_error = f.is_error;
m.is_rtr = f.is_rtr;
m.is_extended = f.is_extended;
for (int i = 0; i < 8; i++) // always copy all data, regardless of dlc.
{
m.data[i] = f.data[i];
}
};
}; // namespace socketcan_bridge
#endif // SOCKETCAN_BRIDGE_SOCKETCAN_TO_TOPIC_H

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

@@ -0,0 +1,70 @@
/*
* Copyright (c) 2016, Ivor Wanders
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef SOCKETCAN_BRIDGE_TOPIC_TO_SOCKETCAN_H
#define SOCKETCAN_BRIDGE_TOPIC_TO_SOCKETCAN_H
#include <socketcan_interface/socketcan.h>
#include <can_msgs/Frame.h>
#include <ros/ros.h>
namespace socketcan_bridge
{
class TopicToSocketCAN
{
public:
TopicToSocketCAN(ros::NodeHandle* nh, ros::NodeHandle* nh_param, can::DriverInterfaceSharedPtr driver);
void setup();
private:
ros::Subscriber can_topic_;
can::DriverInterfaceSharedPtr driver_;
can::StateListenerConstSharedPtr state_listener_;
void msgCallback(const can_msgs::Frame::ConstPtr& msg);
void stateCallback(const can::State & s);
};
void convertMessageToSocketCAN(const can_msgs::Frame& m, can::Frame& f)
{
f.id = m.id;
f.dlc = m.dlc;
f.is_error = m.is_error;
f.is_rtr = m.is_rtr;
f.is_extended = m.is_extended;
for (int i = 0; i < 8; i++) // always copy all data, regardless of dlc.
{
f.data[i] = m.data[i];
}
};
}; // namespace socketcan_bridge
#endif // SOCKETCAN_BRIDGE_TOPIC_TO_SOCKETCAN_H

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

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

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

@@ -0,0 +1,29 @@
/*
* Copyright (c) 2019, Mathias Lüdtke
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <rosconsole_bridge/bridge.h>
REGISTER_ROSCONSOLE_BRIDGE;

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

@@ -0,0 +1,72 @@
/*
* Copyright (c) 2016, Ivor Wanders
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <socketcan_bridge/topic_to_socketcan.h>
#include <socketcan_bridge/socketcan_to_topic.h>
#include <socketcan_interface/threading.h>
#include <socketcan_interface/xmlrpc_settings.h>
#include <memory>
#include <string>
int main(int argc, char *argv[])
{
ros::init(argc, argv, "socketcan_bridge_node");
ros::NodeHandle nh(""), nh_param("~");
std::string can_device;
nh_param.param<std::string>("can_device", can_device, "can0");
can::ThreadedSocketCANInterfaceSharedPtr driver = std::make_shared<can::ThreadedSocketCANInterface> ();
// initialize device at can_device, 0 for no loopback.
if (!driver->init(can_device, 0, XmlRpcSettings::create(nh_param)))
{
ROS_FATAL("Failed to initialize can_device at %s", can_device.c_str());
return 1;
}
else
{
ROS_INFO("Successfully connected to %s.", can_device.c_str());
}
// initialize the bridge both ways.
socketcan_bridge::TopicToSocketCAN to_socketcan_bridge(&nh, &nh_param, driver);
to_socketcan_bridge.setup();
socketcan_bridge::SocketCANToTopic to_topic_bridge(&nh, &nh_param, driver);
to_topic_bridge.setup(nh_param);
ros::spin();
driver->shutdown();
driver.reset();
ros::waitForShutdown();
}

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

@@ -0,0 +1,133 @@
/*
* Copyright (c) 2016, Ivor Wanders
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <socketcan_bridge/socketcan_to_topic.h>
#include <socketcan_interface/string.h>
#include <can_msgs/Frame.h>
#include <string>
namespace can
{
template<> can::FrameFilterSharedPtr tofilter(const XmlRpc::XmlRpcValue &ct)
{
XmlRpc::XmlRpcValue t(ct);
try // try to read as integer
{
uint32_t id = static_cast<int>(t);
return tofilter(id);
}
catch(...) // else read as string
{
return tofilter(static_cast<std::string>(t));
}
}
} // namespace can
namespace socketcan_bridge
{
SocketCANToTopic::SocketCANToTopic(ros::NodeHandle* nh, ros::NodeHandle* nh_param,
can::DriverInterfaceSharedPtr driver)
{
can_topic_ = nh->advertise<can_msgs::Frame>("received_messages",
nh_param->param("received_messages_queue_size", 10));
driver_ = driver;
};
void SocketCANToTopic::setup()
{
// register handler for frames and state changes.
frame_listener_ = driver_->createMsgListenerM(this, &SocketCANToTopic::frameCallback);
state_listener_ = driver_->createStateListenerM(this, &SocketCANToTopic::stateCallback);
};
void SocketCANToTopic::setup(const can::FilteredFrameListener::FilterVector &filters)
{
frame_listener_.reset(new can::FilteredFrameListener(driver_,
std::bind(&SocketCANToTopic::frameCallback,
this,
std::placeholders::_1),
filters));
state_listener_ = driver_->createStateListenerM(this, &SocketCANToTopic::stateCallback);
}
void SocketCANToTopic::setup(XmlRpc::XmlRpcValue filters)
{
setup(can::tofilters(filters));
}
void SocketCANToTopic::setup(ros::NodeHandle nh)
{
XmlRpc::XmlRpcValue filters;
if (nh.getParam("can_ids", filters)) return setup(filters);
return setup();
}
void SocketCANToTopic::frameCallback(const can::Frame& f)
{
// ROS_DEBUG("Message came in: %s", can::tostring(f, true).c_str());
if (!f.isValid())
{
ROS_ERROR("Invalid frame from SocketCAN: id: %#04x, length: %d, is_extended: %d, is_error: %d, is_rtr: %d",
f.id, f.dlc, f.is_extended, f.is_error, f.is_rtr);
return;
}
else
{
if (f.is_error)
{
// can::tostring cannot be used for dlc > 8 frames. It causes an crash
// due to usage of boost::array for the data array. The should always work.
ROS_WARN("Received frame is error: %s", can::tostring(f, true).c_str());
}
}
can_msgs::Frame msg;
// converts the can::Frame (socketcan.h) to can_msgs::Frame (ROS msg)
convertSocketCANToMessage(f, msg);
msg.header.frame_id = ""; // empty frame is the de-facto standard for no frame.
msg.header.stamp = ros::Time::now();
can_topic_.publish(msg);
};
void SocketCANToTopic::stateCallback(const can::State & s)
{
std::string err;
driver_->translateError(s.internal_error, err);
if (!s.internal_error)
{
ROS_INFO("State: %s, asio: %s", err.c_str(), s.error_code.message().c_str());
}
else
{
ROS_ERROR("Error: %s, asio: %s", err.c_str(), s.error_code.message().c_str());
}
};
}; // namespace socketcan_bridge

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

@@ -0,0 +1,69 @@
/*
* Copyright (c) 2016, Ivor Wanders
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <socketcan_bridge/socketcan_to_topic.h>
#include <socketcan_interface/threading.h>
#include <socketcan_interface/string.h>
#include <socketcan_interface/xmlrpc_settings.h>
#include <memory>
#include <string>
int main(int argc, char *argv[])
{
ros::init(argc, argv, "socketcan_to_topic_node");
ros::NodeHandle nh(""), nh_param("~");
std::string can_device;
nh_param.param<std::string>("can_device", can_device, "can0");
can::ThreadedSocketCANInterfaceSharedPtr driver = std::make_shared<can::ThreadedSocketCANInterface> ();
// initialize device at can_device, 0 for no loopback.
if (!driver->init(can_device, 0, XmlRpcSettings::create(nh_param)))
{
ROS_FATAL("Failed to initialize can_device at %s", can_device.c_str());
return 1;
}
else
{
ROS_INFO("Successfully connected to %s.", can_device.c_str());
}
socketcan_bridge::SocketCANToTopic to_topic_bridge(&nh, &nh_param, driver);
to_topic_bridge.setup(nh_param);
ros::spin();
driver->shutdown();
driver.reset();
ros::waitForShutdown();
}

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

@@ -0,0 +1,91 @@
/*
* Copyright (c) 2016, Ivor Wanders
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <socketcan_bridge/topic_to_socketcan.h>
#include <socketcan_interface/string.h>
#include <string>
namespace socketcan_bridge
{
TopicToSocketCAN::TopicToSocketCAN(ros::NodeHandle* nh, ros::NodeHandle* nh_param,
can::DriverInterfaceSharedPtr driver)
{
can_topic_ = nh->subscribe<can_msgs::Frame>("sent_messages", nh_param->param("sent_messages_queue_size", 10),
std::bind(&TopicToSocketCAN::msgCallback, this, std::placeholders::_1));
driver_ = driver;
};
void TopicToSocketCAN::setup()
{
state_listener_ = driver_->createStateListener(
std::bind(&TopicToSocketCAN::stateCallback, this, std::placeholders::_1));
};
void TopicToSocketCAN::msgCallback(const can_msgs::Frame::ConstPtr& msg)
{
// ROS_DEBUG("Message came from sent_messages topic");
// translate it to the socketcan frame type.
can_msgs::Frame m = *msg.get(); // ROS message
can::Frame f; // socketcan type
// converts the can_msgs::Frame (ROS msg) to can::Frame (socketcan.h)
convertMessageToSocketCAN(m, f);
if (!f.isValid()) // check if the id and flags are appropriate.
{
// ROS_WARN("Refusing to send invalid frame: %s.", can::tostring(f, true).c_str());
// can::tostring cannot be used for dlc > 8 frames. It causes an crash
// due to usage of boost::array for the data array. The should always work.
ROS_ERROR("Invalid frame from topic: id: %#04x, length: %d, is_extended: %d", m.id, m.dlc, m.is_extended);
return;
}
bool res = driver_->send(f);
if (!res)
{
ROS_ERROR("Failed to send message: %s.", can::tostring(f, true).c_str());
}
};
void TopicToSocketCAN::stateCallback(const can::State & s)
{
std::string err;
driver_->translateError(s.internal_error, err);
if (!s.internal_error)
{
ROS_INFO("State: %s, asio: %s", err.c_str(), s.error_code.message().c_str());
}
else
{
ROS_ERROR("Error: %s, asio: %s", err.c_str(), s.error_code.message().c_str());
}
};
}; // namespace socketcan_bridge

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

@@ -0,0 +1,69 @@
/*
* Copyright (c) 2016, Ivor Wanders
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the copyright holder nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <ros/ros.h>
#include <socketcan_bridge/topic_to_socketcan.h>
#include <socketcan_interface/threading.h>
#include <socketcan_interface/string.h>
#include <socketcan_interface/xmlrpc_settings.h>
#include <memory>
#include <string>
int main(int argc, char *argv[])
{
ros::init(argc, argv, "topic_to_socketcan_node");
ros::NodeHandle nh(""), nh_param("~");
std::string can_device;
nh_param.param<std::string>("can_device", can_device, "can0");
can::ThreadedSocketCANInterfaceSharedPtr driver = std::make_shared<can::ThreadedSocketCANInterface> ();
// initialize device at can_device, 0 for no loopback.
if (!driver->init(can_device, 0, XmlRpcSettings::create(nh_param)))
{
ROS_FATAL("Failed to initialize can_device at %s", can_device.c_str());
return 1;
}
else
{
ROS_INFO("Successfully connected to %s.", can_device.c_str());
}
socketcan_bridge::TopicToSocketCAN to_socketcan_bridge(&nh, &nh_param, driver);
to_socketcan_bridge.setup();
ros::spin();
driver->shutdown();
driver.reset();
ros::waitForShutdown();
}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

189
Devices/Libraries/Ros/ros_canopen/socketcan_interface/CHANGELOG.rst Executable file
View File

@@ -0,0 +1,189 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package socketcan_interface
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.8.5 (2020-09-22)
------------------
* check settings pointer and print error if null
* initalize settings properly in deprecated SocketCANInterface::init
* Contributors: Mathias Lüdtke
0.8.4 (2020-08-22)
------------------
* make parse_error_mask a static member function
* pass settings from ROS node to SocketCANInterface
* add support for recursive XmlRpcSettings lookups
* implemented report-only and fatal errors for SocketCANInterface
* added settings parameter to DriverInterface::init
* moved XmlRpcSettings to socketcan_interface
* moved canopen::Settings into can namespace
* Contributors: Mathias Lüdtke
0.8.3 (2020-05-07)
------------------
* Fixed Boost link in test-dispacher
Signed-off-by: ahcorde <ahcorde@gmail.com>
* Bump CMake version to avoid CMP0048 warning
Signed-off-by: ahcorde <ahcorde@gmail.com>
* do not print ERROR in candump
* Contributors: Mathias Lüdtke, ahcorde
0.8.2 (2019-11-04)
------------------
* enable rosconsole_bridge bindings
* switch to new logging macros
* add logging based on console_bridge
* handle extended frame strings like candump
* implement Frame::fullid()
* removed implicit Header operator
* move stream operators into can namespace
* Contributors: Mathias Lüdtke
0.8.1 (2019-07-14)
------------------
* Set C++ standard to c++14
* implemented test for dispatcher
* Replacing typedefs in socketcan_interface with using aliases.
* 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, pzzlr
0.8.0 (2018-07-11)
------------------
* migrated to std::function and std::bind
* got rid of boost::noncopyable
* replaced BOOST_FOREACH
* migrated to std::unordered_map and std::unordered_set
* migrated to std:array
* migrated to std pointers
* removed deprecated types
* introduced ROSCANOPEN_MAKE_SHARED
* 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)
------------------
* pull make_shared into namespaces
* added types for all shared_ptrs
* fix catkin_lint warnings in filter tests
* migrate to new classloader headers
* find and link the thread library properly
* compile also with boost >= 1.66.0
* explicitly include iostream to compile with boost >= 1.65.0
* address catkin_lint errors/warnings
* added test for FilteredFrameListener
* fix string parsers
* default to relaxed filter handling
works for standard and extended frames
* fix string handling of extended frames
* added filter parsers
should work for vector<unsigned int>, vector<string> and custom vector-like classes
* implemented mask and range filters for can::Frame
* Contributors: Lukas Bulwahn, Mathias Lüdtke
0.7.6 (2017-08-30)
------------------
* make can::Header/Frame::isValid() const
* Contributors: Mathias Lüdtke
0.7.5 (2017-05-29)
------------------
* fix rosdep dependency on kernel headers
* Contributors: Mathias Lüdtke
0.7.4 (2017-04-25)
------------------
0.7.3 (2017-04-25)
------------------
0.7.2 (2017-03-28)
------------------
0.7.1 (2017-03-20)
------------------
* stop CAN driver on read errors as well
* expose socketcan handle
* implemented BCMsocket
* introduced BufferedReader::readUntil
* Contributors: Mathias Lüdtke
0.7.0 (2016-12-13)
------------------
0.6.5 (2016-12-10)
------------------
* removed Baseclass typedef since its use prevented virtual functions calls
* add missing chrono dependency
* Added catch-all features in BufferedReader
* hardened code with the help of cppcheck
* styled and sorted CMakeLists.txt
* removed boilerplate comments
* indention
* reviewed exported dependencies
* styled and sorted package.xml
* update package URLs
* Improves StateInterface implementation of the DummyInterface.
The doesLoopBack() method now returns the correct value. A state change is
correctly dispatched when the init() method is called.
* Changes inheritance of DummyInterface to DriverInterface.
Such that this interface can also be used for tests requiring a DriverInterface
class.
Test results of the socketcan_interface tests are unchanged by this
modification as it only uses the CommInterface methods.
* added socketcan_interface_string to test
* moved string functions into separate lib
* Introduced setNotReady, prevent enqueue() to switch from closed to open
* Reading state\_ should be protected by lock
* improved BufferedReader interface and ScopedEnabler
* added flush() and max length support to BufferedReader
* added BufferedReader
* wake multiple waiting threads if needed
* pad hex buffer strings in all cases
* removed unstable StateWaiter::wait_for
* Contributors: Ivor Wanders, Mathias Lüdtke, Michael Stoll
0.6.4 (2015-07-03)
------------------
* added missing include, revised depends etc.
0.6.3 (2015-06-30)
------------------
* dependencies revised
* reordering fix for `#87 <https://github.com/ros-industrial/ros_canopen/issues/87>`_
* intialize structs
* tostring fixed for headers
* removed empty test
* added DummyInterface with first test
* added message string helper
* added missing include
* install socketcan_interface_plugin.xml
* migrated to class_loader for non-ROS parts
* moved ThreadedInterface to dedicated header
* removed bitrate, added loopback to DriverInterface::init
* added socketcan plugin
* CommInterstate and StateInterface are now bases of DriverInterface.
Therefore DispatchedInterface was moved into AsioBase.
* remove debug prints
* shutdown asio driver in destructor
* proper mask shifts
* Contributors: Mathias Lüdtke
0.6.2 (2014-12-18)
------------------
0.6.1 (2014-12-15)
------------------
* remove ipa_* and IPA_* prefixes
* fixed catkin_lint errors
* added descriptions and authors
* renamed ipa_can_interface to socketcaninterface
* Contributors: Florian Weisshardt, Mathias Lüdtke

168
Devices/Libraries/Ros/ros_canopen/socketcan_interface/CMakeLists.txt Executable file
View File

@@ -0,0 +1,168 @@
cmake_minimum_required(VERSION 3.0.2)
project(socketcan_interface)
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_STANDARD 14)
endif()
find_package(catkin REQUIRED
COMPONENTS
class_loader
)
find_package(console_bridge REQUIRED)
find_package(Boost REQUIRED
COMPONENTS
chrono
system
thread
)
find_package(Threads REQUIRED)
catkin_package(
INCLUDE_DIRS
include
LIBRARIES
${PROJECT_NAME}_string
CATKIN_DEPENDS
DEPENDS
Boost
console_bridge
)
include_directories(
include
${Boost_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
${console_bridge_INCLUDE_DIRS}
)
# ${PROJECT_NAME}_string
add_library(${PROJECT_NAME}_string
src/string.cpp
)
# socketcan_dump
add_executable(socketcan_dump
src/candump.cpp
)
target_link_libraries(socketcan_dump
${PROJECT_NAME}_string
${console_bridge_LIBRARIES}
${catkin_LIBRARIES}
${Boost_LIBRARIES}
${CMAKE_THREAD_LIBS_INIT}
)
# socketcan_bcm
add_executable(socketcan_bcm
src/canbcm.cpp
)
target_link_libraries(socketcan_bcm
${PROJECT_NAME}_string
${console_bridge_LIBRARIES}
${catkin_LIBRARIES}
${Boost_LIBRARIES}
)
# ${PROJECT_NAME}_plugin
add_library(${PROJECT_NAME}_plugin
src/${PROJECT_NAME}_plugin.cpp
)
target_link_libraries(${PROJECT_NAME}_plugin
${console_bridge_LIBRARIES}
${catkin_LIBRARIES}
${Boost_LIBRARIES}
)
install(
TARGETS
socketcan_bcm
socketcan_dump
${PROJECT_NAME}_plugin
${PROJECT_NAME}_string
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
${PROJECT_NAME}_plugin.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
if(CATKIN_ENABLE_TESTING)
find_package(xmlrpcpp REQUIRED)
catkin_add_gtest(${PROJECT_NAME}-test_dummy_interface
test/test_dummy_interface.cpp
)
target_link_libraries(${PROJECT_NAME}-test_dummy_interface
${PROJECT_NAME}_string
${console_bridge_LIBRARIES}
${catkin_LIBRARIES}
${Boost_LIBRARIES}
)
catkin_add_gtest(${PROJECT_NAME}-test_delegates
test/test_delegates.cpp
)
target_link_libraries(${PROJECT_NAME}-test_delegates
${PROJECT_NAME}_string
${catkin_LIBRARIES}
${Boost_LIBRARIES}
)
catkin_add_gtest(${PROJECT_NAME}-test_settings
test/test_settings.cpp
)
target_include_directories(${PROJECT_NAME}-test_settings PRIVATE
${xmlrpcpp_INCLUDE_DIRS}
)
target_link_libraries(${PROJECT_NAME}-test_settings
${PROJECT_NAME}_string
${catkin_LIBRARIES}
${xmlrpcpp_LIBRARIES}
)
catkin_add_gtest(${PROJECT_NAME}-test_string
test/test_string.cpp
)
target_link_libraries(${PROJECT_NAME}-test_string
${PROJECT_NAME}_string
${console_bridge_LIBRARIES}
${catkin_LIBRARIES}
)
catkin_add_gtest(${PROJECT_NAME}-test_filter
test/test_filter.cpp
)
target_link_libraries(${PROJECT_NAME}-test_filter
${PROJECT_NAME}_string
${console_bridge_LIBRARIES}
${catkin_LIBRARIES}
${Boost_LIBRARIES}
)
catkin_add_gtest(${PROJECT_NAME}-test_dispatcher
test/test_dispatcher.cpp
)
target_link_libraries(${PROJECT_NAME}-test_dispatcher
${PROJECT_NAME}_string
${catkin_LIBRARIES}
${Boost_LIBRARIES}
)
target_compile_options(${PROJECT_NAME}-test_dispatcher PRIVATE -Wno-deprecated-declarations)
endif()

136
Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/asio_base.h Executable file
View File

@@ -0,0 +1,136 @@
#ifndef H_ASIO_BASE
#define H_ASIO_BASE
#include <socketcan_interface/interface.h>
#include <socketcan_interface/dispatcher.h>
#include <boost/asio.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/thread/thread.hpp>
#include <functional>
namespace can{
template<typename Socket> class AsioDriver : public DriverInterface{
using FrameDispatcher = FilteredDispatcher<unsigned int, CommInterface::FrameListener>;
using StateDispatcher = SimpleDispatcher<StateInterface::StateListener>;
FrameDispatcher frame_dispatcher_;
StateDispatcher state_dispatcher_;
State state_;
boost::mutex state_mutex_;
boost::mutex socket_mutex_;
void shutdown_internal(){
if(socket_.is_open()){
socket_.cancel();
socket_.close();
}
io_service_.stop();
}
protected:
boost::asio::io_service io_service_;
#if BOOST_ASIO_VERSION >= 101200 // Boost 1.66+
boost::asio::io_context::strand strand_;
#else
boost::asio::strand strand_;
#endif
Socket socket_;
Frame input_;
virtual void triggerReadSome() = 0;
virtual bool enqueue(const Frame & msg) = 0;
void dispatchFrame(const Frame &msg){
strand_.post([this, msg]{ frame_dispatcher_.dispatch(msg.key(), msg);} ); // copies msg
}
void setErrorCode(const boost::system::error_code& error){
boost::mutex::scoped_lock lock(state_mutex_);
if(state_.error_code != error){
state_.error_code = error;
state_dispatcher_.dispatch(state_);
}
}
void setInternalError(unsigned int internal_error){
boost::mutex::scoped_lock lock(state_mutex_);
if(state_.internal_error != internal_error){
state_.internal_error = internal_error;
state_dispatcher_.dispatch(state_);
}
}
void setDriverState(State::DriverState state){
boost::mutex::scoped_lock lock(state_mutex_);
if(state_.driver_state != state){
state_.driver_state = state;
state_dispatcher_.dispatch(state_);
}
}
void setNotReady(){
setDriverState(socket_.is_open()?State::open : State::closed);
}
void frameReceived(const boost::system::error_code& error){
if(!error){
dispatchFrame(input_);
triggerReadSome();
}else{
setErrorCode(error);
setNotReady();
}
}
AsioDriver()
: strand_(io_service_), socket_(io_service_)
{}
public:
virtual ~AsioDriver() { shutdown_internal(); }
State getState(){
boost::mutex::scoped_lock lock(state_mutex_);
return state_;
}
virtual void run(){
setNotReady();
if(getState().driver_state == State::open){
io_service_.reset();
boost::asio::io_service::work work(io_service_);
setDriverState(State::ready);
boost::thread post_thread([this]() { io_service_.run(); });
triggerReadSome();
boost::system::error_code ec;
io_service_.run(ec);
setErrorCode(ec);
setNotReady();
}
state_dispatcher_.dispatch(getState());
}
virtual bool send(const Frame & msg){
return getState().driver_state == State::ready && enqueue(msg);
}
virtual void shutdown(){
shutdown_internal();
}
virtual FrameListenerConstSharedPtr createMsgListener(const FrameFunc &delegate){
return frame_dispatcher_.createListener(delegate);
}
virtual FrameListenerConstSharedPtr createMsgListener(const Frame::Header&h , const FrameFunc &delegate){
return frame_dispatcher_.createListener(h.key(), delegate);
}
virtual StateListenerConstSharedPtr createStateListener(const StateFunc &delegate){
return state_dispatcher_.createListener(delegate);
}
};
} // namespace can
#endif

120
Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/bcm.h Executable file
View File

@@ -0,0 +1,120 @@
#ifndef H_CAN_BCM
#define H_CAN_BCM
#include <socketcan_interface/interface.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <linux/can.h>
#include <linux/can/bcm.h>
#include <linux/can/error.h>
#include <cstring>
#include <boost/chrono.hpp>
namespace can {
class BCMsocket{
int s_;
struct Message {
size_t size;
uint8_t *data;
Message(size_t n)
: size(sizeof(bcm_msg_head) + sizeof(can_frame)*n), data(new uint8_t[size])
{
assert(n<=256);
std::memset(data, 0, size);
head().nframes = n;
}
bcm_msg_head& head() {
return *(bcm_msg_head*)data;
}
template<typename T> void setIVal2(T period){
long long usec = boost::chrono::duration_cast<boost::chrono::microseconds>(period).count();
head().ival2.tv_sec = usec / 1000000;
head().ival2.tv_usec = usec % 1000000;
}
void setHeader(Header header){
head().can_id = header.id | (header.is_extended?CAN_EFF_FLAG:0);
}
bool write(int s){
return ::write(s, data, size) > 0;
}
~Message(){
delete[] data;
data = 0;
size = 0;
}
};
public:
BCMsocket():s_(-1){
}
bool init(const std::string &device){
s_ = socket(PF_CAN, SOCK_DGRAM, CAN_BCM);
if(s_ < 0 ) return false;
struct ifreq ifr;
std::strcpy(ifr.ifr_name, device.c_str());
int ret = ioctl(s_, SIOCGIFINDEX, &ifr);
if(ret != 0){
shutdown();
return false;
}
struct sockaddr_can addr = {0};
addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
ret = connect(s_, (struct sockaddr *)&addr, sizeof(addr));
if(ret < 0){
shutdown();
return false;
}
return true;
}
template<typename DurationType> bool startTX(DurationType period, Header header, size_t num, Frame *frames) {
Message msg(num);
msg.setHeader(header);
msg.setIVal2(period);
bcm_msg_head &head = msg.head();
head.opcode = TX_SETUP;
head.flags |= SETTIMER | STARTTIMER;
for(size_t i=0; i < num; ++i){ // msg nr
head.frames[i].can_dlc = frames[i].dlc;
head.frames[i].can_id = head.can_id;
for(size_t j = 0; j < head.frames[i].can_dlc; ++j){ // byte nr
head.frames[i].data[j] = frames[i].data[j];
}
}
return msg.write(s_);
}
bool stopTX(Header header){
Message msg(0);
msg.head().opcode = TX_DELETE;
msg.setHeader(header);
return msg.write(s_);
}
void shutdown(){
if(s_ > 0){
close(s_);
s_ = -1;
}
}
virtual ~BCMsocket(){
shutdown();
}
};
}
#endif

24
Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/delegates.h Executable file
View File

@@ -0,0 +1,24 @@
#ifndef SOCKETCAN_INTERFACE_DELEGATES_H_
#define SOCKETCAN_INTERFACE_DELEGATES_H_
#include <functional>
namespace can
{
template <typename T> class DelegateHelper : public T {
public:
template <typename Object, typename Instance, typename ...Args>
DelegateHelper(Object &&o, typename T::result_type (Instance::*member)(Args... args)) :
T([o, member](Args... args) -> typename T::result_type { return ((*o).*member)(args...); })
{
}
template <typename Callable>
DelegateHelper(Callable &&c) : T(c)
{
}
};
} // namespace can
#endif // SOCKETCAN_INTERFACE_DELEGATES_H_

120
Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/dispatcher.h Executable file
View File

@@ -0,0 +1,120 @@
#ifndef H_CAN_DISPATCHER
#define H_CAN_DISPATCHER
#include <functional>
#include <memory>
#include <list>
#include <unordered_map>
#include <socketcan_interface/interface.h>
#include <boost/thread/mutex.hpp>
namespace can{
template< typename Listener > class SimpleDispatcher{
public:
using Callable = typename Listener::Callable;
using Type = typename Listener::Type;
using ListenerConstSharedPtr = typename Listener::ListenerConstSharedPtr;
protected:
class DispatcherBase;
using DispatcherBaseSharedPtr = std::shared_ptr<DispatcherBase>;
class DispatcherBase {
DispatcherBase(const DispatcherBase&) = delete; // prevent copies
DispatcherBase& operator=(const DispatcherBase&) = delete;
class GuardedListener: public Listener{
std::weak_ptr<DispatcherBase> guard_;
public:
GuardedListener(DispatcherBaseSharedPtr g, const Callable &callable): Listener(callable), guard_(g){}
virtual ~GuardedListener() {
DispatcherBaseSharedPtr d = guard_.lock();
if(d){
d->remove(this);
}
}
};
boost::mutex &mutex_;
std::list<const Listener* > listeners_;
public:
DispatcherBase(boost::mutex &mutex) : mutex_(mutex) {}
void dispatch_nolock(const Type &obj, const Listener* loopback=nullptr) const{
for(typename std::list<const Listener* >::const_iterator it=listeners_.begin(); it != listeners_.end(); ++it){
if (loopback != *it) {
(**it)(obj);
}
}
}
void remove(Listener *d){
boost::mutex::scoped_lock lock(mutex_);
listeners_.remove(d);
}
size_t numListeners(){
boost::mutex::scoped_lock lock(mutex_);
return listeners_.size();
}
static ListenerConstSharedPtr createListener(DispatcherBaseSharedPtr dispatcher, const Callable &callable){
ListenerConstSharedPtr l(new GuardedListener(dispatcher,callable));
dispatcher->listeners_.push_back(l.get());
return l;
}
};
boost::mutex mutex_;
DispatcherBaseSharedPtr dispatcher_;
public:
SimpleDispatcher() : dispatcher_(new DispatcherBase(mutex_)) {}
ListenerConstSharedPtr createListener(const Callable &callable){
boost::mutex::scoped_lock lock(mutex_);
return DispatcherBase::createListener(dispatcher_, callable);
}
void dispatch(const Type &obj){
boost::mutex::scoped_lock lock(mutex_);
dispatcher_->dispatch_nolock(obj);
}
void dispatch_filtered(const Type &obj, ListenerConstSharedPtr without){
boost::mutex::scoped_lock lock(mutex_);
dispatcher_->dispatch_nolock(obj, without.get());
}
size_t numListeners(){
return dispatcher_->numListeners();
}
operator Callable() { return Callable(this,&SimpleDispatcher::dispatch); }
};
template<typename K, typename Listener, typename Hash = std::hash<K> > class FilteredDispatcher: public SimpleDispatcher<Listener>{
using BaseClass = SimpleDispatcher<Listener>;
std::unordered_map<K, typename BaseClass::DispatcherBaseSharedPtr, Hash> filtered_;
public:
using BaseClass::createListener;
typename BaseClass::ListenerConstSharedPtr createListener(const K &key, const typename BaseClass::Callable &callable){
boost::mutex::scoped_lock lock(BaseClass::mutex_);
typename BaseClass::DispatcherBaseSharedPtr &ptr = filtered_[key];
if(!ptr) ptr.reset(new typename BaseClass::DispatcherBase(BaseClass::mutex_));
return BaseClass::DispatcherBase::createListener(ptr, callable);
}
template <typename T>
[[deprecated("provide key explicitly")]]
typename BaseClass::ListenerConstSharedPtr createListener(const T &key, const typename BaseClass::Callable &callable){
return createListener(static_cast<K>(key), callable);
}
void dispatch(const K &key, const typename BaseClass::Type &obj){
boost::mutex::scoped_lock lock(BaseClass::mutex_);
typename BaseClass::DispatcherBaseSharedPtr &ptr = filtered_[key];
if(ptr) ptr->dispatch_nolock(obj);
BaseClass::dispatcher_->dispatch_nolock(obj);
}
[[deprecated("provide key explicitly")]]
void dispatch(const typename BaseClass::Type &obj){
return dispatch(static_cast<K>(obj), obj);
}
operator typename BaseClass::Callable() { return typename BaseClass::Callable(this,&FilteredDispatcher::dispatch); }
};
} // namespace can
#endif

245
Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/dummy.h Executable file
View File

@@ -0,0 +1,245 @@
#ifndef SOCKETCAN_INTERFACE_DUMMY_H
#define SOCKETCAN_INTERFACE_DUMMY_H
#include <deque>
#include <unordered_map>
#include "interface.h"
#include "dispatcher.h"
#include "string.h"
#include "logging.h"
#include "threading.h"
#include <boost/algorithm/string.hpp>
namespace can {
class DummyBus {
public:
using FrameDispatcher = SimpleDispatcher<CommInterface::FrameListener>;
using FrameDispatcherSharedPtr = std::shared_ptr<FrameDispatcher>;
using Buses = std::unordered_map<std::string, FrameDispatcherSharedPtr>;
private:
static Buses& get_buses() {
static Buses buses;
return buses;
}
FrameDispatcherSharedPtr bus_;
public:
const std::string name;
DummyBus(const std::string& name) : name(name), bus_(get_buses().emplace(name, std::make_shared<FrameDispatcher>()).first->second) {
}
~DummyBus() {
get_buses().erase(name);
}
class Connection {
public:
inline Connection(FrameDispatcherSharedPtr bus, FrameListenerConstSharedPtr listener)
: bus_(bus), listener_(listener)
{}
void dispatch(const Frame & msg){
bus_->dispatch_filtered(msg, listener_);
}
private:
FrameDispatcherSharedPtr bus_;
FrameListenerConstSharedPtr listener_;
};
using ConnectionSharedPtr = std::shared_ptr<Connection>;
template <typename Instance, typename Callable> static inline ConnectionSharedPtr connect(const std::string & name, Instance inst, Callable callable) {
FrameDispatcherSharedPtr bus = get_buses().at(name);
return std::make_shared<Connection>(bus, bus->createListener(std::bind(callable, inst, std::placeholders::_1)));
}
};
class DummyInterface : public DriverInterface{
using FrameDispatcher = FilteredDispatcher<unsigned int, CommInterface::FrameListener>;
using StateDispatcher = SimpleDispatcher<StateInterface::StateListener>;
FrameDispatcher frame_dispatcher_;
StateDispatcher state_dispatcher_;
DummyBus::ConnectionSharedPtr bus_;
State state_;
std::deque<can::Frame> in_;
bool loopback_;
bool trace_;
boost::mutex mutex_;
boost::condition_variable cond_;
void setDriverState(State::DriverState state){
boost::mutex::scoped_lock lock(mutex_);
if(state_.driver_state != state){
state_.driver_state = state;
state_dispatcher_.dispatch(state_);
}
cond_.notify_all();
}
void enqueue(const Frame & msg){
boost::mutex::scoped_lock cond_lock(mutex_);
in_.push_back(msg);
cond_lock.unlock();
cond_.notify_all();
}
void shutdown_internal(){
setDriverState(State::closed);
bus_.reset();
};
public:
DummyInterface() : loopback_(false), trace_(false) {}
DummyInterface(bool loopback) : loopback_(loopback), trace_(false) {}
virtual ~DummyInterface() { shutdown_internal(); }
virtual bool send(const Frame & msg){
if (trace_) {
ROSCANOPEN_DEBUG("socketcan_interface", "send: " << msg);
}
if (loopback_) {
enqueue(msg);
}
bus_->dispatch(msg);
return true;
}
virtual FrameListenerConstSharedPtr createMsgListener(const FrameFunc &delegate){
return frame_dispatcher_.createListener(delegate);
}
virtual FrameListenerConstSharedPtr createMsgListener(const Frame::Header&h , const FrameFunc &delegate){
return frame_dispatcher_.createListener(h.key(), delegate);
}
// methods from StateInterface
virtual bool recover(){return false;};
virtual State getState(){
boost::mutex::scoped_lock cond_lock(mutex_);
return state_;
}
virtual void shutdown(){
flush();
shutdown_internal();
};
virtual bool translateError(unsigned int internal_error, std::string & str){
if (!internal_error) {
str = "OK";
return true;
}
return false;
};
virtual bool doesLoopBack() const {
return loopback_;
};
void flush(){
while (true) {
{
boost::mutex::scoped_lock cond_lock(mutex_);
if (in_.empty() || state_.driver_state == State::closed) {
return;
}
}
boost::this_thread::sleep_for(boost::chrono::milliseconds(100));
}
}
virtual void run(){
boost::mutex::scoped_lock cond_lock(mutex_);
while (true) {
state_.driver_state = State::ready;
state_dispatcher_.dispatch(state_);
cond_.wait_for(cond_lock, boost::chrono::seconds(1));
while(!in_.empty()){
const can::Frame msg = in_.front();
in_.pop_front();
if (trace_) {
ROSCANOPEN_DEBUG("socketcan_interface", "receive: " << msg);
}
frame_dispatcher_.dispatch(msg.key(), msg);
}
if (state_.driver_state == State::closed) {
return;
}
}
}
bool init(const std::string &device, bool loopback){
loopback_ = loopback;
bus_ = DummyBus::connect(device, this, &DummyInterface::enqueue);
setDriverState(State::open);
return true;
}
virtual bool init(const std::string &device, bool loopback, SettingsConstSharedPtr settings) {
if(DummyInterface::init(device, loopback)) {
trace_ = settings->get_optional("trace", false);
return true;
} else {
return false;
}
}
virtual StateListenerConstSharedPtr createStateListener(const StateFunc &delegate){
return state_dispatcher_.createListener(delegate);
}
};
using DummyInterfaceSharedPtr = std::shared_ptr<DummyInterface>;
using ThreadedDummyInterface = ThreadedInterface<DummyInterface>;
using ThreadedDummyInterfaceSharedPtr = std::shared_ptr<ThreadedDummyInterface>;
class DummyResponder {
public:
DummyResponder() : dummy_(), listener_(dummy_.createMsgListenerM(this, &DummyResponder::respond)) {
}
bool init(const DummyBus &bus) {
return dummy_.init(bus.name, false, NoSettings::create());
}
void flush() {
dummy_.flush();
}
virtual ~DummyResponder() {}
protected:
void send(const Frame & msg) {
dummy_.send(msg);
}
private:
ThreadedDummyInterface dummy_;
FrameListenerConstSharedPtr listener_;
virtual void respond(const Frame & msg) = 0;
};
class DummyReplay : public DummyResponder {
private:
virtual void respond(const Frame & msg) {
const auto &front = replay_.front();
if (tostring(msg, true) == front.first) {
for(auto &f: front.second) {
send(f);
}
replay_.pop_front();
}
}
std::list<std::pair<std::string, std::vector<Frame> > > replay_;
bool error_;
public:
void add(const std::string &read, const std::initializer_list<std::string> &write){
std::vector<Frame> frames;
frames.reserve(write.size());
for(auto &w : write) {
frames.push_back(toframe(w));
}
replay_.push_back(std::make_pair(boost::to_lower_copy(read), frames));
}
void add(const std::string &read, const std::string &write){
add(read, {write});
}
bool done() { return replay_.empty(); }
};
}
#endif

70
Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/filter.h Executable file
View File

@@ -0,0 +1,70 @@
#ifndef SOCKETCAN_INTERFACE_FILTER_H
#define SOCKETCAN_INTERFACE_FILTER_H
#include <vector>
#include "interface.h"
namespace can {
class FrameFilter {
public:
virtual bool pass(const can::Frame &frame) const = 0;
virtual ~FrameFilter() {}
};
using FrameFilterSharedPtr = std::shared_ptr<FrameFilter>;
class FrameMaskFilter : public FrameFilter {
public:
static const uint32_t MASK_ALL = 0xffffffff;
static const uint32_t MASK_RELAXED = ~Frame::EXTENDED_MASK;
FrameMaskFilter(uint32_t can_id, uint32_t mask = MASK_RELAXED, bool invert = false)
: mask_(mask), masked_id_(can_id & mask), invert_(invert)
{}
virtual bool pass(const can::Frame &frame) const{
const uint32_t k = frame.key();
return ((mask_ & k) == masked_id_) != invert_;
}
private:
const uint32_t mask_;
const uint32_t masked_id_;
const bool invert_;
};
class FrameRangeFilter : public FrameFilter {
public:
FrameRangeFilter(uint32_t min_id, uint32_t max_id, bool invert = false)
: min_id_(min_id), max_id_(max_id), invert_(invert)
{}
virtual bool pass(const can::Frame &frame) const{
const uint32_t k = frame.key();
return (min_id_ <= k && k <= max_id_) != invert_;
}
private:
const uint32_t min_id_;
const uint32_t max_id_;
const bool invert_;
};
class FilteredFrameListener : public CommInterface::FrameListener {
public:
using FilterVector = std::vector<FrameFilterSharedPtr>;
FilteredFrameListener(CommInterfaceSharedPtr comm, const Callable &callable, const FilterVector &filters)
: CommInterface::FrameListener(callable),
filters_(filters),
listener_(comm->createMsgListener([this](const Frame &frame) {
for(FilterVector::const_iterator it=this->filters_.begin(); it != this->filters_.end(); ++it) {
if((*it)->pass(frame)){
(*this)(frame);
break;
}
}
}))
{}
const std::vector<FrameFilterSharedPtr> filters_;
CommInterface::FrameListenerConstSharedPtr listener_;
};
} // namespace can
#endif /*SOCKETCAN_INTERFACE_FILTER_H*/

230
Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/interface.h Executable file
View File

@@ -0,0 +1,230 @@
#ifndef H_CAN_INTERFACE
#define H_CAN_INTERFACE
#include <array>
#include <memory>
#include <functional>
#include <boost/system/error_code.hpp>
#include "socketcan_interface/delegates.h"
#include "socketcan_interface/logging.h"
#include "socketcan_interface/settings.h"
namespace can{
/** Header for CAN id an meta data*/
struct Header{
static const unsigned int ID_MASK = (1u << 29)-1;
static const unsigned int ERROR_MASK = (1u << 29);
static const unsigned int RTR_MASK = (1u << 30);
static const unsigned int EXTENDED_MASK = (1u << 31);
unsigned int id:29; ///< CAN ID (11 or 29 bits valid, depending on is_extended member
unsigned int is_error:1; ///< marks an error frame (only used internally)
unsigned int is_rtr:1; ///< frame is a remote transfer request
unsigned int is_extended:1; ///< frame uses 29 bit CAN identifier
/** check if frame header is valid*/
bool isValid() const{
return id < (is_extended?(1<<29):(1<<11));
}
unsigned int fullid() const { return id | (is_error?ERROR_MASK:0) | (is_rtr?RTR_MASK:0) | (is_extended?EXTENDED_MASK:0); }
unsigned int key() const { return is_error ? (ERROR_MASK) : fullid(); }
[[deprecated("use key() instead")]] explicit operator unsigned int() const { return key(); }
/** constructor with default parameters
* @param[in] i: CAN id, defaults to 0
* @param[in] extended: uses 29 bit identifier, defaults to false
* @param[in] rtr: is rtr frame, defaults to false
*/
Header()
: id(0),is_error(0),is_rtr(0), is_extended(0) {}
Header(unsigned int i, bool extended, bool rtr, bool error)
: id(i),is_error(error?1:0),is_rtr(rtr?1:0), is_extended(extended?1:0) {}
};
struct MsgHeader : public Header{
MsgHeader(unsigned int i=0, bool rtr = false) : Header(i, false, rtr, false) {}
};
struct ExtendedHeader : public Header{
ExtendedHeader(unsigned int i=0, bool rtr = false) : Header(i, true, rtr, false) {}
};
struct ErrorHeader : public Header{
ErrorHeader(unsigned int i=0) : Header(i, false, false, true) {}
};
/** representation of a CAN frame */
struct Frame: public Header{
using value_type = unsigned char;
std::array<value_type, 8> data; ///< array for 8 data bytes with bounds checking
unsigned char dlc; ///< len of data
/** check if frame header and length are valid*/
bool isValid() const{
return (dlc <= 8) && Header::isValid();
}
/**
* constructor with default parameters
* @param[in] i: CAN id, defaults to 0
* @param[in] l: number of data bytes, defaults to 0
* @param[in] extended: uses 29 bit identifier, defaults to false
* @param[in] rtr: is rtr frame, defaults to false
*/
Frame() : Header(), dlc(0) {}
Frame(const Header &h, unsigned char l = 0) : Header(h), dlc(l) {}
value_type * c_array() { return data.data(); }
const value_type * c_array() const { return data.data(); }
};
/** extended error information */
class State{
public:
enum DriverState{
closed, open, ready
} driver_state;
boost::system::error_code error_code; ///< device access error
unsigned int internal_error; ///< driver specific error
State() : driver_state(closed), internal_error(0) {}
virtual bool isReady() const { return driver_state == ready; }
virtual ~State() {}
};
/** template for Listener interface */
template <typename T,typename U> class Listener{
const T callable_;
public:
using Type = U;
using Callable = T;
using ListenerConstSharedPtr = std::shared_ptr<const Listener>;
Listener(const T &callable):callable_(callable){ }
void operator()(const U & u) const { if(callable_) callable_(u); }
virtual ~Listener() {}
};
class StateInterface{
public:
using StateFunc = std::function<void(const State&)>;
using StateDelegate [[deprecated("use StateFunc instead")]] = DelegateHelper<StateFunc>;
using StateListener = Listener<const StateFunc, const State&>;
using StateListenerConstSharedPtr = StateListener::ListenerConstSharedPtr;
/**
* acquire a listener for the specified delegate, that will get called for all state changes
*
* @param[in] delegate: delegate to be bound by the listener
* @return managed pointer to listener
*/
virtual StateListenerConstSharedPtr createStateListener(const StateFunc &delegate) = 0;
template <typename Instance, typename Callable> inline StateListenerConstSharedPtr createStateListenerM(Instance inst, Callable callable) {
return this->createStateListener(std::bind(callable, inst, std::placeholders::_1));
}
virtual ~StateInterface() {}
};
using StateInterfaceSharedPtr = std::shared_ptr<StateInterface>;
using StateListenerConstSharedPtr = StateInterface::StateListenerConstSharedPtr;
class CommInterface{
public:
using FrameFunc = std::function<void(const Frame&)>;
using FrameDelegate [[deprecated("use FrameFunc instead")]] = DelegateHelper<FrameFunc>;
using FrameListener = Listener<const FrameFunc, const Frame&>;
using FrameListenerConstSharedPtr = FrameListener::ListenerConstSharedPtr;
/**
* enqueue frame for sending
*
* @param[in] msg: message to be enqueued
* @return true if frame was enqueued succesfully, otherwise false
*/
virtual bool send(const Frame & msg) = 0;
/**
* acquire a listener for the specified delegate, that will get called for all messages
*
* @param[in] delegate: delegate to be bound by the listener
* @return managed pointer to listener
*/
virtual FrameListenerConstSharedPtr createMsgListener(const FrameFunc &delegate) = 0;
template <typename Instance, typename Callable> inline FrameListenerConstSharedPtr createMsgListenerM(Instance inst, Callable callable) {
return this->createMsgListener(std::bind(callable, inst, std::placeholders::_1));
}
/**
* acquire a listener for the specified delegate, that will get called for messages with demanded ID
*
* @param[in] header: CAN header to restrict listener on
* @param[in] delegate: delegate to be bound listener
* @return managed pointer to listener
*/
virtual FrameListenerConstSharedPtr createMsgListener(const Frame::Header&, const FrameFunc &delegate) = 0;
template <typename Instance, typename Callable> inline FrameListenerConstSharedPtr createMsgListenerM(const Frame::Header& header, Instance inst, Callable callable) {
return this->createMsgListener(header, std::bind(callable, inst, std::placeholders::_1));
}
virtual ~CommInterface() {}
};
using CommInterfaceSharedPtr = std::shared_ptr<CommInterface>;
using FrameListenerConstSharedPtr = CommInterface::FrameListenerConstSharedPtr;
class DriverInterface : public CommInterface, public StateInterface {
public:
[[deprecated("provide settings explicitly")]] virtual bool init(const std::string &device, bool loopback) = 0;
/**
* initialize interface
*
* @param[in] device: driver-specific device name/path
* @param[in] loopback: loop-back own messages
* @param[in] settings: driver-specific settings
* @return true if device was initialized succesfully, false otherwise
*/
virtual bool init(const std::string &device, bool loopback, SettingsConstSharedPtr settings) {
ROSCANOPEN_ERROR("socketcan_interface", "Driver does not support custom settings");
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
return init(device, loopback);
#pragma GCC diagnostic pop
}
/**
* Recover interface after errors and emergency stops
*
* @return true if device was recovered succesfully, false otherwise
*/
virtual bool recover() = 0;
/**
* @return current state of driver
*/
virtual State getState() = 0;
/**
* shutdown interface
*
* @return true if shutdown was succesful, false otherwise
*/
virtual void shutdown() = 0;
virtual bool translateError(unsigned int internal_error, std::string & str) = 0;
virtual bool doesLoopBack() const = 0;
virtual void run() = 0;
virtual ~DriverInterface() {}
};
using DriverInterfaceSharedPtr = std::shared_ptr<DriverInterface>;
} // namespace can
#endif

17
Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/logging.h Executable file
View File

@@ -0,0 +1,17 @@
#ifndef SOCKETCAN_INTERFACE_LOGGING_H
#define SOCKETCAN_INTERFACE_LOGGING_H
#include <console_bridge/console.h>
#include <sstream>
#define ROSCANOPEN_LOG(name, file, line, level, args) { std::stringstream sstr; sstr << name << ": " << args; console_bridge::getOutputHandler()->log(sstr.str(), level, file, line); }
#define ROSCANOPEN_ERROR(name, args) ROSCANOPEN_LOG(name, __FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_ERROR, args)
#define ROSCANOPEN_INFO(name, args) ROSCANOPEN_LOG(name, __FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_INFO, args)
#define ROSCANOPEN_WARN(name, args) ROSCANOPEN_LOG(name, __FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_WARN, args)
#define ROSCANOPEN_DEBUG(name, args) ROSCANOPEN_LOG(name, __FILE__, __LINE__,console_bridge::CONSOLE_BRIDGE_LOG_DEBUG, args)
// extra function to mark it as deprecated
inline __attribute__ ((deprecated("please use ROSCANOPEN_* macros"))) void roscanopen_log_deprecated(const std::string s, const char* f, int l) { console_bridge::getOutputHandler()->log(s, console_bridge::CONSOLE_BRIDGE_LOG_ERROR, f, l); }
#define LOG(args) { std::stringstream sstr; sstr << "LOG: " << args; roscanopen_log_deprecated(sstr.str(), __FILE__, __LINE__); }
#endif

7
Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/make_shared.h Executable file
View File

@@ -0,0 +1,7 @@
#ifndef SOCKETCAN_INTERFACE_MAKE_SHARED_H
#define SOCKETCAN_INTERFACE_MAKE_SHARED_H
#include <memory>
#define ROSCANOPEN_MAKE_SHARED std::make_shared
#endif // ! SOCKETCAN_INTERFACE_MAKE_SHARED_H

114
Devices/Libraries/Ros/ros_canopen/socketcan_interface/include/socketcan_interface/reader.h Executable file
View File

@@ -0,0 +1,114 @@
#ifndef H_CAN_BUFFERED_READER
#define H_CAN_BUFFERED_READER
#include <socketcan_interface/interface.h>
#include <deque>
#include <boost/thread/mutex.hpp>
#include <boost/thread/condition_variable.hpp>
#include <boost/chrono.hpp>
namespace can{
class BufferedReader {
std::deque<can::Frame> buffer_;
boost::mutex mutex_;
boost::condition_variable cond_;
CommInterface::FrameListenerConstSharedPtr listener_;
bool enabled_;
size_t max_len_;
void trim(){
if(max_len_ > 0){
while(buffer_.size() > max_len_){
ROSCANOPEN_ERROR("socketcan_interface", "buffer overflow, discarded oldest message " /*<< tostring(buffer_.front())*/); // enable message printing
buffer_.pop_front();
}
}
}
void handleFrame(const can::Frame & msg){
boost::mutex::scoped_lock lock(mutex_);
if(enabled_){
buffer_.push_back(msg);
trim();
cond_.notify_one();
}else{
ROSCANOPEN_WARN("socketcan_interface", "discarded message " /*<< tostring(msg)*/); // enable message printing
}
}
public:
class ScopedEnabler{
BufferedReader &reader_;
bool before_;
public:
ScopedEnabler(BufferedReader &reader) : reader_(reader), before_(reader_.setEnabled(true)) {}
~ScopedEnabler() { reader_.setEnabled(before_); }
};
BufferedReader() : enabled_(true), max_len_(0) {}
BufferedReader(bool enable, size_t max_len = 0) : enabled_(enable), max_len_(max_len) {}
void flush(){
boost::mutex::scoped_lock lock(mutex_);
buffer_.clear();
}
void setMaxLen(size_t max_len){
boost::mutex::scoped_lock lock(mutex_);
max_len_ = max_len;
trim();
}
bool isEnabled(){
boost::mutex::scoped_lock lock(mutex_);
return enabled_;
}
bool setEnabled(bool enabled){
boost::mutex::scoped_lock lock(mutex_);
bool before = enabled_;
enabled_ = enabled;
return before;
}
void enable(){
boost::mutex::scoped_lock lock(mutex_);
enabled_ = true;
}
void disable(){
boost::mutex::scoped_lock lock(mutex_);
enabled_ = false;
}
void listen(CommInterfaceSharedPtr interface){
boost::mutex::scoped_lock lock(mutex_);
listener_ = interface->createMsgListenerM(this, &BufferedReader::handleFrame);
buffer_.clear();
}
void listen(CommInterfaceSharedPtr interface, const Frame::Header& h){
boost::mutex::scoped_lock lock(mutex_);
listener_ = interface->createMsgListenerM(h, this, &BufferedReader::handleFrame);
buffer_.clear();
}
template<typename DurationType> bool read(can::Frame * msg, const DurationType &duration){
return readUntil(msg, boost::chrono::high_resolution_clock::now() + duration);
}
bool readUntil(can::Frame * msg, boost::chrono::high_resolution_clock::time_point abs_time){
boost::mutex::scoped_lock lock(mutex_);
while(buffer_.empty() && cond_.wait_until(lock,abs_time) != boost::cv_status::timeout)
{}
if(buffer_.empty()){
return false;
}
if(msg){
*msg = buffer_.front();
buffer_.pop_front();
}
return true;
}
};
} // namespace can
#endif

Some files were not shown because too many files have changed in this diff Show More