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

10
Devices/Packages/sick_line_guidance/.gitignore vendored Executable file
View File

@@ -0,0 +1,10 @@
turtlebotDemo/test/scripts/sick_line_guidance_demo\.avi
turtlebotDemo/iam/cmake/decision_making_parsing\.cmake
turtlebotDemo/test/scripts/log/
\.idea/
cmake-build-debug/

332
Devices/Packages/sick_line_guidance/CMakeLists.txt Executable file
View File

@@ -0,0 +1,332 @@
cmake_minimum_required(VERSION 2.8.3)
project(sick_line_guidance)
##
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++11 -g -Wall -Wno-reorder -Wno-sign-compare -Wno-unused-local-typedefs)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
cmake_modules
can_msgs
canopen_chain_node
message_generation
random_numbers
roscpp
rospy
sensor_msgs
std_msgs
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
find_package(TinyXML REQUIRED)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a exec_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
add_message_files(
FILES
MLS_Measurement.msg
OLS_Measurement.msg
)
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
sensor_msgs
std_msgs
can_msgs
)
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
LIBRARIES sick_line_guidance_lib
CATKIN_DEPENDS can_msgs canopen_chain_node message_runtime random_numbers roscpp rospy sensor_msgs std_msgs
DEPENDS TinyXML # system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
include
${catkin_INCLUDE_DIRS}
${TinyXML_INCLUDE_DIRS}
)
## Declare a C++ library
add_library(${PROJECT_NAME}_lib
src/sick_line_guidance_can_subscriber.cpp
src/sick_line_guidance_can_cia401_subscriber.cpp
src/sick_line_guidance_can_mls_subscriber.cpp
src/sick_line_guidance_can_ols_subscriber.cpp
src/sick_line_guidance_canopen_chain.cpp
src/sick_line_guidance_cloud_converter.cpp
src/sick_line_guidance_diagnostic.cpp
src/sick_line_guidance_eds_util.cpp
src/sick_line_guidance_msg_util.cpp
src/sick_canopen_simu_canstate.cpp
src/sick_canopen_simu_device.cpp
src/sick_canopen_simu_verify.cpp
)
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(${PROJECT_NAME}_node src/sick_line_guidance_node.cpp)
add_executable(${PROJECT_NAME}_can_chain_node src/sick_line_guidance_can_chain_node.cpp)
add_executable(${PROJECT_NAME}_cloud_publisher src/sick_line_guidance_cloud_publisher.cpp)
add_executable(${PROJECT_NAME}_can2ros_node src/sick_line_guidance_can2ros_node.cpp)
add_executable(${PROJECT_NAME}_ros2can_node src/sick_line_guidance_ros2can_node.cpp)
add_executable(sick_canopen_simu_node src/sick_canopen_simu_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
add_dependencies(${PROJECT_NAME}_node
random_numbers
canopen_ros_chain
${PROJECT_NAME}_lib
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
add_dependencies(${PROJECT_NAME}_can_chain_node
canopen_ros_chain
${PROJECT_NAME}_lib
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
add_dependencies(${PROJECT_NAME}_cloud_publisher
${PROJECT_NAME}_lib
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
add_dependencies(${PROJECT_NAME}_can2ros_node
canopen_ros_chain
${PROJECT_NAME}_lib
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
add_dependencies(${PROJECT_NAME}_ros2can_node
canopen_ros_chain
${PROJECT_NAME}_lib
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
add_dependencies(${PROJECT_NAME}_lib
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
add_dependencies(sick_canopen_simu_node
random_numbers
canopen_ros_chain
${PROJECT_NAME}_lib
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
## Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}_node
canopen_ros_chain
random_numbers
${PROJECT_NAME}_lib
${Boost_LIBRARIES}
${catkin_LIBRARIES}
${TinyXML_LIBRARIES}
)
target_link_libraries(${PROJECT_NAME}_can_chain_node
canopen_ros_chain
${PROJECT_NAME}_lib
${Boost_LIBRARIES}
${catkin_LIBRARIES}
${TinyXML_LIBRARIES}
)
target_link_libraries(${PROJECT_NAME}_cloud_publisher
${PROJECT_NAME}_lib
${Boost_LIBRARIES}
${catkin_LIBRARIES}
${TinyXML_LIBRARIES}
)
target_link_libraries(${PROJECT_NAME}_can2ros_node
canopen_ros_chain
${PROJECT_NAME}_lib
${Boost_LIBRARIES}
${catkin_LIBRARIES}
${TinyXML_LIBRARIES}
)
target_link_libraries(${PROJECT_NAME}_ros2can_node
canopen_ros_chain
${PROJECT_NAME}_lib
${Boost_LIBRARIES}
${catkin_LIBRARIES}
${TinyXML_LIBRARIES}
)
target_link_libraries(${PROJECT_NAME}_lib
${Boost_LIBRARIES}
${catkin_LIBRARIES}
${TinyXML_LIBRARIES}
)
target_link_libraries(sick_canopen_simu_node
canopen_ros_chain
random_numbers
${PROJECT_NAME}_lib
${Boost_LIBRARIES}
${catkin_LIBRARIES}
${TinyXML_LIBRARIES}
)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
install(TARGETS ${PROJECT_NAME}_node ${PROJECT_NAME}_can_chain_node ${PROJECT_NAME}_cloud_publisher ${PROJECT_NAME}_can2ros_node ${PROJECT_NAME}_ros2can_node sick_canopen_simu_node ${PROJECT_NAME}_lib
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE
)
## Mark other files for installation (e.g. launch and bag files, etc.)
install(FILES
launch/sick_line_guidance.launch
launch/sick_line_guidance_can2ros_node.launch
launch/sick_line_guidance_ols20_twin.launch
launch/sick_line_guidance_ros2can_node.launch
launch/sick_canopen_simu.launch
mls/SICK-MLS.eds
mls/sick_line_guidance_mls.yaml
ols/SICK_OLS10_CO.eds
ols/SICK_OLS20.eds
ols/SICK_OLS20_CO.eds
ols/sick_line_guidance_ols10.yaml
ols/sick_line_guidance_ols20.yaml
ols/sick_line_guidance_ols20_twin.yaml
test/cfg/sick_canopen_simu_cfg.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_sick_line_guidance.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)
if( TURTLEBOT_DEMO )
add_subdirectory( turtlebotDemo/agc_radar )
add_subdirectory( turtlebotDemo/custom_messages )
add_subdirectory( turtlebotDemo/gpio_handling )
add_subdirectory( turtlebotDemo/lidar_obstacle_detection )
add_subdirectory( turtlebotDemo/sick_line_guidance_demo )
endif( TURTLEBOT_DEMO )

201
Devices/Packages/sick_line_guidance/LICENSE Executable file
View File

@@ -0,0 +1,201 @@
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following
boilerplate notice, with the fields enclosed by brackets "[]"
replaced with your own identifying information. (Don't include
the brackets!) The text should be enclosed in the appropriate
comment syntax for the file format. We also recommend that a
file or class name and description of purpose be included on the
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright [yyyy] [name of copyright owner]
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

314
Devices/Packages/sick_line_guidance/README.md Executable file
View File

@@ -0,0 +1,314 @@
# sick_line_guidance
SICK Line Guidance ROS Support
# Introduction
The aim of this project is to provide an ROS connection for lane guidance sensors of the OLS10, OLS20 and MLS family.
Users should regularly inform themselves about updates to this driver (best subscribe under "Watch").
# Supported Hardware
SICK optical and magnetical line sensors OLS10, OLS20 and MLS.
| Product Family | Product Information and Manuals |
| --- | --- |
| OLS10 | https://www.sick.com/ols10 |
| OLS20 | https://www.sick.com/ols20 |
| MLS | https://www.sick.com/mls |
# Installation and setup
## Setup CAN hardware and driver
  1. Install can-utils and socketcan:
```bash
sudo apt-get install can-utils
sudo modprobe can
sudo modprobe vcan
sudo modprobe slcan
```
Details can be found in the following links: https://wiki.linklayer.com/index.php/SocketCAN , https://gribot.org/installing-socketcan/ ,
https://www.kernel.org/doc/Documentation/networking/can.txt
  2. Install linux driver for your CAN hardware:
ROS support for sick line guidance sensors has been developed and tested using PEAK CAN adapter for the CAN communication.
Unless you're using other CAN hardware or driver, we recommend installation of pcan usb adapter by following the installation
instructions on [doc/pcan-linux-installation.md](doc/pcan-linux-installation.md)
## Installation from Source
Run the following script to install sick_line_guidance including all dependancies and packages required:
```bash
source /opt/ros/noetic/setup.bash # currently ros distro melodic and noetic are supported
cd ~ # or change to your project path
mkdir -p catkin_ws/src/
cd catkin_ws/src/
git clone https://github.com/SICKAG/sick_line_guidance.git
git clone https://github.com/ros-industrial/ros_canopen.git
git clone https://github.com/CANopenNode/CANopenSocket.git
git clone https://github.com/linux-can/can-utils.git
git clone https://github.com/ros-planning/random_numbers.git
cd ..
sudo apt-get install libtinyxml-dev
sudo apt-get install libmuparser-dev
catkin_make install
source ./install/setup.bash
```
# Configuration
If not done before, you have to set the CAN bitrate, f.e. to 125 kbit/s (default bitrate for OLS and MLS):
```bash
sudo ip link set can0 up type can bitrate 125000
ip -details link show can0
```
The can node id of the OLS or MLS device is configured in a yaml-file:
- catkin_ws/src/sick_line_guidance/ols/sick_line_guidance_ols10.yaml for OLS10 devices
- catkin_ws/src/sick_line_guidance/ols/sick_line_guidance_ols20.yaml for OLS20 devices
- catkin_ws/src/sick_line_guidance/mls/sick_line_guidance_mls.yaml for MLS devices
The default can node id is 0x0A. To use a different can node id, set entry "id: 0x0A" to the appropriate value in the yaml-file:
```bash
node1:
id: 0x0A # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
```
Install the new configuration with
```bash
cd catkin_ws
catkin_make install
source ./install/setup.bash
```
after modifications.
See [sick_line_guidance configuration](doc/sick_line_guidance_configuration.md) for details about the
sick_line_guidance driver and sensor configuration. See [Configuration for multiple devices](doc/sick_line_guidance_configuration.md#configuration-for-multiple-devices) for the configuration of multiple devices, f.e. of two OLS20 devices.
# Starting
To start the driver for MLS or OLS, the ros package sick_line_guidance and its launch-file has to be started by roslaunch:
```bash
cd ~/catkin_ws # change working directory to the project path
source ./install/setup.bash # set environment
roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_ols10.yaml # start OLS10 driver
roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_ols20.yaml # start OLS20 driver
roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_mls.yaml # start MLS driver
```
After successful start, you can observe the sensor measurement data in a shell by subscribing to ros topics "/ols", "/mls" and "/cloud":
```bash
source ./install/setup.bash
rostopic list
rostopic echo /mls
rostopic echo /ols
rostopic echo /cloud
```
or you can plot the sensor positions by
```bash
source ./install/setup.bash
rqt_plot /mls/position[0] /mls/position[1] /mls/position[2] # plot mls positions
rqt_plot /ols/position[0] /ols/position[1] /ols/position[2] # plot ols positions
```
You can visualize the data by starting rviz, subscribe to topic "/cloud" (PointCloud2) and select "ols_frame" or "mls_frame":
```bash
source ./install/setup.bash
rosrun rviz rviz
```
# Troubleshooting and diagnostics
All measurements are published continously on ros topic "/mls" resp. "/ols". In addition, the current status (ok or error)
and (in case of errors) an error code is published on topic "/diagnostics". Messages on these topics can be views by
```bash
rostopic echo /mls
rostopic echo /ols
rostopic echo /diagnostics
```
The following error codes are defined in header file sick_line_guidance_diagnostic.h:
```bash
/*
* enum DIAGNOSTIC_STATUS enumerates the possible status values of diagnostic messages.
* Higher values mean more severe failures.
*/
typedef enum DIAGNOSTIC_STATUS_ENUM
{
OK, // status okay, no errors
EXIT, // sick_line_guidance exiting
NO_LINE_DETECTED, // device signaled "no line detected"
ERROR_STATUS, // device signaled an error (error flag set in TPDO)
SDO_COMMUNICATION_ERROR, // error in SDO query, timeout on receiving SDO response
CAN_COMMUNICATION_ERROR, // can communication error, shutdown and reset communication
CONFIGURATION_ERROR, // invalid configuration, check configuration files
INITIALIZATION_ERROR, // initialization of CAN driver failed
INTERNAL_ERROR // internal error, should never happen
} DIAGNOSTIC_STATUS;
```
All data transmitted on the CAN bus can be displayed by candump:
```bash
candump -ta can0
```
In case of a successful installation, heartbeats and PDO messages should be visible.
Example output (can master requests object 1018sub4 from can node 8, device responds with its serial number 0x13015015):
```bash
$ candump -ta can0
(1549455524.265601) can0 77F [1] 05 # heartbeat
(1549455524.294836) can0 608 [8] 40 18 10 04 00 00 00 00 # SDO request (0x600+nodeid), 8 byte (0x40) data, object 0x101804
(1549455524.301181) can0 588 [8] 43 18 10 04 15 50 01 13 # SDO response (0x580+nodeid), 4 byte (0x43) value, object 0x101804, value 0x13015015
```
Values in the object dictionary of the CAN device can be viewed by
```bash
# rosservice call /driver/get_object node1 <object_index> <cached>
rosservice call /driver/get_object node1 1018sub4 false # query serial number
```
Example output:
```bash
$ rosservice call /driver/get_object node1 1018sub4 false # query serial number
success: True
message: ''
value: "318853141"
```
The error register of a can device (object index 0x1001) and its pdo mapped objects are published on ros topic "node1_<object_index>"
and can be printed by rostopic:
```bash
# rostopic echo -n 1 /node1_<object_index> # print object <object_index> of can device
rostopic echo -n 1 /node1_1001 # print error register 0x1001 of can device
```
Example output:
```bash
$ rostopic echo -n 1 /node1_1001
data: 0
```
For test purposes or in case of hardware problems, cansend can be used to send CAN messages. Example:
```bash
cansend can0 000#820A # NMT message to can device 0x0A: 0x82, reset communication
cansend can0 60A#4F01100000000000 # PDO request: read error register 1001
```
## Simulation and testing
A software simulation is available for test purposes. This simulation generates synthetical can frames from an input xml-file, and verifies the measurement messages published by
the sick_line_guidance driver. By sending specified can frames, the complete processing chain of the ros driver can be verified by comparing the actual measurement messages
with the expected results. See https://github.com/SICKAG/sick_line_guidance/tree/master/doc/sick_canopen_simu.md for further details.
## TurtleBot demonstration
A demonstration of SICK line guidance with a TurtleBot robot is included in folder sick_line_guidance/turtlebotDemo.
Please see [turtlebotDemo/README.md](turtlebotDemo/README.md) for further details.
## FAQ
### "Network is down"
:question: Question:
```bash
candump -ta can0
```
gives the result
```bash
read: Network is down
```
:white_check_mark: Answer:
(Re-)start can interface by the following commands:
```bash
sudo ip link set can0 type can
sudo ip link set can0 up type can bitrate 125000 # configure the CAN bitrate, f.e. 125000 bit/s
```
### "candump gives no answer"
:question: Question:
```bash
candump -ta can0
```
gives no results.
:white_check_mark: Answer:
Check the baud rate of your device. For a brand new OLS10 this could be 250000 Baud.
For OLS10 please check the baud rate setting by using the device panel (read operation manual of your device).
### "device or resource busy"
:question: Question:
```bash
sudo ip link set can0 up type can bitrate 125000
```
gives the result:
```bash
RTNETLINK answers: Device or resource busy
```
:white_check_mark: Answer:
Check the baud rate of your device. For a brand new OLS10 this could be 250000 Baud.
For OLS10 please check the baud rate setting by using the device panel (read operation manual of your device).
After checking (and changing) the baud rate unplug and replug the usb connector.
### "Device 'can0' does not exist"
:question: Question: After start, the message
```bash
Device "can0" does not exist.
```
is displayed.
:white_check_mark: Answer:
- Check power supply
- Unplug, replug and restart PEAK-USB-Adapter
- If you use a PEAK-USB-Adapter and the error message still displays, re-install the PCAN-driver.
PCAN driver can be overwritten by a default can driver due to system updates ("mainline drivers removed and blacklisted in /etc/modprobe.d/blacklist-peak.conf").
In this case, the PCAN driver must be re-installed. See the quick installation guide https://github.com/SICKAG/sick_line_guidance/tree/master/doc/pcan-linux-installation.md
### "Configuration changes do not take effect"
:question: Question: After editing configuration files, the configuration changes do not take effect.
:white_check_mark: Answer: Modified configuration files have to be installed by
```bash
cd catkin_ws
catkin_make install
source ./install/setup.bash
```
Restart the driver for MLS or OLS. To avoid potential errors due to previous ros nodes or processes still running,
you might kill all ros nodes and the ros core by
```bash
rosnode kill -a # kill all ros nodes
killall rosmaster # kill ros core
```
Please note, that this kills all ros processes, not just those required for sick_line_guidance.
### "Debugging"
:question: Question: How can I run sick_line_guidance or ros_canopen in a debugger, e.g. gdb
:white_check_mark: Answer: A ros node can be started in gdb with prefix gdb, e.g.
```bash
rosrun --prefix 'gdb -ex run --args' sick_line_guidance ...
```
or with argument `launch-prefix="gdb -ex run - -args"` in the launchfile, e.g.
```
<node name="sick_line_guidance_can_chain_node" pkg="sick_line_guidance" type="sick_line_guidance_can_chain_node" launch-prefix="gdb -ex run - -args" output="screen" >
```

54
Devices/Packages/sick_line_guidance/doc/copyright.txt Executable file
View File

@@ -0,0 +1,54 @@
/*
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/

BIN
Devices/Packages/sick_line_guidance/doc/pcan-config/screenshot01.png Executable file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 152 KiB

BIN
Devices/Packages/sick_line_guidance/doc/pcan-config/screenshot02.png Executable file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 102 KiB

BIN
Devices/Packages/sick_line_guidance/doc/pcan-config/screenshot03.png Executable file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 119 KiB

70
Devices/Packages/sick_line_guidance/doc/pcan-linux-installation.md Executable file
View File

@@ -0,0 +1,70 @@
# pcan-linux-installation
ROS support for sick line guidance sensors has been developed and tested using PEAK CAN adapter for the CAN communication.
This is a quick howto install linux driver for the pcan usb adapter.
# Installation
The following installation is recommended:
&nbsp; 1. Download PCAN-View for Linux from https://www.peak-system.com/linux/ or https://www.peak-system.com/quick/ViewLinux_amd64/pcanview-ncurses_0.8.7-0_amd64.deb and install:
```bash
sudo apt-get install libncurses5
sudo dpkg --install pcanview-ncurses_0.8.7-0_amd64.deb
```
&nbsp; 2. If you're running ROS in a virtual machine, make sure the usb-port for the PCAN-USB-adapter is connected to your VM.
&nbsp; 3. Download and unzip peak-linux-driver-8.17.0.tar.gz from https://www.peak-system.com/quick/PCAN-Linux-Driver
&nbsp; 4. Install the linux driver and required packages:
```bash
cd peak-linux-driver-8.17.0
# install required packages
sudo apt-get install can-utils
sudo apt-get install gcc-multilib
sudo apt-get install libelf-dev
sudo apt-get install libpopt-dev
sudo apt-get install tree
# build and install pcan driver
make clean
make NET=NETDEV_SUPPORT
sudo make install
# install the modules
sudo modprobe pcan
sudo modprobe can
sudo modprobe vcan
sudo modprobe slcan
# setup and configure "can0" net device
sudo ip link set can0 type can
sudo ip link set can0 up type can bitrate 125000 # configure the CAN bitrate, f.e. 125000 bit/s
# check installation
./driver/lspcan --all # should print "pcanusb32" and pcan version
tree /dev/pcan-usb # should show a pcan-usb device
ip -a link # should print some "can0: ..." messages
ip -details link show can0 # should print some details about "can0" net device
```
Example output after successfull installation of a pcan usb adapter:
```bash
user@ubuntu-ros:~/peak-linux-driver-8.17.0$ ./driver/lspcan --all
pcanusb32 CAN1 - 8MHz 125k ACTIVE - 1969 0 0
user@ubuntu-ros:~/peak-linux-driver-8.17.0$ tree /dev/pcan-usb
/dev/pcan-usb
├── 0
│   └── can0 -> ../../pcanusb32
└── devid=5 -> ../pcanusb32
user@ubuntu-ros:~/peak-linux-driver-8.17.0$ ip -a link
3: can0: <NOARP> mtu 16 qdisc noop state DOWN mode DEFAULT group default qlen 10
link/can
user@ubuntu-ros:~/peak-linux-driver-8.17.0$ ip -details link show can0
3: can0: <NOARP> mtu 16 qdisc noop state DOWN mode DEFAULT group default qlen 10
link/can promiscuity 0
can state STOPPED restart-ms 0
bitrate 500000 sample-point 0.875
tq 125 prop-seg 6 phase-seg1 7 phase-seg2 2 sjw 1
pcan: tseg1 1..16 tseg2 1..8 sjw 1..4 brp 1..64 brp-inc 1
clock 8000000numtxqueues 1 numrxqueues 1 gso_max_size 65536 gso_max_segs 65535
```
See https://www.peak-system.com/fileadmin/media/linux/files/PCAN-Driver-Linux_UserMan_eng.pdf for further details.

128
Devices/Packages/sick_line_guidance/doc/sick_canopen_simu.md Executable file
View File

@@ -0,0 +1,128 @@
# Simulation and testing
sick_canopen_simu implements a software simulation of MLS and OLS devices for test purposes. This simulation generates synthetical can frames from an input xml-file,
and verifies the measurement messages published by the sick_line_guidance driver. By sending specified can frames, the complete processing chain of the ros driver can be verified
by comparing the actual measurement messages with the expected results. In addition, error messages and error handling can be tested, which otherwise can be challenging.
Please note, that sick_canopen_simu does not implement or simulate the behavior of the MLS or OLS hardware. It just sends specified can frames and checks the measurement messages
from the sick_line_guidance driver after entering the operational state. The purpose of sick_canopen_simu is driver verification and testing, not a fully functional simulation of
can devices.
## Usage
To test the sick_line_guidance driver with simulated can frames, four ros nodes have to be started:
- sick_line_guidance_ros2can_node to send ros messages of type can::Frame to the can driver, which sends the frames to the can bus,
- sick_line_guidance_can2ros_node to receive can frames and to publish them as ros messages of type can::Frame,
- sick_canopen_simu to read the simulation data from file, to convert them to can::Frame messages and to verify the resulting sick_line_guidance messages from the driver,
- sick_line_guidance_node, i.e. the sick_line_guidance driver.
Example for simulation and test of the OLS20 driver:
```bash
# Start can2ros converter
roslaunch -v sick_line_guidance sick_line_guidance_can2ros_node.launch &
sleep 5
# Start ros2can converter
roslaunch -v sick_line_guidance sick_line_guidance_ros2can_node.launch &
sleep 5
# Start OLS20 simulation
roslaunch -v --screen sick_line_guidance sick_canopen_simu.launch device:=OLS20 &
sleep 5
# Start OLS driver incl. canopen_chain_node, sick_line_guidance_node and sick_line_guidance_cloud_publisher
roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_ols20.yaml
```
The same example works for MLS, too, with slightly different arguments (`device:=MLS` and `yaml:=sick_line_guidance_mls.yaml`):
```bash
# Start can2ros converter
roslaunch -v sick_line_guidance sick_line_guidance_can2ros_node.launch &
sleep 5
# Start ros2can converter
roslaunch -v sick_line_guidance sick_line_guidance_ros2can_node.launch &
sleep 5
# Start MLS simulation
roslaunch -v --screen sick_line_guidance sick_canopen_simu.launch device:=MLS &
sleep 5
# Start MLS driver incl. canopen_chain_node, sick_line_guidance_node and sick_line_guidance_cloud_publisher
roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_mls.yaml
```
To simulate and test the OLS10 driver, just use the parameter `device:=OLS10` and `yaml:=sick_line_guidance_ols10.yaml`.
If the verification of measurement messages failed, an error message is displayed.
To run an automated unittest of both the MLS, OLS10 and OLS20 driver, script `runsimu.bash` in folder `sick_line_guidance/test/scripts` is provided:
```bash
cd catkin_ws/src/sick_line_guidance/test/scripts
./runsimu.bash
```
## Configuration
Settings and testcases for the simulation are specified in file `sick_line_guidance/test/cfg/sick_canopen_simu_cfg.xml`. To modify the current unittest
or to append further testcases, open this file in editor, save it and install the new configuration by
```bash
cd catkin_ws
catkin_make install
source ./install/setup.bash
```
Configuration file sick_canopen_simu_cfg.xml has three sections:
1. **sick_canopen_simu/SDO_config** : A lookup table for SDO response from SDO get request.
Whenever the ros driver sends a SDO request, the simulation responds with a SDO request from this table.
Example:
```bash
<SDO_config>
<sdo request="0x4018100400000000" response="0x4318100411AE2201" /> <!-- 1018sub4: Serial number -->
<sdo request="0x4018200000000000" response="0x4F18200000000000" /> <!-- 2018: OLS dev_status -->
</SDO_config>
```
The simulation will send a SDO response 4318100411AE2201 when receiving SDO request 4018100400000000 (serial number, object 1018sub4).
SDO response 4F18200000000000 will be sent after receiving a SDO request 4018200000000000 (OLS device status, object 2018).
Note: SDO responses of PDO mapped objects are modified, whenever a PDO is generated by the simulation.
2. **sick_canopen_simu/OLS20/PDO_config** : A list of sensor states transmitted by PDOs to simulate an OLS20 device.
This list specifies the measurement data transmitted by PDOs (lcp1, lcp2, lcp3, width1, width2, width3, status, barcode, barcode center, line quality and line intensities).
Each measurement is transmitted 25 times with a rate of 50 hertz (PDOs in 20 milliseconds intervall, configurable in the launch file sick_canopen_simu.launch)
before switching to the next sensor state. All measurements are repeated in an endless loop while the simulation is running.
Example:
```bash
<OLS20>
<PDO_config>
<pdo lcp1="0.000" lcp2="0.002" lcp3="0.000" width1="0.000" width2="0.012" width3="0.000" status="0x02" barcode="0x00" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.010" lcp2="-0.020" lcp3="-0.030" width1="0.031" width2="0.032" width3="0.033" status="0xC7" barcode="0x12345678" devstatus="0x00" error="0x00" barcodecenter="0.050" linequality="7" lineintensity1="3" lineintensity2="9" lineintensity3="8" frame_id="ols_simulation_frame"/>
</PDO_config>
</OLS20>
```
The simulation will convert the specified sensor states and transmit PDO1 and PDO2 every 20 milliseconds.
3. **sick_canopen_simu/OLS10/PDO_config** : A list of sensor states transmitted by PDOs to simulate an OLS10 device.
This list is identical to OLS20, except that barcode center, line quality and line intensities are not supported by OLS10 devices and therefor have to be set to 0.
Example:
```bash
<OLS10>
<PDO_config>
<pdo lcp1="0.000" lcp2="0.002" lcp3="0.000" width1="0.000" width2="0.012" width3="0.000" status="0x02" barcode="0x00" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.010" lcp2="-0.020" lcp3="-0.030" width1="0.031" width2="0.032" width3="0.033" status="0xC7" barcode="0x12345678" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
</PDO_config>
</OLS10>
```
4. **sick_canopen_simu/MLS/PDO_config** : A list of sensor states transmitted by PDOs to simulate a MLS device.
This list specifies the measurement data transmitted by PDOs (lcp1, lcp2, lcp3, status and #lcp).
Each measurement is transmitted 5 times with a rate of 50 hertz (PDOs in 20 milliseconds intervall, configurable in the launch file sick_canopen_simu.launch)
before switching to the next sensor state. All measurements are repeated in an endless loop while the simulation is running.
Example:
```bash
<MLS>
<PDO_config>
<pdo lcp1="0.000" lcp2="0.002" lcp3="0.000" status="0x01" lcp="0x02" error="0x00" frame_id="mls_simulation_frame"/>
<pdo lcp1="-0.005" lcp2="-0.006" lcp3="-0.007" status="0x41" lcp="0x57" error="0x00" frame_id="mls_simulation_frame"/>
</PDO_config>
</MLS>
```
The simulation will convert the specified sensor states and transmit PDO1 every 20 milliseconds.

242
Devices/Packages/sick_line_guidance/doc/sick_line_guidance_configuration.md Executable file
View File

@@ -0,0 +1,242 @@
# sick_line_guidance configuration
The ROS drivers for MLS and OLS devices are configured by launch and yaml files. You can modify the default configuration by editing these files.
Please note, that an invalid or improper configuration may cause errors and unexpected results.
The basic configuration uses the default values for OLS and MLS devices. All changes should be tested carefully.
To start the MLS resp. OLS driver, just launch sick_line_guidance with a yaml-configuration file:
```bash
roslaunch sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_mls.yaml # start MLS driver
roslaunch sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_ols10.yaml # start OLS10 driver
roslaunch sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_ols20.yaml # start OLS20 driver
```
The launch file "sick_line_guidance.launch" configures the common driver settings, f.e. the ros nodes to start and the ros topics to be used by
sick_line_guidance. Unless special purposes, these settings do not require customization.
The 3rd argument to roslaunch provides the driver with a yaml-file: sick_line_guidance_mls.yaml for MLS resp. sick_line_guidance_ols10.yaml for OLS10
or sick_line_guidance_ols20.yaml for OLS20.
These two files contain the settings of the can devices and may require customization.
## yaml configuration for MLS
This is the default configuration for a MLS device in file mls/sick_line_guidance_mls.yaml:
```yaml
bus:
device: can0
driver_plugin: can::SocketCANInterface
master_allocator: canopen::SimpleMaster::Allocator
sync:
interval_ms: 10
overflow: 0
nodes:
node1:
id: 0x0A # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
eds_pkg: sick_line_guidance # package name for relative path
eds_file: SICK-MLS.eds # path to EDS/DCF file
publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2022!"]
# MLS: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDO1 = 0x2021sub1 to 0x2021sub4 and 0x2022
# sick_line_guidance configuration of this node:
sick_device_family: "MLS" # can devices of OLS10, OLS20 or MLS family currently supported
sick_topic: "mls" # MLS_Measurement messages are published in topic "/mls"
sick_frame_id: "mls_measurement_frame" # MLS_Measurement messages are published frame_id "mls_measurement_frame"
# device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue"
# example: "2027": "0x01" # Object 2027 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01
# dcf_overlay:
# "2028sub1": "0x01" # UseMarkers (0 = disable marker detection, 1 = enable marker detection), UINT8, DataType=0x0005, defaultvalue=0
# "2028sub2": "0x02" # MarkerStyle (0 = disable marker detection, 1 = standard mode, 2 = extended mode), UINT8, DataType=0x0005, defaultvalue=0
# "2028sub3": "0x01" # FailSafeMode (0 = disabled, 1 = enabled), UINT8, DataType=0x0005, defaultvalue=0
# "2025": "1000" # Min.Level, UINT16, DataType=0x0006, defaultvalue=600
# "2026": "1" # Offset [mm] Nullpunkt, INT16, DataType=0x0003, defaultvalue=0
# "2027": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0
# "2029": "0x01" # LockTeach, UINT8, DataType=0x0005, defaultvalue=0
```
The following table describes these settings in detail:
| parameter name | default value | details |
| --- | --- | --- |
| bus / device | can0 | System name of can interface: net device driver for can hardware (f.e. pcan adapter) are installed to a net device named "can0". Check by system command "ifconfig -a" |
| bus / driver_plugin | can:: SocketCANInterface | Implementation of interface SocketCAN, currently only can::SocketCANInterface is supported. |
| bus / master_allocator | canopen:: SimpleMaster:: Allocator | Implementation of CAN master (network manager), currently only canopen::SimpleMaster::Allocator is supported. |
| sync / interval_ms | 10 | Timeinterval for can sync messages in milliseconds (CAN master will send a nmt sync message each 10 ms) |
| sync / overflow | 0 | Length of can sync message (0: sync message does not contain any data byte) |
| nodes | node1 | Name of the first can node configured. If you're running more than 1 can device (multiple MLS or OLS devices), you can configure each device by providing a named section for each node. |
| **nodes / node1 / id** | **0x0A** | **CAN-Node-ID of can device (default: Node-ID 10=0x0A for OLS and MLS). Change this id, if your MLS uses a different CAN-ID. :exclamation:** |
| nodes / node1 / eds_pkg | sick_line_guidance | Name of sick_line_guidance ros package. Required to resolve filenames and should not be modified. |
| nodes / node1 / eds_file | SICK-MLS.eds | Electronic datasheet for your MLS device |
| nodes / node1 / publish | ["1001", "1018sub1", "1018sub4", "2021sub1!", "2021sub2!", "2021sub3!", "2021sub4!", "2022!"] | List of published objects in the object dictionary of a can device. These objects are both published on ros topic <nodename>_<objectindex> and required by driver internal callbacks to handle PDO messages. Do not remove any PDO mapped objects from this list! |
| nodes / node1 / sick_device_family | "MLS" | Informs the ros driver that this can device is a MLS. Currently supported values are "OLS10", "OLS20" or "MLS". |
| nodes / node1 / sick_topic | "mls" | Name of the ros topic for publishing MLS measurement messages. If modified, the same topic must be set in file sick_line_guidance.launch for the sick_line_guidance_cloud_publisher node (param name="mls_topic_publish" type="str" value="mls"). Otherwise, measurement messges won't be transformed to PointCloud2 messsages. |
| nodes / node1 / sick_frame_id | "mls_measurement_frame" | Ros frame id of the MLS measurement messages. |
| **# nodes / node1 / dcf_overlay** | | **Configuration of device parameter. To set an object in the object dictionary of a can device to a specific value at startup, append dcf_overlay entries with "objectindex": "parametervalue". This section is intensionally left empty to use default values. See operation manual for possible settings. :exclamation:** |
Device specific settings can be configured in section "nodes/node1/dcf_overlay". Example to activate marker detection in extended mode (failsafe configuration):
```yaml
dcf_overlay:
"2028sub1": "0x01" # UseMarkers (0 = disable marker detection, 1 = enable marker detection), UINT8, DataType=0x0005, defaultvalue=0
"2028sub2": "0x02" # MarkerStyle (0 = disable marker detection, 1 = standard mode, 2 = extended mode), UINT8, DataType=0x0005, defaultvalue=0
"2028sub3": "0x01" # FailSafeMode (0 = disabled, 1 = enabled), UINT8, DataType=0x0005, defaultvalue=0
```
## yaml configuration for OLS
The configuration of an OLS device is pretty much the same to MLS, just with a different PDO mapping and a different eds-file. Therefore, the object list specified by `publish`
and by `dcf_overlay` vary and the eds-file "SICK_OLS20.eds" is used. Otherwise, the same configuration apply.
This is the default configuration for a OLS10 device in file ols/sick_line_guidance_ols10.yaml:
```yaml
bus:
device: can0
driver_plugin: can::SocketCANInterface
master_allocator: canopen::SimpleMaster::Allocator
sync:
interval_ms: 10
overflow: 0
nodes:
node1:
id: 0x0A # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
eds_pkg: sick_line_guidance # package name for relative path
eds_file: SICK_OLS10_CO.eds # path to EDS/DCF file
publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2021sub5!","2021sub6!","2021sub7!","2021sub8!"]
# OLS10: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDOs 0x2021sub1 to 0x2021sub8
# sick_line_guidance configuration of this node:
sick_device_family: "OLS10" # can devices currently supported: "OLS10", "OLS20" or "MLS"
sick_topic: "ols" # OLS_Measurement messages are published in topic "/ols"
sick_frame_id: "ols_measurement_frame" # OLS_Measurement messages are published with frame_id "ols_measurement_frame"
# device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue"
# example: "2001sub5": "0x01" # Object 2001sub5 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01
# dcf_overlay:
# "2001sub5": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0
# "2002sub1": "0.015" # Typ. Width, FLOAT32, DataType=0x0008
# "2002sub2": "0.001" # Min. Width, FLOAT32, DataType=0x0008
# "2002sub3": "0.050" # Max. Width, FLOAT32, DataType=0x0008
```
This is the default configuration for a OLS20 device in file ols/sick_line_guidance_ols20.yaml:
```yaml
bus:
device: can0
driver_plugin: can::SocketCANInterface
master_allocator: canopen::SimpleMaster::Allocator
sync:
interval_ms: 10
overflow: 0
nodes:
node1:
id: 0x0A # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
eds_pkg: sick_line_guidance # package name for relative path
eds_file: SICK_OLS20_CO.eds # path to EDS/DCF file
publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2021sub5!","2021sub6!","2021sub7!","2021sub8!"]
# OLS20: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDOs 0x2021sub1 to 0x2021sub8
# sick_line_guidance configuration of this node:
sick_device_family: "OLS20" # can devices currently supported: "OLS10", "OLS20" or "MLS"
sick_topic: "ols" # OLS_Measurement messages are published in topic "/ols"
sick_frame_id: "ols_measurement_frame" # OLS_Measurement messages are published with frame_id "ols_measurement_frame"
# device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue"
# example: "2001sub5": "0x01" # Object 2001sub5 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01
# dcf_overlay:
# "2001sub5": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0
# "2002sub1": "0.015" # Typ. Width, FLOAT32, DataType=0x0008
# "2002sub2": "0.001" # Min. Width, FLOAT32, DataType=0x0008
# "2002sub3": "0.050" # Max. Width, FLOAT32, DataType=0x0008
```
The following table describes these settings in detail:
| parameter name | default value | details |
| --- | --- | --- |
| bus / device | can0 | System name of can interface: net device driver for can hardware (f.e. pcan adapter) are installed to a net device named "can0". Check by system command "ifconfig -a" |
| bus / driver_plugin | can:: SocketCANInterface | Implementation of interface SocketCAN, currently only can::SocketCANInterface is supported. |
| bus / master_allocator | canopen:: SimpleMaster:: Allocator | Implementation of CAN master (network manager), currently only canopen::SimpleMaster::Allocator is supported. |
| sync / interval_ms | 10 | Timeinterval for can sync messages in milliseconds (CAN master will send a nmt sync message each 10 ms) |
| sync / overflow | 0 | Length of can sync message (0: sync message does not contain any data byte) |
| nodes | node1 | Name of the first can node configured. If you're running more than 1 can device (multiple MLS or OLS devices), you can configure each device by providing a named section for each node. |
| **nodes / node1 / id** | **0x0A** | **CAN-Node-ID of can device (default: Node-ID 10=0x0A for OLS and MLS). Change this id, if your OLS uses a different CAN-ID. :exclamation:** |
| nodes / node1 / eds_pkg | sick_line_guidance | Name of sick_line_guidance ros package. Required to resolve filenames and should not be modified. |
| nodes / node1 / eds_file | SICK_OLS20.eds | Electronic datasheet for your OLS device |
| nodes / node1 / publish | ["1001", "1018sub1", "1018sub4", "2021sub1!", "2021sub2!", "2021sub3!", "2021sub4!", "2021sub5!", "2021sub6!", "2021sub7!", "2021sub8!"] | List of published objects in the object dictionary of a can device. These objects are both published on ros topic <nodename>_<objectindex> and required by driver internal callbacks to handle PDO messages. Do not remove any PDO mapped objects from this list! |
| nodes / node1 / sick_device_family | "OLS20" | Informs the ros driver that this can device is an OLS20. Currently supported values are "OLS10", "OLS20" or "MLS". |
| nodes / node1 / sick_topic | "ols" | Name of the ros topic for publishing OLS measurement messages. If modified, the same topic must be set in file sick_line_guidance.launch for the sick_line_guidance_cloud_publisher node (param name="ols_topic_publish" type="str" value="ols"). Otherwise, measurement messges won't be transformed to PointCloud2 messsages. |
| nodes / node1 / sick_frame_id | "ols_measurement_frame" | Ros frame id of the OLS measurement messages. |
| **# nodes / node1 / dcf_overlay** | | **Configuration of device parameter. To set an object in the object dictionary of a can device to a specific value at startup, append dcf_overlay entries with "objectindex": "parametervalue". This section is intensionally left empty to use default values. See operation manual for possible settings. :exclamation:** |
Device specific settings can be configured in section "nodes/node1/dcf_overlay". Example to set object 2001sub5 (sensorFlipped):
```yaml
dcf_overlay:
"2001sub5": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0
```
## Summary of the yaml configuration for MLS and OLS devices
Set the CAN-ID of your device with `id: 0x0A # CAN-Node-ID of MLS (default: 0x0A)` and
configure device specific settings in section `dcf_overlay` by appending a line `"objectindex": "parametervalue"` for each parameter.
## Configuration for multiple devices
If you want to use multiple can devices (MLS or OLS) on one system, you have to append multiple nodes in your yaml-file. Each node must contain the configuration of one can device as shown above. Make sure to use different node names for each device:
```yaml
nodes:
node1:
id: ... # CAN-ID for the 1. device
node2:
id: ... # CAN-ID for the 2. device
```
If you configure different measurement topics for each can device (f.e. `sick_topic: "ols20A"` for the first OLS20 and `sick_topic: "ols20B"` for the second OLS20 device), you have to start multiple sick_line_guidance_cloud_publisher nodes, too. Otherwise, measurement messges from this device can't be transformed to PointCloud2 messsages.
Append multiple sick_line_guidance_cloud_publisher nodes in file sick_line_guidance.launch, each sick_line_guidance_cloud_publisher node configured with the corresponding
ros topic in parameter `"mls_topic_publish"` resp. `"ols_topic_publish"`.
Launchfile [sick_line_guidance_ols20_twin.launch](../launch/sick_line_guidance_ols20_twin.launch) and yaml-configuration [sick_line_guidance_ols20_twin.yaml](../ols/sick_line_guidance_ols20_twin.yaml) show an example how to run two OLS20 devices with can node ids 0x0A and 0x0B. Run the example with
```
rosrun rviz rviz &
roslaunch -v --screen sick_line_guidance sick_line_guidance_ols20_twin.launch
```
The point clouds of both OLS20 devices will be published on topic "cloudA" and "cloudB" with frame ids "olsA_frame" and "olsB_frame". Use a static_transform_publisher to setup a transform for each OLS frame to a reference coordinate system, e.g.
```
rosrun tf static_transform_publisher 0 0 0 0 0 0 olsA_frame cloud 10
rosrun tf static_transform_publisher 0 0 0 0 0 0 olsB_frame cloud 10
```
## Read and write parameter during runtime
You can read and write to the object dictionary of a can device on runtime, too, by using the ros services implemented by the canopen driver.
To read a value from the object dictionary, you can run
```bash
rosservice call /driver/get_object "{node: '<nodename>', object: '<objectindex>', cached: <true|false>}"
```
in your terminal. Example to read object 2001sub5 (sensorFlipped) from node1:
```bash
rosservice call /driver/get_object "{node: 'node1', object: '2001sub5', cached: false}"
```
This command results in a can upload command using a SDO with object 2001sub5. The SDO response will be converted and displayed if the command succeded.
To write a value to the object dictionary, you can run
```bash
rosservice call /driver/get_object "{node: '<nodename>', object: '<objectindex>', value: '<parametervalue>', cached: <true|false>}"
```
in the terminal. Example to write value 0x01 to object 2001sub5 (sensorFlipped):
```bash
rosservice call /driver/set_object "{node: 'node1', object: '2001sub5', value: '0x01', cached: false}"
```
This command results in a can upload command using a SDO with object 2001sub5.
## Configuration of CAN node id
The node id of a CAN device can be configured e.b. with [PCAN-View](https://www.peak-system.com/PCAN-View.242.0.html):<br/>
![screenshot01.png](pcan-config/screenshot01.png)<br/>
![screenshot02.png](pcan-config/screenshot02.png)<br/>
## Installation and bus termination
The following screenshot shows the installation and bus termination for two OLS devices:<br/>
![screenshot03.png](pcan-config/screenshot03.png)

130
Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_canopen_simu_canstate.h Executable file
View File

@@ -0,0 +1,130 @@
/*
* sick_canopen_simu_canstate implements a state engine for can.
*
* Depending on input messages of type can_msgs::Frame,
* the current state is switched between INITIALIZATION,
* PRE_OPERATIONAL, OPERATIONAL and STOPPED.
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#ifndef __SICK_CANOPEN_SIMU_CANSTATE_H_INCLUDED
#define __SICK_CANOPEN_SIMU_CANSTATE_H_INCLUDED
#include <ros/ros.h>
#include <can_msgs/Frame.h>
namespace sick_canopen_simu
{
typedef enum CAN_STATE_ENUM
{
INITIALIZATION,
PRE_OPERATIONAL,
OPERATIONAL,
STOPPED
} CAN_STATE;
/*
* class CanState: handles can nmt messages and implements the state engine for can.
*/
class CanState
{
public:
/*
* Constructor.
*
* @param[in] can_node_id node id of OLS or MLS, default: 0x0A
*/
CanState(int can_node_id);
/*
* Destructor.
*/
virtual ~CanState();
/*
* Constructor.
*
* @param[in] can_node_id node id of OLS or MLS, default: 0x0A
*/
virtual CAN_STATE & state(void)
{
return m_can_state;
}
/*
* @brief Callbacks for ros messages. Switches the state, and returns true, if msg_in is a can nmt message.
* Otherwise, false is returned.
*
* param[in] msg_in input message of type can_msgs::Frame
* param[out] msg_out output message of type can_msgs::Frame (if input message requires a response to send)
* param[out] send_msg true, if input message msg_in requires the response message msg_out to be send
*
* @return true, if input message handled, otherwise false.
*/
virtual bool messageHandler(const can_msgs::Frame &msg_in, can_msgs::Frame & msg_out, bool & send_msg);
protected:
/*
* member data.
*/
int m_can_node_id;
CAN_STATE m_can_state;
}; // class CanState
} // namespace sick_canopen_simu
#endif // __SICK_CANOPEN_SIMU_CANSTATE_H_INCLUDED

177
Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_canopen_simu_compare.h Executable file
View File

@@ -0,0 +1,177 @@
/*
* sick_canopen_simu_compare implements utility functions to compare measurements
* for verification of the sick_line_guidance ros driver.
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#ifndef __SICK_CANOPEN_SIMU_COMPARE_H_INCLUDED
#define __SICK_CANOPEN_SIMU_COMPARE_H_INCLUDED
#include <ros/ros.h>
#include "sick_line_guidance/MLS_Measurement.h"
#include "sick_line_guidance/OLS_Measurement.h"
namespace sick_canopen_simu
{
/*
* class MeasurementComparator implements utility functions to compare measurements
* for verification of the sick_line_guidance ros driver.
*
*/
template<class MsgType>
class MeasurementComparator
{
public:
/*
* @brief Compares the positions of two measurements.
* @return true, if the positions of two measurements are identical (difference below 1 millimeter), or false otherwise.
*/
static bool cmpPosition(const MsgType &A, const MsgType &B)
{
return (std::fabs(A.position[0] - B.position[0]) < 0.001) && (std::fabs(A.position[1] - B.position[1]) < 0.001) && (std::fabs(A.position[2] - B.position[2]) < 0.001);
}
/*
* @brief Compares the width of two measurements.
* @return true, if the width of two measurements are identical (difference below 1 millimeter), or false otherwise.
*/
static bool cmpLinewidth(const MsgType &A, const MsgType &B)
{
return (std::fabs(A.width[0] - B.width[0]) < 0.001) && (std::fabs(A.width[1] - B.width[1]) < 0.001) && (std::fabs(A.width[2] - B.width[2]) < 0.001);
}
/*
* @brief Compares the status of two measurements.
* @return true, if the status of two measurements are identical, or false otherwise.
*/
static bool cmpStatus(const MsgType &A, const MsgType &B)
{
return (A.status == B.status);
}
/*
* @brief Compares the lcp status of two measurements.
* @return true, if the lcp status of two measurements are identical, or false otherwise.
*/
static bool cmpLcp(const MsgType &A, const MsgType &B)
{
return (A.lcp == B.lcp);
}
/*
* @brief Compares the barcode of two measurements.
* @return true, if the barcodes of two measurements are identical, or false otherwise.
*/
static bool cmpBarcode(const MsgType &A, const MsgType &B)
{
return (A.barcode == B.barcode);
}
/*
* @brief Compares the device status of two measurements.
* @return true, if the device status of two measurements are identical, or false otherwise.
*/
static bool cmpDevStatus(const MsgType &A, const MsgType &B)
{
return (A.dev_status == B.dev_status);
}
/*
* @brief Compares the extended code of two measurements.
* @return true, if the extended code of two measurements are identical, or false otherwise.
*/
static bool cmpExtendedCode(const MsgType &A, const MsgType &B)
{
return (A.extended_code == B.extended_code);
}
/*
* @brief Compares the error status of two measurements.
* @return true, if the error status of two measurements are identical, or false otherwise.
*/
static bool cmpError(const MsgType &A, const MsgType &B)
{
return (A.error == B.error);
}
/*
* @brief Compares the barcode center points of two measurements.
* @return true, if the barcode center points of two measurements are identical, or false otherwise.
*/
static bool cmpBarcodeCenter(const MsgType &A, const MsgType &B)
{
return (std::fabs(A.barcode_center_point - B.barcode_center_point) < 0.001);
}
/*
* @brief Compares the line quality of two measurements.
* @return true, if the line quality of two measurements are identical, or false otherwise.
*/
static bool cmpLineQuality(const MsgType &A, const MsgType &B)
{
return (A.quality_of_lines == B.quality_of_lines);
}
/*
* @brief Compares the line intensities of two measurements.
* @return true, if the line intensities of two measurements are identical, or false otherwise.
*/
static bool cmpLineIntensity(const MsgType &A, const MsgType &B)
{
return (A.intensity_of_lines[0] == B.intensity_of_lines[0] && A.intensity_of_lines[1] == B.intensity_of_lines[1] && A.intensity_of_lines[2] == B.intensity_of_lines[2]);
}
};
}
#endif // __SICK_CANOPEN_SIMU_COMPARE_H_INCLUDED

417
Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_canopen_simu_device.h Executable file
View File

@@ -0,0 +1,417 @@
/*
* sick_canopen_simu_device implements simulation of SICK can devices (OLS20 and MLS)
* for tests of sick_line_guidance ros driver.
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#ifndef __SICK_CANOPEN_SIMU_DEVICE_H_INCLUDED
#define __SICK_CANOPEN_SIMU_DEVICE_H_INCLUDED
#include <boost/thread.hpp>
#include <tinyxml.h>
#include <ros/ros.h>
#include <can_msgs/Frame.h>
#include "sick_line_guidance/sick_canopen_simu_canstate.h"
#include "sick_line_guidance/MLS_Measurement.h"
#include "sick_line_guidance/OLS_Measurement.h"
namespace sick_canopen_simu
{
/*
* Class SimulatorBase implements the base class for simulation of SICK can devices (OLS20 and MLS).
*
* ROS messages of type can_msgs::Frame are read from ros topic "can0",
* handled to simulate an OLS20 or MLS device, and resulting can_msgs::Frame
* messages are published on ros topic "ros2can0".
*
* MsgType can be sick_line_guidance::OLS_Measurement (for simulation of OLS devices), or
* or sick_line_guidance::MLS_Measurement (for simulation of MLS devices).
*
* Assumption: ros nodes sick_line_guidance_ros2can_node and sick_line_guidance_can2ros_node
* have to be running to convert ros messages from and to canbus messages.
* sick_line_guidance_ros2can_node converts ros messages of type can_msgs::Frame to can frames,
* sick_line_guidance_can2ros_node converts can frames to ros messages of type can_msgs::Frame.
*
* Subclass MLSSimulator extends class SimulatorBase to simulate an MLS device.
*
* Subclass OLS20Simulator extends class SimulatorBase to simulate an OLS20 device.
*/
template<class MsgType> class SimulatorBase
{
public:
/*
* Constructor
*
* @param[in] nh ros node handle
* @param[in] config_file configuration file with testcases for OLS and MLS simulation
* @param[in] can_node_id node id of OLS or MLS, default: 0x0A
* @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0"
* @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0"
* @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second)
* @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times)
* @param[in] subscribe_queue_size buffer size for ros messages
*/
SimulatorBase(ros::NodeHandle & nh, const std::string & config_file, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size);
/*
* Destructor
*
*/
virtual ~SimulatorBase();
/*
* @brief Callbacks for ros messages. Converts incoming messages of type can_msgs::Frame to simulate a can device
* and publishes simulation results to the configured ros topic.
*
* param[in] msg ros message of type can_msgs::Frame
*/
virtual void messageHandler(const can_msgs::Frame & msg_in);
/*
* @brief Interface to listen to the simulated sensor state. A listener implementing this interface can be notified on
* PDOs sent by a simulation by registring using function registerSimulationListener.
* After receiving both the sensor state from the simulation and the following OLS/MLS-Measurement ros message from the driver,
* the listener can check the correctness by comparing both messages. The sensor state from simulation and from OLS/MLS-Measurement
* messages from the driver must be identical, otherwise some failure occured.
*/
class SimulationListener
{
public:
/*
* @brief Notification callback of a listener. Whenever the simulation sends a PDO, registered listener are notified about the current sensor state
* by calling function pdoSent.
*/
virtual void pdoSent(const MsgType & sensor_state) = 0;
};
/*
* @brief Registers a listener to a simulation. Whenever the simulation sends a PDO, the listener is notified about the current sensor state.
* After receiving both the sensor state and the following OLS/MLS-Measurement ros message from the driver, the listener can check
* the correctness by comparing the sensor state from simulation and the OLS/MLS-Measurement from the driver. Both must be identical,
* otherwise some failure occured.
*
* param[in] pListener listener to current sensor states sent by PDO.
*/
virtual void registerSimulationListener(SimulationListener* pListener);
/*
* @brief Unregisters a listener from a simulation. The listener will not be notified about simulated sensor states.
* This function is the opposite to registerSimulationListener.
*
* param[in] pListener listener to current sensor states sent by PDO.
*/
virtual void unregisterSimulationListener(SimulationListener* pListener);
protected:
/*
* @brief Parses an pdo element from config file and appends it to m_vec_pdo_measurements.
*
* param[in] xml_pdo pdo element from config file
*/
virtual bool parseXmlPDO(TiXmlElement* xml_pdo) = 0;
/*
* reads SDO configuration from xml-file
*
* @param[in] config_file configuration file with testcases for OLS and MLS simulation
*/
virtual bool readSDOconfig(const std::string & config_file);
/*
* reads the PDO configuration from xml-file
*
* @param[in] config_file configuration file with testcases for simulation
* @param[in] sick_device_family "OLS10", "OLS20" or "MLS"
*/
virtual bool readPDOconfig(const std::string & config_file, const std::string & sick_device_family);
/*
* @brief handles SDOs, i.e. sends a SDO response and returns true, if can frame is a SDO request.
* Otherwise, the can frame is not handled and false is returned.
* Note: The SDO response is simply taken from a lookup-table (input: SDO request frame data, output: SDO response frame data).
* If the SDO request frame data (frame.data) is not found in the lookup-table, the can frame is not handled and false is returned, too.
*
* @param[in] msg_in received can frame
*
* @return true if can frame was handled and a SDO request is sent, false otherwise.
*/
virtual bool handleSDO(const can_msgs::Frame & msg_in);
/*
* @brief Publishes PDOs to simulate a MLS or OLS20 device while can state is operational
*/
virtual void runPDOthread(void);
/*
* @brief Convertes an MLS_Measurement into a can_msgs::Frame TPDO1.
* @param[in] measurement MLS_Measurement to be converted
* @param[out] tpdo1_msg output can frame TPDO1,
* @param[out] tpdo2_msg dummy frame TPDO2, unused
* @return Returns the number of TPDOs, i.e. 1 for MLS devices.
*/
virtual int convertToCanFrame(const sick_line_guidance::MLS_Measurement & measurement, can_msgs::Frame & tpdo1_msg, can_msgs::Frame & tpdo2_msg);
/*
* @brief Convertes an OLS_Measurement into two can_msgs::Frame TPDO1 and TPDO2.
* @param[in] measurement OLS_Measurement to be converted
* @param[out] tpdo1_msg output can frame TPDO1, Byte 1-8 := LCP1(LSB,MSB,0x2021sub1), LCP2(LSB,MSB,0x2021sub2), LCP3(LSB,MSB,0x2021sub3), status(UINT8,0x2021sub4), barcode(UINT8,0x2021sub8)
* @param[out] tpdo2_msg output can frame TPDO2, Byte 1-6 := Width1(LSB,MSB,0x2021sub5), Width2(LSB,MSB,0x2021sub6), Width3(LSB,MSB,0x2021sub7)
* @return Returns the number of TPDOs, i.e. 2 for OLS devices.
*/
virtual int convertToCanFrame(const sick_line_guidance::OLS_Measurement & measurement, can_msgs::Frame & tpdo1_msg, can_msgs::Frame & tpdo2_msg);
/*
* @brief Converts a position or width (float value in meter) to lcp (INT16 value in millimeter),
* shortcut for std::round(lcp * 1000) with clipping to range INT16_MIN ... INT16_MAX.
* @param[in] lcp position or width (float value in meter)
* @return INT16 value in millimeter
*/
virtual int convertLCP(float lcp);
/*
* @brief Converts frame.data to uint64_t
* @param[in] frame can frame
* @return frame.data converted to uint64_t
*/
virtual uint64_t frameDataToUInt64(const can_msgs::Frame & frame);
/*
* @brief Converts uint64_t data to frame.data
* @param[in] u64data frame data (uint64_t)
* @param[in+out] frame can frame
*/
virtual void uint64ToFrameData(uint64_t u64data, can_msgs::Frame & frame);
/*
* @brief returns an unsigned integer in reverse byte order,
* f.e. revertByteorder<uint32_t>(0x12345678) returns 0x78563412.
* @param[in] data input data (unsigned integer)
* @return data in reverse byte order
*/
template <class T> static T revertByteorder(T data)
{
T reverted = 0;
for(size_t n = 0; n < sizeof(T); n++)
{
reverted = (reverted << 8);
reverted = (reverted | (data & 0xFF));
data = data >> 8;
}
return reverted;
}
/*
* @brief prints and returns a can_msgs::Frame in short format like candump (f.a. "18A#B4FFCCFF00000300")
*/
virtual std::string tostring(const can_msgs::Frame & canframe);
/*
* member variables
*/
sick_canopen_simu::CanState m_can_state; // the current can state: INITIALIZATION, PRE_OPERATIONAL, OPERATIONAL or STOPPED
uint8_t m_can_node_id; // node id of OLS or MLS device (default: 0x0A)
ros::Subscriber m_ros_subscriber; // subscriber to handle can messages from master (NMT messages and SDOs)
ros::Publisher m_ros_publisher; // publishes can frames (PDO, SDO response, etc.)
boost::mutex m_ros_publisher_mutex; // lock guard for publishing messages with m_ros_publisher
boost::thread* m_pdo_publisher_thread; // thread to publish PDO messages in can state OPERATIONAL
bool m_pdo_publisher_thread_running; // true while m_pdo_publisher_thread is running
ros::Rate m_pdo_rate; // rate of PDOs (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second)
int m_pdo_repeat_cnt; // each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times)
std::map<uint64_t, uint64_t> m_sdo_request_response_map; // lookup table: sdo response data := m_sdo_request_response_map[<sdo request data>]
std::vector<MsgType> m_vec_pdo_measurements; // list of PDOs to simulate OLS or MLS device
std::vector<SimulationListener*> m_vec_simu_listener; // list of registered listeners to the current sensor state simulated
int m_subscribe_queue_size; // buffer size for ros messages
uint64_t m_sdo_response_dev_state; // response to sdo request for dev_status (object 0x2018): MLS and OLS20: 0x4F18200000000000 (sdo response with UINT8 data), OLS10: 0x4B18200000000000 (sdo response with UINT16 data)
bool m_send_tpdo_immediately; // true (OLS10, MLS): send TPDOs immediately in all states (pre-operational and operational), false (default, OLS20): send TPDOs only in state operational
}; // SimulatorBase
/*
* Subclass MLSSimulator extends class SimulatorBase to simulate a MLS device.
*
*/
class MLSSimulator : public SimulatorBase<sick_line_guidance::MLS_Measurement>
{
public:
/*
* Constructor
*
* @param[in] nh ros node handle
* @param[in] config_file configuration file with testcases for OLS and MLS simulation
* @param[in] sick_device_family "OLS10", "OLS20" or "MLS"
* @param[in] can_node_id node id of OLS or MLS, default: 0x0A
* @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0"
* @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0"
* @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second)
* @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times)
* @param[in] subscribe_queue_size buffer size for ros messages
*/
MLSSimulator(ros::NodeHandle & nh, const std::string & config_file, const std::string & sick_device_family, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size);
/*
* @brief Callbacks for ros messages. Converts incoming messages of type can_msgs::Frame to simulate a MLS device
* and publishes simulation results to the configured ros topic.
*
* param[in] msg ros message of type can_msgs::Frame
*/
virtual void messageHandler(const can_msgs::Frame & msg_in);
protected:
/*
* @brief Parses an pdo element from config file and appends it to m_vec_pdo_measurements.
*
* param[in] xml_pdo pdo element from config file
*/
virtual bool parseXmlPDO(TiXmlElement* xml_pdo);
}; // MLSSimulator
/*
* Subclass OLSSimulator extends class SimulatorBase to simulate an OLS devices.
*
*/
class OLSSimulator : public SimulatorBase<sick_line_guidance::OLS_Measurement>
{
public:
/*
* Constructor
*
* @param[in] nh ros node handle
* @param[in] config_file configuration file with testcases for OLS and MLS simulation
* @param[in] sick_device_family "OLS10", "OLS20" or "MLS"
* @param[in] can_node_id node id of OLS or MLS, default: 0x0A
* @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0"
* @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0"
* @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second)
* @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times)
* @param[in] subscribe_queue_size buffer size for ros messages
*/
OLSSimulator(ros::NodeHandle & nh, const std::string & config_file, const std::string & sick_device_family, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size);
/*
* @brief Callbacks for ros messages. Converts incoming messages of type can_msgs::Frame to simulate an OLS device
* and publishes simulation results to the configured ros topic.
*
* param[in] msg ros message of type can_msgs::Frame
*/
virtual void messageHandler(const can_msgs::Frame & msg_in);
protected:
/*
* @brief Parses an pdo element from config file and appends it to m_vec_pdo_measurements.
*
* param[in] xml_pdo pdo element from config file
*/
virtual bool parseXmlPDO(TiXmlElement* xml_pdo);
}; // OLSSimulator
/*
* Subclass OLS10Simulator extends class OLSSimulator to simulate an OLS10 device.
*
*/
class OLS10Simulator : public OLSSimulator
{
public:
/*
* Constructor
*
* @param[in] nh ros node handle
* @param[in] config_file configuration file with testcases for OLS and MLS simulation
* @param[in] sick_device_family "OLS10", "OLS20" or "MLS"
* @param[in] can_node_id node id of OLS or MLS, default: 0x0A
* @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0"
* @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0"
* @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second)
* @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times)
* @param[in] subscribe_queue_size buffer size for ros messages
*/
OLS10Simulator(ros::NodeHandle & nh, const std::string & config_file, const std::string & sick_device_family, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size);
}; // OLS10Simulator
/*
* Subclass OLS20Simulator extends class OLSSimulator to simulate an OLS20 device.
*
*/
class OLS20Simulator : public OLSSimulator
{
public:
/*
* Constructor
*
* @param[in] nh ros node handle
* @param[in] config_file configuration file with testcases for OLS and MLS simulation
* @param[in] sick_device_family "OLS10", "OLS20" or "MLS"
* @param[in] can_node_id node id of OLS or MLS, default: 0x0A
* @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0"
* @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0"
* @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second)
* @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times)
* @param[in] subscribe_queue_size buffer size for ros messages
*/
OLS20Simulator(ros::NodeHandle & nh, const std::string & config_file, const std::string & sick_device_family, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size);
}; // OLS20Simulator
} // sick_canopen_simu
#endif // __SICK_CANOPEN_SIMU_DEVICE_H_INCLUDED

228
Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_canopen_simu_verify.h Executable file
View File

@@ -0,0 +1,228 @@
/*
* sick_canopen_simu_verify tests and verifies the sick_line_guidance ros driver.
*
* sick_canopen_simu_verify listenes to both the simulation (interface sick_canopen_simu::SimulatorBase::SimulationListener)
* and to the ros messages of the sick_line_guidance driver. Whenever a MLS_Measurement or OLS_Measurement message
* is received, the measurement is compared to the current sensor state of the simulation.
* Measurement messages from the driver and sensor states from the simulation should be identical, otherwise an error occured.
* The sick_line_guidance driver test is passed, if no error occured (i.e. all measurement messages from the driver have been
* handled correctly, no mismatches between simulated sensor states and published measurement messages).
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#ifndef __SICK_CANOPEN_SIMU_VERIFY_H_INCLUDED
#define __SICK_CANOPEN_SIMU_VERIFY_H_INCLUDED
#include <ros/ros.h>
#include "sick_line_guidance/sick_canopen_simu_device.h"
namespace sick_canopen_simu
{
/*
* Class MeasurementVerification tests and verifies measurement messages from the sick_line_guidance ros driver.
*
* MeasurementVerification listenes to both the simulation (interface sick_canopen_simu::SimulatorBase::SimulationListener)
* and to the ros messages of the sick_line_guidance driver. Whenever a MLS_Measurement or OLS_Measurement message
* is received, the measurement is compared to the current sensor state of the simulation.
* Measurement messages from the driver and sensor states from the simulation should be identical, otherwise an error occured.
* The sick_line_guidance driver test is passed, if no error occured (i.e. all measurement messages from the driver have been
* handled correctly, no mismatches between simulated sensor states and published measurement messages).
*/
template<class MsgType> class MeasurementVerification : public sick_canopen_simu::SimulatorBase<MsgType>::SimulationListener
{
public:
/*
* Constructor
*
* @param[in] nh ros node handle
* @param[in] measurement_subscribe_topic ros topic for measurement messages, default: "mls" resp. "ols"
* @param[in] sensor_state_queue_size buffer size for simulated sensor states, default: 2
* @param[in] devicename descriptional device name, f.e. "OLS20" or "MLS"
*/
MeasurementVerification(ros::NodeHandle & nh, const std::string & measurement_subscribe_topic, int sensor_state_queue_size, const std::string & devicename);
/*
* Destructor
*
*/
virtual ~MeasurementVerification();
/*
* @brief Implements the notification callback for SimulationListener. Whenever the simulation sends a PDO,
* this function is called by the simulation to notify SimulationListeners about the current sensor state.
*/
virtual void pdoSent(const MsgType & sensor_state);
/*
* @brief Prints the number of verified measuremente and the number of verification failures.
* @return true in case of no verification failures (all measurements verified successfully), false otherwise.
*/
virtual bool printStatistic(void);
protected:
/*
* @brief Callback for measurement messages, called whenever the sick_line_guidance ros driver publishes a
* new measurement message. Compares the measurement message with the sensor states from simulation,
* and reports an error, if an equivalent sensor state hasn't been sent by the simulation.
*
* @param[in] measurement measurement message from sick_line_guidance ros driver
*
* @return true, if the measurement message is equivalent to sensor states (verification passed), or false otherwise (verification failed).
*/
virtual bool verifyMeasurement(const MsgType & measurement);
/*
* @brief Compares and verifies measurement messages from ros driver against sensor states from simulation.
*
* @param[in] measurement_messages list of measurement messages from ros driver (to be compared to sensor_states)
* @param[in] sensor_states list of sensor states from simulation (to be compared to measurement_messages)
*
* @return true, if the measurement messages are equivalent to sensor states (verification passed), or false otherwise (verification failed).
*/
// virtual bool verifyMeasurements(std::list<MsgType> & measurement_messages, std::list<MsgType> & sensor_states);
virtual bool verifyMeasurements(std::list<sick_line_guidance::MLS_Measurement> & measurement_messages, std::list<sick_line_guidance::MLS_Measurement> & sensor_states);
virtual bool verifyMeasurements(std::list<sick_line_guidance::OLS_Measurement> & measurement_messages, std::list<sick_line_guidance::OLS_Measurement> & sensor_states);
/*
* @brief Compares measurement messages from ros driver against sensor states from simulation, using a specified compare function
* (f.e. floating point comparsion for positions with fabs(x-y) < 1 mm and integer comparsion with x == y for barcodes).
*
* @param[in] measurement_messages list of measurement messages from ros driver (to be compared to sensor_states)
* @param[in] sensor_states list of sensor states from simulation (to be compared to measurement_messages)
* @param[in] cmpfunction compare function, called to compare measurement message A to sensor state B
*
* @return true, if the measurement messages are equivalent to sensor states (verification passed), or false otherwise (verification failed).
*/
template<typename T>
bool verifyMeasurementData(std::list<T> & measurement_messages, std::list<T> & sensor_states, bool(*cmpfunction)(const T & A, const T & B));
// virtual bool verifyMeasurementData(std::list<MsgType> & measurement_messages, std::list<MsgType> & sensor_states, bool(*cmpfunction)(const MsgType & A, const MsgType & B));
/*
* member variables
*/
std::string m_devicename; // descriptional device name, f.e. "OLS20" or "MLS"
ros::Subscriber m_measurement_subscriber; // subscriber ros measurement messages from sick_line_guidance driver
std::list<MsgType> m_sensor_states; // list of sensor states from simulation
std::list<MsgType> m_measurement_messages; // list of measurement messages from sick_line_guidance driver
boost::mutex m_sensor_states_mutex; // lock guard for access to m_vec_sensor_states
int m_measurement_messages_cnt; // reporting and statistics: number of verified measurement messages
int m_measurement_verification_error_cnt; // reporting and statistics: number of measurement messages with verification errors
int m_measurement_verification_ignored_cnt; // reporting and statistics: number of measurement messages with verification ignored (f.e. SDO response was still pending)
int m_measurement_verification_failed; // reporting and statistics: messages with verification errors
int m_measurement_verification_jitter; // reporting and statistics: verification jitter (max. 1 error tolerated, since measurement messages can be sent while a SDO response is still pending)
}; // MeasurementVerification
/*
* Subclass MLSMeasurementVerification extends class MeasurementVerification to verify MLS_Measurement messages.
*
*/
class MLSMeasurementVerification : public MeasurementVerification<sick_line_guidance::MLS_Measurement>
{
public:
/*
* Constructor
*
* @param[in] nh ros node handle
* @param[in] measurement_subscribe_topic ros topic for measurement messages, default: "mls"
* @param[in] sensor_state_queue_size buffer size for simulated sensor states, default: 2
* @param[in] devicename descriptional device name, f.e. "MLS"
*/
MLSMeasurementVerification(ros::NodeHandle & nh, const std::string & measurement_subscribe_topic, int sensor_state_queue_size, const std::string & devicename);
/*
* @brief Callback for measurement messages, called whenever the sick_line_guidance ros driver publishes a
* new measurement message. Compares the measurement message with the sensor states from simulation,
* and reports an error, if an equivalent sensor state hasn't been sent by the simulation.
*
* @param[in] measurement measurement message from sick_line_guidance ros driver
*/
virtual void measurementCb(const sick_line_guidance::MLS_Measurement & measurement);
}; // MLSMeasurementVerification
/*
* Subclass OLSMeasurementVerification extends class MeasurementVerification to verify OLS_Measurement messages.
*
*/
class OLSMeasurementVerification : public MeasurementVerification<sick_line_guidance::OLS_Measurement>
{
public:
/*
* Constructor
*
* @param[in] nh ros node handle
* @param[in] measurement_subscribe_topic ros topic for measurement messages, default: "ols"
* @param[in] sensor_state_queue_size buffer size for simulated sensor states, default: 2
* @param[in] devicename descriptional device name, f.e. "OLS20"
*/
OLSMeasurementVerification(ros::NodeHandle & nh, const std::string & measurement_subscribe_topic, int sensor_state_queue_size, const std::string & devicename);
/*
* @brief Callback for measurement messages, called whenever the sick_line_guidance ros driver publishes a
* new measurement message. Compares the measurement message with the sensor states from simulation,
* and reports an error, if an equivalent sensor state hasn't been sent by the simulation.
*
* @param[in] measurement measurement message from sick_line_guidance ros driver
*/
virtual void measurementCb(const sick_line_guidance::OLS_Measurement & measurement);
}; // OLSMeasurementVerification
} // sick_canopen_simu
#endif // __SICK_CANOPEN_SIMU_VERIFY_H_INCLUDED

163
Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_can_cia401_subscriber.h Executable file
View File

@@ -0,0 +1,163 @@
/*
* sick_line_guidance_can_cia401_subscriber implements a ros subscriber to canopen ols messages (example CiA401 device for testing).
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#ifndef __SICK_LINE_GUIDANCE_CAN_CIA401_SUBSCRIBER_H_INCLUDED
#define __SICK_LINE_GUIDANCE_CAN_CIA401_SUBSCRIBER_H_INCLUDED
#include "sick_line_guidance/sick_line_guidance_can_ols_subscriber.h"
namespace sick_line_guidance
{
/*
* class CanCiA401Subscriber implements the ros subscriber to canopen ols messages (example CiA401 device for testing).
*/
class CanCiA401Subscriber : public CanOlsSubscriber
{
public:
/*
* Constructor.
* @param[in] nh ros::NodeHandle
* @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1"
* @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
* @param[in] max_sdo_rate max. sdo query and publish rate
* @param[in] ros_topic topic for ros messages, f.e. "mls" or "ols"
* @param[in] ros_frameid frameid for ros messages, f.e. "mls_measurement_frame" or "ols_measurement_frame"
* @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error)
* @param[in] subscribe_queue_size buffer size for ros messages
*/
CanCiA401Subscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate, const std::string & ros_topic,
const std::string & ros_frameid, int initial_sensor_state = 0, int subscribe_queue_size = 1);
/*
* Destructor.
*/
virtual ~CanCiA401Subscriber();
/*
* @brief subsribes to canopen topics for ols messages and sets the callbacks to handle messages from canopen_chain_node
* after PDO messages (LCP = line center point):
*
* Mapping for OLS:
*
* canopenchain-OLS -> CanSubscriber-callback -> OLS sensor state
* -----------------------------------------------------------------------
* <nodeid>+"_1001" -> cancallbackError(UINT8) -> Error register
* <nodeid>+"_2018" -> cancallbackDevState(UINT8) -> Device state
* <nodeid>+"_2021sub1" -> cancallbackLCP1(INT16) -> LCP1
* <nodeid>+"_2021sub2" -> cancallbackLCP2(INT16) -> LCP2
* <nodeid>+"_2021sub3" -> cancallbackLCP3(INT16) -> LCP3
* <nodeid>+"_2021sub4" -> cancallbackState(UINT8) -> State
* <nodeid>+"_2021sub5" -> cancallbackWidthLCP1(INT16) -> Width LCP1
* <nodeid>+"_2021sub6" -> cancallbackWidthLCP2(INT16) -> Width LCP2
* <nodeid>+"_2021sub7" -> cancallbackWidthLCP3(INT16) -> Width LCP3
* <nodeid>+"_2021sub8" -> cancallbackCode(UINT8) -> Code
* <nodeid>+"_2021sub9" -> cancallbackExtCode(UINT32) -> Extended Code
*
* Mapping for MLS:
*
* canopenchain-MLS -> CanSubscriber-callback -> MLS sensor state
* -----------------------------------------------------------------------
* <nodeid>+"_1001" -> cancallbackError(UINT8) -> Error register
* <nodeid>+"_2021sub1" -> cancallbackLCP1(INT16) -> LCP1
* <nodeid>+"_2021sub2" -> cancallbackLCP2(INT16) -> LCP2
* <nodeid>+"_2021sub3" -> cancallbackLCP3(INT16) -> LCP3
* <nodeid>+"_2021sub4" -> cancallbackMarker(UINT8) -> #LCP and marker
* <nodeid>+"_2022" -> cancallbackState(UINT8) -> State
*
* Mapping for CiA402 (example, testing only):
*
* canopenchain-CiA402 -> CanSubscriber-callback -> CiA402 state
* ---------------------------------------------------------------------
* <nodeid>+"_6000sub1" -> cancallbackError(UINT8) -> Error register
* <nodeid>+"_6000sub2" -> cancallbackState(UINT8) -> State
* <nodeid>+"_6000sub3" -> cancallbackCode(UINT8) -> Code
* <nodeid>+"_6000sub4" -> cancallbackExtCode(UINT8) -> LSB Extended Code
* <nodeid>+"_6401sub1" -> cancallbackLCP1(INT16) -> LCP1
* <nodeid>+"_6401sub2" -> cancallbackLCP2(INT16) -> LCP2
* <nodeid>+"_6401sub3" -> cancallbackLCP3(INT16) -> LCP3
* <nodeid>+"_6401sub4" -> cancallbackWidthLCP1(INT16) -> Width LCP1
* <nodeid>+"_6401sub5" -> cancallbackWidthLCP2(INT16) -> Width LCP2
* <nodeid>+"_6401sub6" -> cancallbackWidthLCP3(INT16) -> Width LCP3
*
* See operation manuals for details (file SICK-OLS-Operating_instructions_OLS10_de_IM0076955.PDF for OLS
* and file SICK-MLS-Operating_instructions_MLS_de_IM0076567.PDF for MLS)
*
* @return true on success, otherwise false.
*/
virtual bool subscribeCanTopics(void);
protected:
/*
* Callbacks for ros messages from canopen_chain_node. There's one callback for each ros topic
* published by canopen_chain_node. Each callback updates the sensor state and publishes an
* OLS-Measurement message.
*/
virtual void cancallbackError(const boost::shared_ptr<std_msgs::UInt8 const>& msg);
virtual void cancallbackLCP1(const boost::shared_ptr<std_msgs::Int16 const>& msg);
virtual void cancallbackLCP2(const boost::shared_ptr<std_msgs::Int16 const>& msg);
virtual void cancallbackLCP3(const boost::shared_ptr<std_msgs::Int16 const>& msg);
virtual void cancallbackState(const boost::shared_ptr<std_msgs::UInt8 const>& msg);
virtual void cancallbackWidthLCP1(const boost::shared_ptr<std_msgs::Int16 const>& msg);
virtual void cancallbackWidthLCP2(const boost::shared_ptr<std_msgs::Int16 const>& msg);
virtual void cancallbackWidthLCP3(const boost::shared_ptr<std_msgs::Int16 const>& msg);
virtual void cancallbackCode(const boost::shared_ptr<std_msgs::UInt8 const>& msg);
}; // class CanCiA401Subscriber
} // namespace sick_line_guidance
#endif // __SICK_LINE_GUIDANCE_CAN_CIA401_SUBSCRIBER_H_INCLUDED

160
Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_can_mls_subscriber.h Executable file
View File

@@ -0,0 +1,160 @@
/*
* sick_line_guidance_can_mls_subscriber implements a ros subscriber to canopen mls messages.
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#ifndef __SICK_LINE_GUIDANCE_CAN_MLS_SUBSCRIBER_H_INCLUDED
#define __SICK_LINE_GUIDANCE_CAN_MLS_SUBSCRIBER_H_INCLUDED
#include "sick_line_guidance/sick_line_guidance_can_subscriber.h"
namespace sick_line_guidance
{
/*
* class CanMlsSubscriber implements the ros subscriber to canopen mls messages.
*/
class CanMlsSubscriber : public CanSubscriber
{
public:
/*
* Constructor.
* @param[in] nh ros::NodeHandle
* @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1"
* @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
* @param[in] max_sdo_rate max. sdo query and publish rate
* @param[in] ros_topic topic for ros messages, f.e. "mls" or "ols"
* @param[in] ros_frameid frameid for ros messages, f.e. "mls_measurement_frame" or "ols_measurement_frame"
* @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error)
* @param[in] subscribe_queue_size buffer size for ros messages
*/
CanMlsSubscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate, const std::string & ros_topic,
const std::string & ros_frameid, int initial_sensor_state = 0, int subscribe_queue_size = 1);
/*
* Destructor.
*/
virtual ~CanMlsSubscriber();
/*
* @brief subsribes to canopen topics for mls messages and sets the callbacks to handle messages from canopen_chain_node
* after PDO messages (LCP = line center point):
*
* Mapping for OLS:
*
* canopenchain-OLS -> CanSubscriber-callback -> OLS sensor state
* -----------------------------------------------------------------------
* <nodeid>+"_1001" -> cancallbackError(UINT8) -> Error register
* <nodeid>+"_2018" -> cancallbackDevState(UINT8) -> Device state
* <nodeid>+"_2021sub1" -> cancallbackLCP1(INT16) -> LCP1
* <nodeid>+"_2021sub2" -> cancallbackLCP2(INT16) -> LCP2
* <nodeid>+"_2021sub3" -> cancallbackLCP3(INT16) -> LCP3
* <nodeid>+"_2021sub4" -> cancallbackState(UINT8) -> State
* <nodeid>+"_2021sub5" -> cancallbackWidthLCP1(INT16) -> Width LCP1
* <nodeid>+"_2021sub6" -> cancallbackWidthLCP2(INT16) -> Width LCP2
* <nodeid>+"_2021sub7" -> cancallbackWidthLCP3(INT16) -> Width LCP3
* <nodeid>+"_2021sub8" -> cancallbackCode(UINT8) -> Code
* <nodeid>+"_2021sub9" -> cancallbackExtCode(UINT32) -> Extended Code
*
* Mapping for MLS:
*
* canopenchain-MLS -> CanSubscriber-callback -> MLS sensor state
* -----------------------------------------------------------------------
* <nodeid>+"_1001" -> cancallbackError(UINT8) -> Error register
* <nodeid>+"_2021sub1" -> cancallbackLCP1(INT16) -> LCP1
* <nodeid>+"_2021sub2" -> cancallbackLCP2(INT16) -> LCP2
* <nodeid>+"_2021sub3" -> cancallbackLCP3(INT16) -> LCP3
* <nodeid>+"_2021sub4" -> cancallbackMarker(UINT8) -> #LCP and marker
* <nodeid>+"_2022" -> cancallbackState(UINT8) -> State
*
* Mapping for CiA402 (example, testing only):
*
* canopenchain-CiA402 -> CanSubscriber-callback -> CiA402 state
* ---------------------------------------------------------------------
* <nodeid>+"_6000sub1" -> cancallbackError(UINT8) -> Error register
* <nodeid>+"_6000sub2" -> cancallbackState(UINT8) -> State
* <nodeid>+"_6000sub3" -> cancallbackCode(UINT8) -> Code
* <nodeid>+"_6000sub4" -> cancallbackExtCode(UINT8) -> LSB Extended Code
* <nodeid>+"_6401sub1" -> cancallbackLCP1(INT16) -> LCP1
* <nodeid>+"_6401sub2" -> cancallbackLCP2(INT16) -> LCP2
* <nodeid>+"_6401sub3" -> cancallbackLCP3(INT16) -> LCP3
* <nodeid>+"_6401sub4" -> cancallbackWidthLCP1(INT16) -> Width LCP1
* <nodeid>+"_6401sub5" -> cancallbackWidthLCP2(INT16) -> Width LCP2
* <nodeid>+"_6401sub6" -> cancallbackWidthLCP3(INT16) -> Width LCP3
*
* See operation manuals for details (file SICK-OLS-Operating_instructions_OLS10_de_IM0076955.PDF for OLS
* and file SICK-MLS-Operating_instructions_MLS_de_IM0076567.PDF for MLS)
*
* @return true on success, otherwise false.
*/
virtual bool subscribeCanTopics(void);
protected:
/*
* Callbacks for ros messages from canopen_chain_node. There's one callback for each ros topic
* published by canopen_chain_node. Each callback updates the sensor state and publishes an
* OLS-Measurement message.
*/
virtual void cancallbackLCP1(const boost::shared_ptr<std_msgs::Int16 const>& msg);
virtual void cancallbackLCP2(const boost::shared_ptr<std_msgs::Int16 const>& msg);
virtual void cancallbackLCP3(const boost::shared_ptr<std_msgs::Int16 const>& msg);
virtual void cancallbackMarker(const boost::shared_ptr<std_msgs::UInt8 const>& msg);
virtual void cancallbackState(const boost::shared_ptr<std_msgs::UInt8 const>& msg);
// virtual void cancallbackError(const boost::shared_ptr<std_msgs::UInt8 const>& msg);
}; // class CanMlsSubscriber
} // namespace sick_line_guidance
#endif // __SICK_LINE_GUIDANCE_CAN_MLS_SUBSCRIBER_H_INCLUDED

213
Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_can_ols_subscriber.h Executable file
View File

@@ -0,0 +1,213 @@
/*
* sick_line_guidance_can_ols_subscriber implements a ros subscriber to canopen ols messages.
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#ifndef __SICK_LINE_GUIDANCE_CAN_OLS_SUBSCRIBER_H_INCLUDED
#define __SICK_LINE_GUIDANCE_CAN_OLS_SUBSCRIBER_H_INCLUDED
#include "sick_line_guidance/sick_line_guidance_can_subscriber.h"
namespace sick_line_guidance
{
/*
* class CanOlsSubscriber implements the base ros subscriber to canopen ols messages.
*/
class CanOlsSubscriber : public CanSubscriber
{
public:
/*
* Constructor.
* @param[in] nh ros::NodeHandle
* @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1"
* @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
* @param[in] max_sdo_rate max. sdo query and publish rate
* @param[in] ros_topic topic for ros messages, f.e. "mls" or "ols"
* @param[in] ros_frameid frameid for ros messages, f.e. "mls_measurement_frame" or "ols_measurement_frame"
* @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error)
* @param[in] subscribe_queue_size buffer size for ros messages
*/
CanOlsSubscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate, const std::string & ros_topic,
const std::string & ros_frameid, int initial_sensor_state = 0, int subscribe_queue_size = 1);
/*
* Destructor.
*/
virtual ~CanOlsSubscriber();
/*
* @brief subsribes to canopen topics for ols messages and sets the callbacks to handle messages from canopen_chain_node
* after PDO messages (LCP = line center point):
*
* Mapping for OLS:
*
* canopenchain-OLS -> CanSubscriber-callback -> OLS sensor state
* -----------------------------------------------------------------------
* <nodeid>+"_1001" -> cancallbackError(UINT8) -> Error register
* <nodeid>+"_2018" -> cancallbackDevState(UINT8) -> Device state
* <nodeid>+"_2021sub1" -> cancallbackLCP1(INT16) -> LCP1
* <nodeid>+"_2021sub2" -> cancallbackLCP2(INT16) -> LCP2
* <nodeid>+"_2021sub3" -> cancallbackLCP3(INT16) -> LCP3
* <nodeid>+"_2021sub4" -> cancallbackState(UINT8) -> State
* <nodeid>+"_2021sub5" -> cancallbackWidthLCP1(INT16) -> Width LCP1
* <nodeid>+"_2021sub6" -> cancallbackWidthLCP2(INT16) -> Width LCP2
* <nodeid>+"_2021sub7" -> cancallbackWidthLCP3(INT16) -> Width LCP3
* <nodeid>+"_2021sub8" -> cancallbackCode(UINT8) -> Code
* <nodeid>+"_2021sub9" -> cancallbackExtCode(UINT32) -> Extended Code
*
* Mapping for MLS:
*
* canopenchain-MLS -> CanSubscriber-callback -> MLS sensor state
* -----------------------------------------------------------------------
* <nodeid>+"_1001" -> cancallbackError(UINT8) -> Error register
* <nodeid>+"_2021sub1" -> cancallbackLCP1(INT16) -> LCP1
* <nodeid>+"_2021sub2" -> cancallbackLCP2(INT16) -> LCP2
* <nodeid>+"_2021sub3" -> cancallbackLCP3(INT16) -> LCP3
* <nodeid>+"_2021sub4" -> cancallbackMarker(UINT8) -> #LCP and marker
* <nodeid>+"_2022" -> cancallbackState(UINT8) -> State
*
* Mapping for CiA402 (example, testing only):
*
* canopenchain-CiA402 -> CanSubscriber-callback -> CiA402 state
* ---------------------------------------------------------------------
* <nodeid>+"_6000sub1" -> cancallbackError(UINT8) -> Error register
* <nodeid>+"_6000sub2" -> cancallbackState(UINT8) -> State
* <nodeid>+"_6000sub3" -> cancallbackCode(UINT8) -> Code
* <nodeid>+"_6000sub4" -> cancallbackExtCode(UINT8) -> LSB Extended Code
* <nodeid>+"_6401sub1" -> cancallbackLCP1(INT16) -> LCP1
* <nodeid>+"_6401sub2" -> cancallbackLCP2(INT16) -> LCP2
* <nodeid>+"_6401sub3" -> cancallbackLCP3(INT16) -> LCP3
* <nodeid>+"_6401sub4" -> cancallbackWidthLCP1(INT16) -> Width LCP1
* <nodeid>+"_6401sub5" -> cancallbackWidthLCP2(INT16) -> Width LCP2
* <nodeid>+"_6401sub6" -> cancallbackWidthLCP3(INT16) -> Width LCP3
*
* See operation manuals for details (file SICK-OLS-Operating_instructions_OLS10_de_IM0076955.PDF for OLS
* and file SICK-MLS-Operating_instructions_MLS_de_IM0076567.PDF for MLS)
*
* @return true on success, otherwise false.
*/
virtual bool subscribeCanTopics(void);
protected:
/*
* Callbacks for ros messages from canopen_chain_node. There's one callback for each ros topic
* published by canopen_chain_node. Each callback updates the sensor state and publishes an
* OLS-Measurement message.
*/
virtual void cancallbackLCP1(const boost::shared_ptr<std_msgs::Int16 const>& msg);
virtual void cancallbackLCP2(const boost::shared_ptr<std_msgs::Int16 const>& msg);
virtual void cancallbackLCP3(const boost::shared_ptr<std_msgs::Int16 const>& msg);
virtual void cancallbackState(const boost::shared_ptr<std_msgs::UInt8 const>& msg);
virtual void cancallbackWidthLCP1(const boost::shared_ptr<std_msgs::Int16 const>& msg);
virtual void cancallbackWidthLCP2(const boost::shared_ptr<std_msgs::Int16 const>& msg);
virtual void cancallbackWidthLCP3(const boost::shared_ptr<std_msgs::Int16 const>& msg);
virtual void cancallbackCode(const boost::shared_ptr<std_msgs::UInt8 const>& msg);
// virtual void cancallbackError(const boost::shared_ptr<std_msgs::UInt8 const>& msg);
// virtual void cancallbackDevState(const boost::shared_ptr<std_msgs::UInt8 const>& msg);
// virtual void cancallbackExtCode(const boost::shared_ptr<std_msgs::UInt32 const>& msg);
bool m_bOLS20queries; // OLS20 only: query objects 2021subA (barcode center point, INT16), 2021subB (quality of lines, UINT8) and 2023sub1 to 2023sub3 (intensity line 1 - 3, UINT8)
}; // class CanOlsSubscriber
/*
* class CanOls10Subscriber derives from CanOlsSubscriber to implements OLS10 specific handling of the ros canopen messages.
*/
class CanOls10Subscriber : public CanOlsSubscriber
{
public:
/*
* Constructor.
* @param[in] nh ros::NodeHandle
* @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1"
* @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
* @param[in] max_sdo_rate max. sdo query and publish rate
* @param[in] ros_topic topic for ros messages, f.e. "mls" or "ols"
* @param[in] ros_frameid frameid for ros messages, f.e. "mls_measurement_frame" or "ols_measurement_frame"
* @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error)
* @param[in] subscribe_queue_size buffer size for ros messages
*/
CanOls10Subscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate, const std::string & ros_topic,
const std::string & ros_frameid, int initial_sensor_state = 0, int subscribe_queue_size = 1);
}; // class CanOls10Subscriber
/*
* class CanOls20Subscriber derives from CanOlsSubscriber to implements OLS20 specific handling of the ros canopen messages.
*/
class CanOls20Subscriber : public CanOlsSubscriber
{
public:
/*
* Constructor.
* @param[in] nh ros::NodeHandle
* @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1"
* @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
* @param[in] max_sdo_rate max. sdo query and publish rate
* @param[in] ros_topic topic for ros messages, f.e. "mls" or "ols"
* @param[in] ros_frameid frameid for ros messages, f.e. "mls_measurement_frame" or "ols_measurement_frame"
* @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error)
* @param[in] subscribe_queue_size buffer size for ros messages
*/
CanOls20Subscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate, const std::string & ros_topic,
const std::string & ros_frameid, int initial_sensor_state = 0, int subscribe_queue_size = 1);
}; // class CanOls20Subscriber
} // namespace sick_line_guidance
#endif // __SICK_LINE_GUIDANCE_CAN_OLS_SUBSCRIBER_H_INCLUDED

415
Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_can_subscriber.h Executable file
View File

@@ -0,0 +1,415 @@
/*
* sick_line_guidance_can_subscriber implements the base class for ros subscriber to canopen messages for OLS and MLS.
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#ifndef __SICK_LINE_GUIDANCE_CAN_SUBSCRIBER_H_INCLUDED
#define __SICK_LINE_GUIDANCE_CAN_SUBSCRIBER_H_INCLUDED
#include <boost/thread.hpp>
#include <ros/ros.h>
#include <std_msgs/Int8.h>
#include <std_msgs/Int16.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Float32.h>
#include <std_msgs/String.h>
#include <std_msgs/UInt8.h>
#include <std_msgs/UInt16.h>
#include <std_msgs/UInt32.h>
#include "sick_line_guidance/MLS_Measurement.h"
#include "sick_line_guidance/OLS_Measurement.h"
#include "sick_line_guidance/sick_line_guidance_msg_util.h"
namespace sick_line_guidance
{
/*
* class CanSubscriber: base class for CanOlsSubscriber and CanMlsSubscriber,
* implements the base class for ros subscriber to canopen messages for OLS and MLS.
* Converts canopen messages to MLS/OLS measurement messages and publishes
* MLS/OLS measurement messages on the configured ros topic ("/mls" or "/ols").
*
* class CanSubscriber::MeasurementHandler: queries SDOs (if required) and publishes MLS/OLS measurement messages in a background thread
*/
class CanSubscriber
{
public:
/*
* Constructor.
* @param[in] nh ros::NodeHandle
* @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1"
* @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
* @param[in] max_sdo_rate max. sdo query and publish rate
* @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error)
* @param[in] subscribe_queue_size buffer size for ros messages
*/
CanSubscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error = 2, double max_sdo_rate = 1000, int initial_sensor_state = 0, int subscribe_queue_size = 1);
/*
* Destructor.
*/
virtual ~CanSubscriber();
/*
* @brief subsribes to canopen topics for ols or mls messages and sets the callbacks to handle messages from canopen_chain_node
* after PDO messages (LCP = line center point). Implemented by subclasses CanOlsSubscriber and CanMlsSubscriber
* @return true on success, otherwise false.
*/
virtual bool subscribeCanTopics(void) = 0;
protected:
/*
* @brief Container class for sdo query support. Just collects the pending status of a query (query pending or not pending)
* and the time of the last successfull query.
*/
class QuerySupport
{
public:
/*
* Constructor
* @param[in] query_jitter jitter in seconds (default: 10 ms), i.e. a sdo is requested, if the query is pending and the last successful query is out of the time jitter.
* By default, a sdo request is send, if the query is pending and not done within the last 10 ms.
*/
QuerySupport(double query_jitter = 0.01) : m_bQueryPending(false), m_tLastQueryTime(0), m_tQueryJitter(query_jitter) {}
/*
* returns the pending status, i.e. returns true if the query is pending, returns false if the query is not pending.
*/
virtual bool & pending(void){ return m_bQueryPending; }
/*
* returns the time of the last successful query
*/
virtual ros::Time & time(void){ return m_tLastQueryTime; }
/*
* returns true, if a query is required, i.e. the query is pending and the last successful query is over time, otherwise false
*/
virtual bool required(void) { return m_bQueryPending && (ros::Time::now() - m_tLastQueryTime) > m_tQueryJitter; }
protected:
bool m_bQueryPending; // true if the query is pending, otherwise false
ros::Time m_tLastQueryTime; // time of the last successful query
ros::Duration m_tQueryJitter; // jitter in seconds, i.e. a sdo is requested, if the query is pending and the last successful query is out of a time jitter.
};
/*
* class MeasurementHandler: queries SDOs (if required) and publishes MLS/OLS measurement messages in a background thread
*/
class MeasurementHandler
{
public:
/*
* Constructor.
* @param[in] nh ros::NodeHandle
* @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1"
* @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
* @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error)
* @param[in] max_publish_rate max rate to publish OLS/MLS measurement messages (default: min. 1 ms between two measurement messages)
* @param[in] max_query_rate max rate to query SDOs if required (default: min. 1 ms between sdo queries)
* @param[in] schedule_publish_delay MLS and OLS measurement message are scheduled to be published 5 milliseconds after first PDO is received
* @param[in] max_publish_delay MLS and OLS measurement message are scheduled to be published max. 2*20 milliseconds after first PDO is received, even if a sdo request is pending (max. 2 * tpdo rate)
* @param[in] query_jitter jitter in seconds (default: 10 ms), i.e. a sdo is requested, if the query is pending and the last successful query is out of the time jitter.
* By default, a sdo request is send, if the query is pending and not done within the last 10 ms.
*/
MeasurementHandler(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error = 2, int initial_sensor_state = 0, double max_publish_rate = 1000, double max_query_rate = 1000, double schedule_publish_delay = 0.005, double max_publish_delay = 0.04, double query_jitter = 0.01);
/*
* Destructor.
*/
virtual ~MeasurementHandler();
/*
* @brief Runs the background thread to publish MLS/OLS measurement messages
*/
virtual void runMeasurementPublishThread(void);
/*
* @brief Runs the background thread to query SDOs, if required
*/
virtual void runMeasurementSDOqueryThread(void);
/*
* @brief schedules the publishing of the current MLS measurement message.
* @param[in] schedule if true, publishing is scheduled, otherwise a possibly pending schedule is removed.
*/
virtual void schedulePublishMLSMeasurement(bool schedule);
/*
* @brief schedules the publishing of the current OLS measurement message.
* @param[in] schedule if true, publishing is scheduled, otherwise a possibly pending schedule is removed.
*/
virtual void schedulePublishOLSMeasurement(bool schedule);
/*
* MeasurementHandler member data.
*/
ros::NodeHandle m_nh; // ros nodehandel
std::string m_can_nodeid; // can id for canopen_chain_node, f.e. "node1"
ros::Publisher m_ros_publisher; // ros publisher for ols or mls measurement messages
boost::mutex m_measurement_mutex; // lock guard for publishing and setting mls/ols sensor state
sick_line_guidance::MLS_Measurement m_mls_state; // current state of an mls sensor
sick_line_guidance::OLS_Measurement m_ols_state; // current state of an ols sensor
ros::Rate m_max_publish_rate; // max. rate to publish OLS/MLS measurement message
ros::Rate m_max_sdo_query_rate; // max. rate to query SDOs if required
ros::Time m_publish_mls_measurement; // time to publish next MLS measurement message
ros::Time m_publish_ols_measurement; // time to publish next OLS measurement message
ros::Time m_publish_measurement_latest; // latest time to publish a measurement message (even if a sdo request is pending)
boost::mutex m_publish_measurement_mutex; // lock guard to schedule publishing measurements using m_publish_mls_measurement and m_publish_ols_measurement
ros::Duration m_schedule_publish_delay; // MLS and OLS measurement message are scheduled to be published 5 milliseconds after first PDO is received
ros::Duration m_max_publish_delay; // MLS and OLS measurement message are scheduled to be published max. 2*20 milliseconds after first PDO is received, even if a sdo request is pending (max. 2 * tpdo rate)
QuerySupport m_ols_query_extended_code; // query object 0x2021sub9 (extended code, UINT32) in object dictionary by SDO, if pending
QuerySupport m_ols_query_device_status_u8; // query object 0x2018 (device status register, OLS20: UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread), if pending
QuerySupport m_ols_query_device_status_u16; // query object 0x2018 (device status register, OLS10: UINT16) in object dictionary by SDO (query runs in m_measurement in a background thread), if pending
QuerySupport m_ols_query_error_register; // query object 0x1001 (error register, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread), if pending
QuerySupport m_ols_query_barcode_center_point; // OLS20 only: query object 2021subA (barcode center point, INT16), if pending
QuerySupport m_ols_query_quality_of_lines; // OLS20 only: query object 2021subB (quality of lines, UINT8), if pending
QuerySupport m_ols_query_intensity_of_lines[3]; // OLS20 only: query object 2023sub1 to 2023sub3 (intensity lines 1 - 3, UINT8), if pending
boost::thread * m_measurement_publish_thread; // background thread to publishes MLS/OLS measurement messages
boost::thread * m_measurement_sdo_query_thread; // background thread to query SDOs if required
int m_max_num_retries_after_sdo_error; // After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
protected:
/*
* @brief returns true, if publishing of a MLS measurement is scheduled and time has been reached for publishing the current MLS measurement.
*/
virtual bool isMLSMeasurementTriggered(void);
/*
* @brief returns true, if publishing of a OLS measurement is scheduled and time has been reached for publishing the current OLS measurement.
*/
virtual bool isOLSMeasurementTriggered(void);
/*
* @brief returns true, if publishing of a measurement is scheduled and latest time for publishing has been reached.
*/
virtual bool isLatestTimeForMeasurementPublishing(void);
/*
* @brief returns true, if sdo query is pending, i.e. measurement is not yet completed (sdo request or sdo response still pending)
*/
virtual bool isSDOQueryPending(void);
/*
* @brief queries an object in the object dictionary by SDO and returns its value.
* @param[in] can_object_idx object index in object dictionary, f.e. "2018" (OLS device status) or "2021sub9" (OLS extended code)
* @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
* @param[out] can_object_value object value from SDO response
* @return true on success (can_object_value set to objects value), false otherwise (can_object_value not set)
*/
virtual bool querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, uint8_t & can_object_value);
/*
* @brief queries an object in the object dictionary by SDO and returns its value.
* @param[in] can_object_idx object index in object dictionary, f.e. "2018" (OLS device status) or "2021sub9" (OLS extended code)
* @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
* @param[out] can_object_value object value from SDO response
* @return true on success (can_object_value set to objects value), false otherwise (can_object_value not set)
*/
virtual bool querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, uint16_t & can_object_value);
/*
* @brief queries an object in the object dictionary by SDO and returns its value.
* @param[in] can_object_idx object index in object dictionary, f.e. "2018" (OLS device status) or "2021sub9" (OLS extended code)
* @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
* @param[out] can_object_value object value from SDO response
* @return true on success (can_object_value set to objects value), false otherwise (can_object_value not set)
*/
virtual bool querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, int16_t & can_object_value);
/*
* @brief queries an object in the object dictionary by SDO and returns its value.
* @param[in] can_object_idx object index in object dictionary, f.e. "2018" (OLS device status) or "2021sub9" (OLS extended code)
* @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
* @param[out] can_object_value object value from SDO response
* @return true on success (can_object_value set to objects value), false otherwise (can_object_value not set)
*/
virtual bool querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, uint32_t & can_object_value);
/*
* @brief queries an object in the object dictionary by SDO and returns its value.
* @param[in] can_object_idx object index in object dictionary, f.e. "2018" (OLS device status) or "2021sub9" (OLS extended code)
* @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
* @param[out] can_object_value object value from SDO response
* @return true on success (can_object_value set to objects value), false otherwise (can_object_value not set)
*/
virtual bool querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, std::string & can_object_value);
/*
* @brief queries an object value by SDO, if bQueryPending==true. After SDO query returned, output_value is set and bQueryPending cleared
* (assume bQueryPending==false after this function returned).
*
* @param[in+out] query if query.required() is true, object value is queried by SDO and query is updated (not pending any more). Otherwise, nothing is done.
* @param[in] object_index index in object dictionary, f.e. "2021sub9" for object 0x2021 subindex 9
* @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
* @param[out] output_value object value queried by SDO
* @param[in] norm_factor factor to convert object to output value, f.e. 0.001 to convert millimeter (object value) to meter (output value). Default: 1
*
* @return uint8_t value
*/
template <class S, class T> void querySDOifPending(QuerySupport & query, const std::string & object_index, int max_num_retries_after_sdo_error, T & output_value, T norm_factor)
{
if(query.pending())
{
S sdo_value;
if(query.required() && querySDO(object_index, max_num_retries_after_sdo_error, sdo_value))
{
query.time() = ros::Time::now();
if(query.pending())
{
ROS_INFO_STREAM("sick_line_guidance::CanSubscriber::MeasurementHandler: [" << object_index << "]=" << sick_line_guidance::MsgUtil::toHexString(sdo_value));
boost::lock_guard<boost::mutex> publish_lockguard(m_measurement_mutex);
output_value = (norm_factor * sdo_value);
}
}
query.pending() = false;
}
}
/*
* @brief converts a sdo response to uint8.
* Note: nh.serviceClient<canopen_chain_node::GetObject> returns SDO responses as strings.
* In case of 1 Byte (UINT8) values, the returned "string" contains this byte (uint8 value streamed to std::stringstream).
* Example: UINT8 with value 0 are returned as "\0", not "0". Parsing the SDO response has to handle the UINT8 case,
* which is done by this function
* Note: std::exception are caught (error message and return false in this case)
* @param[in] response sdo response as string
* @param[out] value uint8 value converted from SDO response
* @return true on success, false otherwise
*/
virtual bool convertSDOresponse(const std::string & response, uint8_t & value);
/*
* @brief converts a sdo response to uint32.
* Note: std::exception are caught (error message and return false in this case)
* @param[in] response sdo response as string
* @param[out] value uint32 value converted from SDO response
* @return true on success, false otherwise
*/
virtual bool convertSDOresponse(const std::string & response, uint32_t & value);
/*
* @brief converts a sdo response to int32.
* Note: std::exception are caught (error message and return false in this case)
* @param[in] response sdo response as string
* @param[out] value uint32 value converted from SDO response
* @return true on success, false otherwise
*/
virtual bool convertSDOresponse(const std::string & response, int32_t & value);
}; // class MeasurementHandler
/*
* @brief converts an std_msgs::UInt8/UInt16/UInt32 message to a uint8_t/uint16_t/uint32_t value.
*
* @param[in] msg UINT8 message
* @param[in] defaultvalue default, is returned in case of an invalid message
* @param[in] maxvalue 0xFF, 0xFFFF or 0xFFFFFFFF
* @param[in] dbg_info informal debug message (prints to ROS_DEBUG or ROS_ERROR)
*
* @return uint8_t value
*/
template <class S, class T> T convertIntegerMessage(const boost::shared_ptr<S const>& msg, const T & defaultvalue, unsigned maxvalue, const std::string & dbg_info)
{
T value = defaultvalue;
if(msg)
{
value = ((msg->data)&maxvalue);
ROS_DEBUG("sick_line_guidance::CanSubscriber(%s): data: 0x%02x", dbg_info.c_str(), (unsigned)value);
}
else
{
ROS_ERROR("## ERROR sick_line_guidance::CanSubscriber(%s): invalid message (%s:%d)", dbg_info.c_str(), __FILE__, __LINE__);
}
return value;
}
/*
* @brief converts an INT16 message (line center point lcp in millimeter) to a float lcp in meter.
*
* @param[in] msg INT16 message (line center point lcp in millimeter)
* @param[in] defaultvalue default, is returned in case of an invalid message
* @param[in] dbg_info informal debug message (ROS_INFO/ROS_ERROR)
*
* @return float lcp in meter
*/
virtual float convertToLCP(const boost::shared_ptr<std_msgs::Int16 const>& msg, const float & defaultvalue, const std::string & dbg_info);
/*
* @brief schedules the current MLS measurement message for publishing.
*/
virtual void publishMLSMeasurement(void);
/*
* @brief schedules the current OLS measurement message for publishing.
*/
virtual void publishOLSMeasurement(void);
/*
* member data.
*/
std::vector<ros::Subscriber> m_can_subscriber; // list of subscriber to canopen_chain_node messages
int m_subscribe_queue_size; // buffer size for ros messages (default: 1)
MeasurementHandler m_measurement; // handles MLS/OLS measurement messages, queries SDO if requiered and publishes ros measurement messages.
}; // class CanSubscriber
} // namespace sick_line_guidance
#endif // __SICK_LINE_GUIDANCE_CAN_SUBSCRIBER_H_INCLUDED

229
Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_canopen_chain.h Executable file
View File

@@ -0,0 +1,229 @@
/*
* sick_line_guidance_canopen_chain wraps and adapts RosChain of package canopen_chain_node for sick_line_guidance.
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#ifndef __SICK_LINE_GUIDANCE_CANOPEN_CHAIN_H_INCLUDED
#define __SICK_LINE_GUIDANCE_CANOPEN_CHAIN_H_INCLUDED
#include <socketcan_interface/dispatcher.h>
#include <socketcan_interface/socketcan.h>
#include <canopen_chain_node/ros_chain.h>
namespace sick_line_guidance
{
/*
* class CanopenChain implements canopen support for sick_line_guidance
* based on RosChain of package canopen_chain_node (package ros_canopen).
*/
class CanopenChain : public canopen::RosChain
{
public:
/*
* Constructor
*
* @param[in] nh ros::NodeHandle
* @param[in] nh_priv ros::NodeHandle
*/
CanopenChain(const ros::NodeHandle &nh, const ros::NodeHandle &nh_priv);
/*
* Destructor
*/
virtual ~CanopenChain();
/*
* Connects to CAN bus by calling "init" of canopen_chain_node ros-service
*
* @param[in] nh ros::NodeHandle
*
* @return true, if initialization successful, otherwise false.
*/
static bool connectInitService(ros::NodeHandle &nh);
/*
* Sets all dcf overlays in the can devices. All dcf values are reread from the can device and checked to be identical
* to the configured object value. If all dcf overlays have been set successfully (i.e. successfull write operation,
* successfull re-read operation and object value identical to the configured value), true is returned.
* Otherwise, false is returned (i.e. some dcf overlays could not be set).
*
* dcf overlays are a list of key value pairs with "object_index" as key and "object_value" as value.
* Examples for dcf overlays can be found in the yaml configuration file. Example from sick_line_guidance_ols20.yaml:
* node1:
* id: 0x0A # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
* eds_pkg: sick_line_guidance # package name for relative path
* eds_file: SICK_OLS20.eds # path to EDS/DCF file
* ...
* dcf_overlay:
* "2001sub5": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue 0x00
* "2002sub1": "0.015" # Typ. Width, FLOAT32, DataType=0x0008
* "2002sub2": "0.001" # Min. Width, FLOAT32, DataType=0x0008
* "2002sub3": "0.050" # Max. Width, FLOAT32, DataType=0x0008
*
* @param[in] nh ros::NodeHandle
* @param[in] node_id can node id of the can device
* @param[in] dcf_overlays list of dcf overlays, format "object_index":"object_value"
*
* @return true, if dcf overlays set successfully, otherwise false (write error, read error
* or queried value differs from configured object value).
*/
static bool writeDCFoverlays(ros::NodeHandle &nh, const std::string & node_id, XmlRpc::XmlRpcValue & dcf_overlays);
/*
* Queries an object of a can node from canopen service by its object index.
*
* @param[in] nh ros::NodeHandle
* @param[in] node_id can node id of the can device
* @param[in] object index in the object dictonary of the can device, f.e. "1001sub", "1018sub4"
* @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
* @param[out] output_message informational message in case of errors (responce from canopen service)
* @param[out] output_value value of the object (responce from canopen service)
*
* @return true, if query successful, otherwise false.
*/
static bool queryCanObject(ros::NodeHandle &nh, const std::string & node_id, const std::string & objectidx, int max_num_retries_after_sdo_error,
std::string & output_message, std::string & output_value);
/*
* Sets the value of an object in the object dictionary of a can device.
*
* @param[in] nh ros::NodeHandle
* @param[in] node_id can node id of the can device
* @param[in] object index in the object dictonary of the can device, f.e. "1001sub", "1018sub4"
* @param[in] object_value value of the object
* @param[out] response value of the object (responce from canopen service)
*
* @return true, if SDO successful, otherwise false.
*/
static bool setCanObjectValue(ros::NodeHandle &nh, const std::string & node_id, const std::string & objectidx,
const std::string & object_value, std::string & response);
/*
* Recovers can driver after emergency or other errors. Calls "/driver/recover" from canopen_chain_node
*
* @param[in] nh ros::NodeHandle
*
* @return true, if recover command returned with success, otherwise false.
*/
static bool recoverCanDriver(ros::NodeHandle &nh);
/*
* Shutdown can driver. Calls "/driver/shutdown" from canopen_chain_node
*
* @param[in] nh ros::NodeHandle
*
* @return true, if shutdown command returned with success, otherwise false.
*/
static bool shutdownCanDriver(ros::NodeHandle & nh);
protected:
/*
* Compares a destination value configured by dcf_overlay with the sdo response.
* Comparison by string, integer or float comparison.
* Returns true, if both values are identical, or otherwise false.
* Note: dcf_overlay_value and sdo_response may represent identical numbers, but different strings (different number formatting)
* Therefor, both string and numbers are compared in this function.
*
* @param[in] dcf_overlay_value configured value
* @param[in] sdo_response received value
*
* @return true, if dcf_overlay_value is identical to sdo_response or has identical values.
*/
static bool cmpDCFoverlay(const std::string & dcf_overlay_value, const std::string & sdo_response);
/*
* Converts a string to UINT32. Catches conversion exceptions and returns false in case of number parsing errors.
*
* @param[in] str input string to be parsed
* @param[out] value output value (== input converted to number in case of success)
*
* @return true in case of success, false otherwise
*/
static uint32_t tryParseUINT32(const std::string & str, uint32_t & value);
/*
* Converts a string to INT32. Catches conversion exceptions and returns false in case of number parsing errors.
*
* @param[in] str input string to be parsed
* @param[out] value output value (== input converted to number in case of success)
*
* @return true in case of success, false otherwise
*/
static int32_t tryParseINT32(const std::string & str, int32_t & value);
/*
* Converts a string to FLOAT32. Catches conversion exceptions and returns false in case of number parsing errors.
*
* @param[in] str input string to be parsed
* @param[out] value output value (== input converted to number in case of success)
*
* @return true in case of success, false otherwise
*/
static int32_t tryParseFLOAT(const std::string & str, float & value);
/*
* Prints and converts a sdo response. 1 byte datatypes (UINT8) are converted to decimal (f.e. "\0" is returned as "0").
*
* @param[in] response sdo response
*
* @return printed response.
*/
static std::string sdoResponseToString(const std::string & response);
}; // class CanopenChain
} // namespace sick_line_guidance
#endif // __SICK_LINE_GUIDANCE_CANOPEN_CHAIN_H_INCLUDED

154
Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_cloud_converter.h Executable file
View File

@@ -0,0 +1,154 @@
/*
* sick_line_guidance_cloud_converter converts sensor measurement data to PointCloud2.
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#ifndef __SICK_LINE_GUIDANCE_CLOUD_CONVERTER_H_INCLUDED
#define __SICK_LINE_GUIDANCE_CLOUD_CONVERTER_H_INCLUDED
#include <sensor_msgs/PointCloud2.h>
#include "sick_line_guidance/MLS_Measurement.h"
#include "sick_line_guidance/OLS_Measurement.h"
namespace sick_line_guidance
{
/*
* class CloudConverter implements utility functions to convert measurement data to PointCloud2.
*/
class CloudConverter
{
public:
/*
* @brief converts OLS measurement data to PointCloud2.
*
* @param[in] measurement OLS measurement data
* @param[in]frame_id frame_id of PointCloud2 message
*
* @return sensor_msgs::PointCloud2 data converted from measurement
*/
static sensor_msgs::PointCloud2 convert(const sick_line_guidance::OLS_Measurement & measurement, const std::string & frame_id);
/*
* @brief converts OLS measurement data to PointCloud2.
*
* @param[in] measurement OLS measurement data
* @param[in]frame_id frame_id of PointCloud2 message
*
* @return sensor_msgs::PointCloud2 data converted from measurement
*/
static sensor_msgs::PointCloud2 convert(const sick_line_guidance::MLS_Measurement & measurement, const std::string & frame_id);
/*
* @brief prints a PointCloud2 data field to string.
*
* @param[in] cloud PointCloud2 data
*
* @return PointCloud2 data converted to string
*/
static std::string cloudDataToString(const sensor_msgs::PointCloud2 & cloud);
protected:
/*
* @brief returns the status for each line (detected or not detected).
*
* @param[in] status status byte of measurement data
* @param[in] numlines number of lined (normally 3 lines)
*
* @return detection status for each line
*/
static std::vector<bool> linestatus(uint8_t status, size_t numlines);
/*
* @brief returns the number of lines detected (1, 2 or 3 lines).
*
* @param[in] status status byte of measurement data
*
* @return number of lines detected by OLS or MLS
*/
static int linenumber(uint8_t status);
/*
* @brief returns true, if MLS measurement status is okay, or false otherwise.
*
* @param[in] measurement MLS measurement data
*
* @return true (sensor status okay), or false (sensor status not okay)
*/
static bool measurementstatus(const sick_line_guidance::MLS_Measurement & measurement);
/*
* @brief converts and prints a single field of PointCloud2 according to its dataype.
*
* @param[in] datatype enumeration of the datatye, see sensor_msgs::PointField
* @param[in] data pointer to the data to be converted and printed
* @param[in] end pointer to the end of PointCloud2 data
*
* @return data field converted to string
*/
static std::string cloudDataFieldToString(uint8_t datatype, const uint8_t* data, const uint8_t* end);
/*
* @brief flips the bits of a code, i.e. reverses the bits (output bit 0 := input bit 31, output bit 1 := input bit 30, and so on)
*
* @param[in] code bits to be flipped
*
* @return flipped code
*/
static uint32_t flipBits(uint32_t code);
}; // class CloudConverter
} // namespace sick_line_guidance
#endif // __SICK_LINE_GUIDANCE_CLOUD_CONVERTER_H_INCLUDED

160
Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_diagnostic.h Executable file
View File

@@ -0,0 +1,160 @@
/*
* sick_line_guidance_diagnostic publishes diagnostic messages for sick_line_guidance
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#ifndef __SICK_LINE_GUIDANCE_DIAGNOSTIC_H_INCLUDED
#define __SICK_LINE_GUIDANCE_DIAGNOSTIC_H_INCLUDED
#include <ros/ros.h>
namespace sick_line_guidance
{
/*
* enum DIAGNOSTIC_STATUS enumerates the possible status values of diagnostic messages.
* Higher values mean more severe failures.
*/
typedef enum DIAGNOSTIC_STATUS_ENUM
{
OK, // status okay, no errors
EXIT, // sick_line_guidance exiting
NO_LINE_DETECTED, // device signaled "no line detected"
ERROR_STATUS, // device signaled an error (error flag set in TPDO)
SDO_COMMUNICATION_ERROR, // error in SDO query, timeout on receiving SDO response
CAN_COMMUNICATION_ERROR, // can communication error, shutdown and reset communication
CONFIGURATION_ERROR, // invalid configuration, check configuration files
INITIALIZATION_ERROR, // initialization of CAN driver failed
INTERNAL_ERROR // internal error, should never happen
} DIAGNOSTIC_STATUS;
/*
* class Diagnostic publishes diagnostic messages for sick_line_guidance
*/
class Diagnostic
{
public:
/*
* Initializes the global diagnostic handler.
*
* @param[in] nh ros::NodeHandle
* @param[in] publish_topic ros topic to publish diagnostic messages
* @param[in] component description of the component reporting
*/
static void init(ros::NodeHandle & nh, const std::string & publish_topic, const std::string & component)
{
g_diagnostic_handler.init(nh, publish_topic, component);
}
/*
* Updates and reports the current status.
*
* @param[in] status current status (OK, ERROR, ...)
* @param[in] message optional diagnostic message
*/
static void update(DIAGNOSTIC_STATUS status, const std::string & message = "")
{
g_diagnostic_handler.update(status, message);
}
protected:
/*
* class DiagnosticImpl implements diagnostics for sick_line_guidance
*/
class DiagnosticImpl
{
public:
/*
* Constructor.
*/
DiagnosticImpl();
/*
* Initialization.
*
* @param[in] nh ros::NodeHandle
* @param[in] publish_topic ros topic to publish diagnostic messages
* @param[in] component description of the component reporting
*/
void init(ros::NodeHandle & nh, const std::string & publish_topic, const std::string & component);
/*
* Updates and reports the current status.
*
* @param[in] status current status (OK, ERROR, ...)
* @param[in] message optional diagnostic message
*/
void update(DIAGNOSTIC_STATUS status, const std::string & message = "");
protected:
/*
* member data.
*/
bool m_diagnostic_initialized; // flag indicating proper initialization of diagnostics
ros::Publisher m_diagnostic_publisher; // publishes diagnostic messages
std::string m_diagnostic_component; // name of the component publishing diagnostic messages
}; // class DiagnosticImpl
static DiagnosticImpl g_diagnostic_handler; // singleton, implements the diagnostics for sick_line_guidance
}; // class Diagnostic
} // namespace sick_line_guidance
#endif // __SICK_LINE_GUIDANCE_DIAGNOSTIC_H_INCLUDED

110
Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_eds_util.h Executable file
View File

@@ -0,0 +1,110 @@
/*
* sick_line_guidance_eds_util implements utility functions for eds-files.
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#ifndef __SICK_LINE_GUIDANCE_EDS_UTIL_H_INCLUDED
#define __SICK_LINE_GUIDANCE_EDS_UTIL_H_INCLUDED
#include <canopen_master/objdict.h>
namespace sick_line_guidance
{
/*
* class EdsUtil implements utility functions for eds-files.
*/
class EdsUtil
{
public:
/*
* @brief returns a full qualified filepath from package name and file name.
*
* @param package package name
*
* @return full qualified filepath (or filename if package is empty or couldn't be found).
*/
static std::string filepathFromPackage(const std::string & package, const std::string & filename);
/*
* @brief reads and parses an eds-file and prints all objects.
*
* @param eds_filepath path to eds-file
*
* @return object dictionary of a given eds-file printed to string.
*/
static std::string readParsePrintEdsfile(const std::string & eds_filepath);
/*
* @brief prints the data of a canopen::ObjectDict::Entry value to std::string.
*
* @param entry object dictionary entry to be printed
*
* @return entry converted to string.
*/
static std::string printObjectDictEntry(const canopen::ObjectDict::Entry & entry);
/*
* @brief prints the data bytes of a canopen::HoldAny value to std::string.
*
* @param parameter_name name of the parameter
* @param holdany value to be printed
*
* @return value converted to string.
*/
static std::string printHoldAny(const std::string & parameter_name, const canopen::HoldAny & holdany);
}; // class EdsUtil
} // namespace sick_line_guidance
#endif // __SICK_LINE_GUIDANCE_EDS_UTIL_H_INCLUDED

276
Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_msg_util.h Executable file
View File

@@ -0,0 +1,276 @@
/*
* sick_line_guidance_msg_util implements utility functions for ros messages.
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#ifndef __SICK_LINE_GUIDANCE_MSG_UTIL_H_INCLUDED
#define __SICK_LINE_GUIDANCE_MSG_UTIL_H_INCLUDED
#include <sstream>
#include <ros/ros.h>
#include <std_msgs/Int8.h>
#include <std_msgs/Int16.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Float32.h>
#include <std_msgs/String.h>
#include <std_msgs/UInt8.h>
#include <std_msgs/UInt16.h>
#include <std_msgs/UInt32.h>
#include "sick_line_guidance/MLS_Measurement.h"
#include "sick_line_guidance/OLS_Measurement.h"
namespace sick_line_guidance
{
/*
* class MsgUtil implements utility functions for ros messages.
*/
class MsgUtil
{
public:
/*
* @brief compares two messages, returns true if content is equal, or false otherwise.
*
* @param[in] msg1 message to be compared to msg2
* @param[in] msg2 message to be compared to msg1
*
* @return true if message content is equal, false otherwise.
*/
template<class T> static bool msgIdentical(const T & msg1, const T & msg2)
{
std::stringstream s1;
std::stringstream s2;
s1 << msg1;
s2 << msg2;
return s1.str() == s2.str();
}
/*
* @brief shortcut to convert an uint8 value to hex string "0x...".
*
* @param[in] value value to be printed
*
* @return hex string .
*/
static std::string toHexString(uint8_t value)
{
return toHexString(static_cast<unsigned>(value & 0xFF), 2);
}
/*
* @brief shortcut to convert an uint16 value to hex string "0x...".
*
* @param[in] value value to be printed
*
* @return hex string .
*/
static std::string toHexString(uint16_t value)
{
return toHexString(static_cast<unsigned>(value & 0xFFFF), 4);
}
/*
* @brief shortcut to convert an uint16 value to hex string "0x...".
*
* @param[in] value value to be printed
*
* @return hex string .
*/
static std::string toHexString(int16_t value)
{
std::stringstream str;
str << toHexString(static_cast<uint16_t>(value));
str << "=" << value << "d";
return str.str();
}
/*
* @brief shortcut to convert an uint32 value to hex string "0x...".
*
* @param[in] value value to be printed
*
* @return hex string .
*/
static std::string toHexString(uint32_t value, int w = 8)
{
std::stringstream str;
str << "0x" << std::uppercase << std::hex << std::setfill('0') << std::setw(w) << value;
return str.str();
}
/*
* @brief prints and returns a MLS measurement as informational string
* @param[in] measurement_msg MLS measurement to print
* @return informational string containing the MLS measurement data
*/
static std::string toInfo(const sick_line_guidance::MLS_Measurement & measurement_msg);
/*
* @brief prints and returns a OLS measurement as informational string
* @param[in] measurement_msg OLS measurement to print
* @return informational string containing the OLS measurement data
*/
static std::string toInfo(const sick_line_guidance::OLS_Measurement & measurement_msg);
/*
* @brief initializes a MLS measurement with zero data
* @param[in+out] measurement_msg MLS measurement
*/
static void zero(sick_line_guidance::MLS_Measurement & measurement_msg);
/*
* @brief initializes an OLS measurement with zero data
* @param[in+out] measurement_msg OLS measurement
*/
static void zero(sick_line_guidance::OLS_Measurement & measurement_msg);
/*
* @brief Returns a MLS sensor measurement
*
* @param[in] lcp1 line center point LCP1 in meter, object 0x2021sub1 in object dictionary
* @param[in] lcp2 line center point LCP2 in meter, object 0x2021sub2 in object dictionary
* @param[in] lcp3 line center point LCP3 in meter, object 0x2021sub3 in object dictionary
* @param[in] status status of measurement, object 0x2022 in object dictionary (Bit7=MSBit to Bit0=LSBit): Bit7: 0, Bit6: ReadingCode, Bit5: Polarity, Bit4: SensorFlipped, Bit3: LineLevBit2, Bit2: LineLevBit1, Bit1: LineLevBit0, Bit0: Linegood(1:LineGood,0:NotGood)
* @param[in] lcp LCP-flags (signs and line assignment), in bits (MSB to LSB): Bit7: MarkerBit4, Bit6: MarkerBit3, Bit5: MarkerBit2, Bit4: MarkerBit1, Bit3: MarkerBit0, Bit2: #LCPBit2, Bit1: #LCPBit1, Bit0: #LCPBit0
* @param[in] error error register, object 0x1001 in object dictionary
* @param[in] msg_frame_id frame id of OLS_Measurement message
*
* @return parameter converted to MLS_Measurement
*/
static sick_line_guidance::MLS_Measurement convertMLSMessage(float lcp1, float lcp2, float lcp3, uint8_t status, uint8_t lcp, uint8_t error, const std::string & msg_frame_id);
/*
* @brief Returns an OLS sensor measurement
*
* @param[in] lcp1 line center point LCP1 in meter, object 0x2021sub1 in object dictionary
* @param[in] lcp2 line center point LCP2 in meter, object 0x2021sub2 in object dictionary
* @param[in] lcp3 line center point LCP3 in meter, object 0x2021sub3 in object dictionary
* @param[in] width1 width of line 1 in meter, object 0x2021sub5 in object dictionary
* @param[in] width2 width of line 2 in meter, object 0x2021sub6 in object dictionary
* @param[in] width3 width of line 3 in meter, object 0x2021sub7 in object dictionary
* @param[in] status status of measurement, object 0x2021sub4 in object dictionary, in bits (MSB to LSB): Bit7: CodeValid, Bit6: CodeFlipped, Bit5: x, Bit4: DeviceStatus, Bit3: x, Bit2: #LCPBit2, Bit1: #LCPBit1, Bit0: #LCPBit0
* @param[in] barcode Barcode (> 255: extended barcode), , object 0x2021sub8 and 0x2021sub9 in object dictionary
* @param[in] dev_status Device status, object 0x2018 in object dictionary
* @param[in] error error register, object 0x1001 in object dictionary
* @param[in] barcodecenter barcode_center_point, OLS20 only (0x2021subA), OLS10: always 0
* @param[in] linequality quality_of_lines, OLS20 only (0x2021subB), OLS10: always 0
* @param[in] lineintensity1 intensity_of_lines[0], OLS20 only (0x2023sub1), OLS10: always 0
* @param[in] lineintensity2 intensity_of_lines[1], OLS20 only (0x2023sub2), OLS10: always 0
* @param[in] lineintensity3 intensity_of_lines[2], OLS20 only (0x2023sub3), OLS10: always 0
* @param[in] msg_frame_id frame id of OLS_Measurement message
*
* @return parameter converted to OLS_Measurement
*/
static sick_line_guidance::OLS_Measurement convertOLSMessage(float lcp1, float lcp2, float lcp3, float width1, float width2, float width3, uint8_t status, uint32_t barcode, uint8_t dev_status, uint8_t error,
float barcodecenter, uint8_t linequality, uint8_t lineintensity1, uint8_t lineintensity2, uint8_t lineintensity3, const std::string & msg_frame_id);
/*
* @brief returns true, if OLS device status is okay, or false otherwise.
*
* @param[in] measurement OLS measurement data
*
* @return true (sensor status okay), or false (sensor status not okay)
*/
static bool statusOK(const sick_line_guidance::OLS_Measurement & measurement)
{
// OLS status bit 4: 0 => Sensor ok, 1 => Sensor not ok, see 0x2018 (measurement.dev_status)
return ((measurement.status & (0x1 << 4)) == 0);
}
/*
* @brief returns true, if OLS device detected a line, or false otherwise.
*
* @param[in] measurement OLS measurement data
*
* @return true (line good), or false (no line detected)
*/
static bool lineOK(const sick_line_guidance::OLS_Measurement & measurement)
{
return ((measurement.status & 0x7) != 0); // Bit 0-2 OLS status == 0 => no line found
}
/*
* @brief returns true, if MLS device detected a line, or false otherwise.
*
* @param[in] measurement MLS measurement data
*
* @return true (line good), or false (no line detected)
*/
static bool lineOK(const sick_line_guidance::MLS_Measurement & measurement)
{
// MLS status bit 0 ("Line good") == 0 => no line detected or line too weak, 1 => line detected, MLS #lcp (bit 0-2 == 0) => no line detected
return ((measurement.status & 0x1) != 0) && ((measurement.lcp & 0x7) != 0);
}
/*
* @brief returns a gaussian destributed random line center position (lcp)
*
* @param stddev standard deviation of gaussian random generator
*
* @return random line center position
*/
static float randomLCP(double stddev);
/*
* @brief returns a gaussian destributed random line width
*
* @param stddev standard deviation of gaussian random generator
*
* @return random line width
*/
static float randomLW(double min_linewidth, double max_linewidth);
}; // class MsgUtil
} // namespace sick_line_guidance
#endif // __SICK_LINE_GUIDANCE_MSG_UTIL_H_INCLUDED

89
Devices/Packages/sick_line_guidance/include/sick_line_guidance/sick_line_guidance_version.h Executable file
View File

@@ -0,0 +1,89 @@
/*
* sick_line_guidance_version handles the version of sick_line_guidance ros driver.
* The version concates major version, minor version and patch level, each with three digits.
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#ifndef __SICK_LINE_GUIDANCE_VERSION_H_INCLUDED
#define __SICK_LINE_GUIDANCE_VERSION_H_INCLUDED
#define SICK_LINE_GUIDANCE_MAJOR_VER 1
#define SICK_LINE_GUIDANCE_MINOR_VER 0
#define SICK_LINE_GUIDANCE_PATCH_LEVEL 0
namespace sick_line_guidance
{
/*
* class Version handles the version of sick_line_guidance ros driver.
*/
class Version
{
public:
/*
* @brief returns the version as a string, f.e. "001.002.003".
*
* @return version string.
*/
static std::string getVersionInfo(void)
{
std::stringstream str;
str << std::uppercase << std::setfill('0') << std::setw(3) << SICK_LINE_GUIDANCE_MAJOR_VER << ".";
str << std::uppercase << std::setfill('0') << std::setw(3) << SICK_LINE_GUIDANCE_MINOR_VER << ".";
str << std::uppercase << std::setfill('0') << std::setw(3) << SICK_LINE_GUIDANCE_PATCH_LEVEL;
return str.str();
}
}; // class Version
} // namespace sick_line_guidance
#endif // __SICK_LINE_GUIDANCE_VERSION_H_INCLUDED

18
Devices/Packages/sick_line_guidance/launch/sick_canopen_simu.launch Executable file
View File

@@ -0,0 +1,18 @@
<launch>
<!-- sick_line_guidance: run sick_canopen_simu_node -->
<arg name="device"/> <!-- can device, either "OLS" or "MLS" -->
<node name="sick_canopen_simu_node" pkg="sick_line_guidance" type="sick_canopen_simu_node" />
<param name="sick_canopen_simu_cfg_file" type="str" value="$(find sick_line_guidance)/sick_canopen_simu_cfg.xml" /> <!-- configuration file and testcases for OLS and MLS simulation -->
<param name="can_node_id" type="int" value="10" /> <!-- can node id of OLS/MLS simulator -->
<param name="sick_device_family" type="str" value="$(arg device)" /> <!-- simulation of either "OLS10", "OLS20" or "MLS" device -->
<param name="can_subscribe_topic" type="str" value="can0" /> <!-- topic for ros can messages (input), message type can_msgs::Frame -->
<param name="mls_subscribe_topic" type="str" value="mls" /> <!-- topic for ros measurement messages (input), message type MLS_Measurement -->
<param name="ols_subscribe_topic" type="str" value="ols" /> <!-- topic for ros measurement messages (input), message type OLS_Measurement -->
<param name="publish_topic" type="str" value="ros2can0" /> <!-- topic for ros messages (output), message type can_msgs::Frame -->
<param name="pdo_rate" type="double" value="10" /> <!-- rate of PDOs: 50 PDOs simulated per second, i.e. 20 ms between two PDOs -->
<param name="pdo_repeat_cnt" type="int" value="25" /> <!-- each sensor state spefied in sick_canopen_simu_cfg.xml is repeated 25 times before switching to the next state (sensor state changes after 0.5 seconds) -->
<param name="can_message_queue_size" type="int" value="16" /> <!-- buffer size for can messages -->
<param name="sensor_state_queue_size" type="int" value="2" /> <!-- buffer size for simulated sensor states (OLS: 2 TPDOs) -->
</launch>

35
Devices/Packages/sick_line_guidance/launch/sick_line_guidance.launch Executable file
View File

@@ -0,0 +1,35 @@
<launch>
<!-- sick_line_guidance: global configuration -->
<arg name="yaml" default="$(find sick_line_guidance)/sick_line_guidance_mls.yaml"/>
<rosparam command="load" file="$(arg yaml)" /> <!-- Global CAN configuration by ols or mls yaml-file incl. link to eds-file -->
<!-- sick_line_guidance: run canopen_chain_node -->
<!-- to run sick_line_guidance_can_chain_node in gdb: add parameter launch-prefix="gdb -ex run - -args": -->
<!-- node name="sick_line_guidance_can_chain_node" pkg="sick_line_guidance" type="sick_line_guidance_can_chain_node" launch-prefix="gdb -ex run - -args" output="screen" -->
<node name="sick_line_guidance_can_chain_node" pkg="sick_line_guidance" type="sick_line_guidance_can_chain_node" >
<rosparam command="load" file="$(arg yaml)" /> <!-- Private CAN configuration for canopen_chain_node by ols or mls yaml-file -->
<param name="diagnostic_topic" type="str" value="diagnostics" /> <!-- ROS topic for diagnostic messages -->
</node>
<!-- sick_line_guidance: run sick_line_guidance_node, which implements the ROS driver for OLS and MLS -->
<node name="sick_line_guidance_node" pkg="sick_line_guidance" type="sick_line_guidance_node" >
<param name="diagnostic_topic" type="str" value="diagnostics" /> <!-- ROS topic for diagnostic messages -->
<param name="can_connect_init_at_startup" type="bool" value="true" /> <!-- Additional CAN configuration: if true, canopen services are initialized at startup -->
<param name="initial_sensor_state" type="int" value="7" /> <!-- initial sensor states (f.e. 0x07 for 3 detected lines, or 8 to indicate sensor error) -->
<param name="subscribe_queue_size" type="int" value="16" /> <!-- buffer size for ros messages -->
<param name="max_sdo_rate" type="double" value="100.0" /> <!-- max. sdo query and publish rate -->
<param name="max_num_retries_after_sdo_error" type="int" value="2" /> <!-- After SDO error, the SDO query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. -->
</node>
<!-- sick_line_guidance: cloud_publisher_node to convert OLS_Measurement and MLS_Measurement messages to PointCloud2 -->
<node name="sick_line_guidance_cloud_publisher" pkg="sick_line_guidance" type="sick_line_guidance_cloud_publisher">
<param name="mls_topic_publish" type="str" value="mls" /> <!-- MLS_Measurement data are published in topic "/mls" -->
<param name="ols_topic_publish" type="str" value="ols" /> <!-- OLS_Measurement data are published in topic "/ols" -->
<param name="cloud_topic_publish" type="str" value="cloud" /> <!-- sensor_msgs::PointCloud messages are published in topic "/cloud" -->
<param name="mls_cloud_frame_id" type="str" value="mls_frame" /> <!-- MLS PointCloud data are published with frame id "mls_frame" -->
<param name="ols_cloud_frame_id" type="str" value="ols_frame" /> <!-- OLS PointCloud data are published with frame id "ols_frame" -->
<param name="subscribe_queue_size" type="int" value="1" /> <!-- buffer size for ros messages -->
</node>
</launch>

8
Devices/Packages/sick_line_guidance/launch/sick_line_guidance_can2ros_node.launch Executable file
View File

@@ -0,0 +1,8 @@
<launch>
<!-- sick_line_guidance: run can2ros_node support tool, converts can messages to ros messages -->
<node name="sick_line_guidance_can2ros_node" pkg="sick_line_guidance" type="sick_line_guidance_can2ros_node" />
<param name="can_device" type="str" value="can0" /> <!-- name of can net device (socketcan interface) -->
<param name="ros_topic" type="str" value="can0" /> <!-- topic for ros messages (output), message type can_msgs::Frame -->
</launch>

45
Devices/Packages/sick_line_guidance/launch/sick_line_guidance_ols20_twin.launch Executable file
View File

@@ -0,0 +1,45 @@
<launch>
<!-- sick_line_guidance: configuration for two OLS20 devices (can node ids A and B) -->
<arg name="yaml" default="sick_line_guidance_ols20_twin.yaml"/>
<rosparam command="load" file="$(find sick_line_guidance)/$(arg yaml)" /> <!-- Global CAN configuration two OLS20 devices (can node ids A and B) incl. link to eds-file -->
<!-- sick_line_guidance: run canopen_chain_node -->
<!-- to run sick_line_guidance_can_chain_node in gdb: add parameter launch-prefix="gdb -ex run - -args": -->
<!-- node name="sick_line_guidance_can_chain_node" pkg="sick_line_guidance" type="sick_line_guidance_can_chain_node" launch-prefix="gdb -ex run - -args" output="screen" -->
<node name="sick_line_guidance_can_chain_node" pkg="sick_line_guidance" type="sick_line_guidance_can_chain_node" output="screen" >
<rosparam command="load" file="$(find sick_line_guidance)/$(arg yaml)" /> <!-- Private CAN configuration for canopen_chain_node by ols or mls yaml-file -->
<param name="diagnostic_topic" type="str" value="diagnostics" /> <!-- ROS topic for diagnostic messages -->
</node>
<!-- sick_line_guidance: run sick_line_guidance_node, which implements the ROS driver for OLS and MLS -->
<node name="sick_line_guidance_node" pkg="sick_line_guidance" type="sick_line_guidance_node" output="screen" >
<param name="diagnostic_topic" type="str" value="diagnostics" /> <!-- ROS topic for diagnostic messages -->
<param name="can_connect_init_at_startup" type="bool" value="true" /> <!-- Additional CAN configuration: if true, canopen services are initialized at startup -->
<param name="initial_sensor_state" type="int" value="7" /> <!-- initial sensor states (f.e. 0x07 for 3 detected lines, or 8 to indicate sensor error) -->
<param name="subscribe_queue_size" type="int" value="16" /> <!-- buffer size for ros messages -->
<param name="max_sdo_rate" type="double" value="100.0" /> <!-- max. sdo query and publish rate -->
<param name="max_num_retries_after_sdo_error" type="int" value="2" /> <!-- After SDO error, the SDO query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted. -->
</node>
<!-- sick_line_guidance: cloud_publisher_node to convert OLS_Measurement messages of first OLS20 device (can node id A) to PointCloud2 (topic "cloudA", frame id "olsA_frame") -->
<node name="sick_line_guidance_cloudA_publisher" pkg="sick_line_guidance" type="sick_line_guidance_cloud_publisher" output="screen" >
<param name="mls_topic_publish" type="str" value="mlsA" /> <!-- MLS_Measurement data are published in topic "/mlsA" -->
<param name="ols_topic_publish" type="str" value="olsA" /> <!-- OLS_Measurement data are published in topic "/olsA" -->
<param name="cloud_topic_publish" type="str" value="cloudA" /> <!-- sensor_msgs::PointCloud messages are published in topic "/cloudA" -->
<param name="mls_cloud_frame_id" type="str" value="mlsA_frame" /> <!-- MLS PointCloud data are published with frame id "mlsA_frame" -->
<param name="ols_cloud_frame_id" type="str" value="olsA_frame" /> <!-- OLS PointCloud data are published with frame id "olsA_frame" -->
<param name="subscribe_queue_size" type="int" value="1" /> <!-- buffer size for ros messages -->
</node>
<!-- sick_line_guidance: cloud_publisher_node to convert OLS_Measurement messages of second OLS20 device (can node id B) to PointCloud2 (topic "cloudB", frame id "olsB_frame") -->
<node name="sick_line_guidance_cloudB_publisher" pkg="sick_line_guidance" type="sick_line_guidance_cloud_publisher" output="screen" >
<param name="mls_topic_publish" type="str" value="mlsB" /> <!-- MLS_Measurement data are published in topic "/mlsB" -->
<param name="ols_topic_publish" type="str" value="olsB" /> <!-- OLS_Measurement data are published in topic "/olsB" -->
<param name="cloud_topic_publish" type="str" value="cloudB" /> <!-- sensor_msgs::PointCloud messages are published in topic "/cloudB" -->
<param name="mls_cloud_frame_id" type="str" value="mlsB_frame" /> <!-- MLS PointCloud data are published with frame id "mlsB_frame" -->
<param name="ols_cloud_frame_id" type="str" value="olsB_frame" /> <!-- OLS PointCloud data are published with frame id "olsB_frame" -->
<param name="subscribe_queue_size" type="int" value="1" /> <!-- buffer size for ros messages -->
</node>
</launch>

9
Devices/Packages/sick_line_guidance/launch/sick_line_guidance_ros2can_node.launch Executable file
View File

@@ -0,0 +1,9 @@
<launch>
<!-- sick_line_guidance: run ros2can_node support tool, converts ros messages to can messages -->
<node name="sick_line_guidance_ros2can_node" pkg="sick_line_guidance" type="sick_line_guidance_ros2can_node" />
<param name="can_device" type="str" value="can0" /> <!-- name of can net device (socketcan interface) -->
<param name="ros_topic" type="str" value="ros2can0" /> <!-- topic for ros messages (input), message type can_msgs::Frame -->
<param name="subscribe_queue_size" type="int" value="32" /> <!-- buffer size for ros messages -->
</launch>

483
Devices/Packages/sick_line_guidance/mls/SICK-MLS-MLS.eds Executable file
View File

@@ -0,0 +1,483 @@
[FileInfo]
CreatedBy=Tim Vogt
ModifiedBy=Tim Vogt
Description=Magnetic Lane Sensor
CreationTime=11:15AM
CreationDate=10-10-2017
ModificationTime=11:15AM
ModificationDate=10-10-2017
FileName=MLS.eds
FileVersion=0x01
FileRevision=0x01
EDSVersion=4.0
[DeviceInfo]
VendorName=SICK AG
VendorNumber=0x01000056
ProductName=Magnetic Lane Sensor
BaudRate_10=0
BaudRate_20=0
BaudRate_50=0
BaudRate_125=1
BaudRate_250=0
BaudRate_500=0
BaudRate_800=0
BaudRate_1000=0
SimpleBootUpMaster=0
SimpleBootUpSlave=1
Granularity=0
DynamicChannelsSupported=0
CompactPDO=0
GroupMessaging=0
NrOfRXPDO=0
NrOfTXPDO=1
LSS_Supported=1
[DummyUsage]
Dummy0001=0
Dummy0002=1
Dummy0003=1
Dummy0004=1
Dummy0005=1
Dummy0006=1
Dummy0007=1
[Comments]
Lines=0
[MandatoryObjects]
SupportedObjects=3
1=0x1000
2=0x1001
3=0x1018
[1000]
ParameterName=Device Type
ObjectType=0x7
DataType=0x0007
AccessType=ro
DefaultValue=0x00
PDOMapping=0
[1001]
ParameterName=Error Register
ObjectType=0x7
DataType=0x0005
AccessType=ro
PDOMapping=0
[1018]
ParameterName=Identity Object
ObjectType=0x9
SubNumber=5
[1018sub0]
ParameterName=Number of entries
ObjectType=0x7
DataType=0x0005
AccessType=ro
DefaultValue=4
PDOMapping=0
[1018sub1]
ParameterName=Vendor Id
ObjectType=0x7
DataType=0x0007
AccessType=ro
DefaultValue=0x01000056
PDOMapping=0
[1018sub2]
ParameterName=Product Code
ObjectType=0x7
DataType=0x0007
AccessType=ro
DefaultValue=0x00001100
PDOMapping=0
[1018sub3]
ParameterName=Revision number
ObjectType=0x7
DataType=0x0007
AccessType=ro
DefaultValue=0x00000002
PDOMapping=0
[1018sub4]
ParameterName=Serial number
ObjectType=0x7
DataType=0x0007
AccessType=ro
PDOMapping=0
[OptionalObjects]
SupportedObjects=10
1=0x1008
2=0x1009
3=0x100A
4=0x100C
5=0x100D
6=0x1017
7=0x1200
8=0x1800
9=0x1A00
10=0x1F80
[1008]
ParameterName=Manufacturer Device Name
ObjectType=0x7
DataType=0x0009
AccessType=const
PDOMapping=0
[1009]
ParameterName=Manufacturer Hardware Version
ObjectType=0x7
DataType=0x0009
AccessType=const
PDOMapping=0
[100A]
ParameterName=Manufacturer Software Version
ObjectType=0x7
DataType=0x0009
AccessType=const
PDOMapping=0
[100C]
ParameterName=Guard Time
ObjectType=0x7
DataType=0x0006
AccessType=rw
DefaultValue=0
PDOMapping=0
[100D]
ParameterName=Life Time Factor
ObjectType=0x7
DataType=0x0005
AccessType=rw
DefaultValue=0x00
PDOMapping=0
[1017]
ParameterName=Producer Heartbeat Time
ObjectType=0x7
DataType=0x0006
AccessType=rw
DefaultValue=0
PDOMapping=0
[1200]
ParameterName=Server SDO Parameter 0
ObjectType=0x9
SubNumber=3
[1200sub0]
ParameterName=Number of entries
ObjectType=0x7
DataType=0x0005
AccessType=ro
DefaultValue=2
PDOMapping=0
LowLimit=0x02
HighLimit=0x02
[1200sub1]
ParameterName=COB ID Client to Server
ObjectType=0x7
DataType=0x0007
AccessType=ro
DefaultValue=$NODEID+0x600
PDOMapping=0
[1200sub2]
ParameterName=COB ID Server to Client
ObjectType=0x7
DataType=0x0007
AccessType=ro
DefaultValue=$NODEID+0x580
PDOMapping=0
[1800]
ParameterName=Transmit PDO Communication Parameter 0
ObjectType=0x9
SubNumber=5
[1800sub0]
ParameterName=Number of entries
ObjectType=0x7
DataType=0x0005
AccessType=ro
DefaultValue=5
PDOMapping=0
[1800sub1]
ParameterName=COB ID
ObjectType=0x7
DataType=0x0007
AccessType=rw
DefaultValue=$NODEID+0x180
PDOMapping=0
LowLimit=0x00000181
HighLimit=0xFFFFFFFF
[1800sub2]
ParameterName=Transmission Type
ObjectType=0x7
DataType=0x0005
AccessType=ro
DefaultValue=0xFF
PDOMapping=0
[1800sub4]
ParameterName=Compatibility Entry
ObjectType=0x7
DataType=0x0005
AccessType=rw
PDOMapping=0
[1800sub5]
ParameterName=Event Timer
ObjectType=0x7
DataType=0x0006
AccessType=rw
PDOMapping=0
[1A00]
ParameterName=Transmit PDO Mapping Parameter 0
ObjectType=0x9
SubNumber=6
[1A00sub0]
ParameterName=Number of entries
ObjectType=0x7
DataType=0x0005
AccessType=const
DefaultValue=5
PDOMapping=0
LowLimit=0
HighLimit=5
[1A00sub1]
ParameterName=PDO Mapping Entry
ObjectType=0x7
DataType=0x0007
AccessType=const
DefaultValue=0x20210110
PDOMapping=0
[1A00sub2]
ParameterName=PDO Mapping Entry_2
ObjectType=0x7
DataType=0x0007
AccessType=const
DefaultValue=0x20210210
PDOMapping=0
[1A00sub3]
ParameterName=PDO Mapping Entry_3
ObjectType=0x7
DataType=0x0007
AccessType=const
DefaultValue=0x20210310
PDOMapping=0
[1A00sub4]
ParameterName=PDO Mapping Entry_4
ObjectType=0x7
DataType=0x0007
AccessType=const
DefaultValue=0x20210408
PDOMapping=0
[1A00sub5]
ParameterName=PDO Mapping Entry_5
ObjectType=0x7
DataType=0x0007
AccessType=const
DefaultValue=0x20220008
PDOMapping=0
[1F80]
ParameterName=NMTStartup
ObjectType=0x7
DataType=0x0007
AccessType=rw
PDOMapping=0
[ManufacturerObjects]
SupportedObjects=14
1=0x2019
2=0x2020
3=0x2021
4=0x2022
5=0x2023
6=0x2024
7=0x2025
8=0x2026
9=0x2027
10=0x2028
11=0x2029
12=0x202A
13=0x202B
14=0x202C
[2019]
ParameterName=Order Number
ObjectType=0x7
DataType=0x0007
AccessType=ro
PDOMapping=0
[2020]
ParameterName=Passwort
ObjectType=0x7
DataType=0x0009
AccessType=wo
PDOMapping=0
[2021]
ParameterName=SSPs
ObjectType=0x9
SubNumber=5
[2021sub0]
ParameterName=Number of SSPs
ObjectType=0x7
DataType=0x0005
AccessType=ro
DefaultValue=0x04
PDOMapping=0
[2021sub1]
ParameterName=SSP1
ObjectType=0x7
DataType=0x0003
AccessType=ro
PDOMapping=1
[2021sub2]
ParameterName=SSP2
ObjectType=0x7
DataType=0x0003
AccessType=ro
PDOMapping=1
[2021sub3]
ParameterName=SSP3
ObjectType=0x7
DataType=0x0003
AccessType=ro
PDOMapping=1
[2021sub4]
ParameterName=#SSP
ObjectType=0x7
DataType=0x0005
AccessType=ro
PDOMapping=1
[2022]
ParameterName=Status
ObjectType=0x7
DataType=0x0005
AccessType=ro
PDOMapping=1
[2023]
ParameterName=Track Level
ObjectType=0x7
DataType=0x0006
AccessType=ro
PDOMapping=0
[2024]
ParameterName=Field Level
ObjectType=0x7
DataType=0x0006
AccessType=ro
PDOMapping=0
[2025]
ParameterName=Min Level
ObjectType=0x7
DataType=0x0006
AccessType=rw
PDOMapping=0
[2026]
ParameterName=Offset
ObjectType=0x7
DataType=0x0003
AccessType=rw
PDOMapping=0
[2027]
ParameterName=Sensor flipped
ObjectType=0x7
DataType=0x0001
AccessType=rw
PDOMapping=0
[2028]
ParameterName=Markers
ObjectType=0x9
SubNumber=4
[2028sub0]
ParameterName=NrOfObjects
ObjectType=0x7
DataType=0x0005
AccessType=ro
DefaultValue=3
PDOMapping=0
[2028sub1]
ParameterName=Use Markers
ObjectType=0x7
DataType=0x0001
AccessType=rw
PDOMapping=0
[2028sub2]
ParameterName=Marker Style
ObjectType=0x7
DataType=0x0005
AccessType=rw
PDOMapping=0
[2028sub3]
ParameterName=Fail Safe Mode
ObjectType=0x7
DataType=0x0001
AccessType=rw
PDOMapping=0
[2029]
ParameterName=Lock Teach
ObjectType=0x7
DataType=0x0001
AccessType=rw
PDOMapping=0
[202A]
ParameterName=Set Param to Default
ObjectType=0x7
DataType=0x0001
AccessType=wo
PDOMapping=0
[202B]
ParameterName=Trigger User Offset Calibration
ObjectType=0x7
DataType=0x0001
AccessType=wo
PDOMapping=0
[202C]
ParameterName=Trigger Zero Position Teach
ObjectType=0x7
DataType=0x0001
AccessType=wo
PDOMapping=0

483
Devices/Packages/sick_line_guidance/mls/SICK-MLS.eds Executable file
View File

@@ -0,0 +1,483 @@
[FileInfo]
CreatedBy=Tim Vogt
ModifiedBy=Tim Vogt
Description=Magnetic Lane Sensor
CreationTime=11:15AM
CreationDate=10-10-2017
ModificationTime=11:15AM
ModificationDate=10-10-2017
FileName=MLS.eds
FileVersion=0x01
FileRevision=0x01
EDSVersion=4.0
[DeviceInfo]
VendorName=SICK AG
VendorNumber=0x01000056
ProductName=Magnetic Lane Sensor
BaudRate_10=0
BaudRate_20=0
BaudRate_50=0
BaudRate_125=1
BaudRate_250=0
BaudRate_500=0
BaudRate_800=0
BaudRate_1000=0
SimpleBootUpMaster=0
SimpleBootUpSlave=1
Granularity=0
DynamicChannelsSupported=0
CompactPDO=0
GroupMessaging=0
NrOfRXPDO=0
NrOfTXPDO=1
LSS_Supported=1
[DummyUsage]
Dummy0001=0
Dummy0002=1
Dummy0003=1
Dummy0004=1
Dummy0005=1
Dummy0006=1
Dummy0007=1
[Comments]
Lines=0
[MandatoryObjects]
SupportedObjects=3
1=0x1000
2=0x1001
3=0x1018
[1000]
ParameterName=Device Type
ObjectType=0x7
DataType=0x0007
AccessType=ro
DefaultValue=0x00
PDOMapping=0
[1001]
ParameterName=Error Register
ObjectType=0x7
DataType=0x0005
AccessType=ro
PDOMapping=0
[1018]
ParameterName=Identity Object
ObjectType=0x9
SubNumber=5
[1018sub0]
ParameterName=Number of entries
ObjectType=0x7
DataType=0x0005
AccessType=ro
DefaultValue=4
PDOMapping=0
[1018sub1]
ParameterName=Vendor Id
ObjectType=0x7
DataType=0x0007
AccessType=ro
DefaultValue=0x01000056
PDOMapping=0
[1018sub2]
ParameterName=Product Code
ObjectType=0x7
DataType=0x0007
AccessType=ro
DefaultValue=0x00001100
PDOMapping=0
[1018sub3]
ParameterName=Revision number
ObjectType=0x7
DataType=0x0007
AccessType=ro
DefaultValue=0x00000002
PDOMapping=0
[1018sub4]
ParameterName=Serial number
ObjectType=0x7
DataType=0x0007
AccessType=ro
PDOMapping=0
[OptionalObjects]
SupportedObjects=10
1=0x1008
2=0x1009
3=0x100A
4=0x100C
5=0x100D
6=0x1017
7=0x1200
8=0x1800
9=0x1A00
10=0x1F80
[1008]
ParameterName=Manufacturer Device Name
ObjectType=0x7
DataType=0x0009
AccessType=const
PDOMapping=0
[1009]
ParameterName=Manufacturer Hardware Version
ObjectType=0x7
DataType=0x0009
AccessType=const
PDOMapping=0
[100A]
ParameterName=Manufacturer Software Version
ObjectType=0x7
DataType=0x0009
AccessType=const
PDOMapping=0
[100C]
ParameterName=Guard Time
ObjectType=0x7
DataType=0x0006
AccessType=rw
DefaultValue=0
PDOMapping=0
[100D]
ParameterName=Life Time Factor
ObjectType=0x7
DataType=0x0005
AccessType=rw
DefaultValue=0x00
PDOMapping=0
[1017]
ParameterName=Producer Heartbeat Time
ObjectType=0x7
DataType=0x0006
AccessType=rw
DefaultValue=0
PDOMapping=0
[1200]
ParameterName=Server SDO Parameter 0
ObjectType=0x9
SubNumber=3
[1200sub0]
ParameterName=Number of entries
ObjectType=0x7
DataType=0x0005
AccessType=ro
DefaultValue=2
PDOMapping=0
LowLimit=0x02
HighLimit=0x02
[1200sub1]
ParameterName=COB ID Client to Server
ObjectType=0x7
DataType=0x0007
AccessType=ro
DefaultValue=$NODEID+0x600
PDOMapping=0
[1200sub2]
ParameterName=COB ID Server to Client
ObjectType=0x7
DataType=0x0007
AccessType=ro
DefaultValue=$NODEID+0x580
PDOMapping=0
[1800]
ParameterName=Transmit PDO Communication Parameter 0
ObjectType=0x9
SubNumber=5
[1800sub0]
ParameterName=Number of entries
ObjectType=0x7
DataType=0x0005
AccessType=ro
DefaultValue=5
PDOMapping=0
[1800sub1]
ParameterName=COB ID
ObjectType=0x7
DataType=0x0007
AccessType=rw
DefaultValue=$NODEID+0x180
PDOMapping=0
LowLimit=0x00000181
HighLimit=0xFFFFFFFF
[1800sub2]
ParameterName=Transmission Type
ObjectType=0x7
DataType=0x0005
AccessType=ro
DefaultValue=0xFF
PDOMapping=0
[1800sub4]
ParameterName=Compatibility Entry
ObjectType=0x7
DataType=0x0005
AccessType=rw
PDOMapping=0
[1800sub5]
ParameterName=Event Timer
ObjectType=0x7
DataType=0x0006
AccessType=rw
PDOMapping=0
[1A00]
ParameterName=Transmit PDO Mapping Parameter 0
ObjectType=0x9
SubNumber=6
[1A00sub0]
ParameterName=Number of entries
ObjectType=0x7
DataType=0x0005
AccessType=const
DefaultValue=5
PDOMapping=0
LowLimit=0
HighLimit=5
[1A00sub1]
ParameterName=PDO Mapping Entry
ObjectType=0x7
DataType=0x0007
AccessType=const
DefaultValue=0x20210110
PDOMapping=0
[1A00sub2]
ParameterName=PDO Mapping Entry_2
ObjectType=0x7
DataType=0x0007
AccessType=const
DefaultValue=0x20210210
PDOMapping=0
[1A00sub3]
ParameterName=PDO Mapping Entry_3
ObjectType=0x7
DataType=0x0007
AccessType=const
DefaultValue=0x20210310
PDOMapping=0
[1A00sub4]
ParameterName=PDO Mapping Entry_4
ObjectType=0x7
DataType=0x0007
AccessType=const
DefaultValue=0x20210408
PDOMapping=0
[1A00sub5]
ParameterName=PDO Mapping Entry_5
ObjectType=0x7
DataType=0x0007
AccessType=const
DefaultValue=0x20220008
PDOMapping=0
[1F80]
ParameterName=NMTStartup
ObjectType=0x7
DataType=0x0007
AccessType=rw
PDOMapping=0
[ManufacturerObjects]
SupportedObjects=14
1=0x2019
2=0x2020
3=0x2021
4=0x2022
5=0x2023
6=0x2024
7=0x2025
8=0x2026
9=0x2027
10=0x2028
11=0x2029
12=0x202A
13=0x202B
14=0x202C
[2019]
ParameterName=Order Number
ObjectType=0x7
DataType=0x0007
AccessType=ro
PDOMapping=0
[2020]
ParameterName=Passwort
ObjectType=0x7
DataType=0x0009
AccessType=wo
PDOMapping=0
[2021]
ParameterName=SSPs
ObjectType=0x9
SubNumber=5
[2021sub0]
ParameterName=Number of SSPs
ObjectType=0x7
DataType=0x0005
AccessType=ro
DefaultValue=0x04
PDOMapping=0
[2021sub1]
ParameterName=SSP1
ObjectType=0x7
DataType=0x0003
AccessType=ro
PDOMapping=1
[2021sub2]
ParameterName=SSP2
ObjectType=0x7
DataType=0x0003
AccessType=ro
PDOMapping=1
[2021sub3]
ParameterName=SSP3
ObjectType=0x7
DataType=0x0003
AccessType=ro
PDOMapping=1
[2021sub4]
ParameterName=#SSP
ObjectType=0x7
DataType=0x0005
AccessType=ro
PDOMapping=1
[2022]
ParameterName=Status
ObjectType=0x7
DataType=0x0005
AccessType=ro
PDOMapping=1
[2023]
ParameterName=Track Level
ObjectType=0x7
DataType=0x0006
AccessType=ro
PDOMapping=0
[2024]
ParameterName=Field Level
ObjectType=0x7
DataType=0x0006
AccessType=ro
PDOMapping=0
[2025]
ParameterName=Min Level
ObjectType=0x7
DataType=0x0006
AccessType=rw
PDOMapping=0
[2026]
ParameterName=Offset
ObjectType=0x7
DataType=0x0003
AccessType=rw
PDOMapping=0
[2027]
ParameterName=Sensor flipped
ObjectType=0x7
DataType=0x0005
AccessType=rw
PDOMapping=0
[2028]
ParameterName=Markers
ObjectType=0x9
SubNumber=4
[2028sub0]
ParameterName=NrOfObjects
ObjectType=0x7
DataType=0x0005
AccessType=ro
DefaultValue=3
PDOMapping=0
[2028sub1]
ParameterName=Use Markers
ObjectType=0x7
DataType=0x0005
AccessType=rw
PDOMapping=0
[2028sub2]
ParameterName=Marker Style
ObjectType=0x7
DataType=0x0005
AccessType=rw
PDOMapping=0
[2028sub3]
ParameterName=Fail Safe Mode
ObjectType=0x7
DataType=0x0005
AccessType=rw
PDOMapping=0
[2029]
ParameterName=Lock Teach
ObjectType=0x7
DataType=0x0005
AccessType=rw
PDOMapping=0
[202A]
ParameterName=Set Param to Default
ObjectType=0x7
DataType=0x0005
AccessType=wo
PDOMapping=0
[202B]
ParameterName=Trigger User Offset Calibration
ObjectType=0x7
DataType=0x0005
AccessType=wo
PDOMapping=0
[202C]
ParameterName=Trigger Zero Position Teach
ObjectType=0x7
DataType=0x0005
AccessType=wo
PDOMapping=0

35
Devices/Packages/sick_line_guidance/mls/sick_line_guidance_mls.yaml Executable file
View File

@@ -0,0 +1,35 @@
bus:
device: can0
driver_plugin: can::SocketCANInterface
master_allocator: canopen::SimpleMaster::Allocator
sync:
interval_ms: 10
overflow: 0
#
# heartbeat: # simple heartbeat producer (optional, not supported by OLS or MLS, do not enable heartbeats)
# rate: 100 # heartbeat rate (1/rate in seconds)
# msg: "77f#05" # message to send, cansend format: heartbeat of node 127 with status 5=Started
nodes:
node1:
id: 0x01 # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
eds_pkg: sick_line_guidance # package name for relative path
eds_file: SICK-MLS.eds # path to EDS/DCF file
publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2022!"]
# MLS: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDO1 = 0x2021sub1 to 0x2021sub4 and 0x2022
# sick_line_guidance configuration of this node:
sick_device_family: "MLS" # can devices of OLS10, OLS20 or MLS family currently supported
sick_topic: "mls" # MLS_Measurement messages are published in topic "/mls"
sick_frame_id: "mls_measurement_frame" # MLS_Measurement messages are published frame_id "mls_measurement_frame"
# device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue"
# example: "2027": "0x01" # Object 2027 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01
# dcf_overlay:
# "2028sub1": "0x01" # UseMarkers (0 = disable marker detection, 1 = enable marker detection), UINT8, DataType=0x0005, defaultvalue=0
# "2028sub2": "0x02" # MarkerStyle (0 = disable marker detection, 1 = standard mode, 2 = extended mode), UINT8, DataType=0x0005, defaultvalue=0
# "2028sub3": "0x01" # FailSafeMode (0 = disabled, 1 = enabled), UINT8, DataType=0x0005, defaultvalue=0
# "2025": "1000" # Min.Level, UINT16, DataType=0x0006, defaultvalue=600
# "2026": "1" # Offset [mm] Nullpunkt, INT16, DataType=0x0003, defaultvalue=0
# "2027": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0
# "2029": "0x01" # LockTeach, UINT8, DataType=0x0005, defaultvalue=0
#

21
Devices/Packages/sick_line_guidance/msg/MLS_Measurement.msg Executable file
View File

@@ -0,0 +1,21 @@
# MLS_Measurement.msg defines a single measurement of a SICK MLS.
# See operation instructions for details (www.sick.com/mls).
#
# Header with sequence id, timestamp of the measurement and frame id
Header header
# Array of measured positions for up to 3 lines.
# Each position is the distance to the center of a line (line center point) in meter.
# More than one line is detected in case of junctions.
float32[] position # distance to the line center point [m]
# LCP-flags (signs and line assignment)
uint8 lcp # flags (signs and assignment, see chap. 8 of operation instructions)
# Detection status
uint8 status # status (see chap. 8 of operation instructions)
# Error register
uint8 error # error register (0x1001, value 0 = okay, see chap. 8 of operation instructions)

39
Devices/Packages/sick_line_guidance/msg/OLS_Measurement.msg Executable file
View File

@@ -0,0 +1,39 @@
# OLS_Measurement.msg defines a single measurement of a SICK OLS.
# See operation instructions for details (www.sick.com/ols).
# Header with sequence id, timestamp of the measurement and frame id
Header header
# Array of measured positions for up to 3 lines.
# Each position is the distance to the center of a line (line center point) in meter.
# More than one line is detected in case of junctions.
float32[] position # distance to the line center point [m]
# Array of up to 3 line widths.
# For each detected line, its width is measured.
float32[] width # width of line [m]
# Detection status
uint8 status # status (see chap. 8 of operation instructions)
# Barcode data
uint8 barcode # barcode data (0 indicates no barcode)
# Device status
uint16 dev_status # device status (0x2018, value 0 = okay, see chap. 8 of operation instructions)
# Extended code
uint32 extended_code # extended code (0x2021sub9, see chap. 8 of operation instructions)
# Error register
uint8 error # error register (0x1001, value 0 = okay, see chap. 8 of operation instructions)
# barcode center point [m] (OLS20 only)
float32 barcode_center_point # OLS20 only (0x2021subA), OLS10: always 0
# quality of lines
uint8 quality_of_lines # OLS20 only (0x2021subB), OLS10: always 0
# Intensity of lines 1 to 3
uint8[] intensity_of_lines # OLS20 only (0x2023sub1 to 0x2023sub3), OLS10: always 0

602
Devices/Packages/sick_line_guidance/ols/SICK_OLS10_CO.eds Executable file
View File

@@ -0,0 +1,602 @@
[FileInfo]
CreatedBy=Tim Vogt
ModifiedBy=Alexander Bierbaum
Description=Optical Line Guidance Sensor
CreationTime=09:47AM
CreationDate=12-15-2017
ModificationTime=02:25PM
ModificationDate=03-13-2019
FileName=SICK_OLS10_CO.eds
FileVersion=0x02
FileRevision=0x0A
EDSVersion=4.0
[DeviceInfo]
VendorName=SICK AG
VendorNumber=0x01000056
ProductName=Optical Line Guidance Sensor
ProductNumber=0x00001003
RevisionNumber=0x00010001
OrderCode=-
BaudRate_10=1
BaudRate_20=1
BaudRate_50=1
BaudRate_125=1
BaudRate_250=1
BaudRate_500=1
BaudRate_800=1
BaudRate_1000=1
SimpleBootUpMaster=0
SimpleBootUpSlave=1
Granularity=0
DynamicChannelsSupported=0
CompactPDO=0
GroupMessaging=0
NrOfRXPDO=0
NrOfTXPDO=2
LSS_Supported=1
[DummyUsage]
Dummy0001=0
Dummy0002=1
Dummy0003=1
Dummy0004=1
Dummy0005=1
Dummy0006=1
Dummy0007=1
[Comments]
Lines=0
[MandatoryObjects]
SupportedObjects=3
1=0x1000
2=0x1001
3=0x1018
[1000]
ParameterName=Device Type
ObjectType=0x7
DataType=0x0007
AccessType=ro
DefaultValue=0x00
PDOMapping=0
[1001]
ParameterName=Error Register
ObjectType=0x7
DataType=0x0005
AccessType=ro
DefaultValue=0
PDOMapping=0
[1018]
ParameterName=Identity Object
ObjectType=0x9
SubNumber=5
[1018sub0]
ParameterName=Number of entries
ObjectType=0x7
DataType=0x0005
AccessType=ro
DefaultValue=4
PDOMapping=0
LowLimit=1
HighLimit=4
[1018sub1]
ParameterName=Vendor Id
ObjectType=0x7
DataType=0x0007
AccessType=ro
DefaultValue=0x01000056
PDOMapping=0
[1018sub2]
ParameterName=Product Code
ObjectType=0x7
DataType=0x0007
AccessType=ro
DefaultValue=0x00001003
PDOMapping=0
[1018sub3]
ParameterName=Revision number
ObjectType=0x7
DataType=0x0007
AccessType=ro
DefaultValue=0x00010001
PDOMapping=0
[1018sub4]
ParameterName=Serial number
ObjectType=0x7
DataType=0x0007
AccessType=ro
PDOMapping=0
[OptionalObjects]
SupportedObjects=13
1=0x1005
2=0x1008
3=0x1009
4=0x100A
5=0x100C
6=0x100D
7=0x1017
8=0x1200
9=0x1800
10=0x1801
11=0x1A00
12=0x1A01
13=0x1F80
[1005]
ParameterName=COB ID SYNC
ObjectType=0x7
DataType=0x0007
AccessType=rw
DefaultValue=0x00000080
PDOMapping=0
LowLimit=0x00000080
[1008]
ParameterName=Manufacturer Device Name
ObjectType=0x7
DataType=0x0009
AccessType=const
DefaultValue=OLS10-BP112311
PDOMapping=0
[1009]
ParameterName=Manufacturer Hardware Version
ObjectType=0x7
DataType=0x0009
AccessType=const
DefaultValue=1.0
PDOMapping=0
[100A]
ParameterName=Manufacturer Software Version
ObjectType=0x7
DataType=0x0009
AccessType=const
DefaultValue=Firmwareversion
PDOMapping=0
[100C]
ParameterName=Guard Time
ObjectType=0x7
DataType=0x0006
AccessType=rw
DefaultValue=0
PDOMapping=0
[100D]
ParameterName=Life Time Factor
ObjectType=0x7
DataType=0x0005
AccessType=rw
DefaultValue=0x00
PDOMapping=0
[1017]
ParameterName=Producer Heartbeat Time
ObjectType=0x7
DataType=0x0006
AccessType=rw
DefaultValue=0
PDOMapping=0
[1200]
ParameterName=Server SDO Parameter 0
ObjectType=0x9
SubNumber=3
[1200sub0]
ParameterName=Number of entries
ObjectType=0x7
DataType=0x0005
AccessType=ro
DefaultValue=2
PDOMapping=0
LowLimit=0x02
HighLimit=0x02
[1200sub1]
ParameterName=COB ID Client to Server
ObjectType=0x7
DataType=0x0007
AccessType=ro
DefaultValue=$NODEID+0x600
PDOMapping=0
[1200sub2]
ParameterName=COB ID Server to Client
ObjectType=0x7
DataType=0x0007
AccessType=ro
DefaultValue=$NODEID+0x580
PDOMapping=0
[1800]
ParameterName=Transmit PDO Communication Parameter 0
ObjectType=0x9
SubNumber=5
[1800sub0]
ParameterName=Number of entries
ObjectType=0x7
DataType=0x0005
AccessType=ro
DefaultValue=5
PDOMapping=0
[1800sub1]
ParameterName=COB ID
ObjectType=0x7
DataType=0x0007
AccessType=rw
DefaultValue=$NODEID+0x180
PDOMapping=0
[1800sub2]
ParameterName=Transmission Type
ObjectType=0x7
DataType=0x0005
AccessType=rw
DefaultValue=0xFF
PDOMapping=0
[1800sub4]
ParameterName=Compatibility Entry
ObjectType=0x7
DataType=0x0005
AccessType=rw
DefaultValue=0x00
PDOMapping=0
[1800sub5]
ParameterName=Event Timer
ObjectType=0x7
DataType=0x0006
AccessType=rw
DefaultValue=20
PDOMapping=0
[1801]
ParameterName=Transmit PDO Communication Parameter 1
ObjectType=0x9
SubNumber=5
[1801sub0]
ParameterName=Number of entries
ObjectType=0x7
DataType=0x0005
AccessType=ro
DefaultValue=5
PDOMapping=0
[1801sub1]
ParameterName=COB ID
ObjectType=0x7
DataType=0x0007
AccessType=rw
DefaultValue=$NODEID+0x280
PDOMapping=0
[1801sub2]
ParameterName=Transmission Type
ObjectType=0x7
DataType=0x0005
AccessType=rw
DefaultValue=0xFF
PDOMapping=0
[1801sub4]
ParameterName=Compatibility Entry
ObjectType=0x7
DataType=0x0005
AccessType=rw
DefaultValue=0x00
PDOMapping=0
[1801sub5]
ParameterName=Event Timer
ObjectType=0x7
DataType=0x0006
AccessType=rw
DefaultValue=0
PDOMapping=0
[1A00]
ParameterName=Transmit PDO Mapping Parameter 0
ObjectType=0x9
SubNumber=6
[1A00sub0]
ParameterName=Number of entries
ObjectType=0x7
DataType=0x0005
AccessType=const
DefaultValue=5
PDOMapping=0
LowLimit=0
HighLimit=5
[1A00sub1]
ParameterName=PDO Mapping Entry
ObjectType=0x7
DataType=0x0007
AccessType=const
DefaultValue=0x20210110
PDOMapping=0
[1A00sub2]
ParameterName=PDO Mapping Entry_2
ObjectType=0x7
DataType=0x0007
AccessType=const
DefaultValue=0x20210210
PDOMapping=0
[1A00sub3]
ParameterName=PDO Mapping Entry_3
ObjectType=0x7
DataType=0x0007
AccessType=const
DefaultValue=0x20210310
PDOMapping=0
[1A00sub4]
ParameterName=PDO Mapping Entry_4
ObjectType=0x7
DataType=0x0007
AccessType=const
DefaultValue=0x20210408
PDOMapping=0
[1A00sub5]
ParameterName=PDO Mapping Entry_5
ObjectType=0x7
DataType=0x0007
AccessType=const
DefaultValue=0x20210808
PDOMapping=0
[1A01]
ParameterName=Transmit PDO Mapping Parameter 1
ObjectType=0x9
SubNumber=4
[1A01sub0]
ParameterName=Number of entries
ObjectType=0x7
DataType=0x0005
AccessType=const
DefaultValue=3
PDOMapping=0
LowLimit=0
HighLimit=3
[1A01sub1]
ParameterName=PDO Mapping Entry
ObjectType=0x7
DataType=0x0007
AccessType=const
DefaultValue=0x20210510
PDOMapping=0
[1A01sub2]
ParameterName=PDO Mapping Entry_2
ObjectType=0x7
DataType=0x0007
AccessType=const
DefaultValue=0x20210610
PDOMapping=0
[1A01sub3]
ParameterName=PDO Mapping Entry_3
ObjectType=0x7
DataType=0x0007
AccessType=const
DefaultValue=0x20210710
PDOMapping=0
[1F80]
ParameterName=NMTStartup
ObjectType=0x7
DataType=0x0007
AccessType=rw
PDOMapping=0
[ManufacturerObjects]
SupportedObjects=7
1=0x2001
2=0x2002
3=0x2003
4=0x2018
5=0x2019
6=0x2020
7=0x2021
[2001]
ParameterName=Mounting parameters
ObjectType=0x9
SubNumber=2
[2001sub0]
ParameterName=Number of mounting parameters
ObjectType=0x7
DataType=0x0005
AccessType=ro
DefaultValue=5
PDOMapping=0
[2001sub5]
ParameterName=Flipped upside down
ObjectType=0x7
DataType=0x0005
AccessType=rw
DefaultValue=0
PDOMapping=0
[2002]
ParameterName=Tape parameters
ObjectType=0x9
SubNumber=4
[2002sub0]
ParameterName=Number of tape parameters
ObjectType=0x7
DataType=0x0005
AccessType=ro
DefaultValue=3
PDOMapping=0
[2002sub1]
ParameterName=Typ. width [m]
ObjectType=0x7
DataType=0x0008
AccessType=rw
PDOMapping=0
[2002sub2]
ParameterName=Min. width [m]
ObjectType=0x7
DataType=0x0008
AccessType=rw
PDOMapping=0
[2002sub3]
ParameterName=Max. width [m]
ObjectType=0x7
DataType=0x0008
AccessType=rw
PDOMapping=0
[2003]
ParameterName=Advanced settings
ObjectType=0x9
SubNumber=3
[2003sub0]
ParameterName=Number of advanced settings
ObjectType=0x7
DataType=0x0005
AccessType=ro
DefaultValue=3
PDOMapping=0
[2003sub1]
ParameterName=Max. number of missing track readings
ObjectType=0x7
DataType=0x0006
AccessType=rw
DefaultValue=10
PDOMapping=0
[2003sub3]
ParameterName=Position smoothing filter coefficient (dt/tau)
ObjectType=0x7
DataType=0x0008
AccessType=rw
PDOMapping=0
[2018]
ParameterName=Device status
ObjectType=0x7
DataType=0x0006
AccessType=ro
PDOMapping=0
[2019]
ParameterName=Order number
ObjectType=0x7
DataType=0x0007
AccessType=ro
PDOMapping=0
[2020]
ParameterName=Login
ObjectType=0x7
DataType=0x000a
AccessType=wo
PDOMapping=0
[2021]
ParameterName=Result data (LCPs)
ObjectType=0x9
SubNumber=10
[2021sub0]
ParameterName=Number of result data
ObjectType=0x7
DataType=0x0005
AccessType=ro
DefaultValue=9
PDOMapping=0
[2021sub1]
ParameterName=LCP1
ObjectType=0x7
DataType=0x0003
AccessType=ro
PDOMapping=1
[2021sub2]
ParameterName=LCP2
ObjectType=0x7
DataType=0x0003
AccessType=ro
PDOMapping=1
[2021sub3]
ParameterName=LCP3
ObjectType=0x7
DataType=0x0003
AccessType=ro
PDOMapping=1
[2021sub4]
ParameterName=Status
ObjectType=0x7
DataType=0x0005
AccessType=ro
PDOMapping=1
[2021sub5]
ParameterName=Width line1
ObjectType=0x7
DataType=0x0003
AccessType=ro
PDOMapping=1
[2021sub6]
ParameterName=Width line2
ObjectType=0x7
DataType=0x0003
AccessType=ro
PDOMapping=1
[2021sub7]
ParameterName=Width line3
ObjectType=0x7
DataType=0x0003
AccessType=ro
PDOMapping=1
[2021sub8]
ParameterName=Code
ObjectType=0x7
DataType=0x0005
AccessType=ro
PDOMapping=1
[2021sub9]
ParameterName=Extended Code
ObjectType=0x7
DataType=0x0007
AccessType=ro
PDOMapping=0

797
Devices/Packages/sick_line_guidance/ols/SICK_OLS20.eds Executable file
View File

@@ -0,0 +1,797 @@
; This EDS file was created by the CANopen Design Tool 2.3.19.0.
; port GmbH Halle/Saale Germany, http://www.port.de, mailto:service@port.de
; DefaultValues added by Michael Lehning.
[FileInfo]
FileName=OLS20.eds
FileVersion=1
FileRevision=1
EDSVersion=4.0
Description=Optical Line Guidance
CreationTime=09:08AM
CreationDate=08-15-2018
CreatedBy=herrmra
ModificationTime=10:00AM
ModificationDate=15-02-2019
ModifiedBy=Michael Lehning
[DeviceInfo]
VendorName=SICK AG
VendorNumber=0x01000056
ProductName=OLS20
ProductNumber=0x00001004
RevisionNumber=1
OrderCode=-
BaudRate_10=1
BaudRate_20=1
BaudRate_50=1
BaudRate_125=1
BaudRate_250=1
BaudRate_500=1
BaudRate_800=0
BaudRate_1000=1
DynamicChannelsSupported=0
GroupMessaging=0
LSS_Supported=1
Granularity=8
SimpleBootUpSlave=1
SimpleBootUpMaster=0
NrOfRXPDO=0
NrOfTXPDO=2
[Comments]
Lines=0
[DummyUsage]
Dummy0001=0
Dummy0002=0
Dummy0003=0
Dummy0004=0
Dummy0005=0
Dummy0006=0
Dummy0007=0
[MandatoryObjects]
SupportedObjects=3
1=0x1000
2=0x1001
3=0x1018
[OptionalObjects]
SupportedObjects=16
1=0x1005
2=0x1008
3=0x1009
4=0x100A
5=0x100C
6=0x100D
7=0x1014
8=0x1015
9=0x1016
10=0x1017
11=0x1200
12=0x1800
13=0x1801
14=0x1A00
15=0x1A01
16=0x1F80
[ManufacturerObjects]
SupportedObjects=6
1=0x2001
2=0x2002
3=0x2003
4=0x2018
5=0x2019
6=0x2021
[1000]
ParameterName=Device Type
ObjectType=0x07
DataType=0x0007
AccessType=const
DefaultValue=0x0000000
PDOMapping=0
[1001]
ParameterName=Error Register
ObjectType=0x07
DataType=0x0005
AccessType=ro
DefaultValue=0x00
PDOMapping=0
[1005]
ParameterName=COB-ID SYNC
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000080
PDOMapping=0
[1008]
ParameterName=Manufacturer Device Name
ObjectType=0x07
DataType=0x0009
AccessType=const
DefaultValue=OLS20
PDOMapping=0
[1009]
ParameterName=Manufacturer Hardware Version
ObjectType=0x07
DataType=0x0009
AccessType=const
PDOMapping=0
[100A]
ParameterName=Manufacturer Software Version
ObjectType=0x07
DataType=0x0009
AccessType=const
PDOMapping=0
[100C]
ParameterName=Guard Time
ObjectType=0x07
DataType=0x0006
AccessType=rw
DefaultValue=0x0000
PDOMapping=0
[100D]
ParameterName=Life Time Factor
ObjectType=0x07
DataType=0x0005
AccessType=rw
DefaultValue=0x00
PDOMapping=0
[1014]
ParameterName=COB-ID EMCY
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=$NODEID+0x80
PDOMapping=0
[1015]
ParameterName=Inhibit Time Emergency
ObjectType=0x07
DataType=0x0006
AccessType=rw
DefaultValue=0x0000
PDOMapping=0
[1016]
SubNumber=3
ParameterName=Heartbeat Consumer Entries
ObjectType=0x08
[1016sub0]
ParameterName=Highest sub-index supported
ObjectType=0x07
DataType=0x0005
AccessType=const
DefaultValue=0x02
PDOMapping=0
[1016sub1]
ParameterName=Consumer Heartbeat Time 1
ObjectType=0x07
DataType=0x0007
AccessType=rw
PDOMapping=0
[1016sub2]
ParameterName=Consumer Heartbeat Time 2
ObjectType=0x07
DataType=0x0007
AccessType=rw
PDOMapping=0
[1017]
ParameterName=Producer Heartbeat Time
ObjectType=0x07
DataType=0x0006
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1018]
SubNumber=5
ParameterName=Identity Object
ObjectType=0x09
[1018sub0]
ParameterName=number of entries
ObjectType=0x07
DataType=0x0005
AccessType=const
DefaultValue=0x4
PDOMapping=0
[1018sub1]
ParameterName=Vendor Id
ObjectType=0x07
DataType=0x0007
AccessType=ro
DefaultValue=0x01000056
PDOMapping=0
[1018sub2]
ParameterName=Product Code
ObjectType=0x07
DataType=0x0007
AccessType=ro
DefaultValue=0x00001004
PDOMapping=0
[1018sub3]
ParameterName=Revision number
ObjectType=0x07
DataType=0x0007
AccessType=ro
DefaultValue=0x00000001
PDOMapping=0
[1018sub4]
ParameterName=Serial number
ObjectType=0x07
DataType=0x0007
AccessType=ro
PDOMapping=0
[1200]
SubNumber=3
ParameterName=Server SDO Parameter 1
ObjectType=0x09
[1200sub0]
ParameterName=Highest sub-index supported
ObjectType=0x07
DataType=0x0005
AccessType=const
DefaultValue=0x02
PDOMapping=0
[1200sub1]
ParameterName=COB-ID Client -> Server
ObjectType=0x07
DataType=0x0007
AccessType=ro
DefaultValue=$NODEID+0x600
PDOMapping=0
[1200sub2]
ParameterName=COB-ID Server -> Client
ObjectType=0x07
DataType=0x0007
AccessType=ro
DefaultValue=$NODEID+0x580
PDOMapping=0
[1800]
SubNumber=6
ParameterName=Transmit PDO Communication Parameter 1
ObjectType=0x09
[1800sub0]
ParameterName=Highest sub-index supported
ObjectType=0x07
DataType=0x0005
AccessType=const
DefaultValue=0x05
PDOMapping=0
LowLimit=0x2
HighLimit=0x6
[1800sub1]
ParameterName=COB-ID
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=$NODEID+0x180
PDOMapping=0
LowLimit=0x00000181
HighLimit=0xFFFFFFFF
[1800sub2]
ParameterName=Transmission Type
ObjectType=0x07
DataType=0x0005
AccessType=rw
DefaultValue=0xFF
PDOMapping=0
LowLimit=0x0
HighLimit=0xFF
[1800sub3]
ParameterName=Inhibit Time
ObjectType=0x07
DataType=0x0006
AccessType=rw
DefaultValue=0x0
PDOMapping=0
LowLimit=0x0000
HighLimit=0xFFFF
[1800sub4]
ParameterName=Compatibility Entry
ObjectType=0x07
DataType=0x0005
AccessType=rw
DefaultValue=0x00
PDOMapping=0
LowLimit=0x00
HighLimit=0xFF
[1800sub5]
ParameterName=Event Timer
ObjectType=0x07
DataType=0x0006
AccessType=rw
DefaultValue=0x14
PDOMapping=0
LowLimit=0x0
HighLimit=0xFFFF
[1801]
SubNumber=6
ParameterName=Transmit PDO Communication Parameter 2
ObjectType=0x09
[1801sub0]
ParameterName=Highest sub-index supported
ObjectType=0x07
DataType=0x0005
AccessType=const
DefaultValue=0x05
PDOMapping=0
LowLimit=0x02
HighLimit=0x06
[1801sub1]
ParameterName=COB-ID
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=$NODEID+0x280
PDOMapping=0
LowLimit=0x00000281
HighLimit=0xFFFFFFFF
[1801sub2]
ParameterName=Transmission Type
ObjectType=0x07
DataType=0x0005
AccessType=rw
DefaultValue=0xFF
PDOMapping=0
LowLimit=0x0
HighLimit=0xFF
[1801sub3]
ParameterName=Inhibit Time
ObjectType=0x07
DataType=0x0006
AccessType=rw
DefaultValue=0x0
PDOMapping=0
LowLimit=0x0000
HighLimit=0xFFFF
[1801sub4]
ParameterName=Compatibility Entry
ObjectType=0x07
DataType=0x0005
AccessType=rw
DefaultValue=0x00
PDOMapping=0
LowLimit=0x00
HighLimit=0xFF
[1801sub5]
ParameterName=Event Timer
ObjectType=0x07
DataType=0x0006
AccessType=rw
DefaultValue=0x14
PDOMapping=0
LowLimit=0x0
HighLimit=0xFFFF
[1A00]
SubNumber=9
ParameterName=Transmit PDO Mapping Parameter 1
ObjectType=0x09
[1A00sub0]
ParameterName=Number of mapped objects
ObjectType=0x07
DataType=0x0005
AccessType=rw
DefaultValue=0x05
PDOMapping=0
LowLimit=0x00
HighLimit=0x08
[1A00sub1]
ParameterName=Mapping Entry 1
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x20210110
PDOMapping=0
[1A00sub2]
ParameterName=Mapping Entry 2
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x20210210
PDOMapping=0
[1A00sub3]
ParameterName=Mapping Entry 3
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x20210310
PDOMapping=0
[1A00sub4]
ParameterName=Mapping Entry 4
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x20210408
PDOMapping=0
[1A00sub5]
ParameterName=Mapping Entry 5
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x20210808
PDOMapping=0
[1A00sub6]
ParameterName=Mapping Entry 6
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1A00sub7]
ParameterName=Mapping Entry 7
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1A00sub8]
ParameterName=Mapping Entry 8
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1A01]
SubNumber=9
ParameterName=Transmit PDO Mapping Parameter 2
ObjectType=0x09
[1A01sub0]
ParameterName=Number of mapped objects
ObjectType=0x07
DataType=0x0005
AccessType=rw
DefaultValue=0x03
PDOMapping=0
LowLimit=0x0
HighLimit=0x8
[1A01sub1]
ParameterName=Mapping Entry 1
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x20210510
PDOMapping=0
[1A01sub2]
ParameterName=Mapping Entry 2
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x20210610
PDOMapping=0
[1A01sub3]
ParameterName=Mapping Entry 3
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x20210710
PDOMapping=0
[1A01sub4]
ParameterName=Mapping Entry 4
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1A01sub5]
ParameterName=Mapping Entry 5
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1A01sub6]
ParameterName=Mapping Entry 6
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1A01sub7]
ParameterName=Mapping Entry 7
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1A01sub8]
ParameterName=Mapping Entry 8
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1F80]
ParameterName=NMT Startup
ObjectType=0x07
DataType=0x0007
AccessType=const
PDOMapping=0
LowLimit=0x0
HighLimit=0x3F
[2001]
SubNumber=6
ParameterName=Mounting parameters
ObjectType=0x09
[2001sub0]
ParameterName=numOfEntries
ObjectType=0x07
DataType=0x0005
AccessType=const
DefaultValue=0x5
PDOMapping=0
[2001sub1]
ParameterName=reserved1
ObjectType=0x07
DataType=0x0007
AccessType=ro
PDOMapping=0
[2001sub2]
ParameterName=reserved2
ObjectType=0x07
DataType=0x0007
AccessType=ro
PDOMapping=0
[2001sub3]
ParameterName=reserved3
ObjectType=0x07
DataType=0x0007
AccessType=ro
PDOMapping=0
[2001sub4]
ParameterName=reserved4
ObjectType=0x07
DataType=0x0007
AccessType=ro
PDOMapping=0
[2001sub5]
ParameterName=sensorFlipped
ObjectType=0x07
DataType=0x0005
AccessType=rw
DefaultValue=0x00
PDOMapping=0
LowLimit=0x0
HighLimit=0xFF
[2002]
SubNumber=4
ParameterName=Tape Parameters
ObjectType=0x09
[2002sub0]
ParameterName=Number of mapped objects
ObjectType=0x07
DataType=0x0005
AccessType=const
DefaultValue=0x3
PDOMapping=0
[2002sub1]
ParameterName=Typ. Width
ObjectType=0x07
DataType=0x0008
AccessType=rw
PDOMapping=0
[2002sub2]
ParameterName=Min. Width
ObjectType=0x07
DataType=0x0008
AccessType=rw
PDOMapping=0
[2002sub3]
ParameterName=Max. Width
ObjectType=0x07
DataType=0x0008
AccessType=rw
PDOMapping=0
[2003]
SubNumber=3
ParameterName=Advanced Settings
ObjectType=0x09
[2003sub0]
ParameterName=Number of mapped objects
ObjectType=0x07
DataType=0x0005
AccessType=const
DefaultValue=0x2
PDOMapping=0
[2003sub1]
ParameterName=Off Delay Track Detection
ObjectType=0x07
DataType=0x0006
AccessType=rw
DefaultValue=0x0064
PDOMapping=0
LowLimit=0x0
HighLimit=0xFFFF
[2003sub2]
ParameterName=Off Delay Code Detection
ObjectType=0x07
DataType=0x0006
AccessType=rw
DefaultValue=0x0064
PDOMapping=0
LowLimit=0x0
HighLimit=0xFFFF
[2018]
ParameterName=Device Status
ObjectType=0x07
DataType=0x0005
AccessType=ro
PDOMapping=0
[2019]
ParameterName=Order number
ObjectType=0x07
DataType=0x0007
AccessType=ro
PDOMapping=0
[2021]
SubNumber=10
ParameterName=Result data (LCPs)
ObjectType=0x09
[2021sub0]
ParameterName=Number of mapped objects
ObjectType=0x07
DataType=0x0005
AccessType=const
DefaultValue=0x9
PDOMapping=0
[2021sub1]
ParameterName=LCP1
ObjectType=0x07
DataType=0x0003
AccessType=ro
PDOMapping=1
LowLimit=0x8000
HighLimit=0x7FFF
[2021sub2]
ParameterName=LCP2
ObjectType=0x07
DataType=0x0003
AccessType=ro
PDOMapping=1
LowLimit=0x8000
HighLimit=0x7FFF
[2021sub3]
ParameterName=LCP3
ObjectType=0x07
DataType=0x0003
AccessType=ro
PDOMapping=1
LowLimit=0x8000
HighLimit=0x7FFF
[2021sub4]
ParameterName=Status
ObjectType=0x07
DataType=0x0005
AccessType=ro
PDOMapping=1
LowLimit=0x00
HighLimit=0xFF
[2021sub5]
ParameterName=Width LCP1
ObjectType=0x07
DataType=0x0003
AccessType=ro
PDOMapping=1
LowLimit=0x8000
HighLimit=0x7FFF
[2021sub6]
ParameterName=Width LCP2
ObjectType=0x07
DataType=0x0003
AccessType=ro
PDOMapping=1
LowLimit=0x8000
HighLimit=0x7FFF
[2021sub7]
ParameterName=Width LCP3
ObjectType=0x07
DataType=0x0003
AccessType=ro
PDOMapping=1
LowLimit=0x8000
HighLimit=0x7FFF
[2021sub8]
ParameterName=Code
ObjectType=0x07
DataType=0x0005
AccessType=ro
PDOMapping=1
LowLimit=0x0
HighLimit=0xFF
[2021sub9]
ParameterName=Extended Code
ObjectType=0x07
DataType=0x0007
AccessType=ro
PDOMapping=0
LowLimit=0x0
HighLimit=0xFFFFFFFF

867
Devices/Packages/sick_line_guidance/ols/SICK_OLS20_CO.eds Executable file
View File

@@ -0,0 +1,867 @@
; This EDS file was created by the CANopen Design Tool 2.3.26.0.
; port GmbH Halle/Saale Germany, http://www.port.de, mailto:service@port.de
[FileInfo]
FileName=OLS20.eds
FileVersion=1
FileRevision=2
EDSVersion=4.0
Description=Optical Line Guidance
CreationTime=09:08AM
CreationDate=08-15-2018
CreatedBy=herrmra
ModificationTime=04:29PM
ModificationDate=04-10-2019
ModifiedBy=herrmra
[DeviceInfo]
VendorName=SICK AG
VendorNumber=0x01000056
ProductName=OLS20
ProductNumber=0x00001004
RevisionNumber=1
OrderCode=-
BaudRate_10=1
BaudRate_20=1
BaudRate_50=1
BaudRate_125=1
BaudRate_250=1
BaudRate_500=1
BaudRate_800=0
BaudRate_1000=1
DynamicChannelsSupported=0
GroupMessaging=0
LSS_Supported=1
Granularity=8
SimpleBootUpSlave=1
SimpleBootUpMaster=0
NrOfRXPDO=0
NrOfTXPDO=2
[Comments]
Lines=0
[DummyUsage]
Dummy0001=0
Dummy0002=0
Dummy0003=0
Dummy0004=0
Dummy0005=0
Dummy0006=0
Dummy0007=0
[MandatoryObjects]
SupportedObjects=3
1=0x1000
2=0x1001
3=0x1018
[OptionalObjects]
SupportedObjects=16
1=0x1005
2=0x1008
3=0x1009
4=0x100A
5=0x100C
6=0x100D
7=0x1014
8=0x1015
9=0x1016
10=0x1017
11=0x1200
12=0x1800
13=0x1801
14=0x1A00
15=0x1A01
16=0x1F80
[ManufacturerObjects]
SupportedObjects=7
1=0x2001
2=0x2002
3=0x2003
4=0x2018
5=0x2019
6=0x2021
7=0x2023
[1000]
ParameterName=Device Type
ObjectType=0x07
DataType=0x0007
AccessType=const
DefaultValue=0x0000000
PDOMapping=0
[1001]
ParameterName=Error Register
ObjectType=0x07
DataType=0x0005
AccessType=ro
DefaultValue=0
PDOMapping=0
[1005]
ParameterName=COB-ID SYNC
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000080
PDOMapping=0
[1008]
ParameterName=Manufacturer Device Name
ObjectType=0x07
DataType=0x0009
AccessType=const
DefaultValue=OLS20
PDOMapping=0
[1009]
ParameterName=Manufacturer Hardware Version
ObjectType=0x07
DataType=0x0009
AccessType=const
PDOMapping=0
[100A]
ParameterName=Manufacturer Software Version
ObjectType=0x07
DataType=0x0009
AccessType=const
PDOMapping=0
[100C]
ParameterName=Guard Time
ObjectType=0x07
DataType=0x0006
AccessType=rw
DefaultValue=0x00
PDOMapping=0
[100D]
ParameterName=Life Time Factor
ObjectType=0x07
DataType=0x0005
AccessType=rw
DefaultValue=0x00
PDOMapping=0
[1014]
ParameterName=COB-ID EMCY
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=$NODEID+0x80
PDOMapping=0
[1015]
ParameterName=Inhibit Time Emergency
ObjectType=0x07
DataType=0x0006
AccessType=rw
DefaultValue=0x0000
PDOMapping=0
[1016]
SubNumber=3
ParameterName=Heartbeat Consumer Entries
ObjectType=0x08
[1016sub0]
ParameterName=Highest sub-index supported
ObjectType=0x07
DataType=5
AccessType=const
DefaultValue=0x02
PDOMapping=0
[1016sub1]
ParameterName=Consumer Heartbeat Time 1
ObjectType=0x07
DataType=0x0007
AccessType=rw
PDOMapping=0
[1016sub2]
ParameterName=Consumer Heartbeat Time 2
ObjectType=0x07
DataType=0x0007
AccessType=rw
PDOMapping=0
[1017]
ParameterName=Producer Heartbeat Time
ObjectType=0x07
DataType=0x0006
AccessType=rw
DefaultValue=0x00
PDOMapping=0
[1018]
SubNumber=5
ParameterName=Identity Object
ObjectType=0x09
[1018sub0]
ParameterName=Highest sub-index supported
ObjectType=0x07
DataType=0x0005
AccessType=const
DefaultValue=0x4
PDOMapping=0
[1018sub1]
ParameterName=Vendor Id
ObjectType=0x07
DataType=0x0007
AccessType=ro
DefaultValue=0x01000056
PDOMapping=0
[1018sub2]
ParameterName=Product Code
ObjectType=0x07
DataType=0x0007
AccessType=ro
DefaultValue=0x00001004
PDOMapping=0
[1018sub3]
ParameterName=Revision number
ObjectType=0x07
DataType=0x0007
AccessType=ro
DefaultValue=1
PDOMapping=0
[1018sub4]
ParameterName=Serial number
ObjectType=0x07
DataType=0x0007
AccessType=ro
PDOMapping=0
[1200]
SubNumber=3
ParameterName=Server SDO Parameter 1
ObjectType=0x09
[1200sub0]
ParameterName=Highest sub-index supported
ObjectType=0x07
DataType=0x0005
AccessType=const
DefaultValue=0x02
PDOMapping=0
[1200sub1]
ParameterName=COB-ID Client -> Server
ObjectType=0x07
DataType=0x0007
AccessType=ro
DefaultValue=$NODEID+0x600
PDOMapping=0
[1200sub2]
ParameterName=COB-ID Server -> Client
ObjectType=0x07
DataType=0x0007
AccessType=ro
DefaultValue=$NODEID+0x580
PDOMapping=0
[1800]
SubNumber=6
ParameterName=Transmit PDO Communication Parameter 1
ObjectType=0x09
[1800sub0]
ParameterName=Highest sub-index supported
ObjectType=0x07
DataType=0x0005
AccessType=const
DefaultValue=0x05
PDOMapping=0
LowLimit=0x2
HighLimit=0x6
[1800sub1]
ParameterName=COB-ID
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=$NODEID+0x180
PDOMapping=0
LowLimit=0x00000181
HighLimit=0xFFFFFFFF
[1800sub2]
ParameterName=Transmission Type
ObjectType=0x07
DataType=0x0005
AccessType=rw
DefaultValue=0xFF
PDOMapping=0
LowLimit=0x0
HighLimit=0xFF
[1800sub3]
ParameterName=Inhibit Time
ObjectType=0x07
DataType=0x0006
AccessType=rw
DefaultValue=0x0
PDOMapping=0
LowLimit=0x0000
HighLimit=0xFFFF
[1800sub4]
ParameterName=Compatibility Entry
ObjectType=0x07
DataType=0x0005
AccessType=rw
DefaultValue=0x00
PDOMapping=0
LowLimit=0x00
HighLimit=0xFF
[1800sub5]
ParameterName=Event Timer
ObjectType=0x07
DataType=0x0006
AccessType=rw
DefaultValue=0x14
PDOMapping=0
LowLimit=0x0
HighLimit=0xFFFF
[1801]
SubNumber=6
ParameterName=Transmit PDO Communication Parameter 2
ObjectType=0x09
[1801sub0]
ParameterName=Highest sub-index supported
ObjectType=0x07
DataType=0x0005
AccessType=const
DefaultValue=0x05
PDOMapping=0
LowLimit=0x02
HighLimit=0x06
[1801sub1]
ParameterName=COB-ID
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=$NODEID+0x280
PDOMapping=0
LowLimit=0x00000281
HighLimit=0xFFFFFFFF
[1801sub2]
ParameterName=Transmission Type
ObjectType=0x07
DataType=0x0005
AccessType=rw
DefaultValue=0xFF
PDOMapping=0
LowLimit=0x0
HighLimit=0xFF
[1801sub3]
ParameterName=Inhibit Time
ObjectType=0x07
DataType=0x0006
AccessType=rw
DefaultValue=0x0
PDOMapping=0
LowLimit=0x0000
HighLimit=0xFFFF
[1801sub4]
ParameterName=Compatibility Entry
ObjectType=0x07
DataType=0x0005
AccessType=rw
DefaultValue=0x00
PDOMapping=0
LowLimit=0x00
HighLimit=0xFF
[1801sub5]
ParameterName=Event Timer
ObjectType=0x07
DataType=0x0006
AccessType=rw
DefaultValue=0x14
PDOMapping=0
LowLimit=0x0
HighLimit=0xFFFF
[1A00]
SubNumber=9
ParameterName=Transmit PDO Mapping Parameter 1
ObjectType=0x09
[1A00sub0]
ParameterName=Number of mapped objects
ObjectType=0x07
DataType=0x0005
AccessType=rw
DefaultValue=0x05
PDOMapping=0
LowLimit=0x00
HighLimit=0x08
[1A00sub1]
ParameterName=Mapping Entry 1
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x20210110
PDOMapping=0
[1A00sub2]
ParameterName=Mapping Entry 2
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x20210210
PDOMapping=0
[1A00sub3]
ParameterName=Mapping Entry 3
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x20210310
PDOMapping=0
[1A00sub4]
ParameterName=Mapping Entry 4
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x20210408
PDOMapping=0
[1A00sub5]
ParameterName=Mapping Entry 5
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x20210808
PDOMapping=0
[1A00sub6]
ParameterName=Mapping Entry 6
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1A00sub7]
ParameterName=Mapping Entry 7
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1A00sub8]
ParameterName=Mapping Entry 8
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1A01]
SubNumber=9
ParameterName=Transmit PDO Mapping Parameter 2
ObjectType=0x09
[1A01sub0]
ParameterName=Number of mapped objects
ObjectType=0x07
DataType=0x0005
AccessType=rw
DefaultValue=0x03
PDOMapping=0
LowLimit=0x0
HighLimit=0x8
[1A01sub1]
ParameterName=Mapping Entry 1
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x20210510
PDOMapping=0
[1A01sub2]
ParameterName=Mapping Entry 2
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x20210610
PDOMapping=0
[1A01sub3]
ParameterName=Mapping Entry 3
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x20210710
PDOMapping=0
[1A01sub4]
ParameterName=Mapping Entry 4
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x0
PDOMapping=0
[1A01sub5]
ParameterName=Mapping Entry 5
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x0
PDOMapping=0
[1A01sub6]
ParameterName=Mapping Entry 6
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x0
PDOMapping=0
[1A01sub7]
ParameterName=Mapping Entry 7
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x0
PDOMapping=0
[1A01sub8]
ParameterName=Mapping Entry 8
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x0
PDOMapping=0
[1F80]
ParameterName=NMT Startup
ObjectType=0x07
DataType=0x0007
AccessType=const
PDOMapping=0
LowLimit=0x0
HighLimit=0x3F
[2001]
SubNumber=6
ParameterName=Mounting parameters
ObjectType=0x09
[2001sub0]
ParameterName=Highest sub-index supported
ObjectType=0x07
DataType=0x0005
AccessType=const
DefaultValue=0x5
PDOMapping=0
[2001sub1]
ParameterName=Reserved 1
ObjectType=0x07
DataType=0x0007
AccessType=ro
PDOMapping=0
[2001sub2]
ParameterName=Reserved 2
ObjectType=0x07
DataType=0x0007
AccessType=ro
PDOMapping=0
[2001sub3]
ParameterName=Reserved 3
ObjectType=0x07
DataType=0x0007
AccessType=ro
PDOMapping=0
[2001sub4]
ParameterName=Reserved 4
ObjectType=0x07
DataType=0x0007
AccessType=ro
PDOMapping=0
[2001sub5]
ParameterName=Sensor flipped
ObjectType=0x07
DataType=0x0005
AccessType=rw
DefaultValue=0x0
PDOMapping=0
LowLimit=0x0
HighLimit=0xFF
[2002]
SubNumber=4
ParameterName=Tape parameters
ObjectType=0x09
[2002sub0]
ParameterName=Highest sub-index supported
ObjectType=0x07
DataType=0x0005
AccessType=const
DefaultValue=0x3
PDOMapping=0
[2002sub1]
ParameterName=Typ. width
ObjectType=0x07
DataType=0x0008
AccessType=rw
PDOMapping=0
[2002sub2]
ParameterName=Min. width
ObjectType=0x07
DataType=0x0008
AccessType=rw
PDOMapping=0
[2002sub3]
ParameterName=Max. width
ObjectType=0x07
DataType=0x0008
AccessType=rw
PDOMapping=0
[2003]
SubNumber=4
ParameterName=Advanced settings
ObjectType=0x09
[2003sub0]
ParameterName=Highest sub-index supported
ObjectType=0x07
DataType=0x0005
AccessType=const
DefaultValue=0x3
PDOMapping=0
[2003sub1]
ParameterName=Off delay track detection
ObjectType=0x07
DataType=0x0006
AccessType=rw
DefaultValue=0x64
PDOMapping=0
LowLimit=0x0
HighLimit=0xFFFF
[2003sub2]
ParameterName=Off delay code detection
ObjectType=0x07
DataType=0x0006
AccessType=rw
DefaultValue=0x64
PDOMapping=0
LowLimit=0x0
HighLimit=0xFFFF
[2003sub3]
ParameterName=Position smoothing filter coefficient
ObjectType=0x07
DataType=0x0008
AccessType=rw
DefaultValue=0.0
PDOMapping=0
[2018]
ParameterName=Device status
ObjectType=0x07
DataType=0x0005
AccessType=ro
PDOMapping=0
[2019]
ParameterName=Order number
ObjectType=0x07
DataType=0x0007
AccessType=ro
PDOMapping=0
[2021]
SubNumber=12
ParameterName=Result data (LCPs)
ObjectType=0x09
[2021sub0]
ParameterName=Highest sub-index supported
ObjectType=0x07
DataType=0x0005
AccessType=const
DefaultValue=0x0B
PDOMapping=0
[2021sub1]
ParameterName=LCP 1
ObjectType=0x07
DataType=0x0003
AccessType=ro
PDOMapping=1
LowLimit=0x8000
HighLimit=0x7FFF
[2021sub2]
ParameterName=LCP 2
ObjectType=0x07
DataType=0x0003
AccessType=ro
PDOMapping=1
LowLimit=0x8000
HighLimit=0x7FFF
[2021sub3]
ParameterName=LCP 3
ObjectType=0x07
DataType=0x0003
AccessType=ro
PDOMapping=1
LowLimit=0x8000
HighLimit=0x7FFF
[2021sub4]
ParameterName=Status
ObjectType=0x07
DataType=0x0005
AccessType=ro
PDOMapping=1
LowLimit=0x00
HighLimit=0xFF
[2021sub5]
ParameterName=Width LCP 1
ObjectType=0x07
DataType=0x0003
AccessType=ro
PDOMapping=1
LowLimit=0x8000
HighLimit=0x7FFF
[2021sub6]
ParameterName=Width LCP 2
ObjectType=0x07
DataType=0x0003
AccessType=ro
PDOMapping=1
LowLimit=0x8000
HighLimit=0x7FFF
[2021sub7]
ParameterName=Width LCP 3
ObjectType=0x07
DataType=0x0003
AccessType=ro
PDOMapping=1
LowLimit=0x8000
HighLimit=0x7FFF
[2021sub8]
ParameterName=Code
ObjectType=0x07
DataType=0x0005
AccessType=ro
PDOMapping=1
LowLimit=0x0
HighLimit=0xFF
[2021sub9]
ParameterName=Extended code
ObjectType=0x07
DataType=0x0007
AccessType=ro
PDOMapping=0
LowLimit=0x0
HighLimit=0xFFFFFFFF
[2021subA]
ParameterName=Barcode center point
ObjectType=0x07
DataType=0x0003
AccessType=ro
DefaultValue=0x0000
PDOMapping=0
LowLimit=0x8000
HighLimit=0x7FFF
[2021subB]
ParameterName=Quality of lines
ObjectType=0x07
DataType=0x0005
AccessType=ro
DefaultValue=0x64
PDOMapping=0
LowLimit=0x00
HighLimit=0x64
[2023]
SubNumber=4
ParameterName=Line level
ObjectType=0x09
[2023sub0]
ParameterName=Highest sub-index supported
ObjectType=0x07
DataType=0x0005
AccessType=const
DefaultValue=0x3
PDOMapping=0
LowLimit=0x1
HighLimit=0x3
[2023sub1]
ParameterName=Intensity line 1
ObjectType=0x07
DataType=0x0005
AccessType=ro
PDOMapping=0
LowLimit=0x0
HighLimit=0x82
[2023sub2]
ParameterName=Intensity line 2
ObjectType=0x07
DataType=0x0005
AccessType=ro
PDOMapping=0
LowLimit=0x0
HighLimit=0x82
[2023sub3]
ParameterName=Intensity line 3
ObjectType=0x07
DataType=0x0005
AccessType=ro
PDOMapping=0
LowLimit=0x0
HighLimit=0x82

32
Devices/Packages/sick_line_guidance/ols/sick_line_guidance_ols10.yaml Executable file
View File

@@ -0,0 +1,32 @@
bus:
device: can0
driver_plugin: can::SocketCANInterface
master_allocator: canopen::SimpleMaster::Allocator
sync:
interval_ms: 10
overflow: 0
#
# heartbeat: # simple heartbeat producer (optional, not supported by OLS or MLS, do not enable heartbeats)
# rate: 100 # heartbeat rate (milliseconds)
# msg: "77f#05" # message to send, cansend format: heartbeat of node 127 with status 5=Started
nodes:
node1:
id: 0x0A # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
eds_pkg: sick_line_guidance # package name for relative path
eds_file: SICK_OLS10_CO.eds # path to EDS/DCF file
publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2021sub5!","2021sub6!","2021sub7!","2021sub8!"]
# OLS10: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDOs 0x2021sub1 to 0x2021sub8
# sick_line_guidance configuration of this node:
sick_device_family: "OLS10" # can devices currently supported: "OLS10", "OLS20" or "MLS"
sick_topic: "ols" # OLS_Measurement messages are published in topic "/ols"
sick_frame_id: "ols_measurement_frame" # OLS_Measurement messages are published with frame_id "ols_measurement_frame"
# device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue"
# example: "2001sub5": "0x01" # Object 2001sub5 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01
# dcf_overlay:
# "2001sub5": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0
# "2002sub1": "0.015" # Typ. Width, FLOAT32, DataType=0x0008
# "2002sub2": "0.001" # Min. Width, FLOAT32, DataType=0x0008
# "2002sub3": "0.050" # Max. Width, FLOAT32, DataType=0x0008

32
Devices/Packages/sick_line_guidance/ols/sick_line_guidance_ols20.yaml Executable file
View File

@@ -0,0 +1,32 @@
bus:
device: can0
driver_plugin: can::SocketCANInterface
master_allocator: canopen::SimpleMaster::Allocator
sync:
interval_ms: 10
overflow: 0
#
# heartbeat: # simple heartbeat producer (optional, not supported by OLS or MLS, do not enable heartbeats)
# rate: 100 # heartbeat rate (milliseconds)
# msg: "77f#05" # message to send, cansend format: heartbeat of node 127 with status 5=Started
nodes:
node1:
id: 0x0A # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
eds_pkg: sick_line_guidance # package name for relative path
eds_file: SICK_OLS20_CO.eds # path to EDS/DCF file
publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2021sub5!","2021sub6!","2021sub7!","2021sub8!"]
# OLS20: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDOs 0x2021sub1 to 0x2021sub8
# sick_line_guidance configuration of this node:
sick_device_family: "OLS20" # can devices currently supported: "OLS10", "OLS20" or "MLS"
sick_topic: "ols" # OLS_Measurement messages are published in topic "/ols"
sick_frame_id: "ols_measurement_frame" # OLS_Measurement messages are published with frame_id "ols_measurement_frame"
# device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue"
# example: "2001sub5": "0x01" # Object 2001sub5 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01
# dcf_overlay:
# "2001sub5": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0
# "2002sub1": "0.015" # Typ. Width, FLOAT32, DataType=0x0008
# "2002sub2": "0.001" # Min. Width, FLOAT32, DataType=0x0008
# "2002sub3": "0.050" # Max. Width, FLOAT32, DataType=0x0008

44
Devices/Packages/sick_line_guidance/ols/sick_line_guidance_ols20_twin.yaml Executable file
View File

@@ -0,0 +1,44 @@
bus:
device: can0
driver_plugin: can::SocketCANInterface
master_allocator: canopen::SimpleMaster::Allocator
sync:
interval_ms: 10
overflow: 0
#
# heartbeat: # simple heartbeat producer (optional, not supported by OLS or MLS, do not enable heartbeats)
# rate: 100 # heartbeat rate (milliseconds)
# msg: "77f#05" # message to send, cansend format: heartbeat of node 127 with status 5=Started
nodes:
node1:
id: 0x0A # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
eds_pkg: sick_line_guidance # package name for relative path
eds_file: SICK_OLS20_CO.eds # path to EDS/DCF file
publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2021sub5!","2021sub6!","2021sub7!","2021sub8!"]
# OLS20: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDOs 0x2021sub1 to 0x2021sub8
# sick_line_guidance configuration of this node:
sick_device_family: "OLS20" # can devices currently supported: "OLS10", "OLS20" or "MLS"
sick_topic: "olsA" # OLS_Measurement messages are published in topic "/ols"
sick_frame_id: "ols20A_measurement_frame" # OLS_Measurement messages are published with frame_id "ols_measurement_frame"
# device configuration of writable parameter by dcf_overlay: "objectindex": "parametervalue"
# example: "2001sub5": "0x01" # Object 2001sub5 (sensorFlipped, defaultvalue 0x00) will be configured with value 0x01
# dcf_overlay:
# "2001sub5": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue=0
# "2002sub1": "0.015" # Typ. Width, FLOAT32, DataType=0x0008
# "2002sub2": "0.001" # Min. Width, FLOAT32, DataType=0x0008
# "2002sub3": "0.050" # Max. Width, FLOAT32, DataType=0x0008
node2:
id: 0x0B # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
eds_pkg: sick_line_guidance # package name for relative path
eds_file: SICK_OLS20_CO.eds # path to EDS/DCF file
publish: ["1001","1018sub1","1018sub4","2021sub1!","2021sub2!","2021sub3!","2021sub4!","2021sub5!","2021sub6!","2021sub7!","2021sub8!"]
# OLS20: 1001 = error register, 1018sub1 = VendorID, 1018sub4 = SerialNumber, TPDOs 0x2021sub1 to 0x2021sub8
# sick_line_guidance configuration of this node:
sick_device_family: "OLS20" # can devices currently supported: "OLS10", "OLS20" or "MLS"
sick_topic: "olsB" # OLS_Measurement messages are published in topic "/ols"
sick_frame_id: "ols20B_measurement_frame" # OLS_Measurement messages are published with frame_id "ols_measurement_frame"

81
Devices/Packages/sick_line_guidance/package.xml Executable file
View File

@@ -0,0 +1,81 @@
<?xml version="1.0"?>
<package format="2">
<name>sick_line_guidance</name>
<version>0.0.0</version>
<description>SICK Line Guidance ROS Support</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="ros.test@lehning.de">rostest</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>Apache</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/sick_line_guidance</url> -->
<url type="website">https://github.com/SICKAG/sick_line_guidance</url>
<url type="website">https://www.sick.com/ols</url>
<url type="website">https://www.sick.com/mls</url>
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<author email="ros.test@lehning.de">Michael Lehning</author>
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>cmake_modules</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>random_numbers</build_depend>
<build_depend>can_msgs</build_depend>
<build_depend>canopen_chain_node</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>message_generation</build_export_depend>
<build_export_depend>random_numbers</build_export_depend>
<build_export_depend>can_msgs</build_export_depend>
<build_export_depend>canopen_chain_node</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>message_runtime</exec_depend>
<exec_depend>random_numbers</exec_depend>
<exec_depend>can_msgs</exec_depend>
<exec_depend>canopen_chain_node</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

133
Devices/Packages/sick_line_guidance/src/sick_canopen_simu_canstate.cpp Executable file
View File

@@ -0,0 +1,133 @@
/*
* sick_canopen_simu_canstate implements a state engine for can.
*
* Depending on input messages of type can_msgs::Frame,
* the current state is switched between INITIALIZATION,
* PRE_OPERATIONAL, OPERATIONAL and STOPPED.
*
* class CanState: handles can nmt messages and implements the state engine for can.
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#include <ros/ros.h>
#include <can_msgs/Frame.h>
#include "sick_line_guidance/sick_canopen_simu_canstate.h"
/*
* Constructor.
*
* @param[in] can_node_id node id of OLS or MLS, default: 0x0A
*/
sick_canopen_simu::CanState::CanState(int can_node_id) : m_can_node_id(can_node_id), m_can_state(INITIALIZATION)
{
}
/*
* Destructor.
*/
sick_canopen_simu::CanState::~CanState()
{
}
/*
* @brief Callbacks for ros messages. Switches the state, and returns true, if msg_in is a can nmt message.
* Otherwise, false is returned.
*
* param[in] msg_in input message of type can_msgs::Frame
* param[out] msg_out output message of type can_msgs::Frame (if input message requires a response to send)
* param[out] send_msg true, if input message msg_in requires the response message msg_out to be send
*
* @return true, if input message handled, otherwise false.
*/
bool sick_canopen_simu::CanState::messageHandler(const can_msgs::Frame &msg_in, can_msgs::Frame & msg_out, bool & send_msg)
{
bool msg_handled = false;
send_msg = false;
// 000#... (msg_in.id == 0 && msg_in.dlc >= ): nmt message from can master (network manager) with 2 byte (command and nodeid)
// msg_in.data[0] : nmt command (0x1, 0x2, 0x80, 0x81 or 0x82)
// msg_in.data[1] : nodeid, message has to be handled if nodeid==0 (broadcast to all devices), or nodeid==m_can_node_id
if (msg_in.id == 0 && msg_in.dlc >= 2 && (msg_in.data[1] == 0 || msg_in.data[1] == m_can_node_id))
{
msg_out = msg_in;
switch (msg_in.data[0])
{
case 0x80: // 000#0x80...: Pre-operational -> switch to PRE_OPERATIONAL and send bootup message (0x700+$NODEID)#0x00
case 0x81: // 000#0x81...: Reset node -> switch to PRE_OPERATIONAL and send bootup message (0x700+$NODEID)#0x00
case 0x82: // 000#0x82...: Reset communication -> switch to PRE_OPERATIONAL and send bootup message (0x700+$NODEID)#0x00
msg_out.id = 0x700 + m_can_node_id;
msg_out.dlc = 1;
msg_out.data[0] = 0;
msg_out.header.stamp = ros::Time::now();
send_msg = true; // msg_out contains the bootup message to send
m_can_state = PRE_OPERATIONAL;
ROS_INFO_STREAM("sick_canopen_simu::CanState::messageHandler(): switched to state PRE_OPERATIONAL.");
msg_handled = true;
break;
case 0x01: // 000#0x01: switch to OPERATIONAL
m_can_state = OPERATIONAL;
ROS_INFO_STREAM("sick_canopen_simu::CanState::messageHandler(): switched to state OPERATIONAL.");
msg_handled = true;
break;
case 0x02: // 000#0x02: switch to STOPPED
m_can_state = STOPPED;
ROS_INFO_STREAM("sick_canopen_simu::CanState::messageHandler(): switched to state STOPPED.");
msg_handled = true;
break;
default:
break;
}
}
return msg_handled;
}

763
Devices/Packages/sick_line_guidance/src/sick_canopen_simu_device.cpp Executable file
View File

@@ -0,0 +1,763 @@
/*
* Class SimulatorBase implements the base class for simulation of SICK can devices (OLS20 and MLS).
*
* ROS messages of type can_msgs::Frame are read from ros topic "can0",
* handled to simulate an OLS20 or MLS device, and resulting can_msgs::Frame
* messages are published on ros topic "ros2can0".
*
* MsgType can be sick_line_guidance::OLS_Measurement (for simulation of OLS devices), or
* or sick_line_guidance::MLS_Measurement (for simulation of MLS devices).
*
* Assumption: ros nodes sick_line_guidance_ros2can_node and sick_line_guidance_can2ros_node
* have to be running to convert ros messages from and to canbus messages.
* sick_line_guidance_ros2can_node converts ros messages of type can_msgs::Frame to can frames,
* sick_line_guidance_can2ros_node converts can frames to ros messages of type can_msgs::Frame.
*
* Subclass MLSSimulator extends class SimulatorBase to simulate an MLS device.
*
* Subclass OLS20Simulator extends class SimulatorBase to simulate an OLS20 device.
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#include <boost/algorithm/clamp.hpp>
#include <ros/ros.h>
#include "sick_line_guidance/sick_canopen_simu_device.h"
#include "sick_line_guidance/sick_line_guidance_msg_util.h"
/*
* Constructor
*
* @param[in] nh ros node handle
* @param[in] config_file configuration file with testcases for OLS and MLS simulation
* @param[in] can_node_id node id of OLS or MLS, default: 0x0A
* @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0"
* @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0"
* @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second)
* @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times)
* @param[in] subscribe_queue_size buffer size for ros messages
*/
template<class MsgType>
sick_canopen_simu::SimulatorBase<MsgType>::SimulatorBase(ros::NodeHandle & nh, const std::string & config_file, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size)
: m_can_state(can_node_id), m_can_node_id(can_node_id), m_pdo_rate(pdo_rate), m_pdo_repeat_cnt(pdo_repeat_cnt), m_pdo_publisher_thread(0), m_pdo_publisher_thread_running(false), m_subscribe_queue_size(subscribe_queue_size), m_send_tpdo_immediately(false)
{
m_sdo_response_dev_state = 0x4F18200000000000; // response to sdo request for dev_status (object 0x2018): MLS and OLS20: 0x4F18200000000000 (sdo response with UINT8 data), OLS10: 0x4B18200000000000 (sdo response with UINT16 data)
m_ros_publisher = nh.advertise<can_msgs::Frame>(publish_topic, 1);
if(!readSDOconfig(config_file))
{
ROS_ERROR_STREAM("SimulatorBase: readSDOconfig(" << config_file << ") failed");
}
}
/*
* Destructor
*
*/
template<class MsgType>
sick_canopen_simu::SimulatorBase<MsgType>::~SimulatorBase()
{
m_pdo_publisher_thread_running = false;
if(m_pdo_publisher_thread)
{
m_pdo_publisher_thread->join();
delete(m_pdo_publisher_thread);
m_pdo_publisher_thread = 0;
}
}
/*
* @brief Callbacks for ros messages. Converts incoming messages of type can_msgs::Frame to simulate a can device
* and publishes simulation results to the configured ros topic.
*
* param[in] msg ros message of type can_msgs::Frame
*/
template<class MsgType>
void sick_canopen_simu::SimulatorBase<MsgType>::messageHandler(const can_msgs::Frame & msg_in)
{
// Handle NMT messages
bool send_msg = false;
can_msgs::Frame msg_out = msg_in;
if(m_can_state.messageHandler(msg_in, msg_out, send_msg))
{
if(send_msg)
{
boost::lock_guard<boost::mutex> publish_lockguard(m_ros_publisher_mutex);
m_ros_publisher.publish(msg_out);
}
return; // nmt message handled in m_can_state
}
// Ignore sync frames and bootup messages (node guarding frames) sent by can devices
if((msg_in.id == 0x80) || (msg_in.id >= 0x700 && msg_in.id <= 0x77F))
{
return;
}
// Ignore TPDO1 and TPDO2 frames sent by can devices
if((msg_in.id >= 0x180 && msg_in.id <= 0x1FF) || (msg_in.id >= 0x280 && msg_in.id <= 0x2FF))
{
return;
}
// Handle SDOs
if(handleSDO(msg_in))
{
return; // SDO request handled, SDO response sent
}
ROS_ERROR_STREAM("SimulatorBase::messageHandler(): message " << tostring(msg_in) << " not handled");
}
/*
* @brief Registers a listener to a simulation. Whenever the simulation sends a PDO, the listener is notified about the current sensor state.
* After receiving both the sensor state and the following OLS/MLS-Measurement ros message from the driver, the listener can check
* the correctness by comparing the sensor state from simulation and the OLS/MLS-Measurement from the driver. Both must be identical,
* otherwise some failure occured.
*
* param[in] pListener listener to current sensor states sent by PDO.
*/
template<class MsgType>
void sick_canopen_simu::SimulatorBase<MsgType>::registerSimulationListener(sick_canopen_simu::SimulatorBase<MsgType>::SimulationListener* pListener)
{
m_vec_simu_listener.push_back(pListener); // append pListener to the list of registered listeners
}
/*
* @brief Unregisters a listener from a simulation. The listener will not be notified about simulated sensor states.
* This function is the opposite to registerSimulationListener.
*
* param[in] pListener listener to current sensor states sent by PDO.
*/
template<class MsgType>
void sick_canopen_simu::SimulatorBase<MsgType>::unregisterSimulationListener(sick_canopen_simu::SimulatorBase<MsgType>::SimulationListener* pListener)
{
for(typename std::vector<SimulationListener*>::iterator iter = m_vec_simu_listener.begin(); iter != m_vec_simu_listener.end(); iter++)
{
if(*iter == pListener)
{
m_vec_simu_listener.erase(iter); // remove pListener from the list of registered listeners
break;
}
}
}
/*
* reads SDO configuration from xml-file
*
* @param[in] config_file configuration file with testcases for OLS and MLS simulation
*/
template<class MsgType>
bool sick_canopen_simu::SimulatorBase<MsgType>::readSDOconfig(const std::string & config_file)
{
try
{
TiXmlDocument xml_config(config_file);
if(xml_config.LoadFile())
{
ROS_INFO_STREAM("SimulatorBase::readSDOconfig(): reading SDO map from xml-file " << config_file);
TiXmlElement* xml_sdo_config = xml_config.FirstChild("sick_canopen_simu")->FirstChild("SDO_config")->ToElement();
if(xml_sdo_config)
{
TiXmlElement* xml_sdo = xml_sdo_config->FirstChildElement("sdo");
while(xml_sdo)
{
const std::string sdo_request = xml_sdo->Attribute("request");
const std::string sdo_response = xml_sdo->Attribute("response");
uint64_t u64_sdo_request = std::stoull(sdo_request, 0, 0);
uint64_t u64_sdo_response = std::stoull(sdo_response, 0, 0);
m_sdo_request_response_map[u64_sdo_request] = u64_sdo_response;
xml_sdo = xml_sdo->NextSiblingElement("sdo");
ROS_DEBUG_STREAM("SimulatorBase::readSDOconfig(): sdo_request_response_map[0x" << std::hex << u64_sdo_request << "] = 0x" << std::hex << u64_sdo_response
<< " (sdo_request_response_map[\"" << sdo_request << "\"] = \"" << sdo_response<< "\")");
}
if(!m_sdo_request_response_map.empty())
{
ROS_INFO_STREAM("SimulatorBase::readSDOconfig(" << config_file << "): " << m_sdo_request_response_map.size() << " sdo elements found");
return true;
}
else
{
ROS_ERROR_STREAM("SimulatorBase::readSDOconfig(" << config_file << "): no sdo elements found");
}
}
else
{
ROS_ERROR_STREAM("SimulatorBase::readSDOconfig(" << config_file << "): can't read element sick_canopen_simu/SDO_config");
}
}
else
{
ROS_ERROR_STREAM("SimulatorBase: can't read xml-file " << config_file);
}
}
catch(const std::exception & exc)
{
ROS_ERROR_STREAM("SimulatorBase::readSDOconfig("<< config_file << ") failed: exception " << exc.what());
}
return false;
}
/*
* reads the PDO configuration from xml-file
*
* @param[in] config_file configuration file with testcases for simulation
* @param[in] sick_device_family "OLS10", OLS20" or "MLS"
*/
template<class MsgType>
bool sick_canopen_simu::SimulatorBase<MsgType>::readPDOconfig(const std::string & config_file, const std::string & sick_device_family)
{
try
{
TiXmlDocument xml_config(config_file);
if(xml_config.LoadFile())
{
ROS_INFO_STREAM("SimulatorBase::readPDOconfig(): reading PDO map from xml-file " << config_file);
TiXmlElement* xml_device_config = xml_config.FirstChild("sick_canopen_simu")->FirstChild(sick_device_family)->ToElement();
if(!xml_device_config)
{
ROS_ERROR_STREAM("SimulatorBase::readPDOconfig(" << config_file << "): element sick_canopen_simu/" << sick_device_family << " not found");
return false;
}
TiXmlElement* xml_pdo_config = xml_device_config->FirstChildElement("PDO_config")->ToElement();
if(xml_pdo_config)
{
TiXmlElement* xml_pdo = xml_pdo_config->FirstChildElement("pdo");
while(xml_pdo)
{
parseXmlPDO(xml_pdo);
xml_pdo = xml_pdo->NextSiblingElement("pdo");
ROS_DEBUG_STREAM("SimulatorBase::readPDOconfig(): " << sick_line_guidance::MsgUtil::toInfo(m_vec_pdo_measurements.back()));
}
if(!m_vec_pdo_measurements.empty())
{
ROS_INFO_STREAM("SimulatorBase::readPDOconfig(" << config_file << "): " << m_vec_pdo_measurements.size() << " pdo elements found");
return true;
}
else
{
ROS_ERROR_STREAM("SimulatorBase::readPDOconfig(" << config_file << "): no pdo elements found");
}
}
else
{
ROS_ERROR_STREAM("SimulatorBase::readPDOconfig(" << config_file << "): can't read element sick_canopen_simu/" << sick_device_family << "/PDO_config");
}
}
else
{
ROS_ERROR_STREAM("SimulatorBase::readPDOconfig(): can't read xml-file " << config_file);
}
}
catch(const std::exception & exc)
{
ROS_ERROR_STREAM("SimulatorBase::readPDOconfig("<< config_file << ") failed: exception " << exc.what());
}
return false;
}
/*
* @brief handles SDOs, i.e. sends a SDO response and returns true, if can frame is a SDO request.
* Otherwise, the can frame is not handled and false is returned.
* Note: The SDO response is simply taken from a lookup-table (input: SDO request frame data, output: SDO response frame data).
* If the SDO request frame data (frame.data) is not found in the lookup-table, the can frame is not handled and false is returned, too.
*
* @param[in] msg_in received can frame
*
* @return true if can frame was handled and a SDO request is sent, false otherwise.
*/
template<class MsgType>
bool sick_canopen_simu::SimulatorBase<MsgType>::handleSDO(const can_msgs::Frame & msg_in)
{
// can id == (0x600+$NODEID) with 8 byte data => SDO request => send SDO response (0x580+$NODEID)#...
if(msg_in.id == 0x600 + m_can_node_id && msg_in.dlc == 8)
{
boost::lock_guard<boost::mutex> publish_lockguard(m_ros_publisher_mutex);
can_msgs::Frame msg_out = msg_in;
uint64_t sdo_request_data = frameDataToUInt64(msg_in); // convert msg_in.data to uint64_t
uint64_t sdo_response_data = m_sdo_request_response_map[sdo_request_data]; // lookup table: sdo response data := m_sdo_request_response_map[<sdo request data>]
if(sdo_response_data != 0) // SDO response found in lookup table
{
// send SDO response (0x580+$NODEID)#<sdo_response_data>
msg_out.id = 0x580 + m_can_node_id;
uint64ToFrameData(sdo_response_data, msg_out);
msg_out.header.stamp = ros::Time::now();
m_ros_publisher.publish(msg_out);
ROS_INFO_STREAM("SimulatorBase::handleSDO(): SDO request " << tostring(msg_in) << " handled, SDO response " << tostring(msg_out) << " published.");
return true;
}
else if( (sdo_response_data = m_sdo_request_response_map[(sdo_request_data & 0xFFFFFFFF00000000ULL)]) != 0)
{
// Handle SDO response for a download request, f.e. SDO request = 0x23022003CDCC4C3D = 0x2302200300000000 + <4 byte data>:
// Set value 0xCDCC4C3D in object [2002sub03] -> SDO response = 0x6002200300000000, SDO request = 0x4002200300000000 -> SDO response = 0x43022003CDCC4C3D
uint64_t sdo_object_index = (sdo_request_data & 0x00FFFFFF00000000ULL);
uint64_t sdo_object_value = (sdo_request_data & 0x00000000FFFFFFFFULL);
uint64_t sdo_parameter_bytes = (sdo_request_data & 0x0F00000000000000ULL);
m_sdo_request_response_map[0x4000000000000000 | sdo_object_index] = (0x4000000000000000 | sdo_parameter_bytes | sdo_object_index | sdo_object_value);
// send SDO response (0x580+$NODEID)#<sdo_response_data>
msg_out.id = 0x580 + m_can_node_id;
uint64ToFrameData(sdo_response_data, msg_out);
msg_out.header.stamp = ros::Time::now();
m_ros_publisher.publish(msg_out);
ROS_INFO_STREAM("SimulatorBase::handleSDO(): SDO request " << tostring(msg_in) << " handled, SDO response " << tostring(msg_out) << " published.");
return true;
}
else
{
ROS_ERROR_STREAM("SimulatorBase::handleSDO(): SDO " << tostring(msg_in) << " not handled");
return false;
}
}
// Ignore SDO requests (0x600+$NODEID) or SDO responses (0x580+$NODEID) from other can devices
if((msg_in.id >= 0x600 && msg_in.id <= 0x67F) || (msg_in.id >= 0x580 && msg_in.id <= 0x5FF))
{
return true;
}
return false;
}
/*
* @brief Publishes PDOs to simulate a MLS or OLS20 device while can state is operational
*/
template<class MsgType>
void sick_canopen_simu::SimulatorBase<MsgType>::runPDOthread(void)
{
size_t pdo_cnt = 0;
while(ros::ok() && m_pdo_publisher_thread_running)
{
m_pdo_rate.sleep(); // ros::Duration(0.01).sleep();
if(m_can_state.state() == OPERATIONAL || m_send_tpdo_immediately ) // OLS10, MLS: send TPDOs immediately in all states (pre-operational and operational), OLS20: send TPDOs only in state operational
{
// Note: Depending on the transmission type, PDOs are transmitted either asynchronously or synchronously (https://www.canopensolutions.com/english/about_canopen/pdo.shtml):
// "Asynchronous PDOs are event-controlled ... when at least one of the process variables mapped in a PDO is altered, for example an input value, the PDO is immediately transmitted."
// "Synchronous PDOs are only transmitted after prior reception of a synchronization message (Sync Object)".
// Default transmission type for OLS and MLS is 0xFF (asynchronous).
boost::lock_guard<boost::mutex> publish_lockguard(m_ros_publisher_mutex);
can_msgs::Frame tpdo_msg[2];
int measurement_idx = ((pdo_cnt/m_pdo_repeat_cnt) % (m_vec_pdo_measurements.size())); // simulate a different measurement every 500 milliseconds (25 times, 20 milliseconds each)
MsgType & pdo_measurement = m_vec_pdo_measurements[measurement_idx];
// convert sensor measurement to can frames
int tpdo_msg_cnt = convertToCanFrame(pdo_measurement, tpdo_msg[0], tpdo_msg[1]);
// send can frames
for(int n = 0; n < tpdo_msg_cnt; n++)
{
tpdo_msg[n].header.stamp = ros::Time::now();
m_ros_publisher.publish(tpdo_msg[n]);
ros::Duration(0.001).sleep();
ROS_INFO_STREAM("SimulatorBase::runPDOthread(): pdo frame " << tostring(tpdo_msg[n]) << " published.");
}
// Notify all registered listener
for(typename std::vector<SimulationListener*>::iterator iter = m_vec_simu_listener.begin(); iter != m_vec_simu_listener.end(); iter++)
{
(*iter)->pdoSent(pdo_measurement);
}
pdo_cnt++;
}
}
m_pdo_publisher_thread_running = false;
}
/*
* @brief Convertes an MLS_Measurement into a can_msgs::Frame TPDO1.
* @param[in] measurement MLS_Measurement to be converted
* @param[out] tpdo1_msg output can frame TPDO1,
* @param[out] tpdo2_msg dummy frame TPDO2, unused
* @return Returns the number of TPDOs, i.e. 1 for MLS devices.
*/
template<class MsgType>
int sick_canopen_simu::SimulatorBase<MsgType>::convertToCanFrame(const sick_line_guidance::MLS_Measurement & measurement, can_msgs::Frame & tpdo1_msg, can_msgs::Frame & tpdo2_msg)
{
assert(tpdo1_msg.data.size() == 8);
assert(measurement.position.size() == 3);
// convert position and width in meter to lcp and width in millimeter
int lcp1 = convertLCP(measurement.position[0]);
int lcp2 = convertLCP(measurement.position[1]);
int lcp3 = convertLCP(measurement.position[2]);
// set TPDO1: (180+$NODEID)#<8 byte data>
tpdo1_msg.id = 0x180 + m_can_node_id;
tpdo1_msg.dlc = 8;
tpdo1_msg.is_error = false;
tpdo1_msg.is_rtr = false;
tpdo1_msg.is_extended = false;
tpdo1_msg.data[0] = (lcp1 & 0xFF); // LSB LCP1
tpdo1_msg.data[1] = ((lcp1 >> 8) & 0xFF); // MSB LCP1
tpdo1_msg.data[2] = (lcp2 & 0xFF); // LSB LCP2
tpdo1_msg.data[3] = ((lcp2 >> 8) & 0xFF); // MSB LCP2
tpdo1_msg.data[4] = (lcp3 & 0xFF); // LSB LCP3
tpdo1_msg.data[5] = ((lcp3 >> 8) & 0xFF); // MSB LCP3
tpdo1_msg.data[6] = measurement.lcp;
tpdo1_msg.data[7] = measurement.status;
tpdo1_msg.header.stamp = ros::Time::now();
// set error register 1001 in object dictionary
uint32_t sdo_data_error = ((measurement.error & 0xFFUL) << 24);
m_sdo_request_response_map[0x4001100000000000] = (0x4F01100000000000 | (sdo_data_error & 0xFF000000ULL)); // measurement.error -> set 0x1001 in object dictionary
return 1; // one TPDO set
}
/*
* @brief Convertes an OLS_Measurement into two can_msgs::Frame TPDO1 and TPDO2.
* @param[in] measurement OLS_Measurement to be converted
* @param[out] tpdo1_msg output can frame TPDO1, Byte 1-8 := LCP1(LSB,MSB,0x2021sub1), LCP2(LSB,MSB,0x2021sub2), LCP3(LSB,MSB,0x2021sub3), status(UINT8,0x2021sub4), barcode(UINT8,0x2021sub8)
* @param[out] tpdo2_msg output can frame TPDO2, Byte 1-6 := Width1(LSB,MSB,0x2021sub5), Width2(LSB,MSB,0x2021sub6), Width3(LSB,MSB,0x2021sub7)
* @return Returns the number of TPDOs, i.e. 2 for OLS devices.
*/
template<class MsgType>
int sick_canopen_simu::SimulatorBase<MsgType>::convertToCanFrame(const sick_line_guidance::OLS_Measurement & measurement, can_msgs::Frame & tpdo1_msg, can_msgs::Frame & tpdo2_msg)
{
assert(tpdo1_msg.data.size() == 8);
assert(tpdo2_msg.data.size() == 8);
assert(measurement.position.size() == 3);
assert(measurement.width.size() == 3);
assert(revertByteorder<uint32_t>(0x12345678) == 0x78563412);
// convert position and width in meter to lcp and width in millimeter
int lcp1 = convertLCP(measurement.position[0]);
int lcp2 = convertLCP(measurement.position[1]);
int lcp3 = convertLCP(measurement.position[2]);
int width1 = convertLCP(measurement.width[0]);
int width2 = convertLCP(measurement.width[1]);
int width3 = convertLCP(measurement.width[2]);
// set TPDO1: (180+$NODEID)#<8 byte data>
tpdo1_msg.id = 0x180 + m_can_node_id;
tpdo1_msg.dlc = 8;
tpdo1_msg.is_error = false;
tpdo1_msg.is_rtr = false;
tpdo1_msg.is_extended = false;
tpdo1_msg.data[0] = (lcp1 & 0xFF); // LSB LCP1
tpdo1_msg.data[1] = ((lcp1 >> 8) & 0xFF); // MSB LCP1
tpdo1_msg.data[2] = (lcp2 & 0xFF); // LSB LCP2
tpdo1_msg.data[3] = ((lcp2 >> 8) & 0xFF); // MSB LCP2
tpdo1_msg.data[4] = (lcp3 & 0xFF); // LSB LCP3
tpdo1_msg.data[5] = ((lcp3 >> 8) & 0xFF); // MSB LCP3
tpdo1_msg.data[6] = measurement.status;
tpdo1_msg.data[7] = measurement.barcode;
tpdo1_msg.header.stamp = ros::Time::now();
// set TPDO2: (280+$NODEID)#<6 byte data>
tpdo2_msg.id = 0x280 + m_can_node_id;
tpdo2_msg.dlc = 6;
tpdo2_msg.is_error = false;
tpdo2_msg.is_rtr = false;
tpdo2_msg.is_extended = false;
tpdo2_msg.data[0] = (width1 & 0xFF); // LSB width1
tpdo2_msg.data[1] = ((width1 >> 8) & 0xFF); // MSB width1
tpdo2_msg.data[2] = (width2 & 0xFF); // LSB width2
tpdo2_msg.data[3] = ((width2 >> 8) & 0xFF); // MSB width2
tpdo2_msg.data[4] = (width3 & 0xFF); // LSB width3
tpdo2_msg.data[5] = ((width3 >> 8) & 0xFF); // MSB width3
tpdo2_msg.data[6] = 0;
tpdo2_msg.data[7] = 0;
tpdo2_msg.header.stamp = ros::Time::now();
// set objects in dictionary
uint32_t sdo_data_error = ((measurement.error & 0xFFUL) << 24);
uint32_t sdo_data_dev_status = ((measurement.dev_status & 0xFFUL) << 24);
uint32_t sdo_data_extended_code = revertByteorder<uint32_t>(measurement.extended_code);
m_sdo_request_response_map[0x4001100000000000] = (0x4F01100000000000 | (sdo_data_error & 0xFF000000ULL)); // measurement.error -> set 0x1001 in object dictionary
m_sdo_request_response_map[0x4018200000000000] = (m_sdo_response_dev_state | (sdo_data_dev_status & 0xFF000000ULL)); // measurement.dev_status -> set 0x2018 in object dictionary (OLS20: UINT8, OLS10: UINT16)
m_sdo_request_response_map[0x4021200900000000] = (0x4321200900000000 | (sdo_data_extended_code & 0xFFFFFFFFULL)); // measurement.extended_code -> set 0x2021sub9 in object dictionary
// OLS20 only: simulate barcode center point, object 0x2021subA (INT16), OLS10: always 0
int16_t barcodecenter = (int16_t)(measurement.barcode_center_point * 1000);
uint32_t sdo_data = ((barcodecenter & 0xFFUL) << 24) | ((barcodecenter & 0xFF00UL) << 8);
m_sdo_request_response_map[0x4021200A00000000] = (0x4B21200A00000000 | (sdo_data & 0xFFFF0000ULL));
// OLS20 only: simulate quality of lines, object 0x2021subB (UINT8), OLS10: always 0
sdo_data = ((measurement.quality_of_lines & 0xFFUL) << 24);
m_sdo_request_response_map[0x4021200B00000000] = (0x4F21200B00000000 | (sdo_data & 0xFF000000ULL));
// OLS20 only: simulate intensity of lines, object 0x2023sub1 to 0x2023sub3 (UINT8), OLS10: always 0
sdo_data = ((measurement.intensity_of_lines[0] & 0xFFUL) << 24);
m_sdo_request_response_map[0x4023200100000000] = (0x4F23200100000000 | (sdo_data & 0xFF000000ULL));
sdo_data = ((measurement.intensity_of_lines[1] & 0xFFUL) << 24);
m_sdo_request_response_map[0x4023200200000000] = (0x4F23200200000000 | (sdo_data & 0xFF000000ULL));
sdo_data = ((measurement.intensity_of_lines[2] & 0xFFUL) << 24);
m_sdo_request_response_map[0x4023200300000000] = (0x4F23200300000000 | (sdo_data & 0xFF000000ULL));
return 2; // both TPDOs set
}
/*
* @brief Converts a position or width (float value in meter) to lcp (INT16 value in millimeter),
* shortcut for std::round(lcp * 1000) with clipping to range INT16_MIN ... INT16_MAX.
* @param[in] lcp position or width (float value in meter)
* @return INT16 value in millimeter
*/
template<class MsgType>
int sick_canopen_simu::SimulatorBase<MsgType>::convertLCP(float lcp)
{
return boost::algorithm::clamp((int)(std::round(lcp * 1000)), INT16_MIN, INT16_MAX);
}
/*
* @brief Converts frame.data to uint64_t
* @param[in] frame can frame
* @return frame.data converted to uint64_t
*/
template<class MsgType>
uint64_t sick_canopen_simu::SimulatorBase<MsgType>::frameDataToUInt64(const can_msgs::Frame & frame)
{
assert(frame.data.size() == 8);
uint64_t u64data = 0;
for(size_t n = 0; n < std::min(frame.data.size(), (size_t)(frame.dlc & 0xFF)); n++)
{
u64data = ((u64data << 8) | (frame.data[n] & 0xFF));
}
return u64data;
}
/*
* @brief Converts uint64_t data to frame.data
* @param[in] u64data frame data (uint64_t)
* @param[in+out] frame can frame
*/
template<class MsgType>
void sick_canopen_simu::SimulatorBase<MsgType>::uint64ToFrameData(uint64_t u64data, can_msgs::Frame & frame)
{
assert(frame.data.size() == 8);
frame.dlc = 8;
for(int n = frame.data.size() - 1; n >= 0; n--)
{
frame.data[n] = (u64data & 0xFF);
u64data = (u64data >> 8);
}
}
/*
* @brief prints and returns a can_msgs::Frame in short format like candump (f.a. "18A#B4FFCCFF00000300")
*/
template<class MsgType>
std::string sick_canopen_simu::SimulatorBase<MsgType>::tostring(const can_msgs::Frame & canframe)
{
std::stringstream str;
str << std::uppercase << std::hex << std::setfill('0') << std::setw(3) << (unsigned)(canframe.id) << "#";
for(size_t n = 0; n < std::min((size_t)(canframe.dlc & 0xFF), canframe.data.size()); n++)
str << std::uppercase << std::hex << std::setfill('0') << std::setw(2) << (unsigned)(canframe.data[n] & 0xFF);
return str.str();
}
/*
* Subclass MLSSimulator extends class SimulatorBase to simulate a MLS device.
*
*/
/*
* Constructor
*
* @param[in] nh ros node handle
* @param[in] config_file configuration file with testcases for OLS and MLS simulation
* @param[in] sick_device_family "OLS10", "OLS20" or "MLS"
* @param[in] can_node_id node id of OLS or MLS, default: 0x0A
* @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0"
* @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0"
* @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second)
* @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times)
* @param[in] subscribe_queue_size buffer size for ros messages
*/
sick_canopen_simu::MLSSimulator::MLSSimulator(ros::NodeHandle & nh, const std::string & config_file, const std::string & sick_device_family, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size)
: SimulatorBase(nh, config_file, can_node_id, subscribe_topic, publish_topic, pdo_rate, pdo_repeat_cnt, subscribe_queue_size)
{
m_send_tpdo_immediately = true; // true (OLS10, MLS): send TPDOs immediately in all states (pre-operational and operational), false (default, OLS20): send TPDOs only in state operational
m_ros_subscriber = nh.subscribe(subscribe_topic, m_subscribe_queue_size, &sick_canopen_simu::MLSSimulator::messageHandler, this);
if(!readPDOconfig(config_file, sick_device_family))
{
ROS_ERROR_STREAM("MLSSimulator: readPDOconfig(" << config_file << ") failed");
}
// Start thread for publishing PDOs
m_pdo_publisher_thread_running = true;
m_pdo_publisher_thread = new boost::thread(&sick_canopen_simu::MLSSimulator::runPDOthread, this);
}
/*
* @brief Callbacks for ros messages. Converts incoming messages of type can_msgs::Frame to simulate a MLS device
* and publishes simulation results to the configured ros topic.
*
* param[in] msg ros message of type can_msgs::Frame
*/
void sick_canopen_simu::MLSSimulator::messageHandler(const can_msgs::Frame & msg_in)
{
SimulatorBase::messageHandler(msg_in);
}
/*
* @brief Parses an pdo element from config file and appends it to m_vec_pdo_measurements.
*
* param[in] xml_pdo pdo element from config file
*/
bool sick_canopen_simu::MLSSimulator::parseXmlPDO(TiXmlElement* xml_pdo)
{
try
{
m_vec_pdo_measurements.push_back(sick_line_guidance::MsgUtil::convertMLSMessage(
std::stof(xml_pdo->Attribute("lcp1")), std::stof(xml_pdo->Attribute("lcp2")), std::stof(xml_pdo->Attribute("lcp3")),
std::stoul(xml_pdo->Attribute("status"), 0, 0),
std::stoul(xml_pdo->Attribute("lcp"), 0, 0),
std::stoul(xml_pdo->Attribute("error"), 0, 0),
xml_pdo->Attribute("frame_id")));
return true;
}
catch(const std::exception & exc)
{
ROS_ERROR_STREAM("MLSSimulator::parseXmlPDO() failed: exception " << exc.what());
}
return false;
}
/*
* Subclass OLSSimulator extends class SimulatorBase to simulate an OLS devices.
*
*/
/*
* Constructor
*
* @param[in] nh ros node handle
* @param[in] config_file configuration file with testcases for OLS and MLS simulation
* @param[in] sick_device_family "OLS10", "OLS20" or "MLS"
* @param[in] can_node_id node id of OLS, default: 0x0A
* @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0"
* @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0"
* @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second)
* @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times)
* @param[in] subscribe_queue_size buffer size for ros messages
*/
sick_canopen_simu::OLSSimulator::OLSSimulator(ros::NodeHandle & nh, const std::string & config_file, const std::string & sick_device_family, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size)
: SimulatorBase(nh, config_file, can_node_id, subscribe_topic, publish_topic, pdo_rate, pdo_repeat_cnt, subscribe_queue_size)
{
m_ros_subscriber = nh.subscribe(subscribe_topic, m_subscribe_queue_size, &sick_canopen_simu::OLSSimulator::messageHandler, this);
if(!readPDOconfig(config_file, sick_device_family))
{
ROS_ERROR_STREAM("OLSSimulator: readPDOconfig(" << config_file << ") failed");
}
// Start thread for publishing PDOs
m_pdo_publisher_thread_running = true;
m_pdo_publisher_thread = new boost::thread(&sick_canopen_simu::OLSSimulator::runPDOthread, this);
}
/*
* @brief Callbacks for ros messages. Converts incoming messages of type can_msgs::Frame to simulate an OLS device
* and publishes simulation results to the configured ros topic.
*
* param[in] msg ros message of type can_msgs::Frame
*/
void sick_canopen_simu::OLSSimulator::messageHandler(const can_msgs::Frame & msg_in)
{
SimulatorBase::messageHandler(msg_in);
}
/*
* @brief Parses an pdo element from config file and appends it to m_vec_pdo_measurements.
*
* param[in] xml_pdo pdo element from config file
*/
bool sick_canopen_simu::OLSSimulator::parseXmlPDO(TiXmlElement* xml_pdo)
{
try
{
m_vec_pdo_measurements.push_back(sick_line_guidance::MsgUtil::convertOLSMessage(
std::stof(xml_pdo->Attribute("lcp1")), std::stof(xml_pdo->Attribute("lcp2")), std::stof(xml_pdo->Attribute("lcp3")),
std::stof(xml_pdo->Attribute("width1")), std::stof(xml_pdo->Attribute("width2")), std::stof(xml_pdo->Attribute("width3")),
std::stoul(xml_pdo->Attribute("status"), 0, 0),
std::stoul(xml_pdo->Attribute("barcode"), 0, 0),
std::stoul(xml_pdo->Attribute("devstatus"), 0, 0),
std::stoul(xml_pdo->Attribute("error"), 0, 0),
std::stof(xml_pdo->Attribute("barcodecenter")), // OLS20 only: simulate barcode center point, object 0x2021subA (INT16), OLS10: always 0
std::stoul(xml_pdo->Attribute("linequality"), 0, 0), // OLS20 only: simulate quality of lines, object 0x2021subB (UINT8), OLS10: always 0
std::stoul(xml_pdo->Attribute("lineintensity1"), 0, 0), // OLS20 only: simulate intensity of lines, object 0x2023sub1 (UINT8), OLS10: always 0
std::stoul(xml_pdo->Attribute("lineintensity2"), 0, 0), // OLS20 only: simulate intensity of lines, object 0x2023sub2 (UINT8), OLS10: always 0
std::stoul(xml_pdo->Attribute("lineintensity3"), 0, 0), // OLS20 only: simulate intensity of lines, object 0x2023sub3 (UINT8), OLS10: always 0
xml_pdo->Attribute("frame_id")));
}
catch(const std::exception & exc)
{
ROS_ERROR_STREAM("OLSSimulator::parseXmlPDO() failed: exception " << exc.what());
}
return false;
}
/*
* Subclass OLS10Simulator extends class OLSSimulator to simulate an OLS10 device.
*
*/
/*
* Constructor
*
* @param[in] nh ros node handle
* @param[in] config_file configuration file with testcases for OLS and MLS simulation
* @param[in] sick_device_family "OLS10", "OLS20" or "MLS"
* @param[in] can_node_id node id of OLS or MLS, default: 0x0A
* @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0"
* @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0"
* @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second)
* @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times)
* @param[in] subscribe_queue_size buffer size for ros messages
*/
sick_canopen_simu::OLS10Simulator::OLS10Simulator(ros::NodeHandle & nh, const std::string & config_file, const std::string & sick_device_family, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size)
: OLSSimulator(nh, config_file, sick_device_family, can_node_id, subscribe_topic, publish_topic, pdo_rate, pdo_repeat_cnt, subscribe_queue_size)
{
m_send_tpdo_immediately = true; // true (OLS10, MLS): send TPDOs immediately in all states (pre-operational and operational), false (default, OLS20): send TPDOs only in state operational
m_sdo_response_dev_state = 0x4B18200000000000; // response to sdo request for dev_status (object 0x2018): MLS and OLS20: 0x4F18200000000000 (sdo response with UINT8 data), OLS10: 0x4B18200000000000 (sdo response with UINT16 data)
}
/*
* Subclass OLS20Simulator extends class OLSSimulator to simulate an OLS20 device.
*
*/
/*
* Constructor
*
* @param[in] nh ros node handle
* @param[in] config_file configuration file with testcases for OLS and MLS simulation
* @param[in] sick_device_family "OLS10", "OLS20" or "MLS"
* @param[in] can_node_id node id of OLS or MLS, default: 0x0A
* @param[in] subscribe_topic ros topic to receive input messages of type can_msgs::Frame, default: "can0"
* @param[in] publish_topic ros topic to publish output messages of type can_msgs::Frame, default: "ros2can0"
* @param[in] pdo_rate rate of PDO (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second)
* @param[in] pdo_repeat_cnt each sensor state spefied in sick_canopen_simu_cfg.xml is repeated N times before switching to the next state (default: 5 times)
* @param[in] subscribe_queue_size buffer size for ros messages
*/
sick_canopen_simu::OLS20Simulator::OLS20Simulator(ros::NodeHandle & nh, const std::string & config_file, const std::string & sick_device_family, int can_node_id, const std::string & subscribe_topic, const std::string & publish_topic, const ros::Rate & pdo_rate, int pdo_repeat_cnt, int subscribe_queue_size)
: OLSSimulator(nh, config_file, sick_device_family, can_node_id, subscribe_topic, publish_topic, pdo_rate, pdo_repeat_cnt, subscribe_queue_size)
{
m_send_tpdo_immediately = false; // true (OLS10, MLS): send TPDOs immediately in all states (pre-operational and operational), false (default, OLS20): send TPDOs only in state operational
}

148
Devices/Packages/sick_line_guidance/src/sick_canopen_simu_node.cpp Executable file
View File

@@ -0,0 +1,148 @@
/*
* sick_canopen_simu_node: subscribes to ros topic "can0" (message type can_msgs::Frame),
* simulates an OLS20 or MLS device and publishes can_msgs::Frame messages on ros topic "ros2can0"
* for further transmission to the can bus.
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#include <ros/ros.h>
#include "sick_line_guidance/sick_line_guidance_version.h"
#include "sick_line_guidance/sick_canopen_simu_device.h"
#include "sick_line_guidance/sick_canopen_simu_verify.h"
int main(int argc, char** argv)
{
// Setup and configuration
ros::init(argc, argv, "sick_canopen_simu_node");
ros::NodeHandle nh;
int can_node_id = 0x0A; // default node id for OLS and MLS
int can_message_queue_size = 16; // buffer size for ros messages
int sensor_state_queue_size = 2; // buffer size for simulated sensor states
double pdo_rate = 50; // rate of PDOs (default: pdo_rate = 50, i.e. 20 ms between two PDOs or 50 PDOs per second)
int pdo_repeat_cnt = 25; // each sensor state spefied in sick_canopen_simu_cfg.xml is repeated 25 times before switching to the next state (sensor state changes after 0.5 seconds)
std::string sick_device_family = "OLS20"; // simulation of OLS10, OLS20 or MLS device
std::string can_subscribe_topic = "can0", publish_topic = "ros2can0"; // default can topics
std::string mls_subscribe_topic = "mls", ols_subscribe_topic = "ols"; // default measurement topics
std::string sick_canopen_simu_cfg_file = "sick_canopen_simu_cfg.xml"; // configuration file and testcases for OLS and MLS simulation
nh.param("sick_canopen_simu_cfg_file", sick_canopen_simu_cfg_file, sick_canopen_simu_cfg_file);
nh.param("can_node_id", can_node_id, can_node_id);
nh.param("sick_device_family", sick_device_family, sick_device_family);
nh.param("can_subscribe_topic", can_subscribe_topic, can_subscribe_topic);
nh.param("mls_subscribe_topic", mls_subscribe_topic, mls_subscribe_topic);
nh.param("ols_subscribe_topic", ols_subscribe_topic, ols_subscribe_topic);
nh.param("publish_topic", publish_topic, publish_topic);
nh.param("pdo_rate", pdo_rate, pdo_rate);
nh.param("pdo_repeat_cnt", pdo_repeat_cnt, pdo_repeat_cnt);
nh.param("can_message_queue_size", can_message_queue_size, can_message_queue_size);
nh.param("sensor_state_queue_size", sensor_state_queue_size, sensor_state_queue_size);
ROS_INFO_STREAM("sick_canopen_simu_node: version " << sick_line_guidance::Version::getVersionInfo());
ROS_INFO_STREAM("sick_canopen_simu_node: initializing...");
sick_canopen_simu::MLSMeasurementVerification* mls_measurement_verification = 0;
sick_canopen_simu::OLSMeasurementVerification* ols_measurement_verification = 0;
sick_canopen_simu::MLSSimulator* mls_simulator = 0;
sick_canopen_simu::OLSSimulator* ols_simulator = 0;
if(sick_device_family == "MLS")
{
// Init MLS simulation
mls_simulator = new sick_canopen_simu::MLSSimulator(nh, sick_canopen_simu_cfg_file, sick_device_family, can_node_id, can_subscribe_topic, publish_topic, ros::Rate(pdo_rate), pdo_repeat_cnt,can_message_queue_size);
ROS_INFO_STREAM("sick_canopen_simu_node: MLSSimulator started, can node_id " << can_node_id << ", listening to can topic \""
<< can_subscribe_topic << "\", measurement topic \"" << mls_subscribe_topic << "\", publishing on ros topic \"" << publish_topic << "\" ...");
mls_measurement_verification = new sick_canopen_simu::MLSMeasurementVerification(nh, mls_subscribe_topic, sensor_state_queue_size, sick_device_family);
mls_simulator->registerSimulationListener(mls_measurement_verification);
}
else if(sick_device_family == "OLS10")
{
// Init OLS10 simulation
ols_simulator = new sick_canopen_simu::OLS10Simulator(nh, sick_canopen_simu_cfg_file, sick_device_family, can_node_id, can_subscribe_topic, publish_topic, ros::Rate(pdo_rate), pdo_repeat_cnt, can_message_queue_size);
ROS_INFO_STREAM("sick_canopen_simu_node: OLS10Simulator started, can node_id " << can_node_id << ", listening to can topic \""
<< can_subscribe_topic << "\", measurement topic \"" << ols_subscribe_topic << "\", publishing on ros topic \"" << publish_topic << "\" ...");
ols_measurement_verification = new sick_canopen_simu::OLSMeasurementVerification(nh, ols_subscribe_topic, sensor_state_queue_size, sick_device_family);
ols_simulator->registerSimulationListener(ols_measurement_verification);
}
else if(sick_device_family == "OLS20")
{
// Init OLS20 simulation
ols_simulator = new sick_canopen_simu::OLS20Simulator(nh, sick_canopen_simu_cfg_file, sick_device_family, can_node_id, can_subscribe_topic, publish_topic, ros::Rate(pdo_rate), pdo_repeat_cnt, can_message_queue_size);
ROS_INFO_STREAM("sick_canopen_simu_node: OLS20Simulator started, can node_id " << can_node_id << ", listening to can topic \""
<< can_subscribe_topic << "\", measurement topic \"" << ols_subscribe_topic << "\", publishing on ros topic \"" << publish_topic << "\" ...");
ols_measurement_verification = new sick_canopen_simu::OLSMeasurementVerification(nh, ols_subscribe_topic, sensor_state_queue_size, sick_device_family);
ols_simulator->registerSimulationListener(ols_measurement_verification);
}
else
{
ROS_ERROR_STREAM("sick_canopen_simu_node: sick_device_family \"" << sick_device_family << "\" not supported, aborting ...");
return 1;
}
// Run ros event loop
ROS_INFO_STREAM("sick_canopen_simu_node: running...");
ros::spin();
std::cout << "sick_canopen_simu_node: exiting..." << std::endl;
ROS_INFO_STREAM("sick_canopen_simu_node: exiting...");
if(mls_simulator)
{
mls_simulator->unregisterSimulationListener(mls_measurement_verification);
mls_measurement_verification->printStatistic();
delete(mls_measurement_verification);
delete(mls_simulator);
}
if(ols_simulator)
{
ols_simulator->unregisterSimulationListener(ols_measurement_verification);
ols_measurement_verification->printStatistic();
delete(ols_measurement_verification);
delete(ols_simulator);
}
return 0;
}

320
Devices/Packages/sick_line_guidance/src/sick_canopen_simu_verify.cpp Executable file
View File

@@ -0,0 +1,320 @@
/*
* sick_canopen_simu_verify tests and verifies the sick_line_guidance ros driver.
*
* sick_canopen_simu_verify listenes to both the simulation (interface sick_canopen_simu::SimulatorBase::SimulationListener)
* and to the ros messages of the sick_line_guidance driver. Whenever a MLS_Measurement or OLS_Measurement message
* is received, the measurement is compared to the current sensor state of the simulation.
* Measurement messages from the driver and sensor states from the simulation should be identical, otherwise an error occured.
* The sick_line_guidance driver test is passed, if no error occured (i.e. all measurement messages from the driver have been
* handled correctly, no mismatches between simulated sensor states and published measurement messages).
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#include <ros/ros.h>
#include "sick_line_guidance/sick_canopen_simu_verify.h"
#include "sick_line_guidance/sick_line_guidance_msg_util.h"
#include "sick_line_guidance/sick_canopen_simu_compare.h"
/*
* Constructor
*
* @param[in] nh ros node handle
* @param[in] measurement_subscribe_topic ros topic for measurement messages, default: "mls" resp. "ols"
* @param[in] sensor_state_queue_size buffer size for simulated sensor states, default: 2
* @param[in] devicename descriptional device name, f.e. "OLS20" or "MLS"
*/
template<class MsgType>
sick_canopen_simu::MeasurementVerification<MsgType>::MeasurementVerification(ros::NodeHandle & nh, const std::string & measurement_subscribe_topic, int sensor_state_queue_size, const std::string & devicename)
{
m_devicename = devicename;
m_sensor_states.resize(sensor_state_queue_size);
m_measurement_messages.resize(sensor_state_queue_size);
m_measurement_verification_error_cnt = 0;
m_measurement_verification_ignored_cnt = 0;
m_measurement_verification_failed = 0;
m_measurement_verification_jitter = 4; // max. 4 consecutive errors tolerated, since measurement messages can be sent while a SDO response is still pending (OLS20: up to 9 SDO queries required per TPDO measurement)
m_measurement_messages_cnt = -sensor_state_queue_size; // start verification after the first two measurements
for(typename std::list<MsgType>::iterator iter = m_sensor_states.begin(); iter != m_sensor_states.end(); iter++)
sick_line_guidance::MsgUtil::zero(*iter);
for (typename std::list<MsgType>::iterator iter = m_measurement_messages.begin(); iter != m_measurement_messages.end(); iter++)
sick_line_guidance::MsgUtil::zero(*iter);
}
/*
* Destructor
*
*/
template<class MsgType>
sick_canopen_simu::MeasurementVerification<MsgType>::~MeasurementVerification()
{
}
/*
* @brief Implements the notification callback for SimulationListener. Whenever the simulation sends a PDO,
* this function is called by the simulation to notify SimulationListeners about the current sensor state.
*/
template<class MsgType>
void sick_canopen_simu::MeasurementVerification<MsgType>::pdoSent(const MsgType & sensor_state)
{
ROS_INFO_STREAM("MeasurementVerification::pdoSent(" << sick_line_guidance::MsgUtil::toInfo(sensor_state) << ")");
// push sensor_state to m_sensor_states (const list size, pop first element and push new sensor state at the back)
boost::lock_guard<boost::mutex> state_lockguard(m_sensor_states_mutex);
m_sensor_states.pop_front();
m_sensor_states.push_back(sensor_state);
}
/*
* @brief Prints the number of verified measuremente and the number of verification failures.
* @return true in case of no verification failures (all measurements verified successfully), false otherwise.
*/
template<class MsgType>
bool sick_canopen_simu::MeasurementVerification<MsgType>::printStatistic(void)
{
std::stringstream message;
if(m_measurement_messages_cnt > 0 && m_measurement_verification_error_cnt > 0)
{
message << m_devicename << " MeasurementVerificationStatistic failures: " << m_measurement_verification_error_cnt << " of " << m_measurement_messages_cnt << " measurements failed, "
<< std::fixed << std::setprecision(2) << (m_measurement_verification_error_cnt*100.00/m_measurement_messages_cnt) << " % errors, " << m_measurement_verification_ignored_cnt << " measurements ignored.";
std::cerr << message.str() << std::endl;
ROS_ERROR_STREAM(message.str());
}
else if(m_measurement_messages_cnt > 0)
{
message << m_devicename << " MeasurementVerificationStatistic okay: " << m_measurement_verification_failed << " of " << m_measurement_messages_cnt << " measurements failed, " << m_measurement_verification_ignored_cnt << " measurements ignored.";
std::cout << message.str() << std::endl;
ROS_INFO_STREAM(message.str());
}
return (m_measurement_verification_error_cnt == 0);
}
/*
* @brief Callback for measurement messages, called whenever the sick_line_guidance ros driver publishes a
* new measurement message. Compares the measurement message with the sensor states from simulation,
* and reports an error, if an equivalent sensor state hasn't been sent by the simulation.
*
* @param[in] measurement measurement message from sick_line_guidance ros driver
*
* @return true, if the measurement message is equivalent to sensor states (verification passed), or false otherwise (verification failed).
*/
template<class MsgType>
bool sick_canopen_simu::MeasurementVerification<MsgType>::verifyMeasurement(const MsgType & measurement)
{
ROS_INFO_STREAM(m_devicename << " MeasurementVerification::verifyMeasurement(" << sick_line_guidance::MsgUtil::toInfo(measurement) << ")");
// push measurement to m_measurement_messages (const list size, pop first element and push new measurement at the back)
boost::lock_guard<boost::mutex> state_lockguard(m_sensor_states_mutex);
m_measurement_messages.pop_front();
m_measurement_messages.push_back(measurement);
bool measurement_verified = true;
if(m_measurement_messages_cnt > 0) // start verification after 2 measurements
{
measurement_verified = verifyMeasurements(m_measurement_messages, m_sensor_states);
if(measurement_verified)
{
ROS_INFO_STREAM(m_devicename << " MeasurementVerification::verifyMeasurement(" << sick_line_guidance::MsgUtil::toInfo(measurement) << ") succeeded.");
m_measurement_verification_failed = 0;
}
else
{
std::stringstream errormsg;
errormsg << m_devicename << " MeasurementVerification::verifyMeasurement(" << sick_line_guidance::MsgUtil::toInfo(measurement) << ") failed.";
for (typename std::list<MsgType>::iterator iter_measurement = m_measurement_messages.begin(); iter_measurement != m_measurement_messages.end(); iter_measurement++)
errormsg << m_devicename << " MeasurementVerification: measurement " << sick_line_guidance::MsgUtil::toInfo(*iter_measurement);
for(typename std::list<MsgType>::iterator iter_state = m_sensor_states.begin(); iter_state != m_sensor_states.end(); iter_state++)
errormsg << m_devicename << " MeasurementVerification: sensor state " << sick_line_guidance::MsgUtil::toInfo(*iter_state);
m_measurement_verification_failed++;
if(m_measurement_verification_failed > m_measurement_verification_jitter)
{
m_measurement_verification_error_cnt++; // error: 2 consecutive failures (measurement message different to simulated sensor state)
ROS_ERROR_STREAM(errormsg.str());
}
else
{
m_measurement_verification_ignored_cnt++; // possible error (max. 1 error tolerated, since measurement messages can be sent while a SDO response is still pending)
ROS_WARN_STREAM(errormsg.str());
}
}
}
m_measurement_messages_cnt++;
return measurement_verified;
}
/*
* @brief Compares and verifies MLS measurement messages from ros driver against sensor states from simulation.
*
* @param[in] measurement_messages list of measurement messages from ros driver (to be compared to sensor_states)
* @param[in] sensor_states list of sensor states from simulation (to be compared to measurement_messages)
*
* @return true, if the measurement messages are equivalent to sensor states (verification passed), or false otherwise (verification failed).
*/
template<class MsgType>
bool sick_canopen_simu::MeasurementVerification<MsgType>::verifyMeasurements(std::list<sick_line_guidance::MLS_Measurement> & measurement_messages, std::list<sick_line_guidance::MLS_Measurement> & sensor_states)
{
// If the sensor state changed between two PDOs, sensor data from PDOs (position and width) might represent the new state, while sensor data from SDOs (barcode)
// are still from the previous sensor state (SDO still pending or SDO response not yet received). Or vice versa, after receiving the PDOs, the simuulation
// switches to a new state, which is returned by the next SDO (but before the new state is published by a new PDO). In this case, measurement verification
// might fail exactly once when SDO based sensor data (barcodes) are switched right between two PDOs. To avoid verification problems when entries in the
// object dictionary are modified between two PDOs, we compare the current and the last sensor state. One of them always has to be verified, either the
// current or the previous state. This way, we tolerate a 10-milliseconds-jitter in our verification.
sick_canopen_simu::MeasurementComparator<sick_line_guidance::MLS_Measurement> comparator;
return verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpPosition)
&& verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpLcp)
&& verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpStatus)
&& verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpError);
}
/*
* @brief Compares and verifies OLS measurement messages from ros driver against sensor states from simulation.
*
* @param[in] measurement_messages list of measurement messages from ros driver (to be compared to sensor_states)
* @param[in] sensor_states list of sensor states from simulation (to be compared to measurement_messages)
*
* @return true, if the measurement messages are equivalent to sensor states (verification passed), or false otherwise (verification failed).
*/
template<class MsgType>
bool sick_canopen_simu::MeasurementVerification<MsgType>::verifyMeasurements(std::list<sick_line_guidance::OLS_Measurement> & measurement_messages, std::list<sick_line_guidance::OLS_Measurement> & sensor_states)
{
// If the sensor state changed between two PDOs, sensor data from PDOs (position and width) might represent the new state, while sensor data from SDOs (barcode)
// are still from the previous sensor state (SDO still pending or SDO response not yet received). Or vice versa, after receiving the PDOs, the simuulation
// switches to a new state, which is returned by the next SDO (but before the new state is published by a new PDO). In this case, measurement verification
// might fail exactly once when SDO based sensor data (barcodes) are switched right between two PDOs. To avoid verification problems when entries in the
// object dictionary are modified between two PDOs, we compare the current and the last sensor state. One of them always has to be verified, either the
// current or the previous state. This way, we tolerate a 10-milliseconds-jitter in our verification.
sick_canopen_simu::MeasurementComparator<sick_line_guidance::OLS_Measurement> comparator;
return verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpPosition)
&& verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpLinewidth)
&& verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpStatus)
&& verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpBarcode)
&& verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpDevStatus)
&& verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpExtendedCode)
&& verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpError)
&& verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpBarcodeCenter)
&& verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpLineQuality)
&& verifyMeasurementData(measurement_messages, sensor_states, comparator.cmpLineIntensity);
}
/*
* @brief Compares measurement messages from ros driver against sensor states from simulation, using a specified compare function
* (f.e. floating point comparsion for positions with fabs(x-y) < 1 mm and integer comparsion with x == y for barcodes).
*
* @param[in] measurement_messages list of measurement messages from ros driver (to be compared to sensor_states)
* @param[in] sensor_states list of sensor states from simulation (to be compared to measurement_messages)
* @param[in] cmpfunction compare function, called to compare measurement message A to sensor state B
*
* @return true, if the measurement messages are equivalent to sensor states (verification passed), or false otherwise (verification failed).
*/
template<class MsgType> template<typename T>
bool sick_canopen_simu::MeasurementVerification<MsgType>::verifyMeasurementData(std::list<T> & measurement_messages, std::list<T> & sensor_states, bool(*cmpfunction)(const T & A, const T & B))
{
for (typename std::list<T>::iterator iter_measurement = measurement_messages.begin(); iter_measurement != measurement_messages.end(); iter_measurement++)
{
for (typename std::list<T>::iterator iter_state = sensor_states.begin(); iter_state != sensor_states.end(); iter_state++)
{
if (cmpfunction(*iter_state, *iter_measurement))
{
return true;
}
}
}
return false;
}
/*
* Constructor. Subclass MLSMeasurementVerification extends class MeasurementVerification to verify MLS_Measurement messages.
*
* @param[in] nh ros node handle
* @param[in] measurement_subscribe_topic ros topic for measurement messages, default: "mls"
* @param[in] sensor_state_queue_size buffer size for simulated sensor states, default: 2
* @param[in] devicename descriptional device name, f.e. "MLS"
*/
sick_canopen_simu::MLSMeasurementVerification::MLSMeasurementVerification(ros::NodeHandle & nh, const std::string & measurement_subscribe_topic, int sensor_state_queue_size, const std::string & devicename)
: sick_canopen_simu::MeasurementVerification<sick_line_guidance::MLS_Measurement>::MeasurementVerification(nh, measurement_subscribe_topic, sensor_state_queue_size, devicename)
{
m_measurement_subscriber = nh.subscribe(measurement_subscribe_topic, sensor_state_queue_size, &sick_canopen_simu::MLSMeasurementVerification::measurementCb, this);
}
/*
* @brief Callback for measurement messages, called whenever the sick_line_guidance ros driver publishes a
* new measurement message. Compares the measurement message with the sensor states from simulation,
* and reports an error, if an equivalent sensor state hasn't been sent by the simulation.
*
* @param[in] measurement measurement message from sick_line_guidance ros driver
*/
void sick_canopen_simu::MLSMeasurementVerification::measurementCb(const sick_line_guidance::MLS_Measurement & measurement)
{
verifyMeasurement(measurement);
}
/*
* Constructor. Subclass OLSMeasurementVerification extends class MeasurementVerification to verify OLS_Measurement messages.
*
* @param[in] nh ros node handle
* @param[in] measurement_subscribe_topic ros topic for measurement messages, default: "ols"
* @param[in] sensor_state_queue_size buffer size for simulated sensor states, default: 2
* @param[in] devicename descriptional device name, f.e. "OLS20"
*/
sick_canopen_simu::OLSMeasurementVerification::OLSMeasurementVerification(ros::NodeHandle & nh, const std::string & measurement_subscribe_topic, int sensor_state_queue_size, const std::string & devicename)
: sick_canopen_simu::MeasurementVerification<sick_line_guidance::OLS_Measurement>::MeasurementVerification(nh, measurement_subscribe_topic, sensor_state_queue_size, devicename)
{
m_measurement_subscriber = nh.subscribe(measurement_subscribe_topic, sensor_state_queue_size, &sick_canopen_simu::OLSMeasurementVerification::measurementCb, this);
}
/*
* @brief Callback for measurement messages, called whenever the sick_line_guidance ros driver publishes a
* new measurement message. Compares the measurement message with the sensor states from simulation,
* and reports an error, if an equivalent sensor state hasn't been sent by the simulation.
*
* @param[in] measurement measurement message from sick_line_guidance ros driver
*/
void sick_canopen_simu::OLSMeasurementVerification::measurementCb(const sick_line_guidance::OLS_Measurement & measurement)
{
verifyMeasurement(measurement);
}

242
Devices/Packages/sick_line_guidance/src/sick_line_guidance_can2ros_node.cpp Executable file
View File

@@ -0,0 +1,242 @@
/*
* sick_line_guidance_can2ros_node: implements a ros node, which publishes all can messages.
* A simple candump to ros messages.
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#include <ros/ros.h>
#include <can_msgs/Frame.h>
#include <socketcan_interface/socketcan.h>
#include <socketcan_interface/filter.h>
#include <canopen_chain_node/ros_chain.h>
#include "sick_line_guidance/sick_line_guidance_version.h"
namespace sick_line_guidance
{
/*
* class SocketCANListener: implements a simple listener to a SocketCANInterface.
*/
class SocketCANListener
{
public:
/*
* Constructor
*/
SocketCANListener() : m_socketcan_interface(0), m_socketcan_thread(0), m_socketcan_running(false), m_socketcan_state_listener(0), m_socketcan_frame_listener(0), m_filter_sync(true)
{
}
/*
* Register state and frame listener to the SocketCANInterface and create ros publisher.
* param[in] nh ros handle
* param[in] topic topic for ros messages, message type can_msgs::Frame
* param[in] p_socketcan_interface pointer to SocketCANInterface
* param[in] filter_sync if true (default), can sync messages are not published
* @return true on success, otherwise false;
*/
bool init(ros::NodeHandle & nh, const std::string & topic, can::DriverInterfaceSharedPtr p_socketcan_interface, bool filter_sync = true)
{
m_socketcan_interface = p_socketcan_interface;
m_filter_sync = filter_sync;
m_ros_publisher = nh.advertise<can_msgs::Frame>(topic, 1);
m_socketcan_state_listener = m_socketcan_interface->createStateListener(can::StateInterface::StateDelegate(this, &SocketCANListener::socketcanStateCallback));
m_socketcan_frame_listener = m_socketcan_interface->createMsgListener(can::CommInterface::FrameDelegate(this, &SocketCANListener::socketcanFrameCallback));
return m_socketcan_state_listener != 0 && m_socketcan_frame_listener != 0;
}
/*
* @brief create a thread and run can::SocketCANInterface::run() in a background task
*/
void start()
{
m_socketcan_running = true;
m_socketcan_thread = new boost::thread(&sick_line_guidance::SocketCANListener::run, this);
}
/*
* @brief shuts down can::SocketCANInterface and stops the thread running can::SocketCANInterface::run()
*/
void stop()
{
m_socketcan_running = false;
if(m_socketcan_thread)
{
m_socketcan_interface->shutdown();
m_socketcan_thread->join();
delete(m_socketcan_thread);
}
m_socketcan_thread = 0;
}
protected:
/*
* member variables and functions
*/
can::DriverInterfaceSharedPtr m_socketcan_interface; // can::SocketCANInterface instance
boost::thread* m_socketcan_thread; // thread to run can::SocketCANInterface in background
bool m_socketcan_running; // flag indicating start and stop of m_socketcan_thread
can::FrameListenerConstSharedPtr m_socketcan_frame_listener; // can frame listener
can::StateListenerConstSharedPtr m_socketcan_state_listener; // can state listener
ros::Publisher m_ros_publisher; // publishes a ros message for each received can frame
bool m_filter_sync; // if true (default), can sync messages are not published
/*
* Callback for can state messages, called by SocketCANInterface.
* param[in] canstate can state message
*/
void socketcanStateCallback(const can::State & canstate)
{
// ROS_DEBUG_STREAM("sick_line_guidance::SocketCANListener::socketcanStateCallback(): can state message: " << canstate.driver_state << " (error code: " << canstate.error_code << ")");
if(canstate.error_code)
ROS_ERROR_STREAM("sick_line_guidance::SocketCANListener::socketcanStateCallback(): can error code: " << canstate.error_code);
}
/*
* Callback for can frame messages, called by SocketCANInterface.
* param[in] canframe can frame message
*/
void socketcanFrameCallback(const can::Frame & canframe)
{
if(canframe.isValid())
{
if(m_filter_sync && canframe.id == 0x80) // can sync message
{
ROS_DEBUG_STREAM("sick_line_guidance::SocketCANListener::socketcanFrameCallback(): can sync message: " << can::tostring(canframe, false));
return;
}
ROS_DEBUG_STREAM("sick_line_guidance::SocketCANListener::socketcanFrameCallback(): can frame message: " << can::tostring(canframe, false));
}
else
{
ROS_ERROR_STREAM("sick_line_guidance::SocketCANListener::socketcanFrameCallback(): invalid can frame message: " << can::tostring(canframe, false));
}
can_msgs::Frame canframe_msg;
canframe_msg.id = canframe.id;
canframe_msg.dlc = std::min(static_cast<uint8_t>(canframe_msg.data.size()), canframe.dlc);
canframe_msg.is_error = canframe.is_error;
canframe_msg.is_rtr = canframe.is_rtr;
canframe_msg.is_extended = canframe.is_extended;
canframe_msg.data.assign(0);
for(size_t n = 0, n_max = std::min(canframe_msg.data.size(),canframe.data.size()); n < n_max; n++)
canframe_msg.data[n] = canframe.data[n];
canframe_msg.header.stamp = ros::Time::now();
m_ros_publisher.publish(canframe_msg);
}
/*
* @brief runs can::SocketCANInterface::run() in an endless loop
*/
void run()
{
while(m_socketcan_running && ros::ok())
{
m_socketcan_interface->run();
}
}
}; // class SocketCANListener
} // namespace sick_line_guidance
/*
* sick_line_guidance_can2ros_node: implements a ros node, which publishes all can messages.
* A simple candump to ros messages.
*/
int main(int argc, char** argv)
{
// Setup and configuration
ros::init(argc, argv, "sick_line_guidance_can2ros_node");
ros::NodeHandle nh;
std::string can_device = "can0", ros_topic = "can0";
nh.param("can_device", can_device, can_device); // name of can net device (socketcan interface)
nh.param("ros_topic", ros_topic, ros_topic); // topic for ros messages, message type can_msgs::Frame
ROS_INFO_STREAM("sick_line_guidance_can2ros_node: version " << sick_line_guidance::Version::getVersionInfo());
ROS_INFO_STREAM("sick_line_guidance_can2ros_node: starting...");
// Create the SocketCANInterface
sick_line_guidance::SocketCANListener socketcan_listener;
can::DriverInterfaceSharedPtr p_socketcan_interface = 0;
canopen::GuardedClassLoader<can::DriverInterface> driver_loader("socketcan_interface", "can::DriverInterface");
try
{
ROS_INFO("sick_line_guidance_can2ros_node: initializing SocketCANInterface...");
p_socketcan_interface = driver_loader.createInstance("can::SocketCANInterface");
if(!p_socketcan_interface->init(can_device, false, can::NoSettings::create()))
ROS_ERROR("sick_line_guidance_can2ros_node: SocketCANInterface::init() failed.");
ROS_INFO("sick_line_guidance_can2ros_node: initializing socketcan listener ...");
if(!socketcan_listener.init(nh, ros_topic, p_socketcan_interface, true))
ROS_ERROR("sick_line_guidance_can2ros_node: SocketCANListener::registerListener() failed.");
socketcan_listener.start();
}
catch(pluginlib::PluginlibException& ex)
{
ROS_ERROR_STREAM("sick_line_guidance_can2ros_node: createInstance(\"can::SocketCANInterface\") failed: " << ex.what() << ", exit with error");
return 1;
}
ROS_INFO_STREAM("sick_line_guidance_can2ros_node: SocketCANInterface created, state = " << p_socketcan_interface->getState().driver_state);
// Run interface event loop
// while(ros::ok())
// p_socketcan_interface->run();
// Run ros event loop
ros::spin();
std::cout << "sick_line_guidance_can2ros_node: exiting..." << std::endl;
ROS_INFO("sick_line_guidance_can2ros_node: exiting...");
socketcan_listener.stop();
p_socketcan_interface = 0;
return 0;
}

94
Devices/Packages/sick_line_guidance/src/sick_line_guidance_can_chain_node.cpp Executable file
View File

@@ -0,0 +1,94 @@
/*
* sick_line_guidance_can_chain_node wraps CanopenChain for sick_line_guidance ros driver.
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#include <ros/ros.h>
#include <ros/console.h>
#include <console_bridge/console.h>
#include <log4cxx/logger.h>
#include "sick_line_guidance/sick_line_guidance_version.h"
#include "sick_line_guidance/sick_line_guidance_canopen_chain.h"
#include "sick_line_guidance/sick_line_guidance_diagnostic.h"
int main(int argc, char** argv)
{
ros::init(argc, argv, "sick_line_guidance_can_chain_node");
// log4cxx::LoggerPtr node_logger = log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME);
// node_logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Debug]);
// console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_DEBUG);
ros::NodeHandle nh;
ros::NodeHandle nh_priv("~");
ROS_INFO_STREAM("sick_line_guidance_can_chain_node: version " << sick_line_guidance::Version::getVersionInfo());
// Start canopen_chain_node
std::string diagnostic_topic = "diagnostics";
nh.param("/sick_line_guidance_can_chain_node/diagnostic_topic", diagnostic_topic, diagnostic_topic);
sick_line_guidance::Diagnostic::init(nh, diagnostic_topic, "sick_line_guidance_can_chain_node");
sick_line_guidance::CanopenChain canopen_chain(nh, nh_priv);
ROS_INFO_STREAM("sick_line_guidance_can_chain_node: canopen setup...");
if(!canopen_chain.setup())
{
ROS_ERROR_STREAM("sick_line_guidance_can_chain_node: CanopenChain::setup() failed.");
sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::INITIALIZATION_ERROR, "CanopenChain::setup() failed");
return 1;
}
ROS_INFO_STREAM("sick_line_guidance_can_chain_node: canopen setup successfull.");
sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::OK);
// Run ros event loop
ros::spin();
sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::EXIT);
return 0;
}

194
Devices/Packages/sick_line_guidance/src/sick_line_guidance_can_cia401_subscriber.cpp Executable file
View File

@@ -0,0 +1,194 @@
/*
* class CanCiA401Subscriber implements the ros subscriber to canopen ols messages (example CiA401 device for testing).
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#include <ros/ros.h>
#include <canopen_chain_node/ros_chain.h>
#include "sick_line_guidance/sick_line_guidance_can_cia401_subscriber.h"
/*
* Constructor.
* @param[in] nh ros::NodeHandle
* @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1"
* @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
* @param[in] max_sdo_rate max. sdo query and publish rate
* @param[in] ros_topic topic for ros messages, f.e. "mls" or "ols"
* @param[in] ros_frameid frameid for ros messages, f.e. "mls_measurement_frame" or "ols_measurement_frame"
* @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error)
* @param[in] subscribe_queue_size buffer size for ros messages
*/
sick_line_guidance::CanCiA401Subscriber::CanCiA401Subscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate,
const std::string & ros_topic, const std::string & ros_frameid, int initial_sensor_state, int subscribe_queue_size)
: sick_line_guidance::CanOlsSubscriber::CanOlsSubscriber(nh, can_nodeid, max_num_retries_after_sdo_error, max_sdo_rate, ros_topic, ros_frameid, initial_sensor_state, subscribe_queue_size)
{
}
/*
* Destructor.
*/
sick_line_guidance::CanCiA401Subscriber::~CanCiA401Subscriber()
{
}
/*
* @brief subsribes to canopen topics for ols messages and sets the callbacks to handle messages from canopen_chain_node
* after PDO messages (LCP = line center point):
*
* Mapping for OLS:
*
* canopenchain-OLS -> CanSubscriber-callback -> OLS sensor state
* -----------------------------------------------------------------------
* <nodeid>+"_1001" -> cancallbackError(UINT8) -> Error register
* <nodeid>+"_2018" -> cancallbackDevState(UINT8) -> Device state
* <nodeid>+"_2021sub1" -> cancallbackLCP1(INT16) -> LCP1
* <nodeid>+"_2021sub2" -> cancallbackLCP2(INT16) -> LCP2
* <nodeid>+"_2021sub3" -> cancallbackLCP3(INT16) -> LCP3
* <nodeid>+"_2021sub4" -> cancallbackState(UINT8) -> State
* <nodeid>+"_2021sub5" -> cancallbackWidthLCP1(INT16) -> Width LCP1
* <nodeid>+"_2021sub6" -> cancallbackWidthLCP2(INT16) -> Width LCP2
* <nodeid>+"_2021sub7" -> cancallbackWidthLCP3(INT16) -> Width LCP3
* <nodeid>+"_2021sub8" -> cancallbackCode(UINT8) -> Code
* <nodeid>+"_2021sub9" -> cancallbackExtCode(UINT32) -> Extended Code
*
* Mapping for MLS:
*
* canopenchain-MLS -> CanSubscriber-callback -> MLS sensor state
* -----------------------------------------------------------------------
* <nodeid>+"_1001" -> cancallbackError(UINT8) -> Error register
* <nodeid>+"_2021sub1" -> cancallbackLCP1(INT16) -> LCP1
* <nodeid>+"_2021sub2" -> cancallbackLCP2(INT16) -> LCP2
* <nodeid>+"_2021sub3" -> cancallbackLCP3(INT16) -> LCP3
* <nodeid>+"_2021sub4" -> cancallbackMarker(UINT8) -> #LCP and marker
* <nodeid>+"_2022" -> cancallbackState(UINT8) -> State
*
* Mapping for CiA402 (example, testing only):
*
* canopenchain-CiA402 -> CanSubscriber-callback -> CiA402 state
* ---------------------------------------------------------------------
* <nodeid>+"_6000sub1" -> cancallbackError(UINT8) -> Error register
* <nodeid>+"_6000sub2" -> cancallbackState(UINT8) -> State
* <nodeid>+"_6000sub3" -> cancallbackCode(UINT8) -> Code
* <nodeid>+"_6000sub4" -> cancallbackExtCode(UINT8) -> LSB Extended Code
* <nodeid>+"_6401sub1" -> cancallbackLCP1(INT16) -> LCP1
* <nodeid>+"_6401sub2" -> cancallbackLCP2(INT16) -> LCP2
* <nodeid>+"_6401sub3" -> cancallbackLCP3(INT16) -> LCP3
* <nodeid>+"_6401sub4" -> cancallbackWidthLCP1(INT16) -> Width LCP1
* <nodeid>+"_6401sub5" -> cancallbackWidthLCP2(INT16) -> Width LCP2
* <nodeid>+"_6401sub6" -> cancallbackWidthLCP3(INT16) -> Width LCP3
*
* See operation manuals for details (file SICK-OLS-Operating_instructions_OLS10_de_IM0076955.PDF for OLS
* and file SICK-MLS-Operating_instructions_MLS_de_IM0076567.PDF for MLS)
*
* @return true on success, otherwise false.
*/
bool sick_line_guidance::CanCiA401Subscriber::subscribeCanTopics(void)
{
m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_6000sub1", m_subscribe_queue_size, &sick_line_guidance::CanCiA401Subscriber::cancallbackError, this));
m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_6401sub1", m_subscribe_queue_size, &sick_line_guidance::CanCiA401Subscriber::cancallbackLCP1, this));
m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_6401sub2", m_subscribe_queue_size, &sick_line_guidance::CanCiA401Subscriber::cancallbackLCP2, this));
m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_6401sub3", m_subscribe_queue_size, &sick_line_guidance::CanCiA401Subscriber::cancallbackLCP3, this));
m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_6000sub2", m_subscribe_queue_size, &sick_line_guidance::CanCiA401Subscriber::cancallbackState, this));
m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_6401sub4", m_subscribe_queue_size, &sick_line_guidance::CanCiA401Subscriber::cancallbackWidthLCP1, this));
m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_6401sub5", m_subscribe_queue_size, &sick_line_guidance::CanCiA401Subscriber::cancallbackWidthLCP2, this));
m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_6401sub6", m_subscribe_queue_size, &sick_line_guidance::CanCiA401Subscriber::cancallbackWidthLCP3, this));
m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_6000sub3", m_subscribe_queue_size, &sick_line_guidance::CanCiA401Subscriber::cancallbackCode, this));
return true;
}
/*
* Callbacks for ros messages from canopen_chain_node. There's one callback for each ros topic
* published by canopen_chain_node. Each callback updates the sensor state and publishes an
* OLS-Measurement message.
*/
void sick_line_guidance::CanCiA401Subscriber::cancallbackError(const boost::shared_ptr<std_msgs::UInt8 const>& msg)
{
// Note: Object 0x1001 (error register) is NOT PDO mapped -> Query 0x1001 ((error register) in case of error flag in m_ols_state.status
// In CanCiA401Subscriber we simulate error status by PDO mapped object 6000sub1
boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
m_measurement.m_ols_state.error = convertIntegerMessage<std_msgs::UInt8,uint8_t>(msg, m_measurement.m_ols_state.error, 0xFF, "OLS/error");
publishOLSMeasurement();
}
void sick_line_guidance::CanCiA401Subscriber::cancallbackLCP1(const boost::shared_ptr<std_msgs::Int16 const>& msg)
{
sick_line_guidance::CanOlsSubscriber::cancallbackLCP1(msg);
}
void sick_line_guidance::CanCiA401Subscriber::cancallbackLCP2(const boost::shared_ptr<std_msgs::Int16 const>& msg)
{
sick_line_guidance::CanOlsSubscriber::cancallbackLCP2(msg);
}
void sick_line_guidance::CanCiA401Subscriber::cancallbackLCP3(const boost::shared_ptr<std_msgs::Int16 const>& msg)
{
sick_line_guidance::CanOlsSubscriber::cancallbackLCP3(msg);
}
void sick_line_guidance::CanCiA401Subscriber::cancallbackState(const boost::shared_ptr<std_msgs::UInt8 const>& msg)
{
sick_line_guidance::CanOlsSubscriber::cancallbackState(msg);
}
void sick_line_guidance::CanCiA401Subscriber::cancallbackWidthLCP1(const boost::shared_ptr<std_msgs::Int16 const>& msg)
{
sick_line_guidance::CanOlsSubscriber::cancallbackWidthLCP1(msg);
}
void sick_line_guidance::CanCiA401Subscriber::cancallbackWidthLCP2(const boost::shared_ptr<std_msgs::Int16 const>& msg)
{
sick_line_guidance::CanOlsSubscriber::cancallbackWidthLCP2(msg);
}
void sick_line_guidance::CanCiA401Subscriber::cancallbackWidthLCP3(const boost::shared_ptr<std_msgs::Int16 const>& msg)
{
sick_line_guidance::CanOlsSubscriber::cancallbackWidthLCP3(msg);
}
void sick_line_guidance::CanCiA401Subscriber::cancallbackCode(const boost::shared_ptr<std_msgs::UInt8 const>& msg)
{
sick_line_guidance::CanOlsSubscriber::cancallbackCode(msg);
}

200
Devices/Packages/sick_line_guidance/src/sick_line_guidance_can_mls_subscriber.cpp Executable file
View File

@@ -0,0 +1,200 @@
/*
* class CanMlsSubscriber implements the ros subscriber to canopen mls messages.
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#include <ros/ros.h>
#include <canopen_chain_node/ros_chain.h>
#include "sick_line_guidance/sick_line_guidance_can_mls_subscriber.h"
/*
* Constructor.
* @param[in] nh ros::NodeHandle
* @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1"
* @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
* @param[in] max_sdo_rate max. sdo query and publish rate
* @param[in] ros_topic topic for ros messages, f.e. "mls" or "ols"
* @param[in] ros_frameid frameid for ros messages, f.e. "mls_measurement_frame" or "ols_measurement_frame"
* @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error)
* @param[in] subscribe_queue_size buffer size for ros messages
*/
sick_line_guidance::CanMlsSubscriber::CanMlsSubscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate,
const std::string & ros_topic, const std::string & ros_frameid, int initial_sensor_state, int subscribe_queue_size)
: sick_line_guidance::CanSubscriber::CanSubscriber(nh, can_nodeid, max_num_retries_after_sdo_error, max_sdo_rate, initial_sensor_state, subscribe_queue_size)
{
m_measurement.m_mls_state.header.frame_id = ros_frameid;
m_measurement.m_ros_publisher = nh.advertise<sick_line_guidance::MLS_Measurement>(ros_topic, 1);
}
/*
* Destructor.
*/
sick_line_guidance::CanMlsSubscriber::~CanMlsSubscriber()
{
}
/*
* @brief subsribes to canopen topics for mls messages and sets the callbacks to handle messages from canopen_chain_node
* after PDO messages (LCP = line center point):
*
* Mapping for OLS:
*
* canopenchain-OLS -> CanSubscriber-callback -> OLS sensor state
* -----------------------------------------------------------------------
* <nodeid>+"_1001" -> cancallbackError(UINT8) -> Error register
* <nodeid>+"_2018" -> cancallbackDevState(UINT8) -> Device state
* <nodeid>+"_2021sub1" -> cancallbackLCP1(INT16) -> LCP1
* <nodeid>+"_2021sub2" -> cancallbackLCP2(INT16) -> LCP2
* <nodeid>+"_2021sub3" -> cancallbackLCP3(INT16) -> LCP3
* <nodeid>+"_2021sub4" -> cancallbackState(UINT8) -> State
* <nodeid>+"_2021sub5" -> cancallbackWidthLCP1(INT16) -> Width LCP1
* <nodeid>+"_2021sub6" -> cancallbackWidthLCP2(INT16) -> Width LCP2
* <nodeid>+"_2021sub7" -> cancallbackWidthLCP3(INT16) -> Width LCP3
* <nodeid>+"_2021sub8" -> cancallbackCode(UINT8) -> Code
* <nodeid>+"_2021sub9" -> cancallbackExtCode(UINT32) -> Extended Code
*
* Mapping for MLS:
*
* canopenchain-MLS -> CanSubscriber-callback -> MLS sensor state
* -----------------------------------------------------------------------
* <nodeid>+"_1001" -> cancallbackError(UINT8) -> Error register
* <nodeid>+"_2021sub1" -> cancallbackLCP1(INT16) -> LCP1
* <nodeid>+"_2021sub2" -> cancallbackLCP2(INT16) -> LCP2
* <nodeid>+"_2021sub3" -> cancallbackLCP3(INT16) -> LCP3
* <nodeid>+"_2021sub4" -> cancallbackMarker(UINT8) -> #LCP and marker
* <nodeid>+"_2022" -> cancallbackState(UINT8) -> State
*
* Mapping for CiA402 (example, testing only):
*
* canopenchain-CiA402 -> CanSubscriber-callback -> CiA402 state
* ---------------------------------------------------------------------
* <nodeid>+"_6000sub1" -> cancallbackError(UINT8) -> Error register
* <nodeid>+"_6000sub2" -> cancallbackState(UINT8) -> State
* <nodeid>+"_6000sub3" -> cancallbackCode(UINT8) -> Code
* <nodeid>+"_6000sub4" -> cancallbackExtCode(UINT8) -> LSB Extended Code
* <nodeid>+"_6401sub1" -> cancallbackLCP1(INT16) -> LCP1
* <nodeid>+"_6401sub2" -> cancallbackLCP2(INT16) -> LCP2
* <nodeid>+"_6401sub3" -> cancallbackLCP3(INT16) -> LCP3
* <nodeid>+"_6401sub4" -> cancallbackWidthLCP1(INT16) -> Width LCP1
* <nodeid>+"_6401sub5" -> cancallbackWidthLCP2(INT16) -> Width LCP2
* <nodeid>+"_6401sub6" -> cancallbackWidthLCP3(INT16) -> Width LCP3
*
* See operation manuals for details (file SICK-OLS-Operating_instructions_OLS10_de_IM0076955.PDF for OLS
* and file SICK-MLS-Operating_instructions_MLS_de_IM0076567.PDF for MLS)
*
* @return true on success, otherwise false.
*/
bool sick_line_guidance::CanMlsSubscriber::subscribeCanTopics(void)
{
// m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_1001", m_subscribe_queue_size, &sick_line_guidance::CanMlsSubscriber::cancallbackError, this));
m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub1", m_subscribe_queue_size, &sick_line_guidance::CanMlsSubscriber::cancallbackLCP1, this));
m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub2", m_subscribe_queue_size, &sick_line_guidance::CanMlsSubscriber::cancallbackLCP2, this));
m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub3", m_subscribe_queue_size, &sick_line_guidance::CanMlsSubscriber::cancallbackLCP3, this));
m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub4", m_subscribe_queue_size, &sick_line_guidance::CanMlsSubscriber::cancallbackMarker, this));
m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2022", m_subscribe_queue_size, &sick_line_guidance::CanMlsSubscriber::cancallbackState, this));
return true;
}
/*
* Callbacks for ros messages from canopen_chain_node. There's one callback for each ros topic
* published by canopen_chain_node. Each callback updates the sensor state and publishes an
* OLS-Measurement message.
*/
void sick_line_guidance::CanMlsSubscriber::cancallbackLCP1(const boost::shared_ptr<std_msgs::Int16 const>& msg)
{
boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
m_measurement.m_mls_state.position[0] = convertToLCP(msg, m_measurement.m_mls_state.position[0], "MLS/LCP1");
publishMLSMeasurement();
}
void sick_line_guidance::CanMlsSubscriber::cancallbackLCP2(const boost::shared_ptr<std_msgs::Int16 const>& msg)
{
boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
m_measurement.m_mls_state.position[1] = convertToLCP(msg, m_measurement.m_mls_state.position[1], "MLS/LCP2");
publishMLSMeasurement();
}
void sick_line_guidance::CanMlsSubscriber::cancallbackLCP3(const boost::shared_ptr<std_msgs::Int16 const>& msg)
{
boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
m_measurement.m_mls_state.position[2] = convertToLCP(msg, m_measurement.m_mls_state.position[2], "MLS/LCP3");
publishMLSMeasurement();
}
void sick_line_guidance::CanMlsSubscriber::cancallbackMarker(const boost::shared_ptr<std_msgs::UInt8 const>& msg)
{
boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
m_measurement.m_mls_state.lcp = convertIntegerMessage<std_msgs::UInt8,uint8_t>(msg, m_measurement.m_mls_state.lcp, 0xFF, "MLS/lcp");
publishMLSMeasurement();
}
void sick_line_guidance::CanMlsSubscriber::cancallbackState(const boost::shared_ptr<std_msgs::UInt8 const>& msg)
{
boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
m_measurement.m_mls_state.status = convertIntegerMessage<std_msgs::UInt8,uint8_t>(msg, m_measurement.m_mls_state.status, 0xFF, "MLS/status");
// m_measurement.m_mls_state.error = 0;
// m_measurement.m_mls_query_error_register = false;
// if((m_measurement.m_mls_state.status & 0x0F) == 0) // Bit0 to Bit 3 == 0: bad line, no magnetic field recognized -> SDO request 0x1001 (error register, UINT8)
// {
// m_measurement.m_mls_query_error_register = true; // query object 0x1001 (error register, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread)
// }
publishMLSMeasurement();
}
// void sick_line_guidance::CanMlsSubscriber::cancallbackError(const boost::shared_ptr<std_msgs::UInt8 const>& msg)
// {
// // Note: Object 0x1001 (error register) is NOT PDO mapped -> Query 0x1001 ((error register) in case of error flag in m_mls_state.status
// boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
// m_measurement.m_mls_state.error = convertIntegerMessage<std_msgs::UInt8,uint8_t>(msg, m_measurement.m_mls_state.error, 0xFF, "MLS/error");
// publishMLSMeasurement();
// }

367
Devices/Packages/sick_line_guidance/src/sick_line_guidance_can_ols_subscriber.cpp Executable file
View File

@@ -0,0 +1,367 @@
/*
* class CanOlsSubscriber implements the ros subscriber to canopen ols messages.
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#include <ros/ros.h>
#include <canopen_chain_node/ros_chain.h>
#include "sick_line_guidance/sick_line_guidance_canopen_chain.h"
#include "sick_line_guidance/sick_line_guidance_can_ols_subscriber.h"
#include "sick_line_guidance/sick_line_guidance_msg_util.h"
/*
* class CanOlsSubscriber implements the base ros subscriber to canopen ols messages.
*/
/*
* Constructor.
* @param[in] nh ros::NodeHandle
* @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1"
* @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
* @param[in] max_sdo_rate max. sdo query and publish rate
* @param[in] ros_topic topic for ros messages, f.e. "mls" or "ols"
* @param[in] ros_frameid frameid for ros messages, f.e. "mls_measurement_frame" or "ols_measurement_frame"
* @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error)
* @param[in] subscribe_queue_size buffer size for ros messages
*/
sick_line_guidance::CanOlsSubscriber::CanOlsSubscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate,
const std::string & ros_topic, const std::string & ros_frameid, int initial_sensor_state, int subscribe_queue_size)
: sick_line_guidance::CanSubscriber::CanSubscriber(nh, can_nodeid, max_num_retries_after_sdo_error, max_sdo_rate, initial_sensor_state, subscribe_queue_size)
{
m_bOLS20queries = false;
m_measurement.m_ols_state.header.frame_id = ros_frameid;
m_measurement.m_ros_publisher = nh.advertise<sick_line_guidance::OLS_Measurement>(ros_topic, 1);
}
/*
* Destructor.
*/
sick_line_guidance::CanOlsSubscriber::~CanOlsSubscriber()
{
}
/*
* @brief subsribes to canopen topics for ols messages and sets the callbacks to handle messages from canopen_chain_node
* after PDO messages (LCP = line center point):
*
* Mapping for OLS:
*
* canopenchain-OLS -> CanSubscriber-callback -> OLS sensor state
* -----------------------------------------------------------------------
* <nodeid>+"_1001" -> cancallbackError(UINT8) -> Error register
* <nodeid>+"_2018" -> cancallbackDevState(UINT8) -> Device state
* <nodeid>+"_2021sub1" -> cancallbackLCP1(INT16) -> LCP1
* <nodeid>+"_2021sub2" -> cancallbackLCP2(INT16) -> LCP2
* <nodeid>+"_2021sub3" -> cancallbackLCP3(INT16) -> LCP3
* <nodeid>+"_2021sub4" -> cancallbackState(UINT8) -> State
* <nodeid>+"_2021sub5" -> cancallbackWidthLCP1(INT16) -> Width LCP1
* <nodeid>+"_2021sub6" -> cancallbackWidthLCP2(INT16) -> Width LCP2
* <nodeid>+"_2021sub7" -> cancallbackWidthLCP3(INT16) -> Width LCP3
* <nodeid>+"_2021sub8" -> cancallbackCode(UINT8) -> Code
* <nodeid>+"_2021sub9" -> cancallbackExtCode(UINT32) -> Extended Code
*
* Mapping for MLS:
*
* canopenchain-MLS -> CanSubscriber-callback -> MLS sensor state
* -----------------------------------------------------------------------
* <nodeid>+"_1001" -> cancallbackError(UINT8) -> Error register
* <nodeid>+"_2021sub1" -> cancallbackLCP1(INT16) -> LCP1
* <nodeid>+"_2021sub2" -> cancallbackLCP2(INT16) -> LCP2
* <nodeid>+"_2021sub3" -> cancallbackLCP3(INT16) -> LCP3
* <nodeid>+"_2021sub4" -> cancallbackMarker(UINT8) -> #LCP and marker
* <nodeid>+"_2022" -> cancallbackState(UINT8) -> State
*
* Mapping for CiA402 (example, testing only):
*
* canopenchain-CiA402 -> CanSubscriber-callback -> CiA402 state
* ---------------------------------------------------------------------
* <nodeid>+"_6000sub1" -> cancallbackError(UINT8) -> Error register
* <nodeid>+"_6000sub2" -> cancallbackState(UINT8) -> State
* <nodeid>+"_6000sub3" -> cancallbackCode(UINT8) -> Code
* <nodeid>+"_6000sub4" -> cancallbackExtCode(UINT8) -> LSB Extended Code
* <nodeid>+"_6401sub1" -> cancallbackLCP1(INT16) -> LCP1
* <nodeid>+"_6401sub2" -> cancallbackLCP2(INT16) -> LCP2
* <nodeid>+"_6401sub3" -> cancallbackLCP3(INT16) -> LCP3
* <nodeid>+"_6401sub4" -> cancallbackWidthLCP1(INT16) -> Width LCP1
* <nodeid>+"_6401sub5" -> cancallbackWidthLCP2(INT16) -> Width LCP2
* <nodeid>+"_6401sub6" -> cancallbackWidthLCP3(INT16) -> Width LCP3
*
* See operation manuals for details (file SICK-OLS-Operating_instructions_OLS10_de_IM0076955.PDF for OLS
* and file SICK-MLS-Operating_instructions_MLS_de_IM0076567.PDF for MLS)
*
* @return true on success, otherwise false.
*/
bool sick_line_guidance::CanOlsSubscriber::subscribeCanTopics(void)
{
// m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_1001", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackError, this));
// m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2018", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackDevState, this));
m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub1", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackLCP1, this));
m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub2", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackLCP2, this));
m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub3", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackLCP3, this));
m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub4", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackState, this));
m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub5", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackWidthLCP1, this));
m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub6", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackWidthLCP2, this));
m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub7", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackWidthLCP3, this));
m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub8", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackCode, this));
// m_can_subscriber.push_back(m_measurement.m_nh.subscribe(m_measurement.m_can_nodeid + "_2021sub9", m_subscribe_queue_size, &sick_line_guidance::CanOlsSubscriber::cancallbackExtCode, this));
return true;
}
/*
* Callbacks for ros messages from canopen_chain_node. There's one callback for each ros topic
* published by canopen_chain_node. Each callback updates the sensor state and publishes an
* OLS-Measurement message.
*/
void sick_line_guidance::CanOlsSubscriber::cancallbackLCP1(const boost::shared_ptr<std_msgs::Int16 const>& msg)
{
boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
m_measurement.m_ols_state.position[0] = convertToLCP(msg, m_measurement.m_ols_state.position[0], "OLS/LCP1");
if(m_bOLS20queries)
{
m_measurement.m_ols_query_quality_of_lines.pending() = true; // OLS20 only: query object 2021subB (quality of lines, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread)
m_measurement.m_ols_query_intensity_of_lines[0].pending() = true; // OLS20 only: query object 2023sub1 (intensity line 1, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread)
}
else
{
m_measurement.m_ols_state.quality_of_lines = 0;
m_measurement.m_ols_state.intensity_of_lines[0] = 0;
m_measurement.m_ols_query_quality_of_lines.pending() = false;
m_measurement.m_ols_query_intensity_of_lines[0].pending() = false;
}
publishOLSMeasurement();
}
void sick_line_guidance::CanOlsSubscriber::cancallbackLCP2(const boost::shared_ptr<std_msgs::Int16 const>& msg)
{
boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
m_measurement.m_ols_state.position[1] = convertToLCP(msg, m_measurement.m_ols_state.position[1], "OLS/LCP2");
if(m_bOLS20queries)
{
m_measurement.m_ols_query_quality_of_lines.pending() = true; // OLS20 only: query object 2021subB (quality of lines, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread)
m_measurement.m_ols_query_intensity_of_lines[1].pending() = true; // OLS20 only: query object 2023sub2 (intensity line 2, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread)
}
else
{
m_measurement.m_ols_state.quality_of_lines = 0;
m_measurement.m_ols_state.intensity_of_lines[1] = 0;
m_measurement.m_ols_query_quality_of_lines.pending() = false;
m_measurement.m_ols_query_intensity_of_lines[1].pending() = false;
}
publishOLSMeasurement();
}
void sick_line_guidance::CanOlsSubscriber::cancallbackLCP3(const boost::shared_ptr<std_msgs::Int16 const>& msg)
{
boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
m_measurement.m_ols_state.position[2] = convertToLCP(msg, m_measurement.m_ols_state.position[2], "OLS/LCP3");
if(m_bOLS20queries)
{
m_measurement.m_ols_query_quality_of_lines.pending() = true; // OLS20 only: query object 2021subB (quality of lines, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread)
m_measurement.m_ols_query_intensity_of_lines[2].pending() = true; // OLS20 only: query object 2023sub3 (intensity line 3, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread)
}
else
{
m_measurement.m_ols_state.quality_of_lines = 0;
m_measurement.m_ols_state.intensity_of_lines[2] = 0;
m_measurement.m_ols_query_quality_of_lines.pending() = false;
m_measurement.m_ols_query_intensity_of_lines[2].pending() = false;
}
publishOLSMeasurement();
}
void sick_line_guidance::CanOlsSubscriber::cancallbackState(const boost::shared_ptr<std_msgs::UInt8 const>& msg)
{
boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
m_measurement.m_ols_state.status = convertIntegerMessage<std_msgs::UInt8,uint8_t>(msg, m_measurement.m_ols_state.status, 0xFF, "OLS/status");
// If Bit 4 of m_ols_state.status is set (i.e. device status != 0), an error occured. In this case, we have to query SDOs:
// a) object 0x1001 in object dictionary -> measurement.error
// b) object 0x2018 in object dictionary -> measurement.dev_status
if(!sick_line_guidance::MsgUtil::statusOK(m_measurement.m_ols_state)) // Bit 4 OLS status != 0 -> SDO request 0x1001 (error register, UINT8) and 0x2018 (device status register, UINT8)
{
m_measurement.m_ols_query_error_register.pending() = true; // query object 0x1001 (error register, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread)
if(m_bOLS20queries) // OLS20: query object 0x2018 (device status register, UINT8) in object dictionary by SDO (query runs in m_measurement in a background thread)
{
m_measurement.m_ols_query_device_status_u8.pending() = true;
m_measurement.m_ols_query_device_status_u16.pending() = false;
}
else // OLS10: query object 0x2018 (device status register, UINT16) in object dictionary by SDO (query runs in m_measurement in a background thread)
{
m_measurement.m_ols_query_device_status_u8.pending() = false;
m_measurement.m_ols_query_device_status_u16.pending() = true;
}
}
else
{
m_measurement.m_ols_state.error = 0;
m_measurement.m_ols_state.dev_status = 0;
m_measurement.m_ols_query_device_status_u8.pending() = false;
m_measurement.m_ols_query_device_status_u16.pending() = false;
m_measurement.m_ols_query_error_register.pending() = false;
}
publishOLSMeasurement();
}
void sick_line_guidance::CanOlsSubscriber::cancallbackWidthLCP1(const boost::shared_ptr<std_msgs::Int16 const>& msg)
{
boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
m_measurement.m_ols_state.width[0] = convertToLCP(msg, m_measurement.m_ols_state.width[0], "OLS/WidthLCP1");
publishOLSMeasurement();
}
void sick_line_guidance::CanOlsSubscriber::cancallbackWidthLCP2(const boost::shared_ptr<std_msgs::Int16 const>& msg)
{
boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
m_measurement.m_ols_state.width[1] = convertToLCP(msg, m_measurement.m_ols_state.width[1], "OLS/WidthLCP2");
publishOLSMeasurement();
}
void sick_line_guidance::CanOlsSubscriber::cancallbackWidthLCP3(const boost::shared_ptr<std_msgs::Int16 const>& msg)
{
boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
m_measurement.m_ols_state.width[2] = convertToLCP(msg, m_measurement.m_ols_state.width[2], "OLS/WidthLCP3");
publishOLSMeasurement();
}
void sick_line_guidance::CanOlsSubscriber::cancallbackCode(const boost::shared_ptr<std_msgs::UInt8 const>& msg)
{
boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
m_measurement.m_ols_state.barcode = convertIntegerMessage<std_msgs::UInt8,uint8_t>(msg, m_measurement.m_ols_state.barcode, 0xFF, "OLS/barcode");
// If barcode >= 255, we have to query object 0x2021sub9 (extended code, UINT32) in object dictionary by SDOs
if(m_measurement.m_ols_state.barcode >= 255)
{
m_measurement.m_ols_query_extended_code.pending() = true; // query object 0x2021sub9 (extended code, UINT32) in object dictionary by SDO (query runs in m_measurement in a background thread)
}
else
{
m_measurement.m_ols_query_extended_code.pending() = false;
m_measurement.m_ols_state.extended_code = 0;
}
// OLS20 only: query object 2021subA (barcode center point, INT16) in object dictionary by SDO (query runs in m_measurement in a background thread)
if(m_measurement.m_ols_state.barcode > 0 && m_bOLS20queries)
{
m_measurement.m_ols_query_barcode_center_point.pending() = true;
}
else
{
m_measurement.m_ols_state.barcode_center_point = 0;
m_measurement.m_ols_query_barcode_center_point.pending() = false;
}
publishOLSMeasurement();
}
// void sick_line_guidance::CanOlsSubscriber::cancallbackError(const boost::shared_ptr<std_msgs::UInt8 const>& msg)
// {
// // Note: Object 0x1001 (error register) is NOT PDO mapped -> Query 0x1001 ((error register) in case of error flag in m_ols_state.status
// boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
// m_measurement.m_ols_state.error = convertIntegerMessage<std_msgs::UInt8,uint8_t>(msg, m_measurement.m_ols_state.error, 0xFF, "OLS/error");
// publishOLSMeasurement();
// }
// void sick_line_guidance::CanOlsSubscriber::cancallbackDevState(const boost::shared_ptr<std_msgs::UInt8 const>& msg)
// {
// // Note: Object 0x2018 is NOT PDO mapped -> Query 0x2018 (device state) in case of error flag in m_ols_state.status
// boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
// m_measurement.m_ols_state.dev_status = convertIntegerMessage<std_msgs::UInt8,uint8_t>(msg, m_measurement.m_ols_state.dev_status, 0xFF, "OLS/DevState");
// publishOLSMeasurement();
// }
// void sick_line_guidance::CanOlsSubscriber::cancallbackExtCode(const boost::shared_ptr<std_msgs::UInt32 const>& msg)
// {
// // Note: Object 0x2021sub9 is NOT PDO mapped -> Query 0x2021sub9 (extended code) in case of m_ols_state.barcode == 0xFF
// boost::lock_guard<boost::mutex> publish_lockguard(m_measurement.m_measurement_mutex);
// m_measurement.m_ols_state.extended_code = convertIntegerMessage<std_msgs::UInt32,uint32_t>(msg, m_measurement.m_ols_state.extended_code, 0xFFFFFFFF, "OLS/extcode");
// publishOLSMeasurement();
// }
/*
* class CanOls10Subscriber derives from CanOlsSubscriber to implements OLS10 specific handling of the ros canopen messages.
*/
/*
* Constructor.
* @param[in] nh ros::NodeHandle
* @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1"
* @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
* @param[in] max_sdo_rate max. sdo query and publish rate
* @param[in] ros_topic topic for ros messages, f.e. "mls" or "ols"
* @param[in] ros_frameid frameid for ros messages, f.e. "mls_measurement_frame" or "ols_measurement_frame"
* @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error)
* @param[in] subscribe_queue_size buffer size for ros messages
*/
sick_line_guidance::CanOls10Subscriber::CanOls10Subscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate,
const std::string & ros_topic, const std::string & ros_frameid, int initial_sensor_state, int subscribe_queue_size)
: sick_line_guidance::CanOlsSubscriber::CanOlsSubscriber(nh, can_nodeid, max_num_retries_after_sdo_error, max_sdo_rate, ros_topic, ros_frameid, initial_sensor_state, subscribe_queue_size)
{
}
/*
* class CanOls20Subscriber derives from CanOlsSubscriber to implements OLS20 specific handling of the ros canopen messages.
*/
/*
* Constructor.
* @param[in] nh ros::NodeHandle
* @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1"
* @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
* @param[in] max_sdo_rate max. sdo query and publish rate
* @param[in] ros_topic topic for ros messages, f.e. "mls" or "ols"
* @param[in] ros_frameid frameid for ros messages, f.e. "mls_measurement_frame" or "ols_measurement_frame"
* @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error)
* @param[in] subscribe_queue_size buffer size for ros messages
*/
sick_line_guidance::CanOls20Subscriber::CanOls20Subscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate,
const std::string & ros_topic, const std::string & ros_frameid, int initial_sensor_state, int subscribe_queue_size)
: sick_line_guidance::CanOlsSubscriber::CanOlsSubscriber(nh, can_nodeid, max_num_retries_after_sdo_error, max_sdo_rate, ros_topic, ros_frameid, initial_sensor_state, subscribe_queue_size)
{
m_bOLS20queries = true; // OLS20 only: query objects 2021subA (barcode center point, INT16), 2021subB (quality of lines, UINT8) and 2023sub1 to 2023sub3 (intensity line 1 - 3, UINT8)
}

523
Devices/Packages/sick_line_guidance/src/sick_line_guidance_can_subscriber.cpp Executable file
View File

@@ -0,0 +1,523 @@
/*
* class CanSubscriber: base class for CanOlsSubscriber and CanMlsSubscriber,
* implements the base class for ros subscriber to canopen messages for OLS and MLS.
* Converts canopen messages to MLS/OLS measurement messages and publishes
* MLS/OLS measurement messages on the configured ros topic ("/mls" or "/ols").
*
* class CanSubscriber::MeasurementHandler: queries SDOs (if required) and publishes MLS/OLS measurement messages in a background thread
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#include <cstdint>
#include "sick_line_guidance/sick_line_guidance_canopen_chain.h"
#include "sick_line_guidance/sick_line_guidance_can_subscriber.h"
#include "sick_line_guidance/sick_line_guidance_diagnostic.h"
#include "sick_line_guidance/sick_line_guidance_msg_util.h"
/*
* Constructor.
* @param[in] nh ros::NodeHandle
* @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1"
* @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error)
* @param[in] max_publish_rate max rate to publish OLS/MLS measurement messages (default: min. 1 ms between two measurement messages)
* @param[in] max_query_rate max rate to query SDOs if required (default: min. 1 ms between sdo queries)
* @param[in] schedule_publish_delay MLS and OLS measurement message are scheduled to be published 5 milliseconds after first PDO is received
* @param[in] max_publish_delay MLS and OLS measurement message are scheduled to be published max. 2*20 milliseconds after first PDO is received, even if a sdo request is pending (max. 2 * tpdo rate)
* @param[in] query_jitter jitter in seconds (default: 10 ms), i.e. a sdo is requested, if the query is pending and the last successful query is out of the time jitter.
* @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
* By default, a sdo request is send, if the query is pending and not done within the last 10 ms.
*/
sick_line_guidance::CanSubscriber::MeasurementHandler::MeasurementHandler(ros::NodeHandle &nh, const std::string &can_nodeid, int max_num_retries_after_sdo_error, int initial_sensor_state, double max_publish_rate, double max_query_rate, double schedule_publish_delay, double max_publish_delay, double query_jitter)
: m_nh(nh), m_can_nodeid(can_nodeid), m_max_publish_rate(ros::Rate(max_publish_rate)), m_max_sdo_query_rate(ros::Rate(max_query_rate)),
m_schedule_publish_delay(ros::Duration(schedule_publish_delay)), m_max_publish_delay(ros::Duration(max_publish_delay)), m_max_num_retries_after_sdo_error(max_num_retries_after_sdo_error)
{
// initialize MLS/OLS sensor states
sick_line_guidance::MsgUtil::zero(m_mls_state);
m_mls_state.header.stamp = ros::Time::now();
m_mls_state.lcp = static_cast<uint8_t>(initial_sensor_state);
m_mls_state.status = ((initial_sensor_state & 0x7) ? 1 : 0);
sick_line_guidance::MsgUtil::zero(m_ols_state);
m_ols_state.header.stamp = ros::Time::now();
m_ols_state.status = initial_sensor_state;
// initialize publisher thread
m_publish_mls_measurement = ros::Time(0);
m_publish_ols_measurement = ros::Time(0);
m_publish_measurement_latest = ros::Time(0);
m_ols_query_extended_code = QuerySupport(query_jitter);
m_ols_query_device_status_u8 = QuerySupport(query_jitter);
m_ols_query_device_status_u16 = QuerySupport(query_jitter);
m_ols_query_error_register = QuerySupport(query_jitter);
m_ols_query_barcode_center_point = QuerySupport(query_jitter);
m_ols_query_quality_of_lines = QuerySupport(query_jitter);
m_ols_query_intensity_of_lines[0] = QuerySupport(query_jitter);
m_ols_query_intensity_of_lines[1] = QuerySupport(query_jitter);
m_ols_query_intensity_of_lines[2] = QuerySupport(query_jitter);
m_measurement_publish_thread = new boost::thread(&sick_line_guidance::CanSubscriber::MeasurementHandler::runMeasurementPublishThread, this);
m_measurement_sdo_query_thread = new boost::thread(&sick_line_guidance::CanSubscriber::MeasurementHandler::runMeasurementSDOqueryThread, this);
}
/*
* Destructor.
*/
sick_line_guidance::CanSubscriber::MeasurementHandler::~MeasurementHandler()
{
if (m_measurement_sdo_query_thread)
{
m_measurement_sdo_query_thread->join();
delete (m_measurement_sdo_query_thread);
m_measurement_sdo_query_thread = 0;
}
if (m_measurement_publish_thread)
{
m_measurement_publish_thread->join();
delete (m_measurement_publish_thread);
m_measurement_publish_thread = 0;
}
}
/*
* @brief Runs the background thread to publish MLS/OLS measurement messages
*/
void sick_line_guidance::CanSubscriber::MeasurementHandler::runMeasurementPublishThread(void)
{
while(ros::ok())
{
// Publish mls measurement (if one has been triggered, i.e. a pdo has been received recently)
if(isMLSMeasurementTriggered())
{
sick_line_guidance::MLS_Measurement measurement_msg;
{
boost::lock_guard<boost::mutex> publish_lockguard(m_measurement_mutex);
m_mls_state.header.stamp = ros::Time::now();
measurement_msg = m_mls_state;
}
m_ros_publisher.publish(measurement_msg);
schedulePublishMLSMeasurement(false);
bool line_good = sick_line_guidance::MsgUtil::lineOK(measurement_msg); // MLS status bit 0 ("Line good") == 0 => no line detected or line too weak, 1 => line detected, MLS #lcp (bit 0-2 == 0) => no line detected
ROS_INFO_STREAM("sick_line_guidance::MLS_Measurement: {" << sick_line_guidance::MsgUtil::toInfo(measurement_msg) << ",line_good=" << line_good << "}");
/* if(line_good)
sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::OK, "MLS Measurement published");
else
sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::NO_LINE_DETECTED, "MLS Measurement published, no line"); */
}
// Publish ols measurement (if one has been triggered, i.e. a pdo has been received recently)
if(isOLSMeasurementTriggered() && (!isSDOQueryPending() || isLatestTimeForMeasurementPublishing())) // no sdo requests are pending or latest time to publish a new measurement is reached
{
sick_line_guidance::OLS_Measurement measurement_msg;
{
boost::lock_guard<boost::mutex> publish_lockguard(m_measurement_mutex);
m_ols_state.header.stamp = ros::Time::now();
measurement_msg = m_ols_state;
}
m_ros_publisher.publish(measurement_msg);
schedulePublishOLSMeasurement(false);
bool status_ok = sick_line_guidance::MsgUtil::statusOK(measurement_msg); // OLS status bit 4: 0 => Sensor ok, 1 => Sensor not ok => 0x2018 (measurement_msg.dev_status)
bool line_good = status_ok && sick_line_guidance::MsgUtil::lineOK(measurement_msg); // Bit 0-2 OLS status == 0 => no line found
ROS_INFO_STREAM("sick_line_guidance::OLS_Measurement: {" << sick_line_guidance::MsgUtil::toInfo(measurement_msg) << ",status_ok=" << status_ok << ",line_good=" << line_good << "}");
if(!status_ok)
sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::ERROR_STATUS, "OLS Measurement published, status error " + sick_line_guidance::MsgUtil::toHexString(measurement_msg.dev_status));
/* else if(!line_good)
sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::NO_LINE_DETECTED, "OLS Measurement published, no line");
else
sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::OK, "OLS Measurement published"); */
}
m_max_publish_rate.sleep();
}
}
/*
* @brief Runs the background thread to query SDOs, if required
*/
void sick_line_guidance::CanSubscriber::MeasurementHandler::runMeasurementSDOqueryThread(void)
{
while(ros::ok())
{
// Query SDOs if required
querySDOifPending<uint32_t, uint32_t>(m_ols_query_extended_code, "2021sub9", m_max_num_retries_after_sdo_error, m_ols_state.extended_code, 1); // OLS: query object 0x2021sub9 (extended code, UINT32) in object dictionary by SDO
querySDOifPending<uint8_t, uint16_t>(m_ols_query_device_status_u8, "2018", m_max_num_retries_after_sdo_error, m_ols_state.dev_status, 1); // OLS20: query object 0x2018 (device status register, UINT8) in object dictionary by SDO
querySDOifPending<uint16_t, uint16_t>(m_ols_query_device_status_u16, "2018", m_max_num_retries_after_sdo_error, m_ols_state.dev_status, 1); // OLS10: query object 0x2018 (device status register, UINT16) in object dictionary by SDO
querySDOifPending<uint8_t, uint8_t> (m_ols_query_error_register, "1001", m_max_num_retries_after_sdo_error, m_ols_state.error, 1); // OLS: query object 0x1001 (error register, UINT8) in object dictionary by SDO
querySDOifPending<int16_t, float> (m_ols_query_barcode_center_point, "2021subA", m_max_num_retries_after_sdo_error, m_ols_state.barcode_center_point, 0.001f); // OLS20 only: query object 2021subA (barcode center point, INT16) in object dictionary by SDO
querySDOifPending<uint8_t, uint8_t> (m_ols_query_quality_of_lines, "2021subB", m_max_num_retries_after_sdo_error, m_ols_state.quality_of_lines, 1); // OLS20 only: query object 2021subB (quality of lines, UINT8) in object dictionary by SDO
querySDOifPending<uint8_t, uint8_t> (m_ols_query_intensity_of_lines[0], "2023sub1", m_max_num_retries_after_sdo_error, m_ols_state.intensity_of_lines[0], 1); // OLS20 only: query object 2023sub1 (intensity line 1, UINT8)
querySDOifPending<uint8_t, uint8_t> (m_ols_query_intensity_of_lines[1], "2023sub2", m_max_num_retries_after_sdo_error, m_ols_state.intensity_of_lines[1], 1); // OLS20 only: query object 2023sub2 (intensity line 2, UINT8)
querySDOifPending<uint8_t, uint8_t> (m_ols_query_intensity_of_lines[2], "2023sub3", m_max_num_retries_after_sdo_error, m_ols_state.intensity_of_lines[2], 1); // OLS20 only: query object 2023sub3 (intensity line 3, UINT8)
// Clear all pending status
m_ols_query_extended_code.pending() = false;
m_ols_query_device_status_u8.pending() = false;
m_ols_query_device_status_u16.pending() = false;
m_ols_query_error_register.pending() = false;
m_ols_query_barcode_center_point.pending() = false;
m_ols_query_quality_of_lines.pending() = false;
m_ols_query_intensity_of_lines[0].pending() = false;
m_ols_query_intensity_of_lines[1].pending() = false;
m_ols_query_intensity_of_lines[2].pending() = false;
m_max_sdo_query_rate.sleep();
}
}
/*
* @brief queries an object in the object dictionary by SDO and returns its value.
* @param[in] can_object_idx object index in object dictionary, f.e. "2018" (OLS device status) or "2021sub9" (OLS extended code)
* @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
* @param[out] can_object_value object value from SDO response
* @return true on success (can_object_value set to objects value), false otherwise (can_object_value not set)
*/
bool sick_line_guidance::CanSubscriber::MeasurementHandler::querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, uint8_t & can_object_value)
{
std::string can_object_entry = "";
if(querySDO(can_object_idx, max_num_retries_after_sdo_error, can_object_entry) && convertSDOresponse(can_object_entry, can_object_value))
return true;
ROS_ERROR_STREAM("querySDO(" << can_object_idx << ",uint8_t) failed, value=" << sick_line_guidance::MsgUtil::toHexString(can_object_value) );
return false;
}
/*
* @brief queries an object in the object dictionary by SDO and returns its value.
* @param[in] can_object_idx object index in object dictionary, f.e. "2018" (OLS device status) or "2021sub9" (OLS extended code)
* @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
* @param[out] can_object_value object value from SDO response
* @return true on success (can_object_value set to objects value), false otherwise (can_object_value not set)
*/
bool sick_line_guidance::CanSubscriber::MeasurementHandler::querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, int16_t & can_object_value)
{
int32_t value = 0;
std::string can_object_entry = "";
if(querySDO(can_object_idx, max_num_retries_after_sdo_error, can_object_entry) && convertSDOresponse(can_object_entry, value) && value >= INT16_MIN && value <= INT16_MAX)
{
can_object_value = (int16_t)value;
return true;
}
ROS_ERROR_STREAM("querySDO(" << can_object_idx << ",int16_t) failed, value=" << sick_line_guidance::MsgUtil::toHexString(can_object_value) );
return false;
}
/*
* @brief queries an object in the object dictionary by SDO and returns its value.
* @param[in] can_object_idx object index in object dictionary, f.e. "2018" (OLS device status) or "2021sub9" (OLS extended code)
* @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
* @param[out] can_object_value object value from SDO response
* @return true on success (can_object_value set to objects value), false otherwise (can_object_value not set)
*/
bool sick_line_guidance::CanSubscriber::MeasurementHandler::querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, uint16_t & can_object_value)
{
uint32_t value = 0;
std::string can_object_entry = "";
if(querySDO(can_object_idx, max_num_retries_after_sdo_error, can_object_entry) && convertSDOresponse(can_object_entry, value) && value <= UINT16_MAX)
{
can_object_value = (uint16_t)value;
return true;
}
ROS_ERROR_STREAM("querySDO(" << can_object_idx << ",uint16_t) failed, value=" << sick_line_guidance::MsgUtil::toHexString(can_object_value) );
return false;
}
/*
* @brief queries an object in the object dictionary by SDO and returns its value.
* @param[in] can_object_idx object index in object dictionary, f.e. "2018" (OLS device status) or "2021sub9" (OLS extended code)
* @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
* @param[out] can_object_value object value from SDO response
* @return true on success (can_object_value set to objects value), false otherwise (can_object_value not set)
*/
bool sick_line_guidance::CanSubscriber::MeasurementHandler::querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, uint32_t & can_object_value)
{
std::string can_object_entry = "";
if(querySDO(can_object_idx, max_num_retries_after_sdo_error, can_object_entry) && convertSDOresponse(can_object_entry, can_object_value))
return true;
ROS_ERROR_STREAM("querySDO(" << can_object_idx << ",uint32_t) failed, value=" << sick_line_guidance::MsgUtil::toHexString(can_object_value) );
return false;
}
/*
* @brief queries an object in the object dictionary by SDO and returns its value.
* @param[in] can_object_idx object index in object dictionary, f.e. "2018" (OLS device status) or "2021sub9" (OLS extended code)
* @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
* @param[out] can_object_value object value from SDO response
* @return true on success (can_object_value set to objects value), false otherwise (can_object_value not set)
*/
bool sick_line_guidance::CanSubscriber::MeasurementHandler::querySDO(const std::string & can_object_idx, int max_num_retries_after_sdo_error, std::string & can_object_value)
{
std::string can_msg = "";
can_object_value = "";
bool sdo_success = sick_line_guidance::CanopenChain::queryCanObject(m_nh, m_can_nodeid, can_object_idx, max_num_retries_after_sdo_error, can_msg, can_object_value);
if(sdo_success)
{
ROS_INFO_STREAM("sick_line_guidance::CanopenChain::queryCanObject(" << m_can_nodeid << "): [" << can_object_idx << "]=" << can_object_value );
/* sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::OK, "SDO"); */
}
else
{
ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(" << m_can_nodeid << "): [" << can_object_idx << "]=\"" << can_object_value << "\" failed " << can_msg);
sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::SDO_COMMUNICATION_ERROR, "querySDO failed");
}
return sdo_success;
}
/*
* @brief converts a sdo response to uint8.
* Note: nh.serviceClient<canopen_chain_node::GetObject> returns SDO responses as strings.
* In case of 1 Byte (UINT8) values, the returned "string" contains this byte (uint8 value streamed to std::stringstream).
* Example: UINT8 with value 0 are returned as "\0", not "0". Parsing the SDO response has to handle the UINT8 case,
* which is done by this function
* Note: std::exception are caught (error message and return false in this case)
* @param[in] response sdo response as string
* @param[out] value uint8 value converted from SDO response
* @return true on success, false otherwise
*/
bool sick_line_guidance::CanSubscriber::MeasurementHandler::convertSDOresponse(const std::string & response, uint8_t & value)
{
value = 0;
try
{
if(response.empty())
value = 0;
else if(response.size() == 1)
value = static_cast<uint8_t>(response[0]);
else
{
uint32_t u32val = std::stoul(response, 0, 0);
if((u32val & 0xFFFFFF00) != 0)
ROS_WARN_STREAM("convertSDOresponse(" << response << ") to UINT8: value " << u32val << " out of UINT8 range, possible loss of data.");
value = static_cast<uint8_t>(u32val & 0xFF);
}
return true;
}
catch(const std::exception & exc)
{
ROS_ERROR_STREAM("convertSDOresponse(" << response << ") to UINT8 failed: exception = " << exc.what());
}
return false;
}
/*
* @brief converts a sdo response to uint32.
* Note: std::exception are caught (error message and return false in this case)
* @param[in] response sdo response as string
* @param[out] value uint32 value converted from SDO response
* @return true on success, false otherwise
*/
bool sick_line_guidance::CanSubscriber::MeasurementHandler::convertSDOresponse(const std::string & response, uint32_t & value)
{
value = 0;
try
{
value = std::stoul(response, 0, 0);
return true;
}
catch(const std::exception & exc)
{
ROS_ERROR_STREAM("convertSDOresponse(" << response << ") to UINT32 failed: exception = " << exc.what());
}
return false;
}
/*
* @brief converts a sdo response to int32.
* Note: std::exception are caught (error message and return false in this case)
* @param[in] response sdo response as string
* @param[out] value uint32 value converted from SDO response
* @return true on success, false otherwise
*/
bool sick_line_guidance::CanSubscriber::MeasurementHandler::convertSDOresponse(const std::string & response, int32_t & value)
{
value = 0;
try
{
value = std::stol(response, 0, 0);
return true;
}
catch(const std::exception & exc)
{
ROS_ERROR_STREAM("convertSDOresponse(" << response << ") to INT32 failed: exception = " << exc.what());
}
return false;
}
/*
* @brief returns true, if publishing of a MLS measurement is scheduled and time has been reached for publishing the current MLS measurement.
*/
bool sick_line_guidance::CanSubscriber::MeasurementHandler::isMLSMeasurementTriggered(void)
{
boost::lock_guard<boost::mutex> schedule_lockguard(m_publish_measurement_mutex);
return !m_publish_mls_measurement.isZero() && ros::Time::now() > m_publish_mls_measurement;
}
/*
* @brief returns true, if publishing of a OLS measurement is scheduled and time has been reached for publishing the current OLS measurement.
*/
bool sick_line_guidance::CanSubscriber::MeasurementHandler::isOLSMeasurementTriggered(void)
{
boost::lock_guard<boost::mutex> schedule_lockguard(m_publish_measurement_mutex);
return !m_publish_ols_measurement.isZero() && ros::Time::now() > m_publish_ols_measurement;
}
/*
* @brief returns true, if publishing of a measurement is scheduled and latest time for publishing has been reached.
*/
bool sick_line_guidance::CanSubscriber::MeasurementHandler::isLatestTimeForMeasurementPublishing(void)
{
boost::lock_guard<boost::mutex> schedule_lockguard(m_publish_measurement_mutex);
return !m_publish_measurement_latest.isZero() && ros::Time::now() > m_publish_measurement_latest;
}
/*
* @brief returns true, if sdo query is pending, i.e. measurement is not yet completed (sdo request or sdo response still pending)
*/
bool sick_line_guidance::CanSubscriber::MeasurementHandler::isSDOQueryPending(void)
{
boost::lock_guard<boost::mutex> schedule_lockguard(m_publish_measurement_mutex);
return (m_ols_query_extended_code.pending() || m_ols_query_device_status_u8.pending() || m_ols_query_device_status_u16.pending() || m_ols_query_error_register.pending() || m_ols_query_barcode_center_point.pending()
|| m_ols_query_quality_of_lines.pending() || m_ols_query_intensity_of_lines[0].pending() || m_ols_query_intensity_of_lines[1].pending() || m_ols_query_intensity_of_lines[2].pending());
}
/*
* @brief schedules the publishing of the current MLS measurement message.
* @param[in] schedule if true, publishing is scheduled, otherwise a possibly pending schedule is removed.
*/
void sick_line_guidance::CanSubscriber::MeasurementHandler::schedulePublishMLSMeasurement(bool schedule)
{
boost::lock_guard<boost::mutex> schedule_lockguard(m_publish_measurement_mutex);
if(schedule && m_publish_mls_measurement.isZero()) // otherwise publishing the measurement is already scheduled
{
m_publish_mls_measurement = ros::Time::now() + m_schedule_publish_delay;
m_publish_measurement_latest = ros::Time::now() + m_max_publish_delay;
}
if(!schedule && !m_publish_mls_measurement.isZero()) // remove pending schedule
{
m_publish_mls_measurement = ros::Time(0);
m_publish_measurement_latest = ros::Time(0);
}
}
/*
* @brief schedules the publishing of the current OLS measurement message.
* @param[in] schedule if true, publishing is scheduled, otherwise a possibly pending schedule is removed.
*/
void sick_line_guidance::CanSubscriber::MeasurementHandler::schedulePublishOLSMeasurement(bool schedule)
{
boost::lock_guard<boost::mutex> schedule_lockguard(m_publish_measurement_mutex);
if(schedule && m_publish_ols_measurement.isZero()) // otherwise publishing the measurement is already scheduled
{
m_publish_ols_measurement = ros::Time::now() + m_schedule_publish_delay;
m_publish_measurement_latest = ros::Time::now() + m_max_publish_delay;
}
if(!schedule && !m_publish_ols_measurement.isZero()) // remove pending schedule
{
m_publish_ols_measurement = ros::Time(0);
m_publish_measurement_latest = ros::Time(0);
}
}
/*
* Constructor.
* @param[in] nh ros::NodeHandle
* @param[in] can_nodeid can id for canopen_chain_node, f.e. "node1"
* @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
* @param[in] max_sdo_rate max. sdo query and publish rate
* @param[in] initial_sensor_state initial sensor state (f.e. 0x07 for 3 detected lines, or (1 << 4) to indicate sensor error)
* @param[in] subscribe_queue_size buffer size for ros messages
*/
sick_line_guidance::CanSubscriber::CanSubscriber(ros::NodeHandle & nh, const std::string & can_nodeid, int max_num_retries_after_sdo_error, double max_sdo_rate, int initial_sensor_state, int subscribe_queue_size)
: m_measurement(nh, can_nodeid, max_num_retries_after_sdo_error, initial_sensor_state, max_sdo_rate, max_sdo_rate), m_subscribe_queue_size(subscribe_queue_size)
{
}
/*
* Destructor.
*/
sick_line_guidance::CanSubscriber::~CanSubscriber()
{
}
/*
* @brief converts an INT16 message (line center point lcp in millimeter) to a float lcp in meter.
*
* @param[in] msg INT16 message (line center point lcp in millimeter)
* @param[in] defaultvalue default, is returned in case of an invalid message
* @param[in] dbg_info informal debug message (ROS_INFO/ROS_ERROR)
*
* @return float lcp in meter
*/
float sick_line_guidance::CanSubscriber::convertToLCP(const boost::shared_ptr<std_msgs::Int16 const>& msg,
const float & defaultvalue, const std::string & dbg_info)
{
float value = defaultvalue;
if(msg)
{
int16_t data = ((msg->data)&0xFFFF);
value = data / 1000.0;
ROS_DEBUG("sick_line_guidance::CanSubscriber(%s): data: 0x%04x -> %.3f", dbg_info.c_str(), (int)(data), value);
}
else
{
ROS_ERROR("## ERROR sick_line_guidance::CanSubscriber(%s): invalid message (%s:%d)", dbg_info.c_str(), __FILE__, __LINE__);
}
return value;
}
/*
* @brief schedules the current MLS measurement message for publishing.
*/
void sick_line_guidance::CanSubscriber::publishMLSMeasurement(void)
{
m_measurement.schedulePublishMLSMeasurement(true);
}
/*
* @brief schedules the current OLS measurement message for publishing.
*/
void sick_line_guidance::CanSubscriber::publishOLSMeasurement(void)
{
m_measurement.schedulePublishOLSMeasurement(true);
}

550
Devices/Packages/sick_line_guidance/src/sick_line_guidance_canopen_chain.cpp Executable file
View File

@@ -0,0 +1,550 @@
/*
* sick_line_guidance_canopen_chain wraps and adapts RosChain of package canopen_chain_node for sick_line_guidance.
*
* class CanopenChain implements canopen support for sick_line_guidance
* based on RosChain of package canopen_chain_node (package ros_canopen).
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#include <ros/ros.h>
#include <std_srvs/Trigger.h>
#include "sick_line_guidance/sick_line_guidance_canopen_chain.h"
#include "sick_line_guidance/sick_line_guidance_diagnostic.h"
/*
* CanopenChain constructor
*
* @param[in] nh ros::NodeHandle
* @param[in] nh_priv ros::NodeHandle
*/
sick_line_guidance::CanopenChain::CanopenChain(const ros::NodeHandle &nh, const ros::NodeHandle &nh_priv)
: canopen::RosChain(nh, nh_priv)
{
}
/*
* CanopenChain destructor
*/
sick_line_guidance::CanopenChain::~CanopenChain()
{
}
/*
* Connects to CAN bus by calling "init" of canopen_chain_node ros-service
*
* @param[in] nh ros::NodeHandle
*
* @return true, if initialization successful, otherwise false.
*/
bool sick_line_guidance::CanopenChain::connectInitService(ros::NodeHandle &nh)
{
bool success = false;
try
{
ros::ServiceClient servclient = nh.serviceClient<std_srvs::Trigger>("/driver/init");
std_srvs::Trigger servtrigger;
success = servclient.call(servtrigger);
if(!success)
{
ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::connectInitService(): ServiceClient::call(\"/driver/init\") failed.");
sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::CAN_COMMUNICATION_ERROR, "CanopenChain: \"/driver/init\" failed");
}
else
{
ROS_INFO_STREAM("sick_line_guidance::CanopenChain::connectInitService(): ServiceClient::call(\"/driver/init\") successfull.");
sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::OK, "CanopenChain: \"/driver/init\" successfull");
}
}
catch(const std::exception & exc)
{
success = false;
ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::connectInitService(): ServiceClient::call(\"/driver/init\") failed: exception = " << exc.what());
sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::CAN_COMMUNICATION_ERROR, "CanopenChain: \"/driver/init\" failed");
}
return success;
}
/*
* Sets all dcf overlays in the can devices. All dcf values are reread from the can device and checked to be identical
* to the configured object value. If all dcf overlays have been set successfully (i.e. successfull write operation,
* successfull re-read operation and object value identical to the configured value), true is returned.
* Otherwise, false is returned (i.e. some dcf overlays could not be set).
*
* dcf overlays are a list of key value pairs with "object_index" as key and "object_value" as value.
* Examples for dcf overlays can be found in the yaml configuration file. Example from sick_line_guidance_ols20.yaml:
* node1:
* id: 0x0A # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS and MLS
* eds_pkg: sick_line_guidance # package name for relative path
* eds_file: SICK_OLS20.eds # path to EDS/DCF file
* ...
* dcf_overlay:
* "2001sub5": "0x01" # sensorFlipped, UINT8, DataType=0x0005, defaultvalue 0x00
* "2002sub1": "0.015" # Typ. Width, FLOAT32, DataType=0x0008
* "2002sub2": "0.001" # Min. Width, FLOAT32, DataType=0x0008
* "2002sub3": "0.050" # Max. Width, FLOAT32, DataType=0x0008
*
* @param[in] nh ros::NodeHandle
* @param[in] node_id can node id of the can device
* @param[in] dcf_overlays list of dcf overlays, format "object_index":"object_value"
*
* @return true, if dcf overlays set successfully, otherwise false (write error, read error
* or queried value differs from configured object value).
*/
bool sick_line_guidance::CanopenChain::writeDCFoverlays(ros::NodeHandle &nh, const std::string & node_id, XmlRpc::XmlRpcValue & dcf_overlays)
{
// int32_t i32_val = 0;
// uint32_t u32_val = 0;
// float f32_val = 0;
// assert(tryParseUINT32("1", u32_val) && u32_val == 1);
// assert(tryParseUINT32("0x01", u32_val) && u32_val == 1);
// assert(tryParseUINT32("255", u32_val) && u32_val == 255);
// assert(tryParseUINT32("0xFF", u32_val) && u32_val == 0xFF);
// assert(tryParseUINT32("0xFFFFFFFF", u32_val) && u32_val == 0xFFFFFFFF);
// assert(tryParseINT32("1", i32_val) && i32_val == 1);
// assert(tryParseINT32("-1", i32_val) && i32_val == -1);
// assert(tryParseINT32("0x7FFFFFFF", i32_val) && i32_val == 0x7FFFFFFF);
// assert(tryParseINT32("0xFFFFFFFF", i32_val) && i32_val == -1);
// assert(tryParseFLOAT("1.0", f32_val) && std::fabs(f32_val - 1) < 1.0e-6);
// assert(tryParseFLOAT("-1.0", f32_val) && std::fabs(f32_val + 1) < 1.0e-6);
// assert(tryParseFLOAT("1e6", f32_val) && std::fabs(f32_val - 1.0E6) < 1.0e-6);
// assert(tryParseFLOAT("1.0E06", f32_val) && std::fabs(f32_val - 1.0E6) < 1.0e-6);
// assert(tryParseFLOAT("1e-6", f32_val) && std::fabs(f32_val - 1.0E-6) < 1.0e-6);
// assert(tryParseFLOAT("1.0E-06", f32_val) && std::fabs(f32_val - 1.0E-6) < 1.0e-6);
bool success = true;
for(XmlRpc::XmlRpcValue::iterator dcf_overlay_iter = dcf_overlays.begin(); dcf_overlay_iter != dcf_overlays.end(); ++dcf_overlay_iter)
{
bool dcf_success = true;
std::string dcf_overlay_object_index = dcf_overlay_iter->first;
std::string dcf_overlay_object_value = dcf_overlay_iter->second;
std::string sdo_message = "", sdo_response = "";
// Set value in object dictionary
if(!setCanObjectValue(nh, node_id, dcf_overlay_object_index, dcf_overlay_object_value, sdo_response))
{
dcf_success = false;
ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::writeDCFoverlays(): setCanObjectValue(" << node_id << "," << dcf_overlay_object_index
<< ") failed: sdo_response:\"" << sdoResponseToString(sdo_response) << "\"(\"" << sdo_response << "\")");
sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::SDO_COMMUNICATION_ERROR, "writeDCFoverlays failed");
}
// Re-read value in object dictionary
if(!queryCanObject(nh, node_id, dcf_overlay_object_index, 0, sdo_message, sdo_response))
{
dcf_success = false;
ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::writeDCFoverlays(): queryCanObject(" << node_id << "," << dcf_overlay_object_index
<< ") failed: sdo_message:\"" << sdo_message << "\", sdo_response:\"" << sdoResponseToString(sdo_response) << "\"(\"" << sdo_response << "\")");
sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::SDO_COMMUNICATION_ERROR, "writeDCFoverlays failed");
}
else
{
ROS_INFO_STREAM("sick_line_guidance::CanopenChain::writeDCFoverlays(): queryCanObject(" << node_id << "," << dcf_overlay_object_index
<< ") successfull re-read, sdo_response:\"" << sdoResponseToString(sdo_response) << "\"(\"" << sdo_response << "\")");
}
// Compare destination value dcf_overlay_object_value with queried sdo response
if(!cmpDCFoverlay(dcf_overlay_object_value, sdo_response))
{
ROS_WARN_STREAM("sick_line_guidance::CanopenChain::writeDCFoverlays(): dcf_overlay \"" << dcf_overlay_object_index<< "\": \""
<< dcf_overlay_object_value << "\" possibly failed: queried value=\"" << sdoResponseToString(sdo_response) << "\", expected value=\"" << dcf_overlay_object_value << "\"");
}
if(dcf_success)
{
ROS_INFO_STREAM("sick_line_guidance::CanopenChain::writeDCFoverlays(): dcf_overlay \"" << dcf_overlay_object_index<< "\": \""
<< dcf_overlay_object_value << "\" sucessfull, value=\"" << sdoResponseToString(sdo_response) << "\"(\"" << sdo_response << "\")");
}
else
{
ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::writeDCFoverlays(): dcf_overlay \"" << dcf_overlay_object_index<< "\": \""
<< dcf_overlay_object_value << "\" failed, value=\"" << sdoResponseToString(sdo_response) << "\"(\"" << sdo_response << "\")");
}
success = dcf_success && success;
}
if(success)
sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::OK);
else
sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::SDO_COMMUNICATION_ERROR, "writeDCFoverlays failed");
return success;
}
/*
* Queries an object of a can node from canopen service by its object index.
*
* @param[in] nh ros::NodeHandle
* @param[in] node_id can node id of the can device
* @param[in] object index in the object dictonary of the can device, f.e. "1001sub", "1018sub4"
* @param[in] max_num_retries_after_sdo_error After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
* @param[out] output_message informational message in case of errors (responce from canopen service)
* @param[out] output_value value of the object (responce from canopen service)
*
* @return true, if query successful, otherwise false.
*/
bool sick_line_guidance::CanopenChain::queryCanObject(ros::NodeHandle &nh,
const std::string & node_id, const std::string & objectidx, int max_num_retries_after_sdo_error,
std::string & output_message, std::string & output_value)
{
bool sdo_success = false;
for(int retry_cnt = 0; sdo_success == false; retry_cnt++)
{
try
{
output_message = "";
output_value = "";
ros::ServiceClient servclient = nh.serviceClient<canopen_chain_node::GetObject>("/driver/get_object");
canopen_chain_node::GetObject servobject;
servobject.request.node = node_id;
servobject.request.object = objectidx;
servobject.request.cached = false;
if(servclient.call(servobject))
{
output_message = servobject.response.message;
output_value = servobject.response.value;
sdo_success = servobject.response.success;
if(!sdo_success)
{
ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(): ServiceClient::call(\"/driver/get_object\") failed: " << output_message);
}
}
else
{
ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(): ServiceClient::call(\"/driver/get_object\") failed.");
}
}
catch(const std::exception & exc)
{
sdo_success = false;
ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(" << node_id << "): [" << objectidx << "]=\"" << output_value << "\" failed: " << output_message << " exception = " << exc.what());
}
if(!sdo_success)
{
// SDO failed
sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::SDO_COMMUNICATION_ERROR, "CanopenChain::queryCanObject failed");
if (retry_cnt < max_num_retries_after_sdo_error)
{
ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(" << node_id << ") failed: SDO error, retrying...");
continue;
}
else
{
// SDO failed, reset communication ("driver/recover")
// ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(\" << node_id << \"): resetting can communication after SDO error");
// if(recoverCanDriver(nh))
// ROS_INFO_STREAM("sick_line_guidance::CanopenChain::queryCanObject(\" << node_id << \"): recoverCanDriver() successfull.");
// else
// ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(\" << node_id << \"): recoverCanDriver() failed.");
// SDO failed, shutdown communication ("driver/shutdown") and re-initialize ("driver/init")
ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(" << node_id << "): unrecoverable SDO error, retries not successful, giving up, initiating can driver shutdown and restart after SDO error");
if(shutdownCanDriver(nh))
ROS_INFO_STREAM("sick_line_guidance::CanopenChain::queryCanObject(" << node_id << "): shutdownCanDriver() successfull.");
else
ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(" << node_id << "): shutdownCanDriver() failed.");
if(connectInitService(nh))
ROS_INFO_STREAM("sick_line_guidance::CanopenChain::queryCanObject(" << node_id << "): connectInitService() successfull.");
else
ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::queryCanObject(" << node_id << "): connectInitService() failed.");
break;
}
}
}
return sdo_success;
}
/*
* Sets the value of an object in the object dictionary of a can device.
*
* @param[in] nh ros::NodeHandle
* @param[in] node_id can node id of the can device
* @param[in] object index in the object dictonary of the can device, f.e. "1001sub", "1018sub4"
* @param[in] object_value value of the object
* @param[out] response value of the object (responce from canopen service)
*
* @return true, if SDO successful, otherwise false.
*/
bool sick_line_guidance::CanopenChain::setCanObjectValue(ros::NodeHandle &nh, const std::string & node_id, const std::string & objectidx,
const std::string & object_value, std::string & response)
{
bool sdo_success = false;
try
{
response = "";
ros::ServiceClient servclient = nh.serviceClient<canopen_chain_node::SetObject>("/driver/set_object");
canopen_chain_node::SetObject servobject;
servobject.request.node = node_id;
servobject.request.object = objectidx;
servobject.request.value = object_value;
servobject.request.cached = false;
if(servclient.call(servobject))
{
response = servobject.response.message;
sdo_success = servobject.response.success;
if(!sdo_success)
{
ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::setCanObjectValue(): ServiceClient::call(\"/driver/set_object\") failed: " << response);
}
}
else
{
ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::setCanObjectValue(): ServiceClient::call(\"/driver/set_object\") failed.");
}
}
catch(const std::exception & exc)
{
sdo_success = false;
ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::setCanObjectValue(" << node_id << ", " << objectidx << ") failed: exception = " << exc.what());
}
if(!sdo_success)
sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::SDO_COMMUNICATION_ERROR, "CanopenChain::setCanObjectValue failed");
/* else
sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::OK); */
return sdo_success;
}
/*
* Recovers can driver after emergency or other errors. Calls "/driver/recover" from canopen_chain_node
*
* @param[in] nh ros::NodeHandle
*
* @return true, if recover command returned with success, otherwise false.
*/
bool sick_line_guidance::CanopenChain::recoverCanDriver(ros::NodeHandle & nh)
{
bool success = false;
try
{
ROS_WARN_STREAM("sick_line_guidance::CanopenChain::recoverCanDriver(): resetting can communication by ServiceClient::call(\"/driver/recover\")...");
ros::ServiceClient servclient = nh.serviceClient<std_srvs::Trigger>("/driver/recover");
std_srvs::Trigger servobject;
if(servclient.call(servobject))
{
success = servobject.response.success;
if(success)
{
ROS_INFO_STREAM("sick_line_guidance::CanopenChain::recoverCanDriver(): ServiceClient::call(\"/driver/recover\") successfull " << servobject.response.message.c_str());
}
else
{
ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::recoverCanDriver(): ServiceClient::call(\"/driver/recover\") failed: " << servobject.response.message.c_str());
}
}
else
{
ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::recoverCanDriver(): ServiceClient::call(\"/driver/recover\") failed.");
}
}
catch(const std::exception & exc)
{
success = false;
ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::recoverCanDriver() failed: exception = " << exc.what());
}
if(success)
sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::OK);
else
sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::SDO_COMMUNICATION_ERROR, "CanopenChain::recoverCanDriver failed");
return success;
}
/*
* Shutdown can driver. Calls "/driver/shutdown" from canopen_chain_node
*
* @param[in] nh ros::NodeHandle
*
* @return true, if shutdown command returned with success, otherwise false.
*/
bool sick_line_guidance::CanopenChain::shutdownCanDriver(ros::NodeHandle & nh)
{
bool success = false;
try
{
ROS_WARN_STREAM("sick_line_guidance::CanopenChain::shutdownCanDriver(): shutdown can communication by ServiceClient::call(\"/driver/shutdown\")...");
ros::ServiceClient servclient = nh.serviceClient<std_srvs::Trigger>("/driver/shutdown");
std_srvs::Trigger servobject;
if(servclient.call(servobject))
{
success = servobject.response.success;
if(success)
{
ROS_INFO_STREAM("sick_line_guidance::CanopenChain::shutdownCanDriver(): ServiceClient::call(\"/driver/shutdown\") successfull " << servobject.response.message.c_str());
}
else
{
ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::shutdownCanDriver(): ServiceClient::call(\"/driver/shutdown\") failed: " << servobject.response.message.c_str());
}
}
else
{
ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::shutdownCanDriver(): ServiceClient::call(\"/driver/shutdown\") failed.");
}
}
catch(const std::exception & exc)
{
success = false;
ROS_ERROR_STREAM("sick_line_guidance::CanopenChain::shutdownCanDriver() failed: exception = " << exc.what());
}
return success;
}
/*
* Compares a destination value configured by dcf_overlay with the sdo response.
* Comparison by string, integer or float comparison.
* Returns true, if both values are identical, or otherwise false.
* Note: dcf_overlay_value and sdo_response may represent identical numbers, but different strings (different number formatting)
* Therefor, both string and numbers are compared in this function.
*
* @param[in] dcf_overlay_value configured value
* @param[in] sdo_response received value
*
* @return true, if dcf_overlay_value is identical to sdo_response or has identical values.
*/
bool sick_line_guidance::CanopenChain::cmpDCFoverlay(const std::string & dcf_overlay_value, const std::string & sdo_response)
{
// compare strings
if(dcf_overlay_value == sdo_response)
return true;
float f_dcf_val = 0, f_sdo_val = 0;
int32_t i32_dcf_val = 0, i32_sdo_val = 0;
uint32_t u32_dcf_val = 0, u32_sdo_val = 0;
// try UINT8 comparison (1 byte sdo response)
if(sdo_response.empty())
return tryParseUINT32(dcf_overlay_value, u32_dcf_val) && u32_dcf_val == 0;
if(sdo_response.size() == 1)
return tryParseUINT32(dcf_overlay_value, u32_dcf_val) && u32_dcf_val == ((static_cast<uint8_t>(sdo_response[0])) & 0xFF);
// try UINT32, INT32 and FLOAT comparison (2, 3, or 4 byte sdo response)
return (tryParseUINT32(dcf_overlay_value, u32_dcf_val) && tryParseUINT32(sdo_response, u32_sdo_val) && u32_dcf_val == u32_sdo_val) // identical uint32
|| (tryParseINT32(dcf_overlay_value, i32_dcf_val) && tryParseINT32(sdo_response, i32_sdo_val) && i32_dcf_val == i32_sdo_val) // identical int32
|| (tryParseFLOAT(dcf_overlay_value, f_dcf_val) && tryParseFLOAT(sdo_response, f_sdo_val) && std::fabs(f_dcf_val - f_sdo_val) < 0.001); // float diff < 1 mm (OLS/MLS resolution)
}
/*
* Converts a string to UINT32. Catches conversion exceptions and returns false in case of number parsing errors.
*
* @param[in] str input string to be parsed
* @param[out] value output value (== input converted to number in case of success)
*
* @return true in case of success, false otherwise
*/
uint32_t sick_line_guidance::CanopenChain::tryParseUINT32(const std::string & str, uint32_t & value)
{
bool success = false;
try
{
value = std::stoul(str, 0, 0);
success = true;
}
catch(const std::exception & exc)
{
success = false;
}
return success;
}
/*
* Converts a string to INT32. Catches conversion exceptions and returns false in case of number parsing errors.
*
* @param[in] str input string to be parsed
* @param[out] value output value (== input converted to number in case of success)
*
* @return true in case of success, false otherwise
*/
int32_t sick_line_guidance::CanopenChain::tryParseINT32(const std::string & str, int32_t & value)
{
bool success = false;
try
{
value = std::stol(str, 0, 0);
success = true;
}
catch(const std::exception & exc)
{
success = false;
}
return success;
}
/*
* Converts a string to FLOAT32. Catches conversion exceptions and returns false in case of number parsing errors.
*
* @param[in] str input string to be parsed
* @param[out] value output value (== input converted to number in case of success)
*
* @return true in case of success, false otherwise
*/
int32_t sick_line_guidance::CanopenChain::tryParseFLOAT(const std::string & str, float & value)
{
bool success = false;
try
{
value = std::stof(str);
success = true;
}
catch(const std::exception & exc)
{
success = false;
}
return success;
}
/*
* Prints and converts a sdo response. 1 byte datatypes (UINT8) are converted to decimal (f.e. "\0" is returned as "0").
*
* @param[in] response sdo response
*
* @return printed response.
*/
std::string sick_line_guidance::CanopenChain::sdoResponseToString(const std::string & response)
{
if(response.empty())
return "0";
if(response.size() == 1)
{
uint32_t value = static_cast<uint32_t>((static_cast<uint8_t>(response[0])) & 0xFF);
std::stringstream str;
str << value;
return str.str();
}
return response;
}

413
Devices/Packages/sick_line_guidance/src/sick_line_guidance_cloud_converter.cpp Executable file
View File

@@ -0,0 +1,413 @@
/*
* class CloudConverter implements utility functions to convert measurement data to PointCloud2.
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#include <algorithm>
#include <sstream>
#include <string>
#include <vector>
#include <ros/ros.h>
#include "sick_line_guidance/sick_line_guidance_cloud_converter.h"
#include "sick_line_guidance/sick_line_guidance_msg_util.h"
/*
* @brief returns the status for each line (detected or not detected).
*
* @param[in] status status byte of measurement data
* @param[in] numlines number of lined (normally 3 lines)
*
* @return detection status for each line
*/
std::vector<bool> sick_line_guidance::CloudConverter::linestatus(uint8_t status, size_t numlines)
{
std::vector<bool> lines_valid;
numlines = std::max((size_t)3,numlines);
lines_valid.resize(numlines);
// Spezifikation OLS+MLS: Es gilt folgende Zuordnung:
// 0=000b => keine Spur gefunden
// 2=010b => eine Spur gefunden
// 3=011b => zwei Spuren gefunden: Weiche links
// 6=110b => zwei Spuren gefunden: Weiche rechts
// 7=111b => drei Spuren gefunden
lines_valid[0] = ((status & 0x01) > 0);
lines_valid[1] = ((status & 0x02) > 0);
lines_valid[2] = ((status & 0x04) > 0);
for(size_t n = 3; n < numlines; n++)
lines_valid[n] = false;
return lines_valid;
}
/*
* @brief returns the number of lines detected (1, 2 or 3 lines).
*
* @param[in] status status byte of measurement data
*
* @return number of lines detected by OLS or MLS
*/
int sick_line_guidance::CloudConverter::linenumber(uint8_t status)
{
std::vector<bool> lines_valid = linestatus(status, 3);
int linecnt = 0;
for(size_t n = 0; n < lines_valid.size(); n++)
{
if(lines_valid[n])
linecnt++;
}
return linecnt;
}
/*
* @brief returns true, if MLS measurement status is okay, or false otherwise.
*
* @param[in] measurement MLS measurement data
*
* @return true (measurement status okay), or false (measurement status not okay)
*/
bool sick_line_guidance::CloudConverter::measurementstatus(const sick_line_guidance::MLS_Measurement & measurement)
{
// Spezifikation status bit 0 MLS ("Line good"):
// 0 => keine Spur oder Spur zu schwach
// 1 => ausreichend starke Spur erkannt
return ((measurement.status & 0x1) != 0);
}
/*
* @brief converts OLS measurement data to PointCloud2.
*
* @param[in] measurement OLS measurement data
* @param[in]frame_id frame_id of PointCloud2 message
*
* @return sensor_msgs::PointCloud2 data converted from measurement
*/
sensor_msgs::PointCloud2 sick_line_guidance::CloudConverter::convert(const sick_line_guidance::OLS_Measurement &measurement, const std::string & frame_id)
{
ROS_DEBUG("CloudConverter::convert(OLS_Measurement)");
assert(sizeof(float) == sizeof(uint32_t));
// #ifdef DEBUG
// assert(flipBits(0x04030201) == 0x8040C020);
// assert(flipBits(0xFFAA5500) == 0x00AA55FF);
// #endif
sensor_msgs::PointCloud2 cloud;
// set header
cloud.header.stamp = measurement.header.stamp; // timestamp of measurement
cloud.header.frame_id = frame_id;
cloud.header.seq = 0;
// clear cloud data
cloud.height = 0;
cloud.width = 0;
cloud.data.clear();
// set data header
bool sensorOkay = sick_line_guidance::MsgUtil::statusOK(measurement);
int numMeasuredLines = linenumber(measurement.status);
if(!sensorOkay || numMeasuredLines < 1) // no lines detected
return cloud; // return empty PointCloud2
int numChannels = 12; // "x", "y", "z", "linewidth", "lineidx", "barcode", "status", "dev_status", "error", "barcode_center", "line_quality", "line_intensity"
std::string channelId[] = { "x", "y", "z", "linewidth", "lineidx", "barcode", "status", "dev_status", "error", "barcode_center", "line_quality", "line_intensity" };
cloud.height = 1;
cloud.width = numMeasuredLines; // normally we have 3 positions (3 lines, one position for each line)
cloud.is_bigendian = false;
cloud.is_dense = true;
cloud.point_step = numChannels * sizeof(float);
cloud.row_step = cloud.point_step * cloud.width;
cloud.fields.resize(numChannels);
for (int i = 0; i < numChannels; i++)
{
cloud.fields[i].name = channelId[i];
cloud.fields[i].offset = i * sizeof(float);
cloud.fields[i].count = 1;
}
for (int i = 0; i < numChannels; i++)
{
cloud.fields[i].datatype = sensor_msgs::PointField::UINT32; // default: datatype UINT32
}
cloud.fields[0].datatype = sensor_msgs::PointField::FLOAT32; // "x"
cloud.fields[1].datatype = sensor_msgs::PointField::FLOAT32; // "y"
cloud.fields[2].datatype = sensor_msgs::PointField::FLOAT32; // "z"
cloud.fields[3].datatype = sensor_msgs::PointField::FLOAT32; // "linewidth"
cloud.fields[9].datatype = sensor_msgs::PointField::FLOAT32; // "barcode_center"
// get barcode: barcode valid, if bit 7 of measurement.status is set
uint32_t barcode = 0;
if((measurement.status & 0x80) != 0)
{
barcode = ((measurement.barcode < 255) ? (measurement.barcode) : (measurement.extended_code));
}
// if((measurement.status & 0x40) != 0) // barcode flipped, if bit 6 of measurement.status is set)
// {
// uint32_t flipped_barcode = barcode;
// barcode = flipBits(flipped_barcode);
// #ifdef DEBUG
// assert(flipBits(barcode) == flipped_barcode);
// #endif
// }
// set data values
cloud.data.resize(cloud.row_step * cloud.height);
float* pfdata = reinterpret_cast<float*>(&cloud.data[0]);
uint32_t* pidata = reinterpret_cast<uint32_t*>(&cloud.data[0]);
std::vector<bool> lines_valid = linestatus(measurement.status, measurement.position.size());
for(size_t cloudIdx = 0, meaIdx = 0; meaIdx < measurement.position.size(); meaIdx++)
{
if(lines_valid[meaIdx])
{
pfdata[cloudIdx++] = 0; // "x" := 0
pfdata[cloudIdx++] = measurement.position[meaIdx]; // "y" := measurement.position[i]
pfdata[cloudIdx++] = 0; // "z" := 0
pfdata[cloudIdx++] = measurement.width[meaIdx]; // "linewidth" := measurement.width[i]
pidata[cloudIdx++] = meaIdx + 1; // "lineidx": 1:=LCP1, 2:=LCP2, 3:=LCP3
pidata[cloudIdx++] = barcode; // "barcode"
pidata[cloudIdx++] = measurement.status; // "status"
pidata[cloudIdx++] = measurement.dev_status; // "dev_status"
pidata[cloudIdx++] = measurement.error; // "error"
pfdata[cloudIdx++] = measurement.barcode_center_point; // "barcode_center" (OLS20 only, OLS10: always 0)
pidata[cloudIdx++] = measurement.quality_of_lines; // "line_quality" (OLS20 only, OLS10: always 0)
pidata[cloudIdx++] = measurement.intensity_of_lines[meaIdx]; // "line_intensity" (OLS20 only, OLS10: always 0)
}
}
ROS_DEBUG("CloudConverter::convert(OLS_Measurement): sensorOkay=%d, numMeasuredLines=%d, numChannels=%d, numBytes=%d",
(int)sensorOkay, numMeasuredLines, numChannels, (int)cloud.data.size());
return cloud;
}
/*
* @brief converts OLS measurement data to PointCloud2.
*
* @param[in] measurement OLS measurement data
* @param[in]frame_id frame_id of PointCloud2 message
*
* @return sensor_msgs::PointCloud2 data converted from measurement
*/
sensor_msgs::PointCloud2 sick_line_guidance::CloudConverter::convert(const sick_line_guidance::MLS_Measurement &measurement, const std::string & frame_id)
{
ROS_DEBUG("CloudConverter::convert(MLS_Measurement)");
assert(sizeof(float) == sizeof(uint32_t));
sensor_msgs::PointCloud2 cloud;
// set header
cloud.header.stamp = measurement.header.stamp; // timestamp of measurement
cloud.header.frame_id = frame_id;
cloud.header.seq = 0;
// clear cloud data
cloud.height = 0;
cloud.width = 0;
cloud.data.clear();
// set data header
bool sensorOkay = measurementstatus(measurement);
int numMeasuredLines = linenumber(measurement.lcp);
if(!sensorOkay || numMeasuredLines < 1) // no lines detected
return cloud; // return empty PointCloud2
int numChannels = 8; // "x", "y", "z", "lineidx", "barcode", "lcp", "status", "error"
std::string channelId[] = { "x", "y", "z", "lineidx", "marker", "lcp", "status", "error" };
cloud.height = 1;
cloud.width = numMeasuredLines; // normally we have 3 positions (3 lines, one position for each line)
cloud.is_bigendian = false;
cloud.is_dense = true;
cloud.point_step = numChannels * sizeof(float); // length of a point in bytes
cloud.row_step = cloud.point_step * cloud.width; // length of a row in bytes
cloud.fields.resize(numChannels);
for (int i = 0; i < numChannels; i++)
{
cloud.fields[i].name = channelId[i];
cloud.fields[i].offset = i * sizeof(float);
cloud.fields[i].count = 1;
}
for (int i = 0; i < 3; i++)
{
cloud.fields[i].datatype = sensor_msgs::PointField::FLOAT32; // "x", "y", "z"
}
cloud.fields[3].datatype = sensor_msgs::PointField::UINT32;// "lineidx"
cloud.fields[4].datatype = sensor_msgs::PointField::INT32;// "marker"
for (int i = 5; i < numChannels; i++)
{
cloud.fields[i].datatype = sensor_msgs::PointField::UINT32; // "lcp", "status", "error"
}
// get marker: marker valid, if bit 6 of measurement.status is set
int32_t marker = 0;
if((measurement.status & 0x40) != 0)
{
marker = ((measurement.lcp >> 4) & 0x0F); // Bit 4 - bit 7 of lcp: marker bit
if((measurement.lcp & 0x08) != 0) // // Bit 3 of lcp: sign bit of marker
marker = -marker;
}
// set data values
cloud.data.resize(cloud.row_step * cloud.height);
float* pfdata = reinterpret_cast<float*>(&cloud.data[0]);
uint32_t* pidata = reinterpret_cast<uint32_t*>(&cloud.data[0]);
std::vector<bool> lines_valid = linestatus(measurement.lcp, measurement.position.size());
for(size_t cloudIdx = 0, meaIdx = 0; meaIdx < measurement.position.size(); meaIdx++)
{
if(lines_valid[meaIdx])
{
pfdata[cloudIdx++] = 0; // "x" := 0
pfdata[cloudIdx++] = measurement.position[meaIdx]; // "y" := measurement.position[i]
pfdata[cloudIdx++] = 0; // "z" := 0
pidata[cloudIdx++] = meaIdx + 1; // "lineidx": 1:=LCP1, 2:=LCP2, 3:=LCP3
pidata[cloudIdx++] = marker; // "marker"
pidata[cloudIdx++] = measurement.lcp; // "lcp"
pidata[cloudIdx++] = measurement.status; // "status"
pidata[cloudIdx++] = measurement.error; // "error"
}
}
ROS_DEBUG("CloudConverter::convert(MLS_Measurement): sensorOkay=%d, numMeasuredLines=%d, numPositions=%d, numChannels=%d, numBytes=%d",
(int)sensorOkay, numMeasuredLines, (int)measurement.position.size(), numChannels, (int)cloud.data.size());
return cloud;
}
template<class T> static std::string convertCloudDataElement(const uint8_t* data, const uint8_t* end, bool printhex)
{
std::stringstream out;
const T* pElement = reinterpret_cast<const T*>(data);
if(data + sizeof(*pElement) <= end)
{
if(printhex)
{
out << "0x" << std::uppercase << std::hex << std::setfill('0') << std::setw(2*sizeof(*pElement)) << (uint32_t)(*pElement);
}
else
{
out << (double)(*pElement);
}
data += sizeof(*pElement);
}
return out.str();
}
/*
* @brief converts and prints a single field of PointCloud2 according to its dataype.
*
* @param[in] datatype enumeration of the datatye, see sensor_msgs::PointField
* @param[in] data pointer to the data to be converted and printed
* @param[in] end pointer to the end of PointCloud2 data
*
* @return data field converted to string
*/
std::string sick_line_guidance::CloudConverter::cloudDataFieldToString(uint8_t datatype, const uint8_t* data, const uint8_t* end)
{
switch(datatype)
{
case sensor_msgs::PointField::INT8:
return convertCloudDataElement<int8_t>(data, end, true);
case sensor_msgs::PointField::UINT8:
return convertCloudDataElement<uint8_t>(data, end, true);
case sensor_msgs::PointField::INT16:
return convertCloudDataElement<int16_t>(data, end, true);
case sensor_msgs::PointField::UINT16:
return convertCloudDataElement<uint16_t>(data, end, true);
case sensor_msgs::PointField::INT32:
return convertCloudDataElement<int32_t>(data, end, true);
case sensor_msgs::PointField::UINT32:
return convertCloudDataElement<uint32_t>(data, end, true);
case sensor_msgs::PointField::FLOAT32:
return convertCloudDataElement<float>(data, end, false);
case sensor_msgs::PointField::FLOAT64:
return convertCloudDataElement<double>(data, end, false);
default:
break;
}
return "";
}
/*
* @brief prints a PointCloud2 data field to string.
*
* @param[in] cloud PointCloud2 data
*
* @return PointCloud2 data converted string
*/
std::string sick_line_guidance::CloudConverter::cloudDataToString(const sensor_msgs::PointCloud2 & cloud)
{
std::stringstream out;
size_t numChannels = cloud.fields.size();
size_t numDataBytes = cloud.data.size();
const uint8_t* pCloudData = reinterpret_cast<const uint8_t*>(&cloud.data[0]);
const uint8_t* pCloudLast = &pCloudData[numDataBytes];
ROS_DEBUG("CloudConverter::cloudDataToString(): cloud.height=%d, cloud.width=%d, numChannels=%d, numDataBytes=%d",(int)cloud.height, (int)cloud.width, (int)numChannels, (int)numDataBytes);
if(cloud.height > 0 && cloud.width > 0 && numChannels > 0 && numDataBytes > 0 && pCloudData != 0)
{
for(size_t y = 0; y < cloud.height; y++)
{
for (size_t x = 0; x < cloud.width; x++)
{
const uint8_t* pFieldData = pCloudData + y * cloud.row_step + x * cloud.point_step;
for (size_t n = 0; n < numChannels && pFieldData < pCloudLast; n++)
{
out << cloud.fields[n].name << ":";
const uint8_t* pChannelData = pFieldData + cloud.fields[n].offset;
for (size_t m = 0; m < cloud.fields[n].count && pChannelData < pCloudLast; m++)
{
std::string element = cloudDataFieldToString(cloud.fields[n].datatype, pChannelData, pCloudLast);
out << element << ",";
}
}
}
}
}
return out.str();
}
/*
* @brief flips the bits of a code, i.e. reverses the bits (output bit 0 := input bit 31, output bit 1 := input bit 30, and so on)
*
* @param[in] code bits to be flipped
*
* @return flipped code
*/
uint32_t sick_line_guidance::CloudConverter::flipBits(uint32_t code)
{
uint32_t flipped = 0;
for(uint32_t shift = 0; shift < 32; shift++)
{
flipped = (flipped << 1);
flipped = flipped | (code & 0x1);
code = code >> 1;
}
return flipped;
}

162
Devices/Packages/sick_line_guidance/src/sick_line_guidance_cloud_publisher.cpp Executable file
View File

@@ -0,0 +1,162 @@
/*
* sick_line_guidance_cloud_publisher implements a ros-node to convert sensor measurement data to PointCloud2 data.
* Subscribes topics "/ols" and "/mls", converts the sensor data and publishes sensor positions on topic "/cloud".
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <vector>
#include "sick_line_guidance/sick_line_guidance_version.h"
#include "sick_line_guidance/sick_line_guidance_cloud_converter.h"
#include "sick_line_guidance/sick_line_guidance_msg_util.h"
ros::Publisher cloud_publisher;
std::string mls_cloud_frame_id ="mls_frame";
std::string ols_cloud_frame_id ="ols_frame";
/*
* Callback for OLS measurement messages (topic ("/ols").
* Converts the sensor data of type OLS_Measurement and publishes a PointCloud2 message on topic "/cloud".
*
* @param[in] msg OLS measurement messages (topic ("/ols")
*/
void olsMessageCb(const boost::shared_ptr<sick_line_guidance::OLS_Measurement const>& msg)
{
if(msg)
{
sensor_msgs::PointCloud2 cloud = sick_line_guidance::CloudConverter::convert(*msg, ols_cloud_frame_id);
if(!cloud.data.empty())
{
cloud_publisher.publish(cloud);
std::string cloudmsg = sick_line_guidance::CloudConverter::cloudDataToString(cloud);
ROS_INFO("sick_line_guidance_cloud_publisher: OLS PointCloud data: {%s}", cloudmsg.c_str());
}
else
{
ROS_INFO("sick_line_guidance_cloud_publisher: invalid OLS measurement data, no line detected.");
}
}
else
{
ROS_ERROR("## ERROR sick_line_guidance_cloud_publisher::olsMessageCb(): invalid message (%s:%d)", __FILE__, __LINE__);
}
}
/*
* Callback for MLS measurement messages (topic ("/mls").
* Converts the sensor data of type MLS_Measurement and publishes a PointCloud2 message on topic "/cloud".
*
* @param[in] msg MLS measurement messages (topic ("/mls")
*/
void mlsMessageCb(const boost::shared_ptr<sick_line_guidance::MLS_Measurement const>& msg)
{
if(msg)
{
sensor_msgs::PointCloud2 cloud = sick_line_guidance::CloudConverter::convert(*msg, mls_cloud_frame_id);
if(!cloud.data.empty())
{
cloud_publisher.publish(cloud);
std::string cloudmsg = sick_line_guidance::CloudConverter::cloudDataToString(cloud);
ROS_INFO("sick_line_guidance_cloud_publisher: MLS PointCloud data: {%s}", cloudmsg.c_str());
}
else
{
ROS_INFO("sick_line_guidance_cloud_publisher: invalid MLS measurement data, no line detected.");
}
}
else
{
ROS_ERROR("## ERROR sick_line_guidance_cloud_publisher::mlsMessageCb(): invalid message (%s:%d)", __FILE__, __LINE__);
}
}
/*
* sick_line_guidance_cloud_publisher implements a ros-node to convert sensor measurement data to PointCloud2 data.
* Subscribes topics "/ols" and "/mls", converts the sensor data and publishes sensor positions on topic "/cloud".
*/
int main(int argc, char** argv)
{
ros::init(argc, argv, "sick_line_guidance_cloud_publisher");
ros::NodeHandle nh;
ros::NodeHandle nh_priv("~");
int subscribe_queue_size = 1;
std::string ols_topic_publish = "ols", mls_topic_publish = "mls", cloud_topic_publish = "cloud";
nh_priv.param("mls_topic_publish", mls_topic_publish, mls_topic_publish);
nh_priv.param("ols_topic_publish", ols_topic_publish, ols_topic_publish);
nh_priv.param("cloud_topic_publish", cloud_topic_publish, cloud_topic_publish);
nh_priv.param("mls_cloud_frame_id", mls_cloud_frame_id, mls_cloud_frame_id);
nh_priv.param("ols_cloud_frame_id", ols_cloud_frame_id, ols_cloud_frame_id);
nh_priv.param("subscribe_queue_size", subscribe_queue_size, subscribe_queue_size); // buffer size for ros messages
ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: version " << sick_line_guidance::Version::getVersionInfo());
ROS_INFO_STREAM("sick_line_guidance_cloud_publisher started.");
ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: mls_topic=" << mls_topic_publish);
ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: ols_topic=" << ols_topic_publish);
ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: cloud_topic=" << cloud_topic_publish);
ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: mls_frame_id=" << mls_cloud_frame_id);
ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: ols_frame_id=" << ols_cloud_frame_id);
ROS_INFO_STREAM("sick_line_guidance_cloud_publisher: queue_size=" << subscribe_queue_size);
cloud_publisher = nh.advertise<sensor_msgs::PointCloud2>(cloud_topic_publish, 1);
ros::Subscriber ols_subscriber = nh.subscribe(ols_topic_publish, subscribe_queue_size, olsMessageCb);
ros::Subscriber mls_subscriber = nh.subscribe(mls_topic_publish, subscribe_queue_size, mlsMessageCb);
ros::spin();
std::cout << "sick_line_guidance_cloud_publisher finished." << std::endl;
ROS_INFO_STREAM("sick_line_guidance_cloud_publisher finished.");
return 0;
}

134
Devices/Packages/sick_line_guidance/src/sick_line_guidance_diagnostic.cpp Executable file
View File

@@ -0,0 +1,134 @@
/*
* sick_line_guidance_diagnostic publishes diagnostic messages for sick_line_guidance
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#include <ros/ros.h>
#include <diagnostic_msgs/DiagnosticArray.h>
#include <diagnostic_msgs/DiagnosticStatus.h>
#include "sick_line_guidance/sick_line_guidance_diagnostic.h"
/*
* g_diagnostic_handler: singleton, implements the diagnostics for sick_line_guidance
*/
sick_line_guidance::Diagnostic::DiagnosticImpl sick_line_guidance::Diagnostic::g_diagnostic_handler;
/*
* Constructor.
*/
sick_line_guidance::Diagnostic::DiagnosticImpl::DiagnosticImpl() : m_diagnostic_initialized(false), m_diagnostic_component("")
{
}
/*
* Initialization.
*
* @param[in] nh ros::NodeHandle
* @param[in] publish_topic ros topic to publish diagnostic messages
* @param[in] component description of the component reporting
*/
void sick_line_guidance::Diagnostic::DiagnosticImpl::init(ros::NodeHandle &nh, const std::string & publish_topic, const std::string & component)
{
m_diagnostic_publisher = nh.advertise<diagnostic_msgs::DiagnosticArray>(publish_topic, 1);
m_diagnostic_component = component;
m_diagnostic_initialized = true;
}
/*
* Updates and reports the current status.
*
* @param[in] status current status (OK, ERROR, ...)
* @param[in] message optional diagnostic message
*/
void sick_line_guidance::Diagnostic::DiagnosticImpl::update(DIAGNOSTIC_STATUS status, const std::string &message)
{
if (m_diagnostic_initialized)
{
static std::map<DIAGNOSTIC_STATUS, std::string> status_description = {
{DIAGNOSTIC_STATUS::OK, "OK"},
{DIAGNOSTIC_STATUS::EXIT, "EXIT"},
{DIAGNOSTIC_STATUS::NO_LINE_DETECTED, "NO_LINE_DETECTED"},
{DIAGNOSTIC_STATUS::ERROR_STATUS, "ERROR_STATUS"},
{DIAGNOSTIC_STATUS::SDO_COMMUNICATION_ERROR, "SDO_COMMUNICATION_ERROR"},
{DIAGNOSTIC_STATUS::CAN_COMMUNICATION_ERROR, "CAN_COMMUNICATION_ERROR"},
{DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, "CONFIGURATION_ERROR"},
{DIAGNOSTIC_STATUS::INITIALIZATION_ERROR, "INITIALIZATION_ERROR"},
{DIAGNOSTIC_STATUS::INTERNAL_ERROR, "INTERNAL_ERROR"}
};
// create DiagnosticStatus
diagnostic_msgs::DiagnosticStatus msg;
msg.level = (status == DIAGNOSTIC_STATUS::OK) ? (diagnostic_msgs::DiagnosticStatus::OK) : (diagnostic_msgs::DiagnosticStatus::ERROR); // Level of operation
msg.name = m_diagnostic_component; // description of the test/component reporting
msg.hardware_id = ""; // hardware unique string (tbd)
msg.values.clear(); // array of values associated with the status
// description of the status
msg.message = status_description[status];
if(msg.message.empty())
{
msg.message = "ERROR";
}
if (!message.empty())
{
msg.message = msg.message + ": " + message;
}
// publish DiagnosticStatus in DiagnosticArray
diagnostic_msgs::DiagnosticArray msg_array;
msg_array.header.stamp = ros::Time::now();
msg_array.header.frame_id = "";
msg_array.status.clear();
msg_array.status.push_back(msg);
m_diagnostic_publisher.publish(msg_array);
}
}

170
Devices/Packages/sick_line_guidance/src/sick_line_guidance_eds_util.cpp Executable file
View File

@@ -0,0 +1,170 @@
/*
* class EdsUtil implements utility functions for eds-files.
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#include <iomanip>
#include <boost/filesystem.hpp>
#include <ros/ros.h>
#include <ros/package.h>
#include "sick_line_guidance/sick_line_guidance_eds_util.h"
/*
* @brief returns a full qualified filepath from package name and file name.
*
* @param package package name
*
* @return full qualified filepath (or filename if package is empty or couldn't be found).
*/
std::string sick_line_guidance::EdsUtil::filepathFromPackage(const std::string & package, const std::string & filename)
{
std::string filepath = filename;
if(!package.empty())
{
std::string pkg_path = ros::package::getPath(package);
if(!pkg_path.empty())
{
filepath = (boost::filesystem::path(pkg_path)/filepath).make_preferred().native();
}
}
return filepath;
}
/*
* @brief reads and parses an eds-file and prints all objects.
*
* @param eds_filepath path to eds-file
*
* @return object dictionary of a given eds-file printed to string
*/
std::string sick_line_guidance::EdsUtil::readParsePrintEdsfile(const std::string & eds_filepath)
{
std::stringstream object_dictionary_msg;
try
{
// ROS_DEBUG("sick_line_guidance: object_dictionary from eds_file \"%s\":", eds_filepath.c_str());
canopen::ObjectDict::Overlay object_overlay;
canopen::ObjectDictSharedPtr object_dictionary = canopen::ObjectDict::fromFile(eds_filepath, object_overlay);
canopen::ObjectDict::ObjectDictMap::const_iterator object_dictionary_iter;
while (object_dictionary->iterate(object_dictionary_iter))
{
const canopen::ObjectDict::Key &key = object_dictionary_iter->first;
const canopen::ObjectDict::Entry &entry = *object_dictionary_iter->second;
object_dictionary_msg << key << "=(" << printObjectDictEntry(entry) << ")\n";
}
// ROS_DEBUG("%s", object_dictionary_msg.str().c_str());
}
catch (const canopen::ParseException & err)
{
std::stringstream err_msg;
err_msg << "sick_line_guidance::readParsePrintEdsfile(" << eds_filepath << ") failed: " << err.what();
ROS_ERROR("%s", err_msg.str().c_str());
}
return object_dictionary_msg.str();
}
/*
* @brief prints the data of a canopen::ObjectDict::Entry value to std::string.
*
* @param entry object dictionary entry to be printed
*
* @return entry converted to string.
*/
std::string sick_line_guidance::EdsUtil::printObjectDictEntry(const canopen::ObjectDict::Entry & entry)
{
std::stringstream msg;
msg << "desc:" << entry.desc << ",";
msg << "data_type:" << (int)(entry.data_type) << ",";
msg << "index:" << canopen::ObjectDict::Key(entry.index, entry.sub_index) << ",";
msg << "constant:" << entry.constant << ",";
msg << "readable:" << entry.readable << ",";
msg << "writable:" << entry.writable << ",";
msg << "mappable:" << entry.mappable << ",";
msg << "obj_code:" << (int)(entry.obj_code) << ",";
msg << printHoldAny("value", entry.value()) << ",";
msg << printHoldAny("def_val", entry.def_val) << ",";
msg << printHoldAny("init_val", entry.init_val);
return msg.str();
}
/*
* @brief prints the data bytes of a canopen::HoldAny value to std::string.
*
* @param parameter_name name of the parameter
* @param holdany value to be printed
*
* @return value converted to string.
*/
std::string sick_line_guidance::EdsUtil::printHoldAny(const std::string & parameter_name, const canopen::HoldAny & holdany)
{
std::stringstream msg;
if(!holdany.is_empty())
{
size_t value_size = holdany.type().get_size();
msg << parameter_name << "_size:" << value_size << "," << parameter_name << "_data:0x";
const canopen::String & value_data = holdany.data();
canopen::String::const_iterator iter=value_data.cbegin();
for(size_t n = 0; iter != value_data.cend() && n < value_size; iter++, n++)
{
msg << std::uppercase << std::hex << std::setfill('0') << std::setw(2) << (unsigned)((*iter)&0xFF);
}
if(iter != value_data.cend())
{
msg << "...";
}
}
else
{
msg << parameter_name << ":none";
}
return msg.str();
}

238
Devices/Packages/sick_line_guidance/src/sick_line_guidance_msg_util.cpp Executable file
View File

@@ -0,0 +1,238 @@
/*
* class EdsUtil implements utility functions for eds-files.
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#include <ros/ros.h>
#include <random_numbers/random_numbers.h>
#include "sick_line_guidance/sick_line_guidance_msg_util.h"
/*
* @brief prints and returns a MLS measurement as informational string
* @param[in] measurement_msg MLS measurement to print
* @return informational string containing the MLS measurement data
*/
std::string sick_line_guidance::MsgUtil::toInfo(const sick_line_guidance::MLS_Measurement & measurement_msg)
{
std::stringstream info;
info << "position:[" << std::fixed << std::setprecision(3) << measurement_msg.position[0]
<< "," << std::fixed << std::setprecision(3) << measurement_msg.position[1]
<< "," << std::fixed << std::setprecision(3) << measurement_msg.position[2]
<< "],status=" << sick_line_guidance::MsgUtil::toHexString(measurement_msg.status)
<< ",lcpflags=" << sick_line_guidance::MsgUtil::toHexString(measurement_msg.lcp)
<< ",error=" << sick_line_guidance::MsgUtil::toHexString(measurement_msg.error);
return info.str();
}
/*
* @brief prints and returns a OLS measurement as informational string
* @param[in] measurement_msg OLS measurement to print
* @return informational string containing the OLS measurement data
*/
std::string sick_line_guidance::MsgUtil::toInfo(const sick_line_guidance::OLS_Measurement & measurement_msg)
{
uint32_t barcode = measurement_msg.barcode;
if(measurement_msg.barcode >= 255)
barcode = measurement_msg.extended_code;
std::stringstream info;
info << "position:[" << std::fixed << std::setprecision(3) << measurement_msg.position[0]
<< "," << std::fixed << std::setprecision(3) << measurement_msg.position[1]
<< "," << std::fixed << std::setprecision(3) << measurement_msg.position[2]
<< "],width:[" << std::fixed << std::setprecision(3) << measurement_msg.width[0]
<< "," << std::fixed << std::setprecision(3) << measurement_msg.width[1]
<< "," << std::fixed << std::setprecision(3) << measurement_msg.width[2]
<< "],status=" << sick_line_guidance::MsgUtil::toHexString(measurement_msg.status)
<< ",devstatus=" << sick_line_guidance::MsgUtil::toHexString(measurement_msg.dev_status)
<< ",error=" << sick_line_guidance::MsgUtil::toHexString(measurement_msg.error)
<< ",barcode=0x" << std::uppercase << std::hex << std::setfill('0') << std::setw(2) << barcode
<< ",barcodecenter=" << std::fixed << std::setprecision(3) << measurement_msg.barcode_center_point
<< ",linequality=" << sick_line_guidance::MsgUtil::toHexString(measurement_msg.quality_of_lines)
<< ",lineintensity=[" << sick_line_guidance::MsgUtil::toHexString(measurement_msg.intensity_of_lines[0])
<< "," << sick_line_guidance::MsgUtil::toHexString(measurement_msg.intensity_of_lines[1])
<< "," << sick_line_guidance::MsgUtil::toHexString(measurement_msg.intensity_of_lines[2]) << "]";
return info.str();
}
/*
* @brief initializes a MLS measurement with zero data
* @param[in+out] measurement_msg MLS measurement
*/
void sick_line_guidance::MsgUtil::zero(sick_line_guidance::MLS_Measurement & measurement_msg)
{
measurement_msg.header.stamp = ros::Time(0);
measurement_msg.header.frame_id = "";
measurement_msg.position = { 0, 0, 0 };
measurement_msg.status = 0;
measurement_msg.lcp = 0;
measurement_msg.error = 0;
}
/*
* @brief initializes an OLS measurement with zero data
* @param[in+out] measurement_msg OLS measurement
*/
void sick_line_guidance::MsgUtil::zero(sick_line_guidance::OLS_Measurement & measurement_msg)
{
measurement_msg.header.stamp = ros::Time(0);
measurement_msg.header.frame_id = "";
measurement_msg.position = { 0, 0, 0 };
measurement_msg.width = { 0, 0, 0 };
measurement_msg.status = 0;
measurement_msg.barcode = 0;
measurement_msg.extended_code = 0;
measurement_msg.dev_status = 0;
measurement_msg.error = 0;
measurement_msg.barcode_center_point = 0;
measurement_msg.quality_of_lines = 0;
measurement_msg.intensity_of_lines = { 0, 0, 0 };
}
/*
* @brief Returns a MLS sensor measurement
*
* @param[in] lcp1 line center point LCP1 in meter, object 0x2021sub1 in object dictionary
* @param[in] lcp2 line center point LCP2 in meter, object 0x2021sub2 in object dictionary
* @param[in] lcp3 line center point LCP3 in meter, object 0x2021sub3 in object dictionary
* @param[in] status status of measurement, object 0x2022 in object dictionary (Bit7=MSBit to Bit0=LSBit): Bit7: 0, Bit6: ReadingCode, Bit5: Polarity, Bit4: SensorFlipped, Bit3: LineLevBit2, Bit2: LineLevBit1, Bit1: LineLevBit0, Bit0: Linegood(1:LineGood,0:NotGood)
* @param[in] lcp LCP-flags (signs and line assignment), in bits (MSB to LSB): Bit7: MarkerBit4, Bit6: MarkerBit3, Bit5: MarkerBit2, Bit4: MarkerBit1, Bit3: MarkerBit0, Bit2: #LCPBit2, Bit1: #LCPBit1, Bit0: #LCPBit0
* @param[in] error error register, object 0x1001 in object dictionary
* @param[in] msg_frame_id frame id of OLS_Measurement message
*
* @return parameter converted to MLS_Measurement
*/
sick_line_guidance::MLS_Measurement sick_line_guidance::MsgUtil::convertMLSMessage(float lcp1, float lcp2, float lcp3, uint8_t status, uint8_t lcp, uint8_t error, const std::string & msg_frame_id)
{
sick_line_guidance::MLS_Measurement mls_message;
mls_message.header.stamp = ros::Time::now();
mls_message.header.frame_id = msg_frame_id;
mls_message.position = { lcp1, lcp2, lcp3 };
mls_message.status = status;
mls_message.lcp = lcp;
mls_message.error = error;
return mls_message;
}
/*
* @brief Returns an OLS sensor measurement
*
* @param[in] lcp1 line center point LCP1 in meter, object 0x2021sub1 in object dictionary
* @param[in] lcp2 line center point LCP2 in meter, object 0x2021sub2 in object dictionary
* @param[in] lcp3 line center point LCP3 in meter, object 0x2021sub3 in object dictionary
* @param[in] width1 width of line 1 in meter, object 0x2021sub5 in object dictionary
* @param[in] width2 width of line 2 in meter, object 0x2021sub6 in object dictionary
* @param[in] width3 width of line 3 in meter, object 0x2021sub7 in object dictionary
* @param[in] status status of measurement, object 0x2021sub4 in object dictionary, in bits (MSB to LSB): Bit7: CodeValid, Bit6: CodeFlipped, Bit5: x, Bit4: DeviceStatus, Bit3: x, Bit2: #LCPBit2, Bit1: #LCPBit1, Bit0: #LCPBit0
* @param[in] barcode Barcode (> 255: extended barcode), , object 0x2021sub8 and 0x2021sub9 in object dictionary
* @param[in] dev_status Device status, object 0x2018 in object dictionary
* @param[in] error error register, object 0x1001 in object dictionary
* @param[in] barcodecenter barcode_center_point, OLS20 only (0x2021subA), OLS10: always 0
* @param[in] linequality quality_of_lines, OLS20 only (0x2021subB), OLS10: always 0
* @param[in] lineintensity1 intensity_of_lines[0], OLS20 only (0x2023sub1), OLS10: always 0
* @param[in] lineintensity2 intensity_of_lines[1], OLS20 only (0x2023sub2), OLS10: always 0
* @param[in] lineintensity3 intensity_of_lines[2], OLS20 only (0x2023sub3), OLS10: always 0
* @param[in] msg_frame_id frame id of OLS_Measurement message
*
* @return parameter converted to OLS_Measurement
*/
sick_line_guidance::OLS_Measurement sick_line_guidance::MsgUtil::convertOLSMessage(float lcp1, float lcp2, float lcp3, float width1, float width2, float width3, uint8_t status,
uint32_t barcode, uint8_t dev_status, uint8_t error, float barcodecenter, uint8_t linequality, uint8_t lineintensity1, uint8_t lineintensity2, uint8_t lineintensity3, const std::string & msg_frame_id)
{
sick_line_guidance::OLS_Measurement ols_message;
ols_message.header.stamp = ros::Time::now();
ols_message.header.frame_id = msg_frame_id;
ols_message.position = { lcp1, lcp2, lcp3 };
ols_message.width = { width1, width2, width3 };
ols_message.status = status;
if(barcode > 255)
{
ols_message.barcode = 255;
ols_message.extended_code = barcode;
}
else
{
ols_message.barcode = barcode;
ols_message.extended_code = 0;
}
ols_message.dev_status = dev_status;
ols_message.error = error;
ols_message.barcode_center_point = barcodecenter;
ols_message.quality_of_lines = linequality;
ols_message.intensity_of_lines = { lineintensity1, lineintensity2, lineintensity3 };
return ols_message;
}
/*
* @brief returns a gaussian destributed random line center position (lcp)
*
* @param stddev standard deviation of gaussian random generator
*
* @return random line center position
*/
float sick_line_guidance::MsgUtil::randomLCP(double stddev)
{
static random_numbers::RandomNumberGenerator mea_random_generator;
return (float)mea_random_generator.gaussian(0.0, stddev);
}
/*
* @brief returns a gaussian destributed random line width
*
* @param stddev standard deviation of gaussian random generator
*
* @return random line width
*/
float sick_line_guidance::MsgUtil::randomLW(double min_linewidth, double max_linewidth)
{
static random_numbers::RandomNumberGenerator mea_random_generator;
return (float)mea_random_generator.uniformReal(min_linewidth, max_linewidth);
}

224
Devices/Packages/sick_line_guidance/src/sick_line_guidance_node.cpp Executable file
View File

@@ -0,0 +1,224 @@
/*
* @brief sick_line_guidance_node implements ros driver for OLS20 and MLS devices.
*
* It implements the can master using CanopenChain in package ros_canopen,
* parses and converts the can messages from OLS20 and MLS devices (SDOs and PDOs)
* and publishes sensor messages (type OLS_Measurement or MLS_Measurement)
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#include <ros/ros.h>
#include <std_msgs/Int16.h>
#include <sensor_msgs/PointCloud.h>
#include <canopen_chain_node/ros_chain.h>
#include <vector>
#include "sick_line_guidance/sick_line_guidance_version.h"
#include "sick_line_guidance/sick_line_guidance_canopen_chain.h"
#include "sick_line_guidance/sick_line_guidance_can_mls_subscriber.h"
#include "sick_line_guidance/sick_line_guidance_can_ols_subscriber.h"
#include "sick_line_guidance/sick_line_guidance_can_cia401_subscriber.h"
#include "sick_line_guidance/sick_line_guidance_diagnostic.h"
#include "sick_line_guidance/sick_line_guidance_eds_util.h"
#include "sick_line_guidance/sick_line_guidance_msg_util.h"
typedef enum SICK_LINE_GUIDANCE_EXIT_CODES_ENUM
{
EXIT_OK = 0,
EXIT_ERROR = 1,
EXIT_CONFIG_ERROR
} SICK_LINE_GUIDANCE_EXIT_CODES;
// shortcut to read a member from a XmlRpcValue, or to return a defaultvalue, it the member does not exist
template<class T> static T readMember(XmlRpc::XmlRpcValue & value, const std::string & member, const T & defaultvalue)
{
if(value.hasMember(member))
return value[member];
return defaultvalue;
}
/*
* @brief sick_line_guidance_node implements ros driver for OLS20 and MLS devices.
* It implements the can master using CanopenChain in package ros_canopen,
* parses and converts the can messages from OLS20 and MLS devices (SDOs and PDOs)
* and publishes sensor messages (type OLS_Measurement or MLS_Measurement)
*/
int main(int argc, char** argv)
{
ros::init(argc, argv, "sick_line_guidance_node");
// Configuration
ros::NodeHandle nh;
bool can_connect_init_at_startup = true; // Connect and initialize canopen service
int initial_sensor_state = 0x07; // initial sensor state (f.e. 0x07 for 3 detected lines, or 8 to indicate sensor error)
int subscribe_queue_size = 16; // buffer size for ros messages
int max_num_retries_after_sdo_error = 2; // After SDO error, the query is repeated max. N times (default: N=2). If the SDO error persists, the can driver is shutdown and restarted.
double max_sdo_rate = 1000; // max_sdo_rate max. sdo query and publish rate
std::string diagnostic_topic = "diagnostics";
nh.param("/sick_line_guidance_node/can_connect_init_at_startup", can_connect_init_at_startup, can_connect_init_at_startup); // Connect and initialize canopen service at startup
nh.param("/sick_line_guidance_node/initial_sensor_state", initial_sensor_state, initial_sensor_state); // initial sensor state
nh.param("/sick_line_guidance_node/subscribe_queue_size", subscribe_queue_size, subscribe_queue_size);
nh.param("/sick_line_guidance_node/diagnostic_topic", diagnostic_topic, diagnostic_topic);
nh.param("/sick_line_guidance_node/max_num_retries_after_sdo_error", max_num_retries_after_sdo_error, max_num_retries_after_sdo_error);
nh.param("/sick_line_guidance_node/max_sdo_rate", max_sdo_rate, max_sdo_rate);
ROS_INFO_STREAM("sick_line_guidance_node: version " << sick_line_guidance::Version::getVersionInfo());
sick_line_guidance::Diagnostic::init(nh, diagnostic_topic, "sick_line_guidance_node");
if(can_connect_init_at_startup)
{
ROS_INFO_STREAM("sick_line_guidance_node: connect and init canopen ros-service...");
do
{
ros::Duration(1.0).sleep();
} while (!sick_line_guidance::CanopenChain::connectInitService(nh));
ROS_INFO_STREAM("sick_line_guidance_node: canopen ros-service connected and initialized.");
}
else
{
ROS_INFO_STREAM("sick_line_guidance_node: starting...");
}
// Read configuration of can nodes from yaml-file (there can be more than just "node1")
std::vector<sick_line_guidance::CanSubscriber*> vecCanSubscriber;
XmlRpc::XmlRpcValue nodes;
if(!nh.getParam("nodes", nodes) || nodes.size() < 1)
{
ROS_ERROR("sick_line_guidance_node: no can nodes found in yaml-file, please check configuration. Aborting...");
sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, "no can nodes found in yaml-file");
return EXIT_CONFIG_ERROR;
}
for(XmlRpc::XmlRpcValue::iterator can_node_iter = nodes.begin(); can_node_iter != nodes.end(); ++can_node_iter)
{
std::string can_node_id = readMember<std::string>(can_node_iter->second, "name", can_node_iter->first);
ROS_INFO("sick_line_guidance_node: initializing can_node_id \"%s\"...", can_node_id.c_str());
// Test: Query some common objects from can device
std::string can_msg = "", can_object_value = "";
std::vector <std::string> vec_object_idx = {"1001", "1018sub1", "1018sub4"};
for (std::vector<std::string>::iterator object_iter = vec_object_idx.begin(); object_iter != vec_object_idx.end(); object_iter++)
{
if (!sick_line_guidance::CanopenChain::queryCanObject(nh, can_node_id, *object_iter, max_num_retries_after_sdo_error, can_msg, can_object_value))
ROS_ERROR("sick_line_guidance_node: CanopenChain::queryCanObject(%s, %s) failed: %s", can_node_id.c_str(), object_iter->c_str(), can_msg.c_str());
else
ROS_INFO("sick_line_guidance_node: can %s[%s]=%s", can_node_id.c_str(), object_iter->c_str(), can_object_value.c_str());
}
// Test: read eds-file
std::string eds_file = sick_line_guidance::EdsUtil::filepathFromPackage(
readMember<std::string>(can_node_iter->second, "eds_pkg", ""),
readMember<std::string>(can_node_iter->second, "eds_file", ""));
ROS_INFO("sick_line_guidance_node: eds_file \"%s\"", eds_file.c_str());
std::string eds_file_content = sick_line_guidance::EdsUtil::readParsePrintEdsfile(eds_file);
if(eds_file_content == "")
{
ROS_ERROR("sick_line_guidance_node: read/parse eds file \"%s\" failed", eds_file.c_str());
sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, "read/parse error in eds file");
}
else
ROS_DEBUG("%s", eds_file_content.c_str());
// Parse dcf_overlays, read and overwrite parameter specified in dcf_overlays
if (can_node_iter->second.hasMember("dcf_overlay") &&
!sick_line_guidance::CanopenChain::writeDCFoverlays(nh, can_node_id, can_node_iter->second["dcf_overlay"]))
{
ROS_ERROR("sick_line_guidance: failed to set one or more dcf overlays.");
}
// Subscribe to canopen_chain_node topics, handle PDO messages and publish OLS/MLS measurement messages
std::string sick_device_family = readMember<std::string>(can_node_iter->second, "sick_device_family", "");
std::string sick_topic = readMember<std::string>(can_node_iter->second, "sick_topic", "");
std::string sick_frame_id = readMember<std::string>(can_node_iter->second, "sick_frame_id", "");
boost::to_upper(sick_device_family);
if(sick_device_family.empty() || sick_topic.empty() || sick_frame_id.empty())
{
sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, "invalid node configuration in yaml file");
ROS_ERROR("sick_line_guidance_node: invalid node configuration: sick_device_family=%s, sick_topic=%s, sick_frame_id=%s", sick_device_family.c_str(), sick_topic.c_str(), sick_frame_id.c_str());
continue;
}
sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::OK, "");
sick_line_guidance::CanSubscriber * p_can_subscriber = NULL;
if (sick_device_family == "MLS")
p_can_subscriber = new sick_line_guidance::CanMlsSubscriber(nh, can_node_id, max_num_retries_after_sdo_error, max_sdo_rate, sick_topic, sick_frame_id, initial_sensor_state, subscribe_queue_size);
else if (sick_device_family == "OLS10")
p_can_subscriber = new sick_line_guidance::CanOls10Subscriber(nh, can_node_id, max_num_retries_after_sdo_error, max_sdo_rate, sick_topic, sick_frame_id, initial_sensor_state, subscribe_queue_size);
else if (sick_device_family == "OLS20")
p_can_subscriber = new sick_line_guidance::CanOls20Subscriber(nh, can_node_id, max_num_retries_after_sdo_error, max_sdo_rate, sick_topic, sick_frame_id, initial_sensor_state, subscribe_queue_size);
else if (sick_device_family == "CIA401")
p_can_subscriber = new sick_line_guidance::CanCiA401Subscriber(nh, can_node_id, max_num_retries_after_sdo_error, max_sdo_rate, sick_topic, sick_frame_id, initial_sensor_state, subscribe_queue_size);
if(p_can_subscriber && p_can_subscriber->subscribeCanTopics())
{
vecCanSubscriber.push_back(p_can_subscriber);
}
else
{
ROS_ERROR("sick_line_guidance_node: sick_device_family \"%s\" not supported.", sick_device_family.c_str());
}
}
if(vecCanSubscriber.size() < 1)
{
sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::CONFIGURATION_ERROR, " no can nodes for sick_line_guidance found in yaml-file");
ROS_ERROR("sick_line_guidance_node: no can nodes for sick_line_guidance found in yaml-file, please check configuration. Aborting...");
return EXIT_CONFIG_ERROR;
}
// Run ros event loop
ros::spin();
// Deallocate resources
std::cout << "sick_line_guidance_node: exiting..." << std::endl;
ROS_INFO("sick_line_guidance_node: exiting...");
sick_line_guidance::Diagnostic::update(sick_line_guidance::DIAGNOSTIC_STATUS::EXIT);
for(std::vector<sick_line_guidance::CanSubscriber*>::iterator iter = vecCanSubscriber.begin(); iter != vecCanSubscriber.end(); iter++)
delete(*iter);
return EXIT_OK;
}

206
Devices/Packages/sick_line_guidance/src/sick_line_guidance_ros2can_node.cpp Executable file
View File

@@ -0,0 +1,206 @@
/*
* sick_line_guidance_ros2can_node: subscribes to ros topic (message type can_msgs::Frame),
* converts all messages to can frames (type can::Frame) and writes them to can bus.
* A simple cansend with input by ros messages.
*
* Copyright (C) 2019 Ing.-Buero Dr. Michael Lehning, Hildesheim
* Copyright (C) 2019 SICK AG, Waldkirch
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* All rights reserved.
*
* 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 SICK AG nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission
* * Neither the name of Ing.-Buero Dr. Michael Lehning 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.
*
* Authors:
* Michael Lehning <michael.lehning@lehning.de>
*
* Copyright 2019 SICK AG
* Copyright 2019 Ing.-Buero Dr. Michael Lehning
*
*/
#include <ros/ros.h>
#include <can_msgs/Frame.h>
#include <socketcan_interface/socketcan.h>
#include <socketcan_interface/filter.h>
#include <canopen_chain_node/ros_chain.h>
#include "sick_line_guidance/sick_line_guidance_version.h"
namespace sick_line_guidance
{
/*
* class SocketCANSender implements a callback for ros messages, converts them to can frames and writes the data to can bus.
*/
class SocketCANSender
{
public:
/*
* Constructor
*/
SocketCANSender(can::DriverInterfaceSharedPtr p_socketcan_interface) : m_socketcan_interface(p_socketcan_interface), m_socketcan_thread(0), m_socketcan_running(false)
{
}
/*
* @brief create a thread and run can::SocketCANInterface::run() in a background task
*/
void start()
{
m_socketcan_running = true;
m_socketcan_thread = new boost::thread(&sick_line_guidance::SocketCANSender::run, this);
}
/*
* @brief shuts down can::SocketCANInterface and stops the thread running can::SocketCANInterface::run()
*/
void stop()
{
m_socketcan_running = true;
if(m_socketcan_thread)
{
m_socketcan_interface->shutdown();
m_socketcan_thread->join();
delete(m_socketcan_thread);
}
m_socketcan_thread = 0;
}
/*
* @brief Callbacks for ros messages. Converts a ros message to datatype can::Frame and writes it to can bus.
* param[in] msg ros message of type can_msgs::Frame
*/
void messageCallback(const can_msgs::Frame & msg)
{
can::Frame canframe;
canframe.id = msg.id;
canframe.dlc = msg.dlc;
canframe.is_error = msg.is_error;
canframe.is_rtr = msg.is_rtr;
canframe.is_extended = msg.is_extended;
size_t n = 0, n_max = std::min(msg.data.size(),canframe.data.size());
for(n = 0; n < n_max; n++)
canframe.data[n] = msg.data[n];
for(; n < canframe.data.size(); n++)
canframe.data[n] = 0;
if(!canframe.isValid())
{
ROS_ERROR_STREAM("sick_line_guidance::SocketCANSender::messageCallback(): received invalid can_msgs::Frame message: " << msg);
ROS_ERROR_STREAM("sick_line_guidance::SocketCANSender::messageCallback(): sending invalid can frame " << can::tostring(canframe, false) << " ...");
}
if(m_socketcan_interface->send(canframe))
{
ROS_DEBUG_STREAM("sick_line_guidance::SocketCANSender::messageCallback(): sent can message " << can::tostring(canframe, false));
}
else
{
ROS_ERROR_STREAM("sick_line_guidance::SocketCANSender::messageCallback(): send can message " << can::tostring(canframe, false) << " failed.");
}
}
protected:
can::DriverInterfaceSharedPtr m_socketcan_interface; // can::SocketCANInterface instance
boost::thread* m_socketcan_thread; // thread to run can::SocketCANInterface in background
bool m_socketcan_running; // flag indicating start and stop of m_socketcan_thread
/*
* @brief runs can::SocketCANInterface::run() in an endless loop
*/
void run()
{
while(m_socketcan_running && ros::ok())
{
m_socketcan_interface->run();
}
}
}; // SocketCANSender
} // sick_line_guidance
/*
* sick_line_guidance_ros2can_node: subscribes to ros topic (message type can_msgs::Frame),
* converts all messages to can frames (type can::Frame) and writes them to can bus.
* A simple cansend with input by ros messages.
*/
int main(int argc, char** argv)
{
// Setup and configuration
ros::init(argc, argv, "sick_line_guidance_ros2can_node");
ros::NodeHandle nh;
std::string can_device = "can0", ros_topic = "ros2can0";
int subscribe_queue_size = 32;
nh.param("can_device", can_device, can_device); // name of can net device (socketcan interface)
nh.param("ros_topic", ros_topic, ros_topic); // topic for ros messages, message type can_msgs::Frame
nh.param("subscribe_queue_size", subscribe_queue_size, subscribe_queue_size); // buffer size for ros messages
ROS_INFO_STREAM("sick_line_guidance_ros2can_node: version " << sick_line_guidance::Version::getVersionInfo());
ROS_INFO_STREAM("sick_line_guidance_ros2can_node: starting...");
// Create the SocketCANInterface
can::DriverInterfaceSharedPtr p_socketcan_interface = 0;
canopen::GuardedClassLoader<can::DriverInterface> driver_loader("socketcan_interface", "can::DriverInterface");
try
{
ROS_INFO_STREAM("sick_line_guidance_ros2can_node: initializing SocketCANInterface...");
p_socketcan_interface = driver_loader.createInstance("can::SocketCANInterface");
if(!p_socketcan_interface->init(can_device, false, can::NoSettings::create()))
ROS_ERROR_STREAM("sick_line_guidance_ros2can_node: SocketCANInterface::init() failed.");
}
catch(pluginlib::PluginlibException& ex)
{
ROS_ERROR_STREAM("sick_line_guidance_ros2can_node: createInstance(\"can::SocketCANInterface\") failed: " << ex.what() << ", exit with error");
return 1;
}
ROS_INFO_STREAM("sick_line_guidance_ros2can_node: SocketCANInterface created, state = " << p_socketcan_interface->getState().driver_state);
// Subscribe to ros topic
sick_line_guidance::SocketCANSender socketcan_sender(p_socketcan_interface);
ros::Subscriber ros_subscriber = nh.subscribe(ros_topic, subscribe_queue_size, &sick_line_guidance::SocketCANSender::messageCallback, &socketcan_sender);
socketcan_sender.start();
ROS_INFO_STREAM("sick_line_guidance_ros2can_node: SocketCANSender started, listening to ros topic \"" << ros_topic << "\" ...");
// Run ros event loop
ros::spin();
std::cout << "sick_line_guidance_ros2can_node: exiting..." << std::endl;
ROS_INFO_STREAM("sick_line_guidance_ros2can_node: exiting...");
socketcan_sender.stop();
p_socketcan_interface = 0;
return 0;
}

133
Devices/Packages/sick_line_guidance/test/cfg/sick_canopen_simu_cfg.xml Executable file
View File

@@ -0,0 +1,133 @@
<?xml version="1.0" ?>
<sick_canopen_simu>
<SDO_config>
<!-- Lookup table for SDO response from SDO get request (client->server: upload request -> upload response) -->
<sdo request="0x4001100000000000" response="0x4F01100000000000" /> <!-- 1001: error register (all can devices) -->
<sdo request="0x4001200500000000" response="0x4F01200500000000" /> <!-- 2001sub5: OLS sensor flipped -->
<sdo request="0x4002200100000000" response="0x4302200100000000" /> <!-- 2002sub1: OLS Typ. Width -->
<sdo request="0x4002200200000000" response="0x4302200200000000" /> <!-- 2002sub2: OLS Min. Width -->
<sdo request="0x4002200300000000" response="0x4302200300000000" /> <!-- 2002sub3: OLS Max. Width -->
<sdo request="0x4018100100000000" response="0x4318100156000001" /> <!-- 1018sub1: Vendor Id (OLS and MLS) -->
<sdo request="0x4018100400000000" response="0x4318100411AE2201" /> <!-- 1018sub4: Serial number (OLS and MLS) -->
<!-- sdo request="0x4018200000000000" response="0x4B18200000000000"/ --> <!-- 2018: OLS10 dev_status (UINT16) -->
<sdo request="0x4018200000000000" response="0x4F18200000000000" /> <!-- 2018: OLS20 dev_status (UINT8) -->
<sdo request="0x4021200100000000" response="0x4B21200100000000" /> <!-- 2021sub1: LCP1 (OLS and MLS) -->
<sdo request="0x4021200200000000" response="0x4B21200200000000" /> <!-- 2021sub2: LCP2 (OLS and MLS) -->
<sdo request="0x4021200300000000" response="0x4B21200300000000" /> <!-- 2021sub3: LCP3 (OLS and MLS) -->
<sdo request="0x4021200400000000" response="0x4F21200400000000" /> <!-- 2021sub4: OLS: State, MLS: #LCP and marker -->
<sdo request="0x4021200500000000" response="0x4B21200500000000" /> <!-- 2021sub5: OLS Width LCP1 -->
<sdo request="0x4021200600000000" response="0x4B21200600000000" /> <!-- 2021sub6: OLS Width LCP2 -->
<sdo request="0x4021200700000000" response="0x4B21200700000000" /> <!-- 2021sub7: OLS Width LCP3 -->
<sdo request="0x4021200800000000" response="0x4F21200800000000" /> <!-- 2021sub8: OLS Barcode (UINT8) -->
<sdo request="0x4021200900000000" response="0x4321200900000000" /> <!-- 2021sub9: OLS Extended Barcode (UINT32) -->
<sdo request="0x4022200000000000" response="0x4F22200000000000" /> <!-- 2022: MLS status -->
<sdo request="0x4025200000000000" response="0x4F25200000000000" /> <!-- 2025: MLS Min.Level -->
<sdo request="0x4026200000000000" response="0x4F26200000000000" /> <!-- 2026: MLS Offset -->
<sdo request="0x4027200000000000" response="0x4F27200000000000" /> <!-- 2027: MLS sensorFlipped -->
<sdo request="0x4028200100000000" response="0x4F28200100000000" /> <!-- 2028sub1: MLS UseMarkers -->
<sdo request="0x4028200200000000" response="0x4F28200200000000" /> <!-- 2028sub2: MLS MarkerStyle -->
<sdo request="0x4028200300000000" response="0x4F28200300000000" /> <!-- 2028sub3: MLS FailSafeMode -->
<sdo request="0x4029200000000000" response="0x4F29200000000000" /> <!-- 2029: MLS LockTeach -->
<!--Lookup table for OLS20 only SDOs: objects 2021subA (barcode center point, INT16), 2021subB (quality of lines, UINT8) and 2023sub1 to 2023sub3 (intensity line 1 - 3, UINT8) -->
<sdo request="0x4021200A00000000" response="0x4B21200A00000000" /> <!-- 2021subA: OLS20 Barcode center point (INT16) -->
<sdo request="0x4021200B00000000" response="0x4F21200B00000000" /> <!-- 2021subB: OLS20 Quality of lines (UINT8) -->
<sdo request="0x4023200100000000" response="0x4F23200100000000" /> <!-- 2023sub1: OLS20 Intensity of line 1 (UINT8) -->
<sdo request="0x4023200200000000" response="0x4F23200200000000" /> <!-- 2023sub1: OLS20 Intensity of line 2 (UINT8) -->
<sdo request="0x4023200300000000" response="0x4F23200300000000" /> <!-- 2023sub1: OLS20 Intensity of line 3 (UINT8) -->
<!-- Lookup table for SDO response from SDO set request (dcf overlays, client->server: download request -> download response), -->
<!-- f.e. SDO request = 0x23022003CDCC4C3D = 0x2302200300000000 + {4 byte data}: SDO response = 0x6002200300000000 -->
<sdo request="0x2302200100000000" response="0x6002200100000000" /> <!-- 2002sub1: OLS Typ. Width -->
<sdo request="0x2302200200000000" response="0x6002200200000000" /> <!-- 2002sub2: OLS Min. Width -->
<sdo request="0x2302200300000000" response="0x6002200300000000" /> <!-- 2002sub3: OLS Max. Width -->
<sdo request="0x2B17100000000000" response="0x6017100000000000" /> <!-- 1017: OLS Producer Heartbeat Time -->
<sdo request="0x2B25200000000000" response="0x6025200000000000" /> <!-- 2025: MLS Min.Level -->
<sdo request="0x2B26200000000000" response="0x6026200000000000" /> <!-- 2026: MLS Offset -->
<sdo request="0x2F01200500000000" response="0x6001200501000000" /> <!-- 2001sub5: OLS sensor flipped -->
<sdo request="0x2F27200000000000" response="0x6027200000000000" /> <!-- 2027: MLS sensorFlipped -->
<sdo request="0x2F28200100000000" response="0x6028200100000000" /> <!-- 2028sub1: MLS UseMarkers -->
<sdo request="0x2F28200200000000" response="0x6028200200000000" /> <!-- 2028sub2: MLS MarkerStyle -->
<sdo request="0x2F28200300000000" response="0x6028200300000000" /> <!-- 2028sub3: MLS FailSafeMode -->
<sdo request="0x2F29200000000000" response="0x6029200000000000" /> <!-- 2029: MLS LockTeach -->
</SDO_config>
<OLS10>
<PDO_config>
<!-- List of PDOs to simulate OLS device -->
<!-- Status byte OLS (0x2021sub4 in object dictionary, Bit7=MSBit to Bit0=LSBit): -->
<!-- Bit7: CodeValid, Bit6: CodeFlipped, Bit5: x, Bit4: DeviceStatus, Bit3: x, Bit2: #LCPBit2, Bit1: #LCPBit1, Bit0: #LCPBit0 -->
<!-- Device status byte OLS (0x2018 in object dictionary): -->
<!-- Bit0=0: Device okay, Bit0=1: Device error -->
<!-- OLS10: barcodecenter, linequality and lineintensity1-3 always 0 -->
<pdo lcp1="0.000" lcp2="0.000" lcp3="0.000" width1="0.000" width2="0.000" width3="0.000" status="0x00" barcode="0x00" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.000" lcp2="0.002" lcp3="0.000" width1="0.000" width2="0.012" width3="0.000" status="0x02" barcode="0x00" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.000" lcp2="0.002" lcp3="0.003" width1="0.000" width2="0.012" width3="0.013" status="0x03" barcode="0x00" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.001" lcp2="0.002" lcp3="0.000" width1="0.011" width2="0.012" width3="0.000" status="0x06" barcode="0x00" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.001" lcp2="0.002" lcp3="0.003" width1="0.011" width2="0.012" width3="0.013" status="0x07" barcode="0x00" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.001" lcp2="0.002" lcp3="0.003" width1="0.011" width2="0.012" width3="0.013" status="0x87" barcode="0x78" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="-0.001" lcp2="-0.002" lcp3="-0.003" width1="0.021" width2="0.022" width3="0.023" status="0x87" barcode="0xEF" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.010" lcp2="0.020" lcp3="0.030" width1="0.031" width2="0.032" width3="0.033" status="0x87" barcode="0x100" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.010" lcp2="-0.020" lcp3="0.030" width1="0.031" width2="0.032" width3="0.033" status="0x87" barcode="0x12345678" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.010" lcp2="-0.020" lcp3="-0.030" width1="0.031" width2="0.032" width3="0.033" status="0xC7" barcode="0x12345678" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.000" lcp2="0.000" lcp3="0.000" width1="0.000" width2="0.000" width3="0.000" status="0xD7" barcode="0xFFFFFFFF" devstatus="0x01" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.000" lcp2="0.000" lcp3="0.000" width1="0.000" width2="0.000" width3="0.000" status="0xD7" barcode="0xFFFFFFFF" devstatus="0xAB" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
</PDO_config>
</OLS10>
<OLS20>
<PDO_config>
<!-- List of PDOs to simulate OLS-20 device -->
<!-- Status byte OLS (0x2021sub4 in object dictionary, Bit7=MSBit to Bit0=LSBit): -->
<!-- Bit7: CodeValid, Bit6: CodeFlipped, Bit5: x, Bit4: DeviceStatus, Bit3: x, Bit2: #LCPBit2, Bit1: #LCPBit1, Bit0: #LCPBit0 -->
<!-- Device status byte OLS (0x2018 in object dictionary): -->
<!-- Bit0=0: Device okay, Bit0=1: Device error -->
<!-- OLS20: simulate barcodecenter, linequality and lineintensity1-3 (OLS10: always 0) -->
<pdo lcp1="0.000" lcp2="0.000" lcp3="0.000" width1="0.000" width2="0.000" width3="0.000" status="0x00" barcode="0x00" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.000" lcp2="0.002" lcp3="0.000" width1="0.000" width2="0.012" width3="0.000" status="0x02" barcode="0x00" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.000" lcp2="0.002" lcp3="0.003" width1="0.000" width2="0.012" width3="0.013" status="0x03" barcode="0x00" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.001" lcp2="0.002" lcp3="0.000" width1="0.011" width2="0.012" width3="0.000" status="0x06" barcode="0x00" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.001" lcp2="0.002" lcp3="0.003" width1="0.011" width2="0.012" width3="0.013" status="0x07" barcode="0x00" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.001" lcp2="0.002" lcp3="0.003" width1="0.011" width2="0.012" width3="0.013" status="0x87" barcode="0x78" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="-0.001" lcp2="-0.002" lcp3="-0.003" width1="0.021" width2="0.022" width3="0.023" status="0x87" barcode="0xEF" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.010" lcp2="0.020" lcp3="0.030" width1="0.031" width2="0.032" width3="0.033" status="0x87" barcode="0x100" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.010" lcp2="-0.020" lcp3="0.030" width1="0.031" width2="0.032" width3="0.033" status="0x87" barcode="0x12345678" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.010" lcp2="-0.020" lcp3="-0.030" width1="0.031" width2="0.032" width3="0.033" status="0xC7" barcode="0x12345678" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.000" lcp2="0.000" lcp3="0.000" width1="0.000" width2="0.000" width3="0.000" status="0xD7" barcode="0xFFFFFFFF" devstatus="0x01" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.000" lcp2="0.000" lcp3="0.000" width1="0.000" width2="0.000" width3="0.000" status="0xD7" barcode="0xFFFFFFFF" devstatus="0xAB" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.000" lcp2="0.000" lcp3="0.000" width1="0.000" width2="0.000" width3="0.000" status="0x00" barcode="0x00" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.000" lcp2="0.002" lcp3="0.000" width1="0.000" width2="0.012" width3="0.000" status="0x02" barcode="0x00" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="2" lineintensity1="0" lineintensity2="5" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.000" lcp2="0.002" lcp3="0.003" width1="0.000" width2="0.012" width3="0.013" status="0x03" barcode="0x00" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="6" lineintensity1="0" lineintensity2="5" lineintensity3="4" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.001" lcp2="0.002" lcp3="0.000" width1="0.011" width2="0.012" width3="0.000" status="0x06" barcode="0x00" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="3" lineintensity1="1" lineintensity2="5" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.001" lcp2="0.002" lcp3="0.003" width1="0.011" width2="0.012" width3="0.013" status="0x07" barcode="0x00" devstatus="0x00" error="0x00" barcodecenter="0.000" linequality="7" lineintensity1="1" lineintensity2="5" lineintensity3="4" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.001" lcp2="0.002" lcp3="0.003" width1="0.011" width2="0.012" width3="0.013" status="0x87" barcode="0x78" devstatus="0x00" error="0x00" barcodecenter="0.010" linequality="7" lineintensity1="1" lineintensity2="5" lineintensity3="4" frame_id="ols_simulation_frame"/>
<pdo lcp1="-0.001" lcp2="-0.002" lcp3="-0.003" width1="0.021" width2="0.022" width3="0.023" status="0x87" barcode="0xEF" devstatus="0x00" error="0x00" barcodecenter="0.010" linequality="7" lineintensity1="2" lineintensity2="9" lineintensity3="6" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.010" lcp2="0.020" lcp3="0.030" width1="0.031" width2="0.032" width3="0.033" status="0x87" barcode="0x100" devstatus="0x00" error="0x00" barcodecenter="0.010" linequality="7" lineintensity1="2" lineintensity2="9" lineintensity3="6" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.010" lcp2="-0.020" lcp3="0.030" width1="0.031" width2="0.032" width3="0.033" status="0x87" barcode="0x12345678" devstatus="0x00" error="0x00" barcodecenter="0.050" linequality="7" lineintensity1="3" lineintensity2="9" lineintensity3="8" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.010" lcp2="-0.020" lcp3="-0.030" width1="0.031" width2="0.032" width3="0.033" status="0xC7" barcode="0x12345678" devstatus="0x00" error="0x00" barcodecenter="0.050" linequality="7" lineintensity1="3" lineintensity2="9" lineintensity3="8" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.000" lcp2="0.000" lcp3="0.000" width1="0.000" width2="0.000" width3="0.000" status="0xD7" barcode="0xFFFFFFFF" devstatus="0x01" error="0x00" barcodecenter="0.123" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
<pdo lcp1="0.000" lcp2="0.000" lcp3="0.000" width1="0.000" width2="0.000" width3="0.000" status="0xD7" barcode="0xFFFFFFFF" devstatus="0xAB" error="0x00" barcodecenter="0.123" linequality="0" lineintensity1="0" lineintensity2="0" lineintensity3="0" frame_id="ols_simulation_frame"/>
</PDO_config>
</OLS20>
<MLS>
<PDO_config>
<!-- List of PDOs to simulate MLS device -->
<!-- status byte MLS (2022 in object dictionary, Bit7=MSBit to Bit0=LSBit): -->
<!-- Bit7: 0, Bit6: ReadingCode, Bit5: Polarity, Bit4: SensorFlipped, Bit3: LineLevBit2, Bit2: LineLevBit1, Bit1: LineLevBit0, Bit0: Linegood(1:LineGood,0:NotGood) -->
<!-- lcp byte MLS (0x2021sub4 in object dictionary, Bit7=MSBit to Bit0=LSBit): -->
<!-- Bit7: MarkerBit4, Bit6: MarkerBit3, Bit5: MarkerBit2, Bit4: MarkerBit1, Bit3: MarkerBit0, Bit2: #LCPBit2, Bit1: #LCPBit1, Bit0: #LCPBit0 -->
<pdo lcp1="0.000" lcp2="0.000" lcp3="0.000" status="0x00" lcp="0x00" error="0x00" frame_id="mls_simulation_frame"/>
<pdo lcp1="0.000" lcp2="0.002" lcp3="0.000" status="0x01" lcp="0x02" error="0x00" frame_id="mls_simulation_frame"/>
<pdo lcp1="0.000" lcp2="0.002" lcp3="0.003" status="0x01" lcp="0x03" error="0x00" frame_id="mls_simulation_frame"/>
<pdo lcp1="0.001" lcp2="0.002" lcp3="0.000" status="0x01" lcp="0x06" error="0x00" frame_id="mls_simulation_frame"/>
<pdo lcp1="0.001" lcp2="0.002" lcp3="0.003" status="0x01" lcp="0x07" error="0x00" frame_id="mls_simulation_frame"/>
<pdo lcp1="-0.010" lcp2="0.020" lcp3="0.030" status="0x01" lcp="0x07" error="0x00" frame_id="mls_simulation_frame"/>
<pdo lcp1="0.010" lcp2="-0.020" lcp3="0.030" status="0x01" lcp="0x07" error="0x00" frame_id="mls_simulation_frame"/>
<pdo lcp1="0.010" lcp2="0.020" lcp3="-0.030" status="0x01" lcp="0x07" error="0x00" frame_id="mls_simulation_frame"/>
<pdo lcp1="-0.010" lcp2="-0.020" lcp3="-0.030" status="0x01" lcp="0x07" error="0x00" frame_id="mls_simulation_frame"/>
<pdo lcp1="-0.005" lcp2="-0.006" lcp3="-0.007" status="0x41" lcp="0x57" error="0x00" frame_id="mls_simulation_frame"/>
<pdo lcp1="-0.005" lcp2="-0.006" lcp3="-0.007" status="0x41" lcp="0xAF" error="0x00" frame_id="mls_simulation_frame"/>
<pdo lcp1="-0.005" lcp2="-0.006" lcp3="-0.007" status="0x41" lcp="0xFF" error="0x00" frame_id="mls_simulation_frame"/>
<pdo lcp1="-0.005" lcp2="-0.006" lcp3="-0.007" status="0x41" lcp="0xFF" error="0x00" frame_id="mls_simulation_frame"/>
<pdo lcp1="-0.005" lcp2="-0.006" lcp3="-0.007" status="0x61" lcp="0xFF" error="0x00" frame_id="mls_simulation_frame"/>
<pdo lcp1="-0.005" lcp2="-0.006" lcp3="-0.007" status="0x7F" lcp="0xFF" error="0x00" frame_id="mls_simulation_frame"/>
</PDO_config>
</MLS>
</sick_canopen_simu>

BIN
Devices/Packages/sick_line_guidance/test/eds/CiA401_IO_Node3.bin Executable file
View File

Binary file not shown.

2282
Devices/Packages/sick_line_guidance/test/eds/CiA401_IO_Node3.eds Executable file
View File

File diff suppressed because it is too large Load Diff

143
Devices/Packages/sick_line_guidance/test/eds/CiA401_minimal.eds Executable file
View File

@@ -0,0 +1,143 @@
[FileInfo]
FileName=CiA401_minimal.eds
FileVersion=1
FileRevision=1
EDSVersion=4.0
Description=CANopenIA CiA401 Minimal Example
CreatedBy=rostest
CreationTime=11:00AM
CreationDate=13-02-2019
ModifiedBy=rostest
ModificationTime=11:00AM
ModificationDate=13-02-2019
[DeviceInfo]
VendorName=rostest
VendorNumber=0x01000056
ProductName=rostest
ProductNumber=0x00001004
RevisionNumber=1
OrderCode=-
BaudRate_10=1
BaudRate_20=1
BaudRate_50=1
BaudRate_125=1
BaudRate_250=1
BaudRate_500=1
BaudRate_800=0
BaudRate_1000=1
DynamicChannelsSupported=0
GroupMessaging=0
LSS_Supported=0
Granularity=0
SimpleBootUpSlave=1
SimpleBootUpMaster=0
NrOfRXPDO=0
NrOfTXPDO=2
CompactPDO=0x00
[Comments]
Lines=0
[DummyUsage]
Dummy0001=0
Dummy0002=0
Dummy0003=0
Dummy0004=0
Dummy0005=0
Dummy0006=0
Dummy0007=0
[MandatoryObjects]
SupportedObjects=3
1=0x1000
2=0x1001
3=0x1018
[OptionalObjects]
SupportedObjects=0
[ManufacturerObjects]
SupportedObjects=0
[1000]
ParameterName=Device Type
ObjectType=0x07
DataType=0x0007
LowLimit=
HighLimit=
AccessType=const
DefaultValue=0x000F0191
PDOMapping=0
ObjFlags=0x00
[1001]
ParameterName=Error Register
ObjectType=0x07
DataType=0x0005
LowLimit=
HighLimit=
AccessType=ro
DefaultValue=
PDOMapping=0
ObjFlags=0x00
[1018]
ParameterName=Identity Object
SubNumber=5
ObjectType=0x08
[1018sub0]
ParameterName=number of entries
ObjectType=0x07
DataType=0x0005
LowLimit=1
HighLimit=4
AccessType=const
DefaultValue=4
PDOMapping=0
ObjFlags=0x00
[1018sub1]
ParameterName=Vendor ID
ObjectType=0x07
DataType=0x0007
LowLimit=
HighLimit=
AccessType=const
DefaultValue=0x01455341
PDOMapping=0
ObjFlags=0x00
[1018sub2]
ParameterName=Product Code
ObjectType=0x07
DataType=0x0007
LowLimit=
HighLimit=
AccessType=const
DefaultValue=0xC01A0401
PDOMapping=0
ObjFlags=0x00
[1018sub3]
ParameterName=Revision number
ObjectType=0x07
DataType=0x0007
LowLimit=
HighLimit=
AccessType=ro
DefaultValue=0xFFFFFFFF
PDOMapping=0
ObjFlags=0x00
[1018sub4]
ParameterName=Serial number
ObjectType=0x07
DataType=0x0007
LowLimit=
HighLimit=
AccessType=ro
DefaultValue=0xFFFFFFFF
PDOMapping=0
ObjFlags=0x00

BIN
Devices/Packages/sick_line_guidance/test/eds/CiA402_Stepper_Node8.bin Executable file
View File

Binary file not shown.

1760
Devices/Packages/sick_line_guidance/test/eds/CiA402_Stepper_Node8.eds Executable file
View File

File diff suppressed because it is too large Load Diff

787
Devices/Packages/sick_line_guidance/test/eds/OLS20_experimental.eds Executable file
View File

@@ -0,0 +1,787 @@
; This EDS file was created by the CANopen Design Tool 2.3.19.0.
; port GmbH Halle/Saale Germany, http://www.port.de, mailto:service@port.de
; DefaultValues added by rostest.
[FileInfo]
FileName=OLS20.eds
FileVersion=1
FileRevision=1
EDSVersion=4.0
Description=Optical Line Guidance
CreationTime=09:08AM
CreationDate=08-15-2018
CreatedBy=herrmra
ModificationTime=10:51AM
ModificationDate=12-03-2018
ModifiedBy=rostest
[DeviceInfo]
VendorName=SICK AG
VendorNumber=0x01000056
ProductName=OLS20
ProductNumber=0x00001004
RevisionNumber=1
OrderCode=-
BaudRate_10=1
BaudRate_20=1
BaudRate_50=1
BaudRate_125=1
BaudRate_250=1
BaudRate_500=1
BaudRate_800=0
BaudRate_1000=1
DynamicChannelsSupported=0
GroupMessaging=0
LSS_Supported=1
Granularity=8
SimpleBootUpSlave=1
SimpleBootUpMaster=0
NrOfRXPDO=0
NrOfTXPDO=2
[Comments]
Lines=0
[DummyUsage]
Dummy0001=0
Dummy0002=0
Dummy0003=0
Dummy0004=0
Dummy0005=0
Dummy0006=0
Dummy0007=0
[MandatoryObjects]
SupportedObjects=3
1=0x1000
2=0x1001
3=0x1018
[OptionalObjects]
SupportedObjects=16
1=0x1005
2=0x1008
3=0x1009
4=0x100A
5=0x100C
6=0x100D
7=0x1014
8=0x1015
9=0x1016
10=0x1017
11=0x1200
12=0x1800
13=0x1801
14=0x1A00
15=0x1A01
16=0x1F80
[ManufacturerObjects]
SupportedObjects=6
1=0x2001
2=0x2002
3=0x2003
4=0x2018
5=0x2019
6=0x2021
[1000]
ParameterName=Device Type
ObjectType=0x07
DataType=0x0007
AccessType=const
DefaultValue=0x0000000
PDOMapping=0
[1001]
ParameterName=Error Register
ObjectType=0x07
DataType=0x0005
AccessType=ro
PDOMapping=0
[1005]
ParameterName=COB-ID SYNC
ObjectType=0x07
DataType=0x0007
AccessType=rw
PDOMapping=0
[1008]
ParameterName=Manufacturer Device Name
ObjectType=0x07
DataType=0x0009
AccessType=const
PDOMapping=0
[1009]
ParameterName=Manufacturer Hardware Version
ObjectType=0x07
DataType=0x0009
AccessType=const
PDOMapping=0
[100A]
ParameterName=Manufacturer Software Version
ObjectType=0x07
DataType=0x0009
AccessType=const
PDOMapping=0
[100C]
ParameterName=Guard Time
ObjectType=0x07
DataType=0x0006
AccessType=rw
PDOMapping=0
[100D]
ParameterName=Life Time Factor
ObjectType=0x07
DataType=0x0005
AccessType=rw
PDOMapping=0
[1014]
ParameterName=COB-ID EMCY
ObjectType=0x07
DataType=0x0007
AccessType=rw
PDOMapping=0
[1015]
ParameterName=Inhibit Time Emergency
ObjectType=0x07
DataType=0x0006
AccessType=rw
DefaultValue=0x0000
PDOMapping=0
[1016]
SubNumber=3
ParameterName=Heartbeat Consumer Entries
ObjectType=0x08
[1016sub0]
ParameterName=Highest sub-index supported
ObjectType=0x07
DataType=5
AccessType=const
DefaultValue=0x02
PDOMapping=0
[1016sub1]
ParameterName=Consumer Heartbeat Time 1
ObjectType=0x07
DataType=0x0007
AccessType=rw
PDOMapping=0
[1016sub2]
ParameterName=Consumer Heartbeat Time 2
ObjectType=0x07
DataType=0x0007
AccessType=rw
PDOMapping=0
[1017]
ParameterName=Producer Heartbeat Time
ObjectType=0x07
DataType=0x0006
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1018]
SubNumber=5
ParameterName=Identity Object
ObjectType=0x09
[1018sub0]
ParameterName=number of entries
ObjectType=0x07
DataType=0x0005
AccessType=const
DefaultValue=0x4
PDOMapping=0
[1018sub1]
ParameterName=Vendor Id
ObjectType=0x07
DataType=0x0007
AccessType=ro
DefaultValue=0x01000056
PDOMapping=0
[1018sub2]
ParameterName=Product Code
ObjectType=0x07
DataType=0x0007
AccessType=ro
PDOMapping=0
[1018sub3]
ParameterName=Revision number
ObjectType=0x07
DataType=0x0007
AccessType=ro
PDOMapping=0
[1018sub4]
ParameterName=Serial number
ObjectType=0x07
DataType=0x0007
AccessType=ro
PDOMapping=0
[1200]
SubNumber=3
ParameterName=Server SDO Parameter 1
ObjectType=0x09
[1200sub0]
ParameterName=Highest sub-index supported
ObjectType=0x07
DataType=0x0005
AccessType=const
DefaultValue=0x02
PDOMapping=0
[1200sub1]
ParameterName=COB-ID Client -> Server
ObjectType=0x07
DataType=0x0007
AccessType=ro
PDOMapping=0
[1200sub2]
ParameterName=COB-ID Server -> Client
ObjectType=0x07
DataType=0x0007
AccessType=ro
PDOMapping=0
[1800]
SubNumber=6
ParameterName=Transmit PDO Communication Parameter 1
ObjectType=0x09
[1800sub0]
ParameterName=Highest sub-index supported
ObjectType=0x07
DataType=0x0005
AccessType=const
DefaultValue=0x05
PDOMapping=0
LowLimit=0x2
HighLimit=0x6
[1800sub1]
ParameterName=COB-ID
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=$NODEID+0x180
PDOMapping=0
LowLimit=0x00000001
HighLimit=0xFFFFFFFF
[1800sub2]
ParameterName=Transmission Type
ObjectType=0x07
DataType=0x0005
AccessType=rw
DefaultValue=0xFF
PDOMapping=0
LowLimit=0x0
HighLimit=0xFF
[1800sub3]
ParameterName=Inhibit Time
ObjectType=0x07
DataType=0x0006
AccessType=rw
DefaultValue=0x0
PDOMapping=0
LowLimit=0x0000
HighLimit=0xFFFF
[1800sub4]
ParameterName=Compatibility Entry
ObjectType=0x07
DataType=0x0005
AccessType=rw
DefaultValue=0x00
PDOMapping=0
LowLimit=0x00
HighLimit=0xFF
[1800sub5]
ParameterName=Event Timer
ObjectType=0x07
DataType=0x0006
AccessType=rw
DefaultValue=0x14
PDOMapping=0
LowLimit=0x0
HighLimit=0xFFFF
[1801]
SubNumber=6
ParameterName=Transmit PDO Communication Parameter 2
ObjectType=0x09
[1801sub0]
ParameterName=Highest sub-index supported
ObjectType=0x07
DataType=0x0005
AccessType=const
DefaultValue=0x05
PDOMapping=0
LowLimit=0x02
HighLimit=0x06
[1801sub1]
ParameterName=COB-ID
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=$NODEID+0x280
PDOMapping=0
LowLimit=0x00000001
HighLimit=0xFFFFFFFF
[1801sub2]
ParameterName=Transmission Type
ObjectType=0x07
DataType=0x0005
AccessType=rw
DefaultValue=0xFF
PDOMapping=0
LowLimit=0x0
HighLimit=0xFF
[1801sub3]
ParameterName=Inhibit Time
ObjectType=0x07
DataType=0x0006
AccessType=rw
DefaultValue=0x0
PDOMapping=0
LowLimit=0x0000
HighLimit=0xFFFF
[1801sub4]
ParameterName=Compatibility Entry
ObjectType=0x07
DataType=0x0005
AccessType=rw
DefaultValue=0x00
PDOMapping=0
LowLimit=0x00
HighLimit=0xFF
[1801sub5]
ParameterName=Event Timer
ObjectType=0x07
DataType=0x0006
AccessType=rw
DefaultValue=0x14
PDOMapping=0
LowLimit=0x0
HighLimit=0xFFFF
[1A00]
SubNumber=9
ParameterName=Transmit PDO Mapping Parameter 1
ObjectType=0x09
[1A00sub0]
ParameterName=Number of mapped objects
ObjectType=0x07
DataType=0x0005
AccessType=rw
DefaultValue=0x00
PDOMapping=0
LowLimit=0x00
HighLimit=0x08
[1A00sub1]
ParameterName=Mapping Entry 1
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1A00sub2]
ParameterName=Mapping Entry 2
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1A00sub3]
ParameterName=Mapping Entry 3
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1A00sub4]
ParameterName=Mapping Entry 4
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1A00sub5]
ParameterName=Mapping Entry 5
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1A00sub6]
ParameterName=Mapping Entry 6
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1A00sub7]
ParameterName=Mapping Entry 7
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1A00sub8]
ParameterName=Mapping Entry 8
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1A01]
SubNumber=9
ParameterName=Transmit PDO Mapping Parameter 2
ObjectType=0x09
[1A01sub0]
ParameterName=Number of mapped objects
ObjectType=0x07
DataType=0x0005
AccessType=rw
DefaultValue=0x00
PDOMapping=0
LowLimit=0x0
HighLimit=0x8
[1A01sub1]
ParameterName=Mapping Entry 1
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1A01sub2]
ParameterName=Mapping Entry 2
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1A01sub3]
ParameterName=Mapping Entry 3
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1A01sub4]
ParameterName=Mapping Entry 4
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1A01sub5]
ParameterName=Mapping Entry 5
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1A01sub6]
ParameterName=Mapping Entry 6
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1A01sub7]
ParameterName=Mapping Entry 7
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1A01sub8]
ParameterName=Mapping Entry 8
ObjectType=0x07
DataType=0x0007
AccessType=rw
DefaultValue=0x00000000
PDOMapping=0
[1F80]
ParameterName=NMT Startup
ObjectType=0x07
DataType=0x0007
AccessType=const
PDOMapping=0
LowLimit=0x0
HighLimit=0x3F
[2001]
SubNumber=6
ParameterName=Mounting parameters
ObjectType=0x09
[2001sub0]
ParameterName=numOfEntries
ObjectType=0x07
DataType=0x0005
AccessType=const
DefaultValue=0x5
PDOMapping=0
[2001sub1]
ParameterName=reserved1
ObjectType=0x07
DataType=0x0007
AccessType=ro
PDOMapping=0
[2001sub2]
ParameterName=reserved2
ObjectType=0x07
DataType=0x0007
AccessType=ro
PDOMapping=0
[2001sub3]
ParameterName=reserved3
ObjectType=0x07
DataType=0x0007
AccessType=ro
PDOMapping=0
[2001sub4]
ParameterName=reserved4
ObjectType=0x07
DataType=0x0007
AccessType=ro
PDOMapping=0
[2001sub5]
ParameterName=sensorFlipped
ObjectType=0x07
DataType=0x0005
AccessType=rw
DefaultValue=0x00
PDOMapping=0
LowLimit=0x0
HighLimit=0xFF
[2002]
SubNumber=4
ParameterName=Tape Parameters
ObjectType=0x09
[2002sub0]
ParameterName=Number of mapped objects
ObjectType=0x07
DataType=0x0005
AccessType=const
DefaultValue=0x3
PDOMapping=0
[2002sub1]
ParameterName=Typ. Width
ObjectType=0x07
DataType=0x0008
AccessType=rw
PDOMapping=0
[2002sub2]
ParameterName=Min. Width
ObjectType=0x07
DataType=0x0008
AccessType=rw
PDOMapping=0
[2002sub3]
ParameterName=Max. Width
ObjectType=0x07
DataType=0x0008
AccessType=rw
PDOMapping=0
[2003]
SubNumber=3
ParameterName=Advanced Settings
ObjectType=0x09
[2003sub0]
ParameterName=Number of mapped objects
ObjectType=0x07
DataType=0x0005
AccessType=const
DefaultValue=0x2
PDOMapping=0
[2003sub1]
ParameterName=Off Delay Track Detection
ObjectType=0x07
DataType=0x0006
AccessType=rw
DefaultValue=0x0064
PDOMapping=0
LowLimit=0x0
HighLimit=0xFFFF
[2003sub2]
ParameterName=Off Delay Code Detection
ObjectType=0x07
DataType=0x0006
AccessType=rw
DefaultValue=0x0064
PDOMapping=0
LowLimit=0x0
HighLimit=0xFFFF
[2018]
ParameterName=Device Status
ObjectType=0x07
DataType=0x0005
AccessType=ro
PDOMapping=0
[2019]
ParameterName=Order number
ObjectType=0x07
DataType=0x0007
AccessType=ro
PDOMapping=0
[2021]
SubNumber=10
ParameterName=Result data (LCPs)
ObjectType=0x09
[2021sub0]
ParameterName=Number of mapped objects
ObjectType=0x07
DataType=0x0005
AccessType=const
DefaultValue=0x9
PDOMapping=0
[2021sub1]
ParameterName=LCP1
ObjectType=0x07
DataType=0x0003
AccessType=ro
PDOMapping=1
LowLimit=0x8000
HighLimit=0x7FFF
[2021sub2]
ParameterName=LCP2
ObjectType=0x07
DataType=0x0003
AccessType=ro
PDOMapping=1
LowLimit=0x8000
HighLimit=0x7FFF
[2021sub3]
ParameterName=LCP3
ObjectType=0x07
DataType=0x0003
AccessType=ro
PDOMapping=1
LowLimit=0x8000
HighLimit=0x7FFF
[2021sub4]
ParameterName=Status
ObjectType=0x07
DataType=0x0005
AccessType=ro
PDOMapping=1
LowLimit=0x00
HighLimit=0xFF
[2021sub5]
ParameterName=Width LCP1
ObjectType=0x07
DataType=0x0003
AccessType=ro
PDOMapping=1
LowLimit=0x8000
HighLimit=0x7FFF
[2021sub6]
ParameterName=Width LCP2
ObjectType=0x07
DataType=0x0003
AccessType=ro
PDOMapping=1
LowLimit=0x8000
HighLimit=0x7FFF
[2021sub7]
ParameterName=Width LCP3
ObjectType=0x07
DataType=0x0003
AccessType=ro
PDOMapping=1
LowLimit=0x8000
HighLimit=0x7FFF
[2021sub8]
ParameterName=Code
ObjectType=0x07
DataType=0x0005
AccessType=ro
PDOMapping=1
LowLimit=0x0
HighLimit=0xFF
[2021sub9]
ParameterName=Extended Code
ObjectType=0x07
DataType=0x0007
AccessType=ro
PDOMapping=0
LowLimit=0x0
HighLimit=0xFFFFFFFF

13
Devices/Packages/sick_line_guidance/test/scripts/cleanup.bash Executable file
View File

@@ -0,0 +1,13 @@
#!/bin/bash
pushd ../../../../
echo -e "\n# cleanup.bash: Stopping rosmaster and all rosnodes...\n# rosnode kill -a ; sleep 5 ; killall rosmaster ; sleep 5"
rosnode kill -a ; sleep 5 ; killall roslaunch ; sleep 5 ; killall rosmaster ; sleep 5
echo -e "\n# cleanup.bash: Deleting ros cache and logfiles and catkin folders ./build ./devel ./install"
rosclean purge -y
rm -rf ./build ./devel ./install
rm -rf ~/.ros/*
# rm -rf ~/.ros/log/*
catkin clean --yes --all-profiles --verbose
catkin_make clean
popd

16
Devices/Packages/sick_line_guidance/test/scripts/clion.bash Executable file
View File

@@ -0,0 +1,16 @@
#!/bin/bash
# init ros environment
source /opt/ros/melodic/setup.bash
source ../../../../devel/setup.bash
# start edit resource-files
gedit ./run.bash ./runsimu.bash ../../../sick_line_guidance/launch/sick_canopen_simu.launch ../../../sick_line_guidance/launch/sick_line_guidance.launch ../../../sick_line_guidance/ols/sick_line_guidance_ols10.yaml ../../../sick_line_guidance/ols/sick_line_guidance_ols20.yaml ../../../sick_line_guidance/mls/sick_line_guidance_mls.yaml &
# start clion
# pushd ../../../sick_line_guidance
# ~/Public/clion-2018.3.3/bin/clion.sh &
pushd ../../../..
~/Public/clion-2018.3.3/bin/clion.sh ./src &
popd

21
Devices/Packages/sick_line_guidance/test/scripts/echo_topics.bash Executable file
View File

@@ -0,0 +1,21 @@
#!/bin/bash
source ../../../../install/setup.bash
printf "\033c"
# display PointCloud2 messages
rosrun rviz rviz &
# plot sensor positions
rqt_plot /ols/position[0] /ols/position[1] /ols/position[2] &
# rqt_plot /cloud &
# print some values from the object directory of the can device
rostopic list
rostopic echo -n 1 /node1_1001 & rostopic echo -n 1 /node1_1018sub1 & rostopic echo -n 1 /node1_1018sub4
rosservice call /driver/get_object node1 1001sub false
rosservice call /driver/get_object node1 1018sub1 false
rosservice call /driver/get_object node1 1018sub4 false
# print ros topics for ols and mls
rostopic echo /mls & rostopic echo /ols & rostopic echo /cloud & rostopic echo /diagnostics

30
Devices/Packages/sick_line_guidance/test/scripts/make.bash Executable file
View File

@@ -0,0 +1,30 @@
#!/bin/bash
# delete old logfiles
pushd ../../../..
rm -rf install/lib/sick_line_guidance/*
rm -rf install/share/sick_line_guidance/*
rm -f build/catkin_make_install.log
# make install
catkin_make install 2>&1 | tee -a build/catkin_make_install.log
source ./install/setup.bash
# lint, install by running
# sudo apt-get install python-catkin-lint
# catkin_lint -W1 src/sick_line_guidance
# print warnings and errors
echo -e "\nmake.bash finished.\n"
echo -e "catkin_make warnings:"
cat build/catkin_make_install.log | grep -i "warning:"
echo -e "\ncatkin_make errors:"
cat build/catkin_make_install.log | grep -i "error:"
# print sick_line_guidance install files, libraries, executables
echo -e "\ninstall/lib/sick_line_guidance:"
ls -al install/lib/sick_line_guidance/*
echo -e "\ninstall/share/sick_line_guidance:"
ls install/share/sick_line_guidance/*.*
popd

9
Devices/Packages/sick_line_guidance/test/scripts/makeall.bash Executable file
View File

@@ -0,0 +1,9 @@
#!/bin/bash
echo -e "makeall.bash: build and install ros sick_line_guidance driver"
sudo echo -e "makeall.bash started."
source /opt/ros/noetic/setup.bash
./cleanup.bash
./makepcan.bash
./make.bash
echo -e "makeall.bash finished."

41
Devices/Packages/sick_line_guidance/test/scripts/makepcan.bash Executable file
View File

@@ -0,0 +1,41 @@
#!/bin/bash
# install packages required for peak can device driver
if [ -f ../../../../../peak_systems/pcanview-ncurses_0.8.7-0_amd64.deb ] ; then
pushd ../../../../../peak_systems
sudo apt-get install libncurses5
sudo dpkg --install pcanview-ncurses_0.8.7-0_amd64.deb
popd
fi
# build and install peak can device driver
if [ -d ../../../../../peak_systems/peak-linux-driver-8.17.0 ] ; then
pushd ../../../../../peak_systems/peak-linux-driver-8.17.0
# install required packages
sudo apt-get install can-utils
sudo apt-get install gcc-multilib
sudo apt-get install libelf-dev
sudo apt-get install libpopt-dev
sudo apt-get install tree
# build and install pcan driver
make clean
make NET=NETDEV_SUPPORT
sudo make install
# install the modules
sudo modprobe pcan
sudo modprobe can
sudo modprobe vcan
sudo modprobe slcan
# setup and configure "can0" net device
sudo ip link set can0 type can
sudo ip link set can0 up type can bitrate 125000 # configure the CAN bitrate, f.e. 125000 bit/s
# check installation
# ./driver/lspcan --all # should print "pcanusb32" and pcan version
# tree /dev/pcan-usb # should show a pcan-usb device
# ip -a link # should print some "can0: ..." messages
ip -details link show can0 # should print some details about "can0" net device
popd
fi

69
Devices/Packages/sick_line_guidance/test/scripts/run.bash Executable file
View File

@@ -0,0 +1,69 @@
#!/bin/bash
# set environment, clear screen and logfiles
if [ ! -d ~/.ros/log ] ; then mkdir -p ~/.ros/log ; fi
printf "\033c"
source ../../../../install/setup.bash
echo -e "\n# run.bash: Stopping rosmaster and all rosnodes...\n# rosnode kill -a ; sleep 5 ; killall rosmaster ; sleep 5"
rosnode kill -a ; sleep 5 ; killall rosmaster ; sleep 5
rm -rf ~/.ros/log/*
# initialize can net device, can0 by peak can adapter
# sudo ip link set can0 up type can bitrate 125000
NUM_BITRATE_125000=$(ip -details link show can0 | grep "bitrate 125000" | wc -l)
if [ "$NUM_BITRATE_125000" = "0" ] ; then
echo -e "# run sick_line_guidance:\n# sudo ip link set can0 up type can bitrate 125000"
sudo ip link set can0 up type can bitrate 125000
fi
echo -e "# run sick_line_guidance:\n# ip -details link show can0"
ip -details link show can0
# check can net device by sending 000#0x820A (can command reset communication to device 0x0A), should be answered by 70A#00 (boot message from device 0x0A)
# for ((n=0;n<=2;n++)) ; do
# candump -ta -n 2 can0 &
# cansend can0 000#820A
# sleep 1
# done
# Start OLS20 simulation
# ./runsimu.bash
# start can logging
# printf "\033c" ; candump -ta can0 2>&1 | tee ~/.ros/log/candump.log
candump -ta can0 2>&1 | tee ~/.ros/log/candump.log &
# get/set values from object directory with canopen_chain_node service, f.e. Object 2001sub5 (sensorFlipped, UINT8, defaultvalue: 0x01)
# rosservice call /driver/get_object "{node: 'node1', object: '2001sub5', cached: false}"
# rosservice call /driver/set_object "{node: 'node1', object: '2001sub5', value: '0x01', cached: false}"
# Start ros driver canopen_chain_node, sick_line_guidance_node and sick_line_guidance_cloud_publisher
# echo -e "\n# run sick_line_guidance:\n# roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_ols10.yaml\n"
# roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_ols10.yaml 2>&1 | tee ~/.ros/log/sick_line_guidance_ols10.log # start OLS10 driver
echo -e "\n# run sick_line_guidance:\n# roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_ols20.yaml\n"
roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_ols20.yaml 2>&1 | tee ~/.ros/log/sick_line_guidance_ols20.log # start OLS20 driver
# echo -e "\n# run sick_line_guidance:\n# roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_mls.yaml\n"
# roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=sick_line_guidance_mls.yaml 2>&1 | tee ~/.ros/log/sick_line_guidance_mls.log # start MLS driver
# read some object indices (f.e. 1001=ErrorRegister, 1018sub1=VendorID, 1018sub4=SerialNumber)
# rostopic echo -n 1 /node1_1001 & rostopic echo -n 1 /node1_1018sub1 & rostopic echo -n 1 /node1_1018sub4
# rosservice call /driver/get_object node1 1001sub false
# rosservice call /driver/get_object node1 1018sub1 false
# rosservice call /driver/get_object node1 1018sub4 false
# Check errors and warnings in ros logfiles
killall candump
grep "\[ WARN\]" ~/.ros/log/*.log ~/.ros/log/*/ros*.log ~/.ros/log/*/sick*.log >> ~/.ros/log/ros_log_warnings.txt
grep "\[ERROR\]" ~/.ros/log/*.log ~/.ros/log/*/ros*.log ~/.ros/log/*/sick*.log >> ~/.ros/log/ros_log_errors.txt
cat ~/.ros/log/ros_log_warnings.txt
cat ~/.ros/log/ros_log_errors.txt
# Zip all logfiles
mkdir ./tmp
cp -rf ~/.ros/log ./tmp
now=$(date +"%Y%m%d_%H%M%S")
tar -cvzf ./$now-ros-logfiles.tgz ./tmp/log
rm -rf ./tmp

112
Devices/Packages/sick_line_guidance/test/scripts/runsimu.bash Executable file
View File

@@ -0,0 +1,112 @@
#!/bin/bash
# set environment, clear screen and logfiles
if [ ! -d ~/.ros/log ] ; then mkdir -p ~/.ros/log ; fi
printf "\033c"
source ../../../../install/setup.bash
echo -e "\n# runsimu.bash: Stopping rosmaster and all rosnodes...\n# rosnode kill -a ; sleep 5 ; killall rosmaster ; sleep 5"
rosnode kill -a ; sleep 5 ; killall roslaunch ; sleep 5 ; killall rosmaster ; sleep 5
rm -rf ~/.ros/log/*
# Initialize can net device, can0 by peak can adapter
# sudo ip link set can0 type can loopback on # loopback not supported
# sudo ip link set can0 up type can bitrate 125000 # set default bitrate 125000 bit/s
# Reset can0 net device
# sudo ip link set can0 down
# sleep 1
# sudo ip link set can0 up type can bitrate 125000
# sleep 1
# Set default bitrate 125000 bit/s
NUM_BITRATE_125000=$(ip -details link show can0 | grep "bitrate 125000" | wc -l)
if [ "$NUM_BITRATE_125000" = "0" ] ; then
echo -e "# run ols/mls simulation:\n# sudo ip link set can0 up type can bitrate 125000"
sudo ip link set can0 up type can bitrate 125000
fi
echo -e "# run ols/mls simulation:\n# ip -details link show can0"
ip -details link show can0
sleep 1
# Start can logging
# candump -ta can0 &
# candump -ta can0 2>&1 | tee ~/.ros/log/candump.log &
# Simulation with cangineberry: Upload firmware (BEDS slave) and beds-file CiA401_IO_Node3
# ./coiaupdater -p /dev/ttyS0 -f ../APPS/CANopenIA-BEDS/CgB_COIA_BEDS1.3_sec.bin # install firmware CANopen IA BEDS slave
# ./coiaupdater -p /dev/ttyS0 -d ../APPS/CANopenIA-BEDS/EDS/CiA401_IO_Node3.bin
# Simulation with cangineberry: Read/write objects 6401sub1 (LCP1), 6401sub2 (LCP2), 6401sub3 (LCP3), 6401sub4 (WidthLCP1), 6401sub5 (WidthLCP2), 6401sub6 (WidthLCP3)
# ./coia -p /dev/ttyS0 --read=0x6401,0x01
# ./coia -p /dev/ttyS0 --write=0x6401,0x01,2,0x1234
# ./coia -p /dev/ttyS0 --write=0x6401,0x01,2,0xABCD
# get/set values from object directory with canopen_chain_node service, f.e. Object 2001sub5 (sensorFlipped, UINT8, defaultvalue: 0x01)
# rosservice call /driver/get_object "{node: 'node1', object: '2001sub5', cached: false}"
# rosservice call /driver/set_object "{node: 'node1', object: '2001sub5', value: '0x01', cached: false}"
# Run simulation for OLS and MLS by yaml-file configuration
device_settings_map=( "OLS20:sick_line_guidance_ols20.yaml;120" "OLS10:sick_line_guidance_ols10.yaml;30" "MLS:sick_line_guidance_mls.yaml;30" )
for((device_cnt=0;device_cnt<=2;device_cnt++)) ; do
device=${device_settings_map[$device_cnt]%%:*}
settings_str=${device_settings_map[$device_cnt]##*:}
IFS=';' settings_list=($settings_str)
yaml_file=${settings_list[0]}
run_seconds=${settings_list[1]}
echo -e "runsimu.bash: settings for $device device: yaml_file $yaml_file, run for $run_seconds seconds."
# Start can2ros converter
# echo -e "\n# run ols/mls simulation:\n# roslaunch -v --screen sick_line_guidance sick_line_guidance_can2ros_node.launch\n"
# roslaunch -v --screen sick_line_guidance sick_line_guidance_can2ros_node.launch &
echo -e "\n# run ols/mls simulation:\n# roslaunch -v sick_line_guidance sick_line_guidance_can2ros_node.launch\n"
roslaunch -v sick_line_guidance sick_line_guidance_can2ros_node.launch &
sleep 5
# Start ros2can converter
# echo -e "\n# run ols/mls simulation:\n# roslaunch -v --screen sick_line_guidance sick_line_guidance_ros2can_node.launch\n"
# roslaunch -v --screen sick_line_guidance sick_line_guidance_ros2can_node.launch &
echo -e "\n# run ols/mls simulation:\n# roslaunch -v sick_line_guidance sick_line_guidance_ros2can_node.launch\n"
roslaunch -v sick_line_guidance sick_line_guidance_ros2can_node.launch &
sleep 5
# Start OLS20 simulation
echo -e "\n# run ols/mls simulation:\n# roslaunch -v sick_line_guidance sick_canopen_simu.launch device:=$device\n"
roslaunch -v --screen sick_line_guidance sick_canopen_simu.launch device:=$device 2>&1 | tee ~/.ros/log/sick_canopen_simu_$device.log &
# echo -e "\n# run ols/mls simulation:\n# roslaunch -v sick_line_guidance sick_canopen_simu.launch device:=$device\n"
# roslaunch -v sick_line_guidance sick_canopen_simu.launch device:=$device &
sleep 5
# check can net device by sending 000#0x820A (can command reset communication to device 0x0A), should be answered by 70A#00 (boot message from device 0x0A)
for ((n=0;n<1;n++)) ; do
candump -ta -n 2 can0 &
cansend can0 000#820A
sleep 2
done
# Start ros driver for MLS or OLS, incl. canopen_chain_node, sick_line_guidance_node and sick_line_guidance_cloud_publisher
echo -e "\n# run sick_line_guidance:\n# roslaunch -v sick_line_guidance sick_line_guidance.launch yaml:=$yaml_file\n"
roslaunch -v --screen sick_line_guidance sick_line_guidance.launch yaml:=$yaml_file 2>&1 | tee ~/.ros/log/$yaml_file.log &
# echo -e "\n# run sick_line_guidance:\n# roslaunch -v sick_line_guidance sick_line_guidance.launch yaml:=$yaml_file\n"
# roslaunch -v sick_line_guidance sick_line_guidance.launch yaml:=$yaml_file &
# Run simulation for a while
sleep $run_seconds
# Exit simulation, shutdown all ros nodes
echo -e "\n# runsimu.bash: Stopping rosmaster and all rosnodes...\n# rosnode kill -a ; sleep 5 ; killall rosmaster ; sleep 5"
rosnode kill -a ; sleep 5 ; killall rosmaster ; sleep 5
done
# Check errors and warnings in ros logfiles
killall candump
echo -e "\n#\n# OLS/MLS simulation finished.\n#\n"
grep "\[ WARN\]" ~/.ros/log/*.log ~/.ros/log/*/ros*.log ~/.ros/log/*/sick*.log >> ~/.ros/log/ros_log_warnings.txt
grep "\[ERROR\]" ~/.ros/log/*.log ~/.ros/log/*/ros*.log ~/.ros/log/*/sick*.log >> ~/.ros/log/ros_log_errors.txt
grep "MeasurementVerificationStatistic" ~/.ros/log/*.log ~/.ros/log/*/ros*.log ~/.ros/log/*/sick*.log >> ~/.ros/log/simulation_statistic.txt
echo -e "\nSimulation warnings and errors:\n"
cat ~/.ros/log/ros_log_warnings.txt
cat ~/.ros/log/ros_log_errors.txt
echo -e "\nSimulation statistic:\n"
cat ~/.ros/log/simulation_statistic.txt

15
Devices/Packages/sick_line_guidance/test/scripts/vs_code.bash Executable file
View File

@@ -0,0 +1,15 @@
#!/bin/bash
if [ -f /opt/ros/noetic/setup.bash ] ; then
source /opt/ros/noetic/setup.bash
elif [ -f /opt/ros/foxy/setup.bash ] ; then
source /opt/ros/foxy/setup.bash
fi
pushd ../../../..
if [ -f ./install/setup.bash ] ; then
source ./install/setup.bash
fi
code ./sick_line_guidance_vscode.code-workspace
popd

30
Devices/Packages/sick_line_guidance/test/yaml/sick_line_guidance_example.yaml Executable file
View File

@@ -0,0 +1,30 @@
bus:
device: can0
driver_plugin: can::SocketCANInterface
master_allocator: canopen::SimpleMaster::Allocator
sync:
interval_ms: 10
overflow: 0
# heartbeat: # simple heartbeat producer (optional, not supported by OLS or MLS, do not enable heartbeats, do not configure heartbeat rate or message)
# rate: 100 # heartbeat rate (milliseconds)
# msg: "77f#05" # message to send, cansend format: heartbeat of node 127 with status 5=Started
nodes:
node1:
id: 0x03 # CAN-Node-ID of can device, default: Node-ID 10=0x0A for OLS10 and MLS (CiA401_IO_Node3: 0x03, CiA402_Stepper_Node8: 0x08)
eds_pkg: sick_line_guidance # package name for relative path
eds_file: CiA401_IO_Node3.eds # path to EDS/DCF file
publish: ["1001","1018sub1","1018sub4","6000sub1!","6000sub2!","6000sub3!","6000sub4!","6401sub1!","6401sub2!","6401sub3!","6401sub4!","6401sub5!","6401sub6!"]
# Common objects (CiA401_IO_Node3.eds, CiA402_Stepper_node8.eds, SICK_OLS20.eds, SICK-MLS-MLS.eds): 1001 = ErrorRegister, 1018sub1 = VendorID, 1018sub4 = SerialNumber
# objects SICK_OLS20.eds: 1001 = ErrorRegister, 1018sub1 = VendorID, 1018sub4 = SerialNumber, 0x2018 = DeviceStatus(0=OK), 0x2021sub1 bis 0x2021sub9 = measurementdata
# TPDOs SICK_OLS20.eds: 0x2021sub1 bis 0x2021sub8
# SICK-MLS.eds: 0x2021sub1 bis 0x2021sub4 und 0x2022 = measurementdata
# CiA401_IO_Node3.eds: TPDO mapped: 6000sub1 bis 6000sub8 (jeweils 1 byte), 6401sub1 bis 6401subC (jeweils 2 byte)
# Note: publish with "!" (f.e. publish: ["1001!"]) means reread from object directory
# (not cached, i.e. query by SDO, if no actual PDO is transmitted),
# otherwise cached (use last received SDO or PDO).
# sick_line_guidance configuration of this node:
sick_device_family: "CIA401" # can devices of OLS or MLS family currently supported (or CIA401 for testing)
sick_topic: "ols" # OLS_Measurement messages are published in topic "/ols"
sick_frame_id: "ols_measurement_frame" # OLS_Measurement messages are published frame_id "ols_measurement_frame"

67
Devices/Packages/sick_line_guidance/turtlebotDemo/CMakeLists.txt Executable file
View File

@@ -0,0 +1,67 @@
# toplevel CMakeLists.txt for a catkin workspace
# catkin/cmake/toplevel.cmake
cmake_minimum_required(VERSION 2.8.3)
set(CATKIN_TOPLEVEL TRUE)
# search for catkin within the workspace
set(_cmd "catkin_find_pkg" "catkin" "${CMAKE_SOURCE_DIR}")
execute_process(COMMAND ${_cmd}
RESULT_VARIABLE _res
OUTPUT_VARIABLE _out
ERROR_VARIABLE _err
OUTPUT_STRIP_TRAILING_WHITESPACE
ERROR_STRIP_TRAILING_WHITESPACE
)
if(NOT _res EQUAL 0 AND NOT _res EQUAL 2)
# searching fot catkin resulted in an error
string(REPLACE ";" " " _cmd_str "${_cmd}")
message(FATAL_ERROR "Search for 'catkin' in workspace failed (${_cmd_str}): ${_err}")
endif()
# include catkin from workspace or via find_package()
if(_res EQUAL 0)
set(catkin_EXTRAS_DIR "${CMAKE_SOURCE_DIR}/${_out}/cmake")
# include all.cmake without add_subdirectory to let it operate in same scope
include(${catkin_EXTRAS_DIR}/all.cmake NO_POLICY_SCOPE)
add_subdirectory("${_out}")
else()
# use either CMAKE_PREFIX_PATH explicitly passed to CMake as a command line argument
# or CMAKE_PREFIX_PATH from the environment
if(NOT DEFINED CMAKE_PREFIX_PATH)
if(NOT "$ENV{CMAKE_PREFIX_PATH}" STREQUAL "")
if(NOT WIN32)
string(REPLACE ":" ";" CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
else()
set(CMAKE_PREFIX_PATH $ENV{CMAKE_PREFIX_PATH})
endif()
endif()
endif()
# list of catkin workspaces
set(catkin_search_path "")
foreach(path ${CMAKE_PREFIX_PATH})
if(EXISTS "${path}/.catkin")
list(FIND catkin_search_path ${path} _index)
if(_index EQUAL -1)
list(APPEND catkin_search_path ${path})
endif()
endif()
endforeach()
# search for catkin in all workspaces
set(CATKIN_TOPLEVEL_FIND_PACKAGE TRUE)
find_package(catkin QUIET
NO_POLICY_SCOPE
PATHS ${catkin_search_path}
NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH)
unset(CATKIN_TOPLEVEL_FIND_PACKAGE)
if(NOT catkin_FOUND)
message(FATAL_ERROR "find_package(catkin) failed. catkin was neither found in the workspace nor in the CMAKE_PREFIX_PATH. One reason may be that no ROS setup.sh was sourced before.")
endif()
endif()
catkin_workspace()

201
Devices/Packages/sick_line_guidance/turtlebotDemo/LICENSE Executable file
View File

@@ -0,0 +1,201 @@
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following
boilerplate notice, with the fields enclosed by brackets "[]"
replaced with your own identifying information. (Don't include
the brackets!) The text should be enclosed in the appropriate
comment syntax for the file format. We also recommend that a
file or class name and description of purpose be included on the
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright [yyyy] [name of copyright owner]
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

45
Devices/Packages/sick_line_guidance/turtlebotDemo/README.md Executable file
View File

@@ -0,0 +1,45 @@
# TurtleBot demonstration
Demonstration of SICK line guidance demonstration with TurtleBot
## Build and install
To build and install a TurtleBot demonstration (sick_line_guidance_demo), follow the description [doc/build_install.md](doc/build_install.md)
## Build and run simulation
To build and run the sick_line_guidance_demo simulation, run the following commands:
```console
cd ~/catkin_ws/src
git clone https://github.com/SICKAG/sick_line_guidance.git
cd ./sick_line_guidance/turtlebotDemo/test/scripts
# ./gitCloneInstall.bash # install all packages for sick_line_guidance_demo (required once)
./makeall.bash # build everything for sick_line_guidance_demo
./runsimu.bash # run simulation of sick_line_guidance_demo
```
Note: ``` catkin_make ``` resp. ``` catkin_make install ``` will only build the SICK line guidance ros driver.
To build the TurtleBot demonstration, an additional option ``` --cmake-args -DTURTLEBOT_DEMO="ON" ``` is required:
```console
cd ~/catkin_ws
catkin_make install --cmake-args -DTURTLEBOT_DEMO="ON"
source ./install/setup.bash
```
TurtleBot packages are not required unless this option is enabled (``` --cmake-args -DTURTLEBOT_DEMO="ON" ```).
## Setup TurtleBot
To setup the TurtleBot for sick_line_guidance_demo, follow the description [doc/setup_turtlebot.md](doc/setup_turtlebot.md)
## Run TurtleBot demonstration
To run the SICK line guidance demonstration with a TurtleBot, run the following commands:
```console
cd ~/catkin_ws/src/sick_line_guidance/turtlebotDemo/test/scripts
./runturtlebot.bash
```

49
Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/CMakeLists.txt Executable file
View File

@@ -0,0 +1,49 @@
cmake_minimum_required(VERSION 2.8.3)
project(agc_radar)
## Compile as C++11, supported in ROS Kinetic and newer
add_compile_options(-std=c++17)
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
message_generation
)
add_message_files(
FILES
agc_radar_msg.msg
)
add_service_files(
FILES
agc_radar_config.srv
)
# Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
)
#catkin_package(
# #INCLUDE_DIRS include
# #LIBRARIES agc_radar
# CATKIN_DEPENDS message_generation roscpp std_msgs message_runtime gpio_handling
# #DEPENDS system_lib
#)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
${catkin_INCLUDE_DIRS}
# $HOME/catkin_ws/devel/include
)
add_executable(agc_radar src/agc_radar.cpp)
add_dependencies(agc_radar ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(agc_radar ${catkin_LIBRARIES})

4
Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/msg/agc_radar_msg.msg Executable file
View File

@@ -0,0 +1,4 @@
Header header
float64 Schutzzeit
float64 Stopzeit
bool obsctacle_stop

35
Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/package.xml Executable file
View File

@@ -0,0 +1,35 @@
<?xml version="1.0"?>
<package format="2">
<name>agc_radar</name>
<version>0.0.0</version>
<description>The agc_radar package</description>
<maintainer email="tsprifl@todo.todo">tsprifl</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>gpio_handling</build_depend>
<!-- <build_depend>custom_messages</build_depend> -->
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>message_generation</build_export_depend>
<build_export_depend>gpio_handling</build_export_depend>
<!--<build_export_depend>custom_messages</build_export_depend> -->
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>message_generation</exec_depend>
<exec_depend>message_runtime</exec_depend>
<exec_depend>gpio_handling</exec_depend>
<!--<exec_depend>custom_messages</exec_depend> -->
<export>
</export>
</package>

134
Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/src/agc_radar.cpp Executable file
View File

@@ -0,0 +1,134 @@
#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include "agc_radar/agc_radar_msg.h" // for publishing
#include "agc_radar/agc_radar_config.h" // for service
#include "gpio_handling/gpio_get_config.h"
#include "gpio_handling/gpio_set_config.h"
#include "gpio_handling/gpio_get_pin.h"
#include "gpio_handling/gpio_set_pin.h"
#include <typeinfo>
/*************************************************************************************************
*** Service Client (GPIO-handling)
**************************************************************************************************/
ros::ServiceClient gpio_get_config_client;
ros::ServiceClient gpio_set_config_client;
ros::ServiceClient gpio_get_pin_client;
ros::ServiceClient gpio_set_pin_client;
/*************************************************************************************************
*** Services
**************************************************************************************************/
gpio_handling::gpio_get_config gpio_get_config_srv;
gpio_handling::gpio_set_config gpio_set_config_srv;
gpio_handling::gpio_get_pin gpio_get_pin_srv;
gpio_handling::gpio_set_pin gpio_set_pin_srv;
/*************************************************************************************************
*** Messages
**************************************************************************************************/
agc_radar::agc_radar_msg agc_radar_msg;
bool config_Service_callback(agc_radar::agc_radar_config::Request &req,
agc_radar::agc_radar_config::Request &res){
agc_radar_msg.Schutzzeit = req.Schutzzeit;
ROS_INFO("set Schutzzeit to: %f", req.Schutzzeit);
return true;
}
int main(int argc, char **argv)
{
//defaults
agc_radar_msg.Schutzzeit = 1;
agc_radar_msg.Stopzeit = 0.01;
ros::init(argc, argv, "agc_radar");
ros::NodeHandle nh;
ROS_INFO("Node %s started.", ros::this_node::getName().c_str());
ros::Publisher agc_radar_pub = nh.advertise<agc_radar::agc_radar_msg>("agc_radar", 10);
ROS_INFO("Publishing on topic %s", agc_radar_pub.getTopic().c_str());
ros::Rate loop_rate(100);
ros::ServiceServer config_server = nh.advertiseService("agc_radar_config", config_Service_callback);
ROS_INFO("Service Server for radar configuration started. Call it with \"rosservice call %s\"", config_server.getService().c_str());
/* Configure Service Clients */
gpio_get_config_client = nh.serviceClient<gpio_handling::gpio_get_config>("gpio_get_config");
gpio_set_config_client = nh.serviceClient<gpio_handling::gpio_set_config>("gpio_set_config");
gpio_get_pin_client = nh.serviceClient<gpio_handling::gpio_get_pin>("gpio_get_pin");
gpio_set_pin_client = nh.serviceClient<gpio_handling::gpio_set_pin>("gpio_set_pin");
uint64_t begin = (uint64_t)ros::Time::now().sec *1e9 + (uint64_t)ros::Time::now().nsec;
uint64_t begin_stop = (uint64_t)ros::Time::now().sec *1e9 + (uint64_t)ros::Time::now().nsec;
gpio_get_pin_srv.request.pinNumber = {4};
while(ros::ok())
{
gpio_get_pin_client.call(gpio_get_pin_srv);
if(gpio_get_pin_srv.response.pinValue[0] == true) // radar detected an Object
{
if ((uint64_t)ros::Time::now().sec *1e9 + (uint64_t)ros::Time::now().nsec >= begin + agc_radar_msg.Stopzeit * 1e9)
{
agc_radar_msg.obsctacle_stop = true;
}
begin_stop = (uint64_t)ros::Time::now().sec *1e9 + (uint64_t)ros::Time::now().nsec;
}
else
{
if ((uint64_t)ros::Time::now().sec *1e9 + (uint64_t)ros::Time::now().nsec >= begin_stop + agc_radar_msg.Schutzzeit * 1e9)
{
agc_radar_msg.obsctacle_stop = false;
}
begin = (uint64_t)ros::Time::now().sec *1e9 + (uint64_t)ros::Time::now().nsec;
}
/*
if(gpio_get_msg.pin_value[4] == true) // radar detected an Object
{
sum = (0.99*sum + 0.01);
}
else
{
sum = (0.99*sum - 0.01);
}
if (sum < 0)
{
sum = 0;
}
if( sum > 1)
{
sum = 1;
}
ROS_INFO("sum: %lf", sum);
if (sum > 0.5)
{
agc_radar_msg.obsctacle_stop = true;
begin = (uint64_t)ros::Time::now().sec *1e9 + (uint64_t)ros::Time::now().nsec;
}
else
{
if( ((uint64_t)ros::Time::now().sec *1e9 + (uint64_t)ros::Time::now().nsec) >= (begin + agc_radar_msg.Schutzzeit) )
{
agc_radar_msg.obsctacle_stop = false;
}
}
*/
agc_radar_msg.header.stamp = ros::Time::now();
agc_radar_pub.publish(agc_radar_msg);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}

28
Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/src/agc_radar_config.cpp Executable file
View File

@@ -0,0 +1,28 @@
#include "ros/ros.h"
#include "agc_radar/agc_radar_config.h"
int main(int argc, char **argv) // Node Main Function
{
ros::init(argc, argv, "agc_radar_config"); // Initializes Node Name
if (argc != 2)
{
ROS_INFO("cmd : rosrun own own_client arg0 ");
ROS_INFO("arg0: Schutzzeit, type: float64\n arg1: Stopzeit, type: float64");
return 1;
}
char buffer [128];
sprintf(buffer, "%s/agc_radar_config", ros::this_node::getNamespace().c_str());
ros::NodeHandle nh;
ros::ServiceClient config_client = nh.serviceClient<agc_radar::agc_radar_config>(buffer);
// Declares the 'srv' service that uses the 'SrvTutorial' service file
agc_radar::agc_radar_config srv;
// Parameters entered when the node is executed as a service request value are stored at 'a' and 'b'
srv.request.Schutzzeit = atoll(argv[1]);
srv.request.Stopzeit = atoll(argv[2]);
config_client.call(srv);
return 0;
}

3
Devices/Packages/sick_line_guidance/turtlebotDemo/agc_radar/srv/agc_radar_config.srv Executable file
View File

@@ -0,0 +1,3 @@
float64 Schutzzeit
float64 Stopzeit
---

29
Devices/Packages/sick_line_guidance/turtlebotDemo/custom_messages/CMakeLists.txt Executable file
View File

@@ -0,0 +1,29 @@
cmake_minimum_required(VERSION 2.8.3)
project(custom_messages)
find_package(catkin REQUIRED COMPONENTS
message_generation
std_msgs
roscpp
)
add_message_files(
FILES
gpio.msg
)
generate_messages(
DEPENDENCIES
std_msgs
)
#catkin_package(
#LIBRARIES custom_messages
#CATKIN_DEPENDS std_msgs roscpp
#)
include_directories(
# include
${catkin_INCLUDE_DIRS}
)

2
Devices/Packages/sick_line_guidance/turtlebotDemo/custom_messages/msg/gpio.msg Executable file
View File

@@ -0,0 +1,2 @@
uint8[18] pin_config
bool[18] pin_value

27
Devices/Packages/sick_line_guidance/turtlebotDemo/custom_messages/package.xml Executable file
View File

@@ -0,0 +1,27 @@
<?xml version="1.0"?>
<package format="2">
<name>custom_messages</name>
<version>0.0.0</version>
<description>The custom_messages package</description>
<maintainer email="tsprifl@todo.todo">tsprifl</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>message_generation</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>message_generation</build_export_depend>
<build_export_depend>message_runtime</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>message_generation</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>std_msgs</exec_depend>
<export>
</export>
</package>

BIN
Devices/Packages/sick_line_guidance/turtlebotDemo/doc/OLS_barcode_0101.bmp Executable file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 9.9 KiB

BIN
Devices/Packages/sick_line_guidance/turtlebotDemo/doc/OLS_barcode_0102.bmp Executable file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 9.9 KiB

BIN
Devices/Packages/sick_line_guidance/turtlebotDemo/doc/OLS_barcodes.jpg Executable file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 112 KiB

13
Devices/Packages/sick_line_guidance/turtlebotDemo/doc/backlog.md Executable file
View File

@@ -0,0 +1,13 @@
# Backlog
* Makrolon bekleben nach Vorlage
* Simulation umsetzen
* Störungssimulation definieren (Bandbegrenztes normalverteiles Rauschen, systematischer Fehler und Aussetzer (Salt/Pepper))
* Kidnapping-Problem mit Sick besprechen
* Beschreibung für Setup der Simulation
* Beschreibung der Durchführung der Simulation
Merker: OLS10-Simulation
Spg.-Versorgung 24V an OLS10 anklemmen und Linux booten (20.06.2019)

226
Devices/Packages/sick_line_guidance/turtlebotDemo/doc/build_install.md Executable file
View File

@@ -0,0 +1,226 @@
# Build and install TurtleBot demonstration (sick_line_guidance_demo)
## Prerequisites
To run the demonstrations, Gazebo must be installed in advance. Also the keys for the access to the ROS-Repos. must be updated.
```console
sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-keys F42ED6FBAB17C654
sudo apt-get update
sudo apt-get install ros-melodic-gazebo-ros-pkgs ros-melodic-gazebo-ros-control
```
## Build and install
To build and install the TurtleBot demonstration (sick_line_guidance_demo), run the following commands:
```console
cd ~/catkin_ws/src
git clone https://github.com/SICKAG/sick_line_guidance.git
cd ./sick_line_guidance/turtlebotDemo/test/scripts
./gitCloneInstall.bash
```
This will install sick_line_guidance_demo and all required packages.
Alternatively, checkout the following packages for a manual installation:
```console
# Get ros packages for turtlebot
cd ~/catkin_ws/src
git clone https://github.com/ROBOTIS-GIT/turtlebot3.git
git clone https://github.com/ROBOTIS-GIT/turtlebot3_msgs.git
git clone https://github.com/ROBOTIS-GIT/hls_lfcd_lds_driver.git
git clone https://github.com/ros-drivers/rosserial.git
# Get ros packages for turtlebot simulation
git clone https://github.com/ROBOTIS-GIT/turtlebot3_simulations
git clone https://github.com/ROBOTIS-GIT/turtlebot3_gazebo_plugin.git
# Get can_open packages
git clone https://github.com/ros-industrial/ros_canopen.git
git clone https://github.com/CANopenNode/CANopenSocket.git
git clone https://github.com/linux-can/can-utils.git
# Get sick_line_guidance package
git clone https://github.com/ros-planning/random_numbers.git
git clone https://github.com/SICKAG/sick_line_guidance.git
# Get ros packages required for robot_fsm
git clone https://github.com/uos/sick_tim.git
# Install video support for sick_line_guidance_demo
sudo apt-get install ffmpeg
sudo apt-get install vlc
# Install profiling and performance tools
git clone https://github.com/catkin/catkin_simple.git
sudo svn export https://github.com/ethz-asl/schweizer_messer/trunk/sm_common # common utilities from ethz-asl "schweizer messer" toolbox
sudo svn export https://github.com/ethz-asl/schweizer_messer/trunk/sm_timing # timing utilities from ethz-asl "schweizer messer" toolbox
sudo apt-get install google-perftools libgoogle-perftools-dev graphviz # libprofiler for profiling
```
Modify file ~/.ignition/fuel/config.yaml: Replace "url: https://api.ignitionrobotics.org" by "url: https://api.ignitionfuel.org":
```
url: https://api.ignitionrobotics.org # https://api.ignitionfuel.org
```
Build and install by running
```console
cd ~/catkin_ws
catkin_make install --cmake-args -DTURTLEBOT_DEMO="ON"
source ./install/setup.bash
```
## Run simulation
To test build and install, run a standalone simulation (no Turtlebot or additional hardware required):
```console
cd ~/catkin_ws/src/sick_line_guidance/turtlebotDemo/test/scripts
./runsimu.bash
```
You should see robots positions (marked by a green dot), following the black lines:
![ ](simu01.jpg "screenshot simulation")
## Troubleshooting
### Versioncheck
Depending on OS, ROS and gcc versions, errors during build or run may occur. You can determine your versions by the following commands:
```console
cat /etc/os-release # displays the Linux distribution version
lsb_release -a # displays the Linux/OS version (linux standard base)
uname -a # displays the Linux kernel version
gcc --version # displays the compiler version
rosversion -d # displays the ros distro version
rosversion roscpp # displays the version of ros package roscpp
catkin --version # displays the version of catkin tools
```
### Compiler and build errors
#### "cout is not a member of std"
:question: SynchCout.h: "error: cout is not a member of std" in package decision_making:
:white_check_mark: Include \<iostream\> in file ~/catkin_ws/src/decision_making/decision_making/include/decision_making/SynchCout.h
#### "cmake: project 'XXX' tried to find library -lpthread"
:question: cmake fails with error: "Project 'XXX' tried to find library '-lpthread'":
:white_check_mark: delete component 'thread' from find_package(Boost ... COMPONENTS ...) in CMakeLists.txt of project 'XXX'
#### "include does not exist""
:question: "CMake Error at /opt/ros/melodic/share/catkin/cmake/catkin_package.cmake:302 (message): catkin_package() include dir include does not exist"
:white_check_mark: Uncommment ~/catkin_ws/src/robot_fsm/gpio_handling/CMakeLists.txt line 30 (#INCLUDE_DIRS include)
#### "No launch file Robot_FSM.launch"
:question: "RLException: \[Robot_FSM.launch\] is neither a launch file in package \[iam\] nor is \[iam\] a launch file name"
:white_check_mark: Append the following lines in ~/catkin_ws/src/robot_fsm/iam/CMakeLists.txt
```
install(FILES
launch/Robot_FSM.launch
yaml/AGC.yaml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
```
#### "Could NOT find urdf"
:question: cmake error "Could NOT find urdf":
```
-- ==> add_subdirectory(turtlebot3/turtlebot3_description)
-- Could NOT find urdf (missing: urdf_DIR)
-- Could not find the required component 'urdf'. The following CMake error indicates that you either need to install the package with the same name or change your environment so that it can be found.
CMake Error at /opt/ros/melodic/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
Could not find a package configuration file provided by "urdf" with any of the following names:
urdfConfig.cmake
urdf-config.cmake
Add the installation prefix of "urdf" to CMAKE_PREFIX_PATH or set
"urdf_DIR" to a directory containing one of the above files. If "urdf"
provides a separate development package or SDK, be sure it has been
installed.
Call Stack (most recent call first):
turtlebot3/turtlebot3_description/CMakeLists.txt:10 (find_package)
```
:white_check_mark: Update ros dependencies by
```
cd ~/catkin_ws
rosdep update
rosdep install --from-paths ~/catkin_ws/src/turtlebot3 --ignore-src
rosdep install --from-paths ~/catkin_ws/src/ros_canopen --ignore-src
catkin_make install
```
### Runtime errors
#### "Resource not found: IAM"
:question: "roslaunch iam Robot_FSM.launch" causes "Resource not found: IAM"
:white_check_mark: Modify paths in ~/catkin_ws/src/robot_fsm/iam/launch/Robot_FSM.launch :
replace \<rosparam command="load" file="$(find IAM)/yaml/AGC.yaml"/\> by \<rosparam command="load" file="$(find iam)/AGC.yaml"/\> and
replace \<include file="$(find sick_line_guidance)/launch/sick_line_guidance.launch"\> by \<include file="$(find sick_line_guidance)/sick_line_guidance.launch"\>
#### "libdecision_making_ros.so: No such file or directory"
:question: "catkin_ws/install/lib/iam/robot_fsm: error while loading shared libraries: libdecision_making_ros.so: cannot open shared object file: No such file or directory"
:white_check_mark: Uncomment line 181-185 in file ~/catkin_ws/src/decision_making/decision_making/CMakeLists.txt:
```
install(TARGETS decision_making_ros # decision_making decision_making_node
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
```
#### "invalid topic type: OLS_Measurement"
:question: "ERROR: invalid topic type: OLS_Measurement" when sending an OLS_Measurement message to robot_fsm
:white_check_mark: Package iam (part of the robot_fsm) redefines message type OLS_Measurement. Message type iam/OLS_Measurement may differ from OLS_Measurement
definition in package sick_line_guidance. To avoid OLS_Measurement type conflicts, only OLS_Measurement messages defined in sick_line_guidance should be used.
Replace iam::OLS_Measurement by sick_line_guidance::OLS_Measurement in the following files:
```
// replace iam::OLS_Measurement by sick_line_guidance::OLS_Measurement in file catkin_ws/src/robot_fsm/iam/include/iam/robot_fsm.h:
#include "sick_line_guidance/OLS_Measurement.h" // line 14: "iam/OLS_Measurement.h"
void olsCallback(const sick_line_guidance::OLS_Measurement::ConstPtr& msg); // line 84: void olsCallback(const iam::OLS_Measurement::ConstPtr& msg);
sick_line_guidance::OLS_Measurement::Type ols_msg; // line 99: iam::OLS_Measurement::Type ols_msg;
// replace iam::OLS_Measurement by sick_line_guidance::OLS_Measurement in file catkin_ws/src/robot_fsm/iam/src/robot_fsm.cpp:
void olsCallback(const sick_line_guidance::OLS_Measurement::ConstPtr& msg){ // line 498: void olsCallback(const iam::OLS_Measurement::ConstPtr& msg){
// deactivate iam::OLS_Measurement in add_message_files(...) in file catkin_ws/src/robot_fsm/iam/CMakeLists.txt:
# OLS_Measurement.msg
```
#### "turtlebot3_gazebo: Error in REST request"
:question: "roslaunch turtlebot3_gazebo turtlebot3_world.launch" results in error message
"\[Err\] \[REST.cc:205\] Error in REST request libcurl: (51) SSL: no alternative certificate subject name matches target host name 'api.ignitionfuel.org'"
:white_check_mark: Replace url: https://api.ignitionrobotics.org by https://api.ignitionfuel.org in file ~/.ignition/fuel/config.yaml:
```
url: https://api.ignitionrobotics.org # https://api.ignitionfuel.org
```
#### "turtlebot3_gazebo: VMware: vmw_ioctl_command error Invalid argument. Aborted (core dumped)"
:question: "roslaunch turtlebot3_gazebo turtlebot3_world.launch" results in a core dump with an error message
"VMware: vmw_ioctl_command error Invalid argument. Aborted (core dumped)"
:white_check_mark: Uncheck "Accelerate 3D Graphics" in VMware settings and set
```console
export SVGA_VGPU10=0
export LIBGL_ALWAYS_SOFTWARE=1
```
before launching turtlebot3_gazebo.
#### Connect to bot:
```console
ssh turtlebot@192.168.178.45
```
ps turtlebot3

BIN
Devices/Packages/sick_line_guidance/turtlebotDemo/doc/demo_map.pptx Executable file
View File

Binary file not shown.

BIN
Devices/Packages/sick_line_guidance/turtlebotDemo/doc/demo_map_01.png Executable file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 58 KiB

BIN
Devices/Packages/sick_line_guidance/turtlebotDemo/doc/demo_map_02.png Executable file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 29 KiB

77
Devices/Packages/sick_line_guidance/turtlebotDemo/doc/faq.md Executable file
View File

@@ -0,0 +1,77 @@
# Interne Sammlung von FAQ
## Zugriff auf Turtlebot über WLAN-Lehning
Der Zugriff erfolgt über SSH per WLAN. Der Turtlebot holt sich via DHCP (eigentlich) immer dieselbe IP-Adresse.
Deswegen wurde vorerst auf Static-IP verzichtet.
Die IP-Adresse lautet: 192.168.178.45
Mit dem Zugriff über
```bash
ssh -X turtlebot@192.168.178.45
```
kann die Remote-Verbindung zum Roboter aufgebaut werden.
## PC:
PC-Typ: Ist es der Typ „EC700-BT4051-454-64WT“, den Sie in Ihrer Master-Arbeit erwähnen? -> Ja
Ist ein Account/Ubuntu eingerichtet? -> Ja
Annahme: Ubuntu: 18.04 Betriebssystem mit der ROS-Distribution ROS-Melodic -> Richtig
Wenn „Ja“: Username und Passwort? -> Username: turtlebot, Passwort: turtlebot3
Kann man den PC mit einem der beigefügten Netzteile betreiben? -> Könnte man theoretisch. Ich würde Ihnen empfehlen den Stecker (roter Pfeil) einzustecken. (Aufgrund der Länge passt er nur in die auf dem Bild rechte Buchse). Dazu können Sie das Netzteil für die Powerbank in die linke Buchse einstecken. Dann haben Sie einen "quasi Netzbetrieb".
## TiM:
Eingestellte IP-Adresse? -> Am TiM habe ich nichts konfiguriert. Daher müsste noch die Standard-IP-Adresse konfiguriert sein.
## Spg.-Versorgung:
Batterie wird mit dem Standardnetzteil aufgeladen? Blauer Pfeil? -> Richtig
Spg.-Versorgung zum PC erfolgt einfach durch Einstecken des Hohlsteckers in die Batterie? (roter Pfeil) -> Korrekt
## Miniatur-Lichtschranken:
Ich nehme an, dass die Miniatur-Lichtschranken von Typ "GL2S- E1311" sind (vgl. Masterarbeit) -> korrekt
Sind die Lichtschranken an das OpenCR-Board wie in der Masterarbeit angeschlossen? -> Auf dem "Version 0" Dokument, das Sie haben finden Sie auf Seite 16 die Abbildung 5.1. Die Sensoren sind wie dort abgebildet am OpenCR-Board angeschlossen. Die Einweglichtschranken (GRSE18S) sind auf Ihrem Turtlebot jedoch nicht vorhanden. Zwischen dem Schaltausgang der Miniatur-Lichtschranken GL2S-E1311 und dem GPIO-Pin befindet sich noch ein Serienwiderstand von 57k, um die Spannung an den GPIO-Pins auf 3,3V anzupassen (Thesis S. 20 Abb. 5.6).
## Allgemein:
Sollte ich beim Laden, Betreiben etc. auf irgendetwas besonders achten? -> Ich habe in dem Git-Repo, in dem ich Sie als Collaborator hinzugefügt habe eine Readme.md erstellt. Mit dieser Datei könnte es Ihnen evtl. etwas leichter fallen, das System -- so wie ich es immer in Betrieb genommen habe -- in Betrieb zu nehmen.
## Herunterfahren
```bash
sudo shutdown -h now
# Alternativ:
sudo init 0
```
## Turtlebot: USB stick mounten
```bash
# list all devices:
sudo fdisk -l
# usbstick-device finden , z.B. /dev/sdb1
sudo mkdir /media/usbstick
# usb stick mounten, /dev/sdb1 ggfs ersetzen
sudo mount -t vfat /dev/sdb1 /media/usbstick -o uid=1000,gid=1000,utf8,dmask=027,fmask=137
# usb stick gemounted unter /media/usbstick:
ls /media/usbstick
```
## Turtlebot: Filetransfer mit scp
```bash
# ros logfiles mit scp vom Turtlebot in ein lokales Verzeichnis auf dem Linux-Rechner kopieren:
rostest@ROS-NB:
mkdir -p ~/tmp
cd ~/tmp
scp -r turtlebot@192.168.178.45:/home/turtlebot/.ros/log scp -r turtlebot@192.168.178.45:/home/turtlebot/catkin_ws/src/sick_line_guidance/turtlebotDemo/test/scripts/log .
# scp -r turtlebot@192.168.178.45:/home/turtlebot/.ros/log .
# scp -r turtlebot@192.168.178.45:/home/turtlebot/catkin_ws/src/sick_line_guidance_demo/test/scripts/log .
```

69
Devices/Packages/sick_line_guidance/turtlebotDemo/doc/inbetriebnahme.md Executable file
View File

@@ -0,0 +1,69 @@
# Inbetriebnahme
Die vorliegende Anleitung zeigt die Inbetriebnahme der Hardware für den Turtlebot.
Ausführliche Details findet man unter:
https://github.com/tsprifl/catkin_src
# Lager
Der Turtlebot und die Unterlagen (neben Ladegerät etc.) lagen im Labor in den Kisten <todo>
# Schritte
1. Powerverbindung mit XTPower-Akku herstellen.
2. Ggf. Ladegerät "XTPower TurtleBot Sick" anschließen.
3. Gerät startet
4. Gerät mit Monitor (HDMI an der Frontseite) und USB über HUB mit Tastatur und Maus verbinden.
5. Bei Sick wurde das Gerät hinter einem Proxy betrieben. Der ROS-Master lief auf einem Remote-PC.
Möchte man bei SICK eine Demo starten, muss man den Proxy gemäß Proxy Config einstellen.
6. ROS-Master: Bei einer lokalen Entwicklung auf dem System ROS_MASTER und ROS_HOSTNAME gem. u.a. config eingestellt werden.
Für eine Remote-Verbindung muss die IP-Adresse etc. für Remote-Rechner mit laufendem Ros-Core bekannt sein.
(siehe u.a. Punkte)
7. Nun der o.a. Anleitung folgen (ACHTUNG: In der Anleitungen befinden sich Fehler. Siehe "Start des Bots". Statt
"rosrun iam" muss man "roslaunch iam Robot_FSM.launch" angeben (s.u.).
In Kurzform:
* Terminator: 4 Terminalfenstser starten
* Terminal 1: roslaunch turtlebot3_bringup turtlebot3_robot.launch
* Terminal 2: ols
* Terminal 3: rosrun gpio_handling gpio_handler
* Terminal 4: rosparam load ~/catkin_ws/src/iam/yaml/AGC.yam
* Terminal 4 (erneut): roslaunch iam Robot_FSM.launch
8. Spurführungsband verlegen bzw. Robot auf Demo-System setzen.
9. terminator starten
## Proxy Config
* Bei Lehning : keine
* Bei Sick :
* Host/Port: cloudproxy-sickag.sickcn.net:10415
* Details siehe:
* Webproxy: https://wiki.ubuntuusers.de/Proxyserver/#Unity-und-GNOME-3
* apt proxy: https://askubuntu.com/questions/257290/configure-proxy-for-apt
## ros config remote/local master
IP Config Bash RC
In der Datei ~/.bashrc muss die IP-Adresse des Ros-Master (Remote-PC), sowie die IP-Adresse des Turtlebots eingetragen werden. (Ändern der letzten beiden Zeilen).
für lokalen master 127.0.0.1
export ROS_MASTER_URI=http://127.0.0.1:11311
export ROS_HOSTNAME=127.0.0.1
## Start des Bots
Für jedes Terminal wie gewohnt:
```console
cd catkin_ws
source devel/setup.bash
```
Bei Punkt 6:
statt
```console
rosrun iam
```
```console
roslaunch iam Robot_FSM.launch
```
siehe Anleitung unter
https://github.com/tsprifl/catkin_src

57
Devices/Packages/sick_line_guidance/turtlebotDemo/doc/ols_sensor_properties.md Executable file
View File

@@ -0,0 +1,57 @@
# Properties of OLS line guidance sensor for sick_line_guidance_demo
The OLS sensor on the demo system is mounted 65 mm over ground. Both line distances (lcp, line center points) and line width measured by the sensor
depend on its height over ground. LineSensorConfig configures a scaling between sensor measurement and physical world:
| parameter | default value | details |
| --- | --- | --- |
| line_sensor_scaling_dist | 180.0 / 133.0 | Scaling between physical distance to the line center and measured line center point, depending on mounted sensor height (measurement: lcp = 180 mm, physical: lcp = 133 mm) |
| line_sensor_scaling_width | 29.0 / 20.0 | Scaling between physical line width (20 mm) and measured line width (29 mm), depending on mounted sensor height (sensor mounted 100 mm over ground: scaling = 1, sensor mounted 65 mm over ground: scaling = 100/65 = 1.5) |
## Line distance scaling
Line center points measured with the demo system (OLS mounted 65 mm over ground):
| lcp measured | physical line distance | scaling |
| --- | --- | --- |
| 180 mm | 133 mm | 180/133 = 1.35 |
Line offset: The sensor is not completely centered. If the sensor is centered over the right side of the line,
a line center point with value 0 is detected.
## Line width scaling
Line width measured with the demo system (OLS mounted 65 mm over ground):
| angle | measured width | physical width | scaling |
| --- | --- | --- | --- |
| 0 degree | 29 mm | 20 mm | 29/29 = 1.45 |
| 45 degree | 43 mm | 28 mm | 43/28 = 1.52 |
| max. | 75 mm | | |
## Line switching at junctions
Lines detected by the sensor can switch at junctions depending on robots movement. When taking the left turnout, the left line will become the center line
and the previous main line will become the right line.
The following line switches were observed under a test of the demo system:
Sensor moves from left to right at a junction (2 lines, lcp\[0\], lcp\[1\], lcp\[2\] measured in mm):
| lcp\[0\] | lcp\[1\] | lcp\[2\] | description |
| --- | --- | --- | --- |
| - | 88 | - | start, one line visible |
| - | 0 | 46 | switched to 2 lines, when sensor in the center of left line |
| -32 | 12 | - | next line switch |
Sensor moves from right to left at a junction (2 lines, lcp\[0\], lcp\[1\], lcp\[2\] measured in mm):
## Line detection at barcodes
If a barcode is placed over the line, lines are detected in different ways:
1. There are no lines detected within the lower textzone (the area with human readable numbers).
2. Within the label zone (machine readable barcode label), a line with a width of round about the barcode is detected (line width >= 80 mm).
3. If the yaw angle between barcode and sensor is above 30 degrees, two or three lines are detected with small width (i.e. the markers are detected as lines).
4. No lines are detected within the small upper gap between the label area and the upper barcode border.

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